00001 '''Defines behaviour for Grasp primitive.
00002 '''
00003
00004
00005
00006
00007
00008
00009 import rospy
00010
00011
00012 import tf
00013 from std_msgs.msg import ColorRGBA, String
00014 from geometry_msgs.msg import Vector3, Point, Pose, Quaternion, PoseStamped
00015 from sensor_msgs.msg import PointCloud2
00016 from visualization_msgs.msg import Marker, InteractiveMarker
00017 from visualization_msgs.msg import InteractiveMarkerControl
00018 from visualization_msgs.msg import InteractiveMarkerFeedback
00019 from interactive_markers.menu_handler import MenuHandler
00020 from rail_manipulation_msgs.srv import SuggestGrasps, SuggestGraspsRequest
00021 from rail_manipulation_msgs.msg import GraspFeedback
00022
00023
00024 from fetch_pbd_interaction.primitive import Primitive
00025 from fetch_arm_control.msg import GripperState
00026 from fetch_pbd_interaction.msg import ArmState, Landmark
00027 from fetch_pbd_interaction.srv import GetObjectList, \
00028 GetObjectFromName, GetMostSimilarObject
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038 COLOR_MESH_REACHABLE = ColorRGBA(0.0, 0.0, 1.0, 0.6)
00039 COLOR_MESH_REACHABLE_SELECTED = ColorRGBA(0.0, 1.0, 0.0, 1.0)
00040
00041 COLOR_MESH_UNREACHABLE = ColorRGBA(0.5, 0.5, 0.5, 0.7)
00042 COLOR_MESH_UNREACHABLE_SELECTED = ColorRGBA(0.5, 0.9, 0.5, 0.4)
00043
00044
00045 SCALE_TRAJ_STEP_SPHERES = Vector3(0.02, 0.02, 0.02)
00046 SCALE_OBJ_REF_ARROW = Vector3(0.02, 0.03, 0.04)
00047
00048
00049 STR_MESH_GRIPPER_FOLDER = 'package://fetch_description/meshes/'
00050 STR_GRIPPER_PALM_FILE = STR_MESH_GRIPPER_FOLDER + 'gripper_link.STL'
00051 STR_L_GRIPPER_FINGER_FILE = STR_MESH_GRIPPER_FOLDER + \
00052 'l_gripper_finger_link.STL'
00053 STR_R_GRIPPER_FINGER_FILE = STR_MESH_GRIPPER_FOLDER + \
00054 'r_gripper_finger_link.STL'
00055 DEFAULT_OFFSET = 0.085
00056
00057
00058 MENU_OPTIONS = {
00059 'ref': 'Change target object:',
00060
00061
00062 'del': 'Delete',
00063 'regen': 'Regenerate grasps',
00064 'gen' : 'Generate grasps',
00065 'choice' : 'Switch grasp to:'
00066
00067 }
00068
00069
00070
00071
00072
00073 ID_OFFSET_REF_ARROW = 1000
00074 ID_OFFSET_TRAJ_FIRST = 2000
00075 ID_OFFSET_TRAJ_LAST = 3000
00076
00077
00078 TRAJ_MARKER_LIFETIME = rospy.Duration()
00079 TEXT_Z_OFFSET = 0.1
00080 INT_MARKER_SCALE = 0.2
00081 TRAJ_ENDPOINT_SCALE = 0.05
00082
00083
00084
00085
00086 TOPIC_IM_SERVER = 'programmed_actions'
00087
00088
00089
00090 BASE_LINK = 'base_link'
00091 PREVIOUS_PRIMITIVE = "previous primitive"
00092 EE_LINK = 'wrist_roll_link'
00093
00094
00095
00096
00097
00098 class Grasp(Primitive):
00099 '''Defines behaviour for Grasp primitive. This is for primitives
00100 where the arm moves to a single pose.
00101 '''
00102
00103 _offset = DEFAULT_OFFSET
00104
00105 def __init__(self, robot, tf_listener, im_server,
00106 grasp_suggestion_service_name=None,
00107 grasp_feedback_topic=None,
00108 external_ee_link=None,
00109 landmark=None, number=None):
00110 '''
00111 Args:
00112 robot (Robot) : interface to lower level robot functionality
00113 tf_listener (TransformListener)
00114 im_server (InteractiveMarkerSerever)
00115 arm_state (ArmState, optional)
00116 gripper_state (GripperState.OPEN|GripperState.CLOSED, optional)
00117 number (int, optional): The number of this primitive in the
00118 action sequence
00119 '''
00120 self._name = ''
00121 self._im_server = im_server
00122 self._robot = robot
00123 self._number = number
00124
00125 self._head_busy = False
00126 self._external_ee_link = external_ee_link
00127 self._is_control_visible = False
00128 self._selected = False
00129 self._tf_listener = tf_listener
00130 self._marker_visible = False
00131 self._color_mesh_reachable = COLOR_MESH_REACHABLE
00132 self._color_mesh_unreachable = COLOR_MESH_UNREACHABLE
00133 self._grasp_reachable = False
00134 self._pre_grasp_reachable = False
00135 self._grasp_state = ArmState()
00136 self._grasp_state.ref_landmark = landmark
00137 self._grasp_state.ref_type = ArmState.OBJECT
00138 self._pre_grasp_state = ArmState()
00139 self._pre_grasp_state.ref_landmark = landmark
00140 self._pre_grasp_state.ref_type = ArmState.OBJECT
00141 self._current_grasp_num = None
00142 self._current_grasp_list = []
00143 self._approach_dist = 0.1
00144 self._landmark_found = False
00145
00146 self._menu_handler = MenuHandler()
00147
00148 self._sub_entries = None
00149 self._grasp_menu_entries = None
00150 self._marker_click_cb = None
00151 self._marker_delete_cb = None
00152 self._pose_change_cb = None
00153 self._action_change_cb = None
00154 self._viewed_grasps = []
00155
00156 self._get_object_from_name_srv = rospy.ServiceProxy(
00157 '/fetch_pbd/get_object_from_name',
00158 GetObjectFromName)
00159 self._get_most_similar_obj_srv = rospy.ServiceProxy(
00160 '/fetch_pbd/get_most_similar_object',
00161 GetMostSimilarObject)
00162 self._get_object_list_srv = rospy.ServiceProxy(
00163 '/fetch_pbd/get_object_list',
00164 GetObjectList)
00165 self._grasp_suggestion_srv = \
00166 rospy.ServiceProxy(grasp_suggestion_service_name, SuggestGrasps)
00167 if grasp_feedback_topic != "":
00168 self._grasp_feedback_publisher = rospy.Publisher(grasp_feedback_topic,
00169 GraspFeedback,
00170 queue_size=10,
00171 latch=True)
00172 else:
00173 self._grasp_feedback_publisher = None
00174
00175
00176
00177
00178
00179 def get_json(self):
00180 '''Returns json representation of primitive'''
00181
00182 json = {}
00183
00184 json['name'] = self._name
00185 json['number'] = self._number
00186 json['grasp_state'] = Grasp._get_json_from_arm_state(self._grasp_state)
00187 json['pre_grasp_state'] = Grasp._get_json_from_arm_state(
00188 self._pre_grasp_state)
00189
00190 return {'grasp': json}
00191
00192 def build_from_pose(self, pose_stamped, landmark,
00193 approach_dist=0.1, name=''):
00194 '''Fills out Grasp using information from David's grasp suggestion'''
00195 rospy.loginfo("Building from pose: {}".format(pose_stamped))
00196
00197 self._name = name
00198 self._grasp_state.ref_type = ArmState.OBJECT
00199 new_pose = self._tf_listener.transformPose(landmark.name,
00200 pose_stamped)
00201 self._grasp_state.ee_pose = new_pose
00202 self._grasp_state.ref_landmark = landmark
00203
00204 self._approach_dist = approach_dist
00205
00206
00207
00208
00209
00210 self._set_pre_grasp_state_from_pose(new_pose)
00211
00212 self._pre_grasp_state.ref_type = ArmState.OBJECT
00213
00214 self._pre_grasp_state.ref_landmark = landmark
00215
00216 def build_from_json(self, json):
00217 '''Fills out Grasp using json from db'''
00218
00219 rospy.loginfo("Json: {}".format(json))
00220
00221 self._name = json['name']
00222 self._number = json['number']
00223 grasp_state_json = json['grasp_state']
00224 pre_grasp_state_json = json['pre_grasp_state']
00225
00226
00227 self._grasp_state = ArmState()
00228 self._pre_grasp_state = ArmState()
00229 self._grasp_state.ref_type = grasp_state_json['ref_type']
00230 self._pre_grasp_state.ref_type = pre_grasp_state_json['ref_type']
00231 self._grasp_state.joint_pose = grasp_state_json['joint_pose']
00232 self._pre_grasp_state.joint_pose = pre_grasp_state_json['joint_pose']
00233 grasp_pose = Grasp._get_pose_stamped_from_json(
00234 grasp_state_json['ee_pose'])
00235 pre_grasp_pose = Grasp._get_pose_stamped_from_json(
00236 pre_grasp_state_json['ee_pose'])
00237 self._grasp_state.ee_pose = grasp_pose
00238 self._pre_grasp_state.ee_pose = pre_grasp_pose
00239
00240 self._grasp_state.ref_landmark = Landmark()
00241 self._pre_grasp_state.ref_landmark = Landmark()
00242 landmark_name = grasp_state_json['ref_landmark']['name']
00243 self._grasp_state.ref_landmark.name = landmark_name
00244 self._pre_grasp_state.ref_landmark.name = landmark_name
00245
00246 landmark_pose_json = grasp_state_json['ref_landmark']['pose']
00247 landmark_pose = Grasp._get_pose_from_json(landmark_pose_json)
00248 self._grasp_state.ref_landmark.pose = landmark_pose
00249 self._pre_grasp_state.ref_landmark.pose = landmark_pose
00250
00251 landmark_dimensions = Vector3()
00252 x_dim = grasp_state_json['ref_landmark']['dimensions']['x']
00253 y_dim = grasp_state_json['ref_landmark']['dimensions']['y']
00254 z_dim = grasp_state_json['ref_landmark']['dimensions']['z']
00255 landmark_dimensions.x = x_dim
00256 landmark_dimensions.y = y_dim
00257 landmark_dimensions.z = z_dim
00258
00259
00260 self._grasp_state.ref_landmark.dimensions = landmark_dimensions
00261 self._pre_grasp_state.ref_landmark.dimensions = landmark_dimensions
00262
00263 def check_pre_condition(self):
00264 ''' Currently just a placeholder
00265 Meant to return conditions that need to be met before a
00266 primitive can be executed. This could be something like
00267 "There should only be one object" or something.
00268
00269 Returns:
00270 None
00271 '''
00272 if self._grasp_state.ref_type == ArmState.OBJECT:
00273 if not self._landmark_found:
00274 return False, "No matching object found" + \
00275 " for primitive: {}".format(self.get_name())
00276 if self._current_grasp_num is None:
00277 msg = "Cannot execute action." + \
00278 " No grasp chosen. Right-click the blue" + \
00279 " grasp marker to generate grasp options."
00280 return False, msg
00281 else:
00282 return True, None
00283
00284 def check_post_condition(self):
00285 ''' Currently just a placeholder
00286 Meant to return conditions that need to be met after a
00287 primitive is executed in order for execution to be a success.
00288 This could be something like "object must be 0.1m above surface"
00289
00290 Returns:
00291 None
00292 '''
00293
00294 return True, None
00295
00296 def add_marker_callbacks(self, click_cb, delete_cb, pose_change_cb,
00297 action_change_cb):
00298 '''Adds marker to world'''
00299
00300
00301 self._marker_click_cb = click_cb
00302 self._marker_delete_cb = delete_cb
00303 self._pose_change_cb = pose_change_cb
00304 self._action_change_cb = action_change_cb
00305
00306
00307 def show_marker(self):
00308 '''Adds marker for primitive'''
00309 rospy.loginfo("Showing marker")
00310 self._marker_visible = False
00311 if self.update_ref_frames():
00312 try:
00313 self._update_menu()
00314 self._update_viz_core()
00315 self._menu_handler.apply(self._im_server, self.get_name())
00316 self._im_server.applyChanges()
00317 self._marker_visible = True
00318 except Exception, e:
00319 rospy.logwarn(e)
00320
00321 return self._marker_visible
00322
00323 def hide_marker(self):
00324 '''Removes marker from the world.'''
00325
00326 rospy.loginfo("Deleting marker for: {}".format(self.get_name()))
00327 self._im_server.erase(self.get_name())
00328 self._im_server.applyChanges()
00329 self._marker_visible = False
00330
00331 def marker_visible(self):
00332 '''Return whether or not marker is visible
00333
00334 Returns:
00335 bool
00336 '''
00337 return self._marker_visible
00338
00339 def update_ref_frames(self):
00340 '''Updates and re-assigns coordinate frames when the world changes.'''
00341
00342 rospy.loginfo("Updating primitive reference frames")
00343 if self._grasp_state.ref_type == ArmState.OBJECT:
00344 prev_ref_obj = self._grasp_state.ref_landmark
00345 resp = self._get_most_similar_obj_srv(prev_ref_obj)
00346 if resp.has_similar:
00347 self._grasp_state.ref_landmark = resp.similar_object
00348 self._pre_grasp_state.ref_landmark = resp.similar_object
00349 self._grasp_state.ee_pose.header.frame_id = \
00350 resp.similar_object.name
00351 self._pre_grasp_state.ee_pose.header.frame_id = \
00352 resp.similar_object.name
00353 rospy.loginfo("Found similar")
00354 self._landmark_found = True
00355 return True
00356 else:
00357 self._landmark_found = False
00358 return False
00359 else:
00360 rospy.logwarn("Grasp has non-OBJECT-type reference frame")
00361 return False
00362
00363 def change_ref_frame(self, ref_type, landmark):
00364 '''Sets new reference frame for primitive
00365
00366 Args:
00367
00368 '''
00369 self._grasp_state.ref_type = ref_type
00370 self._pre_grasp_state.ref_type = ref_type
00371
00372 self._convert_ref_frame(landmark)
00373 rospy.loginfo(
00374 "Switching reference frame for primitive " +
00375 self.get_name())
00376 self._menu_handler.reApply(self._im_server)
00377 self.update_viz(False)
00378 self._action_change_cb()
00379 self._im_server.applyChanges()
00380
00381 def get_ref_frame_name(self):
00382 '''Returns the name string for the reference frame object of the
00383 primitive.
00384
00385 Returns:
00386 str|None: Under all normal circumstances, returns the str
00387 reference frame name. Returns None in error.
00388 '''
00389
00390
00391 ref_name = self._grasp_state.ref_landmark.name
00392 rospy.loginfo("Ref frame name: {}".format(ref_name))
00393
00394
00395
00396
00397
00398
00399
00400
00401
00402
00403 return ref_name
00404
00405 def select(self, is_selected):
00406 '''Set whether primitive is selected or not
00407
00408 Args:
00409 is_selected (bool)
00410 '''
00411 rospy.loginfo("Selecting primitive: {}".format(self.get_number()))
00412 self._selected = is_selected
00413 self.set_control_visible(is_selected)
00414 if is_selected:
00415 self._color_mesh_reachable = COLOR_MESH_REACHABLE_SELECTED
00416 self._color_mesh_unreachable = COLOR_MESH_UNREACHABLE_SELECTED
00417 else:
00418 self._color_mesh_reachable = COLOR_MESH_REACHABLE
00419 self._color_mesh_unreachable = COLOR_MESH_UNREACHABLE
00420
00421 self.update_viz(False)
00422
00423 def is_selected(self):
00424 '''Return whether or not primitive is selected
00425
00426 Returns
00427 bool
00428 '''
00429 return self._selected
00430
00431 def is_control_visible(self):
00432 '''Check if the marker control is visible
00433
00434 Returns
00435 bool
00436 '''
00437 return self._is_control_visible
00438
00439 def set_control_visible(self, visible=True):
00440 '''Set visibility of marker controls
00441
00442 Args:
00443 visible (bool, optional)
00444 '''
00445 self._is_control_visible = visible
00446
00447 def update_viz(self, check_reachable=True):
00448 '''Updates visualization fully.
00449 Args:
00450 check_reachable (bool) : whether to evaluate reachability
00451 before drawing marker
00452 '''
00453 rospy.loginfo("Updating viz for: {}".format(self.get_name()))
00454 rospy.loginfo("ee pose header: {}".format(self._grasp_state.ee_pose.header.frame_id))
00455 draw_markers = True
00456 if self._grasp_state.ref_type == ArmState.OBJECT:
00457 landmark_name = self._grasp_state.ref_landmark.name
00458 resp = self._get_object_from_name_srv(landmark_name)
00459 if not resp.has_object:
00460 draw_markers = False
00461
00462 if draw_markers and self._marker_visible:
00463 try:
00464 self._update_menu()
00465
00466 if self._update_viz_core(check_reachable):
00467 self._menu_handler.apply(self._im_server, self.get_name())
00468 self._im_server.applyChanges()
00469 except Exception, e:
00470 rospy.logwarn(e)
00471
00472 def get_primitive_number(self):
00473 '''Returns what number this primitive is in the sequence
00474
00475 Returns:
00476 int
00477 '''
00478 return self._number
00479
00480 def set_primitive_number(self, number):
00481 '''Sets what number this primitive is in the sequence
00482
00483 Args:
00484 num (int)
00485 '''
00486 self._number = number
00487
00488 def is_object_required(self):
00489 '''Check if this primitive requires an object to be present
00490
00491 Returns:
00492 bool
00493 '''
00494 return True
00495
00496 def execute(self):
00497 '''Execute this primitive, assumes that gripper commands just work
00498
00499 Returns
00500 bool : Success of execution
00501 '''
00502 if self._current_grasp_num is None:
00503 msg = "Cannot execute action." + \
00504 " No grasp chosen. Right-click the blue" + \
00505 " grasp marker to generate grasp options."
00506 return False, msg
00507 else:
00508 rospy.loginfo("Executing grasp")
00509 if not self._robot.move_arm_to_pose(self._pre_grasp_state):
00510 return False, "Problem finding IK solution"
00511 if not self._robot.get_gripper_state() == GripperState.OPEN:
00512 self._robot.set_gripper_state(GripperState.OPEN)
00513
00514 if not self._robot.move_arm_to_pose(self._grasp_state):
00515 return False, "Problem finding IK solution"
00516 if not self._robot.get_gripper_state() == GripperState.CLOSED:
00517 self._robot.set_gripper_state(GripperState.CLOSED)
00518 feedback_msg = GraspFeedback()
00519 feedback_msg.indices_considered = self._viewed_grasps
00520 feedback_msg.index_selected = self._current_grasp_num
00521 if not self._grasp_feedback_publisher is None:
00522 self._grasp_feedback_publisher.publish(feedback_msg)
00523 return True, None
00524
00525 def head_busy(self):
00526 '''Return true if head busy
00527
00528 Returns:
00529 bool
00530 '''
00531 return self._head_busy
00532
00533 def is_reachable(self):
00534 '''Check if robot can physically reach target'''
00535 self._grasp_reachable = self._robot.can_reach(self._grasp_state)
00536 self._pre_grasp_reachable = self._robot.can_reach(self._grasp_state)
00537 if not self._grasp_reachable:
00538 rospy.logwarn("Grasp not reachable")
00539 if not self._pre_grasp_reachable:
00540 rospy.logwarn("Pre-grasp not reachable")
00541 return self._grasp_reachable and self._pre_grasp_reachable
00542
00543 def get_relative_pose(self, use_final=True):
00544 '''Returns the absolute pose of the primitive.
00545 Args:
00546 use_final (bool, optional). If true, uses pose of grasp_state
00547 rather than pre_grasp_state
00548 Returns:
00549 PoseStamped
00550 '''
00551 if self._current_grasp_num is None:
00552 return PoseStamped(
00553 header=self._grasp_state.ref_landmark.point_cloud.header,
00554 pose=self._grasp_state.ref_landmark.pose)
00555 else:
00556 if use_final:
00557 return self._grasp_state.ee_pose
00558 else:
00559 return self._pre_grasp_state.ee_pose
00560
00561 def get_absolute_pose(self):
00562 '''Returns the absolute pose of the grasp part of the primitive
00563 (not the pre-grasp).
00564
00565 Args:
00566 None
00567
00568 Returns:
00569 PoseStamped
00570 '''
00571 if self._current_grasp_num is None:
00572 try:
00573 pose = PoseStamped(
00574 header=self._grasp_state.ref_landmark.point_cloud.header,
00575 pose=self._grasp_state.ref_landmark.pose)
00576 abs_pose = self._tf_listener.transformPose('base_link',
00577 pose)
00578 return abs_pose
00579 except Exception, e:
00580 landmark = self._grasp_state.ref_landmark
00581 frame_id = landmark.point_cloud.header.frame_id
00582 rospy.logwarn("Frame: {} does not exist".format(frame_id))
00583 rospy.logwarn(str(e))
00584 return None
00585 else:
00586 try:
00587 abs_pose = self._tf_listener.transformPose('base_link',
00588 self._grasp_state.ee_pose)
00589 return abs_pose
00590 except Exception, e:
00591 frame_id = self._grasp_state.ee_pose.header.frame_id
00592 rospy.logwarn("Frame: {} does not exist".format(frame_id))
00593 rospy.logwarn(str(e))
00594 return None
00595
00596 def get_absolute_marker_pose(self, use_final=True):
00597 '''Returns the absolute pose of the primitive marker.
00598
00599 Args:
00600 use_final (bool, optional). Unused
00601
00602 Returns:
00603 PoseStamped
00604 '''
00605 if self._current_grasp_num is None:
00606 try:
00607 height = self._grasp_state.ref_landmark.point_cloud.height
00608 width = self._grasp_state.ref_landmark.point_cloud.width
00609 if height * width == 0:
00610 return None
00611 pose = PoseStamped(
00612 header=self._grasp_state.ref_landmark.point_cloud.header,
00613 pose=self._grasp_state.ref_landmark.pose)
00614 abs_pose = self._tf_listener.transformPose('base_link',
00615 pose)
00616 return abs_pose
00617 except Exception, e:
00618 landmark = self._grasp_state.ref_landmark
00619 frame_id = landmark.point_cloud.header.frame_id
00620 rospy.logwarn("Frame: {} does not exist".format(frame_id))
00621 rospy.logwarn(str(e))
00622 return None
00623 else:
00624 try:
00625 if use_final:
00626 pose_to_use = self._grasp_state.ee_pose
00627 else:
00628 pose_to_use = self._pre_grasp_state.ee_pose
00629 abs_pose = self._tf_listener.transformPose('base_link',
00630 pose_to_use)
00631 return Grasp._offset_pose(abs_pose)
00632 except Exception, e:
00633 if use_final:
00634 pose_to_use = self._grasp_state.ee_pose
00635 else:
00636 pose_to_use = self._pre_grasp_state.ee_pose
00637 frame_id = pose_to_use.pose.header.frame_id
00638 rospy.logwarn("Frame: {} does not exist".format(frame_id))
00639 rospy.logwarn(str(e))
00640 return None
00641
00642 def get_absolute_marker_position(self, use_final=True):
00643 '''Returns the absolute position of the primitive marker.
00644
00645 Args:
00646 use_final (bool, optional) : Unused
00647 True.
00648
00649 Returns:
00650 Point
00651 '''
00652 abs_pose = self.get_absolute_marker_pose(use_final)
00653 if not abs_pose is None:
00654 return abs_pose.pose.position
00655 else:
00656 return None
00657
00658 def decrease_id(self):
00659 '''Reduces the number of the primitive.'''
00660 self._number -= 1
00661
00662 def set_name(self, name):
00663 '''Sets the display name for the primitive.
00664
00665 Args:
00666 name (str) : A human-readable unique name for the primitive.
00667 '''
00668 self._name = name
00669
00670 def get_name(self):
00671 '''Returns the display name for the primitive.
00672
00673 Returns:
00674 str: A human-readable unique name for the primitive.
00675 '''
00676 return self._name
00677
00678 def get_number(self):
00679 '''Returns number of primitive
00680
00681 Returns:
00682 int
00683 '''
00684 return self._number
00685
00686 def set_pose(self, new_pose):
00687 '''Changes the pose of the primitive to new_pose.
00688
00689 Args:
00690 new_pose (PoseStamped)
00691 '''
00692 rospy.loginfo("Setting new ee pose")
00693 self._grasp_state.ee_pose = new_pose
00694 self._set_pre_grasp_state_from_pose(new_pose)
00695 self.update_viz()
00696 self._action_change_cb()
00697
00698 def pose_editable(self):
00699 '''Return whether pose of primitive is editable
00700
00701 Returns:
00702 bool : True
00703 '''
00704 return False
00705
00706 def get_ref_type(self):
00707 '''Return reference type of primitive
00708
00709 Returns:
00710 ArmState.ROBOT_BASE, etc
00711 '''
00712 return ArmState.OBJECT
00713
00714
00715
00716
00717
00718 @staticmethod
00719 def _get_json_from_arm_state(arm_state):
00720 '''Return json containing data of arm_state
00721
00722 Args:
00723 arm_state (ArmState)
00724 Returns:
00725 json (dict)
00726 '''
00727 json = {}
00728 json['ref_type'] = arm_state.ref_type
00729 json['joint_pose'] = arm_state.joint_pose
00730 pose = Grasp._get_json_from_pose_stamped(arm_state.ee_pose)
00731 json['ee_pose'] = pose
00732 landmark = arm_state.ref_landmark
00733 json['ref_landmark'] = Grasp._get_json_from_landmark(landmark)
00734 return json
00735
00736 @staticmethod
00737 def _get_json_from_pose_stamped(pose_stamped):
00738 '''Return json containing data of pose_stamped
00739
00740 Args:
00741 pose_stamped (PoseStamped)
00742 Returns:
00743 json (dict)
00744 '''
00745 json = {}
00746 json['pose'] = Grasp._get_json_from_pose(pose_stamped.pose)
00747 json['frame_id'] = pose_stamped.header.frame_id
00748
00749 return json
00750
00751 @staticmethod
00752 def _get_json_from_pose(pose):
00753 '''Return json containing data of pose
00754
00755 Args:
00756 pose (Pose)
00757 Returns:
00758 json (dict)
00759 '''
00760 json = {}
00761 json['position'] = {}
00762 json['position']['x'] = pose.position.x
00763 json['position']['y'] = pose.position.y
00764 json['position']['z'] = pose.position.z
00765
00766 json['orientation'] = {}
00767 json['orientation']['x'] = pose.orientation.x
00768 json['orientation']['y'] = pose.orientation.y
00769 json['orientation']['z'] = pose.orientation.z
00770 json['orientation']['w'] = pose.orientation.w
00771
00772 return json
00773
00774 @staticmethod
00775 def _get_json_from_landmark(landmark):
00776 '''Return json containing data of landmark
00777
00778 Args:
00779 landmark (Landmark)
00780 Returns:
00781 json (dict)
00782 '''
00783 json = {}
00784
00785 json['name'] = landmark.name
00786 json['pose'] = Grasp._get_json_from_pose(landmark.pose)
00787 json['dimensions'] = {}
00788 json['dimensions']['x'] = landmark.dimensions.x
00789 json['dimensions']['y'] = landmark.dimensions.y
00790 json['dimensions']['z'] = landmark.dimensions.z
00791
00792 return json
00793
00794 @staticmethod
00795 def _get_pose_stamped_from_json(json):
00796 '''Return PoseStamped msg from json
00797
00798 Args:
00799 json (dict)
00800 Returns:
00801 PoseStamped
00802 '''
00803 pose_stamped = PoseStamped()
00804 pose = Grasp._get_pose_from_json(json['pose'])
00805 pose_stamped.pose = pose
00806 pose_stamped.header.frame_id = json['frame_id']
00807
00808 return pose_stamped
00809
00810 @staticmethod
00811 def _get_pose_from_json(json):
00812 '''Return Pose msg from json
00813
00814 Args:
00815 json (dict)
00816 Returns:
00817 Pose
00818 '''
00819 pose = Pose()
00820
00821 pose.position.x = json['position']['x']
00822 pose.position.y = json['position']['y']
00823 pose.position.z = json['position']['z']
00824
00825 pose.orientation.x = json['orientation']['x']
00826 pose.orientation.y = json['orientation']['y']
00827 pose.orientation.z = json['orientation']['z']
00828 pose.orientation.w = json['orientation']['w']
00829
00830 return pose
00831
00832 @staticmethod
00833 def _offset_pose(pose, constant=1):
00834 '''Offsets the world pose for visualization.
00835
00836 Args:
00837 pose (PoseStamped): The pose to offset.
00838 constant (int, optional): How much to scale the set offset
00839 by (scales Grasp._offset). Defaults to 1.
00840
00841 Returns:
00842 PoseStamped: The offset pose.
00843 '''
00844 transform = Grasp._get_matrix_from_pose(pose)
00845 offset_array = [constant * Grasp._offset, 0, 0]
00846 offset_transform = tf.transformations.translation_matrix(offset_array)
00847 hand_transform = tf.transformations.concatenate_matrices(
00848 transform, offset_transform)
00849
00850 new_pose = PoseStamped()
00851 new_pose.header.frame_id = pose.header.frame_id
00852 new_pose.pose = Grasp._get_pose_from_transform(hand_transform)
00853 return new_pose
00854
00855 @staticmethod
00856 def _get_matrix_from_pose(pose):
00857 '''Returns the transformation matrix for given pose.
00858
00859 Args:
00860 pose (PoseStamped)
00861
00862 Returns:
00863 Matrix3x3
00864 '''
00865 position, orientation = pose.pose.position, pose.pose.orientation
00866 rot_list = [orientation.x, orientation.y, orientation.z, orientation.w]
00867 transformation = tf.transformations.quaternion_matrix(rot_list)
00868 pos_list = [position.x, position.y, position.z]
00869 transformation[:3, 3] = pos_list
00870 return transformation
00871
00872 @staticmethod
00873 def _get_pose_from_transform(transform):
00874 '''Returns pose for transformation matrix.
00875
00876 Args:
00877 transform (Matrix3x3): (I think this is the correct type.
00878 See Grasp as a reference for how to use.)
00879
00880 Returns:
00881 Pose
00882 '''
00883 pos = transform[:3, 3].copy()
00884 rot = tf.transformations.quaternion_from_matrix(transform)
00885 return Pose(
00886 Point(pos[0], pos[1], pos[2]),
00887 Quaternion(rot[0], rot[1], rot[2], rot[3])
00888 )
00889
00890 @staticmethod
00891 def _make_mesh_marker(color):
00892 '''Creates and returns a mesh marker.
00893
00894 Returns:
00895 Marker
00896 '''
00897 mesh = Marker()
00898 mesh.mesh_use_embedded_materials = False
00899 mesh.type = Marker.MESH_RESOURCE
00900 mesh.scale.x = 1.0
00901 mesh.scale.y = 1.0
00902 mesh.scale.z = 1.0
00903 mesh.color = color
00904 return mesh
00905
00906
00907
00908
00909 def _generate_grasps_cb(self, feedback):
00910 '''Callback for when users want to generate grasps for an object'''
00911 self._suggest_grasps(self._grasp_state.ref_landmark)
00912
00913
00914 def _switch_grasp_cb(self, feedback):
00915 '''Callback to switch between grasps indicated by menu entries'''
00916 menu_id = feedback.menu_entry_id
00917 self._current_grasp_num = self._grasp_menu_entries.index(menu_id)
00918 self._viewed_grasps.append(self._current_grasp_num)
00919 grasp_pose = self._current_grasp_list[self._current_grasp_num]
00920 self.build_from_pose(grasp_pose,
00921 self._grasp_state.ref_landmark,
00922 name=self.get_name())
00923 self._pose_change_cb()
00924
00925
00926 def _suggest_grasps(self, landmark):
00927 '''Call grasp suggestion service and process results'''
00928 self._head_busy = True
00929 rospy.loginfo("Getting grasps")
00930 self._robot.look_down()
00931 req = SuggestGraspsRequest()
00932 req.cloud = landmark.point_cloud
00933 rospy.loginfo("Point cloud size: {}".format(
00934 req.cloud.height*req.cloud.width))
00935 rospy.loginfo("PC header: {}".format(landmark.point_cloud.header))
00936 resp = self._grasp_suggestion_srv(landmark.point_cloud)
00937 grasps = resp.grasp_list
00938 if not len(grasps.poses) > 0:
00939 rospy.logwarn("No grasps received")
00940 self._robot.look_forward()
00941 self._head_busy = False
00942 return False
00943 else:
00944 self._current_grasp_list = \
00945 [PoseStamped(header=grasps.header, pose=p) \
00946 for p in grasps.poses]
00947 self._change_grasp_frames(EE_LINK)
00948 pose_stamped = self._current_grasp_list[0]
00949 self.build_from_pose(pose_stamped,
00950 landmark,
00951 name=self.get_name())
00952 rospy.loginfo("Current grasps: {}".format(
00953 len(self._current_grasp_list)))
00954 self._current_grasp_num = 0
00955 self._pose_change_cb()
00956 self._head_busy = False
00957 self._robot.look_forward()
00958 self._viewed_grasps.append(0)
00959 return True
00960
00961 def _change_grasp_frames(self, target_frame):
00962 '''Change frames of grasps in self._current_grasp_list'''
00963
00964 if not self._current_grasp_list:
00965 return
00966 else:
00967 pose_frame = self._external_ee_link
00968 self._tf_listener.waitForTransform(pose_frame,
00969 target_frame,
00970 rospy.Time(0),
00971 rospy.Duration(4.0))
00972 (trans_diff, rot_diff) = \
00973 self._tf_listener.lookupTransform(pose_frame,
00974 target_frame,
00975 rospy.Time(0))
00976 new_list = []
00977
00978 for pose in self._current_grasp_list:
00979
00980 rot_mat = Grasp._get_matrix_from_pose(pose)
00981 x_axis = Vector3(rot_mat[0, 0], rot_mat[1, 0], rot_mat[2, 0])
00982 y_axis = Vector3(rot_mat[0, 1], rot_mat[1, 1], rot_mat[2, 1])
00983 z_axis = Vector3(rot_mat[0, 2], rot_mat[1, 2], rot_mat[2, 2])
00984
00985 pose_temp = PoseStamped()
00986 pose_temp.header.frame_id = BASE_LINK
00987 pose_temp.pose.position.x = pose.pose.position.x + \
00988 x_axis.x*trans_diff[0] + \
00989 y_axis.x*trans_diff[1] + \
00990 z_axis.x*trans_diff[2]
00991 pose_temp.pose.position.y = pose.pose.position.y + \
00992 x_axis.y*trans_diff[0] + \
00993 y_axis.y*trans_diff[1] + \
00994 z_axis.y*trans_diff[2]
00995 pose_temp.pose.position.z = pose.pose.position.z + \
00996 x_axis.z*trans_diff[0] + \
00997 y_axis.z*trans_diff[1] + \
00998 z_axis.z*trans_diff[2]
00999
01000 q = [pose.pose.orientation.x, pose.pose.orientation.y,
01001 pose.pose.orientation.z, pose.pose.orientation.w]
01002 q_temp = tf.transformations.quaternion_multiply(q, rot_diff)
01003 pose_temp.pose.orientation.x = q_temp[0]
01004 pose_temp.pose.orientation.y = q_temp[1]
01005 pose_temp.pose.orientation.z = q_temp[2]
01006 pose_temp.pose.orientation.w = q_temp[3]
01007 rospy.loginfo("Pose: {}".format(pose))
01008 rospy.loginfo("Pose temp: {}".format(pose_temp))
01009 new_list.append(pose_temp)
01010 self._current_grasp_list = new_list
01011
01012 def _update_menu(self):
01013 '''Recreates the menu when something has changed.'''
01014
01015 rospy.loginfo("Making new menu")
01016 self._menu_handler = MenuHandler()
01017
01018
01019 self._sub_entries = []
01020 frame_entry = self._menu_handler.insert(MENU_OPTIONS['ref'])
01021 object_list = self._get_object_list_srv().object_list
01022 refs = [obj.name for obj in object_list]
01023 for ref in refs:
01024 subent = self._menu_handler.insert(
01025 ref, parent=frame_entry, callback=self._change_ref_cb)
01026 self._sub_entries += [subent]
01027
01028
01029 self._menu_handler.insert(
01030 MENU_OPTIONS['del'], callback=self._delete_primitive_cb)
01031
01032 self._grasp_menu_entries = []
01033 if self._current_grasp_list:
01034 self._menu_handler.insert(
01035 MENU_OPTIONS['regen'], callback=self._regenerate_grasps_cb)
01036 grasp_choice_entry = self._menu_handler.insert(
01037 MENU_OPTIONS['choice'])
01038 for i in range(len(self._current_grasp_list)):
01039 grasp_ent = self._menu_handler.insert("grasp_" + str(i),
01040 parent=grasp_choice_entry,
01041 callback=self._switch_grasp_cb)
01042 self._grasp_menu_entries += [grasp_ent]
01043
01044 for grasp_ent in self._grasp_menu_entries:
01045 self._menu_handler.setCheckState(grasp_ent,
01046 MenuHandler.UNCHECKED)
01047 if not self._current_grasp_num is None:
01048 self._menu_handler.setCheckState(
01049 self._grasp_menu_entries[self._current_grasp_num],
01050 MenuHandler.CHECKED)
01051
01052 else:
01053 self._menu_handler.insert(
01054 MENU_OPTIONS['gen'], callback=self._generate_grasps_cb)
01055
01056
01057
01058 for subent in self._sub_entries:
01059 self._menu_handler.setCheckState(subent, MenuHandler.UNCHECKED)
01060
01061
01062 menu_id = self._get_menu_id(self._get_menu_ref())
01063 if not menu_id is None:
01064
01065 self._menu_handler.setCheckState(menu_id, MenuHandler.CHECKED)
01066
01067 def _get_menu_id(self, ref_name):
01068 '''Returns the unique menu id from its name or None if the
01069 object is not found.
01070
01071 Args:
01072 ref_name (str)
01073 Returns:
01074 int (?)|None
01075 '''
01076 object_list = self._get_object_list_srv().object_list
01077 refs = [obj.name for obj in object_list]
01078 if self._number > 0:
01079 refs.append(PREVIOUS_PRIMITIVE)
01080 refs.append(BASE_LINK)
01081 if ref_name in refs:
01082 index = refs.index(ref_name)
01083 if index < len(self._sub_entries):
01084 return self._sub_entries[index]
01085 else:
01086 return None
01087 else:
01088 return None
01089
01090 def _get_menu_name(self, menu_id):
01091 '''Returns the menu name from its unique menu id.
01092
01093 Args:
01094 menu_id (int)
01095 Returns:
01096 str
01097 '''
01098 index = self._sub_entries.index(menu_id)
01099 object_list = self._get_object_list_srv().object_list
01100 refs = [obj.name for obj in object_list]
01101 if self._number > 0:
01102 refs.append(PREVIOUS_PRIMITIVE)
01103 refs.append(BASE_LINK)
01104 return refs[index]
01105
01106 def _set_ref(self, new_ref):
01107 '''Changes the reference frame of the primitive to
01108 new_ref_name.
01109
01110 Args:
01111 new_ref_name
01112 '''
01113
01114 self._grasp_state.ref_type = ArmState.OBJECT
01115 self._pre_grasp_state.ref_type = ArmState.OBJECT
01116 new_ref_obj = self._get_object_from_name_srv(new_ref).obj
01117 rospy.loginfo("Setting reference of primitive" +
01118 "{} to object".format(self._number))
01119 self._grasp_state.ref_landmark = new_ref_obj
01120 self._pre_grasp_state.ref_landmark = new_ref_obj
01121 self._grasp_state.ee_pose.header.frame_id = new_ref_obj.name
01122 self._landmark_found = True
01123
01124 def _convert_ref_frame(self, new_landmark):
01125 '''Convert grasp_state and pre_grasp_state to be in a different
01126 reference frame
01127
01128 Args:
01129 new_landmark (Landmark)
01130 Returns:
01131 ArmState
01132 '''
01133 ee_pose = PoseStamped()
01134 if self._grasp_state.ref_type == ArmState.OBJECT:
01135 rospy.loginfo("Relative to object")
01136 if self._grasp_state.ref_landmark.name != new_landmark.name:
01137 ee_pose = self._tf_listener.transformPose(
01138 new_landmark.name,
01139 self._grasp_state.ee_pose
01140 )
01141 self._grasp_state.ref_landmark = new_landmark
01142 self._grasp_state.ee_pose = ee_pose
01143 self._pre_grasp_state.ref_landmark = new_landmark
01144 self._landmark_found = True
01145
01146 elif self._grasp_state.ref_type == ArmState.ROBOT_BASE:
01147 ee_pose = self._tf_listener.transformPose(
01148 BASE_LINK,
01149 self._grasp_state.ee_pose
01150 )
01151 self._grasp_state.ee_pose = ee_pose
01152 self._grasp_state.ref_landmark = Landmark()
01153 self._pre_grasp_state.ref_landmark = Landmark()
01154 self._landmark_found = False
01155
01156 elif self._grasp_state.ref_type == ArmState.PREVIOUS_TARGET:
01157 prev_frame_name = "primitive_" + str(self._number - 1)
01158 rospy.loginfo("Original pose: {}".format(self._grasp_state.ee_pose))
01159 ee_pose = self._tf_listener.transformPose(
01160 prev_frame_name,
01161 self._grasp_state.ee_pose
01162 )
01163 rospy.loginfo("New pose: {}".format(ee_pose))
01164
01165 self._grasp_state.ee_pose = ee_pose
01166 self._grasp_state.ref_landmark = Landmark()
01167 self._pre_grasp_state.ref_landmark = Landmark()
01168 self._landmark_found = False
01169
01170 self._set_pre_grasp_state_from_pose(ee_pose)
01171
01172 def _set_pre_grasp_state_from_pose(self, pose_stamped):
01173 '''Sets pre_grasp_state based on a pose_stamped msg'''
01174
01175 rot_mat = Grasp._get_matrix_from_pose(pose_stamped)
01176 x_axis = Vector3(rot_mat[0, 0], rot_mat[1, 0], rot_mat[2, 0])
01177 self._pre_grasp_state.ee_pose = PoseStamped()
01178 self._pre_grasp_state.ee_pose.header.frame_id = \
01179 pose_stamped.header.frame_id
01180 self._pre_grasp_state.ee_pose.pose.orientation.x = \
01181 pose_stamped.pose.orientation.x
01182 self._pre_grasp_state.ee_pose.pose.orientation.y = \
01183 pose_stamped.pose.orientation.y
01184 self._pre_grasp_state.ee_pose.pose.orientation.z = \
01185 pose_stamped.pose.orientation.z
01186 self._pre_grasp_state.ee_pose.pose.orientation.w = \
01187 pose_stamped.pose.orientation.w
01188 self._pre_grasp_state.ee_pose.pose.position.x = \
01189 pose_stamped.pose.position.x \
01190 - (x_axis.x * self._approach_dist)
01191 self._pre_grasp_state.ee_pose.pose.position.y = \
01192 pose_stamped.pose.position.y \
01193 - (x_axis.y * self._approach_dist)
01194 self._pre_grasp_state.ee_pose.pose.position.z = \
01195 pose_stamped.pose.position.z \
01196 - (x_axis.z * self._approach_dist)
01197
01198 def _get_marker_pose(self):
01199 '''Returns the pose of the marker for the primitive.
01200
01201 Returns:
01202 Pose
01203 '''
01204 rospy.loginfo("Grasp frame is: {}".format(self.get_ref_frame_name()))
01205 try:
01206 if self._current_grasp_num is None:
01207 base_pose = PoseStamped()
01208 base_pose.header.frame_id = self.get_ref_frame_name()
01209 base_pose.pose.orientation.w = 1.0
01210 self._tf_listener.waitForTransform(BASE_LINK,
01211 base_pose.header.frame_id,
01212 rospy.Time.now(),
01213 rospy.Duration(4.0))
01214 intermediate_pose = self._tf_listener.transformPose(
01215 BASE_LINK,
01216 base_pose)
01217 return intermediate_pose
01218 else:
01219 self._tf_listener.waitForTransform(BASE_LINK,
01220 self._grasp_state.ee_pose.header.frame_id,
01221 rospy.Time.now(),
01222 rospy.Duration(4.0))
01223 intermediate_pose = self._tf_listener.transformPose(
01224 BASE_LINK,
01225 self._grasp_state.ee_pose)
01226 offset_pose = Grasp._offset_pose(intermediate_pose)
01227
01228
01229 return offset_pose
01230 except Exception, e:
01231 rospy.logwarn(e)
01232 rospy.logwarn(
01233 "Frame not available yet: {}".format(self.get_ref_frame_name()))
01234 return None
01235
01236 def _update_viz_core(self, check_reachable=True):
01237 '''Updates visualization after a change.
01238
01239 Args:
01240 check_reachable (bool) : Check reachability of
01241 pose before drawing marker
01242 '''
01243
01244 menu_control = InteractiveMarkerControl()
01245 menu_control.interaction_mode = InteractiveMarkerControl.BUTTON
01246 menu_control.always_visible = True
01247 frame_id = BASE_LINK
01248 pose = self._get_marker_pose()
01249 if self._current_grasp_num is None:
01250 marker = Marker()
01251 marker.type = Marker.CUBE
01252 marker.action = Marker.ADD
01253 marker.scale = self._grasp_state.ref_landmark.dimensions
01254 marker.pose = Pose()
01255 marker.pose.orientation.w = 1.0
01256 if self._selected:
01257 marker.color = COLOR_MESH_REACHABLE_SELECTED
01258 else:
01259 marker.color = COLOR_MESH_REACHABLE
01260 menu_control.markers.append(marker)
01261 else:
01262 if pose is None:
01263 return
01264
01265 if check_reachable and not self._current_grasp_num is None:
01266 self.is_reachable()
01267
01268 menu_control = self._make_gripper_markers(
01269 menu_control)
01270
01271
01272 int_marker = InteractiveMarker()
01273 int_marker.name = self.get_name()
01274 int_marker.header.frame_id = frame_id
01275 int_marker.pose = pose.pose
01276 int_marker.scale = INT_MARKER_SCALE
01277
01278 rospy.loginfo("Marker name: {}".format(self.get_name()))
01279 int_marker.controls.append(menu_control)
01280 prev_marker = self._im_server.get(self.get_name())
01281 prev_color = None
01282 if not prev_marker is None:
01283 if len(prev_marker.controls) > 0:
01284 if len(prev_marker.controls[-1].markers) > 0:
01285 prev_color = prev_marker.controls[-1].markers[-1].color
01286 new_color = None
01287 if len(int_marker.controls) > 0:
01288 if len(int_marker.controls[-1].markers) > 0:
01289 new_color = int_marker.controls[-1].markers[-1].color
01290
01291 if not prev_marker:
01292 self._im_server.insert(
01293 int_marker, self._marker_feedback_cb)
01294 rospy.logwarn("Adding marker for primitive {}".format(self.get_number()))
01295 return True
01296 elif (prev_marker.pose != int_marker.pose) or (prev_color != new_color):
01297 rospy.loginfo("Updating marker")
01298 self._im_server.insert(
01299 int_marker, self._marker_feedback_cb)
01300 return True
01301
01302 rospy.logwarn("Not updating marker for primitive {}".format(self.get_number()))
01303 return False
01304 def _add_6dof_marker(self, int_marker, is_fixed):
01305 '''Adds a 6 DoF control marker to the interactive marker.
01306
01307 Args:
01308 int_marker (InteractiveMarker)
01309 is_fixed (bool): Looks like whether position is fixed (?).
01310 Currently only passed as True.
01311 '''
01312
01313 options = [
01314 ('rotate_x', Quaternion(1, 0, 0, 1), False),
01315 ('move_x', Quaternion(1, 0, 0, 1), True),
01316 ('rotate_z', Quaternion(0, 1, 0, 1), False),
01317 ('move_z', Quaternion(0, 1, 0, 1), True),
01318 ('rotate_y', Quaternion(0, 0, 1, 1), False),
01319 ('move_y', Quaternion(0, 0, 1, 1), True),
01320 ]
01321 for opt in options:
01322 name, orient, is_move = opt
01323 control = self._make_6dof_control(name, orient, is_move, is_fixed)
01324 int_marker.controls.append(control)
01325
01326 def _make_6dof_control(self, name, orientation, is_move, is_fixed):
01327 '''Creates and returns one component of the 6dof controller.
01328
01329 Args:
01330 name (str): Name for hte control
01331 orientation (Quaternion): How the control should be
01332 oriented.
01333 is_move (bool): Looks like whether the marker moves the
01334 object (?). Currently passed as True for moving markers,
01335 False for rotating ones.
01336 is_fixed (bool): Looks like whether position is fixed (?).
01337 Currently always passed as True.
01338
01339 Returns:
01340 InteractiveMarkerControl
01341 '''
01342 control = InteractiveMarkerControl()
01343 control.name = name
01344 control.orientation = orientation
01345 control.always_visible = False
01346 if self._is_control_visible:
01347 if is_move:
01348 control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
01349 else:
01350 control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
01351 else:
01352 control.interaction_mode = InteractiveMarkerControl.NONE
01353 if is_fixed:
01354 control.orientation_mode = InteractiveMarkerControl.FIXED
01355 return control
01356
01357 def _set_new_pose(self, new_pose, frame_id):
01358 '''Changes the pose of the primitive to new_pose.
01359
01360 Args:
01361 new_pose (Pose)
01362 '''
01363 rospy.loginfo("Setting new pose for grasp primitive")
01364 pose_stamped = PoseStamped()
01365 pose_stamped.header.frame_id = frame_id
01366 pose_stamped.pose = new_pose
01367 pose_stamped_transformed = self._tf_listener.transformPose(
01368 self.get_ref_frame_name(),
01369 pose_stamped)
01370 self._grasp_state.ee_pose = Grasp._offset_pose(
01371 pose_stamped_transformed,
01372 -1)
01373 self._set_pre_grasp_state_from_pose(self._grasp_state.ee_pose)
01374 self.update_viz()
01375
01376 def _make_gripper_markers(self, control):
01377 '''Makes a gripper marker, adds it to control, returns control.
01378
01379 Args:
01380 control (InteractiveMarkerControl): IM Control we're using.
01381 is_hand_open (bool, optional): Whether the gripper is open.
01382 Defaults to False (closed).
01383
01384 Returns:
01385 InteractiveMarkerControl: The passed control.
01386 '''
01387
01388 if self._grasp_reachable:
01389 grasp_mesh_color = self._color_mesh_reachable
01390 else:
01391 grasp_mesh_color = self._color_mesh_unreachable
01392
01393 if self._pre_grasp_reachable:
01394 pre_grasp_mesh_color = self._color_mesh_reachable
01395 else:
01396 pre_grasp_mesh_color = self._color_mesh_unreachable
01397
01398 rospy.loginfo("Mesh color: {}".format(grasp_mesh_color))
01399
01400
01401
01402
01403
01404 grasp_mesh1 = Grasp._make_mesh_marker(grasp_mesh_color)
01405 grasp_mesh1.mesh_resource = STR_GRIPPER_PALM_FILE
01406 grasp_mesh1.pose.position.x = Grasp._offset
01407 grasp_mesh1.pose.orientation.w = 1
01408
01409
01410 grasp_mesh2 = Grasp._make_mesh_marker(grasp_mesh_color)
01411 grasp_mesh2.mesh_resource = STR_L_GRIPPER_FINGER_FILE
01412 grasp_mesh2.pose.position.x = 0.08
01413 grasp_mesh2.pose.position.y = -0.116
01414 grasp_mesh2.pose.orientation.w = 1
01415
01416 grasp_mesh3 = Grasp._make_mesh_marker(grasp_mesh_color)
01417 grasp_mesh3.mesh_resource = STR_R_GRIPPER_FINGER_FILE
01418 grasp_mesh3.pose.position.x = 0.08
01419 grasp_mesh3.pose.position.y = 0.116
01420 grasp_mesh3.pose.orientation.w = 1
01421
01422
01423 pre_grasp_mesh1 = Grasp._make_mesh_marker(pre_grasp_mesh_color)
01424 pre_grasp_mesh1.mesh_resource = STR_GRIPPER_PALM_FILE
01425 pre_grasp_mesh1.pose.position.x = Grasp._offset - self._approach_dist
01426 pre_grasp_mesh1.pose.orientation.w = 1
01427
01428 pre_grasp_mesh2 = Grasp._make_mesh_marker(pre_grasp_mesh_color)
01429 pre_grasp_mesh2.mesh_resource = STR_L_GRIPPER_FINGER_FILE
01430 pre_grasp_mesh2.pose.position.x = 0.08 - self._approach_dist
01431 pre_grasp_mesh2.pose.position.y = -0.165
01432 pre_grasp_mesh2.pose.orientation.w = 1
01433
01434 pre_grasp_mesh3 = Grasp._make_mesh_marker(pre_grasp_mesh_color)
01435 pre_grasp_mesh3.mesh_resource = STR_R_GRIPPER_FINGER_FILE
01436 pre_grasp_mesh3.pose.position.x = 0.08 - self._approach_dist
01437 pre_grasp_mesh3.pose.position.y = 0.165
01438 pre_grasp_mesh3.pose.orientation.w = 1
01439
01440
01441 control.markers.append(grasp_mesh1)
01442 control.markers.append(grasp_mesh2)
01443 control.markers.append(grasp_mesh3)
01444 control.markers.append(pre_grasp_mesh1)
01445 control.markers.append(pre_grasp_mesh2)
01446 control.markers.append(pre_grasp_mesh3)
01447
01448 return control
01449
01450 def _delete_primitive_cb(self, feedback):
01451 '''Callback for when delete is requested.
01452
01453 Args:
01454 feedback (InteractiveMarkerFeedback): Unused
01455 '''
01456 self._marker_delete_cb(self._number)
01457
01458 def _regenerate_grasps_cb(self, feedback):
01459 '''Callback for regenerating grasps upon request
01460
01461 Args:
01462 feedback (InteractiveMarkerFeedback): Unused
01463 '''
01464 self.hide_marker()
01465 resp = self._get_object_from_name_srv(self.get_ref_frame_name())
01466 self._suggest_grasps(resp.obj)
01467 self.update_viz()
01468 self.show_marker()
01469 self._action_change_cb()
01470 self._pose_change_cb()
01471
01472 def _move_to_cb(self, feedback):
01473 '''Callback for when moving to a pose is requested.
01474
01475 Args:
01476 feedback (InteractiveMarkerFeedback): Unused
01477 '''
01478
01479 self.execute()
01480
01481 def _change_ref_cb(self, feedback):
01482 '''Callback for when a reference frame change is requested.
01483
01484 Args:
01485 feedback (InteractiveMarkerFeedback (?))
01486 '''
01487 self._menu_handler.setCheckState(
01488 self._get_menu_id(self._get_menu_ref()), MenuHandler.UNCHECKED)
01489 self._menu_handler.setCheckState(
01490 feedback.menu_entry_id, MenuHandler.CHECKED)
01491 new_ref = self._get_menu_name(feedback.menu_entry_id)
01492 self._set_ref(new_ref)
01493 rospy.loginfo(
01494 'Switching reference frame to ' + new_ref + ' for primitive ' +
01495 self.get_name())
01496 self._menu_handler.reApply(self._im_server)
01497 self._im_server.applyChanges()
01498 self.update_viz(False)
01499 self._action_change_cb()
01500
01501 def _marker_feedback_cb(self, feedback):
01502 '''Callback for when an event occurs on the marker.
01503
01504 Args:
01505 feedback (InteractiveMarkerFeedback)
01506 '''
01507 if feedback.event_type == InteractiveMarkerFeedback.MOUSE_UP:
01508
01509
01510
01511
01512 rospy.logdebug("Changing selected-ness.")
01513 self._is_control_visible = not self._is_control_visible
01514 self._marker_click_cb(
01515 self._number, self._is_control_visible)
01516 else:
01517
01518
01519
01520 rospy.logdebug("Unknown event: " + str(feedback.event_type))
01521
01522 def _get_menu_ref(self):
01523 '''Returns the name string for the reference frame object of the
01524 primitive. This is specifically for
01525
01526 Returns:
01527 str|None: Under all normal circumstances, returns the str
01528 reference frame name. Returns None in error.
01529 '''
01530
01531 ref_type = self._grasp_state.ref_type
01532 ref_name = self._grasp_state.ref_landmark.name
01533
01534
01535 if ref_type == ArmState.ROBOT_BASE:
01536 ref_name = BASE_LINK
01537 elif ref_type == ArmState.PREVIOUS_TARGET:
01538 ref_name = PREVIOUS_PRIMITIVE
01539 elif ref_name == '':
01540 ref_name = BASE_LINK
01541 rospy.loginfo("Empty frame: {}".format(self._number))
01542
01543 return ref_name
01544