def main():
rospy.init_node('fake_dm', log_level=rospy.DEBUG)
rospy.loginfo("Script for testing but arm manual manipulation")
# sm_to = move_arm_to_given_positions_assisted()
sm = smach.StateMachine(outcomes=[
'fdm_completed', 'fdm_not_completed', 'fdm_failed', 'fdm_pre-empted'
],
output_keys=['id_of_the_reached_position'])
with sm:
smach.StateMachine.add('simulate_dm',
simulate_dm(),
transitions={
'completed': 'to',
'not_completed': 'fdm_not_completed',
'pre-empted': 'fdm_pre-empted',
'failed': 'fdm_failed'
})
smach.StateMachine.add('to',
move_arm_to_given_positions_assisted(),
transitions={
'completed': 'from',
'not_completed': 'from',
'pre-empted': 'from',
'failed': 'from',
'repeat': 'simulate_dm'
},
remapping={
'list_of_target_positions':
'list_of_target_positions',
'list_of_id_for_target_positions':
'list_of_id_for_target_positions',
'name_of_the_target_object':
'name_of_the_target_object',
'pose_of_the_target_object':
'pose_of_the_target_object',
'bb_of_the_target_object':
'bb_of_the_target_object'
}) # is remapping neccessary?
smach.StateMachine.add('from',
move_arm_from_a_given_position_assisted(),
transitions={
'completed': 'fdm_completed',
'not_completed': 'fdm_not_completed',
'pre-empted': 'fdm_pre-empted',
'failed': 'fdm_failed'
})
rospy.loginfo('Executing state machine...')
output = sm.execute()
rospy.loginfo("ID of the reached position: %d",
sm.userdata.id_of_the_reached_position)
def main():
rospy.init_node('fake_dm', log_level=rospy.DEBUG)
rospy.loginfo("Script for testing but arm manual manipulation")
# sm_to = move_arm_to_given_positions_assisted()
sm = smach.StateMachine(outcomes=['fdm_completed','fdm_not_completed','fdm_failed','fdm_pre-empted'])
with sm:
smach.StateMachine.add('simulate_dm', simulate_dm(),
transitions={'completed':'to',
'not_completed':'fdm_not_completed',
'pre-empted':'fdm_pre-empted',
'failed':'fdm_failed'})
smach.StateMachine.add('to', move_arm_to_given_positions_assisted(),
transitions={'completed':'from',
'not_completed':'from',
'pre-empted':'from',
'failed':'from',
'repeat': 'simulate_dm'},
remapping={'list_of_target_positions':'list_of_target_positions',
'list_of_id_for_target_positions':'list_of_id_for_target_positions',
'name_of_the_target_object':'name_of_the_target_object',
'pose_of_the_target_object':'pose_of_the_target_object',
'bb_of_the_target_object':'bb_of_the_target_object'}) # is remapping neccessary?
smach.StateMachine.add('from',move_arm_from_a_given_position_assisted(),
transitions={'completed':'grasp',
'not_completed':'fdm_not_completed',
'pre-empted':'fdm_pre-empted',
'failed':'fdm_failed'})
smach.StateMachine.add('grasp',grasp_unknown_object_assisted(),
transitions={'completed':'fdm_completed',
'not_completed':'fdm_not_completed',
'pre-empted':'fdm_pre-empted',
'failed':'fdm_failed'})
rospy.loginfo('Executing state machine...')
output = sm.execute()
def assisted_arm_navigation(self, sm):
rospy.loginfo("[UI_BUT]: Executing assisted_arm_navigation state machine...")
_feedback = ui_butFeedback()
_feedback.feedback = "assisted_arm_navigation: init"
self._as.publish_feedback(_feedback)
with sm:
rospy.logwarn("assisted_arm_navigation: Using simulated params...");
smach.StateMachine.add('simulate_dm', simulate_dm(),
transitions={'completed':'move_arm_to_given_positions_assisted',
'not_completed':'not_completed',
'pre-empted':'pre-empted',
'failed':'failed'})
smach.StateMachine.add('move_arm_to_given_positions_assisted', move_arm_to_given_positions_assisted(),
transitions={'completed':'move_arm_from_a_given_position_assisted',
'not_completed':'move_arm_from_a_given_position_assisted',
'pre-empted':'move_arm_from_a_given_position_assisted',
'failed':'move_arm_from_a_given_position_assisted',
'repeat': 'move_arm_to_given_positions_assisted'},
remapping={'list_of_target_positions':'list_of_target_positions',
'list_of_id_for_target_positions':'list_of_id_for_target_positions',
'name_of_the_target_object':'name_of_the_target_object',
'pose_of_the_target_object':'pose_of_the_target_object',
'bb_of_the_target_object':'bb_of_the_target_object'})
smach.StateMachine.add('move_arm_from_a_given_position_assisted', move_arm_from_a_given_position_assisted(),
transitions={'completed':'completed',
'not_completed':'not_completed',
'pre-empted':'pre-empted',
'failed':'failed'})
output = sm.execute()
rospy.loginfo("[UI_BUT]: ...assisted_arm_navigation state has finished.")
return output
