diff --git a/heracles_ros/config/heracles_state_updater_node.yaml b/heracles_ros/config/heracles_state_updater_node.yaml new file mode 100644 index 0000000..6987e0a --- /dev/null +++ b/heracles_ros/config/heracles_state_updater_node.yaml @@ -0,0 +1,8 @@ +--- +/**: + ros__parameters: + heracles_ip: $(env ADT4_HERACLES_IP) + heracles_port: $(env ADT4_HERACLES_PORT) + map_frame: map + robot_name: $(var robot_name) + publish_rate: 1.0 diff --git a/heracles_ros/launch/heracles_state_updater.launch.yaml b/heracles_ros/launch/heracles_state_updater.launch.yaml new file mode 100644 index 0000000..5dec1fb --- /dev/null +++ b/heracles_ros/launch/heracles_state_updater.launch.yaml @@ -0,0 +1,16 @@ +--- +launch: + - arg: {name: spark_env, default: $(env ADT4_ENV)/spark_env} + - arg: {name: robot_name, default: ''} + - pyenv_node: + pkg: heracles_ros + exec: heracles_state_updater_node + name: heracles_state_updater_node + namespace: $(var robot_name) + pyenv: $(var spark_env) + param: + - {name: use_sim_time, value: false} + - {from: $(find-pkg-share heracles_ros)/config/heracles_state_updater_node.yaml, allow_substs: true} + remap: + - from: ~/update_holding_state + to: /$(var robot_name)/update_holding_state diff --git a/heracles_ros/setup.py b/heracles_ros/setup.py index 3f57a15..9acb4de 100644 --- a/heracles_ros/setup.py +++ b/heracles_ros/setup.py @@ -46,7 +46,8 @@ def get_share_info(top_level, pattern, dest=None): tests_require=["pytest"], entry_points={ "console_scripts": [ - "heracles_publisher_node = heracles_ros.heracles_publisher_node:main" + "heracles_publisher_node = heracles_ros.heracles_publisher_node:main", + "heracles_state_updater_node = heracles_ros.heracles_state_updater_node:main", ], }, ) diff --git a/heracles_ros/src/heracles_ros/heracles_state_updater_node.py b/heracles_ros/src/heracles_ros/heracles_state_updater_node.py new file mode 100644 index 0000000..442428b --- /dev/null +++ b/heracles_ros/src/heracles_ros/heracles_state_updater_node.py @@ -0,0 +1,145 @@ +#!/usr/bin/env python3 +import rclpy +import tf2_ros +from dsg_updater.dsg_state_utils import robot_hold_obj, robot_unhold_obj, set_obj_center +from geometry_msgs.msg import TransformStamped +from heracles.query_interface import Neo4jWrapper +from heracles_agents.dsg_interfaces import HeraclesDsgInterface +from heracles_ros_interfaces.srv import UpdateHoldingState +from rclpy.node import Node + + +class HeraclesStateUpdater(Node): + def __init__(self): + super().__init__("heracles_state_updater") + + self.declare_parameter("heracles_ip", "") + self.declare_parameter("heracles_port", -1) + self.declare_parameter("map_frame", "map") + self.declare_parameter("robot_name", "hamilton") + self.declare_parameter("publish_rate", 1.0) + self.map_frame = self.get_parameter("map_frame").value + self.robot_name = self.get_parameter("robot_name").value + self.publish_rate = self.get_parameter("publish_rate").value + + ip = self.get_parameter("heracles_ip").get_parameter_value().string_value + port = self.get_parameter("heracles_port").get_parameter_value().integer_value + self.get_logger().info(f"Port: {port}") + + assert ip != "", "Please set database IP" + assert port > 0, "Please set database port" + + self.dsgdb_conf = HeraclesDsgInterface( + dsg_interface_type="heracles", + uri=f"neo4j://{ip}:{port}", + ) + + self.tf_buffer = tf2_ros.Buffer() + self.tf_listener = tf2_ros.TransformListener(self.tf_buffer, self) + self.timer = self.create_timer(1.0 / self.publish_rate, self.timer_callback) + + self.holding_srv = self.create_service( + UpdateHoldingState, + "~/update_holding_state", + self.update_holding_state_callback, + ) + + def _get_robot_pose(self): + target_frame = f"{self.robot_name}/base_link" + try: + transform: TransformStamped = self.tf_buffer.lookup_transform( + self.map_frame, target_frame, rclpy.time.Time() + ) + except Exception as e: + self.get_logger().warn(f"TF not available yet: {e}") + return None + + t = transform.transform.translation + q = transform.transform.rotation + return t.x, t.y, t.z, q.w, q.x, q.y, q.z + + def update_holding_state_callback(self, request, response): + object_id = request.id + is_holding = request.is_holding + + with Neo4jWrapper( + self.dsgdb_conf.uri, + ( + self.dsgdb_conf.username.get_secret_value(), + self.dsgdb_conf.password.get_secret_value(), + ), + atomic_queries=True, + print_profiles=False, + ) as db: + if is_holding: + response.success = robot_hold_obj(db, self.robot_name, object_id) + else: + robot_pose = self._get_robot_pose() + if robot_pose is None: + response.success = False + else: + x, y, z, _, _, _, _ = robot_pose + last_pos_success = set_obj_center(db, object_id, x, y, z) + unhold_success = robot_unhold_obj(db, self.robot_name, object_id) + response.success = last_pos_success and unhold_success + + if response.success: + self.get_logger().info( + f"Successfully set holding state: robot={self.robot_name}, " + f"object={object_id}, is_holding={is_holding}" + ) + else: + self.get_logger().error( + f"Failed to set holding state: robot={self.robot_name}, " + f"object={object_id}, is_holding={is_holding}" + ) + + return response + + def timer_callback(self): + robot_pose = self._get_robot_pose() + if robot_pose is None: + return + + x, y, z, qw, qx, qy, qz = robot_pose + + query = f""" + MERGE (r:Robot {{name: '{self.robot_name}'}}) + SET r.position = point({{x: {x}, y: {y}, z: {z}}}), + r.qw = {qw}, + r.qx = {qx}, + r.qy = {qy}, + r.qz = {qz} + RETURN r + """ + with Neo4jWrapper( + self.dsgdb_conf.uri, + ( + self.dsgdb_conf.username.get_secret_value(), + self.dsgdb_conf.password.get_secret_value(), + ), + atomic_queries=True, + print_profiles=False, + ) as db: + db.query(query) + + self.get_logger().debug( + f"Updating DB: {self.robot_name} pos=({x:.2f},{y:.2f},{z:.2f})" + ) + + +def main(args=None): + rclpy.init(args=args) + node = HeraclesStateUpdater() + + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/heracles_ros_interfaces/CMakeLists.txt b/heracles_ros_interfaces/CMakeLists.txt new file mode 100644 index 0000000..2b70162 --- /dev/null +++ b/heracles_ros_interfaces/CMakeLists.txt @@ -0,0 +1,24 @@ +cmake_minimum_required(VERSION 3.16) +project(heracles_ros_interfaces) + +set(CMAKE_CXX_STANDARD 17) +set(CMAKE_CXX_STANDARD_REQUIRED ON) + +find_package(ament_cmake REQUIRED) +find_package(builtin_interfaces REQUIRED) +find_package(rosidl_default_generators REQUIRED) +find_package(std_msgs REQUIRED) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra) +endif() + +rosidl_generate_interfaces( + ${PROJECT_NAME} + srv/UpdateHoldingState.srv + DEPENDENCIES + builtin_interfaces + std_msgs +) + +ament_package() diff --git a/heracles_ros_interfaces/package.xml b/heracles_ros_interfaces/package.xml new file mode 100644 index 0000000..7c98426 --- /dev/null +++ b/heracles_ros_interfaces/package.xml @@ -0,0 +1,23 @@ + + + + heracles_ros_interfaces + 0.0.1 + ROS interface definitions for heracles_ros + + Aaron Ray + Toya Takahashi + BSD 3-Clause + + ament_cmake + rosidl_default_generators + builtin_interfaces + std_msgs + rosidl_default_runtime + + rosidl_interface_packages + + + ament_cmake + + diff --git a/heracles_ros_interfaces/srv/UpdateHoldingState.srv b/heracles_ros_interfaces/srv/UpdateHoldingState.srv new file mode 100644 index 0000000..f263f4e --- /dev/null +++ b/heracles_ros_interfaces/srv/UpdateHoldingState.srv @@ -0,0 +1,4 @@ +bool is_holding +string id +--- +bool success