From a044ed711c573393e44bb806c5b10419b7efd027 Mon Sep 17 00:00:00 2001
From: Toya Takahashi <toyatakahashi522@gmail.com>
Date: Tue, 10 Mar 2026 22:47:48 -0400
Subject: [PATCH] transitive holding

---
 .../heracles_ros/heracles_state_updater_node.py | 17 ++++-------------
 1 file changed, 4 insertions(+), 13 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 442428b..246131e 100644
--- a/heracles_ros/src/heracles_ros/heracles_state_updater_node.py
+++ b/heracles_ros/src/heracles_ros/heracles_state_updater_node.py
@@ -1,7 +1,7 @@
 #!/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 dsg_updater.dsg_state_utils import robot_hold_obj, robot_unhold_obj, set_obj_center, set_robot_pose
 from geometry_msgs.msg import TransformStamped
 from heracles.query_interface import Neo4jWrapper
 from heracles_agents.dsg_interfaces import HeraclesDsgInterface
@@ -101,17 +101,8 @@ def timer_callback(self):
         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
-        """
+        x, y, z, _, _, _, _ = robot_pose
+
         with Neo4jWrapper(
             self.dsgdb_conf.uri,
             (
@@ -121,7 +112,7 @@ def timer_callback(self):
             atomic_queries=True,
             print_profiles=False,
         ) as db:
-            db.query(query)
+            set_robot_pose(db, self.robot_name, *robot_pose)
 
             self.get_logger().debug(
                 f"Updating DB: {self.robot_name} pos=({x:.2f},{y:.2f},{z:.2f})"