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 __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)
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 __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 __init__(self, root_tips, suffix=u''):
"""
:param root_tips: list containing root->tip tuple for each interactive marker.
:type root_tips: list
:param suffix: the marker will be called 'eef_control{}'.format(suffix)
:type suffix: str
"""
self.enable_self_collision = rospy.get_param(u'~enable_self_collision',
True)
self.giskard = GiskardWrapper()
if len(root_tips) > 0:
self.roots, self.tips = zip(*root_tips)
else:
self.roots = []
self.tips = []
self.suffix = suffix
self.markers = {}
# marker server
self.server = InteractiveMarkerServer(u'eef_control{}'.format(
self.suffix))
self.menu_handler = MenuHandler()
for root, tip in zip(self.roots, self.tips):
int_marker = self.make_6dof_marker(
InteractiveMarkerControl.MOVE_ROTATE_3D, root, tip)
self.server.insert(
int_marker,
self.process_feedback(self.server, int_marker.name, root, tip,
self.giskard,
self.enable_self_collision))
self.menu_handler.apply(self.server, int_marker.name)
self.server.applyChanges()
def __init__(self, root_link, tip_links):
# tf
self.tfBuffer = Buffer(rospy.Duration(1))
self.tf_listener = TransformListener(self.tfBuffer)
# giskard goal client
# self.client = SimpleActionClient('move', ControllerListAction)
self.client = SimpleActionClient('qp_controller/command',
ControllerListAction)
self.client.wait_for_server()
# marker server
self.server = InteractiveMarkerServer("eef_control")
self.menu_handler = MenuHandler()
for tip_link in tip_links:
int_marker = self.make6DofMarker(
InteractiveMarkerControl.MOVE_ROTATE_3D, root_link, tip_link)
self.server.insert(
int_marker,
self.process_feedback(self.server, self.client, root_link,
tip_link))
self.menu_handler.apply(self.server, int_marker.name)
self.server.applyChanges()
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
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 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 __init__(self, server):
self.server = server
self.pose = Pose()
self.publish = True
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 __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
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 __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 __init__(self, server):
self.server = server
self.pose = Pose()
self.active = True
self.covariance = 0.1
self.int_marker = None
self.make_marker()
self.menu_handler = MenuHandler()
item = self.menu_handler.insert("active", callback=self.menu_callback)
self.menu_handler.setCheckState(item, MenuHandler.CHECKED)
high_conf = self.menu_handler.insert("high confidence", callback=self.conf_callback)
self.menu_handler.setCheckState(high_conf, MenuHandler.CHECKED)
self.menu_handler.apply(self.server, self.marker_name)
def _update_menu(self):
"""Recreates the menu when something has changed."""
self._menu_handler = MenuHandler()
# Insert sub entries.
self._sub_entries = []
self._menu_options = []
frame_entry = self._menu_handler.insert(MENU_OPTIONS['ref'])
refs = ActionStepMarker._ref_names
for ref in refs:
if ref != BASE_LINK and "Obj" not in str(ref):
subent = self._menu_handler.insert(ref + ' center',
parent=frame_entry,
callback=self.subent_cb)
self._sub_entries += [subent]
self._menu_options += [subent]
elif ref == BASE_LINK:
subent = self._menu_handler.insert(ref,
parent=frame_entry,
callback=self.subent_cb)
self._sub_entries += [subent]
self._menu_options += [subent]
select_one = self._menu_handler.insert('Select an object side',
parent=frame_entry,
callback=self.select_one_cb)
self._menu_options += [select_one]
# 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)
self._menu_handler.setCheckState(select_one, MenuHandler.UNCHECKED)
# Check if necessary. Set base_link to checked
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()
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)
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 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 __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 __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 _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 __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 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()
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
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
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 __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 start_once(self):
# giskard goal client
self.client = SimpleActionClient(u'qp_controller/command', MoveAction)
self.client.wait_for_server()
# marker server
self.server = InteractiveMarkerServer(u'eef_control{}'.format(
self.suffix))
self.menu_handler = MenuHandler()
all_goals = {}
for root, tip in zip(self.roots, self.tips):
int_marker = self.make_6dof_marker(
InteractiveMarkerControl.MOVE_ROTATE_3D, root, tip)
self.server.insert(
int_marker,
self.process_feedback(self.server, int_marker.name,
self.client, root, tip, all_goals))
self.menu_handler.apply(self.server, int_marker.name)
self.server.applyChanges()
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 __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 __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()
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
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()
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
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()
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
def __init__(self, server, cam_info):
self.cam_info = cam_info
self.marker_name = "goal"
self.interaction_mode = InteractiveMarkerControl.MOVE_ROTATE
self.absolute_publisher = rospy.Publisher("goal_absolute",
PoseWithCertaintyArray,
queue_size=1)
self.relative_publisher = rospy.Publisher("goal_relative",
PoseWithCertaintyArray,
queue_size=1)
self.relative_posts_publisher = rospy.Publisher("goal_posts_relative",
PoseWithCertaintyArray,
queue_size=1)
super(GoalMarker, self).__init__(server)
self.menu_handler = MenuHandler()
self.pose.position.x = 3.0
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()
rospy.Subscriber('ar_pose_marker', AlvarMarkers, self.marker_cb)
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 __init__(self, topic, initial_x, initial_y, initial_z, initial_roll,
initial_pitch, initial_yaw, frame_id):
self.server = InteractiveMarkerServer("posestamped_im")
self.menu_handler = MenuHandler()
self.pub = rospy.Publisher(topic, PoseStamped, queue_size=1)
rospy.loginfo("Publishing posestampeds at topic: " +
str(self.pub.resolved_name))
pose = Pose()
pose.position.x = initial_x
pose.position.y = initial_y
pose.position.z = initial_z
quat = quaternion_from_euler(initial_roll, initial_pitch, initial_yaw)
pose.orientation = Quaternion(*quat)
self.initial_pose = pose
self.frame_id = frame_id
# self.makeMenuMarker(pose)
self.makeGraspIM(pose)
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 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)
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 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)
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 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 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.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)
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 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