assisted_detection_states.py
Go to the documentation of this file.
00001 import roslib
00002 roslib.load_manifest('srs_states')
00003 import rospy
00004 import smach
00005 import smach_ros
00006 
00007 from math import *
00008 import copy
00009 from struct import *
00010 
00011 from simple_script_server import *
00012 sss = simple_script_server()
00013 
00014 from cob_object_detection_msgs.msg import *
00015 from cob_object_detection_msgs.srv import *
00016 from shared_state_information import *
00017 from eval_objects import *
00018 from cob_3d_mapping_msgs.msg import *
00019 
00020 from srs_symbolic_grounding.srv import *
00021 #from srs_symbolic_grounding.msg import *
00022 
00023 #from srs_grasping.srv import *
00024 from geometry_msgs.msg import *
00025 from sensor_msgs.msg import *
00026 from srs_assisted_detection.srv import *
00027 from geometry_msgs.msg import *
00028 from srs_msgs.msg import * # this is for SRSSpatialInfo()
00029 ## Detect state
00030 #
00031 # outcomes save the outcome from the sm from the service function
00032 outcome_detectObjectSrv  = ''
00033 outcome_user_intervention = ''
00034 #flag will be set if the service was called
00035 assisted_detection_service_called=False
00036 user_intervention_service_called=0
00037 
00038 class detect_object_assited(smach.State):
00039     def __init__(self,object_name = ""):
00040         smach.State.__init__(
00041             self,
00042             outcomes=['succeeded','failed','preempted'],
00043             input_keys=['object_name','object_id'],
00044             output_keys=['object_list'])
00045 
00046         self.object_list = DetectionArray()
00047         self.object_name = object_name   #doesn't seem to be used 
00048         self.object_id = 0
00049         self.srv_name_object_detection = '/object_detection/detect_object'
00050 
00051         self.torso_poses = []
00052         self.torso_poses.append("back_right")
00053         self.torso_poses.append("back")
00054         self.torso_poses.append("back_left")
00055         self.torso_poses.append("back_right_extreme")
00056         self.torso_poses.append("back_extreme")
00057         self.torso_poses.append("back_left_extreme")
00058         #self.listener = tf.TransformListener()
00059         self.the_object = ''
00060         self.the_object_pose = ''
00061 
00062     def execute(self, userdata):
00063         global s
00064         rospy.loginfo("Assisted Detection ready.")
00065         self.object_name = userdata.object_name
00066         self.object_id = userdata.object_id
00067         s = rospy.Service('assisted_detection', UiDetector, self.detectObjectSrv)
00068         s.spin()
00069         if (assisted_detection_service_called):
00070             userdata.object_list=self.object_list
00071             return outcome_detectObjectSrv 
00072     
00073     def detectObjectSrv(self,req): # function regarding UiDetector
00074         global outcome_detectObjectSrv 
00075         #if self.preempt_requested():
00076            #self.service_preempt()
00077            #outcome_detectObjectSrv = 'preempted'
00078            #return detector_response
00079         # move sdh as feedback
00080         sss.move("sdh","cylclosed",False)
00081         # make the robot ready to inspect the scene
00082         sss.say([current_task_info.speaking_language['Search'] + self.object_name + "."],False)
00083         handle_arm = sss.move("arm","folded-to-look_at_table",False)
00084         handle_torso = sss.move("torso","shake",False)
00085         handle_head = sss.move("head","back",False)
00086 
00087         if self.preempt_requested():
00088             self.service_preempt() # !!!check this
00089             #handle_base.set_failed(4)
00090             handle_arm.client.cancel_goal()
00091             handle_torso.client.cancel_goal()
00092             handle_head.client.cancel_goal()
00093             outcome_detectObjectSrv = 'preempted'
00094             return detector_response
00095         else:
00096             handle_arm.wait()
00097             handle_head.wait()
00098             handle_torso.wait()
00099 
00100         # move sdh as feedback
00101         sss.move("sdh","home",False)
00102         # wait for image to become stable
00103         sss.sleep(2)
00104         # check if object detection service is available
00105         # call object detection service
00106         try:
00107             rospy.wait_for_service(self.srv_name_object_detection,10)
00108             detector_service = rospy.ServiceProxy(self.srv_name_object_detection, DetectObjects)
00109             req = DetectObjectsRequest()
00110             req.object_name.data = self.object_name
00111             res = detector_service(req)
00112             self.object_list=res.object_list
00113             outcome_detectObjectSrv = 'succeeded'
00114         except rospy.ServiceException, e:
00115             print "Service call failed: %s"%e
00116             outcome_detectObjectSrv = 'failed'
00117 
00118         detector_response=UiDetectorResponse() # cob_object_detection; see /srv/UiDetector.srv
00119         
00120         #detector_response.object_id = self.object_id #!!! check this!!!
00121         
00122         if len(res.object_list.detections) > 0:
00123             detector_response.object_list.header=res.object_list.header
00124             for x in range(len(res.object_list.detections)):
00125                 detector_response.object_list.detections.insert(x,res.object_list.detections[x])
00126         
00127         #shutdown server and set flag true        
00128         global assisted_detection_service_called
00129         assisted_detection_service_called=True
00130         s.shutdown()
00131         
00132         for item in detector_response.object_list.detections:
00133             item.pose.header.stamp = rospy.Time.now()
00134         self.object_list=detector_response.object_list
00135         #return service answer
00136         return detector_response  
00137     
00138 class user_intervention_on_detection(smach.State):
00139     def __init__(self):
00140         smach.State.__init__(self, 
00141                              outcomes=['succeeded', 'bb_move', 'give_up', 'failed', 'preempted', 'retry'],
00142                              input_keys = ['target_object_name', 'target_object_list'],
00143                              output_keys=['object','object_pose','bb_pose'])
00144         self.target_object_name=''
00145         self.object_list=UiDetectorResponse()
00146         self.object=Detection()
00147         self.object_pose=Pose()
00148         self.bb_pose=Pose2D()
00149 
00150     def execute(self, userdata):
00151         if self.preempt_requested():
00152             self.service_preempt()
00153             return 'preempted'
00154         
00155         self.target_object_name=userdata.target_object_name
00156         self.object_list=userdata.target_object_list
00157         #user_intervention_service_called == 2 # this is for testing
00158         print "###user_intervention_service_called ", user_intervention_service_called
00159         
00160         if (len(self.object_list.detections) > 0):
00161             global s2
00162             global s3
00163             s2 = rospy.Service('assisted_BBmove', BBMove, self.moveBBSrv)
00164             print "###s2 is started..."
00165             s3 = rospy.Service('assisted_answer', UiAnswer, self.answerObjectSrv) # a=array('i',[2,3,4,5]) => (a,0,s)
00166             print "###s3 is started..."
00167             s2.spin()
00168             s3.spin()
00169             rospy.loginfo("assisted_answer: UiAnswer is ready.")
00170             if(user_intervention_service_called==1):
00171                 try:
00172                     #transform object_pose into base_link
00173                     object_pose_in = self.object.pose
00174                     object_pose_in.header.stamp = listener.getLatestCommonTime("/map",object_pose_in.header.frame_id)
00175                     object_pose_map = listener.transformPose("/map", object_pose_in)
00176                 except rospy.ROSException, e:
00177                     print "Transformation not possible: %s"%e
00178                     return 'failed'
00179                 userdata.object_pose=object_pose_map
00180                 userdata.object=self.object
00181                 return outcome_user_intervention
00182             if(user_intervention_service_called==2): 
00183                 rospy.loginfo("assisted_answer: BBMove is ready.")
00184                 print self.bb_pose
00185                 userdata.bb_pose=[self.bb_pose.x,self.bb_pose.y,self.bb_pose.theta]
00186                 return outcome_user_intervention
00187         else:
00188             print "Cannot execute the user intervention, as no object has been detected!"
00189             return 'give_up' # !!! check this
00190         
00191     def answerObjectSrv(self,req):    
00192         global user_intervention_service_called
00193         global outcome_user_intervention
00194         
00195         user_intervention_service_called=1     
00196         
00197         rospy.loginfo("Get Object information")
00198         #response to user when action is finish  std_msgs/String message
00199         #succeeded, give_up or retry
00200         answer=UiAnswerResponse()
00201         
00202         rospy.loginfo("%s", req.action)
00203         if(req.action.data == 'give up'):
00204             outcome_user_intervention = 'give up'
00205             answer.message.data = 'give up, process stopped'
00206         #save
00207         elif(req.action.data == 'succeeded'):
00208             if((len(self.object_list.detections) > 0) and (req.id < len(self.object_list.detections))):
00209                 #get position from good objectshutdown
00210                 pose=Pose()
00211                 pose.position.x=self.object_list.detections[req.id].pose.pose.position.x
00212                 pose.position.y=self.object_list.detections[req.id].pose.pose.position.y
00213                 pose.position.z=self.object_list.detections[req.id].pose.pose.position.z
00214                 pose.orientation.x=self.object_list.detections[req.id].pose.pose.orientation.x
00215                 pose.orientation.y=self.object_list.detections[req.id].pose.pose.orientation.y
00216                 pose.orientation.z=self.object_list.detections[req.id].pose.pose.orientation.z
00217                 pose.orientation.w=self.object_list.detections[req.id].pose.pose.orientation.w
00218             
00219                 print "pose is ", pose
00220                 self.object_pose=pose
00221                 self.object=self.object_list.detections[req.id] # check id
00222                 
00223                 outcome_user_intervention = 'succeeded'
00224                 answer.message.data='succeeded, go to next step'
00225             elif(req.id >= len(self.object_list.detections)):
00226                 outcome_user_intervention = 'retry'
00227                 answer.message.data='id of the selected object is out of index'
00228             else:
00229                 outcome_user_intervention = 'retry'
00230                 answer.message.data='No object has been detected'
00231         else: #retry detection
00232             outcome_user_intervention = 'retry'
00233             answer.message.data='retry, re-detect the object'
00234 
00235         #shutdown both service#s.shutdown()
00236         #s.shutdown()
00237         s2.shutdown()
00238         s3.shutdown()
00239         return answer
00240         
00241     def moveBBSrv(self,req):
00242         rospy.loginfo("Get BB information")
00243         global user_intervention_service_called
00244         global outcome_user_intervention
00245         user_intervention_service_called=2
00246         #BBmove service base and then movement
00247         moveBB=BBMoveResponse()
00248         
00249         try:
00250             rospy.wait_for_service('scan_base_pose',10)
00251         except rospy.ROSException, e:
00252             print "Service not available: %s"%e
00253             s3.shutdown()
00254             s2.shutdown()
00255             moveBB.message.data='service failed try again'
00256 
00257             outcome_detectObjectSrv = 'failed'
00258         
00259         try:
00260             base_pose_service = rospy.ServiceProxy('scan_base_pose', ScanBasePose)
00261             req_scan = ScanBasePoseRequest()
00262             srs_info=SRSSpatialInfo() # what is this?
00263             srs_info.l=req.l
00264             srs_info.w=req.w
00265             srs_info.h=req.h
00266             srs_info.pose=req.pose 
00267             req_scan.parent_obj_geometry=srs_info
00268             res = base_pose_service(req_scan)
00269             self.bb_pose=res.scan_base_pose_list[0]
00270             #print res
00271             #s.shutdown()
00272             #s2.shutdown()
00273             moveBB.message.data='moving to better position'
00274 
00275             outcome_user_intervention = 'bb_move'
00276         except rospy.ServiceException, e:
00277             print "Service call failed: %s"%e
00278             #s.shutdown()
00279             s2.shutdown()
00280             s3.shutdown()
00281             moveBB.message.data='service failed try again'
00282 
00283             outcome_user_intervention = 'failed'
00284         
00285         #service call to get better position 
00286         #pose=Pose()
00287        # pose.position.x=1
00288         #pose.position.y=2
00289         #pose.position.z=3
00290         #self.bb_pose=pose
00291         
00292        # outcome_user_intervention='bb_move'
00293 
00294         #shutdown both service
00295 
00296         return moveBB


srs_states
Author(s): Georg Arbeiter
autogenerated on Mon Oct 6 2014 08:34:06