$search
00001 #!/usr/bin/env python 00002 ############################################################################### 00003 # \file 00004 # 00005 # $Id:$ 00006 # 00007 # Copyright (C) Brno University of Technology 00008 # 00009 # This file is part of software developed by dcgm-robotics@FIT group. 00010 # 00011 # Author: Zdenek Materna (imaterna@fit.vutbr.cz) 00012 # Supervised by: Michal Spanel (spanel@fit.vutbr.cz) 00013 # Date: dd/mm/2012 00014 # 00015 # This file is free software: you can redistribute it and/or modify 00016 # it under the terms of the GNU Lesser General Public License as published by 00017 # the Free Software Foundation, either version 3 of the License, or 00018 # (at your option) any later version. 00019 # 00020 # This file is distributed in the hope that it will be useful, 00021 # but WITHOUT ANY WARRANTY; without even the implied warranty of 00022 # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 00023 # GNU Lesser General Public License for more details. 00024 # 00025 # You should have received a copy of the GNU Lesser General Public License 00026 # along with this file. If not, see <http://www.gnu.org/licenses/>. 00027 # 00028 00029 import roslib; roslib.load_manifest('srs_states') 00030 import rospy 00031 import smach 00032 import smach_ros 00033 import actionlib 00034 00035 from math import fabs, sqrt 00036 import copy 00037 from numpy import power 00038 00039 from geometry_msgs.msg import Vector3, PoseStamped, Pose 00040 from std_msgs.msg import ColorRGBA 00041 00042 from srs_assisted_arm_navigation_msgs.msg import * 00043 from srs_assisted_arm_navigation_msgs.srv import ArmNavCollObj, ArmNavMovePalmLink 00044 from srs_assisted_grasping_msgs.srv import GraspingAllow 00045 00046 from srs_interaction_primitives.srv import AddObject, RemovePrimitive, SetPreGraspPosition, RemovePreGraspPosition, GetUnknownObject, SetAllowObjectInteraction, AddUnknownObject 00047 from srs_interaction_primitives.msg import MoveArmToPreGrasp, PoseType 00048 from srs_env_model_percp.srv import EstimateBBAlt 00049 from srs_object_database_msgs.srv import GetMesh, GetObjectId 00050 00051 from shared_state_information import * 00052 00053 00054 class common_helper_methods(): 00055 00056 def __init__(self): 00057 00058 but_gui_ns = '/interaction_primitives' 00059 self.s_remove_object = but_gui_ns + '/remove_primitive' 00060 self.s_allow_interaction = but_gui_ns + '/set_allow_object_interaction' 00061 00062 def wait_for_srv(self,srv): 00063 00064 available = False 00065 00066 rospy.loginfo('Waiting for %s service',srv) 00067 00068 try: 00069 00070 rospy.wait_for_service(srv,timeout=60) 00071 available = True 00072 00073 except ROSException, e: 00074 00075 rospy.logerr('Cannot set interaction mode for object %s, error: %s',object_name,str(e)) 00076 00077 00078 if not available: 00079 00080 rospy.logerr('Service %s is not available! Error: ',srv,e) 00081 00082 return available 00083 00084 00085 00086 def wait_for_topic(self,topic,topic_type): 00087 00088 available = False 00089 00090 rospy.loginfo('Waiting for %s topic',topic) 00091 00092 try: 00093 00094 rospy.wait_for_message(topic, topic_type, timeout=60) 00095 00096 except ROSException, e: 00097 00098 rospy.logerr('Topic %s is not available! Error: %s',topic,e) 00099 00100 00101 00102 def remove_im(self,object_name): 00103 00104 remove_object = rospy.ServiceProxy(self.s_remove_object, RemovePrimitive) 00105 00106 removed = False 00107 00108 try: 00109 00110 remove_object(name=object_name) 00111 removed = True 00112 00113 except Exception, e: 00114 00115 rospy.logerr('Cannot remove IM object from the scene, error: %s',str(e)) 00116 00117 return removed 00118 00119 def set_interaction(self,object_name,allow_int): 00120 00121 int_allow_srv = rospy.ServiceProxy(self.s_allow_interaction, SetAllowObjectInteraction) 00122 00123 try: 00124 00125 res = int_allow_srv(name = object_name, 00126 allow = allow_int) 00127 00128 00129 except Exception, e: 00130 00131 rospy.logerr('Cannot set interaction mode for object %s, error: %s',object_name,str(e)) 00132 00133 00134 00135 00136 00137 class grasp_unknown_object_assisted(smach.State): 00138 def __init__(self): 00139 smach.State.__init__(self,outcomes=['completed','not_completed','failed','pre-empted'], 00140 input_keys=[''], 00141 output_keys=['']) 00142 00143 global listener 00144 00145 self.hlp = common_helper_methods() 00146 00147 self.object_added = False 00148 00149 self.object_pose = None 00150 self.object_bb = None 00151 00152 but_gui_ns = '/interaction_primitives' 00153 00154 arm_manip_ns = '/but_arm_manip' 00155 self.arm_action_name = arm_manip_ns + '/manual_arm_manip_action' 00156 self.s_grasping_allow = arm_manip_ns + '/grasping_allow' 00157 self.s_allow_interaction = but_gui_ns + '/set_allow_object_interaction' 00158 00159 self.s_bb_est = '/bb_estimator/estimate_bb_alt' 00160 self.s_add_unknown_object = but_gui_ns + '/add_unknown_object' 00161 self.s_get_object = but_gui_ns + '/get_unknown_object' 00162 00163 self.s_coll_obj = arm_manip_ns + '/arm_nav_coll_obj'; 00164 00165 self.unknown_object_name='unknown_object' 00166 self.unknown_object_description='Unknown object to grasp' 00167 00168 def bb_est_feedback(self,feedback): 00169 00170 rospy.loginfo('Feedback received') 00171 00172 est = rospy.ServiceProxy('/bb_estimator/estimate_bb_alt',EstimateBBAlt) 00173 00174 header=rospy.Header() 00175 00176 header.frame_id = '/map' 00177 header.stamp = feedback.timestamp 00178 00179 rospy.loginfo('Calling bb est srv') 00180 00181 try: 00182 00183 est_result = est(header=header, 00184 p1=feedback.p1, 00185 p2=feedback.p2, 00186 mode=1) 00187 00188 except Exception, e: 00189 00190 rospy.logerr("Error on calling service: %s",str(e)) 00191 return 00192 00193 rospy.loginfo('BB estimation performed!') 00194 00195 00196 if self.object_added == True: 00197 00198 if self.hlp.remove_im(self.unknown_object_name) == True: 00199 00200 self.object_added = False 00201 00202 00203 if self.object_added == False: 00204 00205 add_object = rospy.ServiceProxy(self.s_add_unknown_object, AddUnknownObject) 00206 rospy.loginfo('Calling %s service',self.s_add_unknown_object) 00207 00208 try: 00209 00210 #=================================================================== 00211 # add_object(frame_id = '/map', 00212 # name = self.unknown_object_name, 00213 # description = self.unknown_object_description, 00214 # pose = est_result.pose, 00215 # bounding_box_lwh = est_result.bounding_box_lwh, 00216 # color = color, 00217 # use_material = False) 00218 #=================================================================== 00219 00220 add_object(frame_id='/map', 00221 name=self.unknown_object_name, 00222 description=self.unknown_object_description, 00223 pose_type= PoseType.POSE_BASE, 00224 pose = est_result.pose, 00225 scale = est_result.bounding_box_lwh) 00226 00227 self.object_added = True 00228 00229 rospy.loginfo('IM added') 00230 00231 00232 except Exception, e: 00233 00234 rospy.logerr('Cannot add IM object to the scene, error: %s',str(e)) 00235 00236 # allow interaction for this object 00237 self.hlp.set_interaction(self.unknown_object_name,True) 00238 00239 else: 00240 00241 rospy.logerr('Could not add new IM - old one was not removed!'); 00242 00243 return 00244 00245 00246 def get_im_pose(self): 00247 00248 get_object = rospy.ServiceProxy(self.s_get_object, GetUnknownObject) 00249 00250 try: 00251 00252 res = get_object(name=self.unknown_object_name) 00253 00254 if res.frame_id is not ('map' or '/map'): 00255 00256 rospy.logwarn('TODO: Transformation of IM pose needed! Frame_id: %s',res.frame_id) 00257 00258 self.object_pose = res.pose 00259 self.object_bb = res.scale 00260 00261 except Exception, e: 00262 00263 rospy.logerr('Cannot add IM object to the scene, error: %s',str(e)) 00264 00265 if self.object_pose is not None: 00266 00267 # conversion to lwh 00268 self.object_pose.position.y -= res.scale.y/2.0 00269 self.object_pose.position.z -= res.scale.z/2.0 00270 00271 00272 return True 00273 else: 00274 00275 return False 00276 00277 00278 00279 def add_im_for_object(self): 00280 00281 # /but_arm_manip/manual_bb_estimation_action 00282 roi_client = actionlib.SimpleActionClient('/but_arm_manip/manual_bb_estimation_action',ManualBBEstimationAction) 00283 00284 rospy.loginfo("Waiting for ROI action server...") 00285 roi_client.wait_for_server() 00286 00287 goal = ManualBBEstimationGoal() 00288 goal.object_name = 'unknown object' 00289 00290 roi_client.send_goal(goal,feedback_cb=self.bb_est_feedback) 00291 00292 rospy.loginfo("Waiting for result...") 00293 roi_client.wait_for_result() 00294 00295 result = roi_client.get_result() 00296 00297 print result 00298 00299 # not let's suppose that IM is already on its position.... 00300 00301 rospy.loginfo('Action finished') 00302 00303 def add_bb_to_planning(self): 00304 00305 coll_obj = rospy.ServiceProxy(self.s_coll_obj, ArmNavCollObj); 00306 00307 mpose = PoseStamped() 00308 00309 mpose.header.frame_id = '/map' 00310 mpose.pose = self.object_pose 00311 00312 try: 00313 00314 coll_obj(object_name = self.unknown_object_name, 00315 pose = mpose, 00316 bb_lwh = self.object_bb, 00317 allow_collision=True, 00318 attached=False, 00319 attach_to_frame_id=''); 00320 00321 except Exception, e: 00322 00323 rospy.logerr('Cannot add unknown object to the planning scene, error: %s',str(e)) 00324 00325 def change_bb_to_attached(self): 00326 00327 rospy.loginfo('Setting object bb to be attached (not implemented)') 00328 00329 00330 def execute(self,userdata): 00331 00332 global listener 00333 00334 rospy.loginfo('Waiting for needed services and topics...') 00335 00336 if self.hlp.wait_for_srv(self.s_grasping_allow) is False: 00337 return 'failed' 00338 00339 if self.hlp.wait_for_srv(self.s_allow_interaction) is False: 00340 return 'failed' 00341 00342 if self.hlp.wait_for_srv(self.s_bb_est) is False: 00343 return 'failed' 00344 00345 if self.hlp.wait_for_srv(self.s_add_unknown_object) is False: 00346 return 'failed' 00347 00348 if self.hlp.wait_for_srv(self.s_get_object) is False: 00349 return 'failed' 00350 00351 if self.hlp.wait_for_srv(self.s_coll_obj) is False: 00352 return 'failed' 00353 00354 00355 self.s_coll_obj 00356 00357 grasping_client = actionlib.SimpleActionClient('/but_arm_manip/manual_arm_manip_action',ManualArmManipAction) 00358 00359 rospy.loginfo("Waiting for grasping action server...") 00360 grasping_client.wait_for_server() 00361 00362 # ask user to select and tune IM for unknown object 00363 self.add_im_for_object() 00364 00365 if not self.get_im_pose(): 00366 00367 rospy.logerr('Could not get position of unknown object. Lets try to continue without it...') 00368 00369 else: 00370 00371 self.add_bb_to_planning() 00372 00373 00374 00375 # send grasping goal and allow grasping buttons :) 00376 goal = ManualArmManipGoal() 00377 00378 goal.allow_repeat = False 00379 goal.action = "Grasp object and put it on tray" 00380 goal.object_name = self.unknown_object_name 00381 00382 grasping_client.send_goal(goal) 00383 00384 # call allow grasping service 00385 rospy.wait_for_service('/but_arm_manip/grasping_allow') 00386 00387 00388 grasping_allow = rospy.ServiceProxy('/but_arm_manip/grasping_allow',GraspingAllow) 00389 00390 try: 00391 00392 gr = grasping_allow(allow=True) 00393 00394 except Exception, e: 00395 00396 rospy.logerr("Error on calling service: %s",str(e)) 00397 return 'failed' 00398 00399 00400 rospy.loginfo("Waiting for result") 00401 grasping_client.wait_for_result() 00402 00403 result = grasping_client.get_result() 00404 00405 00406 if result.success: 00407 00408 rospy.loginfo('Object should be on tray.') 00409 return 'completed' 00410 00411 if result.failed: 00412 00413 rospy.logwarn('Operator was not able to grasp object and put it on tray') 00414 return 'failed' 00415 00416 if result.timeout: 00417 00418 rospy.logwarn('Action was too long') 00419 return 'not_completed' 00420 00421 00422 rospy.logerr('This should never happen!') 00423 return 'failed' 00424 00425 00426 00427 class move_arm_to_given_positions_assisted(smach.State): 00428 def __init__(self): 00429 smach.State.__init__(self,outcomes=['completed','not_completed','failed','pre-empted','repeat'], 00430 input_keys=['list_of_target_positions','list_of_id_for_target_positions','name_of_the_target_object','pose_of_the_target_object','bb_of_the_target_object'], 00431 output_keys=['id_of_the_reached_position']) 00432 00433 global listener 00434 00435 self.hlp = common_helper_methods() 00436 00437 but_gui_ns = '/interaction_primitives' 00438 self.s_set_gr_pos = but_gui_ns + '/set_pregrasp_position' 00439 self.s_add_object = but_gui_ns + '/add_object' 00440 self.s_remove_object = but_gui_ns + '/remove_primitive' 00441 self.s_allow_interaction = but_gui_ns + '/set_allow_object_interaction' 00442 00443 # object database 00444 self.s_get_object_id = '/get_models' 00445 self.s_get_model_mesh = '/get_model_mesh' 00446 00447 #ns = rospy.get_param('/but_arm_manip','/arm_manip_namespace') 00448 arm_manip_ns = '/but_arm_manip' 00449 #action_name = rospy.get_param('/manual_arm_manip_action','~arm_action_name') 00450 self.action_name = arm_manip_ns + '/manual_arm_manip_action' 00451 self.s_coll_obj = arm_manip_ns + '/arm_nav_coll_obj'; 00452 self.s_move_arm = arm_manip_ns + '/arm_nav_move_palm_link'; 00453 00454 00455 def add_grpos(self,userdata): 00456 00457 global listener 00458 00459 set_gr_pos = rospy.ServiceProxy(self.s_set_gr_pos, SetPreGraspPosition) 00460 rospy.loginfo('Calling service %s',self.s_set_gr_pos) 00461 00462 pregr_tmp = list(userdata.list_of_target_positions) 00463 00464 for idx in range(0,len(userdata.list_of_id_for_target_positions)): 00465 00466 try: 00467 00468 gpose = Pose() 00469 00470 gpose = pregr_tmp[idx].pre_grasp.pose 00471 00472 # shift position to be relative to detected object 00473 gpose.position.x -= userdata.pose_of_the_target_object.pose.position.x 00474 gpose.position.y -= userdata.pose_of_the_target_object.pose.position.y 00475 gpose.position.z -= userdata.pose_of_the_target_object.pose.position.z 00476 00477 00478 rospy.loginfo('Adding gr pos id %d [x=%f,y=%f,z=%f]',userdata.list_of_id_for_target_positions[idx],gpose.position.x,gpose.position.y,gpose.position.z) 00479 00480 res = set_gr_pos(name=userdata.name_of_the_target_object, 00481 pos_id=idx+1, 00482 pose=gpose) 00483 00484 except Exception, e: 00485 00486 rospy.logerr('Cannot add gr pos IM for pos ID: %d, error: %s',idx+1,str(e)) 00487 00488 00489 def add_im(self,userdata): 00490 00491 global listener 00492 00493 get_object_id = rospy.ServiceProxy(self.s_get_object_id, GetObjectId) 00494 rospy.loginfo('Calling service %s',self.s_get_object_id) 00495 00496 obj_db_id = None 00497 00498 try: 00499 00500 res = get_object_id(type=userdata.name_of_the_target_object) 00501 00502 #obj_db_id = int(res.model_ids[0]) 00503 idx = res.model_category.index(userdata.name_of_the_target_object) 00504 obj_db_id = int(res.model_ids[idx]) 00505 00506 rospy.loginfo('Object name (%s) successfully converted to ID (%d)',userdata.name_of_the_target_object,obj_db_id) 00507 00508 except Exception, e: 00509 00510 rospy.logerr('Error on 0 name (%s) to ID... Lets use ID=1. Error: %s',userdata.name_of_the_target_object,str(e)) 00511 obj_db_id = 1 00512 00513 shape = None 00514 mesh = 'package://cob_gazebo_objects/Media/models/milk.dae' 00515 #db_shape = None 00516 use_default_mesh = True 00517 00518 get_model_mesh = rospy.ServiceProxy(self.s_get_model_mesh, GetMesh) 00519 rospy.loginfo('Calling service %s (with ID=%d)',self.s_get_model_mesh,obj_db_id) 00520 00521 00522 try: 00523 00524 object_shape = get_model_mesh(model_ids=[obj_db_id]) 00525 shape = object_shape.msg[0].mesh 00526 use_default_mesh = False 00527 00528 except Exception, e: 00529 00530 rospy.logerr('Cannot get mesh from db. We will use default one for milkbox. Error: %s',str(e)) 00531 00532 add_object = rospy.ServiceProxy(self.s_add_object, AddObject) 00533 rospy.loginfo('Calling %s service',self.s_add_object) 00534 00535 # color of the bounding box 00536 color = ColorRGBA() 00537 color.r = 1 00538 color.g = 1 00539 color.b = 0 00540 color.a = 1 00541 00542 bpose = copy.deepcopy(self.pose_of_the_target_object_in_map.pose) 00543 bpose.position.z += userdata.bb_of_the_target_object['bb_lwh'].z/2 00544 00545 # TODO remove next line - it was just for testing!!!!!!!! 00546 use_default_mesh = True 00547 00548 try: 00549 00550 if use_default_mesh: 00551 00552 add_object(frame_id = '/map', 00553 name = userdata.name_of_the_target_object, 00554 description = 'Object to grasp', 00555 pose = bpose, 00556 bounding_box_lwh = userdata.bb_of_the_target_object['bb_lwh'], 00557 color = color, 00558 resource = 'package://cob_gazebo_objects/Media/models/milk.dae', 00559 #shape = shape, 00560 use_material = True) 00561 else: 00562 00563 add_object(frame_id = '/map', 00564 name = userdata.name_of_the_target_object, 00565 description = 'Object to grasp', 00566 pose = bpose, 00567 bounding_box_lwh = userdata.bb_of_the_target_object['bb_lwh'], 00568 color = color, 00569 #resource = 'package://cob_gazebo_objects/Media/models/milk.dae', 00570 shape = shape, 00571 use_material = True) 00572 00573 00574 except Exception, e: 00575 00576 rospy.logerr('Cannot add IM object to the scene, error: %s',str(e)) 00577 00578 # disable interaction for this object 00579 self.hlp.set_interaction(userdata.name_of_the_target_object, False) 00580 00581 return shape 00582 00583 def pregr_im_callback(self,data): 00584 00585 global listener 00586 00587 rospy.loginfo("Move arm to pregrasp pos. ID=%d, object=%s, x=%f, y=%f, z=%f", 00588 data.pos_id, 00589 data.marker_name, 00590 self.userdata.list_of_target_positions[data.pos_id-1].pre_grasp.pose.position.x, 00591 self.userdata.list_of_target_positions[data.pos_id-1].pre_grasp.pose.position.y, 00592 self.userdata.list_of_target_positions[data.pos_id-1].pre_grasp.pose.position.z); 00593 00594 move_arm = rospy.ServiceProxy(self.s_move_arm, ArmNavMovePalmLink); 00595 00596 print "test - list_of_trg_pos" 00597 print self.userdata.list_of_target_positions[data.pos_id-1] 00598 00599 target_pos = [] 00600 #target_pos = list(self.userdata.list_of_target_positions) 00601 target_pos = copy.deepcopy(self.userdata.list_of_target_positions) 00602 00603 #=============================================================================== 00604 # pose_of_the_target_object_in_base_link = None 00605 # # self.pose_of_the_target_object_in_map -> transform it to /base_link 00606 # 00607 # t = rospy.Time(0) 00608 # 00609 # transf_target = '/base_link' 00610 # 00611 # listener.waitForTransform(transf_target,'/map',t,rospy.Duration(5)) 00612 # 00613 # if listener.canTransform(transf_target,'/map',t): 00614 # 00615 # pose_of_the_target_object_in_base_link = listener.transformPose(transf_target,self.pose_of_the_target_object_in_map) 00616 # 00617 # else: 00618 # 00619 # rospy.logerr('Transformation is not possible!') 00620 # #sys.exit(0) 00621 # return 'failed' 00622 #=============================================================================== 00623 00624 #=========================================================================== 00625 # target_pos[data.pos_id-1].pre_grasp.pose.position.x += pose_of_the_target_object_in_base_link.pose.position.x 00626 # target_pos[data.pos_id-1].pre_grasp.pose.position.y += pose_of_the_target_object_in_base_link.pose.position.y 00627 # target_pos[data.pos_id-1].pre_grasp.pose.position.z += (pose_of_the_target_object_in_base_link.pose.position.z + self.userdata.bb_of_the_target_object['bb_lwh'].z/2) 00628 #=========================================================================== 00629 00630 target_pos[data.pos_id-1].pre_grasp.pose.position.x += self.pose_of_the_target_object_in_map.pose.position.x 00631 target_pos[data.pos_id-1].pre_grasp.pose.position.y += self.pose_of_the_target_object_in_map.pose.position.y 00632 target_pos[data.pos_id-1].pre_grasp.pose.position.z += (self.pose_of_the_target_object_in_map.pose.position.z + self.userdata.bb_of_the_target_object['bb_lwh'].z/2) 00633 00634 00635 print "Moving arm's IM to this position:" 00636 print target_pos[data.pos_id-1] 00637 00638 try: 00639 00640 move_arm(sdh_palm_link_pose=target_pos[data.pos_id-1].pre_grasp.pose); 00641 00642 except Exception, e: 00643 00644 rospy.logerr('Cannot move sdh_palm_link to given position, error: %s',str(e)) 00645 00646 del target_pos 00647 00648 00649 00650 def execute(self,userdata): 00651 00652 global listener 00653 00654 rospy.loginfo('Waiting for needed services...') 00655 00656 if self.hlp.wait_for_srv(self.s_set_gr_pos) is False: 00657 return 'failed' 00658 00659 if self.hlp.wait_for_srv(self.s_add_object) is False: 00660 return 'failed' 00661 00662 if self.hlp.wait_for_srv(self.s_remove_object) is False: 00663 return 'failed' 00664 00665 if self.hlp.wait_for_srv(self.s_allow_interaction) is False: 00666 return 'failed' 00667 00668 if self.hlp.wait_for_srv(self.s_get_object_id) is False: 00669 return 'failed' 00670 00671 if self.hlp.wait_for_srv(self.s_get_model_mesh) is False: 00672 return 'failed' 00673 00674 if self.hlp.wait_for_srv(self.s_coll_obj) is False: 00675 return 'failed' 00676 00677 if self.hlp.wait_for_srv(self.s_move_arm) is False: 00678 return 'failed' 00679 00680 # waiting for action server 00681 client = actionlib.SimpleActionClient(self.action_name,ManualArmManipAction) 00682 00683 rospy.loginfo("Waiting for actionlib server...") 00684 client.wait_for_server() 00685 00686 00687 if len(userdata.list_of_target_positions)==0: 00688 00689 rospy.logerr('There are NO grasping target positions') 00690 return 'failed' 00691 00692 if len(userdata.list_of_target_positions) != len(userdata.list_of_id_for_target_positions): 00693 00694 rospy.logerr('List with gr. positions has different length than list with IDs') 00695 return 'failed' 00696 00697 # try to transform pose of detected object from /base_link to /map 00698 transf_target = '/map' 00699 00700 rospy.loginfo('Lets transform pose of object from %s to %s frame',userdata.pose_of_the_target_object.header.frame_id,transf_target) 00701 00702 t = rospy.Time(0) 00703 00704 userdata.id_of_the_reached_position = -1 00705 00706 00707 pose_of_target = copy.deepcopy(userdata.pose_of_the_target_object) 00708 00709 00710 rospy.loginfo('Waiting for transform for some time...') 00711 listener.waitForTransform(transf_target,pose_of_target.header.frame_id,t,rospy.Duration(10)) 00712 00713 if listener.canTransform(transf_target,pose_of_target.header.frame_id,t): 00714 00715 self.pose_of_the_target_object_in_map = listener.transformPose(transf_target,pose_of_target) 00716 00717 else: 00718 00719 rospy.logerr('Transformation is not possible!') 00720 return 'failed' 00721 00722 00723 self.userdata = userdata 00724 00725 rospy.loginfo('Executing state move_arm_to_given_positions_assisted (%s)',userdata.name_of_the_target_object) 00726 00727 db_shape = None 00728 00729 # add IM to the scene 00730 db_shape = self.add_im(userdata) 00731 00732 # add IM for grasping positions 00733 self.add_grpos(userdata) 00734 00735 pregr_topic = "/interaction_primitives/" + userdata.name_of_the_target_object + "/update/move_arm_to_pregrasp" 00736 rospy.loginfo("Subscribing to %s topic",pregr_topic); 00737 rospy.Subscriber(pregr_topic, MoveArmToPreGrasp, self.pregr_im_callback) 00738 00739 coll_obj = rospy.ServiceProxy(self.s_coll_obj, ArmNavCollObj); 00740 00741 # hack - why is this needed??????? 00742 tpose = self.pose_of_the_target_object_in_map 00743 #tpose.pose.position.y -= userdata.bb_of_the_target_object['bb_lwh'].y/2 00744 00745 try: 00746 00747 coll_obj(object_name = userdata.name_of_the_target_object, 00748 pose = tpose, 00749 bb_lwh = userdata.bb_of_the_target_object['bb_lwh'], 00750 allow_collision=False, 00751 attached=False, 00752 attach_to_frame_id=''); 00753 00754 except Exception, e: 00755 00756 rospy.logerr('Cannot add detected object to the planning scene, error: %s',str(e)) 00757 00758 00759 goal = ManualArmManipGoal() 00760 00761 goal.allow_repeat = True 00762 goal.action = "Move arm to suitable pregrasp position" 00763 goal.object_name = userdata.name_of_the_target_object 00764 00765 rospy.loginfo("Sending goal...") 00766 client.send_goal(goal) 00767 00768 rospy.loginfo("Waiting for result") 00769 client.wait_for_result() 00770 rospy.loginfo("I have result!! :-)") 00771 00772 result = client.get_result() 00773 00774 # clean up 00775 self.hlp.remove_im(userdata.name_of_the_target_object) 00776 00777 rospy.loginfo("Time elapsed: %ss",result.time_elapsed.to_sec()) 00778 00779 if result.success: 00780 rospy.loginfo('Hooray, successful action. Lets find ID of reached position.') 00781 # TODO find closest pregrasp position and send back its ID! 00782 00783 sdh_ps = PoseStamped() 00784 00785 sdh_ps.header.frame_id = '/sdh_palm_link' 00786 sdh_ps.header.stamp = rospy.Time.now() 00787 sdh_ps.pose.position.x = 0 00788 sdh_ps.pose.position.y = 0 00789 sdh_ps.pose.position.z = 0 00790 00791 listener.waitForTransform('/map',sdh_ps.header.frame_id,sdh_ps.header.stamp,rospy.Duration(10)) 00792 00793 if listener.canTransform('/map',sdh_ps.header.frame_id,sdh_ps.header.stamp): 00794 00795 sdh_ps_tr = listener.transformPose('/map',sdh_ps) 00796 00797 else: 00798 00799 rospy.logerr('Transformation is not possible!') 00800 userdata.id_of_the_reached_position = -1 00801 return 'not_completed' 00802 00803 print "sdh_palm_link pose in /map coord. system" 00804 print sdh_ps_tr 00805 00806 print "object pose in /map coord" 00807 print self.pose_of_the_target_object_in_map 00808 00809 min_dist = 1000 00810 min_id = -1 00811 00812 pregrasp_pose_in_map = Vector3() 00813 closest_one = Vector3() 00814 00815 for idx in range(len(userdata.list_of_target_positions)): 00816 00817 pregrasp_pose_in_map.x = self.pose_of_the_target_object_in_map.pose.position.x + userdata.list_of_target_positions[idx].pre_grasp.pose.position.x 00818 pregrasp_pose_in_map.y = self.pose_of_the_target_object_in_map.pose.position.y + userdata.list_of_target_positions[idx].pre_grasp.pose.position.y 00819 pregrasp_pose_in_map.z = self.pose_of_the_target_object_in_map.pose.position.z + userdata.list_of_target_positions[idx].pre_grasp.pose.position.z + self.userdata.bb_of_the_target_object['bb_lwh'].z/2 00820 00821 print "PREGRASP position in /map" 00822 print idx 00823 print pregrasp_pose_in_map 00824 00825 dist = sqrt( power((sdh_ps_tr.pose.position.x - pregrasp_pose_in_map.x),2) + 00826 power((sdh_ps_tr.pose.position.y - pregrasp_pose_in_map.y),2) + 00827 power((sdh_ps_tr.pose.position.z - pregrasp_pose_in_map.z),2) ); 00828 00829 rospy.loginfo('Position ID=%d, distance=%fm',userdata.list_of_id_for_target_positions[idx],dist) 00830 00831 if dist < min_dist: 00832 00833 min_dist = dist; 00834 min_id = userdata.list_of_id_for_target_positions[idx] 00835 closest_one = copy.deepcopy(pregrasp_pose_in_map) 00836 00837 print "pose of closest pregrasp position in /map" 00838 print closest_one 00839 00840 #print userdata.list_of_target_positions[userdata.list_of_id_for_target_positions.index(min_id)].pre_grasp.pose 00841 00842 # BUG!!! 00843 00844 if min_dist > 0.2: 00845 00846 rospy.logerr("Distance to pregrasp position too long (%fm)",min_dist) 00847 min_id = -1 00848 return 'not_completed' 00849 00850 else: 00851 00852 rospy.loginfo("ID of closest position is %d and distance is %f",min_id,min_dist) 00853 00854 userdata.id_of_the_reached_position = min_id 00855 return 'completed' 00856 00857 if result.timeout: 00858 rospy.loginfo('action timeouted') 00859 userdata.id_of_the_reached_position = -1 00860 return 'not_completed' 00861 00862 if result.repeat: 00863 rospy.loginfo('request for repeating action') 00864 userdata.id_of_the_reached_position = -1 00865 return 'repeat' 00866 00867 if result.failed: 00868 rospy.loginfo('Oou... Action failed') 00869 userdata.id_of_the_reached_position = -1 00870 return 'failed' 00871 00872 rospy.loginfo('Ups... Unknown state of action') 00873 userdata.id_of_the_reached_position = -1 00874 return 'failed' 00875 00876 00877 00878 class move_arm_from_a_given_position_assisted(smach.State): 00879 def __init__(self): 00880 smach.State.__init__(self,outcomes=['completed','not_completed','failed','pre-empted']) 00881 00882 00883 but_gui_ns = '/but_arm_manip' 00884 self.action_name = but_gui_ns + '/manual_arm_manip_action' 00885 00886 def execute(self,userdata): 00887 00888 rospy.loginfo('Executing state move_arm_from_a_given_position_assisted') 00889 00890 client = actionlib.SimpleActionClient(self.action_name,ManualArmManipAction) 00891 00892 rospy.loginfo("Waiting for server...") 00893 client.wait_for_server() 00894 00895 goal = ManualArmManipGoal() 00896 #goal.away = True 00897 00898 goal.allow_repeat = False 00899 goal.action = "Move arm to safe position" 00900 goal.object_name = "" 00901 00902 rospy.loginfo("Sending goal...") 00903 client.send_goal(goal) 00904 00905 rospy.loginfo("Waiting for result") 00906 client.wait_for_result() 00907 rospy.loginfo("I have result!! :-)") 00908 00909 result = client.get_result() 00910 00911 rospy.loginfo("Time elapsed: %ss",result.time_elapsed.to_sec()) 00912 00913 if result.success: 00914 rospy.loginfo('Hooray, successful action') 00915 return 'completed' 00916 00917 if result.timeout: 00918 rospy.loginfo('action timeouted') 00919 return 'not_completed' 00920 00921 if result.failed: 00922 rospy.loginfo('Oou... Action failed') 00923 return 'failed' 00924 00925 rospy.loginfo('Ups... Unknown state of action') 00926 return 'failed'