00001
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
00022
00023
00024
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.
00085 score = self.cs.scoreTraj_PosHyst(vx, vy, vtheta)
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
00178
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
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
00343
00344
00345
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()