def create_marker(path_msg, color_msg, description, path_id, width=0.1, delta_z=0.1):
int_marker = InteractiveMarker()
int_marker.header.frame_id = path_msg.header.frame_id
int_marker.name = str(path_id)
int_marker.description = "Path {0}".format(path_id)
# line_marker = Marker()
# line_marker.type = Marker.LINE_STRIP
# line_marker.scale.x = width
# line_marker.color = color_msg
# line_marker.points = [p.pose.position for p in path_msg.poses]
# for point in line_marker.points:
# point.z += delta_z
control = InteractiveMarkerControl()
control.always_visible = True
control.interaction_mode = InteractiveMarkerControl.MENU
# control.markers.append(line_marker)
points = [node(pose, delta_z) for pose in path_msg.poses]
for p1, p2 in zip(points[:-1], points[1:]):
control.markers.append(cylinder_between(p1, p2, color_msg, width))
for p in points:
control.markers.append(sphere_at(p, color_msg, width))
int_marker.controls.append(copy.deepcopy(control))
menu_handler = MenuHandler()
# put all the information in the main menu
#d = menu_handler.insert("Description")
for line in description:
menu_handler.insert(line)#, parent=d)
return menu_handler, int_marker
class WorldLandmark:
'''Class for representing objects'''
def __init__(self, remove_object_cb, pose, index, dimensions,
is_recognized):
'''
Args:
remove_object_cb (callback) : execute when removing objects
pose (Pose): Position of bounding box
index (int): For naming object in world (e.g. "thing 0")
dimensions (Vector3): Size of bounding box
is_recognized (bool): Result of object recognition.
'''
self._remove_object_cb = remove_object_cb
self._assigned_name = None
self.index = index
self.is_recognized = is_recognized
self.object = Landmark(self.get_name(), pose, dimensions)
self.menu_handler = MenuHandler()
self.int_marker = None
self.is_removed = False
self.menu_handler.insert('Remove from scene', callback=self.remove)
# ##################################################################
# Instance methods: Public (API)
# ##################################################################
def get_name(self):
'''Return this object's name.
Returns:
str
'''
if self._assigned_name is None:
if self.is_recognized:
return 'object ' + str(self.index)
else:
return 'thing ' + str(self.index)
else:
return self._assigned_name
def remove(self, feedback):
'''Function for removing object from the world.
Args:
feedback (InteractiveMarkerFeedback): Unused
'''
rospy.loginfo('Will remove object: ' + self.get_name())
self._remove_object_cb(self.object.name)
class WorldObject:
'''Class for representing objects'''
default_color = ColorRGBA(0.2, 0.8, 0.0, 0.6)
fake_color = ColorRGBA(0.4, 0.6, 0.4, 0.6)
selected_colors = [ColorRGBA(0.6, 0.6, 0.0, 0.6), ColorRGBA(0.6, 0.6, 0.2, 0.6)]
def __init__(self, pose, index, dimensions, is_recognized, is_fake=False):
''' Initialization of objects'''
self.index = index
self.assigned_name = None
self.is_recognized = is_recognized
self.object = Object(Object.TABLE_TOP, self.get_name(),
pose, dimensions)
self.menu_handler = MenuHandler()
self.int_marker = None
self.is_removed = False
self.menu_handler.insert('Remove from scene', callback=self.remove)
self.is_marker = False
self.is_fake = is_fake
def remove(self, dummy):
'''Function for removing object from the world'''
rospy.loginfo('Will remove object' + self.get_name())
self.is_removed = True
def assign_name(self, name):
'''Function for assigning a different name'''
self.assigned_name = name
self.object.name = name
def get_name(self):
'''Function to get the object name'''
if (self.assigned_name is None):
if (self.is_recognized):
return 'object' + str(self.index)
else:
return 'thing' + str(self.index)
else:
return self.assigned_name
def decrease_index(self):
'''Function to decrese object index'''
self.index -= 1
class WorldLandmark:
'''Class for representing objects'''
def __init__(self, pose, index, dimensions, is_recognized):
'''
Args:
pose (Pose): Position of bounding box
index (int): For naming object in world (e.g. "thing 0")
dimensions (Vector3): Size of bounding box
is_recognized (bool): Result of object recognition.
'''
self.index = index
self.assigned_name = None
self.is_recognized = is_recognized
self.object = Landmark(
Landmark.TABLE_TOP, self.get_name(), pose, dimensions)
self.menu_handler = MenuHandler()
self.int_marker = None
self.is_removed = False
self.menu_handler.insert('Remove from scene', callback=self.remove)
def get_name(self):
'''Return this object's name.
Returns:
str
'''
if self.assigned_name is None:
if self.is_recognized:
return 'object ' + str(self.index)
else:
return 'thing ' + str(self.index)
else:
return self.assigned_name
def remove(self, __):
'''Function for removing object from the world.
Args:
__ (???): Unused
'''
rospy.loginfo('Will remove object: ' + self.get_name())
self.is_removed = True
class RobocupInteractiveMarker(object):
def __init__(self, server):
self.server = server
self.pose = Pose()
self.publish = True
self.int_marker = None
self.make_marker()
self.menu_handler = MenuHandler()
item = self.menu_handler.insert("publish", callback=self.menu_callback)
self.menu_handler.setCheckState(item, MenuHandler.CHECKED)
self.menu_handler.apply(self.server, self.marker_name)
def feedback(self, feedback):
self.pose = feedback.pose
self.server.applyChanges()
def menu_callback(self, feedback):
item = feedback.menu_entry_id
if self.menu_handler.getCheckState(item) == MenuHandler.CHECKED:
self.menu_handler.setCheckState(item, MenuHandler.UNCHECKED)
self.publish = False
else:
self.publish = True
self.menu_handler.setCheckState(item, MenuHandler.CHECKED)
self.menu_handler.reApply(self.server)
self.server.applyChanges()
def make_marker(self):
self.int_marker = InteractiveMarker()
self.int_marker.header.frame_id = "map"
self.int_marker.pose = self.pose
self.int_marker.scale = 1
self.int_marker.name = self.marker_name
control = InteractiveMarkerControl()
control.orientation.w = math.sqrt(2) / 2
control.orientation.x = 0
control.orientation.y = math.sqrt(2) / 2
control.orientation.z = 0
control.interaction_mode = self.interaction_mode
self.int_marker.controls.append(copy.deepcopy(control))
# make a box which also moves in the plane
markers = self.make_individual_markers(self.int_marker)
for marker in markers:
control.markers.append(marker)
control.always_visible = True
self.int_marker.controls.append(control)
# we want to use our special callback function
self.server.insert(self.int_marker, self.feedback)
class WorldObject:
"""Class for representing objects"""
def __init__(self, pose, index, dimensions, is_recognized):
""" Initialization of objects"""
self.index = index
self.assigned_name = None
self.is_recognized = is_recognized
self.object = Object(Object.TABLE_TOP, self.get_name(), pose, dimensions)
self.menu_handler = MenuHandler()
self.int_marker = None
self.is_removed = False
self.menu_handler.insert("Remove from scene", callback=self.remove)
def remove(self, dummy):
"""Function for removing object from the world"""
rospy.loginfo("Will remove object" + self.get_name())
self.is_removed = True
def assign_name(self, name):
"""Function for assigning a different name"""
self.assigned_name = name
self.object.name = name
def get_name(self):
"""Function to get the object name"""
if self.assigned_name == None:
if self.is_recognized:
return "object" + str(self.index)
else:
return "thing" + str(self.index)
else:
return self.assigned_name
def decrease_index(self):
"""Function to decrese object index"""
self.index -= 1
class WorldObject:
'''Class for representing objects'''
def __init__(self, pose, index, dimensions, is_recognized):
''' Initialization of objects'''
self.index = index
self.assigned_name = None
self.is_recognized = is_recognized
self.object = Object(Object.TABLE_TOP, self.get_name(), pose,
dimensions)
self.menu_handler = MenuHandler()
self.int_marker = None
self.is_removed = False
self.menu_handler.insert('Remove from scene', callback=self.remove)
def remove(self, dummy):
'''Function for removing object from the world'''
rospy.loginfo('Will remove object' + self.get_name())
self.is_removed = True
def assign_name(self, name):
'''Function for assigning a different name'''
self.assigned_name = name
self.object.name = name
def get_name(self):
'''Function to get the object name'''
if (self.assigned_name == None):
if (self.is_recognized):
return 'object' + str(self.index)
else:
return 'thing' + str(self.index)
else:
return self.assigned_name
def decrease_index(self):
'''Function to decrese object index'''
self.index -= 1
class WorldObject:
'''Class for representing objects'''
def __init__(self, pose, index, dimensions, marker_id=None):
''' Initialization of objects'''
self.index = index
self.assigned_name = None
self.position = pose.position
self.object = Object(Object.TABLE_TOP, self.get_name(),
pose, dimensions)
self.menu_handler = MenuHandler()
self.int_marker = None
self.is_removed = False
self.menu_handler.insert('Remove from scene', callback=self.remove)
self.marker_id = marker_id
def remove(self, dummy):
'''Function for removing object from the world'''
rospy.loginfo('Will remove object' + self.get_name())
self.is_removed = True
def assign_name(self, name):
'''Function for assigning a different name'''
self.assigned_name = name
self.object.name = name
def get_name(self):
'''Function to get the object name'''
if (self.assigned_name == None):
return 'marker' + str(self.marker_id)
else:
return self.assigned_name
def decrease_index(self):
'''Function to decrese object index'''
self.index -= 1
class GripperMarkers:
_offset = 0.09
def __init__(self, side_prefix):
self.ik = IK(side_prefix)
self.side_prefix = side_prefix
self._im_server = InteractiveMarkerServer('ik_request_markers')
self._tf_listener = TransformListener()
self._menu_handler = MenuHandler()
self._menu_handler.insert('Move arm here', callback=self.move_to_pose_cb)
self.is_control_visible = False
self.ee_pose = self.get_ee_pose()
self.update_viz()
self._menu_handler.apply(self._im_server, 'ik_target_marker')
self._im_server.applyChanges()
# Code for moving the robots joints
switch_srv_name = 'pr2_controller_manager/switch_controller'
rospy.loginfo('Waiting for switch controller service...')
rospy.wait_for_service(switch_srv_name)
self.switch_service_client = rospy.ServiceProxy(switch_srv_name,
SwitchController)
self.r_joint_names = ['r_shoulder_pan_joint',
'r_shoulder_lift_joint',
'r_upper_arm_roll_joint',
'r_elbow_flex_joint',
'r_forearm_roll_joint',
'r_wrist_flex_joint',
'r_wrist_roll_joint']
self.l_joint_names = ['l_shoulder_pan_joint',
'l_shoulder_lift_joint',
'l_upper_arm_roll_joint',
'l_elbow_flex_joint',
'l_forearm_roll_joint',
'l_wrist_flex_joint',
'l_wrist_roll_joint']
# Create a trajectory action client
r_traj_controller_name = '/r_arm_controller/joint_trajectory_action'
self.r_traj_action_client = SimpleActionClient(r_traj_controller_name, JointTrajectoryAction)
rospy.loginfo('Waiting for a response from the trajectory action server for RIGHT arm...')
self.r_traj_action_client.wait_for_server()
l_traj_controller_name = '/l_arm_controller/joint_trajectory_action'
self.l_traj_action_client = SimpleActionClient(l_traj_controller_name, JointTrajectoryAction)
rospy.loginfo('Waiting for a response from the trajectory action server for LEFT arm...')
self.l_traj_action_client.wait_for_server()
def get_ee_pose(self):
from_frame = 'base_link'
to_frame = self.side_prefix + '_wrist_roll_link'
try:
t = self._tf_listener.getLatestCommonTime(from_frame, to_frame)
(pos, rot) = self._tf_listener.lookupTransform(from_frame, to_frame, t)
except:
rospy.logerr('Could not get end effector pose through TF.')
pos = [1.0, 0.0, 1.0]
rot = [0.0, 0.0, 0.0, 1.0]
return Pose(Point(pos[0], pos[1], pos[2]),
Quaternion(rot[0], rot[1], rot[2], rot[3]))
def move_to_pose_cb(self, dummy):
rospy.loginfo('You pressed the `Move arm here` button from the menu.')
joint_positions = self.ik.get_ik_for_ee(self.ee_pose)
print 'joint: ' + str(joint_positions)
if(joint_positions != None):
self.toggle_arm(self.side_prefix, 'Freeze', False)
#joint_positions = [-0.993560463247, -0.357041076593, 0.0534981848008, -1.24934444608, -3.10068096283, -1.7906747183, -28.4704855309]
print 'returned from IK: ' + str(joint_positions)
#print 'random position : ' + str([-0.993560463247, -0.357041076593, 0.0534981848008, -1.24934444608, -3.10068096283, -1.7906747183, -28.4704855309])
#joint_positions = [-1.03129153, -0.35312086, -0.08, -1.25402906, -2.98718287, -1.7816027, 3.03965124]
self.move_to_joints(self.side_prefix, list(joint_positions), 5.0)
print 'done'
def toggle_arm(self, side, toggle, button):
controller_name = side + '_arm_controller'
print 'toggle'
start_controllers = []
stop_controllers = []
if (toggle == 'Relax'):
stop_controllers.append(controller_name)
else:
start_controllers.append(controller_name)
self.set_arm_mode(start_controllers, stop_controllers)
def set_arm_mode(self, start_controllers, stop_controllers):
try:
self.switch_service_client(start_controllers, stop_controllers, 1)
except rospy.ServiceException:
rospy.logerr('Could not change arm mode.')
def move_to_joints(self, side_prefix, positions, time_to_joint):
'''Moves the arm to the desired joints'''
velocities = [0] * len(positions)
traj_goal = JointTrajectoryGoal()
traj_goal.trajectory.header.stamp = (rospy.Time.now() + rospy.Duration(0.1))
traj_goal.trajectory.points.append(JointTrajectoryPoint(positions=positions,
velocities=velocities, time_from_start=rospy.Duration(time_to_joint)))
if (side_prefix == 'r'):
traj_goal.trajectory.joint_names = self.r_joint_names
self.r_traj_action_client.send_goal(traj_goal)
else:
traj_goal.trajectory.joint_names = self.l_joint_names
self.l_traj_action_client.send_goal(traj_goal)
def marker_clicked_cb(self, feedback):
if feedback.event_type == InteractiveMarkerFeedback.POSE_UPDATE:
self.ee_pose = feedback.pose
self.update_viz()
self._menu_handler.reApply(self._im_server)
self._im_server.applyChanges()
elif feedback.event_type == InteractiveMarkerFeedback.BUTTON_CLICK:
rospy.loginfo('Changing visibility of the pose controls.')
if (self.is_control_visible):
self.is_control_visible = False
else:
self.is_control_visible = True
else:
rospy.loginfo('Unhandled event: ' + str(feedback.event_type))
def update_viz(self):
menu_control = InteractiveMarkerControl()
menu_control.interaction_mode = InteractiveMarkerControl.BUTTON
menu_control.always_visible = True
frame_id = 'base_link'
pose = self.ee_pose
menu_control = self._add_gripper_marker(menu_control)
text_pos = Point()
text_pos.x = pose.position.x
text_pos.y = pose.position.y
text_pos.z = pose.position.z + 0.1
text = 'x=' + str(pose.position.x) + ' y=' + str(pose.position.y) + ' x=' + str(pose.position.z)
menu_control.markers.append(Marker(type=Marker.TEXT_VIEW_FACING,
id=0, scale=Vector3(0, 0, 0.03),
text=text,
color=ColorRGBA(0.0, 0.0, 0.0, 0.5),
header=Header(frame_id=frame_id),
pose=Pose(text_pos, Quaternion(0, 0, 0, 1))))
int_marker = InteractiveMarker()
int_marker.name = 'ik_target_marker'
int_marker.header.frame_id = frame_id
int_marker.pose = pose
int_marker.scale = 0.2
self._add_6dof_marker(int_marker)
int_marker.controls.append(menu_control)
self._im_server.insert(int_marker, self.marker_clicked_cb)
def _add_gripper_marker(self, control):
is_hand_open=False
if is_hand_open:
angle = 28 * numpy.pi / 180.0
else:
angle = 0
transform1 = tf.transformations.euler_matrix(0, 0, angle)
transform1[:3, 3] = [0.07691 - GripperMarkers._offset, 0.01, 0]
transform2 = tf.transformations.euler_matrix(0, 0, -angle)
transform2[:3, 3] = [0.09137, 0.00495, 0]
t_proximal = transform1
t_distal = tf.transformations.concatenate_matrices(transform1, transform2)
mesh1 = self._make_mesh_marker()
mesh1.mesh_resource = ('package://pr2_description/meshes/gripper_v0/gripper_palm.dae')
mesh1.pose.position.x = -GripperMarkers._offset
mesh1.pose.orientation.w = 1
mesh2 = self._make_mesh_marker()
mesh2.mesh_resource = ('package://pr2_description/meshes/gripper_v0/l_finger.dae')
mesh2.pose = GripperMarkers.get_pose_from_transform(t_proximal)
mesh3 = self._make_mesh_marker()
mesh3.mesh_resource = ('package://pr2_description/meshes/gripper_v0/l_finger_tip.dae')
mesh3.pose = GripperMarkers.get_pose_from_transform(t_distal)
quat = tf.transformations.quaternion_multiply(
tf.transformations.quaternion_from_euler(numpy.pi, 0, 0),
tf.transformations.quaternion_from_euler(0, 0, angle))
transform1 = tf.transformations.quaternion_matrix(quat)
transform1[:3, 3] = [0.07691 - GripperMarkers._offset, -0.01, 0]
transform2 = tf.transformations.euler_matrix(0, 0, -angle)
transform2[:3, 3] = [0.09137, 0.00495, 0]
t_proximal = transform1
t_distal = tf.transformations.concatenate_matrices(transform1,transform2)
mesh4 = self._make_mesh_marker()
mesh4.mesh_resource = ('package://pr2_description/meshes/gripper_v0/l_finger.dae')
mesh4.pose = GripperMarkers.get_pose_from_transform(t_proximal)
mesh5 = self._make_mesh_marker()
mesh5.mesh_resource = ('package://pr2_description/meshes/gripper_v0/l_finger_tip.dae')
mesh5.pose = GripperMarkers.get_pose_from_transform(t_distal)
control.markers.append(mesh1)
control.markers.append(mesh2)
control.markers.append(mesh3)
control.markers.append(mesh4)
control.markers.append(mesh5)
return control
@staticmethod
def get_pose_from_transform(transform):
pos = transform[:3,3].copy()
rot = tf.transformations.quaternion_from_matrix(transform)
return Pose(Point(pos[0], pos[1], pos[2]),
Quaternion(rot[0], rot[1], rot[2], rot[3]))
def _make_mesh_marker(self):
mesh = Marker()
mesh.mesh_use_embedded_materials = False
mesh.type = Marker.MESH_RESOURCE
mesh.scale.x = 1.0
mesh.scale.y = 1.0
mesh.scale.z = 1.0
mesh.color = ColorRGBA(0.0, 0.5, 1.0, 0.6)
ik_solution = self.ik.get_ik_for_ee(self.ee_pose)
if (ik_solution == None):
mesh.color = ColorRGBA(1.0, 0.0, 0.0, 0.6)
else:
mesh.color = ColorRGBA(0.0, 1.0, 0.0, 0.6)
return mesh
def _add_6dof_marker(self, int_marker):
is_fixed = True
control = self._make_6dof_control('rotate_x', Quaternion(1, 0, 0, 1), False, is_fixed)
int_marker.controls.append(control)
control = self._make_6dof_control('move_x', Quaternion(1, 0, 0, 1), True, is_fixed)
int_marker.controls.append(control)
control = self._make_6dof_control('rotate_z', Quaternion(0, 1, 0, 1), False, is_fixed)
int_marker.controls.append(control)
control = self._make_6dof_control('move_z', Quaternion(0, 1, 0, 1), True, is_fixed)
int_marker.controls.append(control)
control = self._make_6dof_control('rotate_y', Quaternion(0, 0, 1, 1), False, is_fixed)
int_marker.controls.append(control)
control = self._make_6dof_control('move_y', Quaternion(0, 0, 1, 1), True, is_fixed)
int_marker.controls.append(control)
def _make_6dof_control(self, name, orientation, is_move, is_fixed):
control = InteractiveMarkerControl()
control.name = name
control.orientation = orientation
control.always_visible = False
if (self.is_control_visible):
if is_move:
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
else:
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
else:
control.interaction_mode = InteractiveMarkerControl.NONE
if is_fixed:
control.orientation_mode = InteractiveMarkerControl.FIXED
return control
class IntCollisionEnv(CollisionEnvServices):
def __init__(self, setup, world_frame):
super(IntCollisionEnv, self).__init__(setup, world_frame)
self.im_server = InteractiveMarkerServer(self.NS + "markers")
self.check_attached_timer = rospy.Timer(rospy.Duration(0.1),
self.check_attached_timer_cb)
self.create_menus()
def create_menus(self):
self.global_menu_handler = MenuHandler()
g_art = self.global_menu_handler.insert("Artificial")
self.global_menu_handler.insert("Add primitive",
parent=g_art,
callback=self.global_menu_add_prim_cb)
self.global_menu_handler.insert("Clear all",
parent=g_art,
callback=self.global_menu_clear_all_cb)
self.global_menu_handler.insert(
"Clear all (permanent)",
parent=g_art,
callback=self.global_menu_clear_all_perm_cb)
self.global_menu_handler.insert("Save all",
parent=g_art,
callback=self.global_menu_save_all_cb)
self.global_menu_handler.insert("Reload",
parent=g_art,
callback=self.global_menu_reload_cb)
g_det = self.global_menu_handler.insert("Detected")
self.global_menu_handler.insert("Pause",
parent=g_det,
callback=self.global_menu_pause_cb)
self.global_menu_handler.insert(
"Clear all",
parent=g_det,
callback=self.global_menu_det_clear_all_cb)
self.a_obj_menu_handler = MenuHandler()
mov = self.a_obj_menu_handler.insert("Moveable",
callback=self.menu_moveable_cb)
self.a_obj_menu_handler.setCheckState(mov, MenuHandler.UNCHECKED)
sc = self.a_obj_menu_handler.insert("Scaleable",
callback=self.menu_scaleable_cb)
self.a_obj_menu_handler.setCheckState(sc, MenuHandler.UNCHECKED)
self.a_obj_menu_handler.insert("Save", callback=self.menu_save_cb)
self.a_obj_menu_handler.insert("Clear", callback=self.menu_remove_cb)
self.d_obj_menu_handler = MenuHandler()
self.d_obj_menu_handler.insert("Clear", callback=self.d_menu_clear_cb)
int_marker = InteractiveMarker()
int_marker.header.frame_id = self.world_frame
int_marker.pose.position.z = 1.2
int_marker.scale = 0.5
int_marker.name = "global_menu"
int_marker.description = "Global Menu"
control = InteractiveMarkerControl()
control.interaction_mode = InteractiveMarkerControl.BUTTON
control.always_visible = True
marker = Marker()
marker.type = Marker.CUBE
marker.scale.x = 0.05
marker.scale.y = 0.05
marker.scale.z = 0.05
marker.color.r = 0.5
marker.color.g = 0.5
marker.color.b = 0.5
marker.color.a = 0.5
control.markers.append(marker)
int_marker.controls.append(control)
self.im_server.insert(int_marker, self.ignored_cb)
self.global_menu_handler.apply(self.im_server, int_marker.name)
self.im_server.applyChanges()
def ignored_cb(self, feedback):
pass
def d_menu_clear_cb(self, f):
pass
def global_menu_det_clear_all_cb(self, f):
for name in self.clear_all_det():
self.im_server.erase(name)
self.im_server.applyChanges()
def global_menu_pause_cb(self, f):
handle = f.menu_entry_id
state = self.global_menu_handler.getCheckState(handle)
if state == MenuHandler.CHECKED:
self.global_menu_handler.setCheckState(handle,
MenuHandler.UNCHECKED)
self.paused = False
else:
self.global_menu_handler.setCheckState(handle, MenuHandler.CHECKED)
self.paused = True
self.global_menu_handler.reApply(self.im_server)
self.im_server.applyChanges()
def global_menu_save_all_cb(self, f):
self.save_primitives()
def global_menu_add_prim_cb(self, feedback):
p = CollisionPrimitive()
p.name = self._generate_name()
p.bbox.type = SolidPrimitive.BOX
p.bbox.dimensions = [0.1, 0.1, 0.1]
p.pose.header.frame_id = self.world_frame
p.pose.pose.orientation.w = 1
p.pose.pose.position.z = 0.5
p.setup = self.setup
self.add_primitive(p)
def global_menu_reload_cb(self, feedback):
self.reload()
def global_menu_clear_all_cb(self, feedback):
self.clear_all(permanent=False)
def global_menu_clear_all_perm_cb(self, feedback):
self.clear_all()
def menu_save_cb(self, f):
self.save_primitive(f.marker_name)
def menu_scaleable_cb(self, f):
handle = f.menu_entry_id
state = self.a_obj_menu_handler.getCheckState(handle)
if state == MenuHandler.CHECKED:
self.a_obj_menu_handler.setCheckState(handle,
MenuHandler.UNCHECKED)
# TODO
else:
self.a_obj_menu_handler.setCheckState(handle, MenuHandler.CHECKED)
# TODO
self.a_obj_menu_handler.reApply(self.im_server)
self.im_server.applyChanges()
def menu_moveable_cb(self, feedback):
handle = feedback.menu_entry_id
state = self.a_obj_menu_handler.getCheckState(handle)
if state == MenuHandler.CHECKED:
self.a_obj_menu_handler.setCheckState(handle,
MenuHandler.UNCHECKED)
# if feedback.marker_name in self.moveable:
# self.moveable.remove(feedback.marker_name)
self.im_server.erase(feedback.marker_name)
self.im_server.insert(
make_def(self.artificial_objects[feedback.marker_name]),
self.process_im_feedback)
else:
self.a_obj_menu_handler.setCheckState(handle, MenuHandler.CHECKED)
self.make_interactive(
self.artificial_objects[feedback.marker_name])
self.a_obj_menu_handler.reApply(self.im_server)
self.im_server.applyChanges()
def menu_remove_cb(self, feedback):
self.remove_name(feedback.marker_name)
def process_im_feedback(self, feedback):
if feedback.event_type == InteractiveMarkerFeedback.POSE_UPDATE:
ps = PoseStamped()
ps.header = feedback.header
ps.pose = feedback.pose
if feedback.marker_name in self.artificial_objects:
self.set_primitive_pose(feedback.marker_name, ps)
elif feedback.marker_name:
# detected objects
pass
def make_interactive(self, p):
im = self.im_server.get(p.name)
for v in (("x", (1, 0, 0, 1)), ("y", (0, 0, 1, 1)), ("z", (0, 1, 0,
1))):
im.controls.append(rotate_axis_control("rotate_" + v[0], v[1]))
im.controls.append(move_axis_control("move_" + v[0], v[1]))
def remove_name(self, name):
super(IntCollisionEnv, self).remove_name(name)
self.im_server.erase(name)
self.im_server.applyChanges()
def add_primitive(self, p):
super(IntCollisionEnv, self).add_primitive(p)
self.im_server.insert(make_def(p), self.process_im_feedback)
self.a_obj_menu_handler.apply(self.im_server, p.name)
self.im_server.applyChanges()
def reload(self):
self.im_server.clear()
self.create_menus()
super(IntCollisionEnv, self).reload()
def add_detected(self, name, ps, object_type):
super(IntCollisionEnv, self).add_detected(name, ps, object_type)
p = CollisionPrimitive()
p.name = name
p.pose = ps
p.bbox = object_type.bbox
self.im_server.insert(make_def(p, color=(0, 0, 1)),
self.process_im_feedback)
self.im_server.applyChanges()
def clear_detected(self, name):
super(IntCollisionEnv, self).clear_detected(name)
self.im_server.erase(name)
self.im_server.applyChanges()
def check_attached_timer_cb(self, evt):
for name, ps, bbox in self.get_attached():
p = CollisionPrimitive()
p.name = name
p.pose = ps
p.bbox = bbox
self.im_server.insert(make_def(p, color=(1, 0, 0)),
self.process_im_feedback)
self.im_server.applyChanges()
class ActionStepMarker:
'''Marker for visualizing the steps of an action.'''
_im_server = None
_offset = DEFAULT_OFFSET
_ref_object_list = None
_ref_names = None
_marker_click_cb = None
def __init__(self, step_number, arm_index, action_step, marker_click_cb):
'''
Args:
step_number (int): The 1-based index of the step.
arm_index (int): Side.RIGHT or Side.LEFT
action_step (ActionStep): The action step this marker marks.
marker_click_cb (function(int,bool)): The function to call
when a marker is clicked. Pass the uid of the marker
(as calculated by get_uid(...) as well as whether it's
selected.
'''
if ActionStepMarker._im_server is None:
im_server = InteractiveMarkerServer(TOPIC_IM_SERVER)
ActionStepMarker._im_server = im_server
self.action_step = action_step
self.arm_index = arm_index
self.arm_name = ARM_NAMES[arm_index]
self.step_number = step_number
self.is_requested = False
self.is_deleted = False
self.is_control_visible = False
self.is_edited = False
self.has_object = False
self._sub_entries = None
self._menu_handler = None
self._prev_is_reachable = None
ActionStepMarker._marker_click_cb = marker_click_cb
# ##################################################################
# Static methods: Public (API)
# ##################################################################
@staticmethod
def calc_uid(arm_index, step_number):
'''Returns a unique id of the marker of the arm_index arm with
step_number step.
Args:
arm_index (int): Side.RIGHT or Side.LEFT
step_number (int): The number of the step.
Returns:
int: A number that is unique given the step number and arm
index.
'''
return len(ARM_NAMES) * step_number + arm_index
# ##################################################################
# Static methods: Internal ("private")
# ##################################################################
@staticmethod
def _make_sphere_marker(uid, pose, frame_id, radius):
'''Creates and returns a sphere marker.
Args:
uid (int): The id to use for the marker.
pose (Pose): The pose of the marker.
frame_id (str): Frame the marker is associated with. (See
std_msgs/Header.msg for more info.)
radius (float): Amount to scale the marker by. Scales in all
directions (x, y, and z).
Returns:
Marker
'''
return Marker(
type=Marker.SPHERE,
id=uid,
lifetime=rospy.Duration(2),
scale=Vector3(radius, radius, radius),
pose=pose,
header=Header(frame_id=frame_id),
color=COLOR_TRAJ_ENDPOINT_SPHERES
)
@staticmethod
def _offset_pose(pose, constant=1):
'''Offsets the world pose for visualization.
Args:
pose (Pose): The pose to offset.
constant (int, optional): How much to scale the set offset
by (scales ActionStepMarker._offset). Defaults to 1.
Returns:
Pose: The offset pose.
'''
transform = World.get_matrix_from_pose(pose)
offset_array = [constant * ActionStepMarker._offset, 0, 0]
offset_transform = tf.transformations.translation_matrix(offset_array)
hand_transform = tf.transformations.concatenate_matrices(
transform, offset_transform)
return World.get_pose_from_transform(hand_transform)
# ##################################################################
# Instance methods: Public (API)
# ##################################################################
def get_uid(self):
'''Returns a unique id for this marker.
Returns:
int: A number that is unique given the step number and arm
index.
'''
return ActionStepMarker.calc_uid(self.arm_index, self.step_number)
def decrease_id(self):
'''Reduces the step index of the marker.'''
self.step_number -= 1
self._update_menu()
def update_ref_frames(self, ref_frame_list):
'''Updates and re-assigns coordinate frames when the world changes.
Args:
ref_frame_list ([Object]): List of Object.msg objects, the
reference frames of the system.
'''
# There is a new list of objects. If the current frame is
# relative (already assigned to an object) we need to figure out
# the correspondences.
ActionStepMarker._ref_object_list = ref_frame_list
arm_pose = self.get_target()
if arm_pose.refFrame == ArmState.OBJECT:
prev_ref_obj = arm_pose.refFrameObject
new_ref_obj = World.get_most_similar_obj(
prev_ref_obj, ref_frame_list)
if new_ref_obj is not None:
self.has_object = True
arm_pose.refFrameObject = new_ref_obj
else:
self.has_object = False
# Re-populate cached list of reference names.
ActionStepMarker._ref_names = [BASE_LINK]
for obj in ActionStepMarker._ref_object_list:
ActionStepMarker._ref_names.append(obj.name)
self._update_menu()
def destroy(self):
'''Removes marker from the world.'''
ActionStepMarker._im_server.erase(self._get_name())
ActionStepMarker._im_server.applyChanges()
def set_new_pose(self, new_pose):
'''Changes the pose of the action step to new_pose.
Args:
new_pose (Pose)
'''
if self.action_step.type == ActionStep.ARM_TARGET:
t = self.action_step.armTarget
arm = t.rArm if self.arm_index == Side.RIGHT else t.lArm
arm.ee_pose = ActionStepMarker._offset_pose(new_pose, -1)
self.update_viz()
elif self.action_step.type == ActionStep.ARM_TRAJECTORY:
rospy.logwarn(
'Modification of whole trajectory segments is not ' +
'implemented.')
def get_absolute_position(self, is_start=True):
'''Returns the absolute position of the action step.
Args:
is_start (bool, optional). For trajectories only. Whether to
get the final position in the trajectory. Defaults to
True.
Returns:
Point
'''
return self.get_absolute_pose(is_start).position
def get_absolute_pose(self, is_start=True):
'''Returns the absolute pose of the action step.
Args:
is_start (bool, optional). For trajectories only. Whether to
get the final pose in the trajectory. Defaults to True.
Returns:
Pose
'''
if self.action_step.type == ActionStep.ARM_TARGET:
# "Normal" saved pose.
t = self.action_step.armTarget
arm_pose = t.rArm if self.arm_index == Side.RIGHT else t.lArm
elif self.action_step.type == ActionStep.ARM_TRAJECTORY:
# Trajectory.
t = self.action_step.armTrajectory
arm = t.rArm if self.arm_index == Side.RIGHT else t.lArm
# TODO(mbforbes): Make sure this isn't a bug in the original
# implementation. Wouldn't is_start imply you want the first
# one?
index = len(arm) - 1 if is_start else 0
arm_pose = arm[index]
# TODO(mbforbes): Figure out if there are cases that require
# this, or remove.
# if (arm_pose.refFrame == ArmState.OBJECT and
# World.has_object(arm_pose.refFrameObject.name)):
# return ActionStepMarker._offset_pose(arm_pose.ee_pose)
# else:
world_pose = World.get_absolute_pose(arm_pose)
return ActionStepMarker._offset_pose(world_pose)
def get_pose(self):
'''Returns the pose of the action step.
Returns:
Pose
'''
target = self.get_target()
if target is not None:
return ActionStepMarker._offset_pose(target.ee_pose)
def set_target(self, target):
'''Sets the new ArmState for this action step.
Args:
target (ArmState): Replacement for this target.
'''
if self.action_step.type == ActionStep.ARM_TARGET:
at = self.action_step.armTarget
if self.arm_index == Side.RIGHT:
at.rArm = target
else:
at.lArm = target
# TODO(mbforbes): Why is self.has_object set to True here?
self.has_object = True
self._update_menu()
self.is_edited = False
def get_target(self, traj_index=None):
'''Returns the ArmState for this action step.
Args:
traj_index (int, optional): Which step in the trajectory to
return the ArmState for. Only applicable for
trajectories (not "normal" saved poses). Defaults to
None, in which case the middle point is used.
Returns:
ArmState
'''
if self.action_step.type == ActionStep.ARM_TARGET:
t = self.action_step.armTarget
return t.rArm if self.arm_index == Side.RIGHT else t.lArm
elif self.action_step.type == ActionStep.ARM_TRAJECTORY:
t = self.action_step.armTrajectory
arm = t.rArm if self.arm_index == Side.RIGHT else t.lArm
# If traj_index not passed, use the middle one.
if traj_index is None:
traj_index = int(len(arm) / 2)
return arm[traj_index]
def update_viz(self):
'''Updates visualization fully.'''
self._update_viz_core()
self._menu_handler.reApply(ActionStepMarker._im_server)
ActionStepMarker._im_server.applyChanges()
def pose_reached(self):
'''Update when a requested pose is reached.'''
self.is_requested = False
def change_ref_cb(self, feedback):
'''Callback for when a reference frame change is requested.
Args:
feedback (InteractiveMarkerFeedback (?))
'''
self._menu_handler.setCheckState(
self._get_menu_id(self._get_ref_name()), MenuHandler.UNCHECKED)
self._menu_handler.setCheckState(
feedback.menu_entry_id, MenuHandler.CHECKED)
new_ref = self._get_menu_name(feedback.menu_entry_id)
self._set_ref(new_ref)
rospy.loginfo(
'Switching reference frame to ' + new_ref + ' for action step ' +
self._get_name())
self._menu_handler.reApply(ActionStepMarker._im_server)
ActionStepMarker._im_server.applyChanges()
self.update_viz()
def marker_feedback_cb(self, feedback):
'''Callback for when an event occurs on the marker.
Args:
feedback (InteractiveMarkerFeedback)
'''
if feedback.event_type == InteractiveMarkerFeedback.POSE_UPDATE:
self.set_new_pose(feedback.pose)
self.update_viz()
elif feedback.event_type == InteractiveMarkerFeedback.BUTTON_CLICK:
# Set the visibility of the 6DOF controller.
# This happens a ton, and doesn't need to be logged like
# normal events (e.g. clicking on most marker controls
# fires here).
rospy.logdebug('Changing visibility of the pose controls.')
self.is_control_visible = not self.is_control_visible
ActionStepMarker._marker_click_cb(
self.get_uid(), self.is_control_visible)
else:
# This happens a ton, and doesn't need to be logged like
# normal events (e.g. clicking on most marker controls
# fires here).
rospy.logdebug('Unknown event: ' + str(feedback.event_type))
# TODO(mbforbes): Figure out types of objects sent to these
# callbacks.
def delete_step_cb(self, __):
'''Callback for when delete is requested.
Args:
__ (???): Unused
'''
self.is_deleted = True
def move_to_cb(self, __):
'''Callback for when moving to a pose is requested.
Args:
__ (???): Unused
'''
self.is_requested = True
def move_pose_to_cb(self, __):
'''Callback for when a pose change to current is requested.
Args:
__ (???): Unused
'''
self.is_edited = True
# ##################################################################
# Instance methods: Internal ("private")
# ##################################################################
def _is_reachable(self):
'''Checks and returns whether there is an IK solution for this
action step.
Returns:
bool: Whether this action step is reachable.
'''
dummy, is_reachable = Arms.solve_ik_for_arm(
self.arm_index, self.get_target())
# A bit more complicated logging to avoid spamming the logs
# while still giving useful info. It now logs when reachability
# is first calculated, or changes.
report = False
# See if we've set reachability for the first time.
if self._prev_is_reachable is None:
report = True
reachable_str = (
'is reachable' if is_reachable else 'is not reachable')
# See if we've got a different reachability than we had before.
elif self._prev_is_reachable != is_reachable:
report = True
reachable_str = (
'is now reachable' if is_reachable else
'is no longer reachable')
# Log if it's changed.
if report:
rospy.loginfo(
'Pose (' + str(self.step_number) + ', ' + self.arm_name
+ ') ' + reachable_str)
# Cache and return.
self._prev_is_reachable = is_reachable
return is_reachable
def _get_name(self):
'''Generates the unique name for the marker.
Returns:
str: A human-readable unique name for the marker.
'''
return 'step' + str(self.step_number) + 'arm' + str(self.arm_index)
def _update_menu(self):
'''Recreates the menu when something has changed.'''
self._menu_handler = MenuHandler()
# Insert sub entries.
self._sub_entries = []
frame_entry = self._menu_handler.insert(MENU_OPTIONS['ref'])
refs = ActionStepMarker._ref_names
for ref in refs:
subent = self._menu_handler.insert(
ref, parent=frame_entry, callback=self.change_ref_cb)
self._sub_entries += [subent]
# Inset main menu entries.
self._menu_handler.insert(
MENU_OPTIONS['move_here'], callback=self.move_to_cb)
self._menu_handler.insert(
MENU_OPTIONS['move_current'], callback=self.move_pose_to_cb)
self._menu_handler.insert(
MENU_OPTIONS['del'], callback=self.delete_step_cb)
# Make all unchecked to start.
for subent in self._sub_entries:
self._menu_handler.setCheckState(subent, MenuHandler.UNCHECKED)
# Check if necessary.
menu_id = self._get_menu_id(self._get_ref_name())
if menu_id is None:
self.has_object = False
else:
self._menu_handler.setCheckState(menu_id, MenuHandler.CHECKED)
# Update.
self._update_viz_core()
self._menu_handler.apply(ActionStepMarker._im_server, self._get_name())
ActionStepMarker._im_server.applyChanges()
def _get_menu_id(self, ref_name):
'''Returns the unique menu id from its name or None if the
object is not found.
Returns:
int (?)|None
'''
if ref_name in ActionStepMarker._ref_names:
index = ActionStepMarker._ref_names.index(ref_name)
return self._sub_entries[index]
else:
return None
def _get_menu_name(self, menu_id):
'''Returns the menu name from its unique menu id.
Returns:
str
'''
index = self._sub_entries.index(menu_id)
return ActionStepMarker._ref_names[index]
def _get_ref_name(self):
'''Returns the name string for the reference frame object of the
action step.
Returns:
str|None: Under all normal circumstances, returns the str
reference frame name. Returns None in error.
'''
ref_name = None
if self.action_step.type == ActionStep.ARM_TARGET:
# "Normal" step (saved pose).
t = self.action_step.armTarget
arm = t.rArm if self.arm_index == Side.RIGHT else t.lArm
ref_frame = arm.refFrame
ref_name = arm.refFrameObject.name
elif self.action_step.type == ActionStep.ARM_TRAJECTORY:
# "Trajectory" step.
t = self.action_step.armTrajectory
if self.arm_index == Side.RIGHT:
ref_frame = t.rRefFrame
ref_name = t.rRefFrameObject.name
else:
ref_frame = t.lRefFrame
ref_name = t.lRefFrameObject.name
else:
rospy.logerr(
'Unhandled marker type: ' + str(self.action_step.type))
# Update ref frame name if it's absolute.
if ref_frame == ArmState.ROBOT_BASE:
ref_name = BASE_LINK
return ref_name
def _set_ref(self, new_ref_name):
'''Changes the reference frame of the action step to
new_ref_name.
Args:
new_ref_name
'''
# Get the id of the new ref (an int).
new_ref = World.get_ref_from_name(new_ref_name)
if new_ref != ArmState.ROBOT_BASE:
index = ActionStepMarker._ref_names.index(new_ref_name)
new_ref_obj = ActionStepMarker._ref_object_list[index - 1]
else:
new_ref_obj = Object()
if self.action_step.type == ActionStep.ARM_TARGET:
# Handle "normal" steps (saved poses).
t = self.action_step.armTarget
if self.arm_index == Side.RIGHT:
t.rArm = World.convert_ref_frame(t.rArm, new_ref, new_ref_obj)
else:
t.lArm = World.convert_ref_frame(t.lArm, new_ref, new_ref_obj)
elif self.action_step.type == ActionStep.ARM_TRAJECTORY:
# Handle trajectory steps.
t = self.action_step.armTrajectory
arm = t.rArm if self.arm_index == Side.RIGHT else t.lArm
for i in range(len(arm)):
arm_old = arm[i]
arm_new = World.convert_ref_frame(
arm_old, new_ref, new_ref_obj)
arm[i] = arm_new
# Fix up reference frames.
if self.arm_index == Side.RIGHT:
t.rRefFrameObject = new_ref_obj
t.rRefFrame = new_ref
else:
t.lRefFrameObject = new_ref_obj
t.lRefFrame = new_ref
def _is_hand_open(self):
'''Returns whether the gripper is open for this action step.
Returns:
bool
'''
ga = self.action_step.gripperAction
gstate = ga.rGripper if self.arm_index == Side.RIGHT else ga.lGripper
return gstate.state == GripperState.OPEN
def _get_traj_pose(self, index):
'''Returns this trajectory's pose at index. Only applicable for
trajectories.
Args:
index (int): Which step in the trajectory to return the
pose from.
Returns:
Pose
'''
if self.action_step.type == ActionStep.ARM_TRAJECTORY:
at = self.action_step.armTrajectory
arm_states = at.rArm if self.arm_index == Side.RIGHT else at.lArm
return arm_states[index].ee_pose
else:
rospy.logerr(
'Cannot request trajectory pose on non-trajectory action ' +
'step.')
def _update_viz_core(self):
'''Updates visualization after a change.'''
# Create a new IM control.
menu_control = InteractiveMarkerControl()
menu_control.interaction_mode = InteractiveMarkerControl.BUTTON
menu_control.always_visible = True
frame_id = self._get_ref_name()
pose = self.get_pose()
# Multiplex marker types added based on action step type.
if self.action_step.type == ActionStep.ARM_TARGET:
# Handle "normal" steps (saved poses).
menu_control = self._make_gripper_marker(
menu_control, self._is_hand_open())
elif self.action_step.type == ActionStep.ARM_TRAJECTORY:
# Handle trajectories.
# First, get all trajectory positions.
point_list = []
for j in range(len(self.action_step.armTrajectory.timing)):
point_list.append(self._get_traj_pose(j).position)
# Add a main maker for all points in the trajectory (sphere
# list).
menu_control.markers.append(
Marker(
type=Marker.SPHERE_LIST,
id=self.get_uid(),
lifetime=TRAJ_MARKER_LIFETIME,
scale=SCALE_TRAJ_STEP_SPHERES,
header=Header(frame_id=frame_id),
color=COLOR_TRAJ_STEP_SPHERES,
points=point_list
)
)
# Add a marker for the first step in the trajectory.
menu_control.markers.append(
ActionStepMarker._make_sphere_marker(
self.get_uid() + ID_OFFSET_TRAJ_FIRST,
self._get_traj_pose(0),
frame_id,
TRAJ_ENDPOINT_SCALE
)
)
# Add a marker for the last step in the trajectory.
last_index = len(self.action_step.armTrajectory.timing) - 1
menu_control.markers.append(
ActionStepMarker._make_sphere_marker(
self.get_uid() + ID_OFFSET_TRAJ_LAST,
self._get_traj_pose(last_index),
frame_id,
TRAJ_ENDPOINT_SCALE
)
)
else:
# Neither "normal" pose nor trajectory; error...
rospy.logerr(
'Non-handled action step type ' + str(self.action_step.type))
# Add an arrow to the relative object, if there is one.
ref_frame = World.get_ref_from_name(frame_id)
if ref_frame == ArmState.OBJECT:
menu_control.markers.append(
Marker(
type=Marker.ARROW,
id=(ID_OFFSET_REF_ARROW + self.get_uid()),
lifetime=TRAJ_MARKER_LIFETIME,
scale=SCALE_OBJ_REF_ARROW,
header=Header(frame_id=frame_id),
color=COLOR_OBJ_REF_ARROW,
points=[pose.position, Point(0, 0, 0)]
)
)
# Make and add the text for this step ('Step X').
text_pos = Point()
text_pos.x = pose.position.x
text_pos.y = pose.position.y
text_pos.z = pose.position.z + TEXT_Z_OFFSET
menu_control.markers.append(
Marker(
type=Marker.TEXT_VIEW_FACING,
id=self.get_uid(),
scale=SCALE_STEP_TEXT,
text='Step ' + str(self.step_number),
color=COLOR_STEP_TEXT,
header=Header(frame_id=frame_id),
pose=Pose(text_pos, Quaternion(0, 0, 0, 1))
)
)
# Make and add interactive marker.
int_marker = InteractiveMarker()
int_marker.name = self._get_name()
int_marker.header.frame_id = frame_id
int_marker.pose = pose
int_marker.scale = INT_MARKER_SCALE
self._add_6dof_marker(int_marker, True)
int_marker.controls.append(menu_control)
ActionStepMarker._im_server.insert(
int_marker, self.marker_feedback_cb)
def _add_6dof_marker(self, int_marker, is_fixed):
'''Adds a 6 DoF control marker to the interactive marker.
Args:
int_marker (InteractiveMarker)
is_fixed (bool): Looks like whether position is fixed (?).
Currently only passed as True.
'''
# Each entry in options is (name, orientation, is_move)
options = [
('rotate_x', Quaternion(1, 0, 0, 1), False),
('move_x', Quaternion(1, 0, 0, 1), True),
('rotate_z', Quaternion(0, 1, 0, 1), False),
('move_z', Quaternion(0, 1, 0, 1), True),
('rotate_y', Quaternion(0, 0, 1, 1), False),
('move_y', Quaternion(0, 0, 1, 1), True),
]
for opt in options:
name, orient, is_move = opt
control = self._make_6dof_control(name, orient, is_move, is_fixed)
int_marker.controls.append(control)
def _make_6dof_control(self, name, orientation, is_move, is_fixed):
'''Creates and returns one component of the 6dof controller.
Args:
name (str): Name for hte control
orientation (Quaternion): How the control should be
oriented.
is_move (bool): Looks like whether the marker moves the
object (?). Currently passed as True for moving markers,
False for rotating ones.
is_fixed (bool): Looks like whether position is fixed (?).
Currently always passed as True.
Returns:
InteractiveMarkerControl
'''
control = InteractiveMarkerControl()
control.name = name
control.orientation = orientation
control.always_visible = False
if self.is_control_visible:
if is_move:
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
else:
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
else:
control.interaction_mode = InteractiveMarkerControl.NONE
if is_fixed:
control.orientation_mode = InteractiveMarkerControl.FIXED
return control
def _make_mesh_marker(self):
'''Creates and returns a mesh marker.
Returns:
Marker
'''
mesh = Marker()
mesh.mesh_use_embedded_materials = False
mesh.type = Marker.MESH_RESOURCE
mesh.scale.x = 1.0
mesh.scale.y = 1.0
mesh.scale.z = 1.0
mesh.color = self._get_mesh_marker_color()
return mesh
def _get_mesh_marker_color(self):
'''Gets the color for the mesh marker (thing that looks like a
gripper) for this step.
A simple implementation of this will return one color for
reachable poses, another for unreachable ones. Future
implementations may provide further visual cues.
Returns:
ColorRGBA: The color for the gripper mesh for this step.
'''
if self._is_reachable():
return COLOR_MESH_REACHABLE
else:
return COLOR_MESH_UNREACHABLE
def _make_gripper_marker(self, control, is_hand_open=False):
'''Makes a gripper marker, adds it to control, returns control.
Args:
control (InteractiveMarkerControl): IM Control we're using.
is_hand_open (bool, optional): Whether the gripper is open.
Defaults to False (closed).
Returns:
InteractiveMarkerControl: The passed control.
'''
# Set angle of meshes based on gripper open vs closed.
angle = ANGLE_GRIPPER_OPEN if is_hand_open else ANGLE_GRIPPER_CLOSED
# Make transforms in preparation for meshes 1, 2, and 3.
# NOTE(mbforbes): There are some magic numbers in here that are
# used a couple times. Seems like a good candidate for
# refactoring to constants, but I think they're more clear if
# left in here as (a) they likely won't be changed, and (b) it's
# easier to understand the computations with them here.
transform1 = tf.transformations.euler_matrix(0, 0, angle)
transform1[:3, 3] = [0.07691 - ActionStepMarker._offset, 0.01, 0]
transform2 = tf.transformations.euler_matrix(0, 0, -angle)
transform2[:3, 3] = [0.09137, 0.00495, 0]
t_proximal = transform1
t_distal = tf.transformations.concatenate_matrices(
transform1, transform2)
# Create mesh 1 (palm).
mesh1 = self._make_mesh_marker()
mesh1.mesh_resource = STR_GRIPPER_PALM_FILE
mesh1.pose.position.x = -ActionStepMarker._offset
mesh1.pose.orientation.w = 1
# Create mesh 2 (finger).
mesh2 = self._make_mesh_marker()
mesh2.mesh_resource = STR_GRIPPER_FINGER_FILE
mesh2.pose = World.get_pose_from_transform(t_proximal)
# Create mesh 3 (fingertip).
mesh3 = self._make_mesh_marker()
mesh3.mesh_resource = STR_GRIPPER_FINGERTIP_FILE
mesh3.pose = World.get_pose_from_transform(t_distal)
# Make transforms in preparation for meshes 4 and 5.
quat = tf.transformations.quaternion_multiply(
tf.transformations.quaternion_from_euler(numpy.pi, 0, 0),
tf.transformations.quaternion_from_euler(0, 0, angle)
)
transform1 = tf.transformations.quaternion_matrix(quat)
transform1[:3, 3] = [0.07691 - ActionStepMarker._offset, -0.01, 0]
transform2 = tf.transformations.euler_matrix(0, 0, -angle)
transform2[:3, 3] = [0.09137, 0.00495, 0]
t_proximal = transform1
t_distal = tf.transformations.concatenate_matrices(
transform1, transform2)
# Create mesh 4 (other finger).
mesh4 = self._make_mesh_marker()
mesh4.mesh_resource = STR_GRIPPER_FINGER_FILE
mesh4.pose = World.get_pose_from_transform(t_proximal)
# Create mesh 5 (other fingertip).
mesh5 = self._make_mesh_marker()
mesh5.mesh_resource = STR_GRIPPER_FINGERTIP_FILE
mesh5.pose = World.get_pose_from_transform(t_distal)
# Append all meshes we made.
control.markers.append(mesh1)
control.markers.append(mesh2)
control.markers.append(mesh3)
control.markers.append(mesh4)
control.markers.append(mesh5)
# Return back the control.
# TODO(mbforbes): Why do we do this?
return control
class GripperMarkers:
_offset = 0.09
def __init__(self, side_prefix):
self.side_prefix = side_prefix
self._im_server = InteractiveMarkerServer('ik_request_markers_' +
self.side_prefix)
self._tf_listener = TransformListener()
self._menu_handler = MenuHandler()
self._menu_handler.insert('Move arm here',
callback=self.move_to_pose_cb)
self.r_joint_names = [
'r_shoulder_pan_joint', 'r_shoulder_lift_joint',
'r_upper_arm_roll_joint', 'r_elbow_flex_joint',
'r_forearm_roll_joint', 'r_wrist_flex_joint', 'r_wrist_roll_joint'
]
self.l_joint_names = [
'l_shoulder_pan_joint', 'l_shoulder_lift_joint',
'l_upper_arm_roll_joint', 'l_elbow_flex_joint',
'l_forearm_roll_joint', 'l_wrist_flex_joint', 'l_wrist_roll_joint'
]
# Create a trajectory action client
r_traj_contoller_name = None
l_traj_contoller_name = None
if self.side_prefix == 'r':
r_traj_controller_name = '/r_arm_controller/joint_trajectory_action'
self.r_traj_action_client = SimpleActionClient(
r_traj_controller_name, JointTrajectoryAction)
rospy.loginfo('Waiting for a response from the trajectory ' +
'action server for RIGHT arm...')
self.r_traj_action_client.wait_for_server()
else:
l_traj_controller_name = '/l_arm_controller/joint_trajectory_action'
self.l_traj_action_client = SimpleActionClient(
l_traj_controller_name, JointTrajectoryAction)
rospy.loginfo('Waiting for a response from the trajectory ' +
'action server for LEFT arm...')
self.l_traj_action_client.wait_for_server()
self.is_control_visible = False
self.ee_pose = self.get_ee_pose()
self.ik = IK(side_prefix)
self.update_viz()
self._menu_handler.apply(self._im_server,
'ik_target_marker_' + self.side_prefix)
self._im_server.applyChanges()
print self.ik
def get_ee_pose(self):
from_frame = 'base_link'
to_frame = self.side_prefix + '_wrist_roll_link'
try:
if self._tf_listener.frameExists(
from_frame) and self._tf_listener.frameExists(to_frame):
t = self._tf_listener.getLatestCommonTime(from_frame, to_frame)
# t = rospy.Time.now()
(pos, rot) = self._tf_listener.lookupTransform(
to_frame, from_frame, t) # Note argument order :(
else:
rospy.logerr(
'TF frames do not exist; could not get end effector pose.')
except Exception as err:
rospy.logerr('Could not get end effector pose through TF.')
rospy.logerr(err)
pos = [1.0, 0.0, 1.0]
rot = [0.0, 0.0, 0.0, 1.0]
return Pose(Point(pos[0], pos[1], pos[2]),
Quaternion(rot[0], rot[1], rot[2], rot[3]))
def move_to_pose_cb(self, feedback):
rospy.loginfo('You pressed the `Move arm here` button from the menu.')
'''Moves the arm to the desired joints'''
print feedback
time_to_joint = 2.0
positions = self.ik.get_ik_for_ee(feedback.pose)
velocities = [0] * len(positions)
traj_goal = JointTrajectoryGoal()
traj_goal.trajectory.header.stamp = (rospy.Time.now() +
rospy.Duration(0.1))
traj_goal.trajectory.points.append(
JointTrajectoryPoint(
positions=positions,
velocities=velocities,
time_from_start=rospy.Duration(time_to_joint)))
if (self.side_prefix == 'r'):
traj_goal.trajectory.joint_names = self.r_joint_names
self.r_traj_action_client.send_goal(traj_goal)
else:
traj_goal.trajectory.joint_names = self.l_joint_names
self.l_traj_action_client.send_goal(traj_goal)
pass
def marker_clicked_cb(self, feedback):
if feedback.event_type == InteractiveMarkerFeedback.POSE_UPDATE:
self.ee_pose = feedback.pose
self.update_viz()
self._menu_handler.reApply(self._im_server)
self._im_server.applyChanges()
elif feedback.event_type == InteractiveMarkerFeedback.BUTTON_CLICK:
rospy.loginfo('Changing visibility of the pose controls.')
if (self.is_control_visible):
self.is_control_visible = False
else:
self.is_control_visible = True
else:
rospy.loginfo('Unhandled event: ' + str(feedback.event_type))
def update_viz(self):
menu_control = InteractiveMarkerControl()
menu_control.interaction_mode = InteractiveMarkerControl.BUTTON
menu_control.always_visible = True
frame_id = 'base_link'
pose = self.ee_pose
menu_control = self._add_gripper_marker(menu_control)
text_pos = Point()
text_pos.x = pose.position.x
text_pos.y = pose.position.y
text_pos.z = pose.position.z + 0.1
text = 'x=' + str(pose.position.x) + ' y=' + str(
pose.position.y) + ' x=' + str(pose.position.z)
menu_control.markers.append(
Marker(type=Marker.TEXT_VIEW_FACING,
id=0,
scale=Vector3(0, 0, 0.03),
text=text,
color=ColorRGBA(0.0, 0.0, 0.0, 0.5),
header=Header(frame_id=frame_id),
pose=Pose(text_pos, Quaternion(0, 0, 0, 1))))
int_marker = InteractiveMarker()
int_marker.name = 'ik_target_marker_' + self.side_prefix
int_marker.header.frame_id = frame_id
int_marker.pose = pose
int_marker.scale = 0.2
self._add_6dof_marker(int_marker)
int_marker.controls.append(menu_control)
self._im_server.insert(int_marker, self.marker_clicked_cb)
def _add_gripper_marker(self, control):
is_hand_open = False
if is_hand_open:
angle = 28 * numpy.pi / 180.0
else:
angle = 0
transform1 = tf.transformations.euler_matrix(0, 0, angle)
transform1[:3, 3] = [0.07691 - GripperMarkers._offset, 0.01, 0]
transform2 = tf.transformations.euler_matrix(0, 0, -angle)
transform2[:3, 3] = [0.09137, 0.00495, 0]
t_proximal = transform1
t_distal = tf.transformations.concatenate_matrices(
transform1, transform2)
mesh1 = self._make_mesh_marker()
mesh1.mesh_resource = (
'package://pr2_description/meshes/gripper_v0/gripper_palm.dae')
mesh1.pose.position.x = -GripperMarkers._offset
mesh1.pose.orientation.w = 1
mesh2 = self._make_mesh_marker()
mesh2.mesh_resource = (
'package://pr2_description/meshes/gripper_v0/l_finger.dae')
mesh2.pose = GripperMarkers.get_pose_from_transform(t_proximal)
mesh3 = self._make_mesh_marker()
mesh3.mesh_resource = (
'package://pr2_description/meshes/gripper_v0/l_finger_tip.dae')
mesh3.pose = GripperMarkers.get_pose_from_transform(t_distal)
quat = tf.transformations.quaternion_multiply(
tf.transformations.quaternion_from_euler(numpy.pi, 0, 0),
tf.transformations.quaternion_from_euler(0, 0, angle))
transform1 = tf.transformations.quaternion_matrix(quat)
transform1[:3, 3] = [0.07691 - GripperMarkers._offset, -0.01, 0]
transform2 = tf.transformations.euler_matrix(0, 0, -angle)
transform2[:3, 3] = [0.09137, 0.00495, 0]
t_proximal = transform1
t_distal = tf.transformations.concatenate_matrices(
transform1, transform2)
mesh4 = self._make_mesh_marker()
mesh4.mesh_resource = (
'package://pr2_description/meshes/gripper_v0/l_finger.dae')
mesh4.pose = GripperMarkers.get_pose_from_transform(t_proximal)
mesh5 = self._make_mesh_marker()
mesh5.mesh_resource = (
'package://pr2_description/meshes/gripper_v0/l_finger_tip.dae')
mesh5.pose = GripperMarkers.get_pose_from_transform(t_distal)
control.markers.append(mesh1)
control.markers.append(mesh2)
control.markers.append(mesh3)
control.markers.append(mesh4)
control.markers.append(mesh5)
return control
@staticmethod
def get_pose_from_transform(transform):
pos = transform[:3, 3].copy()
rot = tf.transformations.quaternion_from_matrix(transform)
return Pose(Point(pos[0], pos[1], pos[2]),
Quaternion(rot[0], rot[1], rot[2], rot[3]))
def _make_mesh_marker(self):
mesh = Marker()
mesh.mesh_use_embedded_materials = False
mesh.type = Marker.MESH_RESOURCE
mesh.scale.x = 1.0
mesh.scale.y = 1.0
mesh.scale.z = 1.0
print self
ik_solution = self.ik.get_ik_for_ee(self.ee_pose)
if (ik_solution is None):
mesh.color = ColorRGBA(1.0, 0.0, 0.0, 0.6)
else:
mesh.color = ColorRGBA(0.0, 1.0, 0.0, 0.6)
return mesh
def _add_6dof_marker(self, int_marker):
is_fixed = True
control = self._make_6dof_control('rotate_x', Quaternion(1, 0, 0, 1),
False, is_fixed)
int_marker.controls.append(control)
control = self._make_6dof_control('move_x', Quaternion(1, 0, 0, 1),
True, is_fixed)
int_marker.controls.append(control)
control = self._make_6dof_control('rotate_z', Quaternion(0, 1, 0, 1),
False, is_fixed)
int_marker.controls.append(control)
control = self._make_6dof_control('move_z', Quaternion(0, 1, 0, 1),
True, is_fixed)
int_marker.controls.append(control)
control = self._make_6dof_control('rotate_y', Quaternion(0, 0, 1, 1),
False, is_fixed)
int_marker.controls.append(control)
control = self._make_6dof_control('move_y', Quaternion(0, 0, 1, 1),
True, is_fixed)
int_marker.controls.append(control)
def _make_6dof_control(self, name, orientation, is_move, is_fixed):
control = InteractiveMarkerControl()
control.name = name
control.orientation = orientation
control.always_visible = False
if (self.is_control_visible):
if is_move:
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
else:
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
else:
control.interaction_mode = InteractiveMarkerControl.NONE
if is_fixed:
control.orientation_mode = InteractiveMarkerControl.FIXED
return control
for i in range(len(pathpoints)):
marker = make_waypoint_marker(i, pathpoints[i][0], pathpoints[i][1])
marker_server.insert(marker, move_waypoint)
menu_handler.apply(marker_server, marker.name)
marker_server.applyChanges()
if __name__ == '__main__':
rospy.init_node('path_server')
global marker_server, menu_handler
marker_server = InteractiveMarkerServer('path_server')
menu_handler = MenuHandler()
menu_handler.insert('Remove waypoint', callback=remove_marker)
insert_submenu = menu_handler.insert('Insert waypoint')
menu_handler.insert('Before current', parent=insert_submenu, callback=insert_before)
menu_handler.insert('After current', parent=insert_submenu, callback=insert_after)
# Read input file
filename = sys.argv[1]
with open(filename, 'rb') as f:
map_data = pickle.load(f)
pathpoints = map_data['pathpoints']
global path_pub
path_pub = rospy.Publisher('path', Path, latch=True, queue_size=1)
publish_path(pathpoints)
class MarkerTemplate(object):
def __init__(self, server, id_, name, initial_pose=None):
self._menu_handler = MenuHandler()
self._menu_handler.insert("Delete", callback=self.deleteCallback)
self._server = server
self._key = name + str(id_)
self._marker_map = {}
self._callback_map = {}
# not all templates will have a dynamic reconfigure server
self._dserver = None
self._initial_pose = initial_pose
@property
def initial_pose(self):
"""Return a marker's initial pose if it was passed in."""
return self._initial_pose
# @property
# def key(self):
# """Get the marker template's key on the server."""
# key = self._key + str(self._id)
# return key
@property
def server(self):
"""Get the marker template's interactive marker server."""
return self._server
@property
def marker_map(self):
"""Get the interactive marker map of this marker template."""
return self._marker_map
@property
def callback_map(self):
"""Get the callback map of this marker template."""
return self._callback_map
@property
def menu_handler(self):
"""Get the menu handler of this marker template."""
return self._menu_handler
@property
def setNudgeDistance(self, distance):
self._nudgeDistance = distance
def getNudgeDistance(self):
return self._nudgeDistance
def initNudgeMenu(self):
nudgeControl = self.menu_handler.insert("Nudge hand " +
str(self._nudgeDistance) + "m")
self.menu_handler.insert("World X",
parent=nudgeControl,
command_type=MenuEntry.FEEDBACK,
command="",
callback=self.nudgeX)
self.menu_handler.insert("World Y",
parent=nudgeControl,
command_type=MenuEntry.FEEDBACK,
command="",
callback=self.nudgeY)
self.menu_handler.insert("World Z",
parent=nudgeControl,
command_type=MenuEntry.FEEDBACK,
command="",
callback=self.nudgeZ)
self.menu_handler.insert("World -X",
parent=nudgeControl,
command_type=MenuEntry.FEEDBACK,
command="",
callback=self.nudgeXNeg)
self.menu_handler.insert("World -Y",
parent=nudgeControl,
command_type=MenuEntry.FEEDBACK,
command="",
callback=self.nudgeYNeg)
self.menu_handler.insert("World -Z",
parent=nudgeControl,
command_type=MenuEntry.FEEDBACK,
command="",
callback=self.nudgeZNeg)
def addInteractiveMarker(self, marker, callback=None):
name = marker.name
self._marker_map[name] = marker
if callback:
self._callback_map[name] = callback
return self.server.insert(marker, callback)
else:
self._callback_map[name] = self.processFeedback
return self.server.insert(marker, self.processFeedback)
def removeInteractiveMarker(self, marker_name):
if self.server.erase(marker_name):
if marker_name in self._marker_map:
del self._marker_map[marker_name]
del self._callback_map[marker_name]
return True
return False
def attachMenuHandler(self, marker):
return self.menu_handler.apply(self.server, marker.name)
def getMarker(self):
return self.server.get(self._key)
def hasMarker(self):
if self._key in self._marker_map.keys():
return True
else:
return False
def deleteCallback(self, feedback):
for key in self._marker_map.keys():
self.server.erase(key)
self.server.applyChanges()
self.tearDown()
def tearDown(self, keepAlive=False):
# forcefully shut down service before killing node
if self._dserver:
self._dserver.set_service.shutdown("User deleted template.")
# for rostest (and potentially other cases), we want to clean up but keep the node alive
if not keepAlive:
rospy.signal_shutdown("User deleted template.")
def processFeedback(self, feedback):
self.server.applyChanges()
def create_menu(self):
"""
Create and populates all the menu entries
"""
menu_handler = MenuHandler()
menu_handler.insert("Point the head", callback=self.move_head)
menu_handler.insert("Add position to trajectory",
callback=self.add_point)
menu_handler.insert("Place marker over gripper",
callback=self.place_gripper)
menu_handler.insert("Execute trjectory",
callback=self.execute_trajectory)
menu_handler.insert("Clear trajectory", callback=self.clear_trajectory)
menu_handler.insert("Publish trajectory",
callback=self.publish_trajectory)
menu_handler.insert("Move the arm (planning)", callback=self.plan_arm)
menu_handler.insert("Move the arm (collision-free)",
callback=self.collision_free_arm)
menu_handler.insert(
"Move the arm to trajectory start (collision-free)",
callback=self.arm_trajectory_start)
menu_handler.insert("Update planning scene",
callback=self.update_planning)
menu_handler.apply(self.server, self.int_marker.name)
def add_marker(self, initial_pose, frame="base_link", description=None):
self.markers_created_cnt += 1
self.marker_cnt += 1
int_marker = InteractiveMarker()
int_marker.header.frame_id = frame
int_marker.pose.position = initial_pose.position
int_marker.scale = 0.1
int_marker.name = rospy.get_name() + str(self.markers_created_cnt)
if description is None:
int_marker.description = int_marker.name
else:
int_marker.description = description
self.name_to_marker_dict[int_marker.name] = int_marker
self.name_to_position_only_flag[int_marker.name] = True
# insert a box # TODO: may change geometry / eff mesh
make_box_control_marker(int_marker)
int_marker.controls[0].interaction_mode = InteractiveMarkerControl.MOVE_ROTATE_3D
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 1
control.orientation.y = 0
control.orientation.z = 0
control.name = "rotate_x"
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
int_marker.controls.append(control)
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 1
control.orientation.y = 0
control.orientation.z = 0
control.name = "move_x"
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
int_marker.controls.append(control)
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 0
control.orientation.y = 1
control.orientation.z = 0
control.name = "rotate_z"
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
int_marker.controls.append(control)
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 0
control.orientation.y = 1
control.orientation.z = 0
control.name = "move_z"
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
int_marker.controls.append(control)
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 0
control.orientation.y = 0
control.orientation.z = 1
control.name = "rotate_y"
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
int_marker.controls.append(control)
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 0
control.orientation.y = 0
control.orientation.z = 1
control.name = "move_y"
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
int_marker.controls.append(control)
self.server.insert(int_marker, self._process_feedback)
# add menus
menu_handler = MenuHandler()
pos_opt_handle = menu_handler.insert("Position-Only Ctrl", callback=self._process_feedback)
self._menu_cmds['position_only'] = pos_opt_handle
menu_handler.setCheckState(pos_opt_handle, MenuHandler.CHECKED)
menu_handler.apply(self.server, int_marker.name)
self._menu_handlers[int_marker.name] = menu_handler
self.server.applyChanges()
self._int_marker_name_list.append(int_marker.name)
return int_marker.name
class ArmStepMarker:
''' Marker for visualizing the steps of an action'''
_offset = 0.09
_marker_click_cb = None
_reference_change_cb = None
def __init__(self, step_number, arm_index, action_step,
marker_click_cb, reference_change_cb, parent_sequence):
self.action_step = action_step
self.arm_index = arm_index
self.step_number = step_number
self.is_requested = False
self.is_deleted = False
self.is_control_visible = False
self.is_edited = False
self.has_object = False
self.is_dimmed = False
self.is_fake = False
self._sub_entries = None
self._menu_handler = None
ArmStepMarker._marker_click_cb = marker_click_cb
ArmStepMarker._reference_change_cb = reference_change_cb
self.parent_step_sequence = parent_sequence
def _is_reachable(self):
'''Checks if there is an IK solution for action step'''
dummy, is_reachable = Robot.solve_ik_for_arm(self.arm_index,
self.get_target())
# rospy.loginfo('Reachability of pose in ArmStepMarker : ' +
# str(is_reachable))
return is_reachable
def get_uid(self):
'''Returns a unique id of the marker'''
return (2 * self.step_number + self.arm_index)
def _get_name(self):
'''Generates the unique name for the marker'''
return ('step' + str(self.step_number)
+ 'arm' + str(self.arm_index))
def decrease_id(self):
'''Reduces the index of the marker'''
self.step_number -= 1
self._update_menu()
def update_ref_frames(self, ref_frame_list, new_ref_obj):
'''Updates and re-assigns coordinate frames when the world changes'''
# There is a new list of objects
# If the current frames are already assigned to object,
# we need to figure out the correspondences
self.parent_step_sequence.ref_object_list = ref_frame_list
arm_pose = self.get_target()
if (arm_pose.refFrame == ArmState.OBJECT):
self.has_object = False
if (new_ref_obj != None):
self.has_object = True
arm_pose.refFrameObject = new_ref_obj
self.parent_step_sequence.ref_names = ['base_link']
for i in range(len(self.parent_step_sequence.ref_object_list)):
self.parent_step_sequence.ref_names.append(
self.parent_step_sequence.ref_object_list[i].name)
self._update_menu()
def destroy(self):
'''Removes marker from the world'''
self.parent_step_sequence.im_server.erase(self._get_name())
self.parent_step_sequence.im_server.applyChanges()
def _update_menu(self):
'''Recreates the menu when something has changed'''
self._menu_handler = MenuHandler()
frame_entry = self._menu_handler.insert('Reference frame')
self._sub_entries = [None] * len(self.parent_step_sequence.ref_names)
for i in range(len(self.parent_step_sequence.ref_names)):
self._sub_entries[i] = self._menu_handler.insert(
self.parent_step_sequence.ref_names[i], parent=frame_entry,
callback=self.change_ref_cb)
self._menu_handler.insert('Move arm here', callback=self.move_to_cb)
self._menu_handler.insert('Move to current arm pose',
callback=self.move_pose_to_cb)
self._menu_handler.insert('Delete', callback=self.delete_step_cb)
for i in range(len(self.parent_step_sequence.ref_names)):
self._menu_handler.setCheckState(self._sub_entries[i],
MenuHandler.UNCHECKED)
menu_id = self._get_menu_id(self._get_ref_name())
if menu_id is None:
self.has_object = False
else:
self._menu_handler.setCheckState(menu_id,
MenuHandler.CHECKED)
self._update_viz_core()
self._menu_handler.apply(self.parent_step_sequence.im_server, self._get_name())
self.parent_step_sequence.im_server.applyChanges()
def _get_menu_id(self, ref_name):
'''Returns the unique menu id from its name
None if the object is not found'''
if ref_name in self.parent_step_sequence.ref_names:
index = self.parent_step_sequence.ref_names.index(ref_name)
return self._sub_entries[index]
else:
return None
def _get_menu_name(self, menu_id):
'''Returns the menu name from its unique menu id'''
index = self._sub_entries.index(menu_id)
return self.parent_step_sequence.ref_names[index]
def _get_ref_name(self):
'''Returns the name string for the reference
frame object of the action step'''
ref_name = None
if (self.action_step.type == ArmStepType.ARM_TARGET):
if self.arm_index == 0:
ref_frame = self.action_step.armTarget.rArm.refFrame
ref_name = self.action_step.armTarget.rArm.refFrameObject.name
else:
ref_frame = self.action_step.armTarget.lArm.refFrame
ref_name = self.action_step.armTarget.lArm.refFrameObject.name
elif (self.action_step.type == ArmStepType.ARM_TRAJECTORY):
if self.arm_index == 0:
ref_frame = self.action_step.armTrajectory.rRefFrame
ref_name = self.action_step.armTrajectory.rRefFrameOject.name
else:
ref_frame = self.action_step.armTrajectory.lRefFrame
ref_name = self.action_step.armTrajectory.lRefFrameOject.name
else:
rospy.logerr('Unhandled marker type: ' +
str(self.action_step.type))
if (ref_frame == ArmState.ROBOT_BASE):
ref_name = 'base_link'
return ref_name
def _set_ref(self, new_ref_name):
'''Changes the reference frame of the action step'''
new_ref = World.get_world().get_ref_from_name(new_ref_name)
if (new_ref != ArmState.ROBOT_BASE):
index = self.parent_step_sequence.ref_names.index(new_ref_name)
new_ref_obj = self.parent_step_sequence.ref_object_list[index - 1]
else:
new_ref_obj = Object()
if (self.action_step.type == ArmStepType.ARM_TARGET):
if self.arm_index == 0:
self.action_step.armTarget.rArm = World.get_world().convert_ref_frame(
self.action_step.armTarget.rArm, new_ref, new_ref_obj)
else:
self.action_step.armTarget.lArm = World.get_world().convert_ref_frame(
self.action_step.armTarget.lArm, new_ref, new_ref_obj)
elif (self.action_step.type == ArmStepType.ARM_TRAJECTORY):
for i in range(len(self.action_step.armTrajectory.timing)):
if self.arm_index == 0:
arm_old = self.action_step.armTrajectory.rArm[i]
arm_new = World.get_world().convert_ref_frame(arm_old,
new_ref, new_ref_obj)
self.action_step.armTrajectory.rArm[i] = arm_new
else:
arm_old = self.action_step.armTrajectory.lArm[i]
arm_new = World.get_world().convert_ref_frame(arm_old,
new_ref, new_ref_obj)
self.action_step.armTrajectory.lArm[i] = arm_new
if self.arm_index == 0:
self.action_step.armTrajectory.rRefFrameObject = new_ref_obj
self.action_step.armTrajectory.rRefFrame = new_ref
else:
self.action_step.armTrajectory.lRefFrameObject = new_ref_obj
self.action_step.armTrajectory.lRefFrame = new_ref
ArmStepMarker._reference_change_cb(self.get_uid(), new_ref, new_ref_obj)
def _is_hand_open(self):
'''Checks if the gripper is open for the action step'''
if self.arm_index == 0:
gripper_state = self.action_step.gripperAction.rGripper
else:
gripper_state = self.action_step.gripperAction.lGripper
return (gripper_state == GripperState.OPEN)
def set_new_pose(self, new_pose):
'''Changes the pose of the action step'''
if (self.action_step.type == ArmStepType.ARM_TARGET):
if self.arm_index == 0:
pose = ArmStepMarker._offset_pose(new_pose, -1)
self.action_step.armTarget.rArm.ee_pose = pose
else:
pose = ArmStepMarker._offset_pose(new_pose, -1)
self.action_step.armTarget.lArm.ee_pose = pose
rospy.loginfo('Set new pose for action step.')
self.update_viz()
elif (self.action_step.type == ArmStepType.ARM_TRAJECTORY):
rospy.logwarn('Modification of whole trajectory ' +
'segments is not implemented.')
def get_absolute_position(self, is_start=True):
'''Returns the absolute position of the action step'''
pose = self.get_absolute_pose(is_start)
return pose.position
def get_absolute_pose(self, is_start=True):
'''Returns the absolute pose of the action step'''
if (self.action_step.type == ArmStepType.ARM_TARGET):
if self.arm_index == 0:
arm_pose = self.action_step.armTarget.rArm
else:
arm_pose = self.action_step.armTarget.lArm
elif (self.action_step.type == ArmStepType.ARM_TRAJECTORY):
if self.arm_index == 0:
if is_start:
index = len(self.action_step.armTrajectory.rArm) - 1
arm_pose = self.action_step.armTrajectory.rArm[index]
else:
arm_pose = self.action_step.armTrajectory.rArm[0]
else:
if is_start:
index = len(self.action_step.armTrajectory.lArm) - 1
arm_pose = self.action_step.armTrajectory.lArm[index]
else:
arm_pose = self.action_step.armTrajectory.lArm[0]
#if (arm_pose.refFrame == ArmState.OBJECT and
# World.has_object(arm_pose.refFrameObject.name)):
# return ArmStepMarker._offset_pose(arm_pose.ee_pose)
#else:
world_pose = World.get_world().get_absolute_pose(arm_pose)
return ArmStepMarker._offset_pose(world_pose)
def get_pose(self):
'''Returns the pose of the action step'''
target = self.get_target()
if (target != None):
return ArmStepMarker._offset_pose(target.ee_pose)
@staticmethod
def _offset_pose(pose, constant=1):
'''Offsets the world pose for visualization'''
transform = World.get_world().get_matrix_from_pose(pose)
offset_array = [constant * ArmStepMarker._offset, 0, 0]
offset_transform = tf.transformations.translation_matrix(offset_array)
hand_transform = tf.transformations.concatenate_matrices(transform,
offset_transform)
return World.get_world().get_pose_from_transform(hand_transform)
def set_target(self, target):
'''Sets the new pose for the action step'''
if (self.action_step.type == ArmStepType.ARM_TARGET):
if self.arm_index == 0:
self.action_step.armTarget.rArm = target
else:
self.action_step.armTarget.lArm = target
self.has_object = True
self._update_menu()
self.is_edited = False
def get_target(self, traj_index=None):
'''Returns the pose for the action step'''
if (self.action_step.type == ArmStepType.ARM_TARGET):
if self.arm_index == 0:
return self.action_step.armTarget.rArm
else:
return self.action_step.armTarget.lArm
elif (self.action_step.type == ArmStepType.ARM_TRAJECTORY):
if self.arm_index == 0:
if traj_index is None:
traj = self.action_step.armTrajectory.rArm
traj_index = int(len(traj) / 2)
return self.action_step.armTrajectory.rArm[traj_index]
else:
if traj_index is None:
traj = self.action_step.armTrajectory.lArm
traj_index = int(len(traj) / 2)
return self.action_step.armTrajectory.lArm[traj_index]
def _get_traj_pose(self, index):
'''Returns a single pose for trajectory'''
if (self.action_step.type == ArmStepType.ARM_TRAJECTORY):
if self.arm_index == 0:
target = self.action_step.armTrajectory.rArm[index]
else:
target = self.action_step.armTrajectory.lArm[index]
return target.ee_pose
else:
rospy.logerr('Cannot request trajectory pose ' +
'on non-trajectory action step.')
def _update_viz_core(self):
'''Updates visualization after a change'''
menu_control = InteractiveMarkerControl()
menu_control.interaction_mode = InteractiveMarkerControl.BUTTON
menu_control.always_visible = True
frame_id = self._get_ref_name()
pose = self.get_pose()
if (self.action_step.type == ArmStepType.ARM_TARGET):
menu_control = self._make_gripper_marker(menu_control,
self._is_hand_open())
elif (self.action_step.type == ArmStepType.ARM_TRAJECTORY):
point_list = []
for j in range(len(self.action_step.armTrajectory.timing)):
point_list.append(self._get_traj_pose(j).position)
main_marker = Marker(type=Marker.SPHERE_LIST, id=self.get_uid(),
lifetime=rospy.Duration(2),
scale=Vector3(0.02, 0.02, 0.02),
header=Header(frame_id=frame_id),
color=ColorRGBA(0.8, 0.4, 0.0, 0.8),
points=point_list)
menu_control.markers.append(main_marker)
menu_control.markers.append(ArmStepMarker.make_sphere_marker(
self.get_uid() + 2000,
self._get_traj_pose(0), frame_id, 0.05))
last_index = len(self.action_step.armTrajectory.timing) - 1
menu_control.markers.append(ArmStepMarker.make_sphere_marker(
self.get_uid() + 3000, self._get_traj_pose(last_index),
frame_id, 0.05))
else:
rospy.logerr('Non-handled action step type '
+ str(self.action_step.type))
ref_frame = World.get_world().get_ref_from_name(frame_id)
if (ref_frame == ArmState.OBJECT):
# The following is needed to properly display the arrtow in browser, due to the fact that ros3djs
# displays all nested markers in the reference frame of the interactive marker.
# Thus, we need to calculate the position of the object in the reference frame of the interactive marker.
quat = [pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w]
inv_quat_matrix = quaternion_matrix(quaternion_inverse(quat))
pose_vec = numpy.array((pose.position.x, pose.position.y, pose.position.z, 0))
new_pose = numpy.dot(inv_quat_matrix, pose_vec)
menu_control.markers.append(Marker(type=Marker.ARROW,
id=(1000 + self.get_uid()),
lifetime=rospy.Duration(2),
scale=Vector3(0.01, 0.01, 0.0001),
header=Header(frame_id=frame_id),
color=ColorRGBA(0.8, 0.8, 0.0, 0.6),
points=[Point(0, 0, 0), Point(-new_pose[0], -new_pose[1], -new_pose[2])]))
# Calculate text position so that they "orbit" around the marker;
# this is done so that poses in identical or similar positions
# have non-overlapping text. Note that to do this without moving
# the text around as the camera is moved, we assume that the viewer
# is always looking directly at the robot, so we assume the x dimension
# is constant and "orbin" in the y-z plane.
n_orbitals = 8 # this should be a constant
offset = 0.15 # this should be a constant
orbital = (self.step_number - 1) % n_orbitals # - 1 to make 0-based
angle_rad = (float(orbital) / n_orbitals) * (-2 * numpy.pi) + \
(numpy.pi / 2.0) # start above, at pi/2 (90 degrees)
text_pos = Point()
text_pos.x = 0
text_pos.y = numpy.cos(angle_rad) * offset
text_pos.z = numpy.sin(angle_rad) * offset
r,g,b = self.get_marker_color()
menu_control.markers.append(Marker(type=Marker.TEXT_VIEW_FACING,
id=self.get_uid(), scale=Vector3(0.06, 0.06, 0.06),
lifetime=rospy.Duration(1.5),
text='Step ' + str(self.step_number),
color=ColorRGBA(r, g, b, 1.0),
header=Header(frame_id=frame_id),
pose=Pose(text_pos, Quaternion(0, 0, 0, 1))))
int_marker = InteractiveMarker()
int_marker.name = self._get_name()
int_marker.header.frame_id = frame_id
int_marker.pose = pose
int_marker.scale = 0.2
self._add_6dof_marker(int_marker, True)
int_marker.controls.append(menu_control)
self.parent_step_sequence.im_server.insert(int_marker,
self.marker_feedback_cb)
@staticmethod
def make_sphere_marker(uid, pose, frame_id, radius):
'''Creates a sphere marker'''
return Marker(type=Marker.SPHERE, id=uid, lifetime=rospy.Duration(2),
scale=Vector3(radius, radius, radius),
pose=pose, header=Header(frame_id=frame_id),
color=ColorRGBA(1.0, 0.5, 0.0, 0.8))
def marker_feedback_cb(self, feedback):
'''Callback for when an event occurs on the marker'''
if feedback.event_type == InteractiveMarkerFeedback.POSE_UPDATE:
self.set_new_pose(feedback.pose)
self.update_viz()
elif feedback.event_type == InteractiveMarkerFeedback.BUTTON_CLICK:
# Set the visibility of the 6DOF controller
rospy.loginfo('Changing visibility of the pose controls.')
if (self.is_control_visible):
self.is_control_visible = False
else:
self.is_control_visible = True
ArmStepMarker._marker_click_cb(self.get_uid(),
self.is_control_visible)
else:
rospy.loginfo('Unknown event' + str(feedback.event_type))
def delete_step_cb(self, dummy):
'''Callback for when delete is requested'''
self.is_deleted = True
def move_to_cb(self, dummy):
'''Callback for when moving to a pose is requested'''
self.is_requested = True
def move_pose_to_cb(self, dummy):
'''Callback for when a pose change to current is requested'''
self.is_edited = True
def pose_reached(self):
'''Update when a requested pose is reached'''
self.is_requested = False
def change_ref_cb(self, feedback):
'''Callback for when a reference frame change is requested'''
self._menu_handler.setCheckState(
self._get_menu_id(self._get_ref_name()),
MenuHandler.UNCHECKED)
self._menu_handler.setCheckState(feedback.menu_entry_id,
MenuHandler.CHECKED)
new_ref = self._get_menu_name(feedback.menu_entry_id)
self._set_ref(new_ref)
rospy.loginfo('Switching reference frame to '
+ new_ref + ' for action step ' + self._get_name())
self._menu_handler.reApply(self.parent_step_sequence.im_server)
self.parent_step_sequence.im_server.applyChanges()
self.update_viz()
def update_viz(self):
'''Updates visualization fully'''
self._update_viz_core()
self._menu_handler.reApply(self.parent_step_sequence.im_server)
self.parent_step_sequence.im_server.applyChanges()
def _add_6dof_marker(self, int_marker, is_fixed):
'''Adds a 6 DoF control marker to the interactive marker'''
control = self._make_6dof_control('rotate_x',
Quaternion(1, 0, 0, 1), False, is_fixed)
int_marker.controls.append(control)
control = self._make_6dof_control('move_x',
Quaternion(1, 0, 0, 1), True, is_fixed)
int_marker.controls.append(control)
control = self._make_6dof_control('rotate_z',
Quaternion(0, 1, 0, 1), False, is_fixed)
int_marker.controls.append(control)
control = self._make_6dof_control('move_z',
Quaternion(0, 1, 0, 1), True, is_fixed)
int_marker.controls.append(control)
control = self._make_6dof_control('rotate_y',
Quaternion(0, 0, 1, 1), False, is_fixed)
int_marker.controls.append(control)
control = self._make_6dof_control('move_y',
Quaternion(0, 0, 1, 1), True, is_fixed)
int_marker.controls.append(control)
def _make_6dof_control(self, name, orientation, is_move, is_fixed):
'''Creates one component of the 6dof controller'''
control = InteractiveMarkerControl()
control.name = name
control.orientation = orientation
control.always_visible = False
if (self.is_control_visible):
if is_move:
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
else:
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
else:
control.interaction_mode = InteractiveMarkerControl.NONE
if is_fixed:
control.orientation_mode = InteractiveMarkerControl.FIXED
return control
def get_marker_color(self):
'''Makes marker colors in a gradient according to the progression set in
_get_rgb_from_abs_pos() (below).
returns (r,g,b) tuple, each from 0.0 to 1.0
'''
total = self.parent_step_sequence.total_n_markers
# These are 1-based indexing; turn them into 0-based indexing.
idx = self.step_number - 1
# First calculate "absolute" color position, plotting on a 0.0 - 1.0
# scale. This only applies if there's more than one step; otherwise we
# set to 0.0 (though it could be anything, as it just gets multiplied
# by 0, as the idx must be 0 if there's only one step total).
abs_step_size = 1.0 / float(total - 1) if total > 1 else 0.0
abs_pos = abs_step_size * idx
# Then, use our helper method to turn this into an RGB value.
return self._get_rgb_from_abs_pos(abs_pos)
def _get_rgb_from_abs_pos(self, abs_pos):
'''Turns abs_pos, a float from 0.0 to 1.0 inclusive, into an rgb value
of the range currently programmed. The progression is as follows (in
order to avoid green hues, which could be confused with the objects),
by gradient "bucket" step:
- 0 yellow (start) -> red
- 1 red -> purple
- 2 purple -> blue
- 3 blue -> cyan (end)
Returns (r,g,b) tuple of floats, each from 0.0 to 1.0 inclusive.'''
# Bucket settings (make constant)
bucket = self.arm_index
bucket_pos = abs_pos
# Now translate to colors; todo later implement with better data
# structure.
r = 0.0
g = 0.0
b = 0.0
if bucket == 0:
# yellow -> red
r = 1.0
g = 1.0 - bucket_pos
elif bucket == 1:
# cyan -> blue
g = 1.0 - bucket_pos
b = 1.0
else:
# Set white as error color
rospy.logwarn("Bad color gradient; bucket " + str(bucket) +
" and bucket position " + str(bucket_pos))
r,g,b = 1.0, 1.0, 1.0
return (r,g,b)
def _make_mesh_marker(self):
'''Creates a mesh marker'''
mesh = Marker()
mesh.mesh_use_embedded_materials = False
mesh.type = Marker.MESH_RESOURCE
mesh.scale.x = 1.0
mesh.scale.y = 1.0
mesh.scale.z = 1.0
alpha = 0.6
if self.is_dimmed:
alpha = 0.1
if self.is_fake:
alpha = 0.4
mesh.color = ColorRGBA(0.3, 0.3, 0.3, alpha)
return mesh
if self._is_reachable():
# Original: some kinda orange
# r,g,b = 1.0, 0.5, 0.0
# New: rainbow! See method comment for details.
r,g,b = self.get_marker_color()
mesh.color = ColorRGBA(r, g, b, alpha)
else:
mesh.color = ColorRGBA(0.5, 0.5, 0.5, alpha)
return mesh
def _make_gripper_marker(self, control, is_hand_open=False):
'''Makes a gripper marker'''
if is_hand_open:
angle = 28 * numpy.pi / 180.0
else:
angle = 0
transform1 = tf.transformations.euler_matrix(0, 0, angle)
transform1[:3, 3] = [0.07691 - ArmStepMarker._offset, 0.01, 0]
transform2 = tf.transformations.euler_matrix(0, 0, -angle)
transform2[:3, 3] = [0.09137, 0.00495, 0]
t_proximal = transform1
t_distal = tf.transformations.concatenate_matrices(transform1,
transform2)
mesh1 = self._make_mesh_marker()
mesh1.mesh_resource = ('package://pr2_description/meshes/' +
'gripper_v0/gripper_palm.dae')
mesh1.pose.position.x = -ArmStepMarker._offset
mesh1.pose.orientation.w = 1
mesh2 = self._make_mesh_marker()
mesh2.mesh_resource = ('package://pr2_description/meshes/' +
'gripper_v0/l_finger.dae')
mesh2.pose = World.get_world().get_pose_from_transform(t_proximal)
mesh3 = self._make_mesh_marker()
mesh3.mesh_resource = ('package://pr2_description/meshes/' +
'gripper_v0/l_finger_tip.dae')
mesh3.pose = World.get_world().get_pose_from_transform(t_distal)
quat = tf.transformations.quaternion_multiply(
tf.transformations.quaternion_from_euler(numpy.pi, 0, 0),
tf.transformations.quaternion_from_euler(0, 0, angle))
transform1 = tf.transformations.quaternion_matrix(quat)
transform1[:3, 3] = [0.07691 - ArmStepMarker._offset, -0.01, 0]
transform2 = tf.transformations.euler_matrix(0, 0, -angle)
transform2[:3, 3] = [0.09137, 0.00495, 0]
t_proximal = transform1
t_distal = tf.transformations.concatenate_matrices(transform1,
transform2)
mesh4 = self._make_mesh_marker()
mesh4.mesh_resource = ('package://pr2_description/meshes/' +
'gripper_v0/l_finger.dae')
mesh4.pose = World.get_world().get_pose_from_transform(t_proximal)
mesh5 = self._make_mesh_marker()
mesh5.mesh_resource = ('package://pr2_description/meshes/' +
'gripper_v0/l_finger_tip.dae')
mesh5.pose = World.get_world().get_pose_from_transform(t_distal)
control.markers.append(mesh1)
control.markers.append(mesh2)
control.markers.append(mesh3)
control.markers.append(mesh4)
control.markers.append(mesh5)
return control
class InteractiveFirstGuess(object):
def __init__(self, args):
# command line arguments
self.args = args
# calibration config
self.config = None
# sensors
self.sensors = []
# robot URDF
self.urdf = None
# interactive markers server
self.server = None
self.menu = None
def init(self):
# The parameter /robot_description must exist
if not rospy.has_param('/robot_description'):
rospy.logerr(
"Parameter '/robot_description' must exist to continue")
sys.exit(1)
self.config = loadJSONConfig(self.args['calibration_file'])
if self.config is None:
sys.exit(1) # loadJSON should tell you why.
# self.config = CalibConfig()
# ok = self.config.loadJSON(self.args['calibration_file'])
# # Exit if it fails to open a valid calibration file
# if not ok:
# sys.exit(1)
# Load the urdf from /robot_description
self.urdf = URDF.from_parameter_server()
# ok = validateLinks(self.config.world_link, self.config.sensors, self.urdf)
# if not ok:
# sys.exit(1)
print('Number of sensors: ' + str(len(self.config['sensors'])))
# Init interaction
self.server = InteractiveMarkerServer("interactive_first_guess")
self.menu = MenuHandler()
self.menu.insert("Save sensors configuration",
callback=self.onSaveFirstGuess)
self.menu.insert("Reset to initial configuration",
callback=self.onReset)
# For each node generate an interactive marker.
for name, sensor in self.config['sensors'].items():
print(Fore.BLUE + '\nSensor name is ' + name + Style.RESET_ALL)
params = {
"frame_world": self.config['world_link'],
"frame_opt_parent": sensor['parent_link'],
"frame_opt_child": sensor['child_link'],
"frame_sensor": sensor['link'],
"marker_scale": self.args['marker_scale']
}
# Append to the list of sensors
self.sensors.append(Sensor(name, self.server, self.menu, **params))
self.server.applyChanges()
def onSaveFirstGuess(self, feedback):
for sensor in self.sensors:
# find corresponding joint for this sensor
for joint in self.urdf.joints:
if sensor.opt_child_link == joint.child and sensor.opt_parent_link == joint.parent:
trans = sensor.optT.getTranslation()
euler = sensor.optT.getEulerAngles()
joint.origin.xyz = list(trans)
joint.origin.rpy = list(euler)
# Write the urdf file with interactive_calibration's
# source path as base directory.
rospack = rospkg.RosPack()
outfile = rospack.get_path(
'interactive_calibration') + os.path.abspath('/' +
self.args['filename'])
with open(outfile, 'w') as out:
print("Writing fist guess urdf to '{}'".format(outfile))
out.write(self.urdf.to_xml_string())
def onReset(self, feedback):
for sensor in self.sensors:
sensor.resetToInitalPose()
class InteractiveMarkerPoseStampedPublisher():
def __init__(self, from_frame, to_frame, position, orientation):
self.server = InteractiveMarkerServer("posestamped_im")
o = orientation
r, p, y = euler_from_quaternion([o.x, o.y, o.z, o.w])
rospy.loginfo("Publishing transform and PoseStamped from: " +
from_frame + " to " + to_frame + " at " +
str(position.x) + " " + str(position.y) + " " +
str(position.z) + " and orientation " + str(o.x) + " " +
str(o.y) + " " + str(o.z) + " " + str(o.w) + " or rpy " +
str(r) + " " + str(p) + " " + str(y))
self.menu_handler = MenuHandler()
self.from_frame = from_frame
self.to_frame = to_frame
# Transform broadcaster
self.tf_br = tf.TransformBroadcaster()
self.pub = rospy.Publisher('/posestamped_obj',
PoseStamped,
queue_size=1)
rospy.loginfo("Publishing posestampeds at topic: " +
str(self.pub.name))
pose = Pose()
pose.position = position
pose.orientation = orientation
self.last_pose = pose
self.print_commands(pose)
self.makeGraspIM(pose)
self.server.applyChanges()
def processFeedback(self, feedback):
"""
:type feedback: InteractiveMarkerFeedback
"""
s = "Feedback from marker '" + feedback.marker_name
s += "' / control '" + feedback.control_name + "'"
mp = ""
if feedback.mouse_point_valid:
mp = " at " + str(feedback.mouse_point.x)
mp += ", " + str(feedback.mouse_point.y)
mp += ", " + str(feedback.mouse_point.z)
mp += " in frame " + feedback.header.frame_id
if feedback.event_type == InteractiveMarkerFeedback.BUTTON_CLICK:
rospy.logdebug(s + ": button click" + mp + ".")
elif feedback.event_type == InteractiveMarkerFeedback.MENU_SELECT:
rospy.logdebug(s + ": menu item " + str(feedback.menu_entry_id) +
" clicked" + mp + ".")
if feedback.menu_entry_id == 1:
rospy.logdebug("Start publishing transform...")
if self.tf_br is None:
self.tf_br = tf.TransformBroadcaster()
elif feedback.menu_entry_id == 2:
rospy.logdebug("Stop publishing transform...")
self.tf_br = None
# When clicking this event triggers!
elif feedback.event_type == InteractiveMarkerFeedback.POSE_UPDATE:
rospy.logdebug(s + ": pose changed")
elif feedback.event_type == InteractiveMarkerFeedback.MOUSE_DOWN:
rospy.logdebug(s + ": mouse down" + mp + ".")
elif feedback.event_type == InteractiveMarkerFeedback.MOUSE_UP:
rospy.logdebug(s + ": mouse up" + mp + ".")
# TODO: Print here the commands
# tf static transform
self.print_commands(feedback.pose)
self.last_pose = deepcopy(feedback.pose)
self.server.applyChanges()
pub = rospy.Publisher('/gazebo/set_model_state',
ModelState,
queue_size=1)
pos = ModelState()
pos.model_name = self.to_frame
pos.pose = self.last_pose
pos.reference_frame = self.from_frame
pub.publish(pos)
def print_commands(self, pose, decimals=4):
rospy.Subscriber("/posestamped", PoseStamped, callback, pose)
def makeArrow(self, msg):
marker = Marker()
# An arrow that's squashed to easier view the orientation on roll
marker.type = Marker.ARROW
marker.scale.x = 0.08
marker.scale.y = 0.08
marker.scale.z = 0.08
marker.color.r = 0.3
marker.color.g = 0.3
marker.color.b = 0.5
marker.color.a = 1.0
return marker
def makeBoxControl(self, msg):
control = InteractiveMarkerControl()
control.always_visible = True
control.markers.append(self.makeArrow(msg))
msg.controls.append(control)
return control
def makeGraspIM(self, pose):
"""
:type pose: Pose
"""
int_marker = InteractiveMarker()
int_marker.header.frame_id = self.from_frame
int_marker.pose = pose
int_marker.scale = 0.3
int_marker.name = "6dof_eef"
int_marker.description = "transform from " + self.from_frame + " to " + self.to_frame
# insert a box, well, an arrow
self.makeBoxControl(int_marker)
int_marker.controls[
0].interaction_mode = InteractiveMarkerControl.MOVE_ROTATE_3D
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 1
control.orientation.y = 0
control.orientation.z = 0
control.name = "rotate_x"
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
control.orientation_mode = InteractiveMarkerControl.FIXED
int_marker.controls.append(control)
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 1
control.orientation.y = 0
control.orientation.z = 0
control.name = "move_x"
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
control.orientation_mode = InteractiveMarkerControl.FIXED
int_marker.controls.append(control)
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 0
control.orientation.y = 1
control.orientation.z = 0
control.name = "rotate_z"
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
control.orientation_mode = InteractiveMarkerControl.FIXED
int_marker.controls.append(control)
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 0
control.orientation.y = 1
control.orientation.z = 0
control.name = "move_z"
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
control.orientation_mode = InteractiveMarkerControl.FIXED
int_marker.controls.append(control)
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 0
control.orientation.y = 0
control.orientation.z = 1
control.name = "rotate_y"
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
control.orientation_mode = InteractiveMarkerControl.FIXED
int_marker.controls.append(control)
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 0
control.orientation.y = 0
control.orientation.z = 1
control.name = "move_y"
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
control.orientation_mode = InteractiveMarkerControl.FIXED
int_marker.controls.append(control)
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 0
control.orientation.y = 0
control.orientation.z = 1
control.name = "move_3d"
control.interaction_mode = InteractiveMarkerControl.MOVE_3D
control.orientation_mode = InteractiveMarkerControl.FIXED
int_marker.controls.append(control)
self.menu_handler.insert("Publish transform",
callback=self.processFeedback)
self.menu_handler.insert("Stop publishing transform",
callback=self.processFeedback)
self.server.insert(int_marker, self.processFeedback)
self.menu_handler.apply(self.server, int_marker.name)
def run(self):
r = pi / 2.0
p = pi
y = pi / 2.0
o = quaternion_from_euler(r, p, y)
opt_frame = self.to_frame + "_rgb_optical_frame"
r = rospy.Rate(20)
while not rospy.is_shutdown():
if self.tf_br is not None:
pos = self.last_pose.position
ori = self.last_pose.orientation
self.tf_br.sendTransform(
(pos.x, pos.y, pos.z), (ori.x, ori.y, ori.z, ori.w),
rospy.Time.now(), self.to_frame + "_link", self.from_frame)
ps = PoseStamped()
ps.pose = self.last_pose
ps.header.frame_id = self.from_frame
ps.header.stamp = rospy.Time.now()
self.pub.publish(ps)
br = tf.TransformBroadcaster()
br.sendTransform((0, 0, 0), o, rospy.Time.now(), opt_frame,
self.to_frame + "_link")
r.sleep()
class ArmControlMarker:
'''Marker for visualizing the steps of an action.'''
_im_server = None
def __init__(self, arm):
'''
Args:
arm (Arm): Arm object for the right or left PR2 arm.
'''
if ArmControlMarker._im_server is None:
im_server = InteractiveMarkerServer('interactive_arm_control')
ArmControlMarker._im_server = im_server
self._arm = arm
self._is_control_visible = False
self._menu_handler = None
self._prev_is_reachable = None
self._pose = self._arm.get_ee_state()
self._lock = threading.Lock()
def update(self):
self._menu_handler = MenuHandler()
# Inset main menu entries.
self._menu_handler.insert(
'Move gripper here', callback=self.move_to_cb)
self._menu_handler.insert(
'Move marker to current gripper pose',
callback=self.move_pose_to_cb)
if self._is_hand_open():
self._menu_handler.insert(
'Close gripper',
callback=self.close_gripper_cb)
else:
self._menu_handler.insert(
'Open gripper',
callback=self.open_gripper_cb)
menu_control = InteractiveMarkerControl()
menu_control.interaction_mode = InteractiveMarkerControl.BUTTON
menu_control.always_visible = True
frame_id = REF_FRAME
pose = self.get_pose()
menu_control = self._make_gripper_marker(
menu_control, self._is_hand_open())
# Make and add interactive marker.
int_marker = InteractiveMarker()
int_marker.name = self._get_name()
int_marker.header.frame_id = frame_id
int_marker.pose = pose
int_marker.scale = INT_MARKER_SCALE
self._add_6dof_marker(int_marker, True)
int_marker.controls.append(menu_control)
ArmControlMarker._im_server.insert(
int_marker, self.marker_feedback_cb)
self._menu_handler.apply(ArmControlMarker._im_server,
self._get_name())
ArmControlMarker._im_server.applyChanges()
def reset(self):
self.set_new_pose(self._arm.get_ee_state(), is_offset=True)
@staticmethod
def get_pose_from_transform(transform):
'''Returns pose for transformation matrix.
Args:
transform (Matrix3x3): (I think this is the correct type.
See ActionStepMarker as a reference for how to use.)
Returns:
Pose
'''
pos = transform[:3, 3].copy()
rot = tf.transformations.quaternion_from_matrix(transform)
return Pose(
Point(pos[0], pos[1], pos[2]),
Quaternion(rot[0], rot[1], rot[2], rot[3])
)
@staticmethod
def get_matrix_from_pose(pose):
'''Returns the transformation matrix for given pose.
Args:
pose (Pose)
Returns:
Matrix3x3: (I think this is the correct type. See
ActionStepMarker as a reference for how to use.)
'''
pp, po = pose.position, pose.orientation
rotation = [po.x, po.y, po.z, po.w]
transformation = tf.transformations.quaternion_matrix(rotation)
position = [pp.x, pp.y, pp.z]
transformation[:3, 3] = position
return transformation
@staticmethod
def _offset_pose(pose, constant=1):
'''Offsets the world pose for visualization.
Args:
pose (Pose): The pose to offset.
constant (int, optional): How much to scale the set offset
by (scales DEFAULT_OFFSET). Defaults to 1.
Returns:
Pose: The offset pose.
'''
transform = ArmControlMarker.get_matrix_from_pose(pose)
offset_array = [constant * DEFAULT_OFFSET, 0, 0]
offset_transform = tf.transformations.translation_matrix(offset_array)
hand_transform = tf.transformations.concatenate_matrices(
transform, offset_transform)
return ArmControlMarker.get_pose_from_transform(hand_transform)
def get_uid(self):
'''Returns a unique id for this marker.
Returns:
int: A number that is unique given the arm
index.
'''
return self._arm.arm_index
def destroy(self):
'''Removes marker from the world.'''
ArmControlMarker._im_server.erase(self._get_name())
ArmControlMarker._im_server.applyChanges()
def set_new_pose(self, new_pose, is_offset=False):
'''Changes the pose of the action step to new_pose.
Args:
new_pose (Pose)
'''
self._lock.acquire()
if is_offset:
self._pose = new_pose
else:
self._pose = ArmControlMarker._offset_pose(new_pose, -1)
self._lock.release()
@staticmethod
def copy_pose(pose):
copy = Pose(
Point(pose.position.x, pose.position.y, pose.position.z),
Quaternion(pose.orientation.x,
pose.orientation.y,
pose.orientation.z,
pose.orientation.w)
)
return copy
def get_pose(self):
'''Returns the pose of the action step.
Returns:
Pose
'''
self._lock.acquire()
pose = ArmControlMarker.copy_pose(self._pose)
self._lock.release()
return ArmControlMarker._offset_pose(pose)
def marker_feedback_cb(self, feedback):
'''Callback for when an event occurs on the marker.
Args:
feedback (InteractiveMarkerFeedback)
'''
if feedback.event_type == InteractiveMarkerFeedback.POSE_UPDATE:
self.set_new_pose(feedback.pose)
elif feedback.event_type == InteractiveMarkerFeedback.BUTTON_CLICK:
# Set the visibility of the 6DOF controller.
# This happens a ton, and doesn't need to be logged like
# normal events (e.g. clicking on most marker controls
# fires here).
rospy.logdebug('Changing visibility of the pose controls.')
self._is_control_visible = not self._is_control_visible
else:
# This happens a ton, and doesn't need to be logged like
# normal events (e.g. clicking on most marker controls
# fires here).
rospy.logdebug('Unknown event: ' + str(feedback.event_type))
def open_gripper_cb(self, __):
self._arm.open_gripper()
def close_gripper_cb(self, __):
self._arm.close_gripper()
def move_to_cb(self, __):
'''Callback for when moving to a pose is requested.
Args:
__ (???): Unused
'''
self._lock.acquire()
pose = ArmControlMarker.copy_pose(self._pose)
self._lock.release()
target_joints = self._arm.get_ik_for_ee(
pose, self._arm.get_joint_state())
if target_joints is not None:
time_to_pose = self._arm.get_time_to_pose(self.get_pose())
thread = threading.Thread(
group=None,
target=self._arm.move_to_joints,
args=(target_joints, time_to_pose),
name='move_to_arm_state_thread'
)
thread.start()
# Log
side_str = self._arm.side()
rospy.loginfo('Started thread to move ' + side_str + ' arm.')
else:
rospy.loginfo('Will not move ' + side_str + ' arm; unreachable.')
def move_pose_to_cb(self, __):
'''Callback for when a pose change to current is requested.
Args:
__ (???): Unused
'''
self.reset()
def _is_reachable(self):
'''Checks and returns whether there is an IK solution for this
action step.
Returns:
bool: Whether this action step is reachable.
'''
self._lock.acquire()
pose = ArmControlMarker.copy_pose(self._pose)
self._lock.release()
target_joints = self._arm.get_ik_for_ee(
pose, self._arm.get_joint_state())
is_reachable = (target_joints is not None)
# A bit more complicated logging to avoid spamming the logs
# while still giving useful info. It now logs when reachability
# is first calculated, or changes.
report = False
# See if we've set reachability for the first time.
if self._prev_is_reachable is None:
report = True
reachable_str = (
'is reachable' if is_reachable else 'is not reachable')
# See if we've got a different reachability than we had before.
elif self._prev_is_reachable != is_reachable:
report = True
reachable_str = (
'is now reachable' if is_reachable else
'is no longer reachable')
# Log if it's changed.
if report:
rospy.loginfo(self._arm.side() + ':' + reachable_str)
# Cache and return.
self._prev_is_reachable = is_reachable
return is_reachable
def _get_name(self):
'''Generates the unique name for the marker.
Returns:
str: A human-readable unique name for the marker.
'''
return 'arm' + str(self._arm.arm_index)
def _is_hand_open(self):
'''Returns whether the gripper is open for this action step.
Returns:
bool
'''
return self._arm.get_gripper_state() == GripperState.OPEN
def _add_6dof_marker(self, int_marker, is_fixed):
'''Adds a 6 DoF control marker to the interactive marker.
Args:
int_marker (InteractiveMarker)
is_fixed (bool): Looks like whether position is fixed (?).
Currently only passed as True.
'''
# Each entry in options is (name, orientation, is_move)
options = [
('rotate_x', Quaternion(1, 0, 0, 1), False),
('move_x', Quaternion(1, 0, 0, 1), True),
('rotate_z', Quaternion(0, 1, 0, 1), False),
('move_z', Quaternion(0, 1, 0, 1), True),
('rotate_y', Quaternion(0, 0, 1, 1), False),
('move_y', Quaternion(0, 0, 1, 1), True),
]
for opt in options:
name, orient, is_move = opt
control = self._make_6dof_control(
name, orient, is_move, is_fixed)
int_marker.controls.append(control)
def _make_6dof_control(self, name, orientation, is_move, is_fixed):
'''Creates and returns one component of the 6dof controller.
Args:
name (str): Name for hte control
orientation (Quaternion): How the control should be
oriented.
is_move (bool): Looks like whether the marker moves the
object (?). Currently passed as True for moving markers,
False for rotating ones.
is_fixed (bool): Looks like whether position is fixed (?).
Currently always passed as True.
Returns:
InteractiveMarkerControl
'''
control = InteractiveMarkerControl()
control.name = name
control.orientation = orientation
control.always_visible = False
if self._is_control_visible:
if is_move:
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
else:
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
else:
control.interaction_mode = InteractiveMarkerControl.NONE
if is_fixed:
control.orientation_mode = InteractiveMarkerControl.FIXED
return control
def _make_mesh_marker(self):
'''Creates and returns a mesh marker.
Returns:
Marker
'''
mesh = Marker()
mesh.mesh_use_embedded_materials = False
mesh.type = Marker.MESH_RESOURCE
mesh.scale.x = GRIPPER_MARKER_SCALE
mesh.scale.y = GRIPPER_MARKER_SCALE
mesh.scale.z = GRIPPER_MARKER_SCALE
mesh.color = self._get_mesh_marker_color()
return mesh
def _get_mesh_marker_color(self):
'''Gets the color for the mesh marker (thing that looks like a
gripper) for this step.
A simple implementation of this will return one color for
reachable poses, another for unreachable ones. Future
implementations may provide further visual cues.
Returns:
ColorRGBA: The color for the gripper mesh for this step.
'''
if self._is_reachable():
return COLOR_MESH_REACHABLE
else:
return COLOR_MESH_UNREACHABLE
def _make_gripper_marker(self, control, is_hand_open=False):
'''Makes a gripper marker, adds it to control, returns control.
Args:
control (InteractiveMarkerControl): IM Control we're using.
is_hand_open (bool, optional): Whether the gripper is open.
Defaults to False (closed).
Returns:
InteractiveMarkerControl: The passed control.
'''
# Set angle of meshes based on gripper open vs closed.
angle = ANGLE_GRIPPER_OPEN if is_hand_open else ANGLE_GRIPPER_CLOSED
# Make transforms in preparation for meshes 1, 2, and 3.
# NOTE(mbforbes): There are some magic numbers in here that are
# used a couple times. Seems like a good candidate for
# refactoring to constants, but I think they're more clear if
# left in here as (a) they likely won't be changed, and (b) it's
# easier to understand the computations with them here.
transform1 = tf.transformations.euler_matrix(0, 0, angle)
transform1[:3, 3] = [0.07691 - DEFAULT_OFFSET, 0.01, 0]
transform2 = tf.transformations.euler_matrix(0, 0, -angle)
transform2[:3, 3] = [0.09137, 0.00495, 0]
t_proximal = transform1
t_distal = tf.transformations.concatenate_matrices(
transform1, transform2)
# Create mesh 1 (palm).
mesh1 = self._make_mesh_marker()
mesh1.mesh_resource = STR_GRIPPER_PALM_FILE
mesh1.pose.position.x = -DEFAULT_OFFSET
mesh1.pose.orientation.w = 1
# Create mesh 2 (finger).
mesh2 = self._make_mesh_marker()
mesh2.mesh_resource = STR_GRIPPER_FINGER_FILE
mesh2.pose = ArmControlMarker.get_pose_from_transform(t_proximal)
# Create mesh 3 (fingertip).
mesh3 = self._make_mesh_marker()
mesh3.mesh_resource = STR_GRIPPER_FINGERTIP_FILE
mesh3.pose = ArmControlMarker.get_pose_from_transform(t_distal)
# Make transforms in preparation for meshes 4 and 5.
quat = tf.transformations.quaternion_multiply(
tf.transformations.quaternion_from_euler(numpy.pi, 0, 0),
tf.transformations.quaternion_from_euler(0, 0, angle)
)
transform1 = tf.transformations.quaternion_matrix(quat)
transform1[:3, 3] = [0.07691 - DEFAULT_OFFSET, -0.01, 0]
transform2 = tf.transformations.euler_matrix(0, 0, -angle)
transform2[:3, 3] = [0.09137, 0.00495, 0]
t_proximal = transform1
t_distal = tf.transformations.concatenate_matrices(
transform1, transform2)
# Create mesh 4 (other finger).
mesh4 = self._make_mesh_marker()
mesh4.mesh_resource = STR_GRIPPER_FINGER_FILE
mesh4.pose = ArmControlMarker.get_pose_from_transform(t_proximal)
# Create mesh 5 (other fingertip).
mesh5 = self._make_mesh_marker()
mesh5.mesh_resource = STR_GRIPPER_FINGERTIP_FILE
mesh5.pose = ArmControlMarker.get_pose_from_transform(t_distal)
# Append all meshes we made.
control.markers.append(mesh1)
control.markers.append(mesh2)
control.markers.append(mesh3)
control.markers.append(mesh4)
control.markers.append(mesh5)
# Return back the control.
# TODO(mbforbes): Why do we do this?
return control
class GoalMarkerServer(object):
def __init__(self, add_goal_service, remove_goal_service,
modify_goal_service):
# Service that will be called
self.add_goal_service = add_goal_service
self.remove_goal_service = remove_goal_service
self.modify_goal_service = modify_goal_service
# Start marker server
self.server = InteractiveMarkerServer("goal_markers")
# Init the marker array and mutex
self.displayed_goals = list()
self.mutex = RLock()
# Init the menu handler and entries
self.init_menu()
def init_menu(self):
self.menu_handler = MenuHandler()
# Remove Goal entry
self.menu_remove_goal_title = "Remove Goal"
self.menu_remove_goal_handle = self.menu_handler.insert(
self.menu_remove_goal_title, callback=self.process_feedback)
def process_feedback(self, feedback):
# The goal was moved
if feedback.event_type == viz_msg.InteractiveMarkerFeedback.MOUSE_UP:
goal_id = feedback.marker_name
new_pose = PoseStamped()
new_pose.pose = feedback.pose
new_pose.header = feedback.header
try:
modify_goal_handle = rospy.ServiceProxy(
self.modify_goal_service, ModifyGoal)
success = modify_goal_handle(new_pose, goal_id).success
if not success:
rospy.logwarn(
"Failure in modifying goal %s in goal marker server" %
goal_id)
except rospy.ServiceException, error:
rospy.logerr(
"Service call failed for modify_goal in goal marker server: %s"
% error)
# A menu entry was clicked
if feedback.event_type == viz_msg.InteractiveMarkerFeedback.MENU_SELECT:
# The goal needs to be removed
if feedback.menu_entry_id == self.menu_remove_goal_handle:
goal_id = feedback.marker_name
try:
remove_goal_handle = rospy.ServiceProxy(
self.remove_goal_service, RemoveGoal)
success = remove_goal_handle(goal_id).success
if not success:
rospy.logwarn(
"Failure in removing goal %s in goal marker server"
% goal_id)
except rospy.ServiceException, error:
rospy.logerr(
"Service call failed for remove_goal in goal marker server: %s"
% error)
class WorldLandmark:
"""WorldLandmark represents a task-relevant part of the scene.
This could include an object, part of an object, or stationary parts of the
scene.
There are two kinds of landmarks: bounding boxes and cloud boxes.
A bounding box is found by segmenting whole objects from a tabletop scene.
The only shape information kept by a bounding box are its dimensions.
A cloud box is a point cloud surrounded by a bounding box. The point cloud
does not necessarily have to be on a tabletop. The bounding box specifies a
margin of empty space that is expected to be around the point cloud. The
point cloud is localized in the scene using ICP.
To create these two kinds of landmarks:
landmark = WorldLandmark.bounding_box(name, pose, dimensions)
landmark = WorldLandmark.cloud_box(name, pose, dimensions, db_id)
All poses are assumed to be relative to the robot's base frame.
The point cloud for cloud boxes are stored in MongoDB.
"""
def __init__(self, name, pose, dimensions, db_id):
"""
Constructs a WorldLandmark object.
Use bounding_box() or cloud_box() instead.
Args:
name (str): Name of this landmark
pose (Pose): Pose of the landmark, in base frame
dimensions (Vector3): Size of bounding box
db_id (str): The MongoDB id if this is a cloud box
"""
self._name = name
self._pose = pose
self._dimensions = dimensions
self._db_id = db_id
if self.is_bounding_box():
self.object = Landmark(type=Landmark.TABLE_TOP,
name=name,
pose=pose,
dimensions=dimensions,
db_id='')
elif self.is_cloud_box():
self.object = Landmark(type=Landmark.CLOUD_BOX,
name=name,
pose=pose,
dimensions=dimensions,
db_id=db_id)
self.int_marker = None
self.is_removed = False
# TODO(jstn): Move this outside.
self.menu_handler = MenuHandler()
self.menu_handler.insert('Remove from scene', callback=self.remove)
@staticmethod
def bounding_box(name, pose, dimensions):
"""Construct a bounding box landmark.
Args:
name (str): The name of this landmark.
pose (Pose): The pose of the bounding box center, in the base frame
dimensions (Vector3): The x, y, and z lengths of the bounding box
"""
return WorldLandmark(name, pose, dimensions, None)
@staticmethod
def cloud_box(name, pose, dimensions, db_id):
"""Construct a bounding box landmark.
Args:
name (str): The name of this landmark.
pose (Pose): The pose of the bounding box center, in the base frame
dimensions (Vector3): The x, y, and z lengths of the bounding box
around this landmark.
db_id (str): MongoDB ID of this landmark.
"""
return WorldLandmark(name, pose, dimensions, db_id)
@staticmethod
def from_msg(msg):
"""Construct from a Landmark msg.
"""
db_id = None
if msg.db_id is not None and msg.db_id != '':
db_id = msg.db_id
return WorldLandmark(msg.name, msg.pose, msg.dimensions, db_id)
def is_bounding_box(self):
"""Returns true if this is a bounding box type landmark."""
return self._db_id is None
def is_cloud_box(self):
"""Returns true if this is a cloud box type landmark."""
return self._db_id is not None
def name(self):
"""Returns the name of this landmark."""
return self._name
def remove(self, __=None):
"""Sets the is_removed flag on this landmark.
Args:
__: Unused
"""
rospy.loginfo('Will remove object: ' + self._name)
self.is_removed = True
class GripperMarkers:
_offset = -0.09
_tf_listener = None
def __init__(self, side_prefix):
if GripperMarkers._tf_listener is None:
GripperMarkers._tf_listener = TransformListener()
self.ik = IK(side_prefix)
self.side_prefix = side_prefix
self._im_server = InteractiveMarkerServer(side_prefix + '_ik_request_markers')
self._menu_handler = MenuHandler()
self._menu_handler.insert('Move arm here', callback=self.move_to_pose_cb)
self.is_control_visible = False
self.marker_pose = self._offset_pose(self.get_ee_pose(), -GripperMarkers._offset)
self.update_viz()
self._menu_handler.apply(self._im_server, 'ik_target_marker')
self._im_server.applyChanges()
def get_ee_pose(self):
from_frame = '/base_link'
to_frame = '/' + self.side_prefix + '_wrist_roll_link'
try:
GripperMarkers._tf_listener.waitForTransform(from_frame, to_frame, rospy.Time(0), rospy.Duration(5))
t = GripperMarkers._tf_listener.getLatestCommonTime(from_frame, to_frame)
(pos, rot) = GripperMarkers._tf_listener.lookupTransform(from_frame, to_frame, t)
except:
rospy.logwarn('Could not get end effector pose through TF.')
pos = [1.0, 0.0, 1.0]
rot = [0.0, 0.0, 0.0, 1.0]
return Pose(Point(pos[0], pos[1], pos[2]),
Quaternion(rot[0], rot[1], rot[2], rot[3]))
def move_to_pose_cb(self, dummy):
rospy.loginfo('You pressed the `Move arm here` button from the menu.')
target_pose = GripperMarkers._offset_pose(self.marker_pose, GripperMarkers._offset)
ik_solution = self.ik.get_ik_for_ee(target_pose)
if (ik_solution == None):
rospy.logwarn('No IK solutions for this pose, cannot move.')
else:
self.ik.move_to_joints(ik_solution, 2.0)
pass
def marker_clicked_cb(self, feedback):
if feedback.event_type == InteractiveMarkerFeedback.POSE_UPDATE:
self.marker_pose = feedback.pose
self.update_viz()
self._menu_handler.reApply(self._im_server)
self._im_server.applyChanges()
elif feedback.event_type == InteractiveMarkerFeedback.BUTTON_CLICK:
rospy.loginfo('Changing visibility of the pose controls.')
if (self.is_control_visible):
self.is_control_visible = False
else:
self.is_control_visible = True
else:
rospy.loginfo('Unhandled event: ' + str(feedback.event_type))
def update_viz(self):
menu_control = InteractiveMarkerControl()
menu_control.interaction_mode = InteractiveMarkerControl.BUTTON
menu_control.always_visible = True
frame_id = 'base_link'
pose = self.marker_pose
menu_control = self._add_gripper_marker(menu_control)
text_pos = Point()
text_pos.x = pose.position.x
text_pos.y = pose.position.y
text_pos.z = pose.position.z + 0.1
#text = 'x=' + str(pose.position.x) + ' y=' + str(pose.position.y) + ' x=' + str(pose.position.z)
text = self.side_prefix + '_arm'
menu_control.markers.append(Marker(type=Marker.TEXT_VIEW_FACING,
id=0, scale=Vector3(0, 0, 0.05),
text=text,
color=ColorRGBA(0.0, 0.0, 0.0, 0.5),
header=Header(frame_id=frame_id),
pose=Pose(text_pos, Quaternion(0, 0, 0, 1))))
int_marker = InteractiveMarker()
int_marker.name = 'ik_target_marker'
int_marker.header.frame_id = frame_id
int_marker.pose = pose
int_marker.scale = 0.2
self._add_6dof_marker(int_marker)
int_marker.controls.append(menu_control)
self._im_server.insert(int_marker, self.marker_clicked_cb)
def _add_gripper_marker(self, control):
is_hand_open=False
if is_hand_open:
angle = 28 * numpy.pi / 180.0
else:
angle = 0
offset = -GripperMarkers._offset
transform1 = tf.transformations.euler_matrix(0, 0, angle)
transform1[:3, 3] = [0.07691 - offset, 0.01, 0]
transform2 = tf.transformations.euler_matrix(0, 0, -angle)
transform2[:3, 3] = [0.09137, 0.00495, 0]
t_proximal = transform1
t_distal = tf.transformations.concatenate_matrices(transform1, transform2)
mesh1 = self._make_mesh_marker()
mesh1.mesh_resource = ('package://pr2_description/meshes/gripper_v0/gripper_palm.dae')
mesh1.pose.position.x = -offset
mesh1.pose.orientation.w = 1
mesh2 = self._make_mesh_marker()
mesh2.mesh_resource = ('package://pr2_description/meshes/gripper_v0/l_finger.dae')
mesh2.pose = GripperMarkers.get_pose_from_transform(t_proximal)
mesh3 = self._make_mesh_marker()
mesh3.mesh_resource = ('package://pr2_description/meshes/gripper_v0/l_finger_tip.dae')
mesh3.pose = GripperMarkers.get_pose_from_transform(t_distal)
quat = tf.transformations.quaternion_multiply(
tf.transformations.quaternion_from_euler(numpy.pi, 0, 0),
tf.transformations.quaternion_from_euler(0, 0, angle))
transform1 = tf.transformations.quaternion_matrix(quat)
transform1[:3, 3] = [0.07691 - offset, -0.01, 0]
transform2 = tf.transformations.euler_matrix(0, 0, -angle)
transform2[:3, 3] = [0.09137, 0.00495, 0]
t_proximal = transform1
t_distal = tf.transformations.concatenate_matrices(transform1,transform2)
mesh4 = self._make_mesh_marker()
mesh4.mesh_resource = ('package://pr2_description/meshes/gripper_v0/l_finger.dae')
mesh4.pose = GripperMarkers.get_pose_from_transform(t_proximal)
mesh5 = self._make_mesh_marker()
mesh5.mesh_resource = ('package://pr2_description/meshes/gripper_v0/l_finger_tip.dae')
mesh5.pose = GripperMarkers.get_pose_from_transform(t_distal)
control.markers.append(mesh1)
control.markers.append(mesh2)
control.markers.append(mesh3)
control.markers.append(mesh4)
control.markers.append(mesh5)
return control
@staticmethod
def get_pose_from_transform(transform):
pos = transform[:3,3].copy()
rot = tf.transformations.quaternion_from_matrix(transform)
return Pose(Point(pos[0], pos[1], pos[2]),
Quaternion(rot[0], rot[1], rot[2], rot[3]))
@staticmethod
def _offset_pose(pose, x_offset):
transform = GripperMarkers.get_matrix_from_pose(pose)
offset_array = [x_offset, 0, 0]
offset_transform = tf.transformations.translation_matrix(offset_array)
hand_transform = tf.transformations.concatenate_matrices(transform,
offset_transform)
return GripperMarkers.get_pose_from_transform(hand_transform)
@staticmethod
def get_matrix_from_pose(pose):
rotation = [pose.orientation.x, pose.orientation.y,
pose.orientation.z, pose.orientation.w]
transformation = tf.transformations.quaternion_matrix(rotation)
position = [pose.position.x, pose.position.y, pose.position.z]
transformation[:3, 3] = position
return transformation
def _make_mesh_marker(self):
mesh = Marker()
mesh.mesh_use_embedded_materials = False
mesh.type = Marker.MESH_RESOURCE
mesh.scale.x = 1.0
mesh.scale.y = 1.0
mesh.scale.z = 1.0
target_pose = GripperMarkers._offset_pose(self.marker_pose, GripperMarkers._offset)
ik_solution = self.ik.get_ik_for_ee(target_pose)
if (ik_solution == None):
mesh.color = ColorRGBA(1.0, 0.0, 0.0, 0.6)
else:
mesh.color = ColorRGBA(0.0, 1.0, 0.0, 0.6)
return mesh
def _add_6dof_marker(self, int_marker):
is_fixed = True
control = self._make_6dof_control('rotate_x', Quaternion(1, 0, 0, 1), False, is_fixed)
int_marker.controls.append(control)
control = self._make_6dof_control('move_x', Quaternion(1, 0, 0, 1), True, is_fixed)
int_marker.controls.append(control)
control = self._make_6dof_control('rotate_z', Quaternion(0, 1, 0, 1), False, is_fixed)
int_marker.controls.append(control)
control = self._make_6dof_control('move_z', Quaternion(0, 1, 0, 1), True, is_fixed)
int_marker.controls.append(control)
control = self._make_6dof_control('rotate_y', Quaternion(0, 0, 1, 1), False, is_fixed)
int_marker.controls.append(control)
control = self._make_6dof_control('move_y', Quaternion(0, 0, 1, 1), True, is_fixed)
int_marker.controls.append(control)
def _make_6dof_control(self, name, orientation, is_move, is_fixed):
control = InteractiveMarkerControl()
control.name = name
control.orientation = orientation
control.always_visible = False
if (self.is_control_visible):
if is_move:
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
else:
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
else:
control.interaction_mode = InteractiveMarkerControl.NONE
if is_fixed:
control.orientation_mode = InteractiveMarkerControl.FIXED
return control
def create_menu(self):
"""
Create and populates all the menu entries
"""
menu_handler = MenuHandler()
menu_handler.insert("Point the head",
callback = self.move_head)
menu_handler.insert("Add position to trajectory",
callback = self.add_point)
menu_handler.insert("Place marker over gripper",
callback = self.place_gripper)
menu_handler.insert("Execute trjectory",
callback = self.execute_trajectory)
menu_handler.insert("Clear trajectory",
callback = self.clear_trajectory)
menu_handler.insert("Publish trajectory",
callback = self.publish_trajectory)
menu_handler.insert("Move the arm (planning)",
callback = self.plan_arm)
menu_handler.insert("Move the arm (collision-free)",
callback = self.collision_free_arm)
menu_handler.insert("Move the arm to trajectory start (collision-free)",
callback = self.arm_trajectory_start)
menu_handler.insert("Update planning scene",
callback = self.update_planning)
menu_handler.apply(self.server, self.int_marker.name)
class GripperMarkers:
_offset = 0.09
def __init__(self, side_prefix):
self.ik = IK(side_prefix)
self.side_prefix = side_prefix
self._im_server = InteractiveMarkerServer('ik_request_markers')
self._tf_listener = TransformListener()
self._menu_handler = MenuHandler()
self._menu_handler.insert('Move arm here', callback=self.move_to_pose_cb)
self.is_control_visible = False
self.ee_pose = self.get_ee_pose()
self.update_viz()
self._menu_handler.apply(self._im_server, 'ik_target_marker')
self._im_server.applyChanges()
def get_ee_pose(self):
from_frame = '/base_link'
to_frame = '/' + self.side_prefix + '_wrist_roll_link'
try:
t = self._tf_listener.getLatestCommonTime(from_frame, to_frame)
(pos, rot) = self._tf_listener.lookupTransform(from_frame, to_frame, t)
except:
rospy.logwarn('Could not get end effector pose through TF.')
pos = [1.0, 0.0, 1.0]
rot = [0.0, 0.0, 0.0, 1.0]
return Pose(Point(pos[0], pos[1], pos[2]),
Quaternion(rot[0], rot[1], rot[2], rot[3]))
def move_to_pose_cb(self, dummy):
rospy.loginfo('You pressed the `Move arm here` button from the menu.')
target_pose = GripperMarkers._offset_pose(self.ee_pose)
ik_solution = self.ik.get_ik_for_ee(target_pose)
#TODO: Send the arms to ik_solution
def marker_clicked_cb(self, feedback):
if feedback.event_type == InteractiveMarkerFeedback.POSE_UPDATE:
self.ee_pose = feedback.pose
self.update_viz()
self._menu_handler.reApply(self._im_server)
self._im_server.applyChanges()
elif feedback.event_type == InteractiveMarkerFeedback.BUTTON_CLICK:
rospy.loginfo('Changing visibility of the pose controls.')
if (self.is_control_visible):
self.is_control_visible = False
else:
self.is_control_visible = True
else:
rospy.loginfo('Unhandled event: ' + str(feedback.event_type))
def update_viz(self):
menu_control = InteractiveMarkerControl()
menu_control.interaction_mode = InteractiveMarkerControl.BUTTON
menu_control.always_visible = True
frame_id = 'base_link'
pose = self.ee_pose
menu_control = self._add_gripper_marker(menu_control)
text_pos = Point()
text_pos.x = pose.position.x
text_pos.y = pose.position.y
text_pos.z = pose.position.z + 0.1
text = 'x=' + str(pose.position.x) + ' y=' + str(pose.position.y) + ' x=' + str(pose.position.z)
menu_control.markers.append(Marker(type=Marker.TEXT_VIEW_FACING,
id=0, scale=Vector3(0, 0, 0.03),
text=text,
color=ColorRGBA(0.0, 0.0, 0.0, 0.5),
header=Header(frame_id=frame_id),
pose=Pose(text_pos, Quaternion(0, 0, 0, 1))))
int_marker = InteractiveMarker()
int_marker.name = 'ik_target_marker'
int_marker.header.frame_id = frame_id
int_marker.pose = pose
int_marker.scale = 0.2
self._add_6dof_marker(int_marker)
int_marker.controls.append(menu_control)
self._im_server.insert(int_marker, self.marker_clicked_cb)
def _add_gripper_marker(self, control):
is_hand_open=False
if is_hand_open:
angle = 28 * numpy.pi / 180.0
else:
angle = 0
transform1 = tf.transformations.euler_matrix(0, 0, angle)
transform1[:3, 3] = [0.07691 - GripperMarkers._offset, 0.01, 0]
transform2 = tf.transformations.euler_matrix(0, 0, -angle)
transform2[:3, 3] = [0.09137, 0.00495, 0]
t_proximal = transform1
t_distal = tf.transformations.concatenate_matrices(transform1, transform2)
mesh1 = self._make_mesh_marker()
mesh1.mesh_resource = ('package://pr2_description/meshes/gripper_v0/gripper_palm.dae')
mesh1.pose.position.x = -GripperMarkers._offset
mesh1.pose.orientation.w = 1
mesh2 = self._make_mesh_marker()
mesh2.mesh_resource = ('package://pr2_description/meshes/gripper_v0/l_finger.dae')
mesh2.pose = GripperMarkers.get_pose_from_transform(t_proximal)
mesh3 = self._make_mesh_marker()
mesh3.mesh_resource = ('package://pr2_description/meshes/gripper_v0/l_finger_tip.dae')
mesh3.pose = GripperMarkers.get_pose_from_transform(t_distal)
quat = tf.transformations.quaternion_multiply(
tf.transformations.quaternion_from_euler(numpy.pi, 0, 0),
tf.transformations.quaternion_from_euler(0, 0, angle))
transform1 = tf.transformations.quaternion_matrix(quat)
transform1[:3, 3] = [0.07691 - GripperMarkers._offset, -0.01, 0]
transform2 = tf.transformations.euler_matrix(0, 0, -angle)
transform2[:3, 3] = [0.09137, 0.00495, 0]
t_proximal = transform1
t_distal = tf.transformations.concatenate_matrices(transform1,transform2)
mesh4 = self._make_mesh_marker()
mesh4.mesh_resource = ('package://pr2_description/meshes/gripper_v0/l_finger.dae')
mesh4.pose = GripperMarkers.get_pose_from_transform(t_proximal)
mesh5 = self._make_mesh_marker()
mesh5.mesh_resource = ('package://pr2_description/meshes/gripper_v0/l_finger_tip.dae')
mesh5.pose = GripperMarkers.get_pose_from_transform(t_distal)
control.markers.append(mesh1)
control.markers.append(mesh2)
control.markers.append(mesh3)
control.markers.append(mesh4)
control.markers.append(mesh5)
return control
@staticmethod
def get_pose_from_transform(transform):
pos = transform[:3,3].copy()
rot = tf.transformations.quaternion_from_matrix(transform)
return Pose(Point(pos[0], pos[1], pos[2]),
Quaternion(rot[0], rot[1], rot[2], rot[3]))
@staticmethod
def _offset_pose(pose):
transform = GripperMarkers.get_matrix_from_pose(pose)
offset_array = [GripperMarkers._offset, 0, 0]
offset_transform = tf.transformations.translation_matrix(offset_array)
hand_transform = tf.transformations.concatenate_matrices(transform, offset_transform)
return GripperMarkers.get_pose_from_transform(hand_transform)
@staticmethod
def get_matrix_from_pose(pose):
rotation = [pose.orientation.x, pose.orientation.y,
pose.orientation.z, pose.orientation.w]
transformation = tf.transformations.quaternion_matrix(rotation)
position = [pose.position.x, pose.position.y, pose.position.z]
transformation[:3, 3] = position
return transformation
def _make_mesh_marker(self):
mesh = Marker()
mesh.mesh_use_embedded_materials = False
mesh.type = Marker.MESH_RESOURCE
mesh.scale.x = 1.0
mesh.scale.y = 1.0
mesh.scale.z = 1.0
target_pose = GripperMarkers._offset_pose(self.ee_pose)
ik_solution = self.ik.get_ik_for_ee(target_pose)
if (ik_solution == None):
mesh.color = ColorRGBA(1.0, 0.0, 0.0, 0.6)
else:
mesh.color = ColorRGBA(0.0, 1.0, 0.0, 0.6)
mesh.color = ColorRGBA(0.0, 0.5, 1.0, 0.6)
return mesh
def _add_6dof_marker(self, int_marker):
is_fixed = True
control = self._make_6dof_control('rotate_x', Quaternion(1, 0, 0, 1), False, is_fixed)
int_marker.controls.append(control)
control = self._make_6dof_control('move_x', Quaternion(1, 0, 0, 1), True, is_fixed)
int_marker.controls.append(control)
control = self._make_6dof_control('rotate_z', Quaternion(0, 1, 0, 1), False, is_fixed)
int_marker.controls.append(control)
control = self._make_6dof_control('move_z', Quaternion(0, 1, 0, 1), True, is_fixed)
int_marker.controls.append(control)
control = self._make_6dof_control('rotate_y', Quaternion(0, 0, 1, 1), False, is_fixed)
int_marker.controls.append(control)
control = self._make_6dof_control('move_y', Quaternion(0, 0, 1, 1), True, is_fixed)
int_marker.controls.append(control)
def _make_6dof_control(self, name, orientation, is_move, is_fixed):
control = InteractiveMarkerControl()
control.name = name
control.orientation = orientation
control.always_visible = False
if (self.is_control_visible):
if is_move:
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
else:
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
else:
control.interaction_mode = InteractiveMarkerControl.NONE
if is_fixed:
control.orientation_mode = InteractiveMarkerControl.FIXED
return control
class InteractiveMarkerPoseStampedPublisher():
def __init__(self, from_frame, to_frame, position, orientation):
self.server = InteractiveMarkerServer("posestamped_im")
o = orientation
r, p, y = euler_from_quaternion([o.x, o.y, o.z, o.w])
rospy.loginfo("Publishing transform and PoseStamped from: " +
from_frame + " to " + to_frame + " at " +
str(position.x) + " " + str(position.y) + " " +
str(position.z) + " and orientation " + str(o.x) + " " +
str(o.y) + " " + str(o.z) + " " + str(o.w) + " or rpy " +
str(r) + " " + str(p) + " " + str(y))
self.menu_handler = MenuHandler()
self.from_frame = from_frame
self.to_frame = to_frame
# Transform broadcaster
self.tf_br = tf.TransformBroadcaster()
self.pub = rospy.Publisher('/posestamped', PoseStamped, queue_size=1)
rospy.loginfo("Publishing posestampeds at topic: " +
str(self.pub.name))
pose = Pose()
pose.position = position
pose.orientation = orientation
self.last_pose = pose
self.print_commands(pose)
self.makeGraspIM(pose)
self.server.applyChanges()
def processFeedback(self, feedback):
"""
:type feedback: InteractiveMarkerFeedback
"""
s = "Feedback from marker '" + feedback.marker_name
s += "' / control '" + feedback.control_name + "'"
mp = ""
if feedback.mouse_point_valid:
mp = " at " + str(feedback.mouse_point.x)
mp += ", " + str(feedback.mouse_point.y)
mp += ", " + str(feedback.mouse_point.z)
mp += " in frame " + feedback.header.frame_id
if feedback.event_type == InteractiveMarkerFeedback.BUTTON_CLICK:
rospy.logdebug(s + ": button click" + mp + ".")
elif feedback.event_type == InteractiveMarkerFeedback.MENU_SELECT:
rospy.logdebug(s + ": menu item " + str(feedback.menu_entry_id) +
" clicked" + mp + ".")
if feedback.menu_entry_id == 1:
rospy.logdebug("Start publishing transform...")
if self.tf_br is None:
self.tf_br = tf.TransformBroadcaster()
elif feedback.menu_entry_id == 2:
rospy.logdebug("Stop publishing transform...")
self.tf_br = None
# When clicking this event triggers!
elif feedback.event_type == InteractiveMarkerFeedback.POSE_UPDATE:
rospy.logdebug(s + ": pose changed")
elif feedback.event_type == InteractiveMarkerFeedback.MOUSE_DOWN:
rospy.logdebug(s + ": mouse down" + mp + ".")
elif feedback.event_type == InteractiveMarkerFeedback.MOUSE_UP:
rospy.logdebug(s + ": mouse up" + mp + ".")
# TODO: Print here the commands
# tf static transform
self.print_commands(feedback.pose)
self.last_pose = deepcopy(feedback.pose)
self.server.applyChanges()
def print_commands(self, pose, decimals=4):
p = pose.position
o = pose.orientation
print "\n---------------------------"
print "Static transform publisher command (with roll pitch yaw):"
common_part = "rosrun tf static_transform_publisher "
pos_part = str(round(p.x, decimals)) + " " + str(round(
p.y, decimals)) + " " + str(round(p.z, decimals))
r, p, y = euler_from_quaternion([o.x, o.y, o.z, o.w])
ori_part = str(round(r, decimals)) + " " + str(round(
p, decimals)) + " " + str(round(y, decimals))
static_tf_cmd = common_part + pos_part + " " + ori_part + " " + self.from_frame + " " + self.to_frame + " 50"
print " " + static_tf_cmd
print
print "Static transform publisher command (with quaternion):"
ori_q = str(round(o.x, decimals)) + " " + str(round(
o.y, decimals)) + " " + str(round(o.z, decimals)) + " " + str(
round(o.w, decimals))
static_tf_cmd = common_part + pos_part + " " + ori_q + " " + self.from_frame + " " + self.to_frame + " 50"
print " " + static_tf_cmd
print
print "Roslaunch line of static transform publisher (rpy):"
node_name = "from_" + self.from_frame + "_to_" + self.to_frame + "_static_tf"
roslaunch_part = ' <node name="' + node_name + '" pkg="tf" type="static_transform_publisher" args="' +\
pos_part + " " + ori_part + " " + self.from_frame + " " + self.to_frame + " 50" + '" />'
print roslaunch_part
print
print "URDF format:" # <origin xyz="0.04149 -0.01221 0.001" rpy="0 0 0" />
print ' <origin xyz="' + pos_part + '" rpy="' + ori_part + '" />'
print "\n---------------------------"
def makeArrow(self, msg):
marker = Marker()
# An arrow that's squashed to easier view the orientation on roll
marker.type = Marker.ARROW
marker.scale.x = 0.08
marker.scale.y = 0.08
marker.scale.z = 0.08
marker.color.r = 0.3
marker.color.g = 0.3
marker.color.b = 0.5
marker.color.a = 1.0
return marker
def makeBoxControl(self, msg):
control = InteractiveMarkerControl()
control.always_visible = True
control.markers.append(self.makeArrow(msg))
msg.controls.append(control)
return control
def makeGraspIM(self, pose):
"""
:type pose: Pose
"""
int_marker = InteractiveMarker()
int_marker.header.frame_id = self.from_frame
int_marker.pose = pose
int_marker.scale = 0.3
int_marker.name = "6dof_eef"
int_marker.description = "transform from " + self.from_frame + " to " + self.to_frame
# insert a box, well, an arrow
self.makeBoxControl(int_marker)
int_marker.controls[
0].interaction_mode = InteractiveMarkerControl.MOVE_ROTATE_3D
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 1
control.orientation.y = 0
control.orientation.z = 0
control.name = "rotate_x"
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
control.orientation_mode = InteractiveMarkerControl.FIXED
int_marker.controls.append(control)
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 1
control.orientation.y = 0
control.orientation.z = 0
control.name = "move_x"
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
control.orientation_mode = InteractiveMarkerControl.FIXED
int_marker.controls.append(control)
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 0
control.orientation.y = 1
control.orientation.z = 0
control.name = "rotate_z"
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
control.orientation_mode = InteractiveMarkerControl.FIXED
int_marker.controls.append(control)
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 0
control.orientation.y = 1
control.orientation.z = 0
control.name = "move_z"
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
control.orientation_mode = InteractiveMarkerControl.FIXED
int_marker.controls.append(control)
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 0
control.orientation.y = 0
control.orientation.z = 1
control.name = "rotate_y"
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
control.orientation_mode = InteractiveMarkerControl.FIXED
int_marker.controls.append(control)
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 0
control.orientation.y = 0
control.orientation.z = 1
control.name = "move_y"
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
control.orientation_mode = InteractiveMarkerControl.FIXED
int_marker.controls.append(control)
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 0
control.orientation.y = 0
control.orientation.z = 1
control.name = "move_3d"
control.interaction_mode = InteractiveMarkerControl.MOVE_3D
control.orientation_mode = InteractiveMarkerControl.FIXED
int_marker.controls.append(control)
self.menu_handler.insert("Publish transform",
callback=self.processFeedback)
self.menu_handler.insert("Stop publishing transform",
callback=self.processFeedback)
self.server.insert(int_marker, self.processFeedback)
self.menu_handler.apply(self.server, int_marker.name)
def run(self):
r = pi / 2.0
p = pi
y = pi / 2.0
o = quaternion_from_euler(r, p, y)
r = rospy.Rate(20)
while not rospy.is_shutdown():
if self.tf_br is not None:
pos = self.last_pose.position
ori = self.last_pose.orientation
self.tf_br.sendTransform(
(pos.x, pos.y, pos.z), (ori.x, ori.y, ori.z, ori.w),
rospy.Time.now(), self.to_frame, self.from_frame)
ps = PoseStamped()
ps.pose = self.last_pose
ps.header.frame_id = self.from_frame
ps.header.stamp = rospy.Time.now()
self.pub.publish(ps)
r.sleep()
class GripperMarkers:
_offset = 0.09
def __init__(self, side_prefix):
self.side_prefix = side_prefix
self._im_server = InteractiveMarkerServer('ik_request_markers_' + self.side_prefix)
self._tf_listener = TransformListener()
self._menu_handler = MenuHandler()
self._menu_handler.insert('Move arm here', callback=self.move_to_pose_cb)
self.r_joint_names = ['r_shoulder_pan_joint',
'r_shoulder_lift_joint',
'r_upper_arm_roll_joint',
'r_elbow_flex_joint',
'r_forearm_roll_joint',
'r_wrist_flex_joint',
'r_wrist_roll_joint']
self.l_joint_names = ['l_shoulder_pan_joint',
'l_shoulder_lift_joint',
'l_upper_arm_roll_joint',
'l_elbow_flex_joint',
'l_forearm_roll_joint',
'l_wrist_flex_joint',
'l_wrist_roll_joint']
# Create a trajectory action client
r_traj_contoller_name = None
l_traj_contoller_name = None
if self.side_prefix == 'r':
r_traj_controller_name = '/r_arm_controller/joint_trajectory_action'
self.r_traj_action_client = SimpleActionClient(r_traj_controller_name,
JointTrajectoryAction)
rospy.loginfo('Waiting for a response from the trajectory '
+ 'action server for RIGHT arm...')
self.r_traj_action_client.wait_for_server()
else:
l_traj_controller_name = '/l_arm_controller/joint_trajectory_action'
self.l_traj_action_client = SimpleActionClient(l_traj_controller_name,
JointTrajectoryAction)
rospy.loginfo('Waiting for a response from the trajectory '
+ 'action server for LEFT arm...')
self.l_traj_action_client.wait_for_server()
self.is_control_visible = False
self.ee_pose = self.get_ee_pose()
self.ik = IK(side_prefix)
self.update_viz()
self._menu_handler.apply(self._im_server, 'ik_target_marker_' + self.side_prefix)
self._im_server.applyChanges()
print self.ik
def get_ee_pose(self):
from_frame = 'base_link'
to_frame = self.side_prefix + '_wrist_roll_link'
try:
if self._tf_listener.frameExists(from_frame) and self._tf_listener.frameExists(to_frame):
t = self._tf_listener.getLatestCommonTime(from_frame, to_frame)
# t = rospy.Time.now()
(pos, rot) = self._tf_listener.lookupTransform(to_frame, from_frame, t) # Note argument order :(
else:
rospy.logerr('TF frames do not exist; could not get end effector pose.')
except Exception as err:
rospy.logerr('Could not get end effector pose through TF.')
rospy.logerr(err)
pos = [1.0, 0.0, 1.0]
rot = [0.0, 0.0, 0.0, 1.0]
return Pose(Point(pos[0], pos[1], pos[2]),
Quaternion(rot[0], rot[1], rot[2], rot[3]))
def move_to_pose_cb(self, feedback):
rospy.loginfo('You pressed the `Move arm here` button from the menu.')
'''Moves the arm to the desired joints'''
print feedback
time_to_joint = 2.0
positions = self.ik.get_ik_for_ee(feedback.pose)
velocities = [0] * len(positions)
traj_goal = JointTrajectoryGoal()
traj_goal.trajectory.header.stamp = (rospy.Time.now() + rospy.Duration(0.1))
traj_goal.trajectory.points.append(JointTrajectoryPoint(positions=positions,
velocities=velocities, time_from_start=rospy.Duration(time_to_joint)))
if (self.side_prefix == 'r'):
traj_goal.trajectory.joint_names = self.r_joint_names
self.r_traj_action_client.send_goal(traj_goal)
else:
traj_goal.trajectory.joint_names = self.l_joint_names
self.l_traj_action_client.send_goal(traj_goal)
pass
def marker_clicked_cb(self, feedback):
if feedback.event_type == InteractiveMarkerFeedback.POSE_UPDATE:
self.ee_pose = feedback.pose
self.update_viz()
self._menu_handler.reApply(self._im_server)
self._im_server.applyChanges()
elif feedback.event_type == InteractiveMarkerFeedback.BUTTON_CLICK:
rospy.loginfo('Changing visibility of the pose controls.')
if (self.is_control_visible):
self.is_control_visible = False
else:
self.is_control_visible = True
else:
rospy.loginfo('Unhandled event: ' + str(feedback.event_type))
def update_viz(self):
menu_control = InteractiveMarkerControl()
menu_control.interaction_mode = InteractiveMarkerControl.BUTTON
menu_control.always_visible = True
frame_id = 'base_link'
pose = self.ee_pose
menu_control = self._add_gripper_marker(menu_control)
text_pos = Point()
text_pos.x = pose.position.x
text_pos.y = pose.position.y
text_pos.z = pose.position.z + 0.1
text = 'x=' + str(pose.position.x) + ' y=' + str(pose.position.y) + ' x=' + str(pose.position.z)
menu_control.markers.append(Marker(type=Marker.TEXT_VIEW_FACING,
id=0, scale=Vector3(0, 0, 0.03),
text=text,
color=ColorRGBA(0.0, 0.0, 0.0, 0.5),
header=Header(frame_id=frame_id),
pose=Pose(text_pos, Quaternion(0, 0, 0, 1))))
int_marker = InteractiveMarker()
int_marker.name = 'ik_target_marker_' + self.side_prefix
int_marker.header.frame_id = frame_id
int_marker.pose = pose
int_marker.scale = 0.2
self._add_6dof_marker(int_marker)
int_marker.controls.append(menu_control)
self._im_server.insert(int_marker, self.marker_clicked_cb)
def _add_gripper_marker(self, control):
is_hand_open=False
if is_hand_open:
angle = 28 * numpy.pi / 180.0
else:
angle = 0
transform1 = tf.transformations.euler_matrix(0, 0, angle)
transform1[:3, 3] = [0.07691 - GripperMarkers._offset, 0.01, 0]
transform2 = tf.transformations.euler_matrix(0, 0, -angle)
transform2[:3, 3] = [0.09137, 0.00495, 0]
t_proximal = transform1
t_distal = tf.transformations.concatenate_matrices(transform1, transform2)
mesh1 = self._make_mesh_marker()
mesh1.mesh_resource = ('package://pr2_description/meshes/gripper_v0/gripper_palm.dae')
mesh1.pose.position.x = -GripperMarkers._offset
mesh1.pose.orientation.w = 1
mesh2 = self._make_mesh_marker()
mesh2.mesh_resource = ('package://pr2_description/meshes/gripper_v0/l_finger.dae')
mesh2.pose = GripperMarkers.get_pose_from_transform(t_proximal)
mesh3 = self._make_mesh_marker()
mesh3.mesh_resource = ('package://pr2_description/meshes/gripper_v0/l_finger_tip.dae')
mesh3.pose = GripperMarkers.get_pose_from_transform(t_distal)
quat = tf.transformations.quaternion_multiply(
tf.transformations.quaternion_from_euler(numpy.pi, 0, 0),
tf.transformations.quaternion_from_euler(0, 0, angle))
transform1 = tf.transformations.quaternion_matrix(quat)
transform1[:3, 3] = [0.07691 - GripperMarkers._offset, -0.01, 0]
transform2 = tf.transformations.euler_matrix(0, 0, -angle)
transform2[:3, 3] = [0.09137, 0.00495, 0]
t_proximal = transform1
t_distal = tf.transformations.concatenate_matrices(transform1,transform2)
mesh4 = self._make_mesh_marker()
mesh4.mesh_resource = ('package://pr2_description/meshes/gripper_v0/l_finger.dae')
mesh4.pose = GripperMarkers.get_pose_from_transform(t_proximal)
mesh5 = self._make_mesh_marker()
mesh5.mesh_resource = ('package://pr2_description/meshes/gripper_v0/l_finger_tip.dae')
mesh5.pose = GripperMarkers.get_pose_from_transform(t_distal)
control.markers.append(mesh1)
control.markers.append(mesh2)
control.markers.append(mesh3)
control.markers.append(mesh4)
control.markers.append(mesh5)
return control
@staticmethod
def get_pose_from_transform(transform):
pos = transform[:3,3].copy()
rot = tf.transformations.quaternion_from_matrix(transform)
return Pose(Point(pos[0], pos[1], pos[2]),
Quaternion(rot[0], rot[1], rot[2], rot[3]))
def _make_mesh_marker(self):
mesh = Marker()
mesh.mesh_use_embedded_materials = False
mesh.type = Marker.MESH_RESOURCE
mesh.scale.x = 1.0
mesh.scale.y = 1.0
mesh.scale.z = 1.0
print self
ik_solution = self.ik.get_ik_for_ee(self.ee_pose)
if (ik_solution is None):
mesh.color = ColorRGBA(1.0, 0.0, 0.0, 0.6)
else:
mesh.color = ColorRGBA(0.0, 1.0, 0.0, 0.6)
return mesh
def _add_6dof_marker(self, int_marker):
is_fixed = True
control = self._make_6dof_control('rotate_x', Quaternion(1, 0, 0, 1), False, is_fixed)
int_marker.controls.append(control)
control = self._make_6dof_control('move_x', Quaternion(1, 0, 0, 1), True, is_fixed)
int_marker.controls.append(control)
control = self._make_6dof_control('rotate_z', Quaternion(0, 1, 0, 1), False, is_fixed)
int_marker.controls.append(control)
control = self._make_6dof_control('move_z', Quaternion(0, 1, 0, 1), True, is_fixed)
int_marker.controls.append(control)
control = self._make_6dof_control('rotate_y', Quaternion(0, 0, 1, 1), False, is_fixed)
int_marker.controls.append(control)
control = self._make_6dof_control('move_y', Quaternion(0, 0, 1, 1), True, is_fixed)
int_marker.controls.append(control)
def _make_6dof_control(self, name, orientation, is_move, is_fixed):
control = InteractiveMarkerControl()
control.name = name
control.orientation = orientation
control.always_visible = False
if (self.is_control_visible):
if is_move:
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
else:
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
else:
control.interaction_mode = InteractiveMarkerControl.NONE
if is_fixed:
control.orientation_mode = InteractiveMarkerControl.FIXED
return control
class GripperMarkers:
_offset = 0.09
def __init__(self, side_prefix):
self._ik_service = IK(side_prefix)
r_traj_controller_name = '/r_arm_controller/joint_trajectory_action'
self.r_traj_action_client = SimpleActionClient(r_traj_controller_name, JointTrajectoryAction)
rospy.loginfo('Waiting for a response from the trajectory action server for RIGHT arm...')
self.r_traj_action_client.wait_for_server()
l_traj_controller_name = '/l_arm_controller/joint_trajectory_action'
self.l_traj_action_client = SimpleActionClient(l_traj_controller_name, JointTrajectoryAction)
rospy.loginfo('Waiting for a response from the trajectory action server for LEFT arm...')
self.l_traj_action_client.wait_for_server()
self.r_joint_names = ['r_shoulder_pan_joint',
'r_shoulder_lift_joint',
'r_upper_arm_roll_joint',
'r_elbow_flex_joint',
'r_forearm_roll_joint',
'r_wrist_flex_joint',
'r_wrist_roll_joint']
self.l_joint_names = ['l_shoulder_pan_joint',
'l_shoulder_lift_joint',
'l_upper_arm_roll_joint',
'l_elbow_flex_joint',
'l_forearm_roll_joint',
'l_wrist_flex_joint',
'l_wrist_roll_joint']
rospy.Subscriber('ar_pose_marker', AlvarMarkers, self.marker_cb)
self.side_prefix = side_prefix
self._im_server = InteractiveMarkerServer("ik_request_markers_{}".format(side_prefix))
self._tf_listener = TransformListener()
self._menu_handler = MenuHandler()
self._menu_handler.insert("Move {} arm here".format(side_prefix), callback=self.move_to_pose_cb)
self.is_control_visible = False
self.ee_pose = self.get_ee_pose()
self.update_viz()
self._menu_handler.apply(self._im_server, 'ik_target_marker')
self._im_server.applyChanges()
def get_ee_pose(self):
from_frame = '/base_link'
to_frame = '/' + self.side_prefix + '_wrist_roll_link'
try:
t = self._tf_listener.getLatestCommonTime(from_frame, to_frame)
(pos, rot) = self._tf_listener.lookupTransform(from_frame, to_frame, t)
except Exception as e:
rospy.logerr('Could not get end effector pose through TF.')
pos = [1.0, 0.0, 1.0]
rot = [0.0, 0.0, 0.0, 1.0]
import traceback
traceback.print_exc()
return Pose(Point(pos[0], pos[1], pos[2]),
Quaternion(rot[0], rot[1], rot[2], rot[3]))
def move_to_pose_cb(self, dummy):
target_pose = GripperMarkers._offset_pose(self.ee_pose)
ik_sol = self._ik_service.get_ik_for_ee(target_pose)
self.move_to_joints(self.side_prefix, [ik_sol], 1.0)
def marker_clicked_cb(self, feedback):
if feedback.event_type == InteractiveMarkerFeedback.POSE_UPDATE:
self.ee_pose = feedback.pose
self.update_viz()
self._menu_handler.reApply(self._im_server)
self._im_server.applyChanges()
elif feedback.event_type == InteractiveMarkerFeedback.BUTTON_CLICK:
rospy.loginfo('Changing visibility of the pose controls.')
if (self.is_control_visible):
self.is_control_visible = False
else:
self.is_control_visible = True
else:
rospy.loginfo('Unhandled event: ' + str(feedback.event_type))
def marker_cb(self, pose_markers):
rospy.loginfo('AR Marker Pose updating')
transform = GripperMarkers.get_matrix_from_pose(pose_markers.markers[0].pose.pose)
offset_array = [0, 0, .03]
offset_transform = tf.transformations.translation_matrix(offset_array)
hand_transform = tf.transformations.concatenate_matrices(transform,
offset_transform)
self.ee_pose = GripperMarkers.get_pose_from_transform(hand_transform)
self.update_viz()
def update_viz(self):
menu_control = InteractiveMarkerControl()
menu_control.interaction_mode = InteractiveMarkerControl.BUTTON
menu_control.always_visible = True
frame_id = 'base_link'
pose = self.ee_pose
menu_control = self._add_gripper_marker(menu_control)
text_pos = Point()
text_pos.x = pose.position.x
text_pos.y = pose.position.y
text_pos.z = pose.position.z + 0.1
text = "x=%.3f y=%.3f z=%.3f" % (pose.position.x, pose.position.y, pose.position.z)
menu_control.markers.append(Marker(type=Marker.TEXT_VIEW_FACING,
id=0, scale=Vector3(0, 0, 0.03),
text=text,
color=ColorRGBA(0.0, 0.0, 0.0, 0.5),
header=Header(frame_id=frame_id),
pose=Pose(text_pos, Quaternion(0, 0, 0, 1))))
label_pos = Point()
label_pos.x = pose.position.x
label_pos.y = pose.position.y
label_pos.z = pose.position.z
label = "{} arm".format(self.side_prefix)
menu_control.markers.append(Marker(type=Marker.TEXT_VIEW_FACING,
id=0, scale=Vector3(0, 0, 0.03),
text=label,
color=ColorRGBA(0.0, 0.0, 0.0, 0.9),
header=Header(frame_id=frame_id),
pose=Pose(label_pos, Quaternion(0, 0, 0, 1))))
int_marker = InteractiveMarker()
int_marker.name = 'ik_target_marker'
int_marker.header.frame_id = frame_id
int_marker.pose = pose
int_marker.scale = 0.2
self._add_6dof_marker(int_marker)
int_marker.controls.append(menu_control)
self._im_server.insert(int_marker, self.marker_clicked_cb)
def _add_gripper_marker(self, control):
is_hand_open=False
if is_hand_open:
angle = 28 * numpy.pi / 180.0
else:
angle = 0
transform1 = tf.transformations.euler_matrix(0, 0, angle)
transform1[:3, 3] = [0.07691 - GripperMarkers._offset, 0.01, 0]
transform2 = tf.transformations.euler_matrix(0, 0, -angle)
transform2[:3, 3] = [0.09137, 0.00495, 0]
t_proximal = transform1
t_distal = tf.transformations.concatenate_matrices(transform1, transform2)
mesh1 = self._make_mesh_marker()
mesh1.mesh_resource = ('package://pr2_description/meshes/gripper_v0/gripper_palm.dae')
mesh1.pose.position.x = -GripperMarkers._offset
mesh1.pose.orientation.w = 1
mesh2 = self._make_mesh_marker()
mesh2.mesh_resource = ('package://pr2_description/meshes/gripper_v0/l_finger.dae')
mesh2.pose = GripperMarkers.get_pose_from_transform(t_proximal)
mesh3 = self._make_mesh_marker()
mesh3.mesh_resource = ('package://pr2_description/meshes/gripper_v0/l_finger_tip.dae')
mesh3.pose = GripperMarkers.get_pose_from_transform(t_distal)
quat = tf.transformations.quaternion_multiply(
tf.transformations.quaternion_from_euler(numpy.pi, 0, 0),
tf.transformations.quaternion_from_euler(0, 0, angle))
transform1 = tf.transformations.quaternion_matrix(quat)
transform1[:3, 3] = [0.07691 - GripperMarkers._offset, -0.01, 0]
transform2 = tf.transformations.euler_matrix(0, 0, -angle)
transform2[:3, 3] = [0.09137, 0.00495, 0]
t_proximal = transform1
t_distal = tf.transformations.concatenate_matrices(transform1,transform2)
mesh4 = self._make_mesh_marker()
mesh4.mesh_resource = ('package://pr2_description/meshes/gripper_v0/l_finger.dae')
mesh4.pose = GripperMarkers.get_pose_from_transform(t_proximal)
mesh5 = self._make_mesh_marker()
mesh5.mesh_resource = ('package://pr2_description/meshes/gripper_v0/l_finger_tip.dae')
mesh5.pose = GripperMarkers.get_pose_from_transform(t_distal)
control.markers.append(mesh1)
control.markers.append(mesh2)
control.markers.append(mesh3)
control.markers.append(mesh4)
control.markers.append(mesh5)
return control
@staticmethod
def _offset_pose(pose):
transform = GripperMarkers.get_matrix_from_pose(pose)
offset_array = [-GripperMarkers._offset, 0, 0]
offset_transform = tf.transformations.translation_matrix(offset_array)
hand_transform = tf.transformations.concatenate_matrices(transform,
offset_transform)
return GripperMarkers.get_pose_from_transform(hand_transform)
@staticmethod
def get_matrix_from_pose(pose):
rotation = [pose.orientation.x, pose.orientation.y,
pose.orientation.z, pose.orientation.w]
transformation = tf.transformations.quaternion_matrix(rotation)
position = [pose.position.x, pose.position.y, pose.position.z]
transformation[:3, 3] = position
return transformation
@staticmethod
def get_pose_from_transform(transform):
pos = transform[:3,3].copy()
rot = tf.transformations.quaternion_from_matrix(transform)
return Pose(Point(pos[0], pos[1], pos[2]),
Quaternion(rot[0], rot[1], rot[2], rot[3]))
def _make_mesh_marker(self):
mesh = Marker()
mesh.mesh_use_embedded_materials = False
mesh.type = Marker.MESH_RESOURCE
mesh.scale.x = 1.0
mesh.scale.y = 1.0
mesh.scale.z = 1.0
target_pose = GripperMarkers._offset_pose(self.ee_pose)
ik_sol = self._ik_service.get_ik_for_ee(target_pose)
if ik_sol == None:
mesh.color = ColorRGBA(1.0, 0.0, 0.0, 0.6)
else:
mesh.color = ColorRGBA(0.0, 1.0, 0.0, 0.6)
return mesh
def _add_6dof_marker(self, int_marker):
is_fixed = True
control = self._make_6dof_control('rotate_x', Quaternion(1, 0, 0, 1), False, is_fixed)
int_marker.controls.append(control)
control = self._make_6dof_control('move_x', Quaternion(1, 0, 0, 1), True, is_fixed)
int_marker.controls.append(control)
control = self._make_6dof_control('rotate_z', Quaternion(0, 1, 0, 1), False, is_fixed)
int_marker.controls.append(control)
control = self._make_6dof_control('move_z', Quaternion(0, 1, 0, 1), True, is_fixed)
int_marker.controls.append(control)
control = self._make_6dof_control('rotate_y', Quaternion(0, 0, 1, 1), False, is_fixed)
int_marker.controls.append(control)
control = self._make_6dof_control('move_y', Quaternion(0, 0, 1, 1), True, is_fixed)
int_marker.controls.append(control)
def _make_6dof_control(self, name, orientation, is_move, is_fixed):
control = InteractiveMarkerControl()
control.name = name
control.orientation = orientation
control.always_visible = False
if (self.is_control_visible):
if is_move:
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
else:
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
else:
control.interaction_mode = InteractiveMarkerControl.NONE
if is_fixed:
control.orientation_mode = InteractiveMarkerControl.FIXED
return control
def move_to_joints(self, side_prefix, positions, time_to_joint):
'''Moves the arm to the desired joints'''
traj_goal = JointTrajectoryGoal()
traj_goal.trajectory.header.stamp = (rospy.Time.now() + rospy.Duration(0.1))
time_move = time_to_joint
print "using following positions %s" % positions
for pose in positions:
velocities = [0] * len(pose)
traj_goal.trajectory.points.append(JointTrajectoryPoint(positions=pose,
velocities=velocities, time_from_start=rospy.Duration(time_move)))
time_move += time_to_joint
if (side_prefix == 'r'):
traj_goal.trajectory.joint_names = self.r_joint_names
self.r_traj_action_client.send_goal(traj_goal)
else:
traj_goal.trajectory.joint_names = self.l_joint_names
self.l_traj_action_client.send_goal(traj_goal)
def add_waypoint_marker(self, waypoint, description=None):
self.markers_created_cnt += 1
self.marker_cnt += 1
int_marker = InteractiveMarker()
int_marker.header.frame_id = "base_link"
int_marker.pose.position = waypoint.end_effector_pose.position
int_marker.scale = 0.1
int_marker.name = str(self.markers_created_cnt)
if description is None:
int_marker.description = int_marker.name
else:
int_marker.description = description
self.name_to_marker_dict[int_marker.name] = int_marker
self.waypoint_to_marker_dict[waypoint] = int_marker
self.name_to_waypoint_dict[int_marker.name] = waypoint
# insert a box # TODO: may change geometry / eff mesh
make_box_control_marker(int_marker)
int_marker.controls[0].interaction_mode = InteractiveMarkerControl.MOVE_ROTATE_3D
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 1
control.orientation.y = 0
control.orientation.z = 0
control.name = "rotate_x"
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
int_marker.controls.append(control)
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 1
control.orientation.y = 0
control.orientation.z = 0
control.name = "move_x"
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
int_marker.controls.append(control)
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 0
control.orientation.y = 1
control.orientation.z = 0
control.name = "rotate_z"
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
int_marker.controls.append(control)
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 0
control.orientation.y = 1
control.orientation.z = 0
control.name = "move_z"
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
int_marker.controls.append(control)
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 0
control.orientation.y = 0
control.orientation.z = 1
control.name = "rotate_y"
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
int_marker.controls.append(control)
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 0
control.orientation.y = 0
control.orientation.z = 1
control.name = "move_y"
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
int_marker.controls.append(control)
self.server.insert(int_marker, self._process_feedback)
# add menus
menu_handler = MenuHandler()
del_sub_menu_handle = menu_handler.insert("Delete Waypoint")
self._menu_cmds['del_wp'] = menu_handler.insert("Yes", parent=del_sub_menu_handle,
callback=self._process_feedback)
menu_handler.insert("No", parent=del_sub_menu_handle, callback=self._process_feedback)
ins_sub_menu_handle = menu_handler.insert("Insert Waypoint")
self._menu_cmds['ins_wp_before'] = menu_handler.insert("Before", parent=ins_sub_menu_handle,
callback=self._process_feedback)
self._menu_cmds['ins_wp_after'] = menu_handler.insert("After", parent=ins_sub_menu_handle,
callback=self._process_feedback)
menu_handler.insert("Cancel", parent=ins_sub_menu_handle, callback=self._process_feedback)
# Set time
time_sub_menu_handle = menu_handler.insert("Set Waypoint Time")
menu_time_cmds = {}
time = self.name_to_waypoint_dict[int_marker.name].time
time_list = [float(num) for num in range((int(m.ceil(time))))]
time_list.append(time)
time_list.extend([m.floor(time) + cnt + 1 for cnt in range(6)])
self._menu_cmds[int_marker.name] = {"menu_time_cmds": menu_time_cmds}
for t in time_list:
time_opt_handle = menu_handler.insert(str(t), parent=time_sub_menu_handle, callback=self._process_feedback)
menu_time_cmds[time_opt_handle] = t
if t == time:
menu_handler.setCheckState(time_opt_handle, MenuHandler.CHECKED)
else:
menu_handler.setCheckState(time_opt_handle, MenuHandler.UNCHECKED)
menu_handler.apply(self.server, int_marker.name)
self._menu_handlers[int_marker.name] = menu_handler
self.server.applyChanges()
if int(int_marker.description)-1 < len(self._int_marker_name_list):
self._int_marker_name_list.insert(int(int_marker.description) - 1, int_marker.name)
else:
self._int_marker_name_list.append(int_marker.name)
model_name.split('_')[:-1]) in target_models:
if server.get(model_name) is None:
make_interactive_marker(msg.pose[index], model_name)
rospy.loginfo("Interactive marker added at %s for model %s",
pose3d2str(msg.pose[index]), model_name)
else:
server.setPose(model_name, msg.pose[index])
server.applyChanges()
if __name__ == "__main__":
rospy.init_node("model_markers")
server = InteractiveMarkerServer("model_markers")
menu_handler.insert("First Entry", callback=process_feedback)
menu_handler.insert("Second Entry", callback=process_feedback)
sub_menu_handle = menu_handler.insert("Submenu")
menu_handler.insert("First Entry",
parent=sub_menu_handle,
callback=process_feedback)
menu_handler.insert("Second Entry",
parent=sub_menu_handle,
callback=process_feedback)
target_models = rospy.get_param('~target_models',
'cat_orange cat_black').split()
state_pub = rospy.Publisher("gazebo/set_model_state",
ModelState,
queue_size=1)
class InteractiveMarkerManager(object):
def __init__(self, parent_instance_handle, server_name, ik_solver):
self.parent_instance_handle = parent_instance_handle
self.server = InteractiveMarkerServer(server_name)
self.ik_solver = ik_solver
self.ik_position_xyz_bounds = [0.01, 0.01, 0.01]
self.ik_position_rpy_bounds = [31416.0, 31416.0, 31416.0] # NOTE: This implements position-only IK
self._menu_handlers = {}
self._menu_cmds = {}
self.menu_handler = MenuHandler()
del_sub_menu_handle = self.menu_handler.insert("Delete Waypoint")
self._menu_cmds['del_wp'] = self.menu_handler.insert("Yes", parent=del_sub_menu_handle, callback=self._process_feedback)
self.menu_handler.insert("No", parent=del_sub_menu_handle, callback=self._process_feedback)
ins_sub_menu_handle = self.menu_handler.insert("Insert Waypoint")
self._menu_cmds['ins_wp_before'] = self.menu_handler.insert("Before", parent=ins_sub_menu_handle, callback=self._process_feedback)
self._menu_cmds['ins_wp_after'] = self.menu_handler.insert("After", parent=ins_sub_menu_handle, callback=self._process_feedback)
self.menu_handler.insert("Cancel", parent=ins_sub_menu_handle, callback=self._process_feedback)
self._int_marker_name_list = []
self.markers_created_cnt = 0
self.marker_cnt = 0
self.name_to_marker_dict = {}
self.waypoint_to_marker_dict = {}
self.name_to_waypoint_dict = {}
def _process_feedback(self, feedback):
s = "Feedback from marker '" + feedback.marker_name
s += "' / control '" + feedback.control_name + "'"
mp = ""
if feedback.mouse_point_valid:
mp = " at " + str(feedback.mouse_point.x)
mp += ", " + str(feedback.mouse_point.y)
mp += ", " + str(feedback.mouse_point.z)
mp += " in frame " + feedback.header.frame_id
if feedback.event_type == InteractiveMarkerFeedback.BUTTON_CLICK:
rospy.logdebug(s + ": button click" + mp + ".")
pass
elif feedback.event_type == InteractiveMarkerFeedback.MENU_SELECT:
rospy.logdebug(s + ": menu item " + str(feedback.menu_entry_id) + " clicked" + mp + ".")
self._process_menu_select(feedback)
elif feedback.event_type == InteractiveMarkerFeedback.POSE_UPDATE: # TODO: I really want to use the other type of feedback
rospy.logdebug(s + ": pose changed")
self._update_waypoint_position(feedback.marker_name, feedback.pose)
elif feedback.event_type == InteractiveMarkerFeedback.MOUSE_DOWN:
rospy.logdebug(s + ": mouse down" + mp + ".")
pass
elif feedback.event_type == InteractiveMarkerFeedback.MOUSE_UP:
rospy.logdebug(s + ": mouse up" + mp + ".")
pass
self.server.applyChanges()
def _process_menu_select(self, feedback):
menu_entry_id = feedback.menu_entry_id
if menu_entry_id == self._menu_cmds['del_wp']:
self._delete_waypoint(feedback)
elif menu_entry_id == self._menu_cmds['ins_wp_before']:
self._insert_waypoint(feedback, before=True)
elif menu_entry_id == self._menu_cmds['ins_wp_after']:
self._insert_waypoint(feedback, before=False)
elif menu_entry_id in self._menu_cmds[feedback.marker_name]['menu_time_cmds']:
self._process_menu_time_cmds(feedback)
else:
pass
def _delete_waypoint(self, feedback):
marker = self.name_to_marker_dict[feedback.marker_name]
waypoint = self.name_to_waypoint_dict[marker.name]
self.parent_instance_handle.delete_waypoint(waypoint)
# Update dictionaries
del self.name_to_marker_dict[marker.name]
del self.waypoint_to_marker_dict[waypoint]
del self.name_to_waypoint_dict[marker.name]
del self._menu_handlers[marker.name]
del self._menu_cmds[marker.name]
# Erase marker from server
self.server.erase(feedback.marker_name)
self.server.applyChanges()
self.marker_cnt -=1
self._int_marker_name_list.remove(marker.name)
self._update_wp_marker_descriptions()
self.server.applyChanges()
def _insert_waypoint(self, feedback, before=True):
ref_marker = self.name_to_marker_dict[feedback.marker_name]
ref_waypoint = self.name_to_waypoint_dict[ref_marker.name]
self.parent_instance_handle.insert_waypoint(ref_waypoint, before=before)
self._update_wp_marker_descriptions()
self.server.applyChanges()
def _process_menu_time_cmds(self, feedback):
menu_entry_handle = feedback.menu_entry_id
menu_handler = self._menu_handlers[feedback.marker_name]
check_state = menu_handler.getCheckState(menu_entry_handle)
if check_state == MenuHandler.UNCHECKED:
menu_handler.setCheckState(menu_entry_handle, MenuHandler.CHECKED)
# Uncheck all other menu entries
for menu_entry_id in self._menu_cmds[feedback.marker_name]['menu_time_cmds']:
if menu_entry_id != menu_entry_handle:
menu_handler.setCheckState(menu_entry_id, MenuHandler.UNCHECKED)
# Update waypoints
waypoint = self.name_to_waypoint_dict[feedback.marker_name]
waypoint.time = self._menu_cmds[feedback.marker_name]['menu_time_cmds'][menu_entry_handle]
# Apply marker changes
menu_handler.reApply(self.server)
self.server.applyChanges()
def _update_waypoint_pose(self):
# TODO maybe handle pose changes differently?
pass
def _update_waypoint_position(self, marker_name, marker_pose):
waypoint = self.name_to_waypoint_dict[marker_name]
eff_pos = marker_pose.position
eff_orient = marker_pose.orientation
target_jt_angles = self.ik_solver.get_ik(waypoint.joint_state.positions,
eff_pos.x, eff_pos.y, eff_pos.z,
eff_orient.w, eff_orient.x, eff_orient.y, eff_orient.z,
self.ik_position_xyz_bounds[0],
self.ik_position_xyz_bounds[1],
self.ik_position_xyz_bounds[2],
self.ik_position_rpy_bounds[0],
self.ik_position_rpy_bounds[1],
self.ik_position_rpy_bounds[2])
if target_jt_angles is not None:
waypoint.end_effector_pose = copy.deepcopy(marker_pose)
waypoint.joint_state.positions = target_jt_angles
self._change_wp_marker_color(marker_name, "white", 0.9)
else:
self._change_wp_marker_color(marker_name, "red", 0.9)
def _change_wp_marker_color(self, wp_marker_name, color, opacity=1.0):
assert 0.0 <= opacity <= 1.0
# set color
int_marker = self.server.get(wp_marker_name)
marker = int_marker.controls[0].markers[0]
replacement_int_marker = copy.deepcopy(int_marker)
replacement_marker = replacement_int_marker.controls[0].markers[0]
rgb = None
if color == "white":
rgb = (1.0, 1.0, 1.0)
elif color == "red":
rgb = (1.0, 0.0, 0.0)
# TODO: Additional colors here
changed = False
if rgb is not None:
if marker.color.r != rgb[0]:
replacement_marker.color.r = rgb[0]
changed = True
print("change to ", rgb)
if marker.color.g != rgb[1]:
replacement_marker.color.g = rgb[1]
changed = True
if marker.color.b != rgb[2]:
replacement_marker.color.b = rgb[2]
changed = True
if marker.color.a != opacity:
replacement_marker.color.a = opacity
changed = True
# update wp marker
if changed:
self._replace_wp_marker(int_marker, replacement_int_marker)
def _update_wp_marker_descriptions(self):
for index, marker_name in enumerate(self._int_marker_name_list):
marker = self.name_to_marker_dict[marker_name]
marker_description = int(marker.description)
if marker_description != index+1:
marker.description = str(index+1)
replacement_marker = copy.deepcopy(self.server.get(marker.name))
replacement_marker.description = marker.description
self._replace_wp_marker(marker, replacement_marker)
def _replace_wp_marker(self, old_int_marker, replacement_int_marker):
# Update dictionaries
self.name_to_marker_dict[old_int_marker.name] = replacement_int_marker
self.waypoint_to_marker_dict[self.name_to_waypoint_dict[old_int_marker.name]] = replacement_int_marker
# Erase marker from server
self.server.erase(old_int_marker.name)
self.server.insert(replacement_int_marker, self._process_feedback)
self.server.applyChanges()
def add_waypoint_marker(self, waypoint, description=None):
self.markers_created_cnt += 1
self.marker_cnt += 1
int_marker = InteractiveMarker()
int_marker.header.frame_id = "base_link"
int_marker.pose.position = waypoint.end_effector_pose.position
int_marker.scale = 0.1
int_marker.name = str(self.markers_created_cnt)
if description is None:
int_marker.description = int_marker.name
else:
int_marker.description = description
self.name_to_marker_dict[int_marker.name] = int_marker
self.waypoint_to_marker_dict[waypoint] = int_marker
self.name_to_waypoint_dict[int_marker.name] = waypoint
# insert a box # TODO: may change geometry / eff mesh
make_box_control_marker(int_marker)
int_marker.controls[0].interaction_mode = InteractiveMarkerControl.MOVE_ROTATE_3D
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 1
control.orientation.y = 0
control.orientation.z = 0
control.name = "rotate_x"
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
int_marker.controls.append(control)
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 1
control.orientation.y = 0
control.orientation.z = 0
control.name = "move_x"
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
int_marker.controls.append(control)
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 0
control.orientation.y = 1
control.orientation.z = 0
control.name = "rotate_z"
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
int_marker.controls.append(control)
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 0
control.orientation.y = 1
control.orientation.z = 0
control.name = "move_z"
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
int_marker.controls.append(control)
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 0
control.orientation.y = 0
control.orientation.z = 1
control.name = "rotate_y"
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
int_marker.controls.append(control)
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 0
control.orientation.y = 0
control.orientation.z = 1
control.name = "move_y"
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
int_marker.controls.append(control)
self.server.insert(int_marker, self._process_feedback)
# add menus
menu_handler = MenuHandler()
del_sub_menu_handle = menu_handler.insert("Delete Waypoint")
self._menu_cmds['del_wp'] = menu_handler.insert("Yes", parent=del_sub_menu_handle,
callback=self._process_feedback)
menu_handler.insert("No", parent=del_sub_menu_handle, callback=self._process_feedback)
ins_sub_menu_handle = menu_handler.insert("Insert Waypoint")
self._menu_cmds['ins_wp_before'] = menu_handler.insert("Before", parent=ins_sub_menu_handle,
callback=self._process_feedback)
self._menu_cmds['ins_wp_after'] = menu_handler.insert("After", parent=ins_sub_menu_handle,
callback=self._process_feedback)
menu_handler.insert("Cancel", parent=ins_sub_menu_handle, callback=self._process_feedback)
# Set time
time_sub_menu_handle = menu_handler.insert("Set Waypoint Time")
menu_time_cmds = {}
time = self.name_to_waypoint_dict[int_marker.name].time
time_list = [float(num) for num in range((int(m.ceil(time))))]
time_list.append(time)
time_list.extend([m.floor(time) + cnt + 1 for cnt in range(6)])
self._menu_cmds[int_marker.name] = {"menu_time_cmds": menu_time_cmds}
for t in time_list:
time_opt_handle = menu_handler.insert(str(t), parent=time_sub_menu_handle, callback=self._process_feedback)
menu_time_cmds[time_opt_handle] = t
if t == time:
menu_handler.setCheckState(time_opt_handle, MenuHandler.CHECKED)
else:
menu_handler.setCheckState(time_opt_handle, MenuHandler.UNCHECKED)
menu_handler.apply(self.server, int_marker.name)
self._menu_handlers[int_marker.name] = menu_handler
self.server.applyChanges()
if int(int_marker.description)-1 < len(self._int_marker_name_list):
self._int_marker_name_list.insert(int(int_marker.description) - 1, int_marker.name)
else:
self._int_marker_name_list.append(int_marker.name)
def clear_waypoint_markers(self):
self.server.clear()
self.server.applyChanges()
self._menu_handlers = {}
self._menu_cmds = {}
self._int_marker_name_list = []
self.markers_created_cnt = 0
self.marker_cnt = 0
self.name_to_marker_dict = {}
self.waypoint_to_marker_dict = {}
self.name_to_waypoint_dict = {}
class ArmControlMarker:
'''Marker for controlling arm of robot.'''
_im_server = None
def __init__(self, arm):
'''
Args:
arm (Arm): Arm object for the robot's arm.
'''
if ArmControlMarker._im_server is None:
im_server = InteractiveMarkerServer('interactive_arm_control')
ArmControlMarker._im_server = im_server
self._arm = arm
self._name = 'arm'
self._is_control_visible = False
self._menu_handler = None
self._prev_is_reachable = None
self._pose = self._arm.get_ee_state()
self._lock = threading.Lock()
self._current_pose = None
self._menu_control = None
self.update()
# ##################################################################
# Instance methods: Public (API)
# ##################################################################
def update(self):
'''Updates marker/arm loop'''
self._menu_handler = MenuHandler()
# Inset main menu entries.
self._menu_handler.insert('Move gripper here',
callback=self._move_to_cb)
self._menu_handler.insert('Move marker to current gripper pose',
callback=self._move_pose_to_cb)
if self._is_hand_open():
self._menu_handler.insert('Close gripper',
callback=self._close_gripper_cb)
else:
self._menu_handler.insert('Open gripper',
callback=self._open_gripper_cb)
frame_id = REF_FRAME
pose = self.get_pose()
# if self._marker_moved() or self._menu_control is None:
rospy.loginfo("Marker moved")
menu_control = InteractiveMarkerControl()
menu_control.interaction_mode = InteractiveMarkerControl.BUTTON
menu_control.always_visible = True
menu_control = self._make_gripper_marker(menu_control,
self._is_hand_open())
self._menu_control = menu_control
# Make and add interactive marker.
int_marker = InteractiveMarker()
int_marker.name = self._get_name()
int_marker.header.frame_id = frame_id
int_marker.pose = pose.pose
int_marker.scale = INT_MARKER_SCALE
self._add_6dof_marker(int_marker, True)
int_marker.controls.append(self._menu_control)
ArmControlMarker._im_server.insert(int_marker,
self._marker_feedback_cb)
self._menu_handler.apply(ArmControlMarker._im_server, self._get_name())
ArmControlMarker._im_server.applyChanges()
def reset(self):
'''Sets marker to current arm pose'''
self.set_new_pose(self._arm.get_ee_state(), is_offset=True)
def destroy(self):
'''Removes marker from the world.'''
ArmControlMarker._im_server.erase(self._get_name())
ArmControlMarker._im_server.applyChanges()
def set_new_pose(self, new_pose, is_offset=False):
'''Changes the pose of the marker to new_pose.
Args:
new_pose (PoseStamped)
'''
self._lock.acquire()
if is_offset:
self._pose = new_pose
else:
self._pose = ArmControlMarker._offset_pose(new_pose, -1)
self._lock.release()
def get_pose(self):
'''Returns the pose of the marker
Returns:
Pose
'''
self._lock.acquire()
pose = ArmControlMarker._copy_pose(self._pose)
self._lock.release()
return ArmControlMarker._offset_pose(pose)
# ##################################################################
# Static methods: Internal ("private")
# ##################################################################
@staticmethod
def _get_pose_from_transform(transform):
'''Returns pose for transformation matrix.
Args:
transform (Matrix3x3)
Returns:
PoseStamped
'''
pos = transform[:3, 3].copy()
rot = tf.transformations.quaternion_from_matrix(transform)
return PoseStamped(
Header(frame_id='base_link'),
Pose(Point(pos[0], pos[1], pos[2]),
Quaternion(rot[0], rot[1], rot[2], rot[3])))
@staticmethod
def _get_matrix_from_pose(pose):
'''Returns the transformation matrix for given pose.
Args:
pose (PoseStamped)
Returns:
Matrix3x3
'''
pp, po = pose.pose.position, pose.pose.orientation
rotation = [po.x, po.y, po.z, po.w]
transformation = tf.transformations.quaternion_matrix(rotation)
position = [pp.x, pp.y, pp.z]
transformation[:3, 3] = position
return transformation
@staticmethod
def _offset_pose(pose, constant=1):
'''Offsets the world pose for visualization.
Args:
pose (PoseStamped): The pose to offset.
constant (int, optional): How much to scale the set offset
by (scales DEFAULT_OFFSET). Defaults to 1.
Returns:
PoseStamped: The offset pose.
'''
transform = ArmControlMarker._get_matrix_from_pose(pose)
offset_array = [constant * DEFAULT_OFFSET, 0, 0]
offset_transform = tf.transformations.translation_matrix(offset_array)
hand_transform = tf.transformations.concatenate_matrices(
transform, offset_transform)
return ArmControlMarker._get_pose_from_transform(hand_transform)
@staticmethod
def _copy_pose(pose):
'''Copies pose msg
Args:
pose (PoseStamped)
Returns:
PoseStamped
'''
copy = PoseStamped(
Header(frame_id=pose.header.frame_id),
Pose(
Point(pose.pose.position.x, pose.pose.position.y,
pose.pose.position.z),
Quaternion(pose.pose.orientation.x, pose.pose.orientation.y,
pose.pose.orientation.z, pose.pose.orientation.w)))
return copy
@staticmethod
def _pose2array(p):
'''Converts pose to array
Args:
p (PoseStamped)
Returns:
numpy.array
'''
return array((p.pose.position.x, p.pose.position.y, p.pose.position.z,
p.pose.orientation.x, p.pose.orientation.y,
p.pose.orientation.z, p.pose.orientation.w))
@staticmethod
def _is_the_same(pose1, pose2, tol=0.001):
'''Checks if poses are the same within tolerance
Args:
pose1 (Pose)
pose2 (Pose)
tol (float, optional)
Returns:
bool
'''
if pose1 is None or pose2 is None:
return True
diff = pose1 - pose2
dist = linalg.norm(diff)
return dist < tol
@staticmethod
def _make_mesh_marker(color):
'''Creates and returns a mesh marker.
Args:
color (ColorRGBA)
Returns:
Marker
'''
mesh = Marker()
mesh.mesh_use_embedded_materials = False
mesh.type = Marker.MESH_RESOURCE
mesh.scale.x = 1.0
mesh.scale.y = 1.0
mesh.scale.z = 1.0
mesh.color = color
return mesh
# ##################################################################
# Instance methods: Internal ("private")
# ##################################################################
def _marker_moved(self):
'''Returns whether marker has moved
Returns:
bool
'''
moved = True
current_pose = self.get_pose()
if not self._current_pose is None:
if ArmControlMarker._is_the_same(
ArmControlMarker._pose2array(current_pose),
ArmControlMarker._pose2array(self._current_pose)):
moved = False
self._current_pose = current_pose
return moved
def _marker_feedback_cb(self, feedback):
'''Callback for when an event occurs on the marker.
Args:
feedback (InteractiveMarkerFeedback)
'''
if feedback.event_type == InteractiveMarkerFeedback.BUTTON_CLICK:
# Set the visibility of the 6DOF controller.
# This happens a ton, and doesn't need to be logged like
# normal events (e.g. clicking on most marker controls
# fires here).
rospy.logdebug('Changing visibility of the pose controls.')
self._is_control_visible = not self._is_control_visible
elif feedback.event_type == InteractiveMarkerFeedback.MOUSE_UP:
self.set_new_pose(
PoseStamped(Header(frame_id='base_link'), feedback.pose))
self.update()
else:
# This happens a ton, and doesn't need to be logged like
# normal events (e.g. clicking on most marker controls
# fires here).
rospy.logdebug('Unknown event: ' + str(feedback.event_type))
def _open_gripper_cb(self, feedback):
'''Callback for opening gripper.
Args:
feedback (InteractiveMarkerFeedback): Unused
'''
self._arm.open_gripper()
for i in range(30):
if self._arm.get_gripper_state() == GripperState.OPEN:
break
rospy.sleep(0.1)
self.update()
def _close_gripper_cb(self, feedback):
'''Callback for closing gripper.
Args:
feedback (InteractiveMarkerFeedback): Unused
'''
self._arm.close_gripper()
for i in range(30):
if self._arm.get_gripper_state() != GripperState.OPEN:
break
rospy.sleep(0.1)
self.update()
def _move_to_cb(self, feedback):
'''Callback for when moving to a pose is requested.
Args:
feedback (InteractiveMarkerFeedback): Unused
'''
if not ArmControlMarker._is_the_same(
ArmControlMarker._pose2array(self._pose),
ArmControlMarker._pose2array(self._arm.get_ee_state()), 0.01):
rospy.loginfo("Move to cb from marker for real")
self._lock.acquire()
pose = ArmControlMarker._copy_pose(self._pose)
self._lock.release()
target_pose = pose
if target_pose is not None:
# time_to_pose = self._arm.get_time_to_pose(self.get_pose())
thread = threading.Thread(group=None,
target=self._arm.move_to_pose,
args=(target_pose, ),
name='move_to_arm_state_thread')
thread.start()
# Log
# side_str = self._arm.side()
rospy.loginfo('Started thread to move arm.')
else:
rospy.loginfo('Will not move arm; unreachable.')
else:
rospy.loginfo("move too small?")
def _move_pose_to_cb(self, feedback):
'''Callback for moving gripper marker to current pose.
Args:
feedback (InteractiveMarkerFeedback): Unused
'''
self.reset()
self.update()
def _is_reachable(self):
'''Checks and returns whether there is an IK solution for this
marker pose
Returns:
bool: Whether this action step is reachable.
'''
self._lock.acquire()
pose = ArmControlMarker._copy_pose(self._pose)
self._lock.release()
target_joints = self._arm.get_ik_for_ee(pose,
self._arm.get_joint_state())
is_reachable = (target_joints is not None)
# A bit more complicated logging to avoid spamming the logs
# while still giving useful info. It now logs when reachability
# is first calculated, or changes.
report = False
# See if we've set reachability for the first time.
if self._prev_is_reachable is None:
report = True
reachable_str = ('is reachable'
if is_reachable else 'is not reachable')
# See if we've got a different reachability than we had before.
elif self._prev_is_reachable != is_reachable:
report = True
reachable_str = ('is now reachable'
if is_reachable else 'is no longer reachable')
# Log if it's changed.
if report:
rospy.loginfo(reachable_str)
# Cache and return.
self._prev_is_reachable = is_reachable
print
return is_reachable
def _get_name(self):
'''Generates the unique name for the marker.
Returns:
str: A human-readable unique name for the marker.
'''
return self._name
def _is_hand_open(self):
'''Returns whether the gripper is open
Returns:
bool
'''
return self._arm.get_gripper_state() == GripperState.OPEN
def _add_6dof_marker(self, int_marker, is_fixed):
'''Adds a 6 DoF control marker to the interactive marker.
Args:
int_marker (InteractiveMarker)
is_fixed (bool): Looks like whether position is fixed (?).
Currently only passed as True.
'''
# Each entry in options is (name, orientation, is_move)
options = [
('rotate_x', Quaternion(1, 0, 0, 1), False),
('move_x', Quaternion(1, 0, 0, 1), True),
('rotate_z', Quaternion(0, 1, 0, 1), False),
('move_z', Quaternion(0, 1, 0, 1), True),
('rotate_y', Quaternion(0, 0, 1, 1), False),
('move_y', Quaternion(0, 0, 1, 1), True),
]
for opt in options:
name, orient, is_move = opt
control = self._make_6dof_control(name, orient, is_move, is_fixed)
int_marker.controls.append(control)
def _make_6dof_control(self, name, orientation, is_move, is_fixed):
'''Creates and returns one component of the 6dof controller.
Args:
name (str): Name for the control
orientation (Quaternion): How the control should be
oriented.
is_move (bool): Looks like whether the marker moves the
object (?). Currently passed as True for moving markers,
False for rotating ones.
is_fixed (bool): Looks like whether position is fixed (?).
Currently always passed as True.
Returns:
InteractiveMarkerControl
'''
control = InteractiveMarkerControl()
control.name = name
control.orientation = orientation
control.always_visible = False
if self._is_control_visible:
if is_move:
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
else:
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
else:
control.interaction_mode = InteractiveMarkerControl.NONE
if is_fixed:
control.orientation_mode = InteractiveMarkerControl.FIXED
return control
def _get_mesh_marker_color(self):
'''Gets the color for the mesh marker (thing that looks like a
gripper) for this step.
A simple implementation of this will return one color for
reachable poses, another for unreachable ones. Future
implementations may provide further visual cues.
Returns:
ColorRGBA: The color for the gripper mesh for this step.
'''
if self._is_reachable():
return COLOR_MESH_REACHABLE
else:
return COLOR_MESH_UNREACHABLE
def _make_gripper_marker(self, control, is_hand_open=False):
'''Makes a gripper marker, adds it to control, returns control.
Args:
control (InteractiveMarkerControl): IM Control we're using.
is_hand_open (bool, optional): Whether the gripper is open.
Defaults to False (closed).
Returns:
InteractiveMarkerControl: The passed control.
'''
mesh_color = self._get_mesh_marker_color()
# Create mesh 1 (palm).
mesh1 = ArmControlMarker._make_mesh_marker(mesh_color)
mesh1.mesh_resource = STR_GRIPPER_PALM_FILE
mesh1.pose.position.x = DEFAULT_OFFSET
mesh1.pose.orientation.w = 1
# TODO (sarah): make all of these numbers into constants
if is_hand_open:
mesh2 = ArmControlMarker._make_mesh_marker(mesh_color)
mesh2.mesh_resource = STR_L_GRIPPER_FINGER_FILE
mesh2.pose.position.x = 0.08
mesh2.pose.position.y = -0.165
mesh2.pose.orientation.w = 1
mesh3 = ArmControlMarker._make_mesh_marker(mesh_color)
mesh3.mesh_resource = STR_R_GRIPPER_FINGER_FILE
mesh3.pose.position.x = 0.08
mesh3.pose.position.y = 0.165
mesh3.pose.orientation.w = 1
else:
mesh2 = ArmControlMarker._make_mesh_marker(mesh_color)
mesh2.mesh_resource = STR_L_GRIPPER_FINGER_FILE
mesh2.pose.position.x = 0.08
mesh2.pose.position.y = -0.116
mesh2.pose.orientation.w = 1
mesh3 = ArmControlMarker._make_mesh_marker(mesh_color)
mesh3.mesh_resource = STR_R_GRIPPER_FINGER_FILE
mesh3.pose.position.x = 0.08
mesh3.pose.position.y = 0.116
mesh3.pose.orientation.w = 1
# Append all meshes we made.
control.markers.append(mesh1)
control.markers.append(mesh2)
control.markers.append(mesh3)
# Return back the control.
# TODO(mbforbes): Why do we do this?
return control
class MarkerTemplate(object):
def __init__(self, server, id_, name, initial_pose=None):
self._menu_handler = MenuHandler()
self._menu_handler.insert("Delete", callback=self.deleteCallback)
self._server = server
self._key = name + str(id_)
self._marker_map = {}
self._callback_map = {}
# not all templates will have a dynamic reconfigure server
self._dserver = None
self._initial_pose = initial_pose
@property
def initial_pose(self):
"""Return a marker's initial pose if it was passed in."""
return self._initial_pose
# @property
# def key(self):
# """Get the marker template's key on the server."""
# key = self._key + str(self._id)
# return key
@property
def server(self):
"""Get the marker template's interactive marker server."""
return self._server
@property
def marker_map(self):
"""Get the interactive marker map of this marker template."""
return self._marker_map
@property
def callback_map(self):
"""Get the callback map of this marker template."""
return self._callback_map
@property
def menu_handler(self):
"""Get the menu handler of this marker template."""
return self._menu_handler
@property
def setNudgeDistance(self, distance):
self._nudgeDistance = distance
def getNudgeDistance(self):
return self._nudgeDistance
def initNudgeMenu(self):
nudgeControl = self.menu_handler.insert("Nudge hand " + str(self._nudgeDistance) + "m")
self.menu_handler.insert("World X", parent=nudgeControl, command_type=MenuEntry.FEEDBACK, command="", callback=self.nudgeX)
self.menu_handler.insert("World Y", parent=nudgeControl, command_type=MenuEntry.FEEDBACK, command="", callback=self.nudgeY)
self.menu_handler.insert("World Z", parent=nudgeControl, command_type=MenuEntry.FEEDBACK, command="", callback=self.nudgeZ)
self.menu_handler.insert("World -X", parent=nudgeControl, command_type=MenuEntry.FEEDBACK, command="", callback=self.nudgeXNeg)
self.menu_handler.insert("World -Y", parent=nudgeControl, command_type=MenuEntry.FEEDBACK, command="", callback=self.nudgeYNeg)
self.menu_handler.insert("World -Z", parent=nudgeControl, command_type=MenuEntry.FEEDBACK, command="", callback=self.nudgeZNeg)
def addInteractiveMarker(self, marker, callback=None):
name = marker.name
self._marker_map[name] = marker
if callback:
self._callback_map[name] = callback
return self.server.insert(marker, callback)
else:
self._callback_map[name] = self.processFeedback
return self.server.insert(marker, self.processFeedback)
def removeInteractiveMarker(self, marker_name):
if self.server.erase(marker_name):
if marker_name in self._marker_map:
del self._marker_map[marker_name]
del self._callback_map[marker_name]
return True
return False
def attachMenuHandler(self, marker):
return self.menu_handler.apply(self.server, marker.name)
def getMarker(self):
return self.server.get(self._key)
def hasMarker(self):
if self._key in self._marker_map.keys():
return True
else:
return False
def deleteCallback(self, feedback):
for key in self._marker_map.keys():
self.server.erase(key)
self.server.applyChanges()
self.tearDown()
def tearDown(self, keepAlive=False):
# forcefully shut down service before killing node
if self._dserver:
self._dserver.set_service.shutdown("User deleted template.")
# for rostest (and potentially other cases), we want to clean up but keep the node alive
if not keepAlive:
rospy.signal_shutdown("User deleted template.")
def processFeedback(self, feedback):
self.server.applyChanges()
class ActionStepMarker:
''' Marker for visualizing the steps of an action'''
_im_server = None
_offset = 0.09
_ref_object_list = None
_ref_names = None
_marker_click_cb = None
def __init__(self, step_number, arm_index, action_step,
marker_click_cb):
if ActionStepMarker._im_server == None:
im_server = InteractiveMarkerServer('programmed_actions')
ActionStepMarker._im_server = im_server
self.action_step = action_step
self.arm_index = arm_index
self.step_number = step_number
self.is_requested = False
self.is_deleted = False
self.is_control_visible = False
self.is_edited = False
self.has_object = False
self._sub_entries = None
self._menu_handler = None
ActionStepMarker._marker_click_cb = marker_click_cb
def _is_reachable(self):
'''Checks if there is an IK solution for action step'''
dummy, is_reachable = Arms.solve_ik_for_arm(self.arm_index,
self.get_target())
rospy.loginfo('Reachability of pose in ActionStepMarker : ' +
str(is_reachable))
return is_reachable
def get_uid(self):
'''Returns a unique id of the marker'''
return (2 * self.step_number + self.arm_index)
def _get_name(self):
'''Generates the unique name for the marker'''
return ('step' + str(self.step_number)
+ 'arm' + str(self.arm_index))
def decrease_id(self):
'''Reduces the index of the marker'''
self.step_number -= 1
self._update_menu()
def update_ref_frames(self, ref_frame_list):
'''Updates and re-assigns coordinate frames when the world changes'''
# There is a new list of objects
# If the current frames are already assigned to object,
# we need to figure out the correspondences
ActionStepMarker._ref_object_list = ref_frame_list
arm_pose = self.get_target()
if (arm_pose.refFrame == ArmState.OBJECT):
prev_ref_obj = arm_pose.refFrameObject
new_ref_obj = World.get_most_similar_obj(prev_ref_obj,
ref_frame_list)
self.has_object = False
if (new_ref_obj != None):
self.has_object = True
arm_pose.refFrameObject = new_ref_obj
ActionStepMarker._ref_names = ['base_link']
for i in range(len(ActionStepMarker._ref_object_list)):
ActionStepMarker._ref_names.append(
ActionStepMarker._ref_object_list[i].name)
self._update_menu()
def destroy(self):
'''Removes marker from the world'''
ActionStepMarker._im_server.erase(self._get_name())
ActionStepMarker._im_server.applyChanges()
def _update_menu(self):
'''Recreates the menu when something has changed'''
self._menu_handler = MenuHandler()
frame_entry = self._menu_handler.insert('Reference frame')
self._sub_entries = [None] * len(ActionStepMarker._ref_names)
for i in range(len(ActionStepMarker._ref_names)):
self._sub_entries[i] = self._menu_handler.insert(
ActionStepMarker._ref_names[i], parent=frame_entry,
callback=self.change_ref_cb)
self._menu_handler.insert('Move arm here', callback=self.move_to_cb)
self._menu_handler.insert('Move to current arm pose',
callback=self.move_pose_to_cb)
self._menu_handler.insert('Delete', callback=self.delete_step_cb)
for i in range(len(ActionStepMarker._ref_names)):
self._menu_handler.setCheckState(self._sub_entries[i],
MenuHandler.UNCHECKED)
menu_id = self._get_menu_id(self._get_ref_name())
if menu_id == None:
self.has_object = False
else:
self._menu_handler.setCheckState(menu_id,
MenuHandler.CHECKED)
self._update_viz_core()
self._menu_handler.apply(ActionStepMarker._im_server, self._get_name())
ActionStepMarker._im_server.applyChanges()
def _get_menu_id(self, ref_name):
'''Returns the unique menu id from its name
None if the object is not found'''
if ref_name in ActionStepMarker._ref_names:
index = ActionStepMarker._ref_names.index(ref_name)
return self._sub_entries[index]
else:
return None
def _get_menu_name(self, menu_id):
'''Returns the menu name from its unique menu id'''
index = self._sub_entries.index(menu_id)
return ActionStepMarker._ref_names[index]
def _get_ref_name(self):
'''Returns the name string for the reference
frame object of the action step'''
ref_name = None
if (self.action_step.type == ActionStep.ARM_TARGET):
if self.arm_index == 0:
ref_frame = self.action_step.armTarget.rArm.refFrame
ref_name = self.action_step.armTarget.rArm.refFrameObject.name
else:
ref_frame = self.action_step.armTarget.lArm.refFrame
ref_name = self.action_step.armTarget.lArm.refFrameObject.name
elif (self.action_step.type == ActionStep.ARM_TRAJECTORY):
if self.arm_index == 0:
ref_frame = self.action_step.armTrajectory.rRefFrame
ref_name = self.action_step.armTrajectory.rRefFrameObject.name
else:
ref_frame = self.action_step.armTrajectory.lRefFrame
ref_name = self.action_step.armTrajectory.lRefFrameObject.name
else:
rospy.logerr('Unhandled marker type: ' +
str(self.action_step.type))
if (ref_frame == ArmState.ROBOT_BASE):
ref_name = 'base_link'
return ref_name
def _set_ref(self, new_ref_name):
'''Changes the reference frame of the action step'''
new_ref = World.get_ref_from_name(new_ref_name)
if (new_ref != ArmState.ROBOT_BASE):
index = ActionStepMarker._ref_names.index(new_ref_name)
new_ref_obj = ActionStepMarker._ref_object_list[index - 1]
else:
new_ref_obj = Object()
if (self.action_step.type == ActionStep.ARM_TARGET):
if self.arm_index == 0:
self.action_step.armTarget.rArm = World.convert_ref_frame(
self.action_step.armTarget.rArm, new_ref, new_ref_obj)
else:
self.action_step.armTarget.lArm = World.convert_ref_frame(
self.action_step.armTarget.lArm, new_ref, new_ref_obj)
elif (self.action_step.type == ActionStep.ARM_TRAJECTORY):
for i in range(len(self.action_step.armTrajectory.timing)):
if self.arm_index == 0:
arm_old = self.action_step.armTrajectory.rArm[i]
arm_new = World.convert_ref_frame(arm_old,
new_ref, new_ref_obj)
self.action_step.armTrajectory.rArm[i] = arm_new
else:
arm_old = self.action_step.armTrajectory.lArm[i]
arm_new = World.convert_ref_frame(arm_old,
new_ref, new_ref_obj)
self.action_step.armTrajectory.lArm[i] = arm_new
if self.arm_index == 0:
self.action_step.armTrajectory.rRefFrameObject = new_ref_obj
self.action_step.armTrajectory.rRefFrame = new_ref
else:
self.action_step.armTrajectory.lRefFrameObject = new_ref_obj
self.action_step.armTrajectory.lRefFrame = new_ref
def _is_hand_open(self):
'''Checks if the gripper is open for the action step'''
if self.arm_index == 0:
gripper_state = self.action_step.gripperAction.rGripper
else:
gripper_state = self.action_step.gripperAction.lGripper
return (gripper_state == GripperState.OPEN)
def set_new_pose(self, new_pose):
'''Changes the pose of the action step'''
if (self.action_step.type == ActionStep.ARM_TARGET):
if self.arm_index == 0:
pose = ActionStepMarker._offset_pose(new_pose, -1)
self.action_step.armTarget.rArm.ee_pose = pose
else:
pose = ActionStepMarker._offset_pose(new_pose, -1)
self.action_step.armTarget.lArm.ee_pose = pose
self.update_viz()
elif (self.action_step.type == ActionStep.ARM_TRAJECTORY):
rospy.logwarn('Modification of whole trajectory ' +
'segments is not implemented.')
def get_absolute_position(self, is_start=True):
'''Returns the absolute position of the action step'''
pose = self.get_absolute_pose(is_start)
return pose.position
def get_absolute_pose(self, is_start=True):
'''Returns the absolute pose of the action step'''
if (self.action_step.type == ActionStep.ARM_TARGET):
if self.arm_index == 0:
arm_pose = self.action_step.armTarget.rArm
else:
arm_pose = self.action_step.armTarget.lArm
elif (self.action_step.type == ActionStep.ARM_TRAJECTORY):
if self.arm_index == 0:
if is_start:
index = len(self.action_step.armTrajectory.rArm) - 1
arm_pose = self.action_step.armTrajectory.rArm[index]
else:
arm_pose = self.action_step.armTrajectory.rArm[0]
else:
if is_start:
index = len(self.action_step.armTrajectory.lArm) - 1
arm_pose = self.action_step.armTrajectory.lArm[index]
else:
arm_pose = self.action_step.armTrajectory.lArm[0]
#if (arm_pose.refFrame == ArmState.OBJECT and
# World.has_object(arm_pose.refFrameObject.name)):
# return ActionStepMarker._offset_pose(arm_pose.ee_pose)
#else:
world_pose = World.get_absolute_pose(arm_pose)
return ActionStepMarker._offset_pose(world_pose)
def get_pose(self):
'''Returns the pose of the action step'''
target = self.get_target()
if (target != None):
return ActionStepMarker._offset_pose(target.ee_pose)
@staticmethod
def _offset_pose(pose, constant=1):
'''Offsets the world pose for visualization'''
transform = World.get_matrix_from_pose(pose)
offset_array = [constant * ActionStepMarker._offset, 0, 0]
offset_transform = tf.transformations.translation_matrix(offset_array)
hand_transform = tf.transformations.concatenate_matrices(transform,
offset_transform)
return World.get_pose_from_transform(hand_transform)
def set_target(self, target):
'''Sets the new pose for the action step'''
if (self.action_step.type == ActionStep.ARM_TARGET):
if self.arm_index == 0:
self.action_step.armTarget.rArm = target
else:
self.action_step.armTarget.lArm = target
self.has_object = True
self._update_menu()
self.is_edited = False
def get_target(self, traj_index=None):
'''Returns the pose for the action step'''
if (self.action_step.type == ActionStep.ARM_TARGET):
if self.arm_index == 0:
return self.action_step.armTarget.rArm
else:
return self.action_step.armTarget.lArm
elif (self.action_step.type == ActionStep.ARM_TRAJECTORY):
if self.arm_index == 0:
if traj_index == None:
traj = self.action_step.armTrajectory.rArm
traj_index = int(len(traj) / 2)
return self.action_step.armTrajectory.rArm[traj_index]
else:
if traj_index == None:
traj = self.action_step.armTrajectory.lArm
traj_index = int(len(traj) / 2)
return self.action_step.armTrajectory.lArm[traj_index]
def _get_traj_pose(self, index):
'''Returns a single pose for trajectory'''
if (self.action_step.type == ActionStep.ARM_TRAJECTORY):
if self.arm_index == 0:
target = self.action_step.armTrajectory.rArm[index]
else:
target = self.action_step.armTrajectory.lArm[index]
return target.ee_pose
else:
rospy.logerr('Cannot request trajectory pose ' +
'on non-trajectory action step.')
def _update_viz_core(self):
'''Updates visualization after a change'''
menu_control = InteractiveMarkerControl()
menu_control.interaction_mode = InteractiveMarkerControl.BUTTON
menu_control.always_visible = True
frame_id = self._get_ref_name()
pose = self.get_pose()
if (self.action_step.type == ActionStep.ARM_TARGET):
menu_control = self._make_gripper_marker(menu_control,
self._is_hand_open())
elif (self.action_step.type == ActionStep.ARM_TRAJECTORY):
point_list = []
for j in range(len(self.action_step.armTrajectory.timing)):
point_list.append(self._get_traj_pose(j).position)
main_marker = Marker(type=Marker.SPHERE_LIST, id=self.get_uid(),
lifetime=rospy.Duration(2),
scale=Vector3(0.02, 0.02, 0.02),
header=Header(frame_id=frame_id),
color=ColorRGBA(0.8, 0.4, 0.0, 0.8),
points=point_list)
menu_control.markers.append(main_marker)
menu_control.markers.append(ActionStepMarker.make_sphere_marker(
self.get_uid() + 2000,
self._get_traj_pose(0), frame_id, 0.05))
last_index = len(self.action_step.armTrajectory.timing) - 1
menu_control.markers.append(ActionStepMarker.make_sphere_marker(
self.get_uid() + 3000, self._get_traj_pose(last_index),
frame_id, 0.05))
else:
rospy.logerr('Non-handled action step type '
+ str(self.action_step.type))
ref_frame = World.get_ref_from_name(frame_id)
if (ref_frame == ArmState.OBJECT):
menu_control.markers.append(Marker(type=Marker.ARROW,
id=(1000 + self.get_uid()),
lifetime=rospy.Duration(2),
scale=Vector3(0.02, 0.03, 0.04),
header=Header(frame_id=frame_id),
color=ColorRGBA(1.0, 0.8, 0.2, 0.5),
points=[pose.position, Point(0, 0, 0)]))
text_pos = Point()
text_pos.x = pose.position.x
text_pos.y = pose.position.y
text_pos.z = pose.position.z + 0.1
menu_control.markers.append(Marker(type=Marker.TEXT_VIEW_FACING,
id=self.get_uid(), scale=Vector3(0, 0, 0.03),
text='Step' + str(self.step_number),
color=ColorRGBA(0.0, 0.0, 0.0, 0.5),
header=Header(frame_id=frame_id),
pose=Pose(text_pos, Quaternion(0, 0, 0, 1))))
int_marker = InteractiveMarker()
int_marker.name = self._get_name()
int_marker.header.frame_id = frame_id
int_marker.pose = pose
int_marker.scale = 0.2
self._add_6dof_marker(int_marker, True)
int_marker.controls.append(menu_control)
ActionStepMarker._im_server.insert(int_marker,
self.marker_feedback_cb)
@staticmethod
def make_sphere_marker(uid, pose, frame_id, radius):
'''Creates a sphere marker'''
return Marker(type=Marker.SPHERE, id=uid, lifetime=rospy.Duration(2),
scale=Vector3(radius, radius, radius),
pose=pose, header=Header(frame_id=frame_id),
color=ColorRGBA(1.0, 0.5, 0.0, 0.8))
def marker_feedback_cb(self, feedback):
'''Callback for when an event occurs on the marker'''
if feedback.event_type == InteractiveMarkerFeedback.POSE_UPDATE:
self.set_new_pose(feedback.pose)
self.update_viz()
elif feedback.event_type == InteractiveMarkerFeedback.BUTTON_CLICK:
# Set the visibility of the 6DOF controller
rospy.loginfo('Changing visibility of the pose controls.')
if (self.is_control_visible):
self.is_control_visible = False
else:
self.is_control_visible = True
ActionStepMarker._marker_click_cb(self.get_uid(),
self.is_control_visible)
else:
rospy.loginfo('Unknown event' + str(feedback.event_type))
def delete_step_cb(self, dummy):
'''Callback for when delete is requested'''
self.is_deleted = True
def move_to_cb(self, dummy):
'''Callback for when moving to a pose is requested'''
self.is_requested = True
def move_pose_to_cb(self, dummy):
'''Callback for when a pose change to current is requested'''
self.is_edited = True
def pose_reached(self):
'''Update when a requested pose is reached'''
self.is_requested = False
def change_ref_cb(self, feedback):
'''Callback for when a reference frame change is requested'''
self._menu_handler.setCheckState(
self._get_menu_id(self._get_ref_name()),
MenuHandler.UNCHECKED)
self._menu_handler.setCheckState(feedback.menu_entry_id,
MenuHandler.CHECKED)
new_ref = self._get_menu_name(feedback.menu_entry_id)
self._set_ref(new_ref)
rospy.loginfo('Switching reference frame to '
+ new_ref + ' for action step ' + self._get_name())
self._menu_handler.reApply(ActionStepMarker._im_server)
ActionStepMarker._im_server.applyChanges()
self.update_viz()
def update_viz(self):
'''Updates visualization fully'''
self._update_viz_core()
self._menu_handler.reApply(ActionStepMarker._im_server)
ActionStepMarker._im_server.applyChanges()
def _add_6dof_marker(self, int_marker, is_fixed):
'''Adds a 6 DoF control marker to the interactive marker'''
control = self._make_6dof_control('rotate_x',
Quaternion(1, 0, 0, 1), False, is_fixed)
int_marker.controls.append(control)
control = self._make_6dof_control('move_x',
Quaternion(1, 0, 0, 1), True, is_fixed)
int_marker.controls.append(control)
control = self._make_6dof_control('rotate_z',
Quaternion(0, 1, 0, 1), False, is_fixed)
int_marker.controls.append(control)
control = self._make_6dof_control('move_z',
Quaternion(0, 1, 0, 1), True, is_fixed)
int_marker.controls.append(control)
control = self._make_6dof_control('rotate_y',
Quaternion(0, 0, 1, 1), False, is_fixed)
int_marker.controls.append(control)
control = self._make_6dof_control('move_y',
Quaternion(0, 0, 1, 1), True, is_fixed)
int_marker.controls.append(control)
def _make_6dof_control(self, name, orientation, is_move, is_fixed):
'''Creates one component of the 6dof controller'''
control = InteractiveMarkerControl()
control.name = name
control.orientation = orientation
control.always_visible = False
if (self.is_control_visible):
if is_move:
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
else:
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
else:
control.interaction_mode = InteractiveMarkerControl.NONE
if is_fixed:
control.orientation_mode = InteractiveMarkerControl.FIXED
return control
def _make_mesh_marker(self):
'''Creates a mesh marker'''
mesh = Marker()
mesh.mesh_use_embedded_materials = False
mesh.type = Marker.MESH_RESOURCE
mesh.scale.x = 1.0
mesh.scale.y = 1.0
mesh.scale.z = 1.0
if self._is_reachable():
mesh.color = ColorRGBA(1.0, 0.5, 0.0, 0.6)
else:
mesh.color = ColorRGBA(0.5, 0.5, 0.5, 0.6)
return mesh
def _make_gripper_marker(self, control, is_hand_open=False):
'''Makes a gripper marker'''
if is_hand_open:
angle = 28 * numpy.pi / 180.0
else:
angle = 0
transform1 = tf.transformations.euler_matrix(0, 0, angle)
transform1[:3, 3] = [0.07691 - ActionStepMarker._offset, 0.01, 0]
transform2 = tf.transformations.euler_matrix(0, 0, -angle)
transform2[:3, 3] = [0.09137, 0.00495, 0]
t_proximal = transform1
t_distal = tf.transformations.concatenate_matrices(transform1,
transform2)
mesh1 = self._make_mesh_marker()
mesh1.mesh_resource = ('package://pr2_description/meshes/' +
'gripper_v0/gripper_palm.dae')
mesh1.pose.position.x = -ActionStepMarker._offset
mesh1.pose.orientation.w = 1
mesh2 = self._make_mesh_marker()
mesh2.mesh_resource = ('package://pr2_description/meshes/' +
'gripper_v0/l_finger.dae')
mesh2.pose = World.get_pose_from_transform(t_proximal)
mesh3 = self._make_mesh_marker()
mesh3.mesh_resource = ('package://pr2_description/meshes/' +
'gripper_v0/l_finger_tip.dae')
mesh3.pose = World.get_pose_from_transform(t_distal)
quat = tf.transformations.quaternion_multiply(
tf.transformations.quaternion_from_euler(numpy.pi, 0, 0),
tf.transformations.quaternion_from_euler(0, 0, angle))
transform1 = tf.transformations.quaternion_matrix(quat)
transform1[:3, 3] = [0.07691 - ActionStepMarker._offset, -0.01, 0]
transform2 = tf.transformations.euler_matrix(0, 0, -angle)
transform2[:3, 3] = [0.09137, 0.00495, 0]
t_proximal = transform1
t_distal = tf.transformations.concatenate_matrices(transform1,
transform2)
mesh4 = self._make_mesh_marker()
mesh4.mesh_resource = ('package://pr2_description/meshes/' +
'gripper_v0/l_finger.dae')
mesh4.pose = World.get_pose_from_transform(t_proximal)
mesh5 = self._make_mesh_marker()
mesh5.mesh_resource = ('package://pr2_description/meshes/' +
'gripper_v0/l_finger_tip.dae')
mesh5.pose = World.get_pose_from_transform(t_distal)
control.markers.append(mesh1)
control.markers.append(mesh2)
control.markers.append(mesh3)
control.markers.append(mesh4)
control.markers.append(mesh5)
return control
class SimpleInteractiveMarker(object):
"""Creates a group of markers whose poses can be edited."""
def __init__(self, server, name, X_WM, markers):
self._server = server
self.name = name
self._X_WM = X_WM
self._menu = MenuHandler()
self._edit_id = self._menu.insert("Edit", callback=self._edit_callback)
self._delete_id = self._menu.insert("Delete", callback=self._delete_callback)
self._menu.insert(self.name, callback=noop) # For annotation.
self._t_click_prev = -float('inf')
# Make template marker.
template = InteractiveMarker()
template.name = self.name
template.description = name
template.header.frame_id = "world"
viz = InteractiveMarkerControl()
viz.interaction_mode = InteractiveMarkerControl.BUTTON
viz.always_visible = True
viz.markers.extend(markers)
template.controls.append(viz)
self._template = template
# Initialize.
self._edit = False
self._update()
def get_pose(self):
return self._X_WM
def set_pose(self, X_WM):
self._X_WM = X_WM
self._server.setPose(self.name, self._X_WM)
self._server.applyChanges()
def delete(self):
self._X_WM = None
self._server.erase(self.name)
self._server.applyChanges()
def _update(self):
# Construct interactive marker.
marker = copy.deepcopy(self._template)
marker.pose = self._X_WM
if self._edit:
marker.controls += make_6dof_controls()
state = MenuHandler.CHECKED
else:
state = MenuHandler.UNCHECKED
self._menu.setCheckState(self._edit_id, state)
self._server.insert(marker, self._marker_callback)
self._menu.apply(self._server, marker.name)
self._server.applyChanges()
def _marker_callback(self, msg):
if msg.event_type == InteractiveMarkerFeedback.POSE_UPDATE:
self._X_WM = msg.pose
if msg.event_type == InteractiveMarkerFeedback.BUTTON_CLICK:
# Require double-click.
t_click = time.time()
dt_click = t_click - self._t_click_prev
self._t_click_prev = t_click
if dt_click < DT_DOUBLE_CLICK:
self._edit = not self._edit
self._update()
def _edit_callback(self, _):
# Toggle.
self._edit = not self._edit
self._update()
def _delete_callback(self, _):
self.delete()
class ActionStepMarker:
''' Marker for visualizing the steps of an action'''
_im_server = None
_offset = 0.09
_ref_object_list = None
_ref_names = None
_marker_click_cb = None
def __init__(self, step_number, arm_index, action_step, marker_click_cb):
if ActionStepMarker._im_server == None:
im_server = InteractiveMarkerServer('programmed_actions')
ActionStepMarker._im_server = im_server
self.action_step = action_step
self.arm_index = arm_index
self.step_number = step_number
self.is_requested = False
self.is_deleted = False
self.is_control_visible = False
self.is_edited = False
self.has_object = False
self._sub_entries = None
self._menu_handler = None
ActionStepMarker._marker_click_cb = marker_click_cb
def _is_reachable(self):
'''Checks if there is an IK solution for action step'''
dummy, is_reachable = Arms.solve_ik_for_arm(self.arm_index,
self.get_target())
rospy.loginfo('Reachability of pose in ActionStepMarker : ' +
str(is_reachable))
return is_reachable
def get_uid(self):
'''Returns a unique id of the marker'''
return (2 * self.step_number + self.arm_index)
def _get_name(self):
'''Generates the unique name for the marker'''
return ('step' + str(self.step_number) + 'arm' + str(self.arm_index))
def decrease_id(self):
'''Reduces the index of the marker'''
self.step_number -= 1
self._update_menu()
def update_ref_frames(self, ref_frame_list):
'''Updates and re-assigns coordinate frames when the world changes'''
# There is a new list of objects
# If the current frames are already assigned to object,
# we need to figure out the correspondences
ActionStepMarker._ref_object_list = ref_frame_list
arm_pose = self.get_target()
if (arm_pose.refFrame == ArmState.OBJECT):
prev_ref_obj = arm_pose.refFrameObject
new_ref_obj = World.get_most_similar_obj(prev_ref_obj,
ref_frame_list)
self.has_object = False
if (new_ref_obj != None):
self.has_object = True
arm_pose.refFrameObject = new_ref_obj
ActionStepMarker._ref_names = ['base_link']
for i in range(len(ActionStepMarker._ref_object_list)):
ActionStepMarker._ref_names.append(
ActionStepMarker._ref_object_list[i].name)
self._update_menu()
def destroy(self):
'''Removes marker from the world'''
ActionStepMarker._im_server.erase(self._get_name())
ActionStepMarker._im_server.applyChanges()
def _update_menu(self):
'''Recreates the menu when something has changed'''
self._menu_handler = MenuHandler()
frame_entry = self._menu_handler.insert('Reference frame')
self._sub_entries = [None] * len(ActionStepMarker._ref_names)
for i in range(len(ActionStepMarker._ref_names)):
self._sub_entries[i] = self._menu_handler.insert(
ActionStepMarker._ref_names[i],
parent=frame_entry,
callback=self.change_ref_cb)
self._menu_handler.insert('Move arm here', callback=self.move_to_cb)
self._menu_handler.insert('Move to current arm pose',
callback=self.move_pose_to_cb)
self._menu_handler.insert('Delete', callback=self.delete_step_cb)
for i in range(len(ActionStepMarker._ref_names)):
self._menu_handler.setCheckState(self._sub_entries[i],
MenuHandler.UNCHECKED)
menu_id = self._get_menu_id(self._get_ref_name())
if menu_id == None:
self.has_object = False
else:
self._menu_handler.setCheckState(menu_id, MenuHandler.CHECKED)
self._update_viz_core()
self._menu_handler.apply(ActionStepMarker._im_server, self._get_name())
ActionStepMarker._im_server.applyChanges()
def _get_menu_id(self, ref_name):
'''Returns the unique menu id from its name
None if the object is not found'''
if ref_name in ActionStepMarker._ref_names:
index = ActionStepMarker._ref_names.index(ref_name)
return self._sub_entries[index]
else:
return None
def _get_menu_name(self, menu_id):
'''Returns the menu name from its unique menu id'''
index = self._sub_entries.index(menu_id)
return ActionStepMarker._ref_names[index]
def _get_ref_name(self):
'''Returns the name string for the reference
frame object of the action step'''
ref_name = None
if (self.action_step.type == ActionStep.ARM_TARGET):
if self.arm_index == 0:
ref_frame = self.action_step.armTarget.rArm.refFrame
ref_name = self.action_step.armTarget.rArm.refFrameObject.name
else:
ref_frame = self.action_step.armTarget.lArm.refFrame
ref_name = self.action_step.armTarget.lArm.refFrameObject.name
elif (self.action_step.type == ActionStep.ARM_TRAJECTORY):
if self.arm_index == 0:
ref_frame = self.action_step.armTrajectory.rRefFrame
ref_name = self.action_step.armTrajectory.rRefFrameObject.name
else:
ref_frame = self.action_step.armTrajectory.lRefFrame
ref_name = self.action_step.armTrajectory.lRefFrameObject.name
else:
rospy.logerr('Unhandled marker type: ' +
str(self.action_step.type))
if (ref_frame == ArmState.ROBOT_BASE):
ref_name = 'base_link'
return ref_name
def _set_ref(self, new_ref_name):
'''Changes the reference frame of the action step'''
new_ref = World.get_ref_from_name(new_ref_name)
if (new_ref != ArmState.ROBOT_BASE):
index = ActionStepMarker._ref_names.index(new_ref_name)
new_ref_obj = ActionStepMarker._ref_object_list[index - 1]
else:
new_ref_obj = Object()
if (self.action_step.type == ActionStep.ARM_TARGET):
if self.arm_index == 0:
self.action_step.armTarget.rArm = World.convert_ref_frame(
self.action_step.armTarget.rArm, new_ref, new_ref_obj)
else:
self.action_step.armTarget.lArm = World.convert_ref_frame(
self.action_step.armTarget.lArm, new_ref, new_ref_obj)
elif (self.action_step.type == ActionStep.ARM_TRAJECTORY):
for i in range(len(self.action_step.armTrajectory.timing)):
if self.arm_index == 0:
arm_old = self.action_step.armTrajectory.rArm[i]
arm_new = World.convert_ref_frame(arm_old, new_ref,
new_ref_obj)
self.action_step.armTrajectory.rArm[i] = arm_new
else:
arm_old = self.action_step.armTrajectory.lArm[i]
arm_new = World.convert_ref_frame(arm_old, new_ref,
new_ref_obj)
self.action_step.armTrajectory.lArm[i] = arm_new
if self.arm_index == 0:
self.action_step.armTrajectory.rRefFrameObject = new_ref_obj
self.action_step.armTrajectory.rRefFrame = new_ref
else:
self.action_step.armTrajectory.lRefFrameObject = new_ref_obj
self.action_step.armTrajectory.lRefFrame = new_ref
def _is_hand_open(self):
'''Checks if the gripper is open for the action step'''
if self.arm_index == 0:
gripper_state = self.action_step.gripperAction.rGripper
else:
gripper_state = self.action_step.gripperAction.lGripper
return (gripper_state == GripperState.OPEN)
def set_new_pose(self, new_pose):
'''Changes the pose of the action step'''
if (self.action_step.type == ActionStep.ARM_TARGET):
if self.arm_index == 0:
pose = ActionStepMarker._offset_pose(new_pose, -1)
self.action_step.armTarget.rArm.ee_pose = pose
else:
pose = ActionStepMarker._offset_pose(new_pose, -1)
self.action_step.armTarget.lArm.ee_pose = pose
self.update_viz()
elif (self.action_step.type == ActionStep.ARM_TRAJECTORY):
rospy.logwarn('Modification of whole trajectory ' +
'segments is not implemented.')
def get_absolute_position(self, is_start=True):
'''Returns the absolute position of the action step'''
pose = self.get_absolute_pose(is_start)
return pose.position
def get_absolute_pose(self, is_start=True):
'''Returns the absolute pose of the action step'''
if (self.action_step.type == ActionStep.ARM_TARGET):
if self.arm_index == 0:
arm_pose = self.action_step.armTarget.rArm
else:
arm_pose = self.action_step.armTarget.lArm
elif (self.action_step.type == ActionStep.ARM_TRAJECTORY):
if self.arm_index == 0:
if is_start:
index = len(self.action_step.armTrajectory.rArm) - 1
arm_pose = self.action_step.armTrajectory.rArm[index]
else:
arm_pose = self.action_step.armTrajectory.rArm[0]
else:
if is_start:
index = len(self.action_step.armTrajectory.lArm) - 1
arm_pose = self.action_step.armTrajectory.lArm[index]
else:
arm_pose = self.action_step.armTrajectory.lArm[0]
#if (arm_pose.refFrame == ArmState.OBJECT and
# World.has_object(arm_pose.refFrameObject.name)):
# return ActionStepMarker._offset_pose(arm_pose.ee_pose)
#else:
world_pose = World.get_absolute_pose(arm_pose)
return ActionStepMarker._offset_pose(world_pose)
def get_pose(self):
'''Returns the pose of the action step'''
target = self.get_target()
if (target != None):
return ActionStepMarker._offset_pose(target.ee_pose)
@staticmethod
def _offset_pose(pose, constant=1):
'''Offsets the world pose for visualization'''
transform = World.get_matrix_from_pose(pose)
offset_array = [constant * ActionStepMarker._offset, 0, 0]
offset_transform = tf.transformations.translation_matrix(offset_array)
hand_transform = tf.transformations.concatenate_matrices(
transform, offset_transform)
return World.get_pose_from_transform(hand_transform)
def set_target(self, target):
'''Sets the new pose for the action step'''
if (self.action_step.type == ActionStep.ARM_TARGET):
if self.arm_index == 0:
self.action_step.armTarget.rArm = target
else:
self.action_step.armTarget.lArm = target
self.has_object = True
self._update_menu()
self.is_edited = False
def get_target(self, traj_index=None):
'''Returns the pose for the action step'''
if (self.action_step.type == ActionStep.ARM_TARGET):
if self.arm_index == 0:
return self.action_step.armTarget.rArm
else:
return self.action_step.armTarget.lArm
elif (self.action_step.type == ActionStep.ARM_TRAJECTORY):
if self.arm_index == 0:
if traj_index == None:
traj = self.action_step.armTrajectory.rArm
traj_index = int(len(traj) / 2)
return self.action_step.armTrajectory.rArm[traj_index]
else:
if traj_index == None:
traj = self.action_step.armTrajectory.lArm
traj_index = int(len(traj) / 2)
return self.action_step.armTrajectory.lArm[traj_index]
def _get_traj_pose(self, index):
'''Returns a single pose for trajectory'''
if (self.action_step.type == ActionStep.ARM_TRAJECTORY):
if self.arm_index == 0:
target = self.action_step.armTrajectory.rArm[index]
else:
target = self.action_step.armTrajectory.lArm[index]
return target.ee_pose
else:
rospy.logerr('Cannot request trajectory pose ' +
'on non-trajectory action step.')
def _update_viz_core(self):
'''Updates visualization after a change'''
menu_control = InteractiveMarkerControl()
menu_control.interaction_mode = InteractiveMarkerControl.BUTTON
menu_control.always_visible = True
frame_id = self._get_ref_name()
pose = self.get_pose()
if (self.action_step.type == ActionStep.ARM_TARGET):
menu_control = self._make_gripper_marker(menu_control,
self._is_hand_open())
elif (self.action_step.type == ActionStep.ARM_TRAJECTORY):
point_list = []
for j in range(len(self.action_step.armTrajectory.timing)):
point_list.append(self._get_traj_pose(j).position)
main_marker = Marker(type=Marker.SPHERE_LIST,
id=self.get_uid(),
lifetime=rospy.Duration(2),
scale=Vector3(0.02, 0.02, 0.02),
header=Header(frame_id=frame_id),
color=ColorRGBA(0.8, 0.4, 0.0, 0.8),
points=point_list)
menu_control.markers.append(main_marker)
menu_control.markers.append(
ActionStepMarker.make_sphere_marker(self.get_uid() + 2000,
self._get_traj_pose(0),
frame_id, 0.05))
last_index = len(self.action_step.armTrajectory.timing) - 1
menu_control.markers.append(
ActionStepMarker.make_sphere_marker(
self.get_uid() + 3000, self._get_traj_pose(last_index),
frame_id, 0.05))
else:
rospy.logerr('Non-handled action step type ' +
str(self.action_step.type))
ref_frame = World.get_ref_from_name(frame_id)
if (ref_frame == ArmState.OBJECT):
menu_control.markers.append(
Marker(type=Marker.ARROW,
id=(1000 + self.get_uid()),
lifetime=rospy.Duration(2),
scale=Vector3(0.02, 0.03, 0.04),
header=Header(frame_id=frame_id),
color=ColorRGBA(1.0, 0.8, 0.2, 0.5),
points=[pose.position, Point(0, 0, 0)]))
text_pos = Point()
text_pos.x = pose.position.x
text_pos.y = pose.position.y
text_pos.z = pose.position.z + 0.1
menu_control.markers.append(
Marker(type=Marker.TEXT_VIEW_FACING,
id=self.get_uid(),
scale=Vector3(0, 0, 0.03),
text='Step' + str(self.step_number),
color=ColorRGBA(0.0, 0.0, 0.0, 0.5),
header=Header(frame_id=frame_id),
pose=Pose(text_pos, Quaternion(0, 0, 0, 1))))
int_marker = InteractiveMarker()
int_marker.name = self._get_name()
int_marker.header.frame_id = frame_id
int_marker.pose = pose
int_marker.scale = 0.2
self._add_6dof_marker(int_marker, True)
int_marker.controls.append(menu_control)
ActionStepMarker._im_server.insert(int_marker, self.marker_feedback_cb)
@staticmethod
def make_sphere_marker(uid, pose, frame_id, radius):
'''Creates a sphere marker'''
return Marker(type=Marker.SPHERE,
id=uid,
lifetime=rospy.Duration(2),
scale=Vector3(radius, radius, radius),
pose=pose,
header=Header(frame_id=frame_id),
color=ColorRGBA(1.0, 0.5, 0.0, 0.8))
def marker_feedback_cb(self, feedback):
'''Callback for when an event occurs on the marker'''
if feedback.event_type == InteractiveMarkerFeedback.POSE_UPDATE:
self.set_new_pose(feedback.pose)
self.update_viz()
elif feedback.event_type == InteractiveMarkerFeedback.BUTTON_CLICK:
# Set the visibility of the 6DOF controller
rospy.loginfo('Changing visibility of the pose controls.')
if (self.is_control_visible):
self.is_control_visible = False
else:
self.is_control_visible = True
ActionStepMarker._marker_click_cb(self.get_uid(),
self.is_control_visible)
else:
rospy.loginfo('Unknown event' + str(feedback.event_type))
def delete_step_cb(self, dummy):
'''Callback for when delete is requested'''
self.is_deleted = True
def move_to_cb(self, dummy):
'''Callback for when moving to a pose is requested'''
self.is_requested = True
def move_pose_to_cb(self, dummy):
'''Callback for when a pose change to current is requested'''
self.is_edited = True
def pose_reached(self):
'''Update when a requested pose is reached'''
self.is_requested = False
def change_ref_cb(self, feedback):
'''Callback for when a reference frame change is requested'''
self._menu_handler.setCheckState(
self._get_menu_id(self._get_ref_name()), MenuHandler.UNCHECKED)
self._menu_handler.setCheckState(feedback.menu_entry_id,
MenuHandler.CHECKED)
new_ref = self._get_menu_name(feedback.menu_entry_id)
self._set_ref(new_ref)
rospy.loginfo('Switching reference frame to ' + new_ref +
' for action step ' + self._get_name())
self._menu_handler.reApply(ActionStepMarker._im_server)
ActionStepMarker._im_server.applyChanges()
self.update_viz()
def update_viz(self):
'''Updates visualization fully'''
self._update_viz_core()
self._menu_handler.reApply(ActionStepMarker._im_server)
ActionStepMarker._im_server.applyChanges()
def _add_6dof_marker(self, int_marker, is_fixed):
'''Adds a 6 DoF control marker to the interactive marker'''
control = self._make_6dof_control('rotate_x', Quaternion(1, 0, 0, 1),
False, is_fixed)
int_marker.controls.append(control)
control = self._make_6dof_control('move_x', Quaternion(1, 0, 0, 1),
True, is_fixed)
int_marker.controls.append(control)
control = self._make_6dof_control('rotate_z', Quaternion(0, 1, 0, 1),
False, is_fixed)
int_marker.controls.append(control)
control = self._make_6dof_control('move_z', Quaternion(0, 1, 0, 1),
True, is_fixed)
int_marker.controls.append(control)
control = self._make_6dof_control('rotate_y', Quaternion(0, 0, 1, 1),
False, is_fixed)
int_marker.controls.append(control)
control = self._make_6dof_control('move_y', Quaternion(0, 0, 1, 1),
True, is_fixed)
int_marker.controls.append(control)
def _make_6dof_control(self, name, orientation, is_move, is_fixed):
'''Creates one component of the 6dof controller'''
control = InteractiveMarkerControl()
control.name = name
control.orientation = orientation
control.always_visible = False
if (self.is_control_visible):
if is_move:
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
else:
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
else:
control.interaction_mode = InteractiveMarkerControl.NONE
if is_fixed:
control.orientation_mode = InteractiveMarkerControl.FIXED
return control
def _make_mesh_marker(self):
'''Creates a mesh marker'''
mesh = Marker()
mesh.mesh_use_embedded_materials = False
mesh.type = Marker.MESH_RESOURCE
mesh.scale.x = 1.0
mesh.scale.y = 1.0
mesh.scale.z = 1.0
if self._is_reachable():
mesh.color = ColorRGBA(1.0, 0.5, 0.0, 0.6)
else:
mesh.color = ColorRGBA(0.5, 0.5, 0.5, 0.6)
return mesh
def _make_gripper_marker(self, control, is_hand_open=False):
'''Makes a gripper marker'''
if is_hand_open:
angle = 28 * numpy.pi / 180.0
else:
angle = 0
transform1 = tf.transformations.euler_matrix(0, 0, angle)
transform1[:3, 3] = [0.07691 - ActionStepMarker._offset, 0.01, 0]
transform2 = tf.transformations.euler_matrix(0, 0, -angle)
transform2[:3, 3] = [0.09137, 0.00495, 0]
t_proximal = transform1
t_distal = tf.transformations.concatenate_matrices(
transform1, transform2)
mesh1 = self._make_mesh_marker()
mesh1.mesh_resource = ('package://pr2_description/meshes/' +
'gripper_v0/gripper_palm.dae')
mesh1.pose.position.x = -ActionStepMarker._offset
mesh1.pose.orientation.w = 1
mesh2 = self._make_mesh_marker()
mesh2.mesh_resource = ('package://pr2_description/meshes/' +
'gripper_v0/l_finger.dae')
mesh2.pose = World.get_pose_from_transform(t_proximal)
mesh3 = self._make_mesh_marker()
mesh3.mesh_resource = ('package://pr2_description/meshes/' +
'gripper_v0/l_finger_tip.dae')
mesh3.pose = World.get_pose_from_transform(t_distal)
quat = tf.transformations.quaternion_multiply(
tf.transformations.quaternion_from_euler(numpy.pi, 0, 0),
tf.transformations.quaternion_from_euler(0, 0, angle))
transform1 = tf.transformations.quaternion_matrix(quat)
transform1[:3, 3] = [0.07691 - ActionStepMarker._offset, -0.01, 0]
transform2 = tf.transformations.euler_matrix(0, 0, -angle)
transform2[:3, 3] = [0.09137, 0.00495, 0]
t_proximal = transform1
t_distal = tf.transformations.concatenate_matrices(
transform1, transform2)
mesh4 = self._make_mesh_marker()
mesh4.mesh_resource = ('package://pr2_description/meshes/' +
'gripper_v0/l_finger.dae')
mesh4.pose = World.get_pose_from_transform(t_proximal)
mesh5 = self._make_mesh_marker()
mesh5.mesh_resource = ('package://pr2_description/meshes/' +
'gripper_v0/l_finger_tip.dae')
mesh5.pose = World.get_pose_from_transform(t_distal)
control.markers.append(mesh1)
control.markers.append(mesh2)
control.markers.append(mesh3)
control.markers.append(mesh4)
control.markers.append(mesh5)
return control
class InteractiveMarkerPoseStampedPublisher():
def __init__(self):
self.server = InteractiveMarkerServer("posestamped_im")
self.menu_handler = MenuHandler()
# self.pub = rospy.Publisher('/whole_body_kinematic_controler/wrist_right_ft_link_goal', PoseStamped, queue_size=1)
# self.pub = rospy.Publisher('/whole_body_kinematic_controler/arm_right_7_link_goal', PoseStamped, queue_size=1)
self.pub = rospy.Publisher('/pose_to_follow',
PoseStamped,
queue_size=1)
rospy.loginfo("Publishing posestampeds at topic: " +
str(self.pub.name))
#self.global_frame_id = 'world'
self.global_frame_id = 'base_footprint'
pose = Pose()
pose.position.x = 0.2
# pose.position.y = 0.35
pose.position.y = -0.35
#pose.position.z = 0.1
pose.position.z = 1.1
pose.orientation.w = 1.0
# pose.orientation.x = 0.35762
# pose.orientation.y = 0.50398
# pose.orientation.z = 0.45213
# pose.orientation.w = 0.64319
#self.makeMenuMarker(pose)
self.makeGraspIM(pose)
self.server.applyChanges()
def processFeedback(self, feedback):
"""
:type feedback: InteractiveMarkerFeedback
"""
s = "Feedback from marker '" + feedback.marker_name
s += "' / control '" + feedback.control_name + "'"
mp = ""
if feedback.mouse_point_valid:
mp = " at " + str(feedback.mouse_point.x)
mp += ", " + str(feedback.mouse_point.y)
mp += ", " + str(feedback.mouse_point.z)
mp += " in frame " + feedback.header.frame_id
if feedback.event_type == InteractiveMarkerFeedback.BUTTON_CLICK:
rospy.loginfo(s + ": button click" + mp + ".")
elif feedback.event_type == InteractiveMarkerFeedback.MENU_SELECT:
rospy.loginfo(s + ": menu item " + str(feedback.menu_entry_id) +
" clicked" + mp + ".")
# When clicking this event triggers!
elif feedback.event_type == InteractiveMarkerFeedback.POSE_UPDATE:
rospy.loginfo(s + ": pose changed")
ps = PoseStamped()
ps.header.frame_id = self.global_frame_id #'/world'
ps.pose = feedback.pose
self.pub.publish(ps)
# TODO
# << "\nposition = "
# << feedback.pose.position.x
# << ", " << feedback.pose.position.y
# << ", " << feedback.pose.position.z
# << "\norientation = "
# << feedback.pose.orientation.w
# << ", " << feedback.pose.orientation.x
# << ", " << feedback.pose.orientation.y
# << ", " << feedback.pose.orientation.z
# << "\nframe: " << feedback.header.frame_id
# << " time: " << feedback.header.stamp.sec << "sec, "
# << feedback.header.stamp.nsec << " nsec" )
elif feedback.event_type == InteractiveMarkerFeedback.MOUSE_DOWN:
rospy.loginfo(s + ": mouse down" + mp + ".")
elif feedback.event_type == InteractiveMarkerFeedback.MOUSE_UP:
rospy.loginfo(s + ": mouse up" + mp + ".")
self.server.applyChanges()
def makeArrow(self, msg):
marker = Marker()
marker.type = Marker.ARROW
marker.scale.x = 0.15 #msg.scale * 0.3
marker.scale.y = 0.08 #msg.scale * 0.1
marker.scale.z = 0.03 #msg.scale * 0.03
marker.color.r = 0.3
marker.color.g = 0.3
marker.color.b = 0.5
marker.color.a = 1.0
return marker
def makeBoxControl(self, msg):
control = InteractiveMarkerControl()
control.always_visible = True
control.markers.append(self.makeArrow(msg))
msg.controls.append(control)
return control
def makeGraspIM(self, pose):
"""
:type pose: Pose
"""
int_marker = InteractiveMarker()
int_marker.header.frame_id = self.global_frame_id # "/world"
int_marker.pose = pose
int_marker.scale = 0.3
int_marker.name = "6dof_eef"
int_marker.description = "" #"EEF 6DOF control"
# insert a box, well, an arrow
self.makeBoxControl(int_marker)
int_marker.controls[
0].interaction_mode = InteractiveMarkerControl.MOVE_ROTATE_3D
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 1
control.orientation.y = 0
control.orientation.z = 0
control.name = "rotate_x"
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
int_marker.controls.append(control)
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 1
control.orientation.y = 0
control.orientation.z = 0
control.name = "move_x"
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
int_marker.controls.append(control)
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 0
control.orientation.y = 1
control.orientation.z = 0
control.name = "rotate_z"
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
int_marker.controls.append(control)
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 0
control.orientation.y = 1
control.orientation.z = 0
control.name = "move_z"
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
int_marker.controls.append(control)
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 0
control.orientation.y = 0
control.orientation.z = 1
control.name = "rotate_y"
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
int_marker.controls.append(control)
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 0
control.orientation.y = 0
control.orientation.z = 1
control.name = "move_y"
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
int_marker.controls.append(control)
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 0
control.orientation.y = 0
control.orientation.z = 1
control.name = "move_3d"
control.interaction_mode = InteractiveMarkerControl.MOVE_3D
int_marker.controls.append(control)
self.menu_handler.insert("Do stuff", callback=self.processFeedback)
## This makes the floating text appear
# make one control using default visuals
# control = InteractiveMarkerControl()
# control.interaction_mode = InteractiveMarkerControl.MENU
# control.description="Menu"
# control.name = "menu_only_control"
# int_marker.controls.append(copy.deepcopy(control))
# marker = self.makeArrow( int_marker )
# control.markers.append( marker )
# control.always_visible = False
# int_marker.controls.append(control)
self.server.insert(int_marker, self.processFeedback)
self.menu_handler.apply(self.server, int_marker.name)
class GripperMarkers:
_offset = 0.09
def __init__(self, side_prefix):
self.ik = IK(side_prefix)
self.side_prefix = side_prefix
self._im_server = InteractiveMarkerServer('ik_request_markers')
self._tf_listener = TransformListener()
self._menu_handler = MenuHandler()
self._menu_handler.insert('Move arm here',
callback=self.move_to_pose_cb)
self.is_control_visible = False
self.ee_pose = self.get_ee_pose()
self.update_viz()
self._menu_handler.apply(self._im_server, 'ik_target_marker')
self._im_server.applyChanges()
# Code for moving the robots joints
switch_srv_name = 'pr2_controller_manager/switch_controller'
rospy.loginfo('Waiting for switch controller service...')
rospy.wait_for_service(switch_srv_name)
self.switch_service_client = rospy.ServiceProxy(
switch_srv_name, SwitchController)
self.r_joint_names = [
'r_shoulder_pan_joint', 'r_shoulder_lift_joint',
'r_upper_arm_roll_joint', 'r_elbow_flex_joint',
'r_forearm_roll_joint', 'r_wrist_flex_joint', 'r_wrist_roll_joint'
]
self.l_joint_names = [
'l_shoulder_pan_joint', 'l_shoulder_lift_joint',
'l_upper_arm_roll_joint', 'l_elbow_flex_joint',
'l_forearm_roll_joint', 'l_wrist_flex_joint', 'l_wrist_roll_joint'
]
# Create a trajectory action client
r_traj_controller_name = '/r_arm_controller/joint_trajectory_action'
self.r_traj_action_client = SimpleActionClient(r_traj_controller_name,
JointTrajectoryAction)
rospy.loginfo(
'Waiting for a response from the trajectory action server for RIGHT arm...'
)
self.r_traj_action_client.wait_for_server()
l_traj_controller_name = '/l_arm_controller/joint_trajectory_action'
self.l_traj_action_client = SimpleActionClient(l_traj_controller_name,
JointTrajectoryAction)
rospy.loginfo(
'Waiting for a response from the trajectory action server for LEFT arm...'
)
self.l_traj_action_client.wait_for_server()
def get_ee_pose(self):
from_frame = 'base_link'
to_frame = self.side_prefix + '_wrist_roll_link'
try:
t = self._tf_listener.getLatestCommonTime(from_frame, to_frame)
(pos,
rot) = self._tf_listener.lookupTransform(from_frame, to_frame, t)
except:
rospy.logerr('Could not get end effector pose through TF.')
pos = [1.0, 0.0, 1.0]
rot = [0.0, 0.0, 0.0, 1.0]
return Pose(Point(pos[0], pos[1], pos[2]),
Quaternion(rot[0], rot[1], rot[2], rot[3]))
def move_to_pose_cb(self, dummy):
rospy.loginfo('You pressed the `Move arm here` button from the menu.')
joint_positions = self.ik.get_ik_for_ee(self.ee_pose)
print 'joint: ' + str(joint_positions)
if (joint_positions != None):
self.toggle_arm(self.side_prefix, 'Freeze', False)
#joint_positions = [-0.993560463247, -0.357041076593, 0.0534981848008, -1.24934444608, -3.10068096283, -1.7906747183, -28.4704855309]
print 'returned from IK: ' + str(joint_positions)
#print 'random position : ' + str([-0.993560463247, -0.357041076593, 0.0534981848008, -1.24934444608, -3.10068096283, -1.7906747183, -28.4704855309])
#joint_positions = [-1.03129153, -0.35312086, -0.08, -1.25402906, -2.98718287, -1.7816027, 3.03965124]
self.move_to_joints(self.side_prefix, list(joint_positions), 5.0)
print 'done'
def toggle_arm(self, side, toggle, button):
controller_name = side + '_arm_controller'
print 'toggle'
start_controllers = []
stop_controllers = []
if (toggle == 'Relax'):
stop_controllers.append(controller_name)
else:
start_controllers.append(controller_name)
self.set_arm_mode(start_controllers, stop_controllers)
def set_arm_mode(self, start_controllers, stop_controllers):
try:
self.switch_service_client(start_controllers, stop_controllers, 1)
except rospy.ServiceException:
rospy.logerr('Could not change arm mode.')
def move_to_joints(self, side_prefix, positions, time_to_joint):
'''Moves the arm to the desired joints'''
velocities = [0] * len(positions)
traj_goal = JointTrajectoryGoal()
traj_goal.trajectory.header.stamp = (rospy.Time.now() +
rospy.Duration(0.1))
traj_goal.trajectory.points.append(
JointTrajectoryPoint(
positions=positions,
velocities=velocities,
time_from_start=rospy.Duration(time_to_joint)))
if (side_prefix == 'r'):
traj_goal.trajectory.joint_names = self.r_joint_names
self.r_traj_action_client.send_goal(traj_goal)
else:
traj_goal.trajectory.joint_names = self.l_joint_names
self.l_traj_action_client.send_goal(traj_goal)
def marker_clicked_cb(self, feedback):
if feedback.event_type == InteractiveMarkerFeedback.POSE_UPDATE:
self.ee_pose = feedback.pose
self.update_viz()
self._menu_handler.reApply(self._im_server)
self._im_server.applyChanges()
elif feedback.event_type == InteractiveMarkerFeedback.BUTTON_CLICK:
rospy.loginfo('Changing visibility of the pose controls.')
if (self.is_control_visible):
self.is_control_visible = False
else:
self.is_control_visible = True
else:
rospy.loginfo('Unhandled event: ' + str(feedback.event_type))
def update_viz(self):
menu_control = InteractiveMarkerControl()
menu_control.interaction_mode = InteractiveMarkerControl.BUTTON
menu_control.always_visible = True
frame_id = 'base_link'
pose = self.ee_pose
menu_control = self._add_gripper_marker(menu_control)
text_pos = Point()
text_pos.x = pose.position.x
text_pos.y = pose.position.y
text_pos.z = pose.position.z + 0.1
text = 'x=' + str(pose.position.x) + ' y=' + str(
pose.position.y) + ' x=' + str(pose.position.z)
menu_control.markers.append(
Marker(type=Marker.TEXT_VIEW_FACING,
id=0,
scale=Vector3(0, 0, 0.03),
text=text,
color=ColorRGBA(0.0, 0.0, 0.0, 0.5),
header=Header(frame_id=frame_id),
pose=Pose(text_pos, Quaternion(0, 0, 0, 1))))
int_marker = InteractiveMarker()
int_marker.name = 'ik_target_marker'
int_marker.header.frame_id = frame_id
int_marker.pose = pose
int_marker.scale = 0.2
self._add_6dof_marker(int_marker)
int_marker.controls.append(menu_control)
self._im_server.insert(int_marker, self.marker_clicked_cb)
def _add_gripper_marker(self, control):
is_hand_open = False
if is_hand_open:
angle = 28 * numpy.pi / 180.0
else:
angle = 0
transform1 = tf.transformations.euler_matrix(0, 0, angle)
transform1[:3, 3] = [0.07691 - GripperMarkers._offset, 0.01, 0]
transform2 = tf.transformations.euler_matrix(0, 0, -angle)
transform2[:3, 3] = [0.09137, 0.00495, 0]
t_proximal = transform1
t_distal = tf.transformations.concatenate_matrices(
transform1, transform2)
mesh1 = self._make_mesh_marker()
mesh1.mesh_resource = (
'package://pr2_description/meshes/gripper_v0/gripper_palm.dae')
mesh1.pose.position.x = -GripperMarkers._offset
mesh1.pose.orientation.w = 1
mesh2 = self._make_mesh_marker()
mesh2.mesh_resource = (
'package://pr2_description/meshes/gripper_v0/l_finger.dae')
mesh2.pose = GripperMarkers.get_pose_from_transform(t_proximal)
mesh3 = self._make_mesh_marker()
mesh3.mesh_resource = (
'package://pr2_description/meshes/gripper_v0/l_finger_tip.dae')
mesh3.pose = GripperMarkers.get_pose_from_transform(t_distal)
quat = tf.transformations.quaternion_multiply(
tf.transformations.quaternion_from_euler(numpy.pi, 0, 0),
tf.transformations.quaternion_from_euler(0, 0, angle))
transform1 = tf.transformations.quaternion_matrix(quat)
transform1[:3, 3] = [0.07691 - GripperMarkers._offset, -0.01, 0]
transform2 = tf.transformations.euler_matrix(0, 0, -angle)
transform2[:3, 3] = [0.09137, 0.00495, 0]
t_proximal = transform1
t_distal = tf.transformations.concatenate_matrices(
transform1, transform2)
mesh4 = self._make_mesh_marker()
mesh4.mesh_resource = (
'package://pr2_description/meshes/gripper_v0/l_finger.dae')
mesh4.pose = GripperMarkers.get_pose_from_transform(t_proximal)
mesh5 = self._make_mesh_marker()
mesh5.mesh_resource = (
'package://pr2_description/meshes/gripper_v0/l_finger_tip.dae')
mesh5.pose = GripperMarkers.get_pose_from_transform(t_distal)
control.markers.append(mesh1)
control.markers.append(mesh2)
control.markers.append(mesh3)
control.markers.append(mesh4)
control.markers.append(mesh5)
return control
@staticmethod
def get_pose_from_transform(transform):
pos = transform[:3, 3].copy()
rot = tf.transformations.quaternion_from_matrix(transform)
return Pose(Point(pos[0], pos[1], pos[2]),
Quaternion(rot[0], rot[1], rot[2], rot[3]))
def _make_mesh_marker(self):
mesh = Marker()
mesh.mesh_use_embedded_materials = False
mesh.type = Marker.MESH_RESOURCE
mesh.scale.x = 1.0
mesh.scale.y = 1.0
mesh.scale.z = 1.0
mesh.color = ColorRGBA(0.0, 0.5, 1.0, 0.6)
ik_solution = self.ik.get_ik_for_ee(self.ee_pose)
if (ik_solution == None):
mesh.color = ColorRGBA(1.0, 0.0, 0.0, 0.6)
else:
mesh.color = ColorRGBA(0.0, 1.0, 0.0, 0.6)
return mesh
def _add_6dof_marker(self, int_marker):
is_fixed = True
control = self._make_6dof_control('rotate_x', Quaternion(1, 0, 0, 1),
False, is_fixed)
int_marker.controls.append(control)
control = self._make_6dof_control('move_x', Quaternion(1, 0, 0, 1),
True, is_fixed)
int_marker.controls.append(control)
control = self._make_6dof_control('rotate_z', Quaternion(0, 1, 0, 1),
False, is_fixed)
int_marker.controls.append(control)
control = self._make_6dof_control('move_z', Quaternion(0, 1, 0, 1),
True, is_fixed)
int_marker.controls.append(control)
control = self._make_6dof_control('rotate_y', Quaternion(0, 0, 1, 1),
False, is_fixed)
int_marker.controls.append(control)
control = self._make_6dof_control('move_y', Quaternion(0, 0, 1, 1),
True, is_fixed)
int_marker.controls.append(control)
def _make_6dof_control(self, name, orientation, is_move, is_fixed):
control = InteractiveMarkerControl()
control.name = name
control.orientation = orientation
control.always_visible = False
if (self.is_control_visible):
if is_move:
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
else:
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
else:
control.interaction_mode = InteractiveMarkerControl.NONE
if is_fixed:
control.orientation_mode = InteractiveMarkerControl.FIXED
return control
class InteractiveMarkerPoseStampedPublisher():
def __init__(self, from_frame, to_frame, position, orientation):
self.server = InteractiveMarkerServer("posestamped_im")
o = orientation
r, p, y = euler_from_quaternion([o.x, o.y, o.z, o.w])
rospy.loginfo("Publishing transform and PoseStamped from: " +
from_frame + " to " + to_frame +
" at " + str(position.x) + " " + str(position.y) + " " + str(position.z) +
" and orientation " + str(o.x) + " " + str(o.y) +
" " + str(o.z) + " " + str(o.w) + " or rpy " +
str(r) + " " + str(p) + " " + str(y))
self.menu_handler = MenuHandler()
self.from_frame = from_frame
self.to_frame = to_frame
# Transform broadcaster
self.tf_br = tf.TransformBroadcaster()
self.pub = rospy.Publisher('/posestamped', PoseStamped, queue_size=1)
rospy.loginfo("Publishing posestampeds at topic: " + str(self.pub.name))
pose = Pose()
pose.position = position
pose.orientation = orientation
self.last_pose = pose
self.print_commands(pose)
self.makeGraspIM(pose)
self.server.applyChanges()
def processFeedback(self, feedback):
"""
:type feedback: InteractiveMarkerFeedback
"""
s = "Feedback from marker '" + feedback.marker_name
s += "' / control '" + feedback.control_name + "'"
mp = ""
if feedback.mouse_point_valid:
mp = " at " + str(feedback.mouse_point.x)
mp += ", " + str(feedback.mouse_point.y)
mp += ", " + str(feedback.mouse_point.z)
mp += " in frame " + feedback.header.frame_id
if feedback.event_type == InteractiveMarkerFeedback.BUTTON_CLICK:
rospy.logdebug(s + ": button click" + mp + ".")
elif feedback.event_type == InteractiveMarkerFeedback.MENU_SELECT:
rospy.logdebug(s + ": menu item " + str(feedback.menu_entry_id) + " clicked" + mp + ".")
if feedback.menu_entry_id == 1:
rospy.logdebug("Start publishing transform...")
if self.tf_br is None:
self.tf_br = tf.TransformBroadcaster()
elif feedback.menu_entry_id == 2:
rospy.logdebug("Stop publishing transform...")
self.tf_br = None
# When clicking this event triggers!
elif feedback.event_type == InteractiveMarkerFeedback.POSE_UPDATE:
rospy.logdebug(s + ": pose changed")
elif feedback.event_type == InteractiveMarkerFeedback.MOUSE_DOWN:
rospy.logdebug(s + ": mouse down" + mp + ".")
elif feedback.event_type == InteractiveMarkerFeedback.MOUSE_UP:
rospy.logdebug(s + ": mouse up" + mp + ".")
# TODO: Print here the commands
# tf static transform
self.print_commands(feedback.pose)
self.last_pose = deepcopy(feedback.pose)
self.server.applyChanges()
def print_commands(self, pose, decimals=4):
p = pose.position
o = pose.orientation
print "\n---------------------------"
print "Static transform publisher command (with roll pitch yaw):"
common_part = "rosrun tf static_transform_publisher "
pos_part = str(round(p.x, decimals)) + " " + str(round(p.y, decimals)) + " "+ str(round(p.z, decimals))
r, p, y = euler_from_quaternion([o.x, o.y, o.z, o.w])
ori_part = str(round(r, decimals)) + " " + str(round(p, decimals)) + " " + str(round(y, decimals))
static_tf_cmd = common_part + pos_part + " " + ori_part + " " + self.from_frame + " " + self.to_frame + " 50"
print " " + static_tf_cmd
print
print "Static transform publisher command (with quaternion):"
ori_q = str(round(o.x, decimals)) + " " + str(round(o.y, decimals)) + " " + str(round(o.z, decimals)) + " " + str(round(o.w, decimals))
static_tf_cmd = common_part + pos_part + " " + ori_q + " " + self.from_frame + " " + self.to_frame + " 50"
print " " + static_tf_cmd
print
print "Roslaunch line of static transform publisher (rpy):"
node_name = "from_" + self.from_frame + "_to_" + self.to_frame + "_static_tf"
roslaunch_part = ' <node name="' + node_name + '" pkg="tf" type="static_transform_publisher" args="' +\
pos_part + " " + ori_part + " " + self.from_frame + " " + self.to_frame + " 50" + '" />'
print roslaunch_part
print
print "URDF format:" # <origin xyz="0.04149 -0.01221 0.001" rpy="0 0 0" />
print ' <origin xyz="' + pos_part + '" rpy="' + ori_part + '" />'
print "\n---------------------------"
def makeArrow(self, msg):
marker = Marker()
# An arrow that's squashed to easier view the orientation on roll
marker.type = Marker.ARROW
marker.scale.x = 0.08
marker.scale.y = 0.08
marker.scale.z = 0.08
marker.color.r = 0.3
marker.color.g = 0.3
marker.color.b = 0.5
marker.color.a = 1.0
return marker
def makeBoxControl(self, msg):
control = InteractiveMarkerControl()
control.always_visible = True
control.markers.append(self.makeArrow(msg))
msg.controls.append(control)
return control
def makeGraspIM(self, pose):
"""
:type pose: Pose
"""
int_marker = InteractiveMarker()
int_marker.header.frame_id = self.from_frame
int_marker.pose = pose
int_marker.scale = 0.3
int_marker.name = "6dof_eef"
int_marker.description = "transform from " + self.from_frame + " to " + self.to_frame
# insert a box, well, an arrow
self.makeBoxControl(int_marker)
int_marker.controls[0].interaction_mode = InteractiveMarkerControl.MOVE_ROTATE_3D
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 1
control.orientation.y = 0
control.orientation.z = 0
control.name = "rotate_x"
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
control.orientation_mode = InteractiveMarkerControl.FIXED
int_marker.controls.append(control)
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 1
control.orientation.y = 0
control.orientation.z = 0
control.name = "move_x"
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
control.orientation_mode = InteractiveMarkerControl.FIXED
int_marker.controls.append(control)
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 0
control.orientation.y = 1
control.orientation.z = 0
control.name = "rotate_z"
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
control.orientation_mode = InteractiveMarkerControl.FIXED
int_marker.controls.append(control)
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 0
control.orientation.y = 1
control.orientation.z = 0
control.name = "move_z"
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
control.orientation_mode = InteractiveMarkerControl.FIXED
int_marker.controls.append(control)
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 0
control.orientation.y = 0
control.orientation.z = 1
control.name = "rotate_y"
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
control.orientation_mode = InteractiveMarkerControl.FIXED
int_marker.controls.append(control)
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 0
control.orientation.y = 0
control.orientation.z = 1
control.name = "move_y"
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
control.orientation_mode = InteractiveMarkerControl.FIXED
int_marker.controls.append(control)
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 0
control.orientation.y = 0
control.orientation.z = 1
control.name = "move_3d"
control.interaction_mode = InteractiveMarkerControl.MOVE_3D
control.orientation_mode = InteractiveMarkerControl.FIXED
int_marker.controls.append(control)
self.menu_handler.insert("Publish transform",
callback=self.processFeedback)
self.menu_handler.insert("Stop publishing transform",
callback=self.processFeedback)
self.server.insert(int_marker, self.processFeedback)
self.menu_handler.apply(self.server, int_marker.name)
def run(self):
r = pi/2.0
p = pi
y = pi/2.0
o = quaternion_from_euler(r, p, y)
opt_frame = self.to_frame + "_rgb_optical_frame"
r = rospy.Rate(20)
while not rospy.is_shutdown():
if self.tf_br is not None:
pos = self.last_pose.position
ori = self.last_pose.orientation
self.tf_br.sendTransform(
(pos.x, pos.y, pos.z),
(ori.x, ori.y, ori.z, ori.w),
rospy.Time.now(),
self.to_frame + "_link",
self.from_frame
)
ps = PoseStamped()
ps.pose = self.last_pose
ps.header.frame_id = self.from_frame
ps.header.stamp = rospy.Time.now()
self.pub.publish(ps)
br = tf.TransformBroadcaster()
br.sendTransform((0, 0, 0),
o,
rospy.Time.now(),
opt_frame,
self.to_frame + "_link")
r.sleep()