Browse Source

Add stretch_driver topic for end_of_arm tool

pull/106/head
Binit Shah 1 year ago
parent
commit
ba3e4c8b24
4 changed files with 30 additions and 8 deletions
  1. +13
    -2
      hello_helpers/src/hello_helpers/hello_misc.py
  2. +6
    -0
      stretch_core/nodes/stretch_driver
  3. +1
    -1
      stretch_demos/nodes/grasp_object
  4. +10
    -5
      stretch_funmap/src/stretch_funmap/manipulation_planning.py

+ 13
- 2
hello_helpers/src/hello_helpers/hello_misc.py View File

@ -20,6 +20,7 @@ from trajectory_msgs.msg import JointTrajectoryPoint
import tf2_ros import tf2_ros
from sensor_msgs.msg import PointCloud2 from sensor_msgs.msg import PointCloud2
from std_srvs.srv import Trigger, TriggerRequest from std_srvs.srv import Trigger, TriggerRequest
from std_msgs.msg import String
####################### #######################
@ -69,6 +70,7 @@ class HelloNode:
def __init__(self): def __init__(self):
self.joint_state = None self.joint_state = None
self.point_cloud = None self.point_cloud = None
self.tool = None
@classmethod @classmethod
def quick_create(cls, name, wait_for_first_pointcloud=False): def quick_create(cls, name, wait_for_first_pointcloud=False):
@ -81,7 +83,10 @@ class HelloNode:
def point_cloud_callback(self, point_cloud): def point_cloud_callback(self, point_cloud):
self.point_cloud = point_cloud self.point_cloud = point_cloud
def tool_callback(self, tool_string):
self.tool = tool_string.data
def move_to_pose(self, pose, return_before_done=False, custom_contact_thresholds=False, custom_full_goal=False): def move_to_pose(self, pose, return_before_done=False, custom_contact_thresholds=False, custom_full_goal=False):
point = JointTrajectoryPoint() point = JointTrajectoryPoint()
point.time_from_start = rospy.Duration(0.0) point.time_from_start = rospy.Duration(0.0)
@ -182,6 +187,10 @@ class HelloNode:
rospy.logdebug(f"{self.node_name}'s HelloNode.stop_the_robot: got message {trigger_result.message}") rospy.logdebug(f"{self.node_name}'s HelloNode.stop_the_robot: got message {trigger_result.message}")
return trigger_result.success return trigger_result.success
def get_tool(self):
assert(self.tool is not None)
return self.tool
def main(self, node_name, node_topic_namespace, wait_for_first_pointcloud=True): def main(self, node_name, node_topic_namespace, wait_for_first_pointcloud=True):
rospy.init_node(node_name) rospy.init_node(node_name)
self.node_name = rospy.get_name() self.node_name = rospy.get_name()
@ -195,7 +204,9 @@ class HelloNode:
self.tf2_buffer = tf2_ros.Buffer() self.tf2_buffer = tf2_ros.Buffer()
self.tf2_listener = tf2_ros.TransformListener(self.tf2_buffer) self.tf2_listener = tf2_ros.TransformListener(self.tf2_buffer)
self.tool_subscriber = rospy.Subscriber('/tool', String, self.tool_callback)
self.point_cloud_subscriber = rospy.Subscriber('/camera/depth/color/points', PointCloud2, self.point_cloud_callback) self.point_cloud_subscriber = rospy.Subscriber('/camera/depth/color/points', PointCloud2, self.point_cloud_callback)
self.point_cloud_pub = rospy.Publisher('/' + node_topic_namespace + '/point_cloud2', PointCloud2, queue_size=1) self.point_cloud_pub = rospy.Publisher('/' + node_topic_namespace + '/point_cloud2', PointCloud2, queue_size=1)

+ 6
- 0
stretch_core/nodes/stretch_driver View File

@ -197,6 +197,11 @@ class StretchDriverNode:
mode_msg.data = self.robot_mode mode_msg.data = self.robot_mode
self.mode_pub.publish(mode_msg) self.mode_pub.publish(mode_msg)
# publish end of arm tool
tool_msg = String()
tool_msg.data = self.robot.end_of_arm.name
self.tool_pub.publish(tool_msg)
################################################## ##################################################
# publish joint state # publish joint state
joint_state = JointState() joint_state = JointState()
@ -508,6 +513,7 @@ class StretchDriverNode:
self.calibration_pub = rospy.Publisher('is_calibrated', Bool, queue_size=1) self.calibration_pub = rospy.Publisher('is_calibrated', Bool, queue_size=1)
self.homed_pub = rospy.Publisher('is_homed', Bool, queue_size=1) self.homed_pub = rospy.Publisher('is_homed', Bool, queue_size=1)
self.mode_pub = rospy.Publisher('mode', String, queue_size=1) self.mode_pub = rospy.Publisher('mode', String, queue_size=1)
self.tool_pub = rospy.Publisher('tool', String, queue_size=1)
self.imu_mobile_base_pub = rospy.Publisher('imu_mobile_base', Imu, queue_size=1) self.imu_mobile_base_pub = rospy.Publisher('imu_mobile_base', Imu, queue_size=1)
self.magnetometer_mobile_base_pub = rospy.Publisher('magnetometer_mobile_base', MagneticField, queue_size=1) self.magnetometer_mobile_base_pub = rospy.Publisher('magnetometer_mobile_base', MagneticField, queue_size=1)

+ 1
- 1
stretch_demos/nodes/grasp_object View File

@ -58,7 +58,7 @@ class GraspObjectNode(hm.HelloNode):
rospy.loginfo('trigger_result = {0}'.format(trigger_result)) rospy.loginfo('trigger_result = {0}'.format(trigger_result))
def look_at_surface(self, scan_time_s=None): def look_at_surface(self, scan_time_s=None):
self.manipulation_view = mp.ManipulationView(self.tf2_buffer, self.debug_directory)
self.manipulation_view = mp.ManipulationView(self.tf2_buffer, self.debug_directory, self.get_tool())
manip = self.manipulation_view manip = self.manipulation_view
head_settle_time_s = 0.02 #1.0 head_settle_time_s = 0.02 #1.0
manip.move_head(self.move_to_pose) manip.move_head(self.move_to_pose)

+ 10
- 5
stretch_funmap/src/stretch_funmap/manipulation_planning.py View File

@ -198,10 +198,15 @@ def detect_cliff(image, m_per_pix, m_per_height_unit, robot_xy_pix, display_text
class ManipulationView(): class ManipulationView():
def __init__(self, tf2_buffer, debug_directory=None):
def __init__(self, tf2_buffer, debug_directory=None, tool='tool_stretch_gripper'):
self.debug_directory = debug_directory self.debug_directory = debug_directory
print('ManipulationView __init__: self.debug_directory =', self.debug_directory) print('ManipulationView __init__: self.debug_directory =', self.debug_directory)
self.gripper_links = {
'tool_stretch_gripper': 'link_gripper',
'tool_stretch_dex_wrist': 'link_straight_gripper'
}
self.tool = tool
# Define the volume of interest for planning using the current # Define the volume of interest for planning using the current
# view. # view.
@ -420,7 +425,7 @@ class ManipulationView():
# The planar component of the link gripper x_axis is parallel # The planar component of the link gripper x_axis is parallel
# to the middle of the gripper, but points in the opposite # to the middle of the gripper, but points in the opposite
# direction. # direction.
gripper_frame = 'link_gripper'
gripper_frame = self.gripper_links[self.tool]
gripper_points_to_image_mat, ip_timestamp = h.get_points_to_image_mat(gripper_frame, tf2_buffer) gripper_points_to_image_mat, ip_timestamp = h.get_points_to_image_mat(gripper_frame, tf2_buffer)
# #
# forward_direction = np.array([1.0, 0.0, 0.0]) # forward_direction = np.array([1.0, 0.0, 0.0])
@ -469,7 +474,7 @@ class ManipulationView():
# The planar component of the link gripper x_axis is parallel # The planar component of the link gripper x_axis is parallel
# to the middle of the gripper, but points in the opposite # to the middle of the gripper, but points in the opposite
# direction. # direction.
gripper_frame = 'link_gripper'
gripper_frame = self.gripper_links[self.tool]
gripper_points_to_image_mat, ip_timestamp = h.get_points_to_image_mat(gripper_frame, tf2_buffer) gripper_points_to_image_mat, ip_timestamp = h.get_points_to_image_mat(gripper_frame, tf2_buffer)
# Obtain the gripper yaw axis location in the image by # Obtain the gripper yaw axis location in the image by
@ -582,7 +587,7 @@ class ManipulationView():
# The planar component of the link gripper x_axis is parallel # The planar component of the link gripper x_axis is parallel
# to the middle of the gripper, but points in the opposite # to the middle of the gripper, but points in the opposite
# direction. # direction.
gripper_frame = 'link_gripper'
gripper_frame = self.gripper_links[self.tool]
gripper_points_to_image_mat, ip_timestamp = h.get_points_to_image_mat(gripper_frame, tf2_buffer) gripper_points_to_image_mat, ip_timestamp = h.get_points_to_image_mat(gripper_frame, tf2_buffer)
# Obtain the gripper yaw axis location in the image by # Obtain the gripper yaw axis location in the image by

Loading…
Cancel
Save