From 139b73d657636720f704ab13f727405b71928e3c Mon Sep 17 00:00:00 2001 From: Michel Breyer Date: Tue, 8 Jun 2021 11:43:58 +0200 Subject: [PATCH] Visualize control target --- bt_sim_node.py | 7 ++++--- policies.py | 9 ++++++--- 2 files changed, 10 insertions(+), 6 deletions(-) diff --git a/bt_sim_node.py b/bt_sim_node.py index cfde5fa..49bbe15 100755 --- a/bt_sim_node.py +++ b/bt_sim_node.py @@ -9,7 +9,7 @@ import tf2_ros import control_msgs.msg import franka_gripper.msg -from geometry_msgs.msg import Pose +from geometry_msgs.msg import PoseStamped from sensor_msgs.msg import JointState, Image, CameraInfo import std_srvs.srv @@ -43,10 +43,11 @@ class BtSimNode: def setup_controllers(self): self.cartesian_pose_controller = CartesianPoseController(self.sim.arm) - rospy.Subscriber("command", Pose, self.target_pose_cb) + rospy.Subscriber("command", PoseStamped, self.target_pose_cb) def target_pose_cb(self, msg): - self.cartesian_pose_controller.x_d = from_pose_msg(msg) + assert msg.header.frame_id == self.sim.arm.base_frame + self.cartesian_pose_controller.x_d = from_pose_msg(msg.pose) def setup_gripper_interface(self): self.gripper_interface = GripperInterface(self.sim.gripper) diff --git a/policies.py b/policies.py index d759f86..41f0621 100644 --- a/policies.py +++ b/policies.py @@ -4,7 +4,7 @@ from pathlib import Path import rospy import scipy.interpolate -from geometry_msgs.msg import Pose +from geometry_msgs.msg import PoseStamped from sensor_msgs.msg import Image, CameraInfo import std_srvs.srv from visualization_msgs.msg import Marker, MarkerArray @@ -93,7 +93,7 @@ class BaseController: self.base_frame = rospy.get_param("~base_frame_id") self.ee_frame = rospy.get_param("~ee_frame_id") self._ee_grasp_offset = Transform.from_list(rospy.get_param("~ee_grasp_offset")) - self.target_pose_pub = rospy.Publisher("/command", Pose, queue_size=10) + self.target_pose_pub = rospy.Publisher("/command", PoseStamped, queue_size=10) self.gripper = PandaGripperClient() def _setup_camera_connection(self): @@ -181,7 +181,10 @@ class BaseController: return grasps[0] if len(grasps) > 0 else None def _send_pose_command(self, target): - self.target_pose_pub.publish(to_pose_msg(target)) + msg = PoseStamped() + msg.header.frame_id = self.base_frame + msg.pose = to_pose_msg(target) + self.target_pose_pub.publish(msg) class SingleViewBaseline(BaseController):