Skip to content

Code update with transitive holding #21

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
toyat522 wants to merge 1 commit into
base: master
Choose a base branch
from
Open

Code update with transitive holding #21

Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
17 changes: 4 additions & 13 deletions heracles_ros/src/heracles_ros/heracles_state_updater_node.py
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -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
Expand Down Expand Up @@ -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,
(
Expand All @@ -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})"
Expand Down
Loading