コード例 #1
0
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)
コード例 #2
0
ファイル: fake_dm.py プロジェクト: beds-tao/srs_public
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()
コード例 #3
0
	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