00001
00002
00003
00004 import roslib; roslib.load_manifest('iri_clean_board')
00005 import rospy
00006 import smach
00007 import smach_ros
00008
00009 from smach_ros import SimpleActionState
00010 from smach_ros import ServiceState
00011
00012 import os
00013 import time
00014 import shutil
00015
00016 from actionlib import *
00017 from actionlib.msg import *
00018
00019 from iri_common_smach import st_sleep
00020 from iri_common_smach import timed_state
00021 from iri_common_smach import timed_service_state
00022
00023 from iri_clean_board.srv import *
00024 from estirabot_msgs.msg import *
00025 from estirabot_msgs.srv import *
00026 from iri_planning_msgs.srv import *
00027
00028 class NeedInfo(smach.State):
00029 def __init__(self):
00030 smach.State.__init__(self, outcomes=['next','failed'])
00031 self.counter = 0
00032
00033 def execute(self, userdata):
00034 rospy.logdebug('Executing state NeedInfo')
00035 return 'next'
00036
00037
00038 class PradaPlanning(timed_state.TimedState):
00039 def __init__(self):
00040 timed_state.TimedState.__init__(self, outcomes=['succeeded','preempted','aborted','no_plan'],
00041 input_keys=['rules_updated_sm'],
00042 output_keys=['target_ellipses','actions'])
00043 self.service_topic = '/iri_clean_plan/prada_planner/getBestAction'
00044
00045 def timed_execute(self, userdata):
00046 rospy.logdebug('Executing state PradaPlanning')
00047 rospy.wait_for_service(self.service_topic)
00048 try:
00049 srv_client = rospy.ServiceProxy(self.service_topic, PradaPlanSrv)
00050 resp = srv_client(userdata.rules_updated_sm)
00051
00052 if resp.succeeded:
00053 userdata.target_ellipses = resp.target_ellipses
00054 userdata.actions = resp.actions
00055 return 'succeeded'
00056 else:
00057 return 'no_plan'
00058
00059 except rospy.ServiceException, e:
00060 print "Service call failed: %s"%e
00061 return 'aborted'
00062
00063
00064 class CalculateTrajectories(timed_state.TimedState):
00065 def __init__(self):
00066 timed_state.TimedState.__init__(self, outcomes=['succeeded','preempted','aborted','no_movements'],
00067 input_keys=['dirty_areas_sm','target_ellipses_sm','actions_sm','ellipses_sm','segmented_pointcloud_sm','plane_coefficients_sm'],
00068 output_keys=['poses','poses_2d_coordinates','secondary_arm'])
00069 self.service_topic = '/iri_clean_plan/cleaning_plan/get_trajectories_from_plan'
00070 self.sequence_actions = ""
00071
00072 def timed_execute(self, userdata):
00073 self.sequence_actions = self.sequence_actions + " - " + userdata.actions_sm[0]
00074 rospy.logdebug('Executing state CalculateTrajectories')
00075 rospy.loginfo('Will execute action: ' + userdata.actions_sm[0])
00076 rospy.wait_for_service(self.service_topic)
00077 try:
00078 get_cleaning_plan = rospy.ServiceProxy(self.service_topic, PlanToTrajectories)
00079 resp = get_cleaning_plan(userdata.segmented_pointcloud_sm, userdata.plane_coefficients_sm, userdata.dirty_areas_sm, userdata.target_ellipses_sm, userdata.actions_sm)
00080
00081 if resp.movement:
00082 userdata.poses = resp.poses
00083 userdata.poses_2d_coordinates = resp.poses_2d_coordinates
00084 userdata.secondary_arm = resp.secondary_arm
00085 return 'succeeded'
00086 else:
00087 return 'no_movements'
00088
00089 except rospy.ServiceException, e:
00090 print "Service call failed: %s"%e
00091 return 'aborted'
00092
00093
00094 class GetStateRepresentation(timed_state.TimedState):
00095 def __init__(self):
00096 timed_state.TimedState.__init__(self, outcomes=['succeeded','preempted','aborted','is_clean','get_full_info'],
00097 input_keys=['segmented_image_sm','segmented_pointcloud_sm','plane_coefficients_sm'],
00098 output_keys=['dirty_areas','distances','state_string','arm_position','segmented_pointcloud_sm','plane_coefficients_sm'])
00099 self.service_topic = '/iri_clean_plan/cleaning_plan/get_dirty_areas_state_representation'
00100 self.surface_clean_perceptions = 0
00101
00102 def timed_execute(self, userdata):
00103 rospy.logdebug('Executing state GetStateRepresentation')
00104 rospy.wait_for_service(self.service_topic)
00105 try:
00106 get_state = rospy.ServiceProxy(self.service_topic, StateRepresentation)
00107 resp = get_state(userdata.segmented_image_sm)
00108
00109 if resp.is_clean:
00110 userdata.state_string = ""
00111 if self.surface_clean_perceptions < 2:
00112 self.surface_clean_perceptions += 1
00113 return 'get_full_info'
00114 else:
00115 return 'is_clean'
00116 else:
00117 self.surface_clean_perceptions = 0
00118 userdata.dirty_areas = resp.dirty_areas
00119 userdata.distances = resp.distances
00120 userdata.arm_position = resp.arm_position
00121 userdata.state_string = resp.state_string
00122 return 'succeeded'
00123 except rospy.ServiceException, e:
00124 print "Service call failed: %s"%e
00125 return 'aborted'
00126
00127
00128 class LearnerAddAction(timed_state.TimedState):
00129 def __init__(self):
00130 timed_state.TimedState.__init__(self, outcomes=['needs_learning','succeeded','preempted','aborted'],
00131 input_keys=['target_ellipses_sm','actions_sm','dirty_areas_sm','distances_sm','state_string_sm','movements_ok_sm'],
00132 output_keys=[])
00133 self.service_topic = '/iri_learning/rule_learner/add_action'
00134
00135 def timed_execute(self, userdata):
00136 rospy.logdebug('Executing state LearnerAddAction')
00137 rospy.wait_for_service(self.service_topic)
00138 try:
00139 srv_client = rospy.ServiceProxy(self.service_topic, AddAction)
00140 resp = srv_client(userdata.actions_sm[0], userdata.dirty_areas_sm, userdata.target_ellipses_sm[0], userdata.distances_sm, userdata.state_string_sm, userdata.movements_ok_sm)
00141
00142 if resp.needs_learning:
00143 return 'needs_learning'
00144 else:
00145 return 'succeeded'
00146
00147 except rospy.ServiceException, e:
00148 print "Service call failed: %s"%e
00149 return 'aborted'
00150
00151
00152 class LearnerAddState(timed_state.TimedState):
00153 def __init__(self):
00154 timed_state.TimedState.__init__(self, outcomes=['succeeded','preempted','aborted'],
00155 input_keys=['state_string_sm'],
00156 output_keys=['rules_updated'])
00157 self.service_topic = '/iri_learning/rule_learner/add_state'
00158
00159 def timed_execute(self, userdata):
00160 rospy.logdebug('Executing state LearnerAddState')
00161 rospy.wait_for_service(self.service_topic)
00162 try:
00163 srv_client = rospy.ServiceProxy(self.service_topic, AddState)
00164 resp = srv_client(userdata.state_string_sm)
00165
00166 userdata.rules_updated = resp.rules_updated
00167 return 'succeeded'
00168
00169 except rospy.ServiceException, e:
00170 print "Service call failed: %s"%e
00171 return 'aborted'
00172
00173
00174 class MoveArm(timed_state.TimedState):
00175 def __init__(self):
00176 timed_state.TimedState.__init__(self, outcomes=['succeeded','failed'],
00177 input_keys=['poses','plane_coefficients_sm','poses_2d_coordinates','secondary_arm'],
00178 output_keys=['movements_ok'])
00179
00180 def timed_execute(self, userdata):
00181 rospy.logdebug('Waiting for service')
00182 rospy.loginfo('Executing action number ' + str(self.num_executions))
00183 rospy.wait_for_service('/iri_clean_plan/arm_movements_by_pose/clean_movement')
00184 try:
00185 move_arm = rospy.ServiceProxy('/iri_clean_plan/arm_movements_by_pose/clean_movement', ArmMovementsPosesSrv)
00186 resp = move_arm(userdata.poses, userdata.secondary_arm, userdata.plane_coefficients_sm, 1)
00187
00188 if resp.success:
00189 print 'Executing service success'
00190 userdata.movements_ok = True
00191 return 'succeeded'
00192 else:
00193
00194 pub = rospy.Publisher('/iri_clean_perception/get_board_information/unreachable_position', ImagePoint)
00195 for idx in resp.too_far_points_indexes:
00196 pub.publish(userdata.poses_2d_coordinates[idx])
00197 print 'Executing service failed. Maybe IK.'
00198 userdata.movements_ok = False
00199 return 'succeeded'
00200 except rospy.ServiceException, e:
00201 print "Service call failed: %s"%e
00202 return 'failed'
00203
00204
00205 class MoveArmBase(timed_state.TimedState):
00206 def __init__(self):
00207 timed_state.TimedState.__init__(self, outcomes=['next','failed'])
00208
00209 def timed_execute(self, userdata):
00210 rospy.loginfo('Waiting for service')
00211 rospy.wait_for_service('/iri_clean_plan/arm_movements_by_pose/clean_movement')
00212 try:
00213
00214 clean_mov = ArmMovementsPosesSrv()
00215 clean_mov.movement_type = 0
00216 clean_mov.poses = [];
00217 move_arm = rospy.ServiceProxy('/iri_clean_plan/arm_movements_by_pose/clean_movement', ArmMovementsPosesSrv)
00218 resp = move_arm(clean_mov.poses, [], [], 0)
00219
00220 if resp.success:
00221 print 'Executing service success'
00222 return 'next'
00223 else:
00224 print 'Executing service failed'
00225 return 'next'
00226 except rospy.ServiceException, e:
00227 print "Service call failed: %s"%e
00228 return 'failed'
00229
00230
00231 class MoveArmBaseWait(timed_state.TimedState):
00232 def __init__(self):
00233 timed_state.TimedState.__init__(self, outcomes=['next','failed'])
00234
00235 def timed_execute(self, userdata):
00236 rospy.loginfo('Waiting for service')
00237 rospy.wait_for_service('/iri_clean_plan/arm_movements_by_pose/clean_movement')
00238 try:
00239
00240 clean_mov = ArmMovementsPosesSrv()
00241 clean_mov.movement_type = 0
00242 clean_mov.poses = [];
00243 move_arm = rospy.ServiceProxy('/iri_clean_plan/arm_movements_by_pose/clean_movement', ArmMovementsPosesSrv)
00244 resp = move_arm(clean_mov.poses, [], [], 0)
00245
00246 if resp.success:
00247 print 'Executing service success'
00248 return 'next'
00249 else:
00250 print 'Executing service failed'
00251 return 'next'
00252 except rospy.ServiceException, e:
00253 print "Service call failed: %s"%e
00254 return 'failed'
00255
00256
00257
00258 def main():
00259 try:
00260 f = open('/tmp/pasula_logs.txt', 'r') # read previous contents
00261 old_pasula_time = float(f.readline())
00262 old_iterations = int(f.readline())
00263 except:
00264 old_pasula_time = 0.0
00265 old_iterations = 0
00266 finally:
00267 f.close()
00268
00269 rospy.init_node('smach_example_state_machine')
00270
00271
00272 sm = smach.StateMachine(outcomes=['finishOk','failedVision','failedPlanning','failedMoveArm'])
00273
00274 st_get_info_camera = timed_service_state.TimedServiceState('/iri_clean_perception/get_board_information/get_board_info_srv',
00275 BoardInfo,
00276 response_slots=['segmented_image',
00277 'pointcloud',
00278 'plane_coefficients'])
00279 st_learner_add_state = LearnerAddState()
00280 st_get_state_representation = GetStateRepresentation()
00281 st_prada_planning = PradaPlanning()
00282 st_calculate_trajectories = CalculateTrajectories()
00283 st_move_arm = MoveArm()
00284 st_learner_add_action = LearnerAddAction()
00285 st_move_arm_base = MoveArmBase()
00286 st_move_arm_base_wait = MoveArmBaseWait()
00287
00288 sleep_time = 2
00289 st_wait_seconds = st_sleep.WaitSeconds(sleep_time)
00290
00291
00292 with sm:
00293
00294 smach.StateMachine.add('NeedInfo', NeedInfo(),
00295 transitions={'next':'GetInfoCamera',
00296 'failed':'failedVision'},)
00297
00298 smach.StateMachine.add('GetInfoCamera', st_get_info_camera,
00299 transitions={'succeeded':'GetStateRepresentation',
00300 'preempted':'failedVision',
00301 'aborted':'failedVision'},
00302 remapping={'segmented_image':'segmented_image_sm',
00303 'pointcloud':'segmented_pointcloud_sm',
00304 'plane_coefficients':'plane_coefficients_sm'})
00305
00306 smach.StateMachine.add('GetStateRepresentation',
00307 st_get_state_representation,
00308 transitions={'is_clean':'finishOk',
00309 'get_full_info':'LearnerAddStateAndGetInfo',
00310 'succeeded':'LearnerAddState',
00311 'preempted':'failedVision',
00312 'aborted':'failedVision'},
00313 remapping={'dirty_areas':'dirty_areas_sm',
00314 'distances':'distances_sm',
00315 'state_string':'state_string_sm'})
00316
00317 smach.StateMachine.add('LearnerAddState', st_learner_add_state,
00318 transitions={'succeeded':'PradaPlanning',
00319 'preempted':'failedPlanning',
00320 'aborted':'failedPlanning'},
00321 remapping={'rules_updated':'rules_updated_sm'})
00322
00323 smach.StateMachine.add('LearnerAddStateAndGetInfo', st_learner_add_state,
00324 transitions={'succeeded':'MoveArmBaseAndWait',
00325 'preempted':'failedPlanning',
00326 'aborted':'failedPlanning'},
00327 remapping={'rules_updated':'rules_updated_sm'})
00328
00329 smach.StateMachine.add('PradaPlanning',
00330 st_prada_planning,
00331 transitions={'succeeded':'CalculateTrajectories',
00332 'preempted':'failedPlanning',
00333 'aborted':'failedPlanning',
00334 'no_plan':'NeedInfo'},
00335 remapping={'target_ellipses':'target_ellipses_sm',
00336 'actions':'actions_sm'})
00337
00338
00339
00340 smach.StateMachine.add('CalculateTrajectories',
00341 st_calculate_trajectories,
00342 transitions={'succeeded':'MoveArm',
00343 'preempted':'failedVision',
00344 'aborted':'failedVision',
00345 'no_movements':'NeedInfo'})
00346
00347
00348 smach.StateMachine.add('MoveArm', st_move_arm,
00349 transitions={'succeeded':'LearnerAddAction',
00350 'failed':'failedMoveArm'},
00351 remapping={'movements_ok':'movements_ok_sm'})
00352
00353 smach.StateMachine.add('LearnerAddAction',
00354 st_learner_add_action,
00355 transitions={'needs_learning':'MoveArmBaseAndWait',
00356 'succeeded':'NeedInfo',
00357 'preempted':'failedPlanning',
00358 'aborted':'failedPlanning'})
00359
00360 smach.StateMachine.add('MoveArmBase', st_move_arm_base,
00361 transitions={'next':'WaitASecond',
00362 'failed':'failedMoveArm'})
00363
00364 smach.StateMachine.add('MoveArmBaseAndWait', st_move_arm_base_wait,
00365 transitions={'next':'WaitASecond',
00366 'failed':'failedMoveArm'})
00367
00368 smach.StateMachine.add('WaitASecond', st_wait_seconds,
00369 transitions={'finish':'NeedInfo'})
00370
00371
00372 sis = smach_ros.IntrospectionServer('iri_cleaning', sm, '/SM_LEARNING')
00373 sis.start()
00374
00375 start_time = time.time()
00376
00377
00378 try:
00379 outcome = sm.execute()
00380 finally:
00381 sis.stop()
00382 total_time = time.time() - start_time
00383
00384
00385 time_perception = st_get_info_camera.get_execution_time()
00386 time_planning = st_prada_planning.get_execution_time()
00387 time_learning_state = st_learner_add_state.get_execution_time()
00388 time_state = st_get_state_representation.get_execution_time()
00389 time_state_traj = st_calculate_trajectories.get_execution_time()
00390 time_moving = st_move_arm.get_execution_time()
00391 time_learning_action = st_learner_add_action.get_execution_time()
00392 time_moving_home = st_move_arm_base.get_execution_time()
00393 time_extra_learn = st_move_arm_base_wait.get_execution_time()
00394
00395
00396 num_actions = st_move_arm.get_number_executions()
00397 num_learned_actions = st_move_arm_base_wait.get_number_executions()
00398 sequence_actions = st_calculate_trajectories.sequence_actions
00399
00400
00401 time_state = time_state + time_state_traj
00402 time_learning = time_learning_action + time_learning_state
00403
00404
00405
00406 time_extra_learn = time_extra_learn + (num_learned_actions*sleep_time)
00407
00408
00409 try:
00410 f = open('/tmp/pasula_logs.txt', 'r')
00411 pasula_time = float(f.readline()) - old_pasula_time
00412 iterations = int(f.readline()) - old_iterations
00413 total_rules = int(f.readline())
00414 pasula_rules = int(f.readline())
00415 learned_rules = int(f.readline())
00416 except:
00417 rospy.logwarn('Exception reading pasula logs')
00418 pasula_time = 0.0
00419 iterations = 0
00420 total_rules = 0
00421 pasula_rules = 0
00422 learned_rules = 0
00423 finally:
00424 f.close()
00425
00426
00427 rospy.loginfo('Time taken: ' + str(total_time))
00428 rospy.loginfo('Time taken perception: ' + str(time_perception))
00429 rospy.loginfo('Time taken state: ' + str(time_state))
00430 rospy.loginfo('Time taken planning: ' + str(time_planning))
00431 rospy.loginfo('Time taken learning: ' + str(time_learning))
00432 rospy.loginfo('Time taken moving: ' + str(time_moving))
00433 rospy.loginfo('Time taken moving_home: ' + str(time_moving_home))
00434 rospy.loginfo('Time taken extra_learn: ' + str(time_extra_learn))
00435 rospy.loginfo('Time taken pasula: ' + str(pasula_time))
00436 rospy.loginfo('Number of pasula iterations: ' + str(iterations))
00437 rospy.loginfo('Number of rules: ' + str(total_rules))
00438 rospy.loginfo('Number of pasula rules: ' + str(pasula_rules))
00439 rospy.loginfo('Number of learned pasula rules: ' + str(learned_rules))
00440 rospy.loginfo('Number of actions: ' + str(num_actions))
00441 rospy.loginfo('Number of learned actions: ' + str(num_learned_actions))
00442 rospy.loginfo('Sequence of actions: ' + sequence_actions)
00443
00444 if len(sys.argv) > 1:
00445 file_name = sys.argv[2]
00446 f = open(file_name, 'a')
00447 f.write('\n')
00448 f.write('Time taken: ' + str(total_time) + '\n')
00449 f.write('Time taken perception: ' + str(time_perception) + '\n')
00450 f.write('Time taken state: ' + str(time_state) + '\n')
00451 f.write('Time taken planning: ' + str(time_planning) + '\n')
00452 f.write('Time taken learning: ' + str(time_learning) + '\n')
00453 f.write('Time taken moving: ' + str(time_moving) + '\n')
00454 f.write('Time taken moving home: ' + str(time_moving_home) + '\n')
00455 f.write('Time taken extra_learn: ' + str(time_extra_learn) + '\n')
00456 f.write('Time taken pasula: ' + str(pasula_time) + '\n')
00457 f.write('Number of pasula iterations: ' + str(iterations) + '\n')
00458 f.write('Number of rules: ' + str(total_rules) + '\n')
00459 f.write('Number of pasula rules: ' + str(pasula_rules) + '\n')
00460 f.write('Number of learned pasula rules: ' + str(learned_rules) + '\n')
00461 f.write('Number of actions: ' + str(num_actions) + '\n')
00462 f.write('Number of learned actions: ' + str(num_learned_actions) + '\n')
00463 f.write('Sequence of actions: ' + sequence_actions + '\n')
00464 f.close()
00465
00466 copy_path='./debugging_rules_' + str(int(time.time()) - 1360000000)
00467 shutil.copy2('/home/dmartinez/ros/iri-ros-pkg/estirabot_robot/iri_clean_board/ext_cfg/debugging_rules.dat', copy_path)
00468 copy_path='./rules_learning_' + str(int(time.time()) - 1360000000)
00469 shutil.copy2('/home/dmartinez/ros/iri-ros-pkg/estirabot_robot/iri_clean_board/ext_cfg/rules_learning.dat', copy_path)
00470 copy_path='./transitions_' + str(int(time.time()) - 1360000000)
00471 shutil.copy2('/home/dmartinez/ros/iri-ros-pkg/estirabot_robot/iri_clean_board/ext_cfg/backup_transitions.dat', copy_path)
00472
00473
00474
00475 if __name__ == '__main__':
00476 main()
00477