def main():
rospy.init_node('unstage_object_server')
# Construct state machine
sm = smach.StateMachine(outcomes=['OVERALL_SUCCESS', 'OVERALL_FAILED'],
input_keys=['goal'],
output_keys=['feedback', 'result'])
with sm:
#add states to the container
smach.StateMachine.add('MOVE_ARM_TO_STAGE_INTERMEDIATE',
gms.move_arm('stage_intermediate'),
transitions={
'succeeded':
'MOVE_ARM_TO_STAGE_INTERMEDIATE_2',
'failed': 'MOVE_ARM_TO_STAGE_INTERMEDIATE'
})
smach.StateMachine.add('MOVE_ARM_TO_STAGE_INTERMEDIATE_2',
gms.move_arm('stage_intermediate_2'),
transitions={
'succeeded': 'SETUP_MOVE_ARM_PRE_STAGE',
'failed': 'MOVE_ARM_TO_STAGE_INTERMEDIATE_2'
})
smach.StateMachine.add('SETUP_MOVE_ARM_PRE_STAGE',
SetupMoveArm('pre'),
transitions={'succeeded': 'MOVE_ARM_PRE_STAGE'})
smach.StateMachine.add('MOVE_ARM_PRE_STAGE',
gms.move_arm_and_gripper('open_narrow'),
transitions={
'succeeded': 'SETUP_MOVE_ARM_STAGE',
'failed': 'MOVE_ARM_PRE_STAGE'
})
smach.StateMachine.add('SETUP_MOVE_ARM_STAGE',
SetupMoveArm('final'),
transitions={'succeeded': 'MOVE_ARM_STAGE'})
smach.StateMachine.add('MOVE_ARM_STAGE',
gms.move_arm(),
transitions={
'succeeded': 'CLOSE_GRIPPER',
'failed': 'MOVE_ARM_STAGE'
})
smach.StateMachine.add(
'CLOSE_GRIPPER',
gms.control_gripper('close'),
transitions={'succeeded': 'SETUP_MOVE_ARM_PRE_STAGE_AGAIN'})
#TODO: verify if object is grasped or not
smach.StateMachine.add(
'SETUP_MOVE_ARM_PRE_STAGE_AGAIN',
SetupMoveArm('pre'),
transitions={'succeeded': 'MOVE_ARM_PRE_STAGE_AGAIN'})
smach.StateMachine.add('MOVE_ARM_PRE_STAGE_AGAIN',
gms.move_arm(),
transitions={
'succeeded':
'MOVE_ARM_TO_STAGE_INTERMEDIATE_2_FINAL',
'failed': 'MOVE_ARM_PRE_STAGE_AGAIN'
})
smach.StateMachine.add('MOVE_ARM_TO_STAGE_INTERMEDIATE_2_FINAL',
gms.move_arm('stage_intermediate_2'),
transitions={
'succeeded':
'MOVE_ARM_TO_STAGE_INTERMEDIATE_FINAL',
'failed':
'MOVE_ARM_TO_STAGE_INTERMEDIATE_2_FINAL'
})
smach.StateMachine.add('MOVE_ARM_TO_STAGE_INTERMEDIATE_FINAL',
gms.move_arm('stage_intermediate'),
transitions={
'succeeded': 'OVERALL_SUCCESS',
'failed':
'MOVE_ARM_TO_STAGE_INTERMEDIATE_FINAL'
})
# smach viewer
if rospy.get_param('~viewer_enabled', False):
sis = smach_ros.IntrospectionServer('unstage_object_smach_viewer', sm,
'/UNSTAGE_OBJECT_SMACH_VIEWER')
sis.start()
# Construct action server wrapper
asw = ActionServerWrapper(server_name='unstage_object_server',
action_spec=GenericExecuteAction,
wrapped_container=sm,
succeeded_outcomes=['OVERALL_SUCCESS'],
aborted_outcomes=['OVERALL_FAILED'],
preempted_outcomes=['PREEMPTED'],
goal_key='goal',
feedback_key='feedback',
result_key='result')
# Run the server in a background thread
asw.run_server()
rospy.spin()
def main():
rospy.init_node("ros_smach_server")
waypoints1 = way_point1()
waypoints2 = way_point2()
stop_points = stop_point_get()
object_point = waypoints1.poses[0:3]
object_index = decide_next_object()
# object_point = object_point_get()
# print waypoints.poses
sm = smach.StateMachine(outcomes=['succeeded', 'aborted', 'preempted'])
with sm:
# smach.StateMachine.add('PATH_FOLLOWING', smach_ros.SimpleActionState("path_following_action", path_followingAction, goal=path_followingGoal(waypoints=waypoints)),{'succeeded':'GRIPPER_MODE'})
sm_sub_grab = smach.StateMachine(
outcomes=['succeeded', 'aborted', 'preempted'])
with sm_sub_grab:
smach.StateMachine.add(
'APPROACH_OBJECT',
smach_ros.SimpleActionState(
"gripper_mode_action",
gripper_modeAction,
goal=gripper_modeGoal(object_point=object_point[
object_index.index].position)),
{'succeeded': 'GRASP_OBJECT'})
smach.StateMachine.add(
'GRASP_OBJECT',
smach_ros.SimpleActionState(
"gripper_grab_action",
gripper_grabAction,
goal=gripper_grabGoal(
grasping_state=True))) #,{'succeeded':'succeeded'})
smach.StateMachine.add('GRIPPER_MODE',
sm_sub_grab,
transitions={'succeeded': 'GO_TO_DESTINATION'})
sm_sub_go_destination = smach.StateMachine(
outcomes=['succeeded', 'aborted', 'preempted'])
with sm_sub_go_destination:
smach.StateMachine.add(
'GO_TO_STOP_SIGN',
smach_ros.SimpleActionState(
"path_following_action",
path_followingAction,
goal=path_followingGoal(waypoints=waypoints1)),
{'succeeded': 'DROP_OBJECT'})
smach.StateMachine.add(
'DROP_OBJECT',
smach_ros.SimpleActionState(
"gripper_grab_action",
gripper_grabAction,
goal=gripper_grabGoal(
grasping_state=False))) #,{'succeeded':'succeeded'})
smach.StateMachine.add('GO_TO_DESTINATION',
sm_sub_go_destination,
transitions={'succeeded': 'RESTART_AND_REDO'})
sm_sub_init = smach.StateMachine(
outcomes=['succeeded', 'aborted', 'preempted'])
with sm_sub_init:
smach.StateMachine.add('DECIDE_NEXT_OBJECT', decide_next_object(),
{'finished': 'GO_TO_CHECKPOINT'})
smach.StateMachine.add(
'GO_TO_CHECKPOINT',
smach_ros.SimpleActionState(
"path_following_action",
path_followingAction,
goal=path_followingGoal(
waypoints=waypoints2))) #,{'succeeded':'succeeded'})
smach.StateMachine.add('RESTART_AND_REDO',
sm_sub_init,
transitions={'succeeded': 'GRIPPER_MODE'})
sis = smach_ros.IntrospectionServer('smach_server', sm, '/SM_ROOT')
sis.start()
sm.execute()
rospy.on_shutdown(onShutdown)
rospy.spin()
sis.stop()
def construct():
rospy.logwarn('align if...')
sm = smach.StateMachine(outcomes=['preempted'])
# Attach helper functions
sm.set_ranges = MethodType(set_ranges, sm, sm.__class__)
sm.get_twist = MethodType(get_twist, sm, sm.__class__)
sm.set_config = MethodType(set_config, sm, sm.__class__)
# Set initial values in userdata
sm.userdata.velocity = (0, 0, 0)
sm.userdata.mode = 1
sm.userdata.clearance = 0.3
sm.userdata.ranges = None
sm.userdata.max_forward_velocity = 0.3
sm.userdata.default_rotational_speed = 0.5
sm.userdata.direction = 1
with sm:
pass
#=========================== YOUR CODE HERE ===========================
# Instructions: construct the state machine by adding the states that
# you have implemented.
# Below is an example how to add a state:
#
# smach.StateMachine.add('SEARCH',
# PreemptableState(search,
# input_keys=['front_min', 'clearance'],
# output_keys=['velocity'],
# outcomes=['found_obstacle']),
# transitions={'found_obstacle': 'ANOTHER_STATE'})
#
# First argument is the state label, an arbitrary string
# (by convention should be uppercase). Second argument is
# an object that implements the state. In our case an
# instance of the helper class PreemptableState is
# created, and the state function in passed. Moreover,
# we have to specify which keys in the userdata the
# function will need to access for reading (input_keys)
# and for writing (output_keys), and the list of possible
# outcomes of the state. Finally, the transitions are
# specified. Normally you would have one transition per
# state outcome.
#
# Note: The first state that you add will become the initial state of
# the state machine.
#======================================================================
smach.StateMachine.add('FINDWALL',
PreemptableState(find_wall,
input_keys=[
'mode', 'front_min',
'clearance', 'right_min',
'ranges',
'max_forward_velocity'
],
output_keys=['velocity'],
outcomes=['wall_found']),
transitions={'wall_found': 'TURN'})
smach.StateMachine.add(
'TURN',
PreemptableState(turn,
input_keys=[
'mode', 'front_min', 'clearance', 'ranges',
'right_min', 'max_forward_velocity'
],
output_keys=['velocity'],
outcomes=['alligned', 'find_wall']),
transitions={
'alligned': 'FOLLOW',
'find_wall': 'FINDWALL'
})
smach.StateMachine.add(
'ALIGN',
PreemptableState(align,
input_keys=[
'mode', 'front_min', 'clearance', 'ranges',
'right_min', 'max_forward_velocity'
],
output_keys=['velocity'],
outcomes=['alligned', 'not_aligned',
'find_wall']),
transitions={
'alligned': 'FOLLOW',
'not_aligned': 'TURN',
'find_wall': 'FINDWALL'
})
smach.StateMachine.add('FOLLOW',
PreemptableState(follow_wall,
input_keys=[
'mode', 'front_min',
'clearance', 'ranges',
'right_min', 'left_min',
'max_forward_velocity'
],
output_keys=['velocity'],
outcomes=[
'corner_found',
'door_found',
'not_aligned', 'find_wall'
]),
transitions={
'corner_found': 'CORNERS',
'door_found': 'DOOR',
'not_aligned': 'ALIGN',
'find_wall': 'FINDWALL'
})
smach.StateMachine.add(
'CORNERS',
PreemptableState(corner_handle,
input_keys=[
'mode', 'back_min', 'clearance', 'ranges',
'right_min', 'max_forward_velocity'
],
output_keys=['velocity'],
outcomes=['alligned', 'find_wall']),
transitions={
'alligned': 'ALIGN',
'find_wall': 'FINDWALL'
})
smach.StateMachine.add(
'DOOR',
PreemptableState(door_handle,
input_keys=[
'mode', 'front_min', 'back_min', 'clearance',
'ranges', 'right_min', 'max_forward_velocity'
],
output_keys=['velocity'],
outcomes=['alligned', 'find_wall']),
transitions={
'alligned': 'ALIGN',
'find_wall': 'FINDWALL'
})
return sm
def main():
rospy.init_node('perceive_cavity_server')
sleep_time = rospy.get_param('~sleep_time', 1.0)
# Construct state machine
sm = smach.StateMachine(
outcomes=['OVERALL_SUCCESS', 'OVERALL_FAILED'],
input_keys=['perceive_cavity_goal'],
output_keys=['perceive_cavity_feedback', 'perceive_cavity_result'])
sm.userdata.next_arm_pose_index = 0
# Open the container
with sm:
# approach to platform
smach.StateMachine.add(
'GET_GOAL',
getGoal(),
transitions={'succeeded': 'PUBLISH_REFERENCE_FRAME_FOR_WBC'})
# generates a pose based on the previous string object topic received
smach.StateMachine.add(
'PUBLISH_REFERENCE_FRAME_FOR_WBC',
gbs.send_event([('/static_transform_publisher_node/event_in',
'e_start')]),
transitions={'success': 'START_POSE_SELECTOR'})
# start the cavity pose selector to accumulate the poses
smach.StateMachine.add(
'START_POSE_SELECTOR',
gbs.send_event([('/mcr_perception/cavity_pose_selector/event_in',
'e_start')]),
transitions={'success': 'SELECT_NEXT_LOOK_POSE'})
# select a look_at_workspace pose
smach.StateMachine.add(
'SELECT_NEXT_LOOK_POSE',
gms.select_arm_pose([
'look_straight_at_workspace_left',
'look_straight_at_workspace_right'
]),
transitions={
'succeeded': 'LOOK_AROUND', # next pose selected
'completed':
'SET_ACTION_LIB_SUCCESS', # we've run out of poses to select, which means we've gone through the list
'failed': 'SET_ACTION_LIB_SUCCESS'
}
) # we've run out of poses to select, which means we've gone through the list
# move arm to selected pose
smach.StateMachine.add('LOOK_AROUND',
gms.move_arm(),
transitions={
'succeeded': 'WAIT_AFTER_ARM_MOTION',
'failed': 'LOOK_AROUND'
})
smach.StateMachine.add('WAIT_AFTER_ARM_MOTION',
wait_for(sleep_time=sleep_time),
transitions={'success': 'RECOGNIZE_CAVITIES'})
# trigger perception pipeline
smach.StateMachine.add('RECOGNIZE_CAVITIES',
gps.find_cavities(retries=1),
transitions={
'cavities_found': 'SELECT_NEXT_LOOK_POSE',
'no_cavities_found': 'SELECT_NEXT_LOOK_POSE'
})
# set action lib result
smach.StateMachine.add('SET_ACTION_LIB_SUCCESS',
SetActionLibResult(True),
transitions={'succeeded': 'OVERALL_SUCCESS'})
# set action lib result
smach.StateMachine.add('SET_ACTION_LIB_FAILURE',
SetActionLibResult(False),
transitions={'succeeded': 'OVERALL_FAILED'})
# smach viewer
sis = smach_ros.IntrospectionServer('perceive_cavity_smach_viewer', sm,
'/PERCEIVE_CAVITY_SMACH_VIEWER')
sis.start()
# Construct action server wrapper
asw = ActionServerWrapper(server_name='perceive_cavity_server',
action_spec=PerceiveLocationAction,
wrapped_container=sm,
succeeded_outcomes=['OVERALL_SUCCESS'],
aborted_outcomes=['OVERALL_FAILED'],
preempted_outcomes=['PREEMPTED'],
goal_key='perceive_cavity_goal',
feedback_key='perceive_cavity_feedback',
result_key='perceive_cavity_result')
# Run the server in a background thread
asw.run_server()
rospy.spin()
def GetAutoChargingStateMachine():
sm = smach.StateMachine(outcomes=[
"no_need_charge", "finished_charging", "already_charging", "interrupt",
"failed"
])
with sm:
smach.StateMachine.add("check_trigger_cmd",
wait_trigger_states.TriggerCmdState(
["prep_serve"]),
transitions={
"prep_serve": "interrupt",
"none": "test_already_charging"
})
sm.set_initial_state(["check_trigger_cmd"])
smach.StateMachine.add("test_already_charging",
TestBatteryIsChargingState(),
transitions={
"charging": "already_charging",
"not_charging": "decide_need_charging",
"unknown": "failed",
})
smach.StateMachine.add("decide_need_charging",
TestBatteryLevelState(),
transitions={
"full": "no_need_charge",
"normal": "no_need_charge",
"low": "go_dock_retry_reset",
"unknown": "failed",
})
smach.StateMachine.add("go_dock_retry_reset",
general_states.ResetRetryCounter(
"go_dock_retry", 3),
transitions={"next": "go_dock_retry_test"},
remapping={"go_dock_retry": "go_dock_retry"})
smach.StateMachine.add(
"go_dock_retry_test",
general_states.DecreaseAndTestRetry("go_dock_retry"),
transitions={
"continue": "go_dock",
"give_up": "failed"
},
remapping={"go_dock_retry": "go_dock_retry"})
smach.StateMachine.add("go_dock",
GoDockingState(),
transitions={
"done": "test_is_charging",
"failed": "go_dock_retry_test"
})
smach.StateMachine.add("test_is_charging",
TestBatteryIsChargingState(),
transitions={
"charging": "test_finished_charging",
"not_charging": "go_dock_retry_test",
"unknown": "failed"
},
remapping={"tour_point_id": "tour_point_id"})
smach.StateMachine.add("test_finished_charging",
TestBatteryLevelState(),
transitions={
"full": "finished_charging",
"normal": "wait_finish_charging",
"low": "wait_finish_charging",
"unknown": "failed"
},
remapping={"tour_point_id": "tour_point_id"})
smach.StateMachine.add("wait_finish_charging",
general_states.WaitTimeState(5),
transitions={"next": "test_finished_charging"})
return sm
def main():
rospy.init_node('sm')
msg_publisher = MsgPublisher()
bag_recorder = ROSBagAPIThreadRecorder()
sm = smach.StateMachine(outcomes=['succeeded', 'aborted'])
sm.userdata.rate = 100.0
sm.userdata.rate = 100.0
sm.userdata.rate = 100.0
sm.userdata.rate = 100.0
sm.userdata.rate = 100.0
sm.userdata.file = ''
sm.userdata.topics = ''
sm.userdata.rate = 100.0
sm.userdata.topic = ''
sm.userdata.point = Point()
sm.userdata.point_topic = 'smacha/rosbag_api_recording_3_point'
sm.userdata.rate_1 = 10.0
sm.userdata.pose = Pose()
sm.userdata.pose_topic = 'smacha/rosbag_api_recording_3_pose'
sm.userdata.rate_2 = 20.0
sm.userdata.pointcloud = PointCloud()
sm.userdata.pointcloud_topic = 'smacha/rosbag_api_recording_3_pointcloud'
sm.userdata.rate_3 = 30.0
sm.userdata.pointcloud2 = PointCloud2()
sm.userdata.pointcloud2_topic = 'smacha/rosbag_api_recording_3_pointcloud2'
sm.userdata.rate_4 = 40.0
sm.userdata.posearray = PoseArray()
sm.userdata.posearray_topic = 'smacha/rosbag_api_recording_3_posearray'
sm.userdata.rate_5 = 50.0
sm.userdata.file_1 = '/tmp/rosbag_api_recording_3_bag_1.bag'
sm.userdata.topics_1 = [
'smacha/rosbag_api_recording_3_point',
'smacha/rosbag_api_recording_3_pose'
]
sm.userdata.file_2 = '/tmp/rosbag_api_recording_3_bag_2.bag'
sm.userdata.topics_2 = [
'smacha/rosbag_api_recording_3_pose',
'smacha/rosbag_api_recording_3_pointcloud'
]
sm.userdata.file_3 = '/tmp/rosbag_api_recording_3_bag_3.bag'
sm.userdata.topics_3 = [
'smacha/rosbag_api_recording_3_pointcloud2',
'smacha/rosbag_api_recording_3_posearray'
]
with sm:
smach.StateMachine.add('PUBLISH_MSG_1',
PublishMsgState('PUBLISH_MSG_1', msg_publisher,
'start'),
transitions={
'aborted': 'aborted',
'succeeded': 'PUBLISH_MSG_2'
},
remapping={
'msg': 'point',
'rate': 'rate_1',
'topic': 'point_topic'
})
smach.StateMachine.add('PUBLISH_MSG_2',
PublishMsgState('PUBLISH_MSG_2', msg_publisher,
'start'),
transitions={
'aborted': 'aborted',
'succeeded': 'PUBLISH_MSG_3'
},
remapping={
'msg': 'pose',
'rate': 'rate_2',
'topic': 'pose_topic'
})
smach.StateMachine.add('PUBLISH_MSG_3',
PublishMsgState('PUBLISH_MSG_3', msg_publisher,
'start'),
transitions={
'aborted': 'aborted',
'succeeded': 'PUBLISH_MSG_4'
},
remapping={
'msg': 'pointcloud',
'rate': 'rate_3',
'topic': 'pointcloud_topic'
})
smach.StateMachine.add('PUBLISH_MSG_4',
PublishMsgState('PUBLISH_MSG_4', msg_publisher,
'start'),
transitions={
'aborted': 'aborted',
'succeeded': 'PUBLISH_MSG_5'
},
remapping={
'msg': 'pointcloud2',
'rate': 'rate_4',
'topic': 'pointcloud2_topic'
})
smach.StateMachine.add('PUBLISH_MSG_5',
PublishMsgState('PUBLISH_MSG_5', msg_publisher,
'start'),
transitions={
'aborted': 'aborted',
'succeeded': 'START_RECORDING_1'
},
remapping={
'msg': 'posearray',
'rate': 'rate_5',
'topic': 'posearray_topic'
})
smach.StateMachine.add('START_RECORDING_1',
RecordROSBagState('START_RECORDING_1',
bag_recorder, 'start'),
transitions={
'aborted': 'aborted',
'succeeded': 'START_RECORDING_2'
},
remapping={
'file': 'file_1',
'topics': 'topics_1'
})
smach.StateMachine.add('START_RECORDING_2',
RecordROSBagState('START_RECORDING_2',
bag_recorder, 'start'),
transitions={
'aborted': 'aborted',
'succeeded': 'START_RECORDING_3'
},
remapping={
'file': 'file_2',
'topics': 'topics_2'
})
smach.StateMachine.add('START_RECORDING_3',
RecordROSBagState('START_RECORDING_3',
bag_recorder, 'start'),
transitions={
'aborted': 'aborted',
'succeeded': 'WAIT'
},
remapping={
'file': 'file_3',
'topics': 'topics_3'
})
smach.StateMachine.add('WAIT',
SleepState(5),
transitions={'succeeded': 'STOP_RECORDING'})
smach.StateMachine.add('STOP_RECORDING',
RecordROSBagState('STOP_RECORDING',
bag_recorder, 'stop_all'),
transitions={
'aborted': 'aborted',
'succeeded': 'UNPUBLISH_MSG'
})
smach.StateMachine.add('UNPUBLISH_MSG',
PublishMsgState('UNPUBLISH_MSG', msg_publisher,
'stop_all'),
transitions={
'aborted': 'aborted',
'succeeded': 'succeeded'
})
outcome = sm.execute()
def main():
rospy.init_node('A_test_state_machine')
# Create the top level SMACH state machine
sm_top = smach.StateMachine(outcomes=['done', 'exit'])
# Open the container
with sm_top:
goal1 = MoveBaseGoal()
goal1.target_pose.header.frame_id = "dtw_robot1/map"
goal1.target_pose.pose.position.x = 2.5
goal1.target_pose.pose.position.y = 2.8
goal1.target_pose.pose.orientation.w = 1.0
smach.StateMachine.add('MOVE1',
smach_ros.SimpleActionState(
'/dtw_robot1/move_base',
MoveBaseAction,
goal=goal1),
transitions={
'succeeded': 'YOLO_START',
'preempted': 'MOVE1',
'aborted': 'MOVE1'
})
sm_sub = smach.StateMachine(outcomes=['done'])
with sm_sub:
smach.StateMachine.add('YOLO',
Callback(),
transitions={'done': 'done'})
smach.StateMachine.add('YOLO_START',
sm_sub,
transitions={'done': 'MOVE2'})
goal2 = MoveBaseGoal()
goal2.target_pose.header.frame_id = "dtw_robot1/map"
goal2.target_pose.pose.position.x = 4.0
goal2.target_pose.pose.position.y = 3.8
goal2.target_pose.pose.orientation.w = 1.0
smach.StateMachine.add('MOVE2',
smach_ros.SimpleActionState(
'/dtw_robot1/move_base',
MoveBaseAction,
goal=goal2),
transitions={
'succeeded': 'MOVE3',
'preempted': 'exit',
'aborted': 'exit'
})
goal = MoveBaseGoal()
goal.target_pose.header.frame_id = "dtw_robot1/map"
goal.target_pose.pose.position.x = 6.8
goal.target_pose.pose.position.y = 1.9
goal.target_pose.pose.orientation.w = 1.0
smach.StateMachine.add('MOVE3',
smach_ros.SimpleActionState(
'/dtw_robot1/move_base',
MoveBaseAction,
goal=goal),
transitions={
'succeeded': 'DETECT_AREA',
'preempted': 'exit',
'aborted': 'MOVE2'
})
sm_sub = smach.StateMachine(outcomes=['done', 'exit'])
with sm_sub:
smach.StateMachine.add('Detect_Start',
State1(),
transitions={
'succeeded': 'done',
'failed': 'exit',
'aborted': 'done'
})
#smach.StateMachine.add('Retry', State2(), transitions={'done':'Detect_Start'})
smach.StateMachine.add('DETECT_AREA',
sm_sub,
transitions={
'done': 'MOVE4',
'exit': 'DETECT_AREA'
})
goal3 = MoveBaseGoal()
goal3.target_pose.header.frame_id = "dtw_robot1/map"
goal3.target_pose.pose.position.x = 7.0
goal3.target_pose.pose.position.y = 0.0
goal3.target_pose.pose.orientation.w = 1.0
smach.StateMachine.add('MOVE4',
smach_ros.SimpleActionState(
'/dtw_robot1/move_base',
MoveBaseAction,
goal=goal3),
transitions={
'succeeded': 'exit',
'preempted': 'MOVE1',
'aborted': 'MOVE2'
})
# Execute SMACH plan
sis = smach_ros.IntrospectionServer('smach_server', sm_top, '/SM_ROOT')
sis.start()
outcome = sm_top.execute()
rospy.spin()
sis.stop()
def __init__(self,
robot,
door_start_wp_designator,
door_dest_wp_designator,
approach_speed=0.1,
push_speed=0.05,
attempts=10):
"""
Push against a door until its open
:param robot: Robot on which to execute this state machine
:param door_entity_designator: The door entity. Defaults to None, which implies the door its in front of
This entity must have 2 fields in its data: push_start_waypoint and push_destination_waypoint,
which must both contain an ID of a waypoint.
push_start_waypoint is from where the robot will start pushing
push_destination_waypoint simply needs to be reachable for the door the be considered 'open'
:param approach_speed: Speed with which to approach the door
:param push_speed: Speed with which to push against the door
:return:
"""
smach.StateMachine.__init__(self, outcomes=['succeeded', 'failed'])
#door_start_wp_designator = WaypointOfDoorDesignator(robot, door_entity_designator, direction='start', name='door_open_start')
#door_dest_wp_designator = WaypointOfDoorDesignator(robot, door_entity_designator, direction='destination', name='door_open_dest')
with self:
smach.StateMachine.add('GOTO_DOOR_START',
states.NavigateToWaypoint(
robot,
door_start_wp_designator,
radius=0.05),
transitions={
'arrived': 'PUSH_DOOR_ITERATOR',
'unreachable': 'failed',
'goal_not_defined': 'failed'
})
# START REPEAT DOOR OPENING
push_door_iterator = smach.Iterator(
outcomes=['open', 'closed', 'failed'],
it=lambda: range(0, attempts),
it_label='counter',
input_keys=[],
output_keys=[],
exhausted_outcome='failed')
with push_door_iterator:
push_door = smach.StateMachine(
outcomes=['open', 'closed', 'failed'])
with push_door:
smach.StateMachine.add('APPROACH_AGAINST_N',
ForceDriveToTouchDoor(
robot, approach_speed),
transitions={
'front': 'closed',
'left': 'CHECK_DOOR_PASSABLE',
'right': 'CHECK_DOOR_PASSABLE',
'failed': 'failed'
})
smach.StateMachine.add(
'CHECK_DOOR_PASSABLE',
CheckDoorPassable(
robot,
destination_designator=door_dest_wp_designator),
transitions={
'blocked': 'closed',
'passable': 'open'
})
smach.Iterator.set_contained_state(
'APPROACH_AGAINST',
push_door,
loop_outcomes=['failed', 'closed'],
break_outcomes=['open'])
smach.StateMachine.add('PUSH_DOOR_ITERATOR', push_door_iterator, {
'open': 'succeeded',
'closed': 'failed',
'failed': 'failed'
})
def main():
rospy.init_node('pick_and_place_state_machine')
namespace = rospy.get_param("~robot_name")
planning_group = rospy.get_param("~planning_group")
# Create the sub SMACH state machine
task_center = smach.StateMachine(
outcomes=['succeeded', 'aborted', 'preempted'])
# Open the container
with task_center:
smach.StateMachine.add('GET_CLOSER_TO_OBJECT',
getCloserToGoal(),
transitions={
'succeeded': 'PICK',
'failed': 'GET_CLOSER_TO_OBJECT'
})
# smach.StateMachine.add('Pick', Go(),transitions={'N':'PICK','Y':'PICK'})
# Create the sub SMACH state machine
# pick_center = smach.StateMachine(outcomes=['succeeded', 'aborted','preempted'])
pick_center = smach.StateMachine(
outcomes=['succeeded', 'aborted', 'preempted'])
with pick_center:
pick_center.userdata.planning_group = planning_group
def joint_position_request_cb(userdata, request):
joint = JointPosition()
joint.position = userdata.input_position
joint.max_velocity_scaling_factor = 1.0
joint.max_accelerations_scaling_factor = 1.0
request.planning_group = userdata.input_planning_group
request.joint_position = joint
return request
def joint_position_response_cb(userdata, response):
if response.is_planned == False:
return 'aborted'
else:
rospy.sleep(3.)
return 'succeeded'
def eef_pose_request_cb(userdata, request):
eef = KinematicsPose()
eef.pose = userdata.input_pose
rospy.loginfo('eef.position.x : %f', eef.pose.position.x)
rospy.loginfo('eef.position.y : %f', eef.pose.position.y)
rospy.loginfo('eef.position.z : %f', eef.pose.position.z)
eef.max_velocity_scaling_factor = 1.0
eef.max_accelerations_scaling_factor = 1.0
eef.tolerance = userdata.input_tolerance
request.planning_group = userdata.input_planning_group
request.kinematics_pose = eef
return request
def align_arm_with_object_response_cb(userdata, response):
if response.is_planned == False:
pick_center.userdata.align_arm_with_object_tolerance += 0.005
rospy.logwarn(
'Set more tolerance[%f]',
pick_center.userdata.align_arm_with_object_tolerance)
return 'aborted'
else:
OFFSET_FOR_STRETCH = 0.030
pick_center.userdata.object_pose.position.x += OFFSET_FOR_STRETCH
rospy.sleep(3.)
return 'succeeded'
def close_to_object_response_cb(userdata, response):
if response.is_planned == False:
pick_center.userdata.close_to_object_tolerance += 0.005
rospy.logwarn(
'Set more tolerance[%f]',
pick_center.userdata.close_to_object_tolerance)
return 'aborted'
else:
OFFSET_FOR_OBJECT_HEIGHT = 0.020
pick_center.userdata.object_pose.position.z += OFFSET_FOR_OBJECT_HEIGHT
rospy.sleep(3.)
return 'succeeded'
def pick_up_object_response_cb(userdata, response):
if response.is_planned == False:
pick_center.userdata.pick_up_object_tolerance += 0.005
rospy.logwarn(
'Set more tolerance[%f]',
pick_center.userdata.pick_up_object_tolerance)
return 'aborted'
else:
rospy.sleep(3.)
return 'succeeded'
def gripper_request_cb(userdata, request):
joint = JointPosition()
joint.position = userdata.input_gripper
joint.max_velocity_scaling_factor = 1.0
joint.max_accelerations_scaling_factor = 1.0
request.planning_group = userdata.input_planning_group
request.joint_position = joint
return request
def gripper_response_cb(userdata, response):
rospy.sleep(1.)
return 'succeeded'
# 如果align环节abort，为避免设置同样的初始位置，就进入这个位置。
# 然后这个位置如果abort 就进入 init 的出事位置
pick_center.userdata.abort_position = [0.0, -0.55, 1.10, -0.44]
smach.StateMachine.add(
'SET_Abort_POSITION',
ServiceState(
planning_group + '/moveit/set_joint_position',
SetJointPosition,
request_cb=joint_position_request_cb,
response_cb=joint_position_response_cb,
input_keys=['input_planning_group', 'input_position']),
transitions={'succeeded': 'ABort_OPEN_GRIPPER'},
remapping={
'input_planning_group': 'planning_group',
'input_position': 'abort_position'
})
pick_center.userdata.open_gripper = [0.005]
smach.StateMachine.add(
'ABort_OPEN_GRIPPER',
ServiceState(
namespace + '/gripper',
SetJointPosition,
request_cb=gripper_request_cb,
response_cb=gripper_response_cb,
input_keys=['input_planning_group', 'input_gripper']),
transitions={'succeeded': 'ABort_GET_POSE_OF_THE_OBJECT'},
remapping={
'input_planning_group': 'planning_group',
'input_gripper': 'open_gripper'
})
pick_center.userdata.object_pose = Pose()
smach.StateMachine.add(
'ABort_GET_POSE_OF_THE_OBJECT',
getPoseOfTheObject(),
transitions={
'succeeded': 'ALIGN_ARM_WITH_OBJECT',
'aborted': 'SET_INIT_POSITION'
},
remapping={'output_object_pose': 'object_pose'})
# 这里设置正常的init position， 对齐不成功，再进入 abort的init position
pick_center.userdata.init_position = [0.0, -0.65, 1.20, -0.54]
smach.StateMachine.add(
'SET_INIT_POSITION',
ServiceState(
planning_group + '/moveit/set_joint_position',
SetJointPosition,
request_cb=joint_position_request_cb,
response_cb=joint_position_response_cb,
input_keys=['input_planning_group', 'input_position']),
transitions={'succeeded': 'OPEN_GRIPPER'},
remapping={
'input_planning_group': 'planning_group',
'input_position': 'init_position'
})
pick_center.userdata.open_gripper = [0.005]
smach.StateMachine.add(
'OPEN_GRIPPER',
ServiceState(
namespace + '/gripper',
SetJointPosition,
request_cb=gripper_request_cb,
response_cb=gripper_response_cb,
input_keys=['input_planning_group', 'input_gripper']),
transitions={'succeeded': 'GET_POSE_OF_THE_OBJECT'},
remapping={
'input_planning_group': 'planning_group',
'input_gripper': 'open_gripper'
})
pick_center.userdata.object_pose = Pose()
smach.StateMachine.add(
'GET_POSE_OF_THE_OBJECT',
getPoseOfTheObject(),
transitions={
'succeeded': 'ALIGN_ARM_WITH_OBJECT',
'aborted': 'SET_Abort_POSITION'
},
remapping={'output_object_pose': 'object_pose'})
# 对齐后的流程照旧
pick_center.userdata.align_arm_with_object_tolerance = 0.01
smach.StateMachine.add(
'ALIGN_ARM_WITH_OBJECT',
ServiceState(planning_group + '/moveit/set_kinematics_pose',
SetKinematicsPose,
request_cb=eef_pose_request_cb,
response_cb=align_arm_with_object_response_cb,
input_keys=[
'input_planning_group', 'input_pose',
'input_tolerance'
]),
transitions={
'succeeded': 'CLOSE_TO_OBJECT',
'aborted': 'ALIGN_ARM_WITH_OBJECT'
},
remapping={
'input_planning_group': 'planning_group',
'input_pose': 'object_pose',
'input_tolerance': 'align_arm_with_object_tolerance'
})
pick_center.userdata.close_to_object_tolerance = 0.01
smach.StateMachine.add(
'CLOSE_TO_OBJECT',
ServiceState(planning_group + '/moveit/set_kinematics_pose',
SetKinematicsPose,
request_cb=eef_pose_request_cb,
response_cb=close_to_object_response_cb,
input_keys=[
'input_planning_group', 'input_pose',
'input_tolerance'
]),
transitions={
'succeeded': 'GRIP_OBJECT',
'aborted': 'CLOSE_TO_OBJECT'
},
remapping={
'input_planning_group': 'planning_group',
'input_pose': 'object_pose',
'input_tolerance': 'close_to_object_tolerance'
})
pick_center.userdata.close_gripper = [-0.005]
smach.StateMachine.add(
'GRIP_OBJECT',
ServiceState(
namespace + '/gripper',
SetJointPosition,
request_cb=gripper_request_cb,
response_cb=gripper_response_cb,
input_keys=['input_planning_group', 'input_gripper']),
transitions={'succeeded': 'PICK_UP_OBJECT'},
remapping={
'input_planning_group': 'planning_group',
'input_gripper': 'close_gripper'
})
pick_center.userdata.pick_up_object_tolerance = 0.01
smach.StateMachine.add(
'PICK_UP_OBJECT',
ServiceState(planning_group + '/moveit/set_kinematics_pose',
SetKinematicsPose,
request_cb=eef_pose_request_cb,
response_cb=pick_up_object_response_cb,
input_keys=[
'input_planning_group', 'input_pose',
'input_tolerance'
]),
transitions={
'succeeded': 'SET_HOLDING_POSITION',
'aborted': 'PICK_UP_OBJECT'
},
remapping={
'input_planning_group': 'planning_group',
'input_pose': 'object_pose',
'input_tolerance': 'pick_up_object_tolerance'
})
pick_center.userdata.holding_position = [0.0, 0, -0.8, 0.20]
smach.StateMachine.add(
'SET_HOLDING_POSITION',
ServiceState(
planning_group + '/moveit/set_joint_position',
SetJointPosition,
request_cb=joint_position_request_cb,
response_cb=joint_position_response_cb,
input_keys=['input_planning_group', 'input_position']),
transitions={'succeeded': 'SET_PLACE_POSITION'},
remapping={
'input_planning_group': 'planning_group',
'input_position': 'holding_position'
})
pick_center.userdata.place_position = [1.57, 0.0, 0.00, 0.4]
smach.StateMachine.add(
'SET_PLACE_POSITION',
ServiceState(
planning_group + '/moveit/set_joint_position',
SetJointPosition,
request_cb=joint_position_request_cb,
response_cb=joint_position_response_cb,
input_keys=['input_planning_group', 'input_position']),
transitions={'succeeded': 'OPEN_GRIPPER_PLACE'},
remapping={
'input_planning_group': 'planning_group',
'input_position': 'place_position'
})
smach.StateMachine.add(
'OPEN_GRIPPER_PLACE',
ServiceState(
namespace + '/gripper',
SetJointPosition,
request_cb=gripper_request_cb,
response_cb=gripper_response_cb,
input_keys=['input_planning_group', 'input_gripper']),
transitions={'succeeded': 'SET_RELEASE_POSITION'},
remapping={
'input_planning_group': 'planning_group',
'input_gripper': 'open_gripper'
})
pick_center.userdata.release_position = [1.57, 0.0, -0.80, 0.4]
smach.StateMachine.add(
'SET_RELEASE_POSITION',
ServiceState(
planning_group + '/moveit/set_joint_position',
SetJointPosition,
request_cb=joint_position_request_cb,
response_cb=joint_position_response_cb,
input_keys=['input_planning_group', 'input_position']),
transitions={'succeeded': 'SET_ENDINIT_POSITION'},
remapping={
'input_planning_group': 'planning_group',
'input_position': 'release_position'
})
pick_center.userdata.end_init_position = [0.0, -0.8, 0.5, -0.54]
smach.StateMachine.add(
'SET_ENDINIT_POSITION',
ServiceState(
planning_group + '/moveit/set_joint_position',
SetJointPosition,
request_cb=joint_position_request_cb,
response_cb=joint_position_response_cb,
input_keys=['input_planning_group', 'input_position']),
transitions={'succeeded': 'succeeded'},
remapping={
'input_planning_group': 'planning_group',
'input_position': 'end_init_position'
})
smach.StateMachine.add('PICK',
pick_center,
transitions={
'succeeded': 'GET_CLOSER_TO_OBJECT',
'aborted': 'GET_CLOSER_TO_OBJECT'
})
sis = smach_ros.IntrospectionServer('server_name', task_center,
'/TASKS_CENTER')
sis.start()
# Execute SMACH plan
outcome = task_center.execute()
# Wait for ctrl-c to stop the application
rospy.spin()
sis.stop()
wait_time_sound = rospy.Time.now() + rospy.Duration(0.5)
while rospy.Time.now()<wait_time_sound:
sound_pub.publish(Sound(Sound.BUTTON))
wait_time = rospy.Time.now() + rospy.Duration(1.4)
while rospy.Time.now()<wait_time:
self.twist.linear.x = 0
self.twist.angular.z = -1.5
cmd_vel_pub.publish(self.twist)
stop = False
stop_count = -200
return 'go'
# follower = Follower()
sm = smach.StateMachine(outcomes=['finish'])
with sm:
# Add states to the container
smach.StateMachine.add('GO', Go(),
transitions={'stop':'STOP'})
smach.StateMachine.add('STOP', Stop(),
transitions={'go':'GO', 'task1': 'TASK1', 'task2': 'TASK2', 'task3': 'TASK3'})
smach.StateMachine.add('TASK1', Task1(),
transitions={'go':'GO'})
smach.StateMachine.add('TASK2', Task2(),
transitions={'go':'GO'})
smach.StateMachine.add('TASK3', Task3(),
transitions={'go':'GO'})
# Create and start the introspection server
sis = smach_ros.IntrospectionServer('server_name', sm, '/SM_ROOT')
def ObjectDetection():
''' Object detection sub state machine; iterates over object_detection action state and recovery
mechanism until an object is detected, it's preempted or there's an error (aborted outcome) '''
class ObjDetectedCondition(smach.State):
''' Check for the object detection result to retry if no objects where detected '''
def __init__(self):
''' '''
smach.State.__init__(self, outcomes=['preempted', 'satisfied', 'fold_arm', 'retry'],
input_keys=['od_attempt', 'object_names'],
output_keys=['od_attempt'])
def execute(self, userdata):
if self.preempt_requested():
self.service_preempt()
return 'preempted'
if len(userdata.object_names) > 0:
userdata.od_attempt = 0
return 'satisfied'
userdata.od_attempt += 1
if userdata.od_attempt == 1:
return 'fold_arm'
return 'retry'
sm = smach.StateMachine(outcomes = ['succeeded','preempted','aborted'],
input_keys = ['od_attempt', 'output_frame'],
output_keys = ['objects', 'object_names', 'support_surf'])
with sm:
smach.StateMachine.add('ClearOctomap',
smach_ros.ServiceState('clear_octomap',
std_srvs.Empty),
transitions={'succeeded':'ObjectDetection',
'preempted':'preempted',
'aborted':'ObjectDetection'})
smach.StateMachine.add('ObjectDetection',
smach_ros.SimpleActionState('object_detection',
thorp_msgs.DetectObjectsAction,
goal_slots=['output_frame'],
result_slots=['objects', 'object_names', 'support_surf']),
remapping={'output_frame':'output_frame',
'object_names':'object_names',
'support_surf':'support_surf'},
transitions={'succeeded':'ObjDetectedCondition',
'preempted':'preempted',
'aborted':'aborted'})
smach.StateMachine.add('ObjDetectedCondition',
ObjDetectedCondition(),
remapping={'object_names':'object_names'},
transitions={'satisfied':'succeeded',
'preempted':'preempted',
'fold_arm':'FoldArm',
'retry':'ClearOctomap'})
smach.StateMachine.add('FoldArm',
FoldArm(),
transitions={'succeeded':'ClearOctomap',
'preempted':'preempted',
'aborted':'ClearOctomap'})
return sm
def main():
# Initialize node with desired node name - ideally task name
rospy.init_node('gateTask')
# Create a SMACH state machine
sm = smach.StateMachine(outcomes=['task_complete'])
# Create and start introspection server - fancy way of saying view gui feature
sis = smach_ros.IntrospectionServer('server_name', sm, '/SM_GATE_TASK')
sis.start()
gate_topic = {'x': '/gate_x', 'y': '/gate_y', 'area': '/gate_area'}
GATE_DEPTH = 18
GATE_YAW_1 = 1.57
GATE_YAW_OFFSET = -0.017 * 5
GATE_YAW_2 = GATE_YAW_1 + GATE_YAW_OFFSET
GATE_CONVERGE_TICKS = 5000
GATE_FORWARD_TICKS = 10000
BUOY_DEPTH = 5 * 12
BUOY_YAW = 0.017 * 45
# Open SMACH container
with sm:
smach.StateMachine.add('IDLE',
state.WaitForTopic('/gate_enable'),
transitions={
'done': 'DEPTH_PID_ENABLE',
'notdone': 'IDLE'
})
smach.StateMachine.add('DEPTH_PID_ENABLE',
state.PublishTopic('/depth_control/pid_enable',
True),
transitions={'done': 'DIVE_GATE_DEPTH'})
smach.StateMachine.add('DIVE_GATE_DEPTH',
state.ChangeDepthToTarget(GATE_DEPTH),
transitions={
'done': 'YAW_PID_ENABLE',
'notdone': 'DIVE_GATE_DEPTH',
'reset': 'RESET'
})
smach.StateMachine.add('YAW_PID_ENABLE',
state.PublishTopic('/yaw_control/pid_enable',
True),
transitions={'done': 'ROTATE_TO_GATE'})
smach.StateMachine.add('ROTATE_TO_GATE',
state.RotateYawToAbsoluteTarget(GATE_YAW_1),
transitions={
'done': 'TRACK_GATE',
'notdone': 'ROTATE_TO_GATE',
'reset': 'RESET'
})
smach.StateMachine.add('TRACK_GATE',
state.TrackObject(gate_topic),
transitions={
'done': 'SET_YAW_GATE_OFFSET',
'notdone': 'TRACK_GATE',
'reset': 'RESET'
})
smach.StateMachine.add('SET_YAW_GATE_OFFSET',
state.PublishTopicRelative(
'/yaw_control/state',
'/yaw_control/setpoint', Float64,
GATE_YAW_OFFSET),
transitions={
'done': 'ROTATE_GATE_LEFT',
'notdone': 'SET_YAW_GATE_OFFSET',
'reset': 'RESET'
})
smach.StateMachine.add('ROTATE_GATE_LEFT',
state.WaitForConvergence(
'/yaw_control/state', Float64, GATE_YAW_2,
0.017, GATE_CONVERGE_TICKS),
transitions={
'done': 'MOVE_FORWARD_GATE',
'notdone': 'ROTATE_GATE_LEFT',
'reset': 'RESET'
})
smach.StateMachine.add('MOVE_FORWARD_GATE',
state.MoveForwardTimed(GATE_FORWARD_TICKS,
True),
transitions={
'done': 'BUOY_DEPTH',
'notdone': 'MOVE_FORWARD_GATE',
'reset': 'RESET'
})
smach.StateMachine.add('BUOY_DEPTH',
state.ChangeDepthToTarget(BUOY_DEPTH),
transitions={
'done': 'ROTATE_TO_BUOY',
'notdone': 'BUOY_DEPTH',
'reset': 'RESET'
})
smach.StateMachine.add('ROTATE_TO_BUOY',
state.RotateYawToAbsoluteTarget(BUOY_YAW),
transitions={
'done': 'COMPLETED',
'notdone': 'ROTATE_TO_BUOY',
'reset': 'RESET'
})
smach.StateMachine.add('COMPLETED',
state.PublishTopic('/task_complete', True),
transitions={'done': 'IDLE'})
smach.StateMachine.add('RESET',
state.Reset(),
transitions={'done': 'IDLE'})
# Execute State Machine
outcome = sm.execute()
# Spin node - fancy way of saying run code in a loop
rospy.spin()
sis.stop()
def __init__(self):
rospy.loginfo("Starting valve task controller...")
rospy.on_shutdown(self.cleanup)
# Initialize the state machine
self.running = False
self.safed = False
self.sm = smach.StateMachine(outcomes=['DONE', 'FAILED', 'SAFE'],
input_keys=['sm_input'],
output_keys=['sm_output'])
# Populate the state machine from the modules
with self.sm:
self.safemode = SafeMode.SAFEMODE()
# Add start state
smach.StateMachine.add('StartTask',
StartTask.STARTTASK(),
transitions={'Start': 'FindValve'},
remapping={
'input': 'sm_input',
'output': 'sm_data'
})
# Add finding step
smach.StateMachine.add('FindValve',
FindValve.FINDVALVE(),
transitions={
'Success': 'PositionRobot',
'Failure': 'ErrorHandler',
'Fatal': 'SafeMode'
},
remapping={
'input': 'sm_data',
'output': 'sm_data'
})
# Add positioning step
smach.StateMachine.add('PositionRobot',
PositionRobot.POSITIONROBOT(),
transitions={
'Success': 'PlanTurning',
'Failure': 'ErrorHandler',
'Fatal': 'SafeMode'
},
remapping={
'input': 'sm_data',
'output': 'sm_data'
})
# Add planning step
smach.StateMachine.add('PlanTurning',
PlanTurning.PLANTURNING(),
transitions={
'Success': 'ExecuteTurning',
'Failure': 'ErrorHandler',
'Fatal': 'SafeMode'
},
remapping={
'input': 'sm_data',
'output': 'sm_data'
})
# Add execution step
smach.StateMachine.add('ExecuteTurning',
ExecuteTurning.EXECUTETURNING(),
transitions={
'Success': 'FinishTask',
'Failure': 'ErrorHandler',
'Fatal': 'SafeMode'
},
remapping={
'input': 'sm_data',
'output': 'sm_data'
})
# Add finishing step
smach.StateMachine.add('FinishTask',
FinishTask.FINISHTASK(),
transitions={
'Success': 'DONE',
'Failure': 'ErrorHandler',
'Fatal': 'SafeMode'
},
remapping={
'input': 'sm_data',
'output': 'sm_output'
})
# Add manual mode
smach.StateMachine.add('ManualMode',
ManualMode.MANUALMODE(),
transitions={
'Success': 'ErrorHandler',
'Failure': 'ErrorHandler',
'Fatal': 'SafeMode'
},
remapping={
'input': 'sm_data',
'output': 'sm_data'
})
# Add safe mode
smach.StateMachine.add('SafeMode',
self.safemode,
transitions={'Safed': 'SAFE'},
remapping={
'input': 'sm_data',
'output': 'sm_output'
})
# Add Error handler
smach.StateMachine.add('ErrorHandler',
ErrorHandler.ERRORHANDLER(),
transitions={
'ReFind': 'FindValve',
'RePosition': 'PositionRobot',
'RePlan': 'PlanTurning',
'ReExecute': 'ExecuteTurning',
'ReFinish': 'FinishTask',
'GoManual': 'ManualMode',
'Failed': 'FAILED',
'Fatal': 'SafeMode'
},
remapping={
'input': 'sm_data',
'output': 'sm_output'
})
# Set up the introspection server
self.sis = smach_ros.IntrospectionServer('valve_task_smach_server',
self.sm, '/SM_ROOT')
self.sis.start()
def main():
print("first")
rospy.init_node('smach_state_machine')
global arm_publisher
arm_publisher = rospy.Publisher('vizzyArmRoutines/command',
Int16,
queue_size=100)
global index_pub
index_pub = rospy.Publisher('gaze_index', Int16, queue_size=100)
global list_of_points
list_of_points = []
list_of_points.append(point(13, 18, 0.685, 0.727))
list_of_points.append(point(14.63, -4.6, -0.99, 0.018))
#list_of_points.append(point(14.63,-4.6,-0.99,0.018))
intent_sub = rospy.Subscriber('continuos_intent_detector_win',
intent_msg_all, callback)
# Create a SMACH state machine
sm = smach.StateMachine(outcomes=[
'No_one_detected', 'person_detected', 'following',
'lost_person_in_follow', 'reach_person', 'fail_speak', 'speaked',
'fail_doing_gesture', 'succeed_doing_gesture', 'detecting_gesture',
'gesture_detected', 'fail_to_detect_gesture', 'fail_Reach_the_point',
'point_reached'
])
#variable about the person being detected
sm.userdata.id_person_detected = -1
# Open the container
with sm:
# Add states to the container
smach.StateMachine.add('STOP',
Stop(),
transitions={
'continue_stop': 'STOP',
'start_roaming': 'ROAMING'
},
remapping={
'Stop_id_in': 'id_person_detected',
'Stop_id_out': 'id_person_detected'
})
smach.StateMachine.add('ROAMING',
Roaming(),
transitions={
'No_one_detected': 'ROAMING',
'person_detected': 'FOLLOW'
},
remapping={
'Roaming_id_in': 'id_person_detected',
'Roaming_id_out': 'id_person_detected'
})
smach.StateMachine.add('FOLLOW',
Follow(),
transitions={
'following': 'FOLLOW',
'lost_person_in_follow': 'ROAMING',
'reach_person': 'SPEAK'
},
remapping={
'Follow_id_in': 'id_person_detected',
'Follow_id_out': 'id_person_detected'
})
smach.StateMachine.add('SPEAK',
Speak(),
transitions={
'fail_speak': 'ROAMING',
'speaked': 'DETECT_GESTURE'
},
remapping={
'Speak_id_in': 'id_person_detected',
'Speak_id_out': 'id_person_detected'
})
smach.StateMachine.add('DETECT_GESTURE',
Detect_gesture(),
transitions={
'detecting_gesture': 'DETECT_GESTURE',
'gesture_detected': 'DO_GESTURE',
'fail_to_detect_gesture': 'ROAMING'
},
remapping={
'Detect_gesture_id_in':
'id_person_detected',
'Detect_gesture_id_out':
'id_person_detected'
})
smach.StateMachine.add('DO_GESTURE',
Do_gesture(),
transitions={
'fail_doing_gesture': 'ROAMING',
'succeed_doing_gesture': 'GO_TO_POINT'
},
remapping={
'Do_gesture_id_in': 'id_person_detected',
'Do_gesture_id_out': 'id_person_detected'
})
smach.StateMachine.add('GO_TO_POINT',
Go_to_point(),
transitions={
'fail_Reach_the_point': 'ROAMING',
'point_reached': 'STOP'
},
remapping={
'Go_point_id_in': 'id_person_detected',
'Go_point_id_out': 'id_person_detected'
})
#to visualization
sis = smach_ros.IntrospectionServer('server_name', sm, '/SM_ROOT')
sis.start()
# Execute SMACH plan
outcome = sm.execute()
rospy.spin()
sis.stop()
def pass_third():
pass_ball_third = smach.StateMachine(outcomes=['successed', 'failed'])
rospy.logwarn("pass_ball_third STARTING!!!")
with pass_ball_third:
smach.StateMachine.add('MOVE_TO_THREE_POINT_LINE',
Move_To_Three_Point_Line(),
transitions={
'successed': 'FindBall1',
'failed': 'failed'
})
smach.StateMachine.add('FindBall1',
Search_Ball_Shun(),
transitions={
'successed': 'SHOOT_ADJUST1',
'failed': 'failed'
})
smach.StateMachine.add('SHOOT_ADJUST1',
Shoot_Adjust1(),
transitions={
'successed': 'SHOOT1',
'failed': 'failed'
})
smach.StateMachine.add('SHOOT1',
Shoot(),
transitions={
'successed': 'SHOVEL_UP',
'failed': 'failed'
})
smach.StateMachine.add('SHOVEL_UP',
Shovel_UP(),
transitions={
'successed': 'FIND_ANOTHER_BALL',
'failed': 'failed'
})
smach.StateMachine.add('FIND_ANOTHER_BALL',
Find_Another_Ball(),
transitions={
'successed': 'FindBall2',
'failed': 'failed'
})
smach.StateMachine.add('FindBall2',
Search_Ball_Ni(),
transitions={
'successed': 'SHOOT_ADJUST2',
'failed': 'failed'
})
smach.StateMachine.add('SHOOT_ADJUST2',
Shoot_Adjust2(),
transitions={
'successed': 'SHOOT2',
'failed': 'failed'
})
smach.StateMachine.add('SHOOT2',
Shoot(),
transitions={
'successed': 'Shovel_Control_Up2',
'failed': 'failed'
})
smach.StateMachine.add('Shovel_Control_Up2',
Shovel_UP(),
transitions={
'successed': 'My Life For Auir',
'failed': 'failed'
})
Auir = smach.Concurrence(outcomes=['successed', 'failed'],
default_outcome='failed',
outcome_map={
'successed': {
'RETURN': 'successed',
'Return_Adjust': 'successed'
}
})
with Auir:
smach.Concurrence.add('RETURN', Return())
smach.Concurrence.add('Return_Adjust', Return_Adjust())
smach.StateMachine.add('My Life For Auir',
Auir,
transitions={
'successed': 'successed',
'failed': 'failed'
})
pass_ball_third.execute()
rospy.logerr('Pass_Ball_Third has finished!!!!')
def CreateStateMachine():
#Create the state machine
sm_rover = smach.StateMachine(outcomes=['DEAD'])
#Open the container
with sm_rover:
smach.StateMachine.add('INITIALISE',
INITIALISE(),
transitions={
'SUCCESS': 'READY',
'REPEAT': 'INITIALISE',
'FAIL': 'ERROR'
})
smach.StateMachine.add('READY',
READY(),
transitions={
'TO_GPS': 'REACH_GPS',
'FAIL': 'ERROR',
'REPEAT': 'READY'
})
smach.StateMachine.add('REACH_GPS',
REACH_GPS(),
transitions={
'SUCCESS': 'FIND_ARTAG',
'ARTAG_INTERRUPT': 'REACH_ARTAG',
'FAIL': 'ERROR',
'REPEAT': 'REACH_GPS'
})
smach.StateMachine.add('FIND_ARTAG',
FIND_ARTAG(),
transitions={
'SUCCESS': 'REACH_ARTAG',
'FAIL': 'ERROR',
'REPEAT': 'FIND_ARTAG'
})
smach.StateMachine.add('REACH_ARTAG',
REACH_ARTAG(),
transitions={
'SUCCESS': 'DEINITIALISE',
'FAIL': 'ERROR',
'REPEAT': 'REACH_ARTAG',
'BACK': 'FIND_ARTAG'
})
smach.StateMachine.add('DEINITIALISE',
DEINITIALISE(),
transitions={
'SUCCESS': 'INITIALISE',
'FAIL': 'ERROR',
'REPEAT': 'DEINITIALISE'
})
smach.StateMachine.add('ERROR',
ERROR(),
transitions={
'KILL': 'DEAD',
'REPEAT': 'ERROR',
'NOT_BAD': 'DEINITIALISE'
})
#Codes for smach viewer
sis = smach_ros.IntrospectionServer('rover20_state_machine', sm_rover,
'/ROVER_SM_ROOT')
sis.start()
outcome = sm_rover.execute()
sis.stop()
def main():
rospy.init_node('torpedo_lynnette_awesomeness', anonymous=False)
rosRate = rospy.Rate(30)
myCom = Comms()
rospy.loginfo("Torpedo Loaded")
sm = smach.StateMachine(outcomes=['succeeded', 'killed', 'failed'])
with sm:
smach.StateMachine.add("DISENGAGE",
Disengage(myCom),
transitions={
'start_complete': "ALIGNBOARD",
'killed': 'failed'
})
smach.StateMachine.add("FOLLOWSONAR",
FollowSonar(myCom),
transitions={
'following_sonar': "FOLLOWSONAR",
'sonar_complete': "ALIGNBOARD",
'aborted': 'killed',
'killed': 'failed'
})
smach.StateMachine.add("ALIGNBOARD",
AlignBoard(myCom),
transitions={
'aligning_board': "ALIGNBOARD",
'alignBoard_complete': "MOVEFORWARD",
'aborted': 'failed',
'killed': 'failed'
})
smach.StateMachine.add("SEARCHCIRCLES",
SearchCircles(myCom),
transitions={
'searchCircles_complete': "ALIGNBOARD",
'lost': 'failed',
'aborted': 'killed',
'killed': 'failed'
})
smach.StateMachine.add("MOVEFORWARD",
MoveForward(myCom),
transitions={
'forwarding': "MOVEFORWARD",
'forward_complete': "CENTERING",
'lost': 'failed',
'aborted': 'killed',
'killed': 'failed'
})
smach.StateMachine.add("CENTERING",
Centering(myCom),
transitions={
'centering': "CENTERING",
'centering_complete': "SHOOTING",
'lost': 'failed',
'aborted': 'killed',
'killed': 'failed'
})
smach.StateMachine.add("SHOOTING",
ShootTorpedo(myCom),
transitions={
'shoot_again': "SEARCHCIRCLES",
'shoot_complete': 'succeeded',
'aborted': 'killed',
'killed': 'failed'
})
#set up introspection Server
introServer = smach_ros.IntrospectionServer('mission_server', sm,
'/MISSION/TORPEDO')
introServer.start()
sm.execute()
def __init__(self, robot, reset_head=True, speak=True):
smach.StateMachine.__init__(
self, outcomes=['arrived', 'unreachable', 'goal_not_defined'])
self.robot = robot
self.speak = speak
with self:
# Create the sub SMACH state machine
sm_nav = smach.StateMachine(outcomes=[
'arrived', 'unreachable', 'goal_not_defined', 'preempted'
])
with sm_nav:
smach.StateMachine.add('GET_PLAN',
getPlan(self.robot,
self.generateConstraint,
self.speak),
transitions={
'unreachable': 'unreachable',
'goal_not_defined':
'goal_not_defined',
'goal_ok': 'EXECUTE_PLAN'
})
smach.StateMachine.add('EXECUTE_PLAN',
executePlan(self.robot,
self.breakOut,
reset_head=reset_head),
transitions={
'succeeded': 'arrived',
'arrived': 'GET_PLAN',
'blocked': 'PLAN_BLOCKED',
'preempted': 'preempted'
})
smach.StateMachine.add('PLAN_BLOCKED',
planBlocked(self.robot),
transitions={
'blocked': 'GET_PLAN',
'free': 'EXECUTE_PLAN'
})
smach.StateMachine.add('START_ANALYSIS',
StartAnalyzer(self.robot),
transitions={'done': 'NAVIGATE'})
smach.StateMachine.add('NAVIGATE',
sm_nav,
transitions={
'arrived': 'STOP_ANALYSIS_SUCCEED',
'unreachable':
'STOP_ANALYSIS_UNREACHABLE',
'goal_not_defined':
'ABORT_ANALYSIS_NOT_DEFINED',
'preempted': 'STOP_ANALYSIS_SUCCEED'
})
smach.StateMachine.add('STOP_ANALYSIS_SUCCEED',
StopAnalyzer(self.robot, 'succeeded'),
transitions={'done': 'arrived'})
smach.StateMachine.add('STOP_ANALYSIS_UNREACHABLE',
StopAnalyzer(self.robot, 'unreachable'),
transitions={'done': 'unreachable'})
smach.StateMachine.add('ABORT_ANALYSIS_NOT_DEFINED',
AbortAnalyzer(self.robot),
transitions={'done': 'goal_not_defined'})
import sys
sys.path.insert(0, '../states/')
from Move import Move
import smach
import rospy
import time
if __name__ == '__main__':
rospy.init_node('barrel_roll_test')
sm = smach.StateMachine(outcomes=['Success', 'Failure'])
with sm:
sm.userdata.type = 'gateManuever'
sm.userdata.args = {}
smach.StateMachine.add('MOVE', Move())
sm.execute()
def main():
# Get list of goal from gui
waypoint = []
goal_1 = rospy.get_param("~goal1", "station1")
goal_2 = rospy.get_param("~goal2", "station2")
goal_3 = rospy.get_param("~goal3", "base_station")
goal_4 = rospy.get_param("~goal4", "None")
goal_5 = rospy.get_param("~goal5", "None")
goal_list = [goal_1, goal_2, goal_3, goal_4, goal_5]
for index in range(len(goal_list)):
if goal_list[index] != "None":
waypoint.append(goal_list[index])
sm_nav = smach.StateMachine(outcomes=['shutdown'])
sm_nav.userdata.goal_list = waypoint
with sm_nav:
smach.StateMachine.add('SYSTEM_AVAILABILITY',
systemAvailability(),
transitions={'system_checked': 'TURN2GOAL'},
remapping={
'goalList_input': 'goal_list',
'goalList_output': 'goal_list'
})
smach.StateMachine.add('TURN2GOAL',
turn2goal(),
transitions={'turn_success': 'MOVE2GOAL'},
remapping={
'goalList_input': 'goal_list',
'goalList_output': 'goal_list'
})
smach.StateMachine.add('MOVE2GOAL',
move2goal(),
transitions={'move_success': 'SM_ACTION'},
remapping={
'goalList_input': 'goal_list',
'goalList_output': 'goal_list'
})
smach.StateMachine.add('WAIT4NEXTROUND',
wait4nextround(),
transitions={'finished_process': 'shutdown'})
sm_act = smach.StateMachine(
outcomes=['align_finished', 'all_finished'])
sm_act.userdata.goal_list = waypoint
with sm_act:
smach.StateMachine.add(
'REACH2GOAL',
reach2goal(),
transitions={'reach_success': 'GOAL_ALIGNMENT'},
remapping={
'goalList_input': 'goal_list',
'goalList_output': 'goal_list'
})
smach.StateMachine.add('GOAL_ALIGNMENT',
goalAlignment(),
transitions={
'align_success': 'align_finished',
'all_success': 'all_finished'
},
remapping={
'goalList_input': 'goal_list',
'goalList_output': 'goal_list'
})
smach.StateMachine.add('SM_ACTION',
sm_act,
transitions={
'align_finished': 'TURN2GOAL',
'all_finished': 'WAIT4NEXTROUND'
})
sis = smach_ros.IntrospectionServer('server_name', sm_nav, 'SM_NAV/SM_ACT')
sis.start()
outcome = sm_nav.execute()
# rospy.spin()
sis.stop()
def main(mokeup=False):
# Open the container
rospy.init_node('insert_object_server')
# Construct state machine
sm = smach.StateMachine(
outcomes=['OVERALL_SUCCESS','OVERALL_FAILED'],
input_keys = ['insert_object_goal'],
output_keys = ['insert_object_feedback', 'insert_object_result'])
with sm:
if not mokeup:
smach.StateMachine.add('SELECT_OBJECT', select_object('/mcr_perception/object_selector/input/object_name'),
transitions={'success':'GENERATE_OBJECT_POSE'})
# generates a pose based on the previous string object topic received
smach.StateMachine.add('GENERATE_OBJECT_POSE', send_event('/mcr_perception/object_selector/event_in', 'e_trigger'),
transitions={'success':'CHECK_IF_OBJECT_IS_AVAILABLE'})
# waits for object selector response, if failure this means that the string published previously was not found in object list
smach.StateMachine.add('CHECK_IF_OBJECT_IS_AVAILABLE', wait_for_event('/mcr_perception/object_selector/event_out', 1.0),
transitions={'success':'PLAN_WHOLE_BODY_MOTION',
'timeout': 'SET_ACTION_LIB_FAILURE',
'failure':'SET_ACTION_LIB_FAILURE'})
smach.StateMachine.add('PLAN_WHOLE_BODY_MOTION', send_event('/whole_body_motion_calculator_pipeline/event_in','e_start'),
transitions={'success':'WAIT_PLAN_WHOLE_BODY_MOTION'})
# wait for the result of the pregrasp planner
smach.StateMachine.add('WAIT_PLAN_WHOLE_BODY_MOTION', wait_for_event('/whole_body_motion_calculator_pipeline/event_out', 15.0),
transitions={'success':'CALCULATE_BASE_MOTION',
'timeout': 'STOP_PLAN_WHOLE_BODY_MOTION_WITH_FAILURE',
'failure':'STOP_PLAN_WHOLE_BODY_MOTION_WITH_FAILURE'})
# pregrasp planner failed or timeout, stop the component and then return overall failure
smach.StateMachine.add('STOP_PLAN_WHOLE_BODY_MOTION_WITH_FAILURE', send_event('/whole_body_motion_calculator_pipeline/event_in','e_stop'),
transitions={'success':'SELECT_PREGRASP_PLANNER_INPUT_2'}) # go to select pose input and plan arm motion
smach.StateMachine.add('CALCULATE_BASE_MOTION', gbs.send_and_wait_events_combined(
event_in_list=[('/base_motion_calculator/event_in','e_start')],
event_out_list=[('/base_motion_calculator/event_out','e_success', True)],
timeout_duration=5),
transitions={'success':'STOP_PLAN_WHOLE_BODY_MOTION',
'timeout':'STOP_MOVE_BASE_TO_OBJECT',
'failure':'STOP_MOVE_BASE_TO_OBJECT'})
# wbc pipeline was successful, so lets stop it since its work is done
smach.StateMachine.add('STOP_PLAN_WHOLE_BODY_MOTION', send_event('/whole_body_motion_calculator_pipeline/event_in','e_stop'),
transitions={'success':'UNSTAGE_OBJECT'})
smach.StateMachine.add('UNSTAGE_OBJECT', unstage(),
transitions={'succeeded': 'MOVE_ROBOT_TO_OBJECT',
'failed' : 'SET_ACTION_LIB_FAILURE'})
# execute robot motion
smach.StateMachine.add('MOVE_ROBOT_TO_OBJECT', gbs.send_and_wait_events_combined(
event_in_list=[('/wbc/mcr_navigation/direct_base_controller/coordinator/event_in','e_start')],
event_out_list=[('/wbc/mcr_navigation/direct_base_controller/coordinator/event_out','e_success', True)],
timeout_duration=5),
transitions={'success':'STOP_MOVE_BASE_TO_OBJECT',
'timeout':'STOP_MOVE_ROBOT_TO_OBJECT_WITH_FAILURE',
'failure':'STOP_MOVE_ROBOT_TO_OBJECT_WITH_FAILURE'})
# send stop event_in to arm motion component
smach.StateMachine.add('STOP_MOVE_BASE_TO_OBJECT',
send_event('/wbc/mcr_navigation/direct_base_controller/coordinator/event_in','e_stop'),
transitions={'success':'SELECT_PREGRASP_PLANNER_INPUT_2'})
# send stop event_in to arm motion component and return failure
smach.StateMachine.add('STOP_MOVE_ROBOT_TO_OBJECT_WITH_FAILURE',
send_event('/wbc/mcr_navigation/direct_base_controller/coordinator/event_in','e_stop'),
transitions={'success':'SELECT_PREGRASP_PLANNER_INPUT_2'})
########################################## PREGRASP PIPELINE 2 ##################################################
# based on a published pose, calls pregrasp planner to generate a graspable pose
smach.StateMachine.add('SELECT_PREGRASP_PLANNER_INPUT_2', send_event('/pregrasp_planner_pipeline/event_in','e_start'),
transitions={'success':'WAIT_PLAN_ARM_MOTION_2'})
# wait for the result of the pregrasp planner
smach.StateMachine.add('WAIT_PLAN_ARM_MOTION_2', wait_for_event('/pregrasp_planner_pipeline/event_out', 15.0),
transitions={'success':'STOP_PLAN_ARM_MOTION_2',
'timeout': 'STOP_MOVE_ARM_TO_OBJECT_WITH_FAILURE_2',
'failure':'STOP_MOVE_ARM_TO_OBJECT_WITH_FAILURE_2'})
smach.StateMachine.add('STOP_PLAN_ARM_MOTION_2', send_event('/pregrasp_planner_pipeline/event_in','e_stop'),
transitions={'success':'MOVE_ARM_TO_OBJECT_2'})
# execute robot motion
smach.StateMachine.add('MOVE_ARM_TO_OBJECT_2', gbs.send_and_wait_events_combined(
event_in_list=[('/waypoint_trajectory_generation/event_in','e_start')],
event_out_list=[('/waypoint_trajectory_generation/event_out','e_success', True)],
timeout_duration=10),
transitions={'success':'STOP_MOVE_ARM_TO_OBJECT_2',
'timeout':'STOP_MOVE_ARM_TO_OBJECT_WITH_FAILURE_2',
'failure':'STOP_MOVE_ARM_TO_OBJECT_WITH_FAILURE_2'})
smach.StateMachine.add('STOP_MOVE_ARM_TO_OBJECT_2', send_event('/waypoint_trajectory_generation/event_out','e_stop'),
transitions={'success':'OPEN_GRIPPER'})
smach.StateMachine.add('STOP_MOVE_ARM_TO_OBJECT_WITH_FAILURE_2', send_event('/waypoint_trajectory_generation/event_out','e_stop'),
transitions={'success':'MOVE_ARM_TO_DEFAULT_PLACE'})
# close gripper
smach.StateMachine.add('OPEN_GRIPPER', gms.control_gripper('open_narrow'),
transitions={'succeeded': 'MOVE_ARM_TO_HOLD'})
# move arm to HOLD position
smach.StateMachine.add('MOVE_ARM_TO_HOLD', gms.move_arm("look_at_turntable"),
transitions={'succeeded':'SET_ACTION_LIB_SUCCESS',
'failed':'MOVE_ARM_TO_HOLD'})
# place in case of failure
smach.StateMachine.add('MOVE_ARM_TO_DEFAULT_PLACE', gms.move_arm("10cm/pose4"),
transitions={'succeeded':'OPEN_GRIPPER_DEFAULT',
'failed':'MOVE_ARM_TO_DEFAULT_PLACE'})
smach.StateMachine.add('OPEN_GRIPPER_DEFAULT', gms.control_gripper('open_narrow'),
transitions={'succeeded': 'MOVE_ARM_BACK'})
smach.StateMachine.add('MOVE_ARM_BACK', gms.move_arm("look_at_turntable"),
transitions={'succeeded':'SET_ACTION_LIB_SUCCESS',
'failed':'SET_ACTION_LIB_SUCCESS'})
# set action lib result
smach.StateMachine.add('SET_ACTION_LIB_SUCCESS', SetActionLibResult(True),
transitions={'succeeded':'OVERALL_SUCCESS'})
# set action lib result
smach.StateMachine.add('SET_ACTION_LIB_FAILURE', SetActionLibResult(False),
transitions={'succeeded':'OVERALL_FAILED'})
"""smach.StateMachine.add('MOVE_ARM_TO_HOLD_FAILURE', gms.move_arm("look_at_turntable"),
transitions={'succeeded':'OVERALL_FAILED',
'failed':'OVERALL_FAILED'})"""
# smach viewer
sis = smach_ros.IntrospectionServer('insert_object_smach_viewer', sm, '/INSERT_OBJECT_SMACH_VIEWER')
sis.start()
# Construct action server wrapper
asw = ActionServerWrapper(
server_name = 'insert_object_server',
action_spec = InsertObjectAction,
wrapped_container = sm,
succeeded_outcomes = ['OVERALL_SUCCESS'],
aborted_outcomes = ['OVERALL_FAILED'],
preempted_outcomes = ['PREEMPTED'],
goal_key = 'insert_object_goal',
feedback_key = 'insert_object_feedback',
result_key = 'insert_object_result')
# Run the server in a background thread
asw.run_server()
rospy.spin()
def setup_statemachine(robot):
sm = smach.StateMachine(outcomes=['Done', 'Aborted'])
with sm:
# Start challenge via StartChallengeRobust
smach.StateMachine.add("START_CHALLENGE_ROBUST",
states.StartChallengeRobust(
robot,
STARTING_POINT,
use_entry_points=True),
transitions={
"Done": "GO_TO_INTERMEDIATE_WAYPOINT",
"Aborted": "GO_TO_INTERMEDIATE_WAYPOINT",
"Failed": "GO_TO_INTERMEDIATE_WAYPOINT"
})
# There is no transition to Failed in StartChallengeRobust (28 May)
smach.StateMachine.add('GO_TO_INTERMEDIATE_WAYPOINT',
states.NavigateToWaypoint(
robot,
EntityByIdDesignator(robot,
id=INTERMEDIATE_1),
radius=0.5),
transitions={
'arrived':
'ASK_CONTINUE',
'unreachable':
'GO_TO_INTERMEDIATE_WAYPOINT_BACKUP1',
'goal_not_defined':
'GO_TO_INTERMEDIATE_WAYPOINT_BACKUP1'
})
smach.StateMachine.add('GO_TO_INTERMEDIATE_WAYPOINT_BACKUP1',
states.NavigateToWaypoint(
robot,
EntityByIdDesignator(robot,
id=INTERMEDIATE_2),
radius=0.5),
transitions={
'arrived':
'ASK_CONTINUE',
'unreachable':
'GO_TO_INTERMEDIATE_WAYPOINT_BACKUP2',
'goal_not_defined':
'GO_TO_INTERMEDIATE_WAYPOINT_BACKUP2'
})
smach.StateMachine.add('GO_TO_INTERMEDIATE_WAYPOINT_BACKUP2',
states.NavigateToWaypoint(
robot,
EntityByIdDesignator(robot,
id=INTERMEDIATE_3),
radius=0.5),
transitions={
'arrived': 'ASK_CONTINUE',
'unreachable': 'ASK_CONTINUE',
'goal_not_defined': 'ASK_CONTINUE'
})
smach.StateMachine.add("ASK_CONTINUE",
states.AskContinue(robot, rospy.Duration(30)),
transitions={
'continue': 'SAY_CONTINUEING',
'no_response': 'SAY_CONTINUEING'
})
smach.StateMachine.add(
'SAY_CONTINUEING',
states.Say(robot, [
"I heard continue, so I will move to the exit now. See you guys later!"
],
block=False),
transitions={'spoken': 'GO_TO_EXIT'})
# Amigo goes to the exit (waypoint stated in knowledge base)
smach.StateMachine.add('GO_TO_EXIT',
states.NavigateToWaypoint(robot,
EntityByIdDesignator(
robot, id=EXIT_1),
radius=0.7),
transitions={
'arrived': 'AT_END',
'unreachable': 'GO_TO_EXIT_2',
'goal_not_defined': 'GO_TO_EXIT_2'
})
smach.StateMachine.add('GO_TO_EXIT_2',
states.NavigateToWaypoint(robot,
EntityByIdDesignator(
robot, id=EXIT_2),
radius=0.5),
transitions={
'arrived': 'AT_END',
'unreachable': 'GO_TO_EXIT_3',
'goal_not_defined': 'GO_TO_EXIT_3'
})
smach.StateMachine.add('GO_TO_EXIT_3',
states.NavigateToWaypoint(robot,
EntityByIdDesignator(
robot, id=EXIT_3),
radius=0.5),
transitions={
'arrived': 'AT_END',
'unreachable': 'RESET_ED_TARGET',
'goal_not_defined': 'AT_END'
})
smach.StateMachine.add('RESET_ED_TARGET',
states.ResetED(robot),
transitions={'done': 'GO_TO_EXIT'})
# Finally amigo will stop and says 'goodbye' to show that he's done.
smach.StateMachine.add('AT_END',
states.Say(robot, "Goodbye"),
transitions={'spoken': 'Done'})
analyse_designators(sm, "rips")
return sm
def main():
"""RSDK Inverse Kinematics Pick and Place Example
A Pick and Place example using the Rethink Inverse Kinematics
Service which returns the joint angles a requested Cartesian Pose.
This ROS Service client is used to request both pick and place
poses in the /base frame of the robot.
Note: This is a highly scripted and tuned demo. The object location
is "known" and movement is done completely open loop. It is expected
behavior that Baxter will eventually mis-pick or drop the block. You
can improve on this demo by adding perception and feedback to close
the loop.
"""
rospy.init_node("pick_n_place_client")
rospy.on_shutdown(shutdown)
#rospy.wait_for_message("/robot/sim/started", Empty)
#ipdb.set_trace()
sm = smach.StateMachine(outcomes=['Done'])
sm.userdata.sm_pick_object_pose = Pose()
sm.userdata.sm_place_object_pose = Pose()
sm.userdata.sm_hover_distance = 0.15
with sm:
smach.StateMachine.add(
'Go_to_Start_Position',
Go_to_Start_Position(),
transitions={'Succeed': 'Setting_Start_and_End_Pose'})
smach.StateMachine.add('Setting_Start_and_End_Pose',
Setting_Start_and_End_Pose(),
transitions={'Succeed': 'Add_Box_Gazebo_Model'},
remapping={
'pick_object_pose': 'sm_pick_object_pose',
'place_object_pose': 'sm_place_object_pose'
})
smach.StateMachine.add(
'Add_Box_Gazebo_Model',
Add_Box_Gazebo_Model(),
transitions={'Succeed': 'Go_to_Pick_Hover_Position'},
remapping={'pick_object_pose': 'sm_pick_object_pose'})
smach.StateMachine.add('Go_to_Pick_Hover_Position',
Go_to_Pick_Hover_Position(),
transitions={
'Succeed': 'Pick_Object',
'IK_Fail': 'Go_to_Start_Position',
'Time_Out': 'Go_to_Start_Position'
},
remapping={
'pick_object_pose': 'sm_pick_object_pose',
'hover_distance': 'sm_hover_distance'
})
smach.StateMachine.add('Pick_Object',
Pick_Object(),
transitions={
'Succeed': 'Go_to_Place_Hover_Position',
'IK_Fail': 'Go_to_Start_Position',
'Time_Out': 'Go_to_Start_Position'
},
remapping={
'pick_object_pose': 'sm_pick_object_pose',
'hover_distance': 'sm_hover_distance'
})
smach.StateMachine.add('Go_to_Place_Hover_Position',
Go_to_Place_Hover_Position(),
transitions={
'Succeed': 'Place_Object',
'IK_Fail': 'Go_to_Start_Position',
'Time_Out': 'Go_to_Start_Position'
},
remapping={
'place_object_pose': 'sm_place_object_pose',
'hover_distance': 'sm_hover_distance'
})
smach.StateMachine.add('Place_Object',
Place_Object(),
transitions={
'Succeed': 'Done',
'IK_Fail': 'Go_to_Start_Position',
'Time_Out': 'Go_to_Start_Position'
},
remapping={
'place_object_pose': 'sm_place_object_pose',
'hover_distance': 'sm_hover_distance'
})
sis = smach_ros.IntrospectionServer('MY_SERVER', sm, '/SM_ROOT')
sis.start()
outcome = sm.execute()
rospy.spin()
smach.StateMachine.add('CENTER_TARGET', TargetColor(Color.GREEN),
transitions={'success': 'DROP_TARGET',
'fail': 'fail',
'timeout': 'DROP_TARGET'})
# Finally, drop the marker.
smach.StateMachine.add('DROP_TARGET',
DropMarker(),
transitions={'success': 'success'})
if __name__ == '__main__':
rospy.init_node('ai')
# Wait for ROS time to properly begin.
while rospy.get_time() == 0:
continue
sm = smach.StateMachine(outcomes=['success', 'fail'])
with sm:
smach.StateMachine.add('START_SWITCH', start_switch(),
transitions={'success': 'ROULETTE'})
smach.StateMachine.add('ROULETTE', RouletteTask(),
transitions={'success': 'success',
'fail': 'fail'})
sis = smach_ros.IntrospectionServer('smach_server', sm, '/SM_ROOT')
sis.start()
outcome = sm.execute()
sis.stop()
# restore to a position from where it can still do the task
pass
class StationKeeping(smach.State):
def __init__(self):
smach.State.__init__(self, outcomes=['success'])
def execute(self):
# just maintain the current pose
pass
if __name__ == '__main__':
rospy.init_node('state_machine')
# Create a SMACH state machine
sm = smach.StateMachine(outcomes=['outcome4'])
# Open the container
with sm:
# Add states to the container
smach.StateMachine.add('GateTask', GateTask(), transitions={'crossed':'MoveToXYZ', 'fatal_fail':'RescueMode', 'cannot_see':'FindFrontTarget'})
smach.StateMachine.add('HoverXYZ', HoverXYZ(), transitions={'detected':'IdentifyTarget', 'cannot_see':'FindFrontTarget'})
smach.StateMachine.add('FindFrontTarget', FindFrontTarget(), transitions = {'found_visually':'IdentifyTarget', 'lost':'MovingToXYZ'})
smach.StateMachine.add('IdentifyTarget', IdentifyTarget())
sis = smach_ros.IntrospectionServer('smach_introspect', sm, '/SM_ROOT')
sis.start()
outcome = sm.execute()
def main():
rospy.init_node('place_object_server')
# Construct state machine
sm = smach.StateMachine(
outcomes=['OVERALL_SUCCESS', 'OVERALL_FAILED'],
input_keys=['place_object_goal', 'place_object_feedback'],
output_keys=['place_object_feedback', 'place_object_result'])
with sm:
#add states to the container
smach.StateMachine.add(
'START_PLACE_POSE_SELECTOR',
gbs.send_event([('/mcr_perception/place_pose_selector/event_in',
'e_start')]),
transitions={'success': 'GET_POSE_TO_PLACE_OBJECT'})
smach.StateMachine.add(
'GET_POSE_TO_PLACE_OBJECT',
GetPoseToPlaceOject(
'/mcr_perception/place_pose_selector/platform_name',
'/mcr_perception/place_pose_selector/place_pose',
'/mcr_perception/place_pose_selector/event_out', 15.0),
transitions={
'succeeded': 'MOVE_ARM_TO_PLACE_OBJECT',
'failed': 'SET_ACTION_LIB_FAILURE'
})
smach.StateMachine.add('MOVE_ARM_TO_PLACE_OBJECT',
gms.move_arm(),
transitions={
'succeeded': 'OPEN_GRIPPER',
'failed': 'OPEN_GRIPPER'
})
smach.StateMachine.add(
'OPEN_GRIPPER',
gms.control_gripper('open'),
transitions={'succeeded': 'STOP_PLACE_POSE_SELECTOR'})
"""smach.StateMachine.add('MOVE_ARM_TO_HOLD', gms.move_arm("navigation"),
transitions={'succeeded':'STOP_PLACE_POSE_SELECTOR',
'failed':'MOVE_ARM_TO_HOLD'})"""
smach.StateMachine.add(
'STOP_PLACE_POSE_SELECTOR',
gbs.send_event([('/mcr_perception/place_pose_selector/event_in',
'e_stop')]),
transitions={'success': 'SET_ACTION_LIB_SUCCESS'})
smach.StateMachine.add('SET_ACTION_LIB_SUCCESS',
SetActionLibResult(True),
transitions={'succeeded': 'OVERALL_SUCCESS'})
smach.StateMachine.add('SET_ACTION_LIB_FAILURE',
SetActionLibResult(False),
transitions={'succeeded': 'OVERALL_FAILED'})
# smach viewer
sis = smach_ros.IntrospectionServer('place_object_smach_viewer', sm,
'/STAGE_OBJECT_SMACH_VIEWER')
sis.start()
# Construct action server wrapper
asw = ActionServerWrapper(server_name='place_object_server',
action_spec=PlaceObjectAction,
wrapped_container=sm,
succeeded_outcomes=['OVERALL_SUCCESS'],
aborted_outcomes=['OVERALL_FAILED'],
preempted_outcomes=['PREEMPTED'],
goal_key='place_object_goal',
feedback_key='place_object_feedback',
result_key='place_object_result')
# Run the server in a background thread
asw.run_server()
rospy.spin()
def makeStowJobStateMachine(planner, tf, system):
listener = tf
sm = smach.StateMachine(outcomes=['success', 'failed', 'error'],
input_keys=[
'current_master_pose', 'job', 'bin_contents',
'tote_contents', 'blacklist'
],
output_keys=['current_master_pose'])
sm.userdata.end_master_pose = None
with sm:
smach.StateMachine.add('Sense In Tote',
SenseInTote(planner, tf, system),
transitions={
'success': 'Sense Target Bin',
'no_detection': 'No Detection',
'no_object_pose': 'No Object Pose',
'error': 'error',
},
remapping={
'job': 'job',
'tote_contents': 'tote_contents',
'tote_pose': 'tote_pose',
'blacklist': 'blacklist',
'object_type': 'object_type',
'object_pose': 'object_pose',
'object_cloud': 'object_cloud',
})
smach.StateMachine.add('Sense Target Bin',
SenseTargetBin(tf, system),
transitions={
'success': 'Plan Grasp',
'no_detection': 'No Detection',
'error': 'error',
},
remapping={
'job': 'job',
'target_bin_pose': 'target_bin_pose',
})
smach.StateMachine.add('Plan Grasp',
PlanToteGrasp(system),
transitions={
'success': 'Plan Stow',
'no_plan': 'No Plan',
'error': 'error',
},
remapping={
'object_type': 'object_type',
'tote_pose': 'tote_pose',
'object_pose': 'object_pose',
'object_cloud': 'object_cloud',
'grasp': 'grasp',
'grasp_plan': 'grasp_plan',
})
smach.StateMachine.add('Plan Stow',
PlanBinStow(system),
transitions={
'success': 'Pick From Tote',
'no_plan': 'No Plan',
'error': 'error',
},
remapping={
'job': 'job',
'object_type': 'object_type',
'target_bin_pose': 'target_bin_pose',
'grasp': 'grasp',
'stow_plan ': 'stow_plan',
})
smach.StateMachine.add('Pick From Tote',
PickFromTote(system),
transitions={
'success': 'Stow In Bin',
'no_grasp': 'No Grasp',
'error': 'error',
},
remapping={
'current_master_pose':
'current_master_pose',
'job': 'job',
'grasp': 'grasp',
'grasp_plan': 'grasp_plan',
})
smach.StateMachine.add('Stow In Bin',
StowInBin(system),
transitions={
'success': 'Success',
'error': 'error',
},
remapping={
'current_master_pose':
'current_master_pose',
'job': 'job',
'grasp': 'grasp',
'stow_plan': 'stow_plan',
})
smach.StateMachine.add('Success',
ReportJobResult(planner, Job.StowSuccess,
'object_type'),
transitions={'succeeded': 'success'})
smach.StateMachine.add('No Detection',
ReportJobResult(planner, Job.NoDetection),
transitions={'succeeded': 'failed'})
smach.StateMachine.add('No Object Pose',
ReportJobResult(planner, Job.NoObjectPose,
'object_type'),
transitions={'succeeded': 'failed'})
smach.StateMachine.add('No Plan',
ReportJobResult(planner, Job.NoPlan,
'object_type'),
transitions={'succeeded': 'failed'})
smach.StateMachine.add('No Grasp',
ReportJobResult(planner, Job.NoGrasp,
'object_type'),
transitions={'succeeded': 'failed'})
return sm
def makeStateMachine():
from .status import Status
# Set default values of parameters
if not rospy.has_param("step_by_step"):
rospy.set_param("step_by_step", 0)
sm = smach.StateMachine(outcomes=["aborted",])
status = Status()
with sm:
from agimus_hpp.client import HppClient
hppclient = HppClient(context="corbaserver", connect=False)
smach.StateMachine.add(
"WaitForInput",
WaitForInput(status, hppclient),
transitions={
"start_path": "Init",
"failed_to_start": "WaitForInput",
"interrupted": "aborted"},
remapping={
"pathId": "pathId",
"times": "times",
"transitionIds": "transitionIds",
"endStateIds": "endStateIds",
"currentSection": "currentSection",
"queue_initialized": "queue_initialized",
},
)
smach.StateMachine.add(
"Init",
InitializePath(status, hppclient),
transitions={
"finished": "WaitForInput",
"move_base": "MoveBase",
"next": "Play",
},
remapping={
"pathId": "pathId",
"times": "times",
"transitionId": "transitionId",
"currentSection": "currentSection",
},
)
smach.StateMachine.add(
"MoveBase",
MoveBase(status),
transitions={
"succeeded": "Init",
"preempted": "Error",
},
remapping={
"currentSection": "currentSection",
"times": "times",
"pathId": "pathId",
},
)
smach.StateMachine.add(
"Play",
PlayPath(status),
transitions={
"succeeded": "Init",
"aborted": "WaitForInput",
"preempted": "Error",
},
remapping={
"transitionId": "transitionId",
"duration": "duration",
"currentSection": "currentSection",
"queue_initialized": "queue_initialized",
},
)
smach.StateMachine.add(
"Error",
ErrorState(status),
transitions={
"finished": "WaitForInput",
},
)
sm.set_initial_state(["WaitForInput"])
sis = smach_ros.IntrospectionServer("agimus", sm, "/AGIMUS")
return sm, sis
def construct():
sm = smach.StateMachine(outcomes=['preempted'])
# Attach helper functions
sm.set_ranges = MethodType(set_ranges, sm, sm.__class__)
sm.get_twist = MethodType(get_twist, sm, sm.__class__)
sm.set_config = MethodType(set_config, sm, sm.__class__)
# Set initial values in userdata
sm.userdata.velocity = (0, 0, 0)
sm.userdata.mode = 1
sm.userdata.clearance = 0.7
sm.userdata.ranges = None
sm.userdata.max_forward_velocity = 0.3
sm.userdata.default_rotational_speed = 0.5
sm.userdata.direction = 1
with sm:
#pass
#=========================== YOUR CODE HERE ===========================
# Instructions: construct the state machine by adding the states that
# you have implemented.
# Below is an example how to add a state:
#
# smach.StateMachine.add('SEARCH',
# PreemptableState(search,
# input_keys=['front_min', 'clearance'],
# output_keys=['velocity'],
# outcomes=['found_obstacle']),
# transitions={'found_obstacle': 'ANOTHER_STATE'})
#
# First argument is the state label, an arbitrary string
# (by convention should be uppercase). Second argument is
# an object that implements the state. In our case an
# instance of the helper class PreemptableState is
# created, and the state function in passed. Moreover,
# we have to specify which keys in the userdata the
# function will need to access for reading (input_keys)
# and for writing (output_keys), and the list of possible
# outcomes of the state. Finally, the transitions are
# specified. Normally you would have one transition per
# state outcome.
#
# Note: The first state that you add will become the initial state of
# the state machine.
#======================================================================
#==============================================================================
# SEARCH : Used to search for any wall, used during initialization
#==============================================================================
smach.StateMachine.add('SEARCH',PreemptableState(search,input_keys=['front_min','clearance'],
output_keys=['velocity'],
outcomes=['found_wall']),
transitions={'found_wall': 'ALIGN'})
#==============================================================================
# ALIGN : Used to alignt to a wall, accroding to mode selected
#==============================================================================
smach.StateMachine.add('ALIGN',PreemptableState(align,input_keys=['front_min', 'clearance',
'left_1','left_2',
'right_1','right_2',
'direction','default_rotational_speed'],
output_keys=['velocity'],
outcomes=['aligned']),
transitions={'aligned': 'FOLLOW_WALL'})
#==============================================================================
# FOLLOW_WALL : Used to follow a wall once aligned
#==============================================================================
smach.StateMachine.add('FOLLOW_WALL',PreemptableState(follow_wall,input_keys=['front_min', 'clearance',
'left_1','left_2',
'right_1','right_2',
'front_right','front_left',
'direction','default_rotational_speed',
'max_forward_velocity','back_right','back_left'],
output_keys=['velocity'],
outcomes=['concave','no_wall','convex','align']),
transitions={'concave': 'CONCAVE',
'no_wall':'TRANSITION_SEARCH',
'convex':'CONVEX',
'align' : 'ALIGN'})
#==============================================================================
# TRANSITION_SEARCH : If mode is changed during wall following, this is
# used to search for wall in the required direction
#==============================================================================
smach.StateMachine.add('TRANSITION_SEARCH',PreemptableState(transition_search,input_keys=['clearance','front_min',
'left_1','left_2',
'right_1','right_2',
'direction','default_rotational_speed'],
output_keys=['velocity'],
outcomes=['found_wall']),
transitions={'found_wall': 'ALIGN'})
#==============================================================================
# Concave State is called to manoeuvre corners with walls straight ahead
#==============================================================================
smach.StateMachine.add('CONCAVE', PreemptableState(concave,input_keys=['front_min', 'clearance',
'front_1','front_2',
'left_1','left_2',
'right_1','right_2',
'front_right','front_left',
'direction','default_rotational_speed'],
output_keys=['velocity'],
outcomes=['navigated']),
transitions={'navigated': 'FOLLOW_WALL' })
#==============================================================================
# Convex State is called to manoeuvre corners with gaps in walls (like doors)
#==============================================================================
smach.StateMachine.add('CONVEX', PreemptableState(convex,input_keys=['front_min', 'clearance','max_forward_velocity',
'front_1','front_2',
'left_1','left_2',
'right_1','right_2',
'front_right','front_left',
'direction','default_rotational_speed'],
output_keys=['velocity'],
outcomes=['navigated']),
transitions={'navigated': 'FOLLOW_WALL' })
"""
#STOP state was used as a redundant temporary state for undefined states
smach.StateMachine.add('STOP',PreemptableState(stop,input_keys=['front_min','clearance'],
output_keys=['velocity'],
outcomes=['stop']),
transitions={'stop': 'STOP'})
"""
return sm
def main(mokeup=False):
# Open the container
rospy.init_node('pick_object_server')
# Construct state machine
sm = smach.StateMachine(
outcomes=['OVERALL_SUCCESS', 'OVERALL_FAILED'],
input_keys=['pick_object_goal'],
output_keys=['pick_object_feedback', 'pick_object_result'])
with sm:
if not mokeup:
# publish object as string to mcr_perception_selectors -> object_selector, this component then publishes
# pose in base_link reference frame when e_trigger is sent (next state)
smach.StateMachine.add(
'SELECT_OBJECT',
select_object(
'/mcr_perception/object_selector/input/object_name'),
transitions={'success': 'OPEN_GRIPPER'})
# open gripper
smach.StateMachine.add(
'OPEN_GRIPPER',
gms.control_gripper('open'),
transitions={'succeeded': 'GENERATE_OBJECT_POSE'})
# generates a pose based on the previous string object topic received
smach.StateMachine.add(
'GENERATE_OBJECT_POSE',
send_event('/mcr_perception/object_selector/event_in',
'e_trigger'),
transitions={'success': 'CHECK_IF_OBJECT_IS_AVAILABLE'})
# waits for object selector response, if failure this means that the string published previously was not found in object list
smach.StateMachine.add(
'CHECK_IF_OBJECT_IS_AVAILABLE',
wait_for_event('/mcr_perception/object_selector/event_out',
1.0),
transitions={
'success': 'RECONFIGURE_PREGRASP_PARAMS',
'timeout': 'SET_ACTION_LIB_FAILURE',
'failure': 'SET_ACTION_LIB_FAILURE'
})
# reconfigure pregrasp planner for pick
smach.StateMachine.add(
'RECONFIGURE_PREGRASP_PARAMS',
gbs.set_named_config('pick_normal_pregrasp'),
transitions={
'success': 'PLAN_ARM_MOTION',
'failure': 'SET_ACTION_LIB_FAILURE',
'timeout': 'RECONFIGURE_PREGRASP_PARAMS'
})
# based on a published pose, calls pregrasp planner to generate a graspable pose and wait for the result of the pregrasp planner
smach.StateMachine.add(
'PLAN_ARM_MOTION',
gbs.send_and_wait_events_combined(
event_in_list=[('/pregrasp_planner_pipeline/event_in',
'e_start')],
event_out_list=[('/pregrasp_planner_pipeline/event_out',
'e_success', True)],
timeout_duration=10),
transitions={
'success': 'STOP_PLAN_ARM_MOTION',
'timeout': 'STOP_PLAN_ARM_MOTION_WITH_FAILURE',
'failure': 'STOP_PLAN_ARM_MOTION_WITH_FAILURE'
})
# pregrasp planner was successful, so lets stop it since its work is done
smach.StateMachine.add('STOP_PLAN_ARM_MOTION',
send_event(
'/pregrasp_planner_pipeline/event_in',
'e_stop'),
transitions={'success': 'MOVE_ARM_TO_OBJECT'})
# pregrasp planner failed or timeout, stop the component and then return overall failure
smach.StateMachine.add(
'STOP_PLAN_ARM_MOTION_WITH_FAILURE',
send_event('/pregrasp_planner_pipeline/event_in', 'e_stop'),
transitions={'success': 'SET_ACTION_LIB_FAILURE'})
# move arm to pregrasp planned pose and wait for its response
smach.StateMachine.add(
'MOVE_ARM_TO_OBJECT',
gbs.send_and_wait_events_combined(
event_in_list=[('/move_arm_planned/event_in', 'e_start')],
event_out_list=[('/move_arm_planned/event_out', 'e_success',
True)],
timeout_duration=7),
transitions={
'success': 'STOP_MOVE_ARM_TO_OBJECT',
'timeout': 'STOP_MOVE_ARM_TO_OBJECT',
'failure': 'STOP_MOVE_ARM_TO_OBJECT_WITH_FAILURE'
})
# send stop event_in to arm motion component and return failure
smach.StateMachine.add(
'STOP_MOVE_ARM_TO_OBJECT_WITH_FAILURE',
send_event('/move_arm_planned/event_in', 'e_stop'),
transitions={'success': 'SET_ACTION_LIB_FAILURE'})
# send stop event_in to arm motion component
smach.StateMachine.add('STOP_MOVE_ARM_TO_OBJECT',
send_event('/move_arm_planned/event_in',
'e_stop'),
transitions={'success': 'CLOSE_GRIPPER'})
# close gripper
smach.StateMachine.add('CLOSE_GRIPPER',
gms.control_gripper('close'),
transitions={'succeeded': 'MOVE_ARM_TO_HOLD'})
# check if gripper changed in position, if not then close again
# smach.StateMachine.add('VERIFY_GRIPPER_CLOSED', gbs.send_and_wait_events_combined(
# event_in_list=[('/gripper_state_monitor/event_in','e_trigger')],
# event_out_list=[('/gripper_state_monitor/event_out','e_gripper_closed', True)],
# timeout_duration=5),
# transitions={'success':'MOVE_ARM_TO_HOLD',
# 'timeout':'CLOSE_GRIPPER',
# 'failure':'CLOSE_GRIPPER'})
# move arm to HOLD position
smach.StateMachine.add('MOVE_ARM_TO_HOLD',
gms.move_arm("look_at_turntable"),
transitions={
'succeeded': 'SET_ACTION_LIB_SUCCESS',
'failed': 'MOVE_ARM_TO_HOLD'
})
# optocoupler check if robot has a object in the gripper, if not then pick failed
# smach.StateMachine.add('VERIFY_OBJECT_GRASPED_OPTOCOUPLER', gbs.send_and_wait_events_combined(
# event_in_list=[('/optocoupler_gripper_status/event_in','e_trigger')],
# event_out_list=[('/optocoupler_gripper_status/event_out','e_grasped', True)],
# timeout_duration=5),
# transitions={'success':'SET_ACTION_LIB_SUCCESS',
# 'timeout':'SET_ACTION_LIB_FAILURE',
# 'failure':'SET_ACTION_LIB_FAILURE'})
# set action lib result
smach.StateMachine.add('SET_ACTION_LIB_SUCCESS',
SetActionLibResult(True),
transitions={'succeeded': 'OVERALL_SUCCESS'})
# set action lib result
smach.StateMachine.add('SET_ACTION_LIB_FAILURE',
SetActionLibResult(False),
transitions={'succeeded': 'OVERALL_FAILED'})
# smach viewer
sis = smach_ros.IntrospectionServer('pick_object_smach_viewer', sm,
'/PICK_OBJECT_SMACH_VIEWER')
sis.start()
# Construct action server wrapper
asw = ActionServerWrapper(server_name='pick_object_server',
action_spec=PickObjectAction,
wrapped_container=sm,
succeeded_outcomes=['OVERALL_SUCCESS'],
aborted_outcomes=['OVERALL_FAILED'],
preempted_outcomes=['PREEMPTED'],
goal_key='pick_object_goal',
feedback_key='pick_object_feedback',
result_key='pick_object_result')
# Run the server in a background thread
asw.run_server()
rospy.spin()
