class FakeMarkerServer():
def __init__(self):
# create a simple TF listener
self.tf_listener = tf.TransformListener()
self.grasp_check = rospy.ServiceProxy(
'/interactive_world_hackathon/grasp_check', GraspCheck)
self.speak = rospy.ServiceProxy('/tts/speak', Speak)
self.play = rospy.ServiceProxy('/tts/play', Speak)
# create the nav client
self.nav = actionlib.SimpleActionClient('navigate_action',
navigateAction)
self.nav.wait_for_server()
# create the backup client
self.backup = actionlib.SimpleActionClient('backup_action',
BackupAction)
self.backup.wait_for_server()
# create the place action client
self.place = actionlib.SimpleActionClient(
'object_manipulator/object_manipulator_place', PlaceAction)
self.place.wait_for_server()
# Segmentation client
self.segclient = actionlib.SimpleActionClient(
'/object_detection_user_command', UserCommandAction)
self.segclient.wait_for_server()
self.recognition = None
# create the IMGUI action client
self.imgui = actionlib.SimpleActionClient('imgui_action', IMGUIAction)
self.imgui.wait_for_server()
# listen for graspable objects
rospy.Subscriber('/interactive_object_recognition_result',
GraspableObjectList, self.proc_grasp_list)
# create the save service
rospy.Service('~save_template', SaveTemplate, self.save)
self.load_server = actionlib.SimpleActionServer('load_template',
LoadAction,
execute_cb=self.load,
auto_start=False)
self.load_server.start()
# create the IM server
self.server = InteractiveMarkerServer('~fake_marker_server')
# create return list of templates
rospy.Service('~print_templates', PrintTemplates, self.get_templates)
# used to get model meshes
self.get_mesh = rospy.ServiceProxy(
'/objects_database_node/get_model_mesh', GetModelMesh)
# hack to get the grasp
rospy.Subscriber(
'/object_manipulator/object_manipulator_pickup/result',
PickupActionResult, self.store_grasp)
self.last_grasp = None
self.objects = []
self.objects.append(OBJECT1)
self.objects.append(OBJECT2)
self.objects.append(OBJECT3)
self.reset_objects()
# check for saved templates
try:
self.templates = pickle.load(open(SAVE_FILE, 'rb'))
rospy.loginfo('Loaded ' + str(len(self.templates.keys())) +
' template(s).')
except:
self.templates = dict()
rospy.loginfo('New template file started.')
self.play(
'/home/rctoris/wav/GLaDOS_generic_security_camera_destroyed-2.wav')
#Service: rosservice call /fake_object_markers/print_templates
#Returns a string of template names
#ex) list: ['test_template1','test_template2','test_template3']
def get_templates(self, req):
temp_list = []
if self.templates.keys() is None:
self.publish_feedback('No templates')
return
for obj in self.templates.keys():
temp_list.append(str(obj))
#print temp_list
return PrintTemplatesResponse(temp_list)
def store_grasp(self, msg):
self.last_grasp = msg.result.grasp
# Given a mesh_id creates a name with format 'object + mesh_id'
# ex.)Given '1234', creates 'object_1234' name
def create_name(self, mesh_id):
return 'object_' + str(mesh_id)
# Creates a mesh of the given object with the given pose to be visualized by template maker
def create_mesh(self, mesh_id, pose):
response = self.get_mesh(mesh_id)
mesh = response.mesh
# build the mesh marker
marker = Marker()
marker.color.r = 1.0
marker.color.g = 0.0
marker.color.b = 0.0
marker.color.a = 0.66
marker.frame_locked = False
marker.type = Marker.TRIANGLE_LIST
# add the mesh
for j in range(len(mesh.triangles)):
marker.points.append(
mesh.vertices[mesh.triangles[j].vertex_indices[0]])
marker.points.append(
mesh.vertices[mesh.triangles[j].vertex_indices[1]])
marker.points.append(
mesh.vertices[mesh.triangles[j].vertex_indices[2]])
# create the interactive marker
name = self.create_name(mesh_id)
self.server.insert(self.create_im(marker, pose, name),
self.process_feedback)
self.server.setCallback(name, self.release,
InteractiveMarkerFeedback.MOUSE_UP)
self.server.applyChanges()
# Creates an interactive marker
# - at the given location and pose
# - with a given name
# - for given marker object
def create_im(self, marker, pose, name):
# create the new interactive marker
int_marker = InteractiveMarker()
int_marker.pose = copy.deepcopy(pose)
int_marker.header.frame_id = 'base_link'
int_marker.name = name
# move freely on the X-Y plane
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 0
control.orientation.y = 1
control.orientation.z = 0
control.interaction_mode = InteractiveMarkerControl.MOVE_PLANE
control.markers.append(marker)
control.always_visible = True
int_marker.controls.append(control)
return int_marker
def process_feedback(self, feedback):
self.last_feedback = feedback
# Returns true if given (x,y) coordinates are within "Graspable/Placeable(?)" range
def check_pose(self, x, y):
return x >= OFFSET + DEPTH_START and x <= OFFSET + DEPTH_END and y >= WIDTH_START and y <= WIDTH_END
# Checks position of hallucinated interactive markers
# Changes color and sets position when user releases mouse click (MOUSE_UP) on object
def release(self, feedback):
im = self.server.get(feedback.marker_name)
# copy the mesh information
marker = copy.deepcopy(im.controls[0].markers[0])
# determine color based on pose
if self.check_pose(feedback.pose.position.x, feedback.pose.position.y):
marker.color.r = 0.0
marker.color.g = 1.0
marker.color.b = 0.0
marker.color.a = 0.66
else:
marker.color.r = 1.0
marker.color.g = 0.0
marker.color.b = 0.0
marker.color.a = 0.66
# create the new interactive marker
self.server.insert(
self.create_im(marker, feedback.pose, feedback.marker_name),
self.process_feedback)
self.server.setCallback(feedback.marker_name, self.release,
InteractiveMarkerFeedback.MOUSE_UP)
self.server.applyChanges()
# updates server
def update(self):
self.server.applyChanges()
# **Run by save_template service**
# Saves given template information to file location
def save(self, req):
# go through each object and check if they are in the valid range
to_save = []
for obj_id in self.objects:
im = self.server.get(self.create_name(obj_id))
pose = im.pose
if (self.check_pose(pose.position.x, pose.position.y)):
to_save.append(copy.deepcopy(im))
# check if we have anything to save
if len(to_save) > 0:
self.templates[req.name] = to_save
# PICKLE IT!
pickle.dump(self.templates, open(SAVE_FILE, 'wb'))
self.play('/home/rctoris/wav/t3_affirmative.wav')
self.reset_objects()
return SaveTemplateResponse(True)
else:
return SaveTemplateResponse(False)
# Publishes feedback of current tasks
def publish_feedback(self, msg):
rospy.loginfo(msg)
self.load_server.publish_feedback(LoadFeedback(msg))
# Publishes final result of action
def publish_result(self, msg):
rospy.loginfo(msg)
self.load_server.set_succeeded(LoadResult(msg))
# Returns how many objects were recognized
def proc_grasp_list(self, msg):
objects = []
# start by going through each
size = len(msg.graspable_objects)
for i in range(size):
obj = msg.graspable_objects[i]
# only take recognized objects
if len(obj.potential_models) is not 0:
objects.append(copy.deepcopy(obj))
rospy.loginfo('Found ' + str(len(objects)) + ' object(s).')
self.recognition = objects
# Drives to and aligns with counter
# Segments objects
def look_for_objects(self):
self.publish_feedback('Driving robot to counter')
# drive the robot
nav_goal = navigateGoal('Snack Nav', True)
self.nav.send_goal_and_wait(nav_goal)
res = self.nav.get_result()
if not res.success:
self.publish_feedback('Counter alignment failed.')
return False
self.publish_feedback('Aligned robot to counter')
self.publish_feedback('Looking for objects')
self.play('/home/rctoris/wav/GLaDOS_10_part1_entry-2.wav')
self.recognition = None
# Segment the table
self.segclient.send_goal(UserCommandGoal(request=1, interactive=False))
self.segclient.wait_for_result()
while self.recognition is None:
time.sleep(1)
# Recognize objects
self.recognition = None
self.segclient.send_goal(UserCommandGoal(request=2, interactive=False))
self.segclient.wait_for_result()
while self.recognition is None:
time.sleep(1)
return True
# **Run by load_template service**
# Identifies remaining objects needed in template
# Moves to and aligns with counter
# Scans and recognizes objects on counter that match template
# Picks up one object
# Backs up from counter
# Drives to table
# Places object in given template location
# Repeats
def load(self, goal):
name = goal.name
self.publish_feedback('Loading template ' + name)
# if requested template does not exist...
if name not in self.templates.keys():
self.publish_result(name + ' template does not exist')
return
template = copy.deepcopy(self.templates[name])
self.publish_feedback('Loaded template ' + name)
self.play('/home/rctoris/wav/help.wav')
# look for any objects we need
while len(template) is not 0:
pickup_arm = None
# if it does not see any objects/could not drive to counter
if not self.look_for_objects():
self.publish_result('Object looking failed.')
return
# for each object in template array...
for template_im in template:
# for each recognized object
for rec_obj in self.recognition:
if template_im.name == self.create_name(
rec_obj.potential_models[0].model_id):
# create the object info for it
obj_info = self.create_object_info(rec_obj)
# pick it up
pickup_arm = self.pickup(rec_obj)
# if neither arm can could pick up object...
if pickup_arm is None:
self.publish_result('Pickup failed.')
return
# make sure we have a grasp
self.publish_feedback('Waiting for grasp')
while self.last_grasp is None:
rospy.sleep(1)
# store the grasp
obj_info.grasp = self.last_grasp
self.last_grasp = None
self.publish_feedback('Grasp found')
# good job robot, place that object
to_place = Pose()
# location of object in template on table
to_place.position.z = TABLE_HEIGHT - PLACE_HEIGHT_OFFSET
to_place.position.x = template_im.pose.position.x
to_place.position.y = template_im.pose.position.y
placed = self.place_object(obj_info, pickup_arm,
to_place)
# if the object could not be placed
if not placed:
self.publish_result('Place failed.')
return
self.publish_feedback('Placed the object!')
if len(template) is not 1:
self.play('/home/rctoris/wav/ill-be-back.wav')
# removes object from list of objects to pick up from template
template.remove(template_im)
# if no objects are found...
if pickup_arm is None:
# No objects found :(
self.publish_result('No objects found that we need :(')
return
# We completed the task!
self.play('/home/rctoris/wav/down.wav')
self.publish_result('Great success!')
# resets collision map of world and rescan
def reset_collision_map(self):
self.publish_feedback('Reseting collision map')
goal = IMGUIGoal()
goal.command.command = 3
goal.options.reset_choice = 4
self.imgui.send_goal(goal)
self.imgui.wait_for_result()
self.publish_feedback('Collision map reset')
# reset hallucinated interactive marker objects' positions on visualized table
def reset_objects(self):
pose = Pose()
pose.position.z = TABLE_HEIGHT
pose.position.x = OFFSET * 3
pose.position.y = -0.25
for obj_id in self.objects:
self.create_mesh(obj_id, pose)
pose.position.y = pose.position.y + 0.25
# Picks up object that matches obj
def pickup(self, obj):
# start by picking up the object
options = IMGUIOptions()
options.collision_checked = True
options.grasp_selection = 1
options.adv_options.lift_steps = 10
options.adv_options.retreat_steps = 10
options.adv_options.reactive_force = False
options.adv_options.reactive_grasping = False
options.adv_options.reactive_place = False
options.adv_options.lift_direction_choice = 0
# check which arm is closer
if obj.potential_models[0].pose.pose.position.y > 0:
options.arm_selection = 1
else:
options.arm_selection = 0
goal = IMGUIGoal()
goal.options = options
goal.options.grasp_selection = 1
goal.options.selected_object = obj
goal.command.command = goal.command.PICKUP
# send it to IMGUI
self.publish_feedback('Attempting to pick up')
self.reset_collision_map()
self.imgui.send_goal(goal)
self.play('/home/rctoris/wav/humnbehv.wav')
self.imgui.wait_for_result()
# check the result
res = self.imgui.get_result()
if res.result.value is not 1:
# try the other arm
if options.arm_selection is 0:
options.arm_selection = 1
else:
options.arm_selection = 0
self.publish_feedback('Initial pickup failed, trying other arm')
self.reset_collision_map()
self.imgui.send_goal(goal)
self.imgui.wait_for_result()
# check the result
res = self.imgui.get_result()
if res.result.value is not 1:
return None
else:
# now check if feedback to see if we actually got it
if options.arm_selection is 0:
arm = 'right'
else:
arm = 'left'
self.publish_feedback('Checking if object was grasped')
resp = self.grasp_check(arm)
if resp.isGrasping is True:
self.publish_feedback('Object was grasped')
# attempt to back up
backup_goal = BackupGoal()
self.backup.send_goal_and_wait(backup_goal)
res = self.backup.get_result()
# if robot could not back up
if not res.success:
self.publish_feedback('Backup failed.')
return None
return options.arm_selection
else:
self.move_arm_to_side(options.arm_selection)
return None
# moves arm to sides
def move_arm_to_side(self, arm_selection):
goal = IMGUIGoal()
goal.command.command = 4
goal.options.arm_selection = arm_selection
goal.options.arm_action_choice = 0
goal.options.arm_planner_choice = 1
self.publish_feedback('Moving arm to the side using planner')
self.imgui.send_goal(goal)
self.imgui.wait_for_result()
# check the result
res = self.imgui.get_result()
if res.result.value is not 1:
# try open loop
self.publish_feedback('Planned arm move failed, trying open loop')
goal.options.arm_planner_choice = 0
self.imgui.send_goal(goal)
self.imgui.wait_for_result()
# check the result
res = self.imgui.get_result()
if res.result.value is not 1:
self.publish_feedback('Arm move failed.')
return False
else:
self.publish_feedback('Arm move was successful')
return True
# place object in given arm to given pose
def place_object(self, obj_info_orig, arm_selection, pose):
#drive to the table
self.publish_feedback('Driving robot to table')
nav_goal = navigateGoal('Dining Table Nav', True)
self.play('/home/rctoris/wav/run.wav')
self.nav.send_goal_and_wait(nav_goal)
res = self.nav.get_result()
if not res.success:
self.publish_feedback('Table alignment failed.')
return False
self.publish_feedback('Aligned robot to table')
self.reset_collision_map()
self.publish_feedback('Attempting to place the object')
# make a copy
obj_info = copy.deepcopy(obj_info_orig)
obj = obj_info.obj
goal = PlaceGoal()
# set the arm
if arm_selection is 0:
goal.arm_name = 'right_arm'
prefix = 'r'
else:
goal.arm_name = 'left_arm'
prefix = 'l'
# rotate and "gu-chunk"
orig_z = pose.position.z
pose.orientation.x = 0
pose.orientation.y = 0
goal.place_locations = []
#iterating through possible x-locations to place object
for x in range(0, 10):
pose.position.x = pose.position.x + ((x - 5) * 0.0025)
#iterating through possible y-locations to place object
for y in range(0, 10):
pose.position.y = pose.position.y + ((y - 5) * 0.0025)
# 'i' is for some rotations
for i in range(0, 10):
rads = (pi * (i / 10.0))
pose.orientation.z = sin(-rads / 2.0)
pose.orientation.w = cos(-rads / 2.0)
# 'j' is for the 'z' height
for j in range(0, 6):
pose.position.z = orig_z + (j * 0.0025)
pose_mat = pose_to_mat(pose)
to_base_link_mat = pose_mat * obj_info.obj_origin_to_bounding_box
grasp_mat = pose_to_mat(obj_info.grasp.grasp_pose.pose)
gripper_mat = to_base_link_mat * grasp_mat
gripper_pose = stamp_pose(mat_to_pose(gripper_mat),
'base_link')
goal.place_locations.append(gripper_pose)
# use the identity as the grasp
obj_info.grasp.grasp_pose.pose = Pose()
obj_info.grasp.grasp_pose.pose.orientation.w = 1
goal.grasp = obj_info.grasp
goal.desired_retreat_distance = 0.1
goal.min_retreat_distance = 0.05
# set the approach
goal.approach = GripperTranslation()
goal.approach.desired_distance = .1
goal.approach.min_distance = 0.05
goal.approach.direction = create_vector3_stamped([0., 0., -1.],
'base_link')
# set the collision info
goal.collision_object_name = obj.collision_name
goal.collision_support_surface_name = 'table'
goal.place_padding = 0.0
goal.additional_link_padding = self.create_gripper_link_padding(prefix)
goal.collision_support_surface_name = 'all'
goal.allow_gripper_support_collision = True
goal.use_reactive_place = False
# send the goal
self.place.send_goal(goal)
# wait for result
finished_within_time = self.place.wait_for_result(rospy.Duration(240))
if not finished_within_time:
self.place.cancel_goal()
return False
# check the result
res = self.place.get_result()
if res.manipulation_result.value == -6 or res.manipulation_result.value == 1:
# attempt to back up
backup_goal = BackupGoal()
self.backup.send_goal_and_wait(backup_goal)
backup_res = self.backup.get_result()
# if robot could not back up
if not backup_res.success:
self.publish_feedback('Backup failed.')
return False
self.move_arm_to_side(arm_selection)
return True
else:
return False
def create_gripper_link_padding(self, prefix):
link_name_list = [
'_gripper_palm_link', '_gripper_r_finger_tip_link',
'_gripper_l_finger_tip_link', '_gripper_l_finger_link',
'_gripper_r_finger_link', '_wrist_roll_link'
]
pad = 0.
arm_link_names = [prefix + link_name for link_name in link_name_list]
link_padding_list = [
LinkPadding(link_name, pad) for link_name in arm_link_names
]
return link_padding_list
def create_object_info(self, obj):
# get the pose
pose_stamped = obj.potential_models[0].pose
# change the frame
obj_frame_pose_stamped = change_pose_stamped_frame(
self.tf_listener, pose_stamped, obj.reference_frame_id)
return ObjectInfo(obj, obj_frame_pose_stamped, self.tf_listener)
class AnnotatorServer(object):
def __init__(self):
self._annotator = Annotator()
self._pose_names_pub = rospy.Publisher("/map_annotator/pose_names",
PoseNames, queue_size=10, latch=True)
# self._map_poses_pub = rospy.Publisher("/map_annotator/map_poses",
# InteractiveMarker, queue_size=10, latch=True)
self._int_marker_server = InteractiveMarkerServer("/map_annotator/map_poses")
self.INITIAL_POSE = Pose()
self.INITIAL_POSE.orientation.w = 1
print("Initializing saved markers: " +
str(self._annotator.get_position_names()))
for name, pose in self._annotator.get_position_items():
self.__create_int_marker__(name, pose)
self.__pub_pose_info__()
print("Initialization finished...")
def __pub_pose_info__(self):
pose_names = PoseNames()
pose_names.names = self._annotator.get_position_names()
pose_names.names.sort()
self._pose_names_pub.publish(pose_names)
def __update_marker_pose__(self, input):
if (input.event_type == InteractiveMarkerFeedback.MOUSE_UP):
name = input.marker_name
new_pose = self._int_marker_server.get(name).pose
# Overwrite the previous pose with the new pose
self._annotator.save_position(name, new_pose)
self._int_marker_server.setPose(name, new_pose)
self._int_marker_server.applyChanges()
print("updated pose for: " + name)
def __create_int_marker__(self, name, pose):
print("creating int marker: " + name)
int_marker = InteractiveMarker()
int_marker.header.frame_id = "map"
int_marker.name = name
int_marker.description = name
int_marker.pose = pose
# Move it 0.25 meters up to make it easier to click
int_marker.pose.position.z = 0.25
text_marker = Marker()
text_marker.text = name
text_marker.type = Marker.TEXT_VIEW_FACING
text_marker.pose.position.z = 2
text_marker.scale.x = 0.4
text_marker.scale.y = 0.4
text_marker.scale.z = 0.4
text_marker.color.r = 0.0
text_marker.color.g = 0.5
text_marker.color.b = 0.5
text_marker.color.a = 1.0
text_control = InteractiveMarkerControl()
text_control.name = "text_control"
text_control.markers.append(text_marker)
text_control.always_visible = True
text_control.interaction_mode = InteractiveMarkerControl.NONE
int_marker.controls.append(text_control)
rotation_ring_control = InteractiveMarkerControl()
rotation_ring_control.name = "position_control"
rotation_ring_control.always_visible = True
rotation_ring_control.orientation.x = 0
rotation_ring_control.orientation.w = 1
rotation_ring_control.orientation.y = 1
rotation_ring_control.orientation.z = 0
rotation_ring_control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
int_marker.controls.append(rotation_ring_control)
arrow_marker = Marker()
arrow_marker.type = Marker.ARROW
arrow_marker.pose.orientation.w = 1
arrow_marker.pose.position.z = 0.15
arrow_marker.pose.position.x = -0.5
arrow_marker.scale.x = 1
arrow_marker.scale.y = 0.25
arrow_marker.scale.z = 0.25
arrow_marker.color.r = 0.0
arrow_marker.color.g = 0.5
arrow_marker.color.b = 0.5
arrow_marker.color.a = 1.0
position_control = InteractiveMarkerControl()
position_control.name = "rotation_control"
position_control.always_visible = True
position_control.markers.append(arrow_marker)
position_control.orientation.w = 1
position_control.orientation.x = 0
position_control.orientation.y = 1
position_control.orientation.z = 0
position_control.interaction_mode = InteractiveMarkerControl.MOVE_PLANE
int_marker.controls.append(position_control)
self._int_marker_server.insert(int_marker, self.__update_marker_pose__)
self._int_marker_server.applyChanges()
def create(self, name, pose=None):
print("creating new pose: " + name)
if pose is None:
pose = self.INITIAL_POSE
self._annotator.save_position(name, pose)
self.__create_int_marker__(name, pose)
self.__pub_pose_info__()
def delete(self, name):
if self._annotator.exists(name):
print("deleting pose: " + name)
self._annotator.delete_position(name)
self._int_marker_server.erase(name)
self._int_marker_server.applyChanges()
self.__pub_pose_info__()
def handle_callback(self, user_action_msg):
cmd = user_action_msg.command
name = user_action_msg.name
if cmd == UserAction.CREATE:
self.create(name)
elif cmd == UserAction.DELETE:
self.delete(name)
elif cmd == UserAction.GOTO:
print("going to pose: " + name)
self._annotator.goto_position(name)
class FakeMarkerServer():
def __init__(self):
# create a simple TF listener
self.tf_listener = tf.TransformListener()
self.grasp_check = rospy.ServiceProxy('/interactive_world_hackathon/grasp_check', GraspCheck)
self.speak = rospy.ServiceProxy('/tts/speak', Speak)
self.play = rospy.ServiceProxy('/tts/play', Speak)
# create the nav client
self.nav = actionlib.SimpleActionClient('navigate_action', navigateAction)
self.nav.wait_for_server()
# create the backup client
self.backup = actionlib.SimpleActionClient('backup_action', BackupAction)
self.backup.wait_for_server()
# create the place action client
self.place = actionlib.SimpleActionClient('object_manipulator/object_manipulator_place', PlaceAction)
self.place.wait_for_server()
# Segmentation client
self.segclient = actionlib.SimpleActionClient('/object_detection_user_command', UserCommandAction)
self.segclient.wait_for_server()
self.recognition=None
# create the IMGUI action client
self.imgui = actionlib.SimpleActionClient('imgui_action', IMGUIAction)
self.imgui.wait_for_server()
# listen for graspable objects
rospy.Subscriber('/interactive_object_recognition_result', GraspableObjectList, self.proc_grasp_list)
# create the save service
rospy.Service('~save_template', SaveTemplate, self.save)
self.load_server = actionlib.SimpleActionServer('load_template', LoadAction, execute_cb=self.load, auto_start=False)
self.load_server.start()
# create the IM server
self.server = InteractiveMarkerServer('~fake_marker_server')
# create return list of templates
rospy.Service('~print_templates', PrintTemplates, self.get_templates)
# used to get model meshes
self.get_mesh = rospy.ServiceProxy('/objects_database_node/get_model_mesh', GetModelMesh)
# hack to get the grasp
rospy.Subscriber('/object_manipulator/object_manipulator_pickup/result', PickupActionResult, self.store_grasp)
self.last_grasp = None
self.objects = []
self.objects.append(OBJECT1)
self.objects.append(OBJECT2)
self.objects.append(OBJECT3)
self.reset_objects()
# check for saved templates
try:
self.templates = pickle.load(open(SAVE_FILE, 'rb'))
rospy.loginfo('Loaded ' + str(len(self.templates.keys())) + ' template(s).')
except:
self.templates = dict()
rospy.loginfo('New template file started.')
self.play('/home/rctoris/wav/GLaDOS_generic_security_camera_destroyed-2.wav')
#Service: rosservice call /fake_object_markers/print_templates
#Returns a string of template names
#ex) list: ['test_template1','test_template2','test_template3']
def get_templates(self, req):
temp_list = []
if self.templates.keys() is None:
self.publish_feedback('No templates')
return
for obj in self.templates.keys():
temp_list.append(str(obj))
#print temp_list
return PrintTemplatesResponse(temp_list)
def store_grasp(self, msg):
self.last_grasp = msg.result.grasp
# Given a mesh_id creates a name with format 'object + mesh_id'
# ex.)Given '1234', creates 'object_1234' name
def create_name(self, mesh_id):
return 'object_' + str(mesh_id)
# Creates a mesh of the given object with the given pose to be visualized by template maker
def create_mesh(self, mesh_id, pose):
response = self.get_mesh(mesh_id)
mesh = response.mesh
# build the mesh marker
marker = Marker()
marker.color.r = 1.0
marker.color.g = 0.0
marker.color.b = 0.0
marker.color.a = 0.66
marker.frame_locked = False
marker.type = Marker.TRIANGLE_LIST
# add the mesh
for j in range(len(mesh.triangles)):
marker.points.append(mesh.vertices[mesh.triangles[j].vertex_indices[0]])
marker.points.append(mesh.vertices[mesh.triangles[j].vertex_indices[1]])
marker.points.append(mesh.vertices[mesh.triangles[j].vertex_indices[2]])
# create the interactive marker
name = self.create_name(mesh_id)
self.server.insert(self.create_im(marker, pose, name), self.process_feedback)
self.server.setCallback(name, self.release, InteractiveMarkerFeedback.MOUSE_UP)
self.server.applyChanges()
# Creates an interactive marker
# - at the given location and pose
# - with a given name
# - for given marker object
def create_im(self, marker, pose, name):
# create the new interactive marker
int_marker = InteractiveMarker()
int_marker.pose = copy.deepcopy(pose)
int_marker.header.frame_id = 'base_link'
int_marker.name = name
# move freely on the X-Y plane
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 0
control.orientation.y = 1
control.orientation.z = 0
control.interaction_mode = InteractiveMarkerControl.MOVE_PLANE
control.markers.append(marker)
control.always_visible = True
int_marker.controls.append(control)
return int_marker
def process_feedback(self, feedback):
self.last_feedback = feedback
# Returns true if given (x,y) coordinates are within "Graspable/Placeable(?)" range
def check_pose(self, x, y):
return x >= OFFSET + DEPTH_START and x <= OFFSET + DEPTH_END and y >= WIDTH_START and y <= WIDTH_END
# Checks position of hallucinated interactive markers
# Changes color and sets position when user releases mouse click (MOUSE_UP) on object
def release(self, feedback):
im = self.server.get(feedback.marker_name)
# copy the mesh information
marker = copy.deepcopy(im.controls[0].markers[0])
# determine color based on pose
if self.check_pose(feedback.pose.position.x, feedback.pose.position.y):
marker.color.r = 0.0
marker.color.g = 1.0
marker.color.b = 0.0
marker.color.a = 0.66
else:
marker.color.r = 1.0
marker.color.g = 0.0
marker.color.b = 0.0
marker.color.a = 0.66
# create the new interactive marker
self.server.insert(self.create_im(marker, feedback.pose, feedback.marker_name), self.process_feedback)
self.server.setCallback(feedback.marker_name, self.release, InteractiveMarkerFeedback.MOUSE_UP)
self.server.applyChanges()
# updates server
def update(self):
self.server.applyChanges()
# **Run by save_template service**
# Saves given template information to file location
def save(self, req):
# go through each object and check if they are in the valid range
to_save = []
for obj_id in self.objects:
im = self.server.get(self.create_name(obj_id))
pose = im.pose
if (self.check_pose(pose.position.x, pose.position.y)):
to_save.append(copy.deepcopy(im))
# check if we have anything to save
if len(to_save) > 0:
self.templates[req.name] = to_save
# PICKLE IT!
pickle.dump(self.templates, open(SAVE_FILE, 'wb'))
self.play('/home/rctoris/wav/t3_affirmative.wav')
self.reset_objects()
return SaveTemplateResponse(True)
else:
return SaveTemplateResponse(False)
# Publishes feedback of current tasks
def publish_feedback(self, msg):
rospy.loginfo(msg)
self.load_server.publish_feedback(LoadFeedback(msg))
# Publishes final result of action
def publish_result(self, msg):
rospy.loginfo(msg)
self.load_server.set_succeeded(LoadResult(msg))
# Returns how many objects were recognized
def proc_grasp_list(self, msg):
objects = []
# start by going through each
size = len(msg.graspable_objects)
for i in range(size):
obj = msg.graspable_objects[i]
# only take recognized objects
if len(obj.potential_models) is not 0:
objects.append(copy.deepcopy(obj))
rospy.loginfo('Found ' + str(len(objects)) + ' object(s).')
self.recognition = objects
# Drives to and aligns with counter
# Segments objects
def look_for_objects(self):
self.publish_feedback('Driving robot to counter')
# drive the robot
nav_goal = navigateGoal('Snack Nav', True)
self.nav.send_goal_and_wait(nav_goal)
res = self.nav.get_result()
if not res.success:
self.publish_feedback('Counter alignment failed.')
return False
self.publish_feedback('Aligned robot to counter')
self.publish_feedback('Looking for objects')
self.play('/home/rctoris/wav/GLaDOS_10_part1_entry-2.wav')
self.recognition = None
# Segment the table
self.segclient.send_goal(UserCommandGoal(request=1,interactive=False))
self.segclient.wait_for_result()
while self.recognition is None:
time.sleep(1)
# Recognize objects
self.recognition = None
self.segclient.send_goal(UserCommandGoal(request=2,interactive=False))
self.segclient.wait_for_result()
while self.recognition is None:
time.sleep(1)
return True
# **Run by load_template service**
# Identifies remaining objects needed in template
# Moves to and aligns with counter
# Scans and recognizes objects on counter that match template
# Picks up one object
# Backs up from counter
# Drives to table
# Places object in given template location
# Repeats
def load(self, goal):
name = goal.name
self.publish_feedback('Loading template ' + name)
# if requested template does not exist...
if name not in self.templates.keys():
self.publish_result(name + ' template does not exist')
return
template = copy.deepcopy(self.templates[name])
self.publish_feedback('Loaded template ' + name)
self.play('/home/rctoris/wav/help.wav')
# look for any objects we need
while len(template) is not 0:
pickup_arm = None
# if it does not see any objects/could not drive to counter
if not self.look_for_objects():
self.publish_result('Object looking failed.')
return
# for each object in template array...
for template_im in template:
# for each recognized object
for rec_obj in self.recognition:
if template_im.name == self.create_name(rec_obj.potential_models[0].model_id):
# create the object info for it
obj_info = self.create_object_info(rec_obj)
# pick it up
pickup_arm = self.pickup(rec_obj)
# if neither arm can could pick up object...
if pickup_arm is None:
self.publish_result('Pickup failed.')
return
# make sure we have a grasp
self.publish_feedback('Waiting for grasp')
while self.last_grasp is None:
rospy.sleep(1)
# store the grasp
obj_info.grasp = self.last_grasp
self.last_grasp = None
self.publish_feedback('Grasp found')
# good job robot, place that object
to_place = Pose()
# location of object in template on table
to_place.position.z = TABLE_HEIGHT - PLACE_HEIGHT_OFFSET
to_place.position.x = template_im.pose.position.x
to_place.position.y = template_im.pose.position.y
placed = self.place_object(obj_info, pickup_arm, to_place)
# if the object could not be placed
if not placed:
self.publish_result('Place failed.')
return
self.publish_feedback('Placed the object!')
if len(template) is not 1:
self.play('/home/rctoris/wav/ill-be-back.wav')
# removes object from list of objects to pick up from template
template.remove(template_im)
# if no objects are found...
if pickup_arm is None:
# No objects found :(
self.publish_result('No objects found that we need :(')
return
# We completed the task!
self.play('/home/rctoris/wav/down.wav')
self.publish_result('Great success!')
# resets collision map of world and rescan
def reset_collision_map(self):
self.publish_feedback('Reseting collision map')
goal = IMGUIGoal()
goal.command.command = 3
goal.options.reset_choice = 4
self.imgui.send_goal(goal)
self.imgui.wait_for_result()
self.publish_feedback('Collision map reset')
# reset hallucinated interactive marker objects' positions on visualized table
def reset_objects(self):
pose = Pose()
pose.position.z = TABLE_HEIGHT
pose.position.x = OFFSET * 3
pose.position.y = -0.25
for obj_id in self.objects:
self.create_mesh(obj_id, pose)
pose.position.y = pose.position.y + 0.25
# Picks up object that matches obj
def pickup(self, obj):
# start by picking up the object
options = IMGUIOptions()
options.collision_checked = True
options.grasp_selection = 1
options.adv_options.lift_steps = 10
options.adv_options.retreat_steps = 10
options.adv_options.reactive_force = False
options.adv_options.reactive_grasping = False
options.adv_options.reactive_place = False
options.adv_options.lift_direction_choice = 0
# check which arm is closer
if obj.potential_models[0].pose.pose.position.y > 0:
options.arm_selection = 1
else:
options.arm_selection = 0
goal = IMGUIGoal()
goal.options = options
goal.options.grasp_selection = 1
goal.options.selected_object = obj
goal.command.command = goal.command.PICKUP
# send it to IMGUI
self.publish_feedback('Attempting to pick up')
self.reset_collision_map()
self.imgui.send_goal(goal)
self.play('/home/rctoris/wav/humnbehv.wav')
self.imgui.wait_for_result()
# check the result
res = self.imgui.get_result()
if res.result.value is not 1:
# try the other arm
if options.arm_selection is 0:
options.arm_selection = 1
else:
options.arm_selection = 0
self.publish_feedback('Initial pickup failed, trying other arm')
self.reset_collision_map()
self.imgui.send_goal(goal)
self.imgui.wait_for_result()
# check the result
res = self.imgui.get_result()
if res.result.value is not 1:
return None
else:
# now check if feedback to see if we actually got it
if options.arm_selection is 0:
arm = 'right'
else:
arm = 'left'
self.publish_feedback('Checking if object was grasped')
resp = self.grasp_check(arm)
if resp.isGrasping is True:
self.publish_feedback('Object was grasped')
# attempt to back up
backup_goal = BackupGoal()
self.backup.send_goal_and_wait(backup_goal)
res = self.backup.get_result()
# if robot could not back up
if not res.success:
self.publish_feedback('Backup failed.')
return None
return options.arm_selection
else:
self.move_arm_to_side(options.arm_selection)
return None
# moves arm to sides
def move_arm_to_side(self, arm_selection):
goal = IMGUIGoal()
goal.command.command = 4
goal.options.arm_selection = arm_selection
goal.options.arm_action_choice = 0
goal.options.arm_planner_choice = 1
self.publish_feedback('Moving arm to the side using planner')
self.imgui.send_goal(goal)
self.imgui.wait_for_result()
# check the result
res = self.imgui.get_result()
if res.result.value is not 1:
# try open loop
self.publish_feedback('Planned arm move failed, trying open loop')
goal.options.arm_planner_choice = 0
self.imgui.send_goal(goal)
self.imgui.wait_for_result()
# check the result
res = self.imgui.get_result()
if res.result.value is not 1:
self.publish_feedback('Arm move failed.')
return False
else:
self.publish_feedback('Arm move was successful')
return True
# place object in given arm to given pose
def place_object(self, obj_info_orig, arm_selection, pose):
#drive to the table
self.publish_feedback('Driving robot to table')
nav_goal = navigateGoal('Dining Table Nav', True)
self.play('/home/rctoris/wav/run.wav')
self.nav.send_goal_and_wait(nav_goal)
res = self.nav.get_result()
if not res.success:
self.publish_feedback('Table alignment failed.')
return False
self.publish_feedback('Aligned robot to table')
self.reset_collision_map()
self.publish_feedback('Attempting to place the object')
# make a copy
obj_info = copy.deepcopy(obj_info_orig)
obj = obj_info.obj
goal = PlaceGoal()
# set the arm
if arm_selection is 0:
goal.arm_name = 'right_arm'
prefix = 'r'
else:
goal.arm_name = 'left_arm'
prefix = 'l'
# rotate and "gu-chunk"
orig_z = pose.position.z
pose.orientation.x = 0
pose.orientation.y = 0
goal.place_locations = []
#iterating through possible x-locations to place object
for x in range(0, 10):
pose.position.x = pose.position.x + ((x - 5) * 0.0025)
#iterating through possible y-locations to place object
for y in range(0, 10):
pose.position.y = pose.position.y + ((y - 5) * 0.0025)
# 'i' is for some rotations
for i in range(0, 10):
rads = (pi * (i/10.0))
pose.orientation.z = sin(-rads/2.0)
pose.orientation.w = cos(-rads/2.0);
# 'j' is for the 'z' height
for j in range (0, 6):
pose.position.z = orig_z + (j * 0.0025)
pose_mat = pose_to_mat(pose)
to_base_link_mat = pose_mat * obj_info.obj_origin_to_bounding_box
grasp_mat = pose_to_mat(obj_info.grasp.grasp_pose.pose)
gripper_mat = to_base_link_mat * grasp_mat
gripper_pose = stamp_pose(mat_to_pose(gripper_mat), 'base_link')
goal.place_locations.append(gripper_pose)
# use the identity as the grasp
obj_info.grasp.grasp_pose.pose = Pose()
obj_info.grasp.grasp_pose.pose.orientation.w = 1
goal.grasp = obj_info.grasp
goal.desired_retreat_distance = 0.1
goal.min_retreat_distance = 0.05
# set the approach
goal.approach = GripperTranslation()
goal.approach.desired_distance = .1
goal.approach.min_distance = 0.05
goal.approach.direction = create_vector3_stamped([0. , 0. , -1.], 'base_link')
# set the collision info
goal.collision_object_name = obj.collision_name
goal.collision_support_surface_name = 'table'
goal.place_padding = 0.0
goal.additional_link_padding = self.create_gripper_link_padding(prefix)
goal.collision_support_surface_name = 'all'
goal.allow_gripper_support_collision = True
goal.use_reactive_place = False
# send the goal
self.place.send_goal(goal)
# wait for result
finished_within_time = self.place.wait_for_result(rospy.Duration(240))
if not finished_within_time:
self.place.cancel_goal()
return False
# check the result
res = self.place.get_result()
if res.manipulation_result.value == -6 or res.manipulation_result.value == 1:
# attempt to back up
backup_goal = BackupGoal()
self.backup.send_goal_and_wait(backup_goal)
backup_res = self.backup.get_result()
# if robot could not back up
if not backup_res.success:
self.publish_feedback('Backup failed.')
return False
self.move_arm_to_side(arm_selection)
return True
else:
return False
def create_gripper_link_padding(self, prefix):
link_name_list = ['_gripper_palm_link', '_gripper_r_finger_tip_link', '_gripper_l_finger_tip_link',
'_gripper_l_finger_link', '_gripper_r_finger_link', '_wrist_roll_link']
pad = 0.
arm_link_names = [prefix + link_name for link_name in link_name_list]
link_padding_list = [LinkPadding(link_name, pad) for link_name in arm_link_names]
return link_padding_list
def create_object_info(self, obj):
# get the pose
pose_stamped = obj.potential_models[0].pose
# change the frame
obj_frame_pose_stamped = change_pose_stamped_frame(self.tf_listener, pose_stamped, obj.reference_frame_id)
return ObjectInfo(obj, obj_frame_pose_stamped, self.tf_listener)
class PR2TrajectoryMarkers(object):
"""
A class to create and store a trajectory for one PR2 arm. The created
trajectory can be published as a PoseArray message.
This class published on the following topics:
~trajectory_markers are the main interactive markers.
~trajectory_poses a markerarray to display the trajectory.
~trajectory_poses a posesarray with the resulting pose
The class subscribes to the topic ~overwrite_trajectory_
to change the stored trajectory. This is useful to resume working on a
trajectory after re-starting the node. The message type is PoseArray.
A std_srvs/Empty service named ~execute_trajectory is provided to
externally trigger the execution of the trajectory.
Constructor:
TrajectoryMarkers(whicharm = "left")
or
TrajectoryMarkers(whicharm = "right")
"""
def __init__(self, whicharm):
self.whicharm = whicharm
self.robot_state = pr2_control_utilities.RobotState()
self.joint_controller = pr2_control_utilities.PR2JointMover(self.robot_state)
self.planner = pr2_control_utilities.PR2MoveArm(self.joint_controller)
self.server = InteractiveMarkerServer("~trajectory_markers")
self.tf_listener = self.planner.tf_listener
self.visualizer_pub = rospy.Publisher("~trajectory_markers_path",
MarkerArray)
self.trajectory_pub = rospy.Publisher("~trajectory_poses",
PoseArray)
self.gripper_pose_pub = rospy.Publisher("~gripper_pose",
PoseStamped)
rospy.Subscriber("~overwrite_trajectory",
PoseArray,
self.overwrite_trajectory)
rospy.Service("~execute_trajectory", Empty,
self.execute_trajectory_srv)
# create an interactive marker for our server
int_marker = InteractiveMarker()
int_marker.header.frame_id = "/base_link"
int_marker.pose.position.x = 0.5
int_marker.pose.position.z = 1.0
int_marker.name = "move_" + whicharm + "_arm"
int_marker.description = "Move the " + whicharm + " arm"
int_marker.scale = 0.2
self.server.insert(int_marker, self.main_callback)
# create the main marker shape
#color = (1,0,0,1)
color = None
self.gripper_marker = utils.makeGripperMarker(color=color)
int_marker.controls.append(self.gripper_marker);
#add the controls
utils.make_6DOF_marker(int_marker)
self.int_marker = int_marker
self.create_menu()
self.server.applyChanges()
self.trajectory = PoseArray()
self.trajectory.header.frame_id = "/base_link"
self.last_gripper_pose = None
if whicharm == "right":
self.ik_utils = self.planner.right_ik
else:
self.ik_utils = self.planner.left_ik
self.planning_waiting_time = 10.0
rospy.loginfo("PR2TrajectoryMarkers (%s) is ready", whicharm)
def create_menu(self):
"""
Create and populates all the menu entries
"""
menu_handler = MenuHandler()
menu_handler.insert("Point the head",
callback = self.move_head)
menu_handler.insert("Add position to trajectory",
callback = self.add_point)
menu_handler.insert("Place marker over gripper",
callback = self.place_gripper)
menu_handler.insert("Execute trjectory",
callback = self.execute_trajectory)
menu_handler.insert("Clear trajectory",
callback = self.clear_trajectory)
menu_handler.insert("Publish trajectory",
callback = self.publish_trajectory)
menu_handler.insert("Move the arm (planning)",
callback = self.plan_arm)
menu_handler.insert("Move the arm (collision-free)",
callback = self.collision_free_arm)
menu_handler.insert("Move the arm to trajectory start (collision-free)",
callback = self.arm_trajectory_start)
menu_handler.insert("Update planning scene",
callback = self.update_planning)
menu_handler.apply(self.server, self.int_marker.name)
def main_callback(self, feedback):
"""
If the mouse button is released change the gripper color according to
the IK result. Quite awkward, trying to get a nicer way to do it.
"""
#publish the gripper pose
gripper_pos = PoseStamped()
gripper_pos.header.frame_id = feedback.header.frame_id
gripper_pos.pose = feedback.pose
self.gripper_pose_pub.publish(gripper_pos)
if feedback.event_type == InteractiveMarkerFeedback.POSE_UPDATE:
self.last_gripper_pose = feedback.pose
if (self.last_gripper_pose and
feedback.event_type == InteractiveMarkerFeedback.MOUSE_UP):
self.last_gripper_pose = None
pos = PoseStamped()
pos.header.frame_id = feedback.header.frame_id
pos.pose = feedback.pose
if self.whicharm == "right":
ik = self.planner.check_ik_right_arm
else:
ik = self.planner.check_ik_left_arm
if ik(pos):
color = None
else:
color = (1,0,0,1)
int_marker = self.server.get(self.int_marker.name)
int_marker.controls.remove(self.gripper_marker)
self.gripper_marker = utils.makeGripperMarker(color=color)
int_marker.controls.append(self.gripper_marker)
#rospy.loginfo("Control: %s", int_marker.controls)
self.server.insert(int_marker, self.main_callback)
self.server.setPose(int_marker.name, self.last_gripper_pose)
self.server.applyChanges()
def overwrite_trajectory(self, msg):
self.trajectory = msg
def add_point(self, feedback):
"""
Add a point to self.trajectory if it is allowed by IK.
"""
pos = PoseStamped()
pos.header.frame_id = feedback.header.frame_id
pos.pose = feedback.pose
if self.whicharm == "right":
ik = self.planner.check_ik_right_arm
else:
ik = self.planner.check_ik_left_arm
if ik(pos):
rospy.loginfo("Pose is reachable")
self.trajectory.poses.append(feedback.pose)
else:
rospy.logerr("Pose is not reachable!")
def place_gripper(self, feedback):
"""
Move the marker where the gripper is
"""
if self.whicharm == "right":
gripper_pos = self.planner.get_right_gripper_pose()
else:
gripper_pos = self.planner.get_left_gripper_pose()
self.server.setPose(self.int_marker.name, gripper_pos.pose,
gripper_pos.header)
self.server.applyChanges()
def execute_trajectory(self, feedback):
"""
Executes the tracjectory memorized so far. It interpolates between
the poses and creates suitable times and velocities.
"""
traj = self.interpolate_poses()
if len(traj) == 0:
rospy.logerr("Something went wrong when interpolating")
return
times, vels = self.ik_utils.trajectory_times_and_vels(traj)
if len(vels) == 0 or len(times) == 0:
rospy.logerr("Something went wrong when finding the times")
return
self.joint_controller.execute_trajectory(traj, times, vels,
self.whicharm,
wait = True)
return
def execute_trajectory_srv(self, _):
"""Same as execute_trajectory, but as a service.
"""
self.execute_trajectory(None)
return EmptyResponse()
def arm_trajectory_start(self, feedback):
"""
Move the gripper to the first pose in the trajectory.
"""
if len(self.trajectory.poses) == 0:
rospy.logwarn("Empty trajectory!")
return
pose = self.trajectory.poses[0]
if self.whicharm == "right":
moveit = self.planner.move_right_arm_non_collision
else:
moveit = self.planner.move_left_arm_non_collision
pos, quat = create_tuples_from_pose(pose)
res = moveit(pos, quat, self.trajectory.header.frame_id, 1.0)
if not res:
rospy.logerr("Something went wrong when moving")
return
def clear_trajectory(self, feedback):
"""
Removes all the points stored so far
"""
self.trajectory.poses = []
def move_head(self, feedback):
"""
Moves the head to face the marker
"""
frame = feedback.header.frame_id
pos = (feedback.pose.position.x,
feedback.pose.position.y,
feedback.pose.position.z,
)
print "Moving the head"
self.joint_controller.time_to_reach = 1.0
self.joint_controller.point_head_to(pos, frame)
def plan_arm(self, feedback):
"""
Moves the arm on the marker using motion collision-aware motion
planning.
"""
frame = feedback.header.frame_id
pos = (feedback.pose.position.x,
feedback.pose.position.y,
feedback.pose.position.z,
)
orientation = (feedback.pose.orientation.x,
feedback.pose.orientation.y,
feedback.pose.orientation.z,
feedback.pose.orientation.w,
)
if self.whicharm == "right":
rospy.loginfo("Moving the right arm")
self.planner.move_right_arm(pos, orientation, frame, self.planning_waiting_time)
else:
rospy.loginfo("Moving the left arm")
self.planner.move_left_arm(pos, orientation, frame, self.planning_waiting_time)
def collision_free_arm(self, feedback):
"""
Moves the rm on the marker using motion NON-collision-aware inverse
kinematiks.
"""
frame = feedback.header.frame_id
pos = (feedback.pose.position.x,
feedback.pose.position.y,
feedback.pose.position.z,
)
orientation = (feedback.pose.orientation.x,
feedback.pose.orientation.y,
feedback.pose.orientation.z,
feedback.pose.orientation.w,
)
if self.whicharm == "right":
rospy.loginfo("Moving the right arm (non collision)")
self.planner.move_right_arm_non_collision(pos, orientation,
frame, 2.0)
else:
rospy.loginfo("Moving the left arm (non collision)")
self.planner.move_left_arm_non_collision(pos, orientation,
frame, 2.0)
def update_planning(self, feedback):
"""
Updates the planning scene.
"""
self.planner.take_static_map()
self.planner.update_planning_scene()
def publish_trajectory(self, feedback):
"""
Publishes the trajectory as a PoseArray message
"""
self.trajectory_pub.publish(self.trajectory)
def interpolate_poses(self):
"""
Refines the trajectory by interpolating between the joints.
"""
if len(self.trajectory.poses) < 2:
rospy.logerr("At least two points in the trajectory are needed")
return []
if self.whicharm == "right":
starting_angles = self.robot_state.right_arm_positions
else:
starting_angles = self.robot_state.left_arm_positions
all_trajectory = [starting_angles]
for i in xrange(len(self.trajectory.poses) - 1):
start = PoseStamped()
start.header = self.trajectory.header
start.pose = self.trajectory.poses[i]
end = PoseStamped()
end.header = self.trajectory.header
end.pose = self.trajectory.poses[i+1]
traj, errs = self.ik_utils.check_cartesian_path(start, end,
all_trajectory[-1],
#starting_angles,
#pos_spacing = 0.05,
collision_aware = 0,
num_steps = 5,
)
if any(e == 3 for e in errs):
rospy.logerr("Error while running IK, codes are: %s", errs)
return []
filtered_traj = [t for (t,e) in zip(traj,errs) if e == 0]
all_trajectory.extend(filtered_traj)
all_trajectory = normalize_trajectory(all_trajectory, starting_angles)
rospy.loginfo("New interpolated trajectory with %d elements",
len(all_trajectory))
return all_trajectory
def publish_trajectory_markers(self, duration):
"""
Publishes markers to visualize the current trajectory.
Paremeters:
duration: how long should the markers visualization last. If this
function is called from a loop they last at least the loop rate.
"""
if len(self.trajectory.poses) == 0:
return
msg = MarkerArray()
marker_id = 0
#creating the path connecting the axes
path = Marker()
path.header.frame_id = self.trajectory.header.frame_id
path.ns = "path"
path.action = Marker.ADD
path.type = Marker.LINE_STRIP
path.lifetime = rospy.Duration(duration)
path.color.r = 1
path.color.g = 0
path.color.b = 1
path.color.a = 1
path.scale.x = 0.01
path.id = marker_id
marker_id += 1
for pose in self.trajectory.poses:
pos = PoseStamped()
pos.header.frame_id = self.trajectory.header.frame_id
pos.pose = pose
markers = utils.axis_marker(pos, marker_id, "axes")
msg.markers.extend(markers)
path.points.append(pose.position)
marker_id += 3 #3 axes
msg.markers.append(path)
self.visualizer_pub.publish(msg)
class POIServer(object):
'''
Node to act as a server to hold a list of points of interest which
can be modified by services or interactive markers
'''
def __init__(self):
'''
Create a POIServer
'''
# TF bootstrap
self.tf_buffer = tf2_ros.Buffer()
self.tf_listener = tf2_ros.TransformListener(self.tf_buffer)
# Radius of interactive marker for POIs
self.marker_scale = rospy.get_param("~marker_scale", 0.5)
# Create intial empty POI array
self.pois = POIArray()
# Get the global frame to be used
self.global_frame = rospy.get_param("~global_frame", "map")
self.pois.header.frame_id = self.global_frame
# Create publisher to notify clients of updates and interactive marker server
self.pub = rospy.Publisher("points_of_interest",
POIArray,
queue_size=1,
latch=True)
self.interactive_marker_server = InteractiveMarkerServer(
"points_of_interest")
# Load initial POIs from params
if rospy.has_param('~initial_pois'):
pois = rospy.get_param('~initial_pois')
assert isinstance(pois, dict)
for key, value in pois.iteritems():
assert type(key) == str
assert type(value) == list
assert len(value) == 3
name = key
position = numpy_to_point(value)
self._add_poi(name, position)
# Update clients / markers of changes from param
self.update()
# Create services to add / delete / move a POI
self.add_poi_server = rospy.Service('~add', AddPOI, self.add_poi_cb)
self.delete_poi_server = rospy.Service('~delete', DeletePOI,
self.delete_poi_cb)
self.move_poi_service = rospy.Service('~move', MovePOI,
self.move_poi_cb)
self.save_to_param = rospy.Service("~save_to_param", Trigger,
self.save_to_param_cb)
def transform_position(self, ps):
'''
Attempty to transform a PointStamped message into the global frame, returning
the position of the transformed point or None if transform failed.
'''
# If no frame, assume user wanted it in the global frame
if ps.header.frame_id == "":
return ps.point
try:
ps_tf = self.tf_buffer.transform(ps,
self.global_frame,
timeout=rospy.Duration(5))
return ps_tf.point
except tf2_ros.TransformException as e:
rospy.logwarn('Error transforming "{}" to "{}": {}'.format(
ps.header.frame_id, self.global_frame, e))
return None
def process_feedback(self, feedback):
'''
Process interactive marker feedback, moving markers internally inresponse to markers moved in RVIZ
'''
# Only look at changes when mouse button is unclicked
if feedback.event_type != InteractiveMarkerFeedback.MOUSE_UP:
return
# Transform new position into global frame
ps = PointStamped()
ps.header = feedback.header
ps.point = feedback.pose.position
position = self.transform_position(ps)
if position is None:
return
# Update position of marker
self.update_position(feedback.marker_name, feedback.pose.position)
@thread_lock(lock)
def save_to_param_cb(self, req):
rospy.set_param('~global_frame', self.global_frame)
d = {}
for poi in self.pois.pois:
d[poi.name] = [
float(poi.position.x),
float(poi.position.y),
float(poi.position.z)
]
rospy.set_param('~initial_pois', d)
return {'success': True}
def add_poi_cb(self, req):
'''
Callback for AddPOI service
'''
name = req.name
# If frame is empty, just initialize it to zero
if len(req.position.header.frame_id) == 0:
position = numpy_to_point([0., 0., 0.])
# Otherwise transform position into global frame
else:
position = self.transform_position(req.position)
if position is None:
return {'success': False, 'message': 'tf error (bad poi)'}
if not self.add_poi(name, position):
return {'success': False, 'message': 'alread exists (bad poi)'}
return {'success': True, 'message': 'good poi'}
def delete_poi_cb(self, req):
'''
Callback for DeletePOI service
'''
# Return error if marker did not exist
if not self.remove_poi(req.name):
return {'success': False, 'message': 'does not exist (bad poi)'}
return {'success': True, 'message': 'good poi'}
def move_poi_cb(self, req):
'''
Callback for MovePOI service
'''
name = req.name
# Transform position into global frame
position = self.transform_position(req.position)
if position is None:
return {'success': False, 'message': 'tf error (bad poi)'}
# Return error if marker did not exist
if not self.update_position(name, position):
return {'success': False, 'message': 'does not exist (bad poi)'}
return {'success': True, 'message': 'good poi'}
@thread_lock(lock)
def add_poi(self, name, position):
'''
Add a POI, updating clients / rviz when done
@return False if POI already exists
'''
if not self._add_poi(name, position):
return False
self.update()
return True
@thread_lock(lock)
def remove_poi(self, name):
'''
Remove a POI, updating clients / rviz when done
@return False if POI with name does not exist
'''
if not self._remove_poi(name):
return False
self.update()
return True
@thread_lock(lock)
def update_position(self, name, position):
'''
Update the position of a POI, updating clients / rviz when done
@param position: a Point message of the new position in global frame
@return False if POI with name does not exist
'''
if not self._update_position(name, position):
return False
self.update()
return True
def update(self):
'''
Update interactive markers server and POI publish of changes
'''
self.pois.header.stamp = rospy.Time.now()
self.pub.publish(self.pois)
self.interactive_marker_server.applyChanges()
def _update_position(self, name, position):
'''
Internal implementation of update_position, which is NOT thread safe and does NOT update clients of change
'''
# Find poi with specified name
for poi in self.pois.pois:
if poi.name == name:
pose = Pose()
pose.orientation.w = 1.0
pose.position = position
if not self.interactive_marker_server.setPose(name, pose):
return False
# Set pose in message
poi.position = position
return True
return False
def _remove_poi(self, name):
'''
Internal implementation of remove_poi, which is NOT thread safe and does NOT update clients of change
'''
# Return false if marker with that name not added to interactive marker server
if not self.interactive_marker_server.erase(name):
return False
# Find POI with specifed name and delete it from list
for i, poi in enumerate(self.pois.pois):
if poi.name == name:
del self.pois.pois[i]
return True
return False
def _add_poi(self, name, position):
'''
Internal implementation of add_poi, which is NOT thread safe and does NOT update clients of change
'''
if self.interactive_marker_server.get(name) is not None:
return False
poi = POI()
poi.name = name
poi.position = position
self.pois.pois.append(poi)
point_marker = Marker()
point_marker.type = Marker.SPHERE
point_marker.scale.x = self.marker_scale
point_marker.scale.y = self.marker_scale
point_marker.scale.z = self.marker_scale
point_marker.color.r = 1.0
point_marker.color.g = 1.0
point_marker.color.b = 1.0
point_marker.color.a = 1.0
text_marker = Marker()
text_marker.type = Marker.TEXT_VIEW_FACING
text_marker.pose.orientation.w = 1.0
text_marker.pose.position.x = 1.5
text_marker.text = poi.name
text_marker.scale.z = 1.0
text_marker.color.r = 1.0
text_marker.color.g = 1.0
text_marker.color.b = 1.0
text_marker.color.a = 1.0
int_marker = InteractiveMarker()
int_marker.header.frame_id = self.global_frame
int_marker.pose.orientation.w = 1.0
int_marker.pose.position = poi.position
int_marker.scale = 1
int_marker.name = poi.name
# insert a box
control = InteractiveMarkerControl()
control.interaction_mode = InteractiveMarkerControl.MOVE_3D
control.always_visible = True
control.markers.append(point_marker)
control.markers.append(text_marker)
int_marker.controls.append(control)
self.interactive_marker_server.insert(int_marker,
self.process_feedback)
return True
class InteractiveMarkerManager(object):
def __init__(self, parent_instance_handle, server_name, ik_solver):
self.parent_instance_handle = parent_instance_handle
self.server = InteractiveMarkerServer(server_name)
self.ik_solver = ik_solver
self.ik_position_xyz_bounds = [0.01, 0.01, 0.01]
self.ik_position_rpy_bounds = [31416.0, 31416.0, 31416.0] # NOTE: This implements position-only IK
self._menu_handlers = {}
self._menu_cmds = {}
self.menu_handler = MenuHandler()
del_sub_menu_handle = self.menu_handler.insert("Delete Waypoint")
self._menu_cmds['del_wp'] = self.menu_handler.insert("Yes", parent=del_sub_menu_handle, callback=self._process_feedback)
self.menu_handler.insert("No", parent=del_sub_menu_handle, callback=self._process_feedback)
ins_sub_menu_handle = self.menu_handler.insert("Insert Waypoint")
self._menu_cmds['ins_wp_before'] = self.menu_handler.insert("Before", parent=ins_sub_menu_handle, callback=self._process_feedback)
self._menu_cmds['ins_wp_after'] = self.menu_handler.insert("After", parent=ins_sub_menu_handle, callback=self._process_feedback)
self.menu_handler.insert("Cancel", parent=ins_sub_menu_handle, callback=self._process_feedback)
self._int_marker_name_list = []
self.markers_created_cnt = 0
self.marker_cnt = 0
self.name_to_marker_dict = {}
self.waypoint_to_marker_dict = {}
self.name_to_waypoint_dict = {}
def _process_feedback(self, feedback):
s = "Feedback from marker '" + feedback.marker_name
s += "' / control '" + feedback.control_name + "'"
mp = ""
if feedback.mouse_point_valid:
mp = " at " + str(feedback.mouse_point.x)
mp += ", " + str(feedback.mouse_point.y)
mp += ", " + str(feedback.mouse_point.z)
mp += " in frame " + feedback.header.frame_id
if feedback.event_type == InteractiveMarkerFeedback.BUTTON_CLICK:
rospy.logdebug(s + ": button click" + mp + ".")
pass
elif feedback.event_type == InteractiveMarkerFeedback.MENU_SELECT:
rospy.logdebug(s + ": menu item " + str(feedback.menu_entry_id) + " clicked" + mp + ".")
self._process_menu_select(feedback)
elif feedback.event_type == InteractiveMarkerFeedback.POSE_UPDATE: # TODO: I really want to use the other type of feedback
rospy.logdebug(s + ": pose changed")
self._update_waypoint_position(feedback.marker_name, feedback.pose)
elif feedback.event_type == InteractiveMarkerFeedback.MOUSE_DOWN:
rospy.logdebug(s + ": mouse down" + mp + ".")
pass
elif feedback.event_type == InteractiveMarkerFeedback.MOUSE_UP:
rospy.logdebug(s + ": mouse up" + mp + ".")
pass
self.server.applyChanges()
def _process_menu_select(self, feedback):
menu_entry_id = feedback.menu_entry_id
if menu_entry_id == self._menu_cmds['del_wp']:
self._delete_waypoint(feedback)
elif menu_entry_id == self._menu_cmds['ins_wp_before']:
self._insert_waypoint(feedback, before=True)
elif menu_entry_id == self._menu_cmds['ins_wp_after']:
self._insert_waypoint(feedback, before=False)
elif menu_entry_id in self._menu_cmds[feedback.marker_name]['menu_time_cmds']:
self._process_menu_time_cmds(feedback)
else:
pass
def _delete_waypoint(self, feedback):
marker = self.name_to_marker_dict[feedback.marker_name]
waypoint = self.name_to_waypoint_dict[marker.name]
self.parent_instance_handle.delete_waypoint(waypoint)
# Update dictionaries
del self.name_to_marker_dict[marker.name]
del self.waypoint_to_marker_dict[waypoint]
del self.name_to_waypoint_dict[marker.name]
del self._menu_handlers[marker.name]
del self._menu_cmds[marker.name]
# Erase marker from server
self.server.erase(feedback.marker_name)
self.server.applyChanges()
self.marker_cnt -=1
self._int_marker_name_list.remove(marker.name)
self._update_wp_marker_descriptions()
self.server.applyChanges()
def _insert_waypoint(self, feedback, before=True):
ref_marker = self.name_to_marker_dict[feedback.marker_name]
ref_waypoint = self.name_to_waypoint_dict[ref_marker.name]
self.parent_instance_handle.insert_waypoint(ref_waypoint, before=before)
self._update_wp_marker_descriptions()
self.server.applyChanges()
def _process_menu_time_cmds(self, feedback):
menu_entry_handle = feedback.menu_entry_id
menu_handler = self._menu_handlers[feedback.marker_name]
check_state = menu_handler.getCheckState(menu_entry_handle)
if check_state == MenuHandler.UNCHECKED:
menu_handler.setCheckState(menu_entry_handle, MenuHandler.CHECKED)
# Uncheck all other menu entries
for menu_entry_id in self._menu_cmds[feedback.marker_name]['menu_time_cmds']:
if menu_entry_id != menu_entry_handle:
menu_handler.setCheckState(menu_entry_id, MenuHandler.UNCHECKED)
# Update waypoints
waypoint = self.name_to_waypoint_dict[feedback.marker_name]
waypoint.time = self._menu_cmds[feedback.marker_name]['menu_time_cmds'][menu_entry_handle]
# Apply marker changes
menu_handler.reApply(self.server)
self.server.applyChanges()
def _update_waypoint_pose(self):
# TODO maybe handle pose changes differently?
pass
def _update_waypoint_position(self, marker_name, marker_pose):
waypoint = self.name_to_waypoint_dict[marker_name]
eff_pos = marker_pose.position
eff_orient = marker_pose.orientation
target_jt_angles = self.ik_solver.get_ik(waypoint.joint_state.positions,
eff_pos.x, eff_pos.y, eff_pos.z,
eff_orient.w, eff_orient.x, eff_orient.y, eff_orient.z,
self.ik_position_xyz_bounds[0],
self.ik_position_xyz_bounds[1],
self.ik_position_xyz_bounds[2],
self.ik_position_rpy_bounds[0],
self.ik_position_rpy_bounds[1],
self.ik_position_rpy_bounds[2])
if target_jt_angles is not None:
waypoint.end_effector_pose = copy.deepcopy(marker_pose)
waypoint.joint_state.positions = target_jt_angles
self._change_wp_marker_color(marker_name, "white", 0.9)
else:
self._change_wp_marker_color(marker_name, "red", 0.9)
def _change_wp_marker_color(self, wp_marker_name, color, opacity=1.0):
assert 0.0 <= opacity <= 1.0
# set color
int_marker = self.server.get(wp_marker_name)
marker = int_marker.controls[0].markers[0]
replacement_int_marker = copy.deepcopy(int_marker)
replacement_marker = replacement_int_marker.controls[0].markers[0]
rgb = None
if color == "white":
rgb = (1.0, 1.0, 1.0)
elif color == "red":
rgb = (1.0, 0.0, 0.0)
# TODO: Additional colors here
changed = False
if rgb is not None:
if marker.color.r != rgb[0]:
replacement_marker.color.r = rgb[0]
changed = True
print("change to ", rgb)
if marker.color.g != rgb[1]:
replacement_marker.color.g = rgb[1]
changed = True
if marker.color.b != rgb[2]:
replacement_marker.color.b = rgb[2]
changed = True
if marker.color.a != opacity:
replacement_marker.color.a = opacity
changed = True
# update wp marker
if changed:
self._replace_wp_marker(int_marker, replacement_int_marker)
def _update_wp_marker_descriptions(self):
for index, marker_name in enumerate(self._int_marker_name_list):
marker = self.name_to_marker_dict[marker_name]
marker_description = int(marker.description)
if marker_description != index+1:
marker.description = str(index+1)
replacement_marker = copy.deepcopy(self.server.get(marker.name))
replacement_marker.description = marker.description
self._replace_wp_marker(marker, replacement_marker)
def _replace_wp_marker(self, old_int_marker, replacement_int_marker):
# Update dictionaries
self.name_to_marker_dict[old_int_marker.name] = replacement_int_marker
self.waypoint_to_marker_dict[self.name_to_waypoint_dict[old_int_marker.name]] = replacement_int_marker
# Erase marker from server
self.server.erase(old_int_marker.name)
self.server.insert(replacement_int_marker, self._process_feedback)
self.server.applyChanges()
def add_waypoint_marker(self, waypoint, description=None):
self.markers_created_cnt += 1
self.marker_cnt += 1
int_marker = InteractiveMarker()
int_marker.header.frame_id = "base_link"
int_marker.pose.position = waypoint.end_effector_pose.position
int_marker.scale = 0.1
int_marker.name = str(self.markers_created_cnt)
if description is None:
int_marker.description = int_marker.name
else:
int_marker.description = description
self.name_to_marker_dict[int_marker.name] = int_marker
self.waypoint_to_marker_dict[waypoint] = int_marker
self.name_to_waypoint_dict[int_marker.name] = waypoint
# insert a box # TODO: may change geometry / eff mesh
make_box_control_marker(int_marker)
int_marker.controls[0].interaction_mode = InteractiveMarkerControl.MOVE_ROTATE_3D
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 1
control.orientation.y = 0
control.orientation.z = 0
control.name = "rotate_x"
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
int_marker.controls.append(control)
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 1
control.orientation.y = 0
control.orientation.z = 0
control.name = "move_x"
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
int_marker.controls.append(control)
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 0
control.orientation.y = 1
control.orientation.z = 0
control.name = "rotate_z"
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
int_marker.controls.append(control)
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 0
control.orientation.y = 1
control.orientation.z = 0
control.name = "move_z"
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
int_marker.controls.append(control)
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 0
control.orientation.y = 0
control.orientation.z = 1
control.name = "rotate_y"
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
int_marker.controls.append(control)
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 0
control.orientation.y = 0
control.orientation.z = 1
control.name = "move_y"
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
int_marker.controls.append(control)
self.server.insert(int_marker, self._process_feedback)
# add menus
menu_handler = MenuHandler()
del_sub_menu_handle = menu_handler.insert("Delete Waypoint")
self._menu_cmds['del_wp'] = menu_handler.insert("Yes", parent=del_sub_menu_handle,
callback=self._process_feedback)
menu_handler.insert("No", parent=del_sub_menu_handle, callback=self._process_feedback)
ins_sub_menu_handle = menu_handler.insert("Insert Waypoint")
self._menu_cmds['ins_wp_before'] = menu_handler.insert("Before", parent=ins_sub_menu_handle,
callback=self._process_feedback)
self._menu_cmds['ins_wp_after'] = menu_handler.insert("After", parent=ins_sub_menu_handle,
callback=self._process_feedback)
menu_handler.insert("Cancel", parent=ins_sub_menu_handle, callback=self._process_feedback)
# Set time
time_sub_menu_handle = menu_handler.insert("Set Waypoint Time")
menu_time_cmds = {}
time = self.name_to_waypoint_dict[int_marker.name].time
time_list = [float(num) for num in range((int(m.ceil(time))))]
time_list.append(time)
time_list.extend([m.floor(time) + cnt + 1 for cnt in range(6)])
self._menu_cmds[int_marker.name] = {"menu_time_cmds": menu_time_cmds}
for t in time_list:
time_opt_handle = menu_handler.insert(str(t), parent=time_sub_menu_handle, callback=self._process_feedback)
menu_time_cmds[time_opt_handle] = t
if t == time:
menu_handler.setCheckState(time_opt_handle, MenuHandler.CHECKED)
else:
menu_handler.setCheckState(time_opt_handle, MenuHandler.UNCHECKED)
menu_handler.apply(self.server, int_marker.name)
self._menu_handlers[int_marker.name] = menu_handler
self.server.applyChanges()
if int(int_marker.description)-1 < len(self._int_marker_name_list):
self._int_marker_name_list.insert(int(int_marker.description) - 1, int_marker.name)
else:
self._int_marker_name_list.append(int_marker.name)
def clear_waypoint_markers(self):
self.server.clear()
self.server.applyChanges()
self._menu_handlers = {}
self._menu_cmds = {}
self._int_marker_name_list = []
self.markers_created_cnt = 0
self.marker_cnt = 0
self.name_to_marker_dict = {}
self.waypoint_to_marker_dict = {}
self.name_to_waypoint_dict = {}
class InteractiveMarkerManager(object):
def __init__(self, server_ns='interactive_markers'):
self.server = InteractiveMarkerServer(server_ns)
self._menu_handlers = {}
self._menu_cmds = {}
self.markers_created_cnt = 0
self.marker_cnt = 0
self._int_marker_name_list = []
self.name_to_marker_dict = {}
self.name_to_position_only_flag = {}
def _process_feedback(self, feedback):
s = "Feedback from marker '" + feedback.marker_name
s += "' / control '" + feedback.control_name + "'"
mp = ""
if feedback.mouse_point_valid:
mp = " at " + str(feedback.mouse_point.x)
mp += ", " + str(feedback.mouse_point.y)
mp += ", " + str(feedback.mouse_point.z)
mp += " in frame " + feedback.header.frame_id
if feedback.event_type == InteractiveMarkerFeedback.BUTTON_CLICK:
rospy.logdebug(s + ": button click" + mp + ".")
pass
elif feedback.event_type == InteractiveMarkerFeedback.MENU_SELECT:
rospy.logdebug(s + ": menu item " + str(feedback.menu_entry_id) + " clicked" + mp + ".")
self._process_menu_select(feedback)
elif feedback.event_type == InteractiveMarkerFeedback.POSE_UPDATE: # TODO: I really want to use the other type of feedback
rospy.logdebug(s + ": pose changed")
self._update_marker_pose(feedback.marker_name, feedback.pose)
elif feedback.event_type == InteractiveMarkerFeedback.MOUSE_DOWN:
rospy.logdebug(s + ": mouse down" + mp + ".")
pass
elif feedback.event_type == InteractiveMarkerFeedback.MOUSE_UP:
rospy.logdebug(s + ": mouse up" + mp + ".")
pass
self.server.applyChanges()
def _process_menu_select(self, feedback):
menu_entry_id = feedback.menu_entry_id
if menu_entry_id == self._menu_cmds['position_only']:
self._set_position_only_checkmark(feedback)
else:
pass
def _set_position_only_checkmark(self, feedback):
menu_entry_handle = feedback.menu_entry_id
menu_handler = self._menu_handlers[feedback.marker_name]
check_state = menu_handler.getCheckState(menu_entry_handle)
if check_state == MenuHandler.UNCHECKED:
menu_handler.setCheckState(menu_entry_handle, MenuHandler.CHECKED)
self.name_to_position_only_flag[feedback.marker_name] = True
else:
menu_handler.setCheckState(menu_entry_handle, MenuHandler.UNCHECKED)
self.name_to_position_only_flag[feedback.marker_name] = False
# Apply marker changes
menu_handler.reApply(self.server)
self.server.applyChanges()
def change_marker_color(self, marker_name, color, opacity=1.0):
assert 0.0 <= opacity <= 1.0
# set color
int_marker = self.server.get(marker_name)
marker = int_marker.controls[0].markers[0]
replacement_int_marker = copy.deepcopy(int_marker)
replacement_marker = replacement_int_marker.controls[0].markers[0]
rgb = None
if color == "white":
rgb = (1.0, 1.0, 1.0)
elif color == "red":
rgb = (1.0, 0.0, 0.0)
# TODO: Additional colors here
changed = False
if rgb is not None:
if marker.color.r != rgb[0]:
replacement_marker.color.r = rgb[0]
changed = True
print("change to ", rgb)
if marker.color.g != rgb[1]:
replacement_marker.color.g = rgb[1]
changed = True
if marker.color.b != rgb[2]:
replacement_marker.color.b = rgb[2]
changed = True
if marker.color.a != opacity:
replacement_marker.color.a = opacity
changed = True
# update marker
if changed:
self._replace_marker(int_marker, replacement_int_marker)
def _replace_marker(self, old_int_marker, replacement_int_marker):
# Update dictionaries
self.name_to_marker_dict[old_int_marker.name] = replacement_int_marker
# Erase marker from server
self.server.erase(old_int_marker.name)
self.server.insert(replacement_int_marker, self._process_feedback)
self.server.applyChanges()
def add_marker(self, initial_pose, frame="base_link", description=None):
self.markers_created_cnt += 1
self.marker_cnt += 1
int_marker = InteractiveMarker()
int_marker.header.frame_id = frame
int_marker.pose.position = initial_pose.position
int_marker.scale = 0.1
int_marker.name = rospy.get_name() + str(self.markers_created_cnt)
if description is None:
int_marker.description = int_marker.name
else:
int_marker.description = description
self.name_to_marker_dict[int_marker.name] = int_marker
self.name_to_position_only_flag[int_marker.name] = True
# insert a box # TODO: may change geometry / eff mesh
make_box_control_marker(int_marker)
int_marker.controls[0].interaction_mode = InteractiveMarkerControl.MOVE_ROTATE_3D
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 1
control.orientation.y = 0
control.orientation.z = 0
control.name = "rotate_x"
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
int_marker.controls.append(control)
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 1
control.orientation.y = 0
control.orientation.z = 0
control.name = "move_x"
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
int_marker.controls.append(control)
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 0
control.orientation.y = 1
control.orientation.z = 0
control.name = "rotate_z"
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
int_marker.controls.append(control)
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 0
control.orientation.y = 1
control.orientation.z = 0
control.name = "move_z"
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
int_marker.controls.append(control)
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 0
control.orientation.y = 0
control.orientation.z = 1
control.name = "rotate_y"
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
int_marker.controls.append(control)
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 0
control.orientation.y = 0
control.orientation.z = 1
control.name = "move_y"
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
int_marker.controls.append(control)
self.server.insert(int_marker, self._process_feedback)
# add menus
menu_handler = MenuHandler()
pos_opt_handle = menu_handler.insert("Position-Only Ctrl", callback=self._process_feedback)
self._menu_cmds['position_only'] = pos_opt_handle
menu_handler.setCheckState(pos_opt_handle, MenuHandler.CHECKED)
menu_handler.apply(self.server, int_marker.name)
self._menu_handlers[int_marker.name] = menu_handler
self.server.applyChanges()
self._int_marker_name_list.append(int_marker.name)
return int_marker.name
def _update_marker_pose(self, marker_name, new_pose):
self.name_to_marker_dict[marker_name].pose = new_pose
def get_marker_pose(self, marker_name):
return self.name_to_marker_dict[marker_name].pose
def clear_markers(self):
self.server.clear()
self.server.applyChanges()
self._menu_handlers = {}
self._menu_cmds = {}
self.markers_created_cnt = 0
self.marker_cnt = 0
self._int_marker_name_list = []
self.name_to_marker_dict = {}
self.name_to_position_only_flag = {}
class PR2TrajectoryMarkers(object):
"""
A class to create and store a trajectory for one PR2 arm. The created
trajectory can be published as a PoseArray message.
This class published on the following topics:
~trajectory_markers are the main interactive markers.
~trajectory_poses a markerarray to display the trajectory.
~trajectory_poses a posesarray with the resulting pose
The class subscribes to the topic ~overwrite_trajectory_
to change the stored trajectory. This is useful to resume working on a
trajectory after re-starting the node. The message type is PoseArray.
A std_srvs/Empty service named ~execute_trajectory is provided to
externally trigger the execution of the trajectory.
Constructor:
TrajectoryMarkers(whicharm = "left")
or
TrajectoryMarkers(whicharm = "right")
"""
def __init__(self, whicharm):
self.whicharm = whicharm
self.robot_state = pr2_control_utilities.RobotState()
self.joint_controller = pr2_control_utilities.PR2JointMover(
self.robot_state)
self.planner = pr2_control_utilities.PR2MoveArm(self.joint_controller)
self.server = InteractiveMarkerServer("~trajectory_markers")
self.tf_listener = self.planner.tf_listener
self.visualizer_pub = rospy.Publisher("~trajectory_markers_path",
MarkerArray)
self.trajectory_pub = rospy.Publisher("~trajectory_poses", PoseArray)
rospy.Subscriber("~overwrite_trajectory", PoseArray,
self.overwrite_trajectory)
rospy.Service("~execute_trajectory", Empty,
self.execute_trajectory_srv)
# create an interactive marker for our server
int_marker = InteractiveMarker()
int_marker.header.frame_id = "/base_link"
int_marker.pose.position.x = 0.5
int_marker.pose.position.z = 1.0
int_marker.name = "move_" + whicharm + "_arm"
int_marker.description = "Move the " + whicharm + " arm"
int_marker.scale = 0.2
self.server.insert(int_marker, self.main_callback)
# create the main marker shape
#color = (1,0,0,1)
color = None
self.gripper_marker = utils.makeGripperMarker(color=color)
int_marker.controls.append(self.gripper_marker)
#add the controls
utils.make_6DOF_marker(int_marker)
self.int_marker = int_marker
self.create_menu()
self.server.applyChanges()
self.trajectory = PoseArray()
self.trajectory.header.frame_id = "/base_link"
self.last_gripper_pose = None
if whicharm == "right":
self.ik_utils = self.planner.right_ik
else:
self.ik_utils = self.planner.left_ik
rospy.loginfo("PR2TrajectoryMarkers (%s) is ready", whicharm)
def create_menu(self):
"""
Create and populates all the menu entries
"""
menu_handler = MenuHandler()
menu_handler.insert("Point the head", callback=self.move_head)
menu_handler.insert("Add position to trajectory",
callback=self.add_point)
menu_handler.insert("Place marker over gripper",
callback=self.place_gripper)
menu_handler.insert("Execute trjectory",
callback=self.execute_trajectory)
menu_handler.insert("Clear trajectory", callback=self.clear_trajectory)
menu_handler.insert("Publish trajectory",
callback=self.publish_trajectory)
menu_handler.insert("Move the arm (planning)", callback=self.plan_arm)
menu_handler.insert("Move the arm (collision-free)",
callback=self.collision_free_arm)
menu_handler.insert(
"Move the arm to trajectory start (collision-free)",
callback=self.arm_trajectory_start)
menu_handler.insert("Update planning scene",
callback=self.update_planning)
menu_handler.apply(self.server, self.int_marker.name)
def main_callback(self, feedback):
"""
If the mouse button is released change the gripper color according to
the IK result. Quite awkward, trying to get a nicer way to do it.
"""
if feedback.event_type == InteractiveMarkerFeedback.POSE_UPDATE:
self.last_gripper_pose = feedback.pose
#rospy.loginfo("Updating Marker: %d", feedback.event_type)
#rospy.loginfo("POS: %s", feedback.pose.position)
if (self.last_gripper_pose
and feedback.event_type == InteractiveMarkerFeedback.MOUSE_UP):
self.last_gripper_pose = None
pos = PoseStamped()
pos.header.frame_id = feedback.header.frame_id
pos.pose = feedback.pose
if self.whicharm == "right":
ik = self.planner.check_ik_right_arm
else:
ik = self.planner.check_ik_left_arm
if ik(pos):
color = None
else:
color = (1, 0, 0, 1)
int_marker = self.server.get(self.int_marker.name)
int_marker.controls.remove(self.gripper_marker)
self.gripper_marker = utils.makeGripperMarker(color=color)
int_marker.controls.append(self.gripper_marker)
#rospy.loginfo("Control: %s", int_marker.controls)
self.server.insert(int_marker, self.main_callback)
self.server.setPose(int_marker.name, self.last_gripper_pose)
self.server.applyChanges()
def overwrite_trajectory(self, msg):
self.trajectory = msg
def add_point(self, feedback):
"""
Add a point to self.trajectory if it is allowed by IK.
"""
pos = PoseStamped()
pos.header.frame_id = feedback.header.frame_id
pos.pose = feedback.pose
if self.whicharm == "right":
ik = self.planner.check_ik_right_arm
else:
ik = self.planner.check_ik_left_arm
if ik(pos):
rospy.loginfo("Pose is reachable")
self.trajectory.poses.append(feedback.pose)
else:
rospy.logerr("Pose is not reachable!")
def place_gripper(self, feedback):
"""
Move the marker where the gripper is
"""
if self.whicharm == "right":
gripper_pos = self.planner.get_right_gripper_pose()
else:
gripper_pos = self.planner.get_left_gripper_pose()
self.server.setPose(self.int_marker.name, gripper_pos.pose,
gripper_pos.header)
self.server.applyChanges()
def execute_trajectory(self, feedback):
"""
Executes the tracjectory memorized so far. It interpolates between
the poses and creates suitable times and velocities.
"""
traj = self.interpolate_poses()
if len(traj) == 0:
rospy.logerr("Something went wrong when interpolating")
return
times, vels = self.ik_utils.trajectory_times_and_vels(traj)
if len(vels) == 0 or len(times) == 0:
rospy.logerr("Something went wrong when finding the times")
return
self.joint_controller.execute_trajectory(traj,
times,
vels,
self.whicharm,
wait=True)
return
def execute_trajectory_srv(self, _):
"""Same as execute_trajectory, but as a service.
"""
self.execute_trajectory(None)
return EmptyResponse()
def arm_trajectory_start(self, feedback):
"""
Move the gripper to the first pose in the trajectory.
"""
if len(self.trajectory.poses) == 0:
rospy.logwarn("Empty trajectory!")
return
pose = self.trajectory.poses[0]
if self.whicharm == "right":
moveit = self.planner.move_right_arm_non_collision
else:
moveit = self.planner.move_left_arm_non_collision
pos, quat = create_tuples_from_pose(pose)
res = moveit(pos, quat, self.trajectory.header.frame_id, 1.0)
if not res:
rospy.logerr("Something went wrong when moving")
return
def clear_trajectory(self, feedback):
"""
Removes all the points stored so far
"""
self.trajectory.poses = []
def move_head(self, feedback):
"""
Moves the head to face the marker
"""
frame = feedback.header.frame_id
pos = (
feedback.pose.position.x,
feedback.pose.position.y,
feedback.pose.position.z,
)
print "Moving the head"
self.joint_controller.time_to_reach = 1.0
self.joint_controller.point_head_to(pos, frame)
def plan_arm(self, feedback):
"""
Moves the arm on the marker using motion collision-aware motion
planning.
"""
frame = feedback.header.frame_id
pos = (
feedback.pose.position.x,
feedback.pose.position.y,
feedback.pose.position.z,
)
orientation = (
feedback.pose.orientation.x,
feedback.pose.orientation.y,
feedback.pose.orientation.z,
feedback.pose.orientation.w,
)
if self.whicharm == "right":
rospy.loginfo("Moving the right arm")
self.planner.move_right_arm(pos, orientation, frame, 2.0)
else:
rospy.loginfo("Moving the left arm")
self.planner.move_left_arm(pos, orientation, frame, 2.0)
def collision_free_arm(self, feedback):
"""
Moves the rm on the marker using motion NON-collision-aware inverse
kinematiks.
"""
frame = feedback.header.frame_id
pos = (
feedback.pose.position.x,
feedback.pose.position.y,
feedback.pose.position.z,
)
orientation = (
feedback.pose.orientation.x,
feedback.pose.orientation.y,
feedback.pose.orientation.z,
feedback.pose.orientation.w,
)
if self.whicharm == "right":
rospy.loginfo("Moving the right arm (non collision)")
self.planner.move_right_arm_non_collision(pos, orientation, frame,
2.0)
else:
rospy.loginfo("Moving the left arm (non collision)")
self.planner.move_left_arm_non_collision(pos, orientation, frame,
2.0)
def update_planning(self, feedback):
"""
Updates the planning scene.
"""
self.planner.take_static_map()
self.planner.update_planning_scene()
def publish_trajectory(self, feedback):
"""
Publishes the trajectory as a PoseArray message
"""
self.trajectory_pub.publish(self.trajectory)
def interpolate_poses(self):
"""
Refines the trajectory by interpolating between the joints.
"""
if len(self.trajectory.poses) < 2:
rospy.logerr("At least two points in the trajectory are needed")
return []
if self.whicharm == "right":
starting_angles = self.robot_state.right_arm_positions
else:
starting_angles = self.robot_state.left_arm_positions
all_trajectory = [starting_angles]
for i in xrange(len(self.trajectory.poses) - 1):
start = PoseStamped()
start.header = self.trajectory.header
start.pose = self.trajectory.poses[i]
end = PoseStamped()
end.header = self.trajectory.header
end.pose = self.trajectory.poses[i + 1]
traj, errs = self.ik_utils.check_cartesian_path(
start,
end,
all_trajectory[-1],
#starting_angles,
#pos_spacing = 0.05,
collision_aware=0,
num_steps=5,
)
if any(e == 3 for e in errs):
rospy.logerr("Error while running IK, codes are: %s", errs)
return []
filtered_traj = [t for (t, e) in zip(traj, errs) if e == 0]
all_trajectory.extend(filtered_traj)
all_trajectory = normalize_trajectory(all_trajectory, starting_angles)
rospy.loginfo("New interpolated a trajectory with %d elements",
len(all_trajectory))
return all_trajectory
def publish_trajectory_markers(self, duration):
"""
Publishes markers to visualize the current trajectory.
Paremeters:
duration: how long should the markers visualization last. If this
function is called from a loop they last at least the loop rate.
"""
if len(self.trajectory.poses) == 0:
return
msg = MarkerArray()
marker_id = 0
#creating the path connecting the axes
path = Marker()
path.header.frame_id = self.trajectory.header.frame_id
path.ns = "path"
path.action = Marker.ADD
path.type = Marker.LINE_STRIP
path.lifetime = rospy.Duration(duration)
path.color.r = 1
path.color.g = 0
path.color.b = 1
path.color.a = 1
path.scale.x = 0.01
path.id = marker_id
marker_id += 1
for pose in self.trajectory.poses:
pos = PoseStamped()
pos.header.frame_id = self.trajectory.header.frame_id
pos.pose = pose
markers = utils.axis_marker(pos, marker_id, "axes")
msg.markers.extend(markers)
path.points.append(pose.position)
marker_id += 3 #3 axes
msg.markers.append(path)
self.visualizer_pub.publish(msg)
class IntCollisionEnv(CollisionEnvServices):
def __init__(self, setup, world_frame):
super(IntCollisionEnv, self).__init__(setup, world_frame)
self.im_server = InteractiveMarkerServer(self.NS + "markers")
self.check_attached_timer = rospy.Timer(rospy.Duration(0.1),
self.check_attached_timer_cb)
self.create_menus()
def create_menus(self):
self.global_menu_handler = MenuHandler()
g_art = self.global_menu_handler.insert("Artificial")
self.global_menu_handler.insert("Add primitive",
parent=g_art,
callback=self.global_menu_add_prim_cb)
self.global_menu_handler.insert("Clear all",
parent=g_art,
callback=self.global_menu_clear_all_cb)
self.global_menu_handler.insert(
"Clear all (permanent)",
parent=g_art,
callback=self.global_menu_clear_all_perm_cb)
self.global_menu_handler.insert("Save all",
parent=g_art,
callback=self.global_menu_save_all_cb)
self.global_menu_handler.insert("Reload",
parent=g_art,
callback=self.global_menu_reload_cb)
g_det = self.global_menu_handler.insert("Detected")
self.global_menu_handler.insert("Pause",
parent=g_det,
callback=self.global_menu_pause_cb)
self.global_menu_handler.insert(
"Clear all",
parent=g_det,
callback=self.global_menu_det_clear_all_cb)
self.a_obj_menu_handler = MenuHandler()
mov = self.a_obj_menu_handler.insert("Moveable",
callback=self.menu_moveable_cb)
self.a_obj_menu_handler.setCheckState(mov, MenuHandler.UNCHECKED)
sc = self.a_obj_menu_handler.insert("Scaleable",
callback=self.menu_scaleable_cb)
self.a_obj_menu_handler.setCheckState(sc, MenuHandler.UNCHECKED)
self.a_obj_menu_handler.insert("Save", callback=self.menu_save_cb)
self.a_obj_menu_handler.insert("Clear", callback=self.menu_remove_cb)
self.d_obj_menu_handler = MenuHandler()
self.d_obj_menu_handler.insert("Clear", callback=self.d_menu_clear_cb)
int_marker = InteractiveMarker()
int_marker.header.frame_id = self.world_frame
int_marker.pose.position.z = 1.2
int_marker.scale = 0.5
int_marker.name = "global_menu"
int_marker.description = "Global Menu"
control = InteractiveMarkerControl()
control.interaction_mode = InteractiveMarkerControl.BUTTON
control.always_visible = True
marker = Marker()
marker.type = Marker.CUBE
marker.scale.x = 0.05
marker.scale.y = 0.05
marker.scale.z = 0.05
marker.color.r = 0.5
marker.color.g = 0.5
marker.color.b = 0.5
marker.color.a = 0.5
control.markers.append(marker)
int_marker.controls.append(control)
self.im_server.insert(int_marker, self.ignored_cb)
self.global_menu_handler.apply(self.im_server, int_marker.name)
self.im_server.applyChanges()
def ignored_cb(self, feedback):
pass
def d_menu_clear_cb(self, f):
pass
def global_menu_det_clear_all_cb(self, f):
for name in self.clear_all_det():
self.im_server.erase(name)
self.im_server.applyChanges()
def global_menu_pause_cb(self, f):
handle = f.menu_entry_id
state = self.global_menu_handler.getCheckState(handle)
if state == MenuHandler.CHECKED:
self.global_menu_handler.setCheckState(handle,
MenuHandler.UNCHECKED)
self.paused = False
else:
self.global_menu_handler.setCheckState(handle, MenuHandler.CHECKED)
self.paused = True
self.global_menu_handler.reApply(self.im_server)
self.im_server.applyChanges()
def global_menu_save_all_cb(self, f):
self.save_primitives()
def global_menu_add_prim_cb(self, feedback):
p = CollisionPrimitive()
p.name = self._generate_name()
p.bbox.type = SolidPrimitive.BOX
p.bbox.dimensions = [0.1, 0.1, 0.1]
p.pose.header.frame_id = self.world_frame
p.pose.pose.orientation.w = 1
p.pose.pose.position.z = 0.5
p.setup = self.setup
self.add_primitive(p)
def global_menu_reload_cb(self, feedback):
self.reload()
def global_menu_clear_all_cb(self, feedback):
self.clear_all(permanent=False)
def global_menu_clear_all_perm_cb(self, feedback):
self.clear_all()
def menu_save_cb(self, f):
self.save_primitive(f.marker_name)
def menu_scaleable_cb(self, f):
handle = f.menu_entry_id
state = self.a_obj_menu_handler.getCheckState(handle)
if state == MenuHandler.CHECKED:
self.a_obj_menu_handler.setCheckState(handle,
MenuHandler.UNCHECKED)
# TODO
else:
self.a_obj_menu_handler.setCheckState(handle, MenuHandler.CHECKED)
# TODO
self.a_obj_menu_handler.reApply(self.im_server)
self.im_server.applyChanges()
def menu_moveable_cb(self, feedback):
handle = feedback.menu_entry_id
state = self.a_obj_menu_handler.getCheckState(handle)
if state == MenuHandler.CHECKED:
self.a_obj_menu_handler.setCheckState(handle,
MenuHandler.UNCHECKED)
# if feedback.marker_name in self.moveable:
# self.moveable.remove(feedback.marker_name)
self.im_server.erase(feedback.marker_name)
self.im_server.insert(
make_def(self.artificial_objects[feedback.marker_name]),
self.process_im_feedback)
else:
self.a_obj_menu_handler.setCheckState(handle, MenuHandler.CHECKED)
self.make_interactive(
self.artificial_objects[feedback.marker_name])
self.a_obj_menu_handler.reApply(self.im_server)
self.im_server.applyChanges()
def menu_remove_cb(self, feedback):
self.remove_name(feedback.marker_name)
def process_im_feedback(self, feedback):
if feedback.event_type == InteractiveMarkerFeedback.POSE_UPDATE:
ps = PoseStamped()
ps.header = feedback.header
ps.pose = feedback.pose
if feedback.marker_name in self.artificial_objects:
self.set_primitive_pose(feedback.marker_name, ps)
elif feedback.marker_name:
# detected objects
pass
def make_interactive(self, p):
im = self.im_server.get(p.name)
for v in (("x", (1, 0, 0, 1)), ("y", (0, 0, 1, 1)), ("z", (0, 1, 0,
1))):
im.controls.append(rotate_axis_control("rotate_" + v[0], v[1]))
im.controls.append(move_axis_control("move_" + v[0], v[1]))
def remove_name(self, name):
super(IntCollisionEnv, self).remove_name(name)
self.im_server.erase(name)
self.im_server.applyChanges()
def add_primitive(self, p):
super(IntCollisionEnv, self).add_primitive(p)
self.im_server.insert(make_def(p), self.process_im_feedback)
self.a_obj_menu_handler.apply(self.im_server, p.name)
self.im_server.applyChanges()
def reload(self):
self.im_server.clear()
self.create_menus()
super(IntCollisionEnv, self).reload()
def add_detected(self, name, ps, object_type):
super(IntCollisionEnv, self).add_detected(name, ps, object_type)
p = CollisionPrimitive()
p.name = name
p.pose = ps
p.bbox = object_type.bbox
self.im_server.insert(make_def(p, color=(0, 0, 1)),
self.process_im_feedback)
self.im_server.applyChanges()
def clear_detected(self, name):
super(IntCollisionEnv, self).clear_detected(name)
self.im_server.erase(name)
self.im_server.applyChanges()
def check_attached_timer_cb(self, evt):
for name, ps, bbox in self.get_attached():
p = CollisionPrimitive()
p.name = name
p.pose = ps
p.bbox = bbox
self.im_server.insert(make_def(p, color=(1, 0, 0)),
self.process_im_feedback)
self.im_server.applyChanges()
