Added robot state updater node to update robot state in Heracles DB

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

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

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",
         ],
     },
 )

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()

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()

heracles_ros_interfaces/package.xml

@@ -0,0 +1,23 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>heracles_ros_interfaces</name>
+  <version>0.0.1</version>
+  <description>ROS interface definitions for heracles_ros</description>
+
+  <maintainer email="aaronray@mit.edu">Aaron Ray</maintainer>
+  <author email="toyat@mit.edu">Toya Takahashi</author>
+  <license>BSD 3-Clause</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+  <buildtool_depend>rosidl_default_generators</buildtool_depend>
+  <depend>builtin_interfaces</depend>
+  <depend>std_msgs</depend>
+  <exec_depend>rosidl_default_runtime</exec_depend>
+
+  <member_of_group>rosidl_interface_packages</member_of_group>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

heracles_ros_interfaces/srv/UpdateHoldingState.srv

@@ -0,0 +1,4 @@
+bool is_holding
+string id
+---
+bool success