sm_pr2_servoing.py
Go to the documentation of this file.
00001 #!/usr/bin/python
00002 
00003 import numpy as np
00004 import functools
00005 
00006 import roslib
00007 roslib.load_manifest('hrl_pr2_arms')
00008 roslib.load_manifest('smach_ros')
00009 roslib.load_manifest('costmap_services')
00010 import rospy
00011 import smach
00012 import smach_ros
00013 from std_msgs.msg import Bool, Int8
00014 
00015 from hrl_pr2_arms.pr2_arm import create_pr2_arm, PR2ArmJointTrajectory
00016 from costmap_services.python_client import CostmapServices
00017 from pr2_viz_servo import PR2VisualServoAR
00018 
00019 outcomes_spa = ['succeeded','preempted','aborted']
00020 
00021 #viz_servo_goals = {
00022 #    "r_pr2_ar_pose_marker" : [0.57226345,  0.32838129, -1.15480113],
00023 #    "l_pr2_ar_pose_marker" : [0.57903398,  0.44215034, -0.76362254]}
00024 #"l_pr2_ar_pose_marker" : [0.59739709,  0.39469539, -0.7088098]}
00025 
00026 class ServoStates:
00027     BEGIN_FIND_TAG = 1
00028     FOUND_TAG = 2
00029     TIMEOUT_FIND_TAG = 3
00030     BEGIN_SERVO = 4
00031     SUCCESS_SERVO = 5
00032     ARM_COLLISION = 6
00033     LASER_COLLISION = 7
00034     LOST_TAG = 8
00035     USER_PREEMPT = 9
00036 
00037 class ArmCollisionDetection(smach.State):
00038     def __init__(self, min_l_torques=[-5.]*7, min_r_torques=[-5.]*7,
00039                        max_l_torques=[5.]*7, max_r_torques=[5.]*7):
00040         smach.State.__init__(self, outcomes=['collision', 'preempted', 'aborted'])
00041         self.min_l_tor = np.array(min_l_torques)
00042         self.min_r_tor = np.array(min_r_torques)
00043         self.max_l_tor = np.array(max_l_torques)
00044         self.max_r_tor = np.array(max_r_torques)
00045         self.l_arm = create_pr2_arm('l', PR2ArmJointTrajectory, timeout=0)
00046         self.r_arm = create_pr2_arm('r', PR2ArmJointTrajectory, timeout=0)
00047 
00048     def execute(self, userdata):
00049         r = rospy.Rate(20)
00050         while not rospy.is_shutdown():
00051             l_tor = self.l_arm.get_joint_efforts()
00052             r_tor = self.r_arm.get_joint_efforts()
00053             if np.any(l_tor < self.min_l_tor):
00054                 print "Collision detected on left arm with torque:", l_tor
00055                 print "Minimum torques:", self.min_l_tor
00056                 return 'collision'
00057             if np.any(l_tor > self.max_l_tor):
00058                 print "Collision detected on left arm with torque:", l_tor
00059                 print "Maximum torques:", self.max_l_tor
00060                 return 'collision'
00061             if np.any(r_tor < self.min_r_tor):
00062                 print "Collision detected on right arm with torque:", r_tor
00063                 print "Minimum torques:", self.min_r_tor
00064                 return 'collision'
00065             if np.any(r_tor > self.max_r_tor):
00066                 print "Collision detected on right arm with torque:", r_tor
00067                 print "Minimum torques:", self.max_r_tor
00068                 return 'collision'
00069                 
00070             if self.preempt_requested():
00071                 self.service_preempt()
00072                 return 'preempted'
00073             r.sleep()
00074         return 'aborted'
00075 
00076 class LaserCollisionDetection(smach.State):
00077     def __init__(self):
00078         smach.State.__init__(self, outcomes=['collision', 'preempted', 'aborted'])
00079         self.cs = CostmapServices(accum=3)
00080 
00081     def execute(self, userdata):
00082         r = rospy.Rate(20)
00083         while not rospy.is_shutdown():
00084             vx, vy, vtheta = 0., 0., 0. #TODO REMOVE
00085             score = self.cs.scoreTraj_PosHyst(vx, vy, vtheta) # TODO
00086             if score < 0:
00087                 print "Base laser detected a collision."
00088                 return 'collision'
00089             if self.preempt_requested():
00090                 self.service_preempt()
00091                 return 'preempted'
00092             r.sleep()
00093         return 'aborted'
00094 
00095 def build_cc_servoing(viz_servos):
00096     def term_cb(outcome_map):
00097         return True
00098     def out_cb(outcome_map):
00099         if outcome_map['ARM_COLLISION_DETECTION'] == 'collision':
00100             return 'arm_collision'
00101         if outcome_map['LASER_COLLISION_DETECTION'] == 'collision':
00102             return 'laser_collision'
00103         if outcome_map['USER_PREEMPT_DETECTION'] == 'true':
00104             return 'user_preempted'
00105         if outcome_map['USER_PREEMPT_DETECTION'] == 'false':
00106             return 'aborted'
00107         return outcome_map['SERVOING']
00108 
00109     cc_servoing = smach.Concurrence(
00110                             outcomes=outcomes_spa+
00111                             ['arm_collision', 'laser_collision', 'lost_tag', 'user_preempted'],
00112                             input_keys=['goal_ar_pose', 'initial_ar_pose', 'servo_topic'],
00113                             default_outcome='aborted',
00114                             child_termination_cb=term_cb,
00115                             outcome_cb=out_cb)
00116     
00117     with cc_servoing:
00118         smach.Concurrence.add('SERVOING',
00119                               ServoARTagState(viz_servos))
00120 
00121         smach.Concurrence.add('ARM_COLLISION_DETECTION',
00122                               ArmCollisionDetection())
00123 
00124         smach.Concurrence.add('LASER_COLLISION_DETECTION',
00125                               LaserCollisionDetection())
00126 
00127         smach.Concurrence.add('USER_PREEMPT_DETECTION',
00128                               BoolTopicState("/pr2_ar_servo/preempt"))
00129                               
00130     return cc_servoing
00131 
00132 class BoolTopicState(smach.State):
00133     def __init__(self, topic, rate=20):
00134         smach.State.__init__(self, outcomes=['true', 'false', 'preempted', 'aborted'])
00135         self.rate = rate
00136         self.ui_list = rospy.Subscriber(topic, Bool, self.ui_cb)
00137     
00138     def ui_cb(self, msg):
00139         self.bool_msg = msg.data
00140         self.got_msg = True
00141 
00142     def execute(self, userdata):
00143         self.got_msg = False
00144         r = rospy.Rate(self.rate)
00145         while not rospy.is_shutdown():
00146             if self.got_msg:
00147                 if self.bool_msg:
00148                     return 'true'
00149                 else:
00150                     return 'false'
00151             if self.preempt_requested():
00152                 self.service_preempt()
00153                 return 'preempted'
00154             r.sleep()
00155         return 'aborted'
00156 
00157 class PublishState(smach.State):
00158     def __init__(self, topic, msg_type, msg):
00159         smach.State.__init__(self, outcomes=['succeeded'])
00160         self.pub = rospy.Publisher(topic, msg_type)
00161         self.msg = msg
00162 
00163     def execute(self, userdata):
00164         self.pub.publish(self.msg)
00165         return 'succeeded'
00166 
00167 class FindARTagState(smach.State):
00168     def __init__(self, viz_servos, viz_servo_goals, timeout=6.):
00169         smach.State.__init__(self, 
00170                              outcomes=['found_tag', 'timeout', 'preempted', 'aborted'],
00171                              output_keys=['initial_ar_pose', 'goal_ar_pose', 'servo_topic'])
00172         self.viz_servos = viz_servos
00173         self.viz_servo_goals = viz_servo_goals
00174         self.timeout = timeout
00175 
00176     def execute(self, userdata):
00177         # This is just a mess.
00178         # Figure this out and rewrite it...
00179         def check_preempt(event):
00180             if self.preempt_requested():
00181                 self.service_preempt()
00182                 for topic in self.viz_servos:
00183                     self.viz_servos[topic].request_preempt()
00184                 
00185         preempt_timer = rospy.Timer(rospy.Duration(0.1), check_preempt)
00186         self.outcome_dict = {}
00187         def call_find_ar_tag(te, viz_servo, fart_state, topic):
00188             mean_ar, outcome = viz_servo.find_ar_tag(self.timeout)
00189             self.outcome_dict[topic] = (mean_ar, outcome)
00190             if outcome == "found_tag":
00191                 self.request_preempt()
00192         for topic in self.viz_servos:
00193             call_find_ar_tag_filled = functools.partial(call_find_ar_tag, 
00194                                                         viz_servo=self.viz_servos[topic],
00195                                                         fart_state=self,
00196                                                         topic=topic)
00197             rospy.Timer(rospy.Duration(0.01), call_find_ar_tag_filled, oneshot=True)
00198         while not rospy.is_shutdown():
00199             if self.preempt_requested() or len(self.outcome_dict) == len(self.viz_servos):
00200                 break
00201             rospy.sleep(0.05)
00202         outcome = "timeout"
00203         for topic in self.outcome_dict:
00204             if self.outcome_dict[topic][1] == "found_tag":
00205                 userdata['initial_ar_pose'] = self.outcome_dict[topic][0]
00206                 userdata['goal_ar_pose'] = self.viz_servo_goals[topic]
00207                 userdata['servo_topic'] = topic
00208                 outcome = "found_tag"
00209                 # FIXME I should be shot for these lines: FIXME
00210                 if topic == "r_pr2_ar_pose_marker":
00211                     rospy.set_param("/shaving_side", 'r')
00212                 else:
00213                     rospy.set_param("/shaving_side", 'l')
00214                 ##################################################
00215                 break
00216             
00217         preempt_timer.shutdown()
00218         return outcome
00219 
00220 class ServoARTagState(smach.State):
00221     def __init__(self, viz_servos):
00222         smach.State.__init__(self, 
00223                              outcomes=['lost_tag'] + outcomes_spa,
00224                              input_keys=['goal_ar_pose', 'initial_ar_pose', 'servo_topic'])
00225         self.viz_servos = viz_servos
00226 
00227     def execute(self, userdata):
00228         if 'initial_ar_pose' in userdata:
00229             initial_ar_pose = userdata.initial_ar_pose
00230         else:
00231             rospy.logerr("Initial AR pose should be in userdata")
00232             initial_ar_pose = None
00233 
00234         servo_topic = userdata['servo_topic']
00235         def check_preempt(event):
00236             if self.preempt_requested():
00237                 self.service_preempt()
00238                 self.viz_servos[servo_topic].request_preempt()
00239                 
00240         preempt_timer = rospy.Timer(rospy.Duration(0.1), check_preempt)
00241         self.viz_servos[servo_topic].preempt_requested = False
00242         outcome = self.viz_servos[servo_topic].servo_to_tag(pose_goal=userdata.goal_ar_pose,
00243                                                             initial_ar_pose=initial_ar_pose)
00244         preempt_timer.shutdown()
00245         return outcome
00246 
00247 def build_full_sm(viz_servo_goals):
00248     find_tag_timeout = 6.
00249 
00250     viz_servos = {}
00251     for viz_servo_topic in viz_servo_goals:
00252         viz_servos[viz_servo_topic] = PR2VisualServoAR(viz_servo_topic)
00253 
00254     sm_pr2_servoing = smach.StateMachine(outcomes=outcomes_spa,
00255                                          input_keys=['goal_ar_pose', 'initial_ar_pose'])
00256     with sm_pr2_servoing:
00257 
00258         smach.StateMachine.add('UI_FIND_TAG_WAIT',
00259                                BoolTopicState("/pr2_ar_servo/find_tag"),
00260                                transitions={'true' : 'BEGIN_FIND_TAG',
00261                                             'false' : 'aborted'})
00262 
00263         smach.StateMachine.add('BEGIN_FIND_TAG',
00264                                PublishState("/pr2_ar_servo/state_feedback",
00265                                             Int8, Int8(ServoStates.BEGIN_FIND_TAG)),
00266                                transitions={'succeeded' : 'FIND_AR_TAG'})
00267 
00268         smach.StateMachine.add('FIND_AR_TAG',
00269                                FindARTagState(viz_servos, viz_servo_goals, timeout=find_tag_timeout),
00270                                transitions={'found_tag' : 'FOUND_TAG',
00271                                             'timeout' : 'TIMEOUT_FIND_TAG'})
00272 
00273         smach.StateMachine.add('FOUND_TAG',
00274                                PublishState("/pr2_ar_servo/state_feedback",
00275                                             Int8, Int8(ServoStates.FOUND_TAG)),
00276                                transitions={'succeeded' : 'UI_SERVO_WAIT'})
00277 
00278         smach.StateMachine.add('TIMEOUT_FIND_TAG',
00279                                PublishState("/pr2_ar_servo/state_feedback",
00280                                             Int8, Int8(ServoStates.TIMEOUT_FIND_TAG)),
00281                                transitions={'succeeded' : 'UI_FIND_TAG_WAIT'})
00282 
00283         smach.StateMachine.add('UI_SERVO_WAIT',
00284                                BoolTopicState("/pr2_ar_servo/tag_confirm"),
00285                                transitions={'true' : 'BEGIN_SERVO',
00286                                             'false' : 'UI_FIND_TAG_WAIT'})
00287 
00288         smach.StateMachine.add('BEGIN_SERVO',
00289                                PublishState("/pr2_ar_servo/state_feedback",
00290                                             Int8, Int8(ServoStates.BEGIN_SERVO)),
00291                                transitions={'succeeded' : 'CC_SERVOING'})
00292 
00293         smach.StateMachine.add('CC_SERVOING', 
00294                                build_cc_servoing(viz_servos),
00295                                transitions={'arm_collision' : 'ARM_COLLISION',
00296                                             'laser_collision' : 'LASER_COLLISION',
00297                                             'user_preempted' : 'USER_PREEMPT',
00298                                             'lost_tag' : 'LOST_TAG',
00299                                             'succeeded' : 'SUCCESS_SERVO'})
00300 
00301         smach.StateMachine.add('SUCCESS_SERVO',
00302                                PublishState("/pr2_ar_servo/state_feedback",
00303                                             Int8, Int8(ServoStates.SUCCESS_SERVO)),
00304                                transitions={'succeeded' : 'UI_FIND_TAG_WAIT'})
00305 
00306         smach.StateMachine.add('ARM_COLLISION',
00307                                PublishState("/pr2_ar_servo/state_feedback",
00308                                             Int8, Int8(ServoStates.ARM_COLLISION)),
00309                                transitions={'succeeded' : 'UI_FIND_TAG_WAIT'})
00310 
00311         smach.StateMachine.add('LASER_COLLISION',
00312                                PublishState("/pr2_ar_servo/state_feedback",
00313                                             Int8, Int8(ServoStates.LASER_COLLISION)),
00314                                transitions={'succeeded' : 'UI_FIND_TAG_WAIT'})
00315 
00316         smach.StateMachine.add('LOST_TAG',
00317                                PublishState("/pr2_ar_servo/state_feedback",
00318                                             Int8, Int8(ServoStates.LOST_TAG)),
00319                                transitions={'succeeded' : 'UI_FIND_TAG_WAIT'})
00320 
00321         smach.StateMachine.add('USER_PREEMPT',
00322                                PublishState("/pr2_ar_servo/state_feedback",
00323                                             Int8, Int8(ServoStates.USER_PREEMPT)),
00324                                transitions={'succeeded' : 'UI_FIND_TAG_WAIT'})
00325 
00326     return sm_pr2_servoing
00327 
00328 def build_test_sm():
00329     viz_servo = PR2VisualServoAR("r_pr2_ar_pose_marker")
00330     
00331     sm_only_servo = smach.StateMachine(outcomes=['lost_tag'] + outcomes_spa,
00332                                        input_keys=['goal_ar_pose', 'initial_ar_pose'])
00333     with sm_only_servo:
00334         smach.StateMachine.add('ONLY_SERVO',
00335                                ServoARTagState(viz_servo))
00336     return sm_only_servo
00337 
00338 def main():
00339     rospy.init_node("sm_pr2_servoing")
00340     userdata = smach.user_data.UserData()
00341 
00342     # old testing
00343     #userdata['goal_ar_pose'] = [ 0.57160106, -0.4300153 , -1.70840111]
00344 
00345     # both sides spot?
00346     userdata['goal_ar_pose'] = [ 0.57226345,  0.32838129, -1.15480113]
00347     viz_servo_goals = rospy.get_param("~ar_servo_poses", {})
00348     print viz_servo_goals
00349 
00350     if True:
00351         sm_pr2_servoing = build_full_sm(viz_servo_goals)
00352         sis = smach_ros.IntrospectionServer('pr2_servo', sm_pr2_servoing, 'UI_FIND_TAG_WAIT')
00353         sis.start()
00354         sm_pr2_servoing.execute(userdata)
00355         sis.stop()
00356     else:
00357         sm_test = build_test_sm()
00358         rospy.sleep(4)
00359         sm_test.execute(userdata)
00360 
00361 if __name__ == "__main__":
00362     main()


kelsey_sandbox
Author(s): kelsey
autogenerated on Wed Nov 27 2013 11:52:04