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 PickAndPlaceNode(Manager):
def __init__(self, robot, *robotargs):
super(PickAndPlaceNode, self).__init__('pp_node')
rospy.loginfo("PickAndPlaceNode")
_post_perceive_trans = defaultdict(lambda: self._pick)
_post_perceive_trans.update({'c': self._calibrate})
_preplace = defaultdict(lambda: self._preplace)
self.transition_table = {
# If calibration has already happened, allow skipping it
'initial': {'c': self._calibrate, 'q': self._perceive,
's': self._preplace},
'calibrate': {'q': self._perceive, 'c': self._calibrate},
'perceive': {'p': self._post_perceive, 'q': self._perceive, 's': self._stop_perceive, 'c': self._calibrate},
'post_perceive': _post_perceive_trans,
'postpick': {'1': self._level, '2': self._level, '9': self._level},
'level': _preplace,
'preplace': {'s': self._place},
'place': {'q': self._perceive, 'c': self._calibrate}
}
rospy.loginfo("PickAndPlaceNode1")
if callable(robot):
self.robot = robot(*robotargs)
else:
self.robot = robot
self.robot.level = 1
rospy.loginfo("PickAndPlaceNode2")
# Hardcoded place for now
self.place_pose = PoseStamped(
Header(0, rospy.Time(0), self.robot.base),
Pose(Point(0.526025806, 0.4780144, -0.161326153),
Quaternion(1, 0, 0, 0)))
self.tl = tf.TransformListener()
self.num_objects = 0
# Would this work too? Both tf and tf2 have (c) 2008...
# self.tf2 = tf2_ros.TransformListener()
self.n_objects_sub = rospy.Subscriber("/num_objects", Int64,
self.update_num_objects,
queue_size=1)
self.perception_pub = rospy.Publisher("/perception/enabled",
Bool,
queue_size=1)
self.alignment_pub = rospy.Publisher("/alignment/doit",
Bool,
queue_size=1)
self.br = tf.TransformBroadcaster()
rospy.loginfo("PickAndPlaceNode3")
self.int_marker_server = InteractiveMarkerServer('int_markers')
# Dict to map imarker names and their updated poses
self.int_markers = {}
# Ideally this call would be in a Factory/Metaclass/Parent
self.show_options()
self.perceive = False
# self.robot.home()
# self.move_calib_position()
def move_calib_position(self):
# move arm to the calibration position
self.calib_pose = PoseStamped(
Header(0, rospy.Time(0), self.robot.base),
Pose(Point(0.338520675898,-0.175860479474,0.0356775075197),
Quaternion(-0.0183493755758,0.708424150944, 0.704712092876, 0.0343413949013)))
self.robot.move_ik(self.calib_pose)
def update_num_objects(self, msg):
self.num_objects = msg.data
def _calibrate(self):
self.state = "calibrate"
self.alignment_pub.publish(Bool(True))
def _perceive(self):
self.state = "perceive"
rospy.loginfo("Asking for perception...")
self.perception_pub.publish(Bool(True))
def _stop_perceive(self):
self.state = "perceive"
self.perception_pub.publish(Bool(False))
def _post_perceive(self):
self.state = "post_perceive"
rospy.loginfo("Asking to stop perception...")
self.perception_pub.publish(Bool(False))
def _preplace(self):
# State not modified until placing succeeds
try:
obj_to_get = int(self.character)
except ValueError:
rospy.logerr("Please provide a number in placing mode")
return
self.state = "preplace"
frame_name = "object_pose_{}".format(obj_to_get)
rospy.loginfo(
"Placing object on top of object {}...".format(obj_to_get))
if self.tl.frameExists(self.robot.base) and self.tl.frameExists(frame_name):
t = self.tl.getLatestCommonTime(self.robot.base, frame_name)
position, quaternion = self.tl.lookupTransform(self.robot.base,
frame_name,
t)
position = list(position)
# Height of cubelet
position[2] += self.robot.level * 0.042
# YCB
# position[2] += self.robot.level * 0.026
# Update pose position from perception
self.place_pose.pose.position = Point(*position)
rospy.loginfo("Adjusting place position...")
imarker_name = 'place_pose'
self.int_markers[imarker_name] = self.place_pose
imarker = make_interactive_marker(imarker_name,
self.place_pose.pose)
# TODO delete imarker at post place
self.int_marker_server.insert(imarker, self.imarker_callback)
self.int_marker_server.setCallback(imarker_name, self.imarker_callback)
self.int_marker_server.applyChanges()
rospy.loginfo("imarker stuff done")
def imarker_callback(self, msg):
# http://docs.ros.org/jade/api/visualization_msgs/html/msg/InteractiveMarkerFeedback.html # noqa
rospy.loginfo('imarker_callback called')
name = msg.marker_name
new_pose = msg.pose
self.int_markers[name] = PoseStamped(msg.header, new_pose)
def _place(self):
self.state = "place"
rospy.loginfo("Placing...")
place_pose = self.int_markers['place_pose']
# It seems positions and orientations are randomly required to
# be actual Point and Quaternion objects or lists/tuples. The
# least coherent API ever seen.
self.br.sendTransform(Point2list(place_pose.pose.position),
Quaternion2list(place_pose.pose.orientation),
rospy.Time.now(),
"place_pose",
self.robot.base)
self.robot.place(place_pose)
# For cubelets:
place_pose.pose.position.z += 0.042
# For YCB:
# place_pose.pose.position.z += 0.026
self.place_pose = place_pose
def _pick(self):
# State not modified until picking succeeds
try:
obj_to_get = int(self.character)
except ValueError:
rospy.logerr("Please provide a number in picking mode")
return
frame_name = "object_pose_{}".format(obj_to_get)
rospy.loginfo("Picking object {}...".format(obj_to_get))
if self.tl.frameExists(self.robot.base) and self.tl.frameExists(frame_name):
t = self.tl.getLatestCommonTime(self.robot.base, frame_name)
position, quaternion = self.tl.lookupTransform(self.robot.base,
frame_name,
t)
print("position", position)
print("quaternion", quaternion)
position = list(position)
# Vertical orientation
self.br.sendTransform(position,
[1, 0, 0, 0],
rospy.Time.now(),
"pick_pose",
self.robot.base)
# Ignore orientation from perception
pose = Pose(Point(*position),
Quaternion(1, 0, 0, 0))
h = Header()
h.stamp = t
h.frame_id = self.robot.base
stamped_pose = PoseStamped(h, pose)
self.robot.pick(stamped_pose)
self.state = 'postpick'
def _level(self):
self.robot.level = int(self.character)
self.state = 'level'
class CircleMarker(object):
def __init__(self):
self._server = InteractiveMarkerServer("simple_marker")
def callback_marker(self, input):
pass
def align_marker(self, feedback):
pose = feedback.pose
self._server.setPose(feedback.marker_name, pose)
print("Arrow Marker's name is ", feedback.marker_name)
self._server.setPose("orientation_ring", pose)
self._server.applyChanges()
def orientation_ring_callback(self, feedback):
if (feedback.event_type == InteractiveMarkerFeedback.POSE_UPDATE):
print("Updating arrow marker's position ")
self._server.setPose("int_arrow_marker", feedback.pose)
self._server.applyChanges()
def create_draggable_marker(self, position):
int_arrow_marker = InteractiveMarker()
int_arrow_marker.header.frame_id = "map"
int_arrow_marker.name = "int_arrow_marker"
int_arrow_marker.description = "right"
int_arrow_marker.pose.position.y = position.y
int_arrow_marker.pose.position.x = position.x
int_arrow_marker.pose.position.z = position.z + 0.1
int_arrow_marker.scale = 1
arrow_marker = Marker()
arrow_marker.ns = "arrow_marker"
arrow_marker.type = Marker.ARROW
arrow_marker.pose.orientation.w = 0
arrow_marker.scale.x = 0.5
arrow_marker.scale.y = 0.05
arrow_marker.scale.z = 0.05
arrow_marker.color.r = 0.0
arrow_marker.color.g = 0.5
arrow_marker.color.b = 0.5
arrow_marker.color.a = 1.0
button_control = InteractiveMarkerControl()
button_control.interaction_mode = InteractiveMarkerControl.MOVE_PLANE
button_control.always_visible = True
button_control.orientation.w = 1
button_control.orientation.x = 0
button_control.orientation.y = 1
button_control.orientation.z = 0
button_control.markers.append(arrow_marker)
int_arrow_marker.controls.append(button_control)
orientation_ring_marker = InteractiveMarker()
orientation_ring_marker.header.frame_id = "map"
orientation_ring_marker.scale = 1
orientation_ring_marker.pose.position.y = position.y
orientation_ring_marker.pose.position.z = position.z + 0.1
orientation_ring_marker.pose.position.x = position.x
orientation_ring_marker.name = "orientation_ring"
orientation_ring_marker.description = "Orientation Ring"
orientation_ring_marker_control = InteractiveMarkerControl()
orientation_ring_marker_control.always_visible = True
orientation_ring_marker_control.orientation.w = 1
orientation_ring_marker_control.orientation.x = 0
orientation_ring_marker_control.orientation.y = 1
orientation_ring_marker_control.orientation.z = 0
orientation_ring_marker_control.interaction_mode = InteractiveMarkerControl.MOVE_ROTATE
orientation_ring_marker.controls.append(orientation_ring_marker_control)
self._server.insert(orientation_ring_marker, self.orientation_ring_callback)
self._server.insert(int_arrow_marker, self.callback_marker)
print"inserted"
self._server.setCallback("int_arrow_marker", self.align_marker, InteractiveMarkerFeedback.POSE_UPDATE)
self._server.applyChanges()
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)
