From 240a0b004422ae416d14a657f0518f9cf4c3bdd5 Mon Sep 17 00:00:00 2001 From: Toya Takahashi Date: Sun, 23 Nov 2025 16:21:44 -0500 Subject: [PATCH 1/6] added robot state updater node --- heracles_ros/setup.py | 3 +- .../heracles_state_updater_node.py | 87 +++++++++++++++++++ 2 files changed, 89 insertions(+), 1 deletion(-) create mode 100644 heracles_ros/src/heracles_ros/heracles_state_updater_node.py 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..d4069af --- /dev/null +++ b/heracles_ros/src/heracles_ros/heracles_state_updater_node.py @@ -0,0 +1,87 @@ +#!/usr/bin/env python3 + +import rclpy +from rclpy.node import Node +from geometry_msgs.msg import TransformStamped +import tf2_ros + +from heracles_agents.dsg_interfaces import HeraclesDsgInterface +from heracles.query_interface import Neo4jWrapper + + +class HeraclesPosePublisher(Node): + def __init__(self): + super().__init__("heracles_pose_publisher") + + 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 + + self.dsgdb_conf = HeraclesDsgInterface( + dsg_interface_type="heracles", + uri="neo4j://$ADT4_HERACLES_IP:$ADT4_HERACLES_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) + + def timer_callback(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 + + t = transform.transform.translation + x, y, z = t.x, t.y, t.z + q = transform.transform.rotation + qw, qx, qy, qz = q.w, q.x, q.y, q.z + + 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 = HeraclesPosePublisher() + + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + main() From 86364c5a3e12b88ce5e40ee27eadb8402773d7e4 Mon Sep 17 00:00:00 2001 From: Toya Takahashi Date: Sun, 23 Nov 2025 22:37:13 -0500 Subject: [PATCH 2/6] added service to update holding state --- .../heracles_state_updater_node.py | 72 ++++++++++++++++--- heracles_ros_interfaces/CMakeLists.txt | 24 +++++++ heracles_ros_interfaces/package.xml | 23 ++++++ .../srv/UpdateHoldingState.srv | 4 ++ 4 files changed, 113 insertions(+), 10 deletions(-) create mode 100644 heracles_ros_interfaces/CMakeLists.txt create mode 100644 heracles_ros_interfaces/package.xml create mode 100644 heracles_ros_interfaces/srv/UpdateHoldingState.srv diff --git a/heracles_ros/src/heracles_ros/heracles_state_updater_node.py b/heracles_ros/src/heracles_ros/heracles_state_updater_node.py index d4069af..278f768 100644 --- a/heracles_ros/src/heracles_ros/heracles_state_updater_node.py +++ b/heracles_ros/src/heracles_ros/heracles_state_updater_node.py @@ -1,17 +1,19 @@ #!/usr/bin/env python3 import rclpy -from rclpy.node import Node -from geometry_msgs.msg import TransformStamped import tf2_ros +from dsg_updater.dsg_state_utils import set_obj_center, robot_hold_obj, robot_unhold_obj +from geometry_msgs.msg import TransformStamped from heracles_agents.dsg_interfaces import HeraclesDsgInterface +from heracles_ros_interfaces.srv import UpdateHoldingState from heracles.query_interface import Neo4jWrapper +from rclpy.node import Node -class HeraclesPosePublisher(Node): +class HeraclesStateUpdater(Node): def __init__(self): - super().__init__("heracles_pose_publisher") + super().__init__("heracles_state_updater") self.declare_parameter("map_frame", "map") self.declare_parameter("robot_name", "hamilton") @@ -29,21 +31,71 @@ def __init__(self): self.tf_listener = tf2_ros.TransformListener(self.tf_buffer, self) self.timer = self.create_timer(1.0 / self.publish_rate, self.timer_callback) - def timer_callback(self): - target_frame = f"{self.robot_name}/base_link" + self.holding_srv = self.create_service( + UpdateHoldingState, + #"update_holding_state", + "/hamilton/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 + return None t = transform.transform.translation - x, y, z = t.x, t.y, t.z q = transform.transform.rotation - qw, qx, qy, qz = q.w, q.x, q.y, q.z + 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}'}}) @@ -72,7 +124,7 @@ def timer_callback(self): def main(args=None): rclpy.init(args=args) - node = HeraclesPosePublisher() + node = HeraclesStateUpdater() try: rclpy.spin(node) 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 From 7bb452a1ee7aecaea398f87f9abcfc20f815f2d6 Mon Sep 17 00:00:00 2001 From: Toya Takahashi Date: Thu, 27 Nov 2025 13:48:19 -0500 Subject: [PATCH 3/6] prevent hard-coded topic name --- heracles_ros/config/heracles_state_updater_node.yaml | 6 ++++++ .../src/heracles_ros/heracles_state_updater_node.py | 4 +--- 2 files changed, 7 insertions(+), 3 deletions(-) create mode 100644 heracles_ros/config/heracles_state_updater_node.yaml 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..107bf3a --- /dev/null +++ b/heracles_ros/config/heracles_state_updater_node.yaml @@ -0,0 +1,6 @@ +--- +/**: + ros__parameters: + map_frame: map + robot_name: $(var robot_name) + publish_rate: 1.0 diff --git a/heracles_ros/src/heracles_ros/heracles_state_updater_node.py b/heracles_ros/src/heracles_ros/heracles_state_updater_node.py index 278f768..a52d3f5 100644 --- a/heracles_ros/src/heracles_ros/heracles_state_updater_node.py +++ b/heracles_ros/src/heracles_ros/heracles_state_updater_node.py @@ -1,5 +1,4 @@ #!/usr/bin/env python3 - import rclpy import tf2_ros @@ -33,8 +32,7 @@ def __init__(self): self.holding_srv = self.create_service( UpdateHoldingState, - #"update_holding_state", - "/hamilton/update_holding_state", + "~/update_holding_state", self.update_holding_state_callback ) From f8bf3dc7a30b21adb026b12836062fc354a274ce Mon Sep 17 00:00:00 2001 From: Toya Takahashi Date: Thu, 27 Nov 2025 14:42:11 -0500 Subject: [PATCH 4/6] fixed lint --- .../src/heracles_ros/heracles_state_updater_node.py | 9 ++++----- 1 file changed, 4 insertions(+), 5 deletions(-) diff --git a/heracles_ros/src/heracles_ros/heracles_state_updater_node.py b/heracles_ros/src/heracles_ros/heracles_state_updater_node.py index a52d3f5..17ed245 100644 --- a/heracles_ros/src/heracles_ros/heracles_state_updater_node.py +++ b/heracles_ros/src/heracles_ros/heracles_state_updater_node.py @@ -1,12 +1,11 @@ #!/usr/bin/env python3 import rclpy import tf2_ros - -from dsg_updater.dsg_state_utils import set_obj_center, robot_hold_obj, robot_unhold_obj +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 heracles.query_interface import Neo4jWrapper from rclpy.node import Node @@ -23,7 +22,7 @@ def __init__(self): self.dsgdb_conf = HeraclesDsgInterface( dsg_interface_type="heracles", - uri="neo4j://$ADT4_HERACLES_IP:$ADT4_HERACLES_PORT" + uri="neo4j://$ADT4_HERACLES_IP:$ADT4_HERACLES_PORT", ) self.tf_buffer = tf2_ros.Buffer() @@ -33,7 +32,7 @@ def __init__(self): self.holding_srv = self.create_service( UpdateHoldingState, "~/update_holding_state", - self.update_holding_state_callback + self.update_holding_state_callback, ) def _get_robot_pose(self): From 604a871781d911ef3ae0870c3d0546a129be7b2a Mon Sep 17 00:00:00 2001 From: Toya Takahashi Date: Mon, 15 Dec 2025 22:07:22 -0500 Subject: [PATCH 5/6] resolv ip and port using rosparam --- .../src/heracles_ros/heracles_state_updater_node.py | 11 ++++++++++- 1 file changed, 10 insertions(+), 1 deletion(-) diff --git a/heracles_ros/src/heracles_ros/heracles_state_updater_node.py b/heracles_ros/src/heracles_ros/heracles_state_updater_node.py index 17ed245..442428b 100644 --- a/heracles_ros/src/heracles_ros/heracles_state_updater_node.py +++ b/heracles_ros/src/heracles_ros/heracles_state_updater_node.py @@ -13,6 +13,8 @@ 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) @@ -20,9 +22,16 @@ def __init__(self): 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="neo4j://$ADT4_HERACLES_IP:$ADT4_HERACLES_PORT", + uri=f"neo4j://{ip}:{port}", ) self.tf_buffer = tf2_ros.Buffer() From 85628c227b239251fd544bb25aa00b95faeb99c5 Mon Sep 17 00:00:00 2001 From: Toya Takahashi Date: Thu, 18 Dec 2025 00:14:36 -0500 Subject: [PATCH 6/6] added launch file for heracles state updater --- .../config/heracles_state_updater_node.yaml | 2 ++ .../launch/heracles_state_updater.launch.yaml | 16 ++++++++++++++++ 2 files changed, 18 insertions(+) create mode 100644 heracles_ros/launch/heracles_state_updater.launch.yaml diff --git a/heracles_ros/config/heracles_state_updater_node.yaml b/heracles_ros/config/heracles_state_updater_node.yaml index 107bf3a..6987e0a 100644 --- a/heracles_ros/config/heracles_state_updater_node.yaml +++ b/heracles_ros/config/heracles_state_updater_node.yaml @@ -1,6 +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