hofee/nbv_sim
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Visualize control target

Browse Source
This commit is contained in:
Michel Breyer 2021-06-08 11:43:58 +02:00
parent d431a95b5a
commit 139b73d657
2 changed files with 10 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
bt_sim_node.py
View File

@ -9,7 +9,7 @@ import tf2_ros
import control_msgs.msg import control_msgs.msg
import franka_gripper.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 from sensor_msgs.msg import JointState, Image, CameraInfo
import std_srvs.srv import std_srvs.srv
@ -43,10 +43,11 @@ class BtSimNode:
def setup_controllers(self): def setup_controllers(self):
self.cartesian_pose_controller = CartesianPoseController(self.sim.arm) 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): 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): def setup_gripper_interface(self):
self.gripper_interface = GripperInterface(self.sim.gripper) self.gripper_interface = GripperInterface(self.sim.gripper)

9
policies.py
View File

@ -4,7 +4,7 @@ from pathlib import Path
import rospy import rospy
import scipy.interpolate import scipy.interpolate
from geometry_msgs.msg import Pose from geometry_msgs.msg import PoseStamped
from sensor_msgs.msg import Image, CameraInfo from sensor_msgs.msg import Image, CameraInfo
import std_srvs.srv import std_srvs.srv
from visualization_msgs.msg import Marker, MarkerArray from visualization_msgs.msg import Marker, MarkerArray
@ -93,7 +93,7 @@ class BaseController:
self.base_frame = rospy.get_param("~base_frame_id") self.base_frame = rospy.get_param("~base_frame_id")
self.ee_frame = rospy.get_param("~ee_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._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() self.gripper = PandaGripperClient()
def _setup_camera_connection(self): def _setup_camera_connection(self):
@ -181,7 +181,10 @@ class BaseController:
return grasps[0] if len(grasps) > 0 else None return grasps[0] if len(grasps) > 0 else None
def _send_pose_command(self, target): 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): class SingleViewBaseline(BaseController):