def configure(self):
sm0 = smach.StateMachine(outcomes=['succeeded','aborted','preempted'], input_keys = ['action_feedback'], output_keys = ['action_feedback'])
sm0.register_transition_cb(self.test_cb)
sis = smach_ros.IntrospectionServer('simple_pnp', sm0, '/simple_pnp_sm')
sis.start()
with sm0:
smach.StateMachine.add('MoveStart', smach_ros.SimpleActionState('MovePTP', MoveLinAction, self.MoveStart_goal), {
"succeeded":"MoveBaseHome",
})
smach.StateMachine.add('MoveBaseHome', smach_ros.SimpleActionState('/move_base', MoveBaseAction, self.MoveBaseHome_goal), {
"succeeded":"MoveBaseShelf",
})
smach.StateMachine.add('MoveBaseShelf', smach_ros.SimpleActionState('/move_base', MoveBaseAction, self.MoveBaseShelf_goal), {
"succeeded":"DetectMarker",
})
smach.StateMachine.add('DetectMarker', smach_ros.SimpleActionState('/cob_marker/object_detection', DetectObjectsAction, self.cob_marker_goal), {
"succeeded":"MovePTP",
})
smach.StateMachine.add('MovePTP', smach_ros.SimpleActionState('MovePTP', MoveLinAction, self.MovePTP_goal), {
"succeeded":"succeeded",
})
# Execute
outcome = sm0.execute()
# protected region configureCode on begin #
# protected region configureCode end #
pass
def define_for(self, robot):
with self:
def look_for_ground_fires_callback(userdata, default_goal):
goal = mbzirc_comm_objs.msg.LookForGroundFiresGoal(
path=userdata.path)
return goal
smach.StateMachine.add(
'LOOK_FOR_GROUND_FIRES',
smach_ros.SimpleActionState(
robot.url + 'look_for_ground_fires_action',
mbzirc_comm_objs.msg.LookForGroundFiresAction,
input_keys=['path'],
goal_cb=look_for_ground_fires_callback),
transitions={'succeeded': 'EXTINGUISH'})
def extinguish_ground_fire_callback(userdata, default_goal):
goal = mbzirc_comm_objs.msg.ExtinguishGroundFireGoal(
color=userdata.color)
return goal
smach.StateMachine.add(
'EXTINGUISH',
smach_ros.SimpleActionState(
robot.url + 'extinguish_ground_fire_action',
mbzirc_comm_objs.msg.ExtinguishGroundFireAction,
input_keys=['color'],
goal_cb=extinguish_ground_fire_callback),
transitions={'succeeded': 'succeeded'})
return self
def main():
rospy.init_node('smach_example_actionlib')
# Start an action server
server = TestServer('test_action')
# Create a SMACH state machine
sm0 = smach.StateMachine(outcomes=['succeeded','aborted','preempted'])
# Open the container
with sm0:
# Add states to the container
# Add a simple action state. This will use an empty, default goal
# As seen in TestServer above, an empty goal will always return with
# GoalStatus.SUCCEEDED, causing this simple action state to return
# the outcome 'succeeded'
smach.StateMachine.add('GOAL_DEFAULT',
smach_ros.SimpleActionState('test_action', TestAction),
{'succeeded':'GOAL_STATIC'})
# Add another simple action state. This will give a goal
# that should abort the action state when it is received, so we
# map 'aborted' for this state onto 'succeeded' for the state machine
smach.StateMachine.add('GOAL_STATIC',
smach_ros.SimpleActionState('test_action', TestAction,
goal = TestGoal(goal=1)),
{'aborted':'GOAL_CB'})
# Add another simple action state. This will give a goal
# that should abort the action state when it is received, so we
# map 'aborted' for this state onto 'succeeded' for the state machine
def goal_callback(userdata, default_goal):
goal = TestGoal()
goal.goal = 2
return goal
smach.StateMachine.add('GOAL_CB',
smach_ros.SimpleActionState('test_action', TestAction,
goal_cb = goal_callback),
{'aborted':'succeeded'})
# For more examples on how to set goals and process results, see
# executive_smach/smach_ros/tests/smach_actionlib.py
# SMACH VIEWER
# create and start the introspection server
sis = smach_ros.IntrospectionServer('test_action', sm0, '/SM_ROOT')
sis.start()
# Execute SMACH plan
outcome = sm0.execute()
rospy.signal_shutdown('All done.')
def main():
rospy.init_node("smach_example_actionlib")
# Start an action server
server = TestServer("test_action")
# Create a SMACH state machine
sm0 = smach.StateMachine(outcomes=["succeeded", "aborted", "preempted"])
# Open the container
with sm0:
# Add states to the container
# Add a simple action state. This will use an empty, default goal
# As seen in TestServer above, an empty goal will always return with
# GoalStatus.SUCCEEDED, causing this simple action state to return
# the outcome 'succeeded'
smach.StateMachine.add(
"GOAL_DEFAULT",
smach_ros.SimpleActionState("test_action", TestAction),
{"succeeded": "GOAL_STATIC"},
)
# Add another simple action state. This will give a goal
# that should abort the action state when it is received, so we
# map 'aborted' for this state onto 'succeeded' for the state machine.
smach.StateMachine.add(
"GOAL_STATIC",
smach_ros.SimpleActionState(
"test_action", TestAction, goal=TestGoal(goal=1)
),
{"aborted": "GOAL_CB"},
)
# Add another simple action state. This will give a goal
# that should abort the action state when it is received, so we
# map 'aborted' for this state onto 'succeeded' for the state machine.
def goal_callback(userdata, default_goal):
goal = TestGoal()
goal.goal = 2
return goal
smach.StateMachine.add(
"GOAL_CB",
smach_ros.SimpleActionState(
"test_action", TestAction, goal_cb=goal_callback
),
{"aborted": "succeeded"},
)
# For more examples on how to set goals and process results, see
# executive_smach/smach_ros/tests/smach_actionlib.py
# Execute SMACH plan
outcome = sm0.execute()
rospy.signal_shutdown("All done.")
def main():
rospy.init_node('smach_example_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 = 0.5
goal1.target_pose.pose.orientation.w = 1.0
smach.StateMachine.add('MOVE1',
smach_ros.SimpleActionState(
'/dtw_robot1/move_base',
MoveBaseAction,
goal=goal1),
transitions={
'succeeded': 'TASK2',
'preempted': 'done',
'aborted': 'done'
})
task2_concurrence = smach.Concurrence(
outcomes=['done', 'exit'],
default_outcome='done',
child_termination_cb=child_term_cb,
outcome_cb=out_cb)
with task2_concurrence:
goal2 = MoveBaseGoal()
goal2.target_pose.header.frame_id = "dtw_robot1/map"
goal2.target_pose.pose.position.x = -0.5
goal2.target_pose.pose.orientation.w = 1.0
smach.Concurrence.add(
'MOVE2',
smach_ros.SimpleActionState('/dtw_robot1/move_base',
MoveBaseAction,
goal=goal2))
smach.Concurrence.add(
'MONITOR2',
smach_ros.MonitorState("/sm_stop", Empty, monitor_cb))
smach.StateMachine.add('TASK2',
task2_concurrence,
transitions={
'done': 'done',
'exit': 'exit'
})
# Execute SMACH plan
sis = smach_ros.IntrospectionServer('smach_server', sm_top, '/SM_ROOT')
sis.start()
outcome = sm_top.execute()
rospy.spin()
sis.stop()
def get_smach_sm():
sm = smach.StateMachine(outcomes=['succeeded', 'aborted', 'preempted'])
sm.userdata.counter = 0
sm.userdata.cardboard_1 = (None, None)
sm.userdata.cardboard_2 = (None, None)
with sm:
smach.StateMachine.add(
'UNDOCK',
smach_ros.SimpleActionState('undock',
UndockAction,
goal=UndockGoal(rotate_in_place=True)),
{
'aborted': 'GO_TO_CARDBOARD_1'
}) # Why does this action always abort when it works every time???
smach.StateMachine.add(
'GO_TO_CARDBOARD_1',
smach_ros.SimpleActionState('move_base',
MoveBaseAction,
goal=tag_1_goal), {
'succeeded': 'TAKE_IMAGE_1',
'aborted': 'GO_TO_DOCK'
})
smach.StateMachine.add('TAKE_IMAGE_1',
smach.CBState(cardboard_imaging_cb),
transitions={'succeeded': 'GO_TO_CARDBOARD_2'},
remapping={'classification': 'cardboard_1'})
smach.StateMachine.add(
'GO_TO_CARDBOARD_2',
smach_ros.SimpleActionState('move_base',
MoveBaseAction,
goal=tag_2_goal), {
'succeeded': 'TAKE_IMAGE_2',
'aborted': 'GO_TO_DOCK'
})
smach.StateMachine.add('TAKE_IMAGE_2',
smach.CBState(cardboard_imaging_cb),
transitions={'succeeded': 'GO_TO_DOCK'},
remapping={'classification': 'cardboard_2'})
smach.StateMachine.add(
'GO_TO_DOCK',
smach_ros.SimpleActionState('move_base',
MoveBaseAction,
goal=dock_goal), {'succeeded': 'DOCK'})
smach.StateMachine.add('DOCK',
smach_ros.SimpleActionState('dock', DockAction),
{'succeeded': 'succeeded'})
return sm
def main():
rospy.init_node('mbf_state_machine')
mbf_sm = smach.StateMachine(outcomes=['preempted', 'succeeded', 'aborted'])
mbf_sm.userdata.recovery_flag = False
with mbf_sm:
smach.StateMachine.add('WAIT_FOR_GOAL',
WaitForGoal(),
transitions={
'succeeded': 'GET_PATH',
'preempted': 'preempted'
})
smach.StateMachine.add('GET_PATH',
smach_ros.SimpleActionState(
'move_base_flex/get_path',
GetPathAction,
goal_cb=get_path_goal_cb,
result_cb=get_path_result_cb),
transitions={
'success': 'EXE_PATH',
'failure': 'WAIT_FOR_GOAL'
})
smach.StateMachine.add('EXE_PATH',
smach_ros.SimpleActionState(
'move_base_flex/exe_path',
ExePathAction,
goal_cb=ex_path_goal_cb,
result_cb=ex_path_result_cb),
transitions={
'success': 'WAIT_FOR_GOAL',
'failure': 'RECOVERY'
})
smach.StateMachine.add('RECOVERY',
smach_ros.SimpleActionState(
'move_base_flex/recovery',
RecoveryAction,
goal_cb=recovery_goal_cb,
result_cb=recovery_result_cb),
transitions={
'success': 'GET_PATH',
'failure': 'WAIT_FOR_GOAL'
})
sis = smach_ros.IntrospectionServer('mbf_state_machine_server', mbf_sm,
'/SM_ROOT')
sis.start()
outcome = mbf_sm.execute()
rospy.spin()
sis.stop()
def construct_state_machine(self):
sm = smach.StateMachine(outcomes=['succeeded', 'aborted', 'preempted'])
sm.userdata.sm_drive_direction = 'none'
sm.userdata.sub_run = 0
# Create state machine
with sm:
smach.StateMachine.add(
'CALIBRATE',
Calibrate(),
transitions={'succeeded': 'DRIVE'},
remapping={'direction_out': 'sm_drive_direction'})
smach.StateMachine.add(
'DRIVE',
smach_ros.SimpleActionState(
'move_base',
MoveBaseAction,
goal_cb=self.drive_goal_cb,
result_cb=self.drive_result_cb,
input_keys=['direction_in', 'sub_run'],
outcomes=['succeeded_dig', 'succeeded_dump']),
transitions={
'succeeded_dig': 'DIG',
'succeeded_dump': 'DUMP'
},
remapping={'direction_in': 'sm_drive_direction'})
smach.StateMachine.add(
'DIG',
smach_ros.SimpleActionState('dig',
TestAction,
goal=TestGoal(0),
result_cb=self.dig_result_cb,
outcomes=['succeeded'],
output_keys=['direction_out']),
transitions={'succeeded': 'DRIVE'},
remapping={'direction_out': 'sm_drive_direction'})
smach.StateMachine.add(
'DUMP',
smach_ros.SimpleActionState(
'dump',
TestAction,
goal=TestGoal(1),
result_cb=self.dump_result_cb,
input_keys=['sub_run'],
output_keys=['sub_run', 'direction_out'],
outcomes=['succeeded']),
transitions={'succeeded': 'DRIVE'},
remapping={'direction_out': 'sm_drive_direction'})
return sm
def __init__(self):
if self._recovery_behaviors is None:
raise ValueError(
"you have to set up planners first by calling MBFStateMachine.set_recovery_behaviors([...])."
)
smach.StateMachine.__init__(self, outcomes=['preempted', 'aborted'])
self.userdata.recovery_behavior_index = 0 # start with first recovery behavior
with self:
smach.StateMachine.add('WAIT_FOR_GOAL',
WaitForGoal(),
transitions={
'succeeded': 'GlobalPlanner',
'preempted': 'preempted'
})
smach.StateMachine.add('RECOVERY',
smach_ros.SimpleActionState(
'move_base_flex/recovery',
RecoveryAction,
goal_cb=self.recovery_goal_cb,
result_cb=self.recovery_result_cb),
transitions={
'succeeded': 'GlobalPlanner',
'failure': 'WAIT_FOR_GOAL'
})
# The first get-path state to get the initial path
state = smach_ros.SimpleActionState(
'move_base_flex/get_path',
GetPathAction,
goal_cb=self.get_path_goal_cb,
result_cb=self.get_path_result_cb)
smach.StateMachine.add('GlobalPlanner',
state,
transitions={
'succeeded': 'EXECUTION',
'failure': 'WAIT_FOR_GOAL',
'preempted': 'preempted'
})
# Here the "magic" happens. The initial path will be executed while the SBPLLatticePlanner plans a better path.
exec_while_replan_sm = ExecWhileReplanStateMachine()
smach.StateMachine.add('EXECUTION',
exec_while_replan_sm,
transitions={
'failure': 'RECOVERY',
'succeeded': 'WAIT_FOR_GOAL'
})
def build():
behaviour = smach.StateMachine(
outcomes=['success', 'preempted', 'aborted'])
with behaviour:
smach.StateMachine.add('GET_NEXT',
get_next_point.getNextPosition(),
transitions={'succeeded': 'GO_TO_POINT'})
smach.StateMachine.add('GO_TO_POINT',
smach_ros.SimpleActionState(
'/positionActionServer',
positionAction,
goal_slots=['x', 'y']),
transitions={
'succeeded': 'MEASURE',
'preempted': 'preempted',
'aborted': 'aborted'
},
remapping={
'x': 'next_x',
'y': 'next_y'
})
smach.StateMachine.add('MEASURE',
wait_state.WaitState(3),
transitions={
'succeeded': 'GET_NEXT',
'preempted': 'preempted'
})
return behaviour
def create_arm_trajectory_state(trajectory,
timeout=rospy.Duration.from_sec(10.0)):
goal = TrajectoryGoal(trajectory)
return smach_ros.SimpleActionState('/aero/jaco/arm_trajectory',
TrajectoryAction,
goal,
exec_timeout=timeout)
def create_skill_sm(skillName):
### Create the sub SMACH state machine for the Skill ###
sm_skill = smach.StateMachine(
outcomes=['succeeded', 'aborted', 'preempted'])
with sm_skill:
smach.StateMachine.add(str(skillName) + '_SETUP',
SkillSetup(),
transitions={
'succeeded': str(skillName) + '_EXECUTION',
'aborted': 'aborted'
})
smach.StateMachine.add(
str(skillName) + '_EXECUTION',
smach_ros.SimpleActionState(
'GenericSkill',
task_manager_msgs.msg.GenericSkillAction,
goal_slots=['exampleSkillProperty0', 'exampleSkillProperty1'],
result_slots=['percentage', 'skillStatus']),
transitions={'succeeded': str(skillName) + '_ANALYSIS'})
smach.StateMachine.add(str(skillName) + '_ANALYSIS',
SkillAnalysis(),
transitions={
'succeeded': 'succeeded',
'aborted': 'aborted'
})
return sm_skill
def __allocMoveConcurrency__(self, slave_namespaces):
map_abort = dict()
for name in slave_namespaces:
map_abort[name + "/Move"] = 'aborted'
map_abort[name + "/Move"] = 'preempted'
map_succed = dict()
for name in slave_namespaces:
map_succed[name + "/Move"] = 'succeeded'
sm_con = smach.Concurrence(outcomes=['success', 'error_occured'],
input_keys=['slaves'],
output_keys=['last_name'],
default_outcome='error_occured',
outcome_map={
'success': map_succed,
'error_occured': map_abort
})
with sm_con:
for name in slave_namespaces:
smach.Concurrence.add(
name + "/Move",
smach_ros.SimpleActionState(name + '/move_base',
MoveBaseAction,
goal_cb=partial(
self.__calcGoal__, name),
input_keys=['slaves'],
output_keys=['last_name']))
return sm_con
def define_for(self, robot):
with self:
def take_off_goal_callback(userdata, default_goal):
robot.set_home(
) # TODO: better do it explicitly BEFORE take off?
goal = mbzirc_comm_objs.msg.TakeOffGoal(height=userdata.height)
return goal
smach.StateMachine.add('TAKE_OFF',
smach_ros.SimpleActionState(
robot.url + 'take_off_action',
mbzirc_comm_objs.msg.TakeOffAction,
input_keys=['height'],
goal_cb=take_off_goal_callback),
transitions={
'succeeded': 'succeeded',
'aborted': 'SLEEP_AND_RETRY'
})
smach.StateMachine.add('SLEEP_AND_RETRY',
SleepAndRetry(3.0, 5),
transitions={
'succeeded': 'TAKE_OFF',
'aborted': 'aborted'
})
return self
def __init__(self):
super(FoldArm,
self).__init__(outcomes=['succeeded', 'preempted', 'aborted'],
default_outcome='succeeded',
outcome_map={
'succeeded': {
'CLOSE_GRIPPER': 'succeeded',
'GOTO_RESTING': 'succeeded'
},
'preempted': {
'CLOSE_GRIPPER': 'preempted',
'GOTO_RESTING': 'preempted'
},
'aborted': {
'CLOSE_GRIPPER': 'aborted',
'GOTO_RESTING': 'aborted'
}
})
with self:
smach.Concurrence.add(
'CLOSE_GRIPPER',
smach_ros.SimpleActionState(
'gripper_controller/gripper_action',
control_msgs.GripperCommandAction,
goal=control_msgs.GripperCommandGoal(
control_msgs.GripperCommand(0.025, 0.0))))
smach.Concurrence.add('GOTO_RESTING', StoredConfig('resting'))
def __init__(self):
if self._recovery_behaviors is None:
raise ValueError(
"you have to set up planners first by calling MBFStateMachine.set_recovery_behaviors([...])."
)
smach.StateMachine.__init__(self, outcomes=['preempted', 'aborted'])
self.userdata.recovery_behavior_index = 0 # start with first recovery behavior
with self:
smach.StateMachine.add('WAIT_FOR_GOAL',
WaitForGoal(),
transitions={
'succeeded': 'PLAN_EXEC',
'preempted': 'preempted'
})
smach.StateMachine.add('RECOVERY',
smach_ros.SimpleActionState(
'move_base_flex/recovery',
RecoveryAction,
goal_cb=self.recovery_goal_cb,
result_cb=self.recovery_result_cb),
transitions={
'succeeded': 'PLAN_EXEC',
'failure': 'WAIT_FOR_GOAL'
})
plan_exec_sm = PlanExecStateMachine()
smach.StateMachine.add('PLAN_EXEC',
plan_exec_sm,
transitions={
'failure': 'RECOVERY',
'succeeded': 'WAIT_FOR_GOAL'
})
def build(point_list):
behaviour = smach.StateMachine(
outcomes=['success', 'preempted', 'aborted'])
with behaviour:
smach.StateMachine.add('GET_NEXT',
get_next_line.getNextLine(point_list),
transitions={
'succeeded': 'GO_TO_POINT',
'aborted': 'aborted'
})
smach.StateMachine.add('GO_TO_POINT',
smach_ros.SimpleActionState(
'/fmExecutors/lineActionServer',
lineAction,
goal_slots=['a_x', 'a_y', 'b_x', 'b_y']),
transitions={
'succeeded': 'GET_NEXT',
'preempted': 'preempted',
'aborted': 'aborted'
},
remapping={
'a_x': 'next_ax',
'a_y': 'next_ay',
'b_x': 'next_bx',
'b_y': 'next_by'
})
return behaviour
def __init__(self):
smach.StateMachine.__init__(
self,
outcomes=['succeeded', 'preempted', 'aborted', 'failed'],
input_keys=['path'],
output_keys=[
'outcome', 'message', 'final_pose', 'dist_to_goal',
'angle_to_goal'
])
with self:
self.userdata.recovery_behavior = None
smach.StateMachine.add(
'EXE_PATH',
smach_ros.SimpleActionState(
'move_base_flex/exe_path',
ExePathAction,
goal_cb=MoveToGoal.controller_goal_cb,
result_cb=MoveToGoal.controller_result_cb),
transitions={
'succeeded': 'succeeded',
'preempted': 'preempted',
'aborted': 'aborted',
'failed': 'failed',
})
def __init__(self):
smach.StateMachine.__init__(
self,
outcomes=['succeeded', 'preempted', 'aborted'],
input_keys=['target_pose'],
output_keys=['outcome', 'message', 'path', 'cost', 'recovery_behavior']
)
with self:
self.userdata.recovery_behavior = None
smach.StateMachine.add(
'GET_PATH',
smach_ros.SimpleActionState(
'move_base_flex/get_path',
GetPathAction,
goal_cb=Planning.planner_goal_cb,
result_cb=Planning.planner_result_cb),
transitions={
'succeeded': 'succeeded',
'preempted': 'preempted',
'aborted': 'aborted'
}
)
def __init__(self):
outcome_map = {
'succeeded': {},
'failure': {},
'preempted': {},
'aborted': {},
}
for i in range(NUM_POSES):
i = str(i)
outcome_map['succeeded'][i] = 'succeeded'
outcome_map['failure'][i] = 'failure'
outcome_map['preempted'][i] = 'preempted'
outcome_map['aborted'][i] = 'aborted'
smach.Concurrence.__init__(
self,
outcomes=['failure', 'succeeded', 'preempted', 'aborted'],
default_outcome='succeeded',
outcome_map=outcome_map,
input_keys=['poses'],
output_keys=['target_pose'],
child_termination_cb=self.
get_best_target_pose_sm_child_termination_cb,
outcome_cb=self.get_best_target_pose_sm_outcome_cb)
with self:
for i in range(NUM_POSES):
state = smach_ros.SimpleActionState(
'move_base_flex/get_path',
GetPathAction,
goal_cb=self.get_path_goal_cb(i),
result_cb=self.get_path_result_cb(i))
smach.Concurrence.add(str(i), state)
def build():
behaviour = smach.StateMachine(
outcomes=['success', 'preempted', 'aborted'],
output_keys=['next_x', 'next_y'])
# Next go to point
behaviour.userdata.next_x = 0
behaviour.userdata.next_y = 0
with behaviour:
smach.StateMachine.add('GO_TO_POINT',
smach_ros.SimpleActionState(
'/fmExecutors/position_planner',
positionAction,
goal_slots=['x', 'y']),
transitions={
'succeeded': 'GET_NEXT',
'preempted': 'preempted',
'aborted': 'aborted'
},
remapping={
'x': 'next_x',
'y': 'next_y'
})
smach.StateMachine.add('GET_NEXT',
get_next_point.getNextPosition(),
transitions={'succeeded': 'GO_TO_POINT'})
return behaviour
def __init__(self):
if self._recovery_behaviors is None:
raise ValueError(
"you have to set up planners first by calling MBFStateMachine.set_recovery_behaviors([...])."
)
smach.StateMachine.__init__(self, outcomes=['preempted', 'aborted'])
self.userdata.recovery_behavior_index = 0 # start with first recovery behavior
with self: # Just simple plug and play with the states.
smach.StateMachine.add(
'WAIT_FOR_GOAL',
WaitForGoal(),
transitions={
#'succeeded': 'GET_POSES',
'received_goal': 'GET_POSES',
'preempted': 'preempted'
})
# This state takes a target_pose, lookup the room. If a room is found, it will put an array
# of poses into the userdata
smach.StateMachine.add('GET_POSES',
GetPoses(),
transitions={
'succeeded': 'GET_BEST_TARGET_POSE',
'room_not_found': 'PLAN_EXEC',
'failure': 'WAIT_FOR_GOAL',
'preempted': 'preempted'
})
# Takes an array of poses and planns with the GlobalPlanner parallel to every target_pose
# Sets the target_pose in the userdata as the best target pose
smach.StateMachine.add('GET_BEST_TARGET_POSE',
GetBestTargetPose(),
transitions={
'succeeded': 'PLAN_EXEC',
'failure': 'WAIT_FOR_GOAL',
'preempted': 'preempted'
})
smach.StateMachine.add('RECOVERY',
smach_ros.SimpleActionState(
'move_base_flex/recovery',
RecoveryAction,
goal_cb=self.recovery_goal_cb,
result_cb=self.recovery_result_cb),
transitions={
'succeeded': 'PLAN_EXEC',
'failure': 'WAIT_FOR_GOAL'
})
plan_exec_sm = PlanExecStateMachine()
smach.StateMachine.add('PLAN_EXEC',
plan_exec_sm,
transitions={
'failure': 'RECOVERY',
'succeeded': 'WAIT_FOR_GOAL',
'invalid': 'WAIT_FOR_GOAL'
})
def __init__(self):
button_topic = rospy.get_param('~button_topic', '/drone/joy/buttons[4]')
button_topic_class, button_real_topic, self.button_eval_func = rostopic.get_topic_class(rospy.resolve_name(button_topic))
self.sub_button = rospy.Subscriber(button_real_topic, button_topic_class, self.button_cb)
self.last_button = None
self.button_pressed = False
self.button_released = False
self.land_action_ns = rospy.get_param('~land_action_ns', '/drone/land_action')
robot_odom_topic = rospy.get_param('~robot_odom_topic', '/drone/odom')
self.sub_odom = rospy.Subscriber(robot_odom_topic, Odometry, self.robot_odom_cb)
self.robot_current_pose = PoseStamped()
self.state_name_topic = rospy.get_param('~state_name_topic', '~state')
self.pub_state_name = rospy.Publisher(self.state_name_topic, String, queue_size = 10, latch = True)
robot_state_topic = rospy.get_param('~robot_state_topic', '/drone/flight_state')
self.last_known_flight_state = FlightState.Landed
self.sub_flight_state = rospy.Subscriber(robot_state_topic, FlightState, self.flight_state_cb)
self.flight_state_event = threading.Event()
landing_spot = rospy.get_param('landing_spot', {'x': float('nan'), 'y': float('nan'), 'z': float('nan'), 'tolerance': float('nan')})
self.landing_spot, self.landing_tolerance = self.get_landing_spot(**landing_spot)
if math.isnan(self.landing_spot.Norm()):
rospy.logwarn('Landing spot is not specified, allow landing anywhere')
self.sm = smach.StateMachine(outcomes = ['FINISH'])
with self.sm:
# smach.StateMachine.add('WAIT_USER',
# smach.CBState(self.check_button, cb_kwargs = {'context': self}),
# transitions = {'pressed': 'FOLLOW_POINTING',
# 'preempted': 'FINISH'})
smach.StateMachine.add('WAIT_USER',
smach.CBState(self.wait_for_flying, cb_kwargs = {'context': self}),
transitions = {'succeeded': 'FOLLOW_POINTING',
'aborted': 'FINISH',
'preempted': 'WAIT_USER'})
smach.StateMachine.add('FOLLOW_POINTING',
smach.CBState(self.wait_for_landing, cb_kwargs = {'context': self}),
transitions = {'succeeded': 'LAND',
'aborted': 'FOLLOW_POINTING',
'preempted': 'FINISH'})
smach.StateMachine.add('LAND',
smach_ros.SimpleActionState(self.land_action_ns, EmptyAction),
transitions = {'succeeded': 'WAIT_USER',
'preempted': 'FINISH',
'aborted': 'WAIT_USER'})
self.sm.register_start_cb(self.state_transition_cb, cb_args = [self.sm])
self.sm.register_transition_cb(self.state_transition_cb, cb_args = [self.sm])
self.sis = smach_ros.IntrospectionServer('smach_server', self.sm, '/SM_JOY')
def build():
behaviour = smach.StateMachine(outcomes=['inspectionDone','preempted','aborted'], input_keys=['next_x', 'next_y'])
# Wrigle goals
wriggle_left_goal = make_turnGoal(amount=1 , vel=0.3, forward_vel=0)
wriggle_right_goal = make_turnGoal(amount=-2, vel=0.3, forward_vel=0)
wriggle_center_goal = make_turnGoal(amount=1, vel=0.3, forward_vel=0)
with behaviour:
smach.StateMachine.add('get_next_search_point', get_step_towards_point.getStepTowardsPoint(),transitions={'succeeded':'wriggle_left'})
smach.StateMachine.add('wriggle_left', smach_ros.SimpleActionState('/fmExecutors/make_turn', make_turnAction, wriggle_left_goal), transitions={'succeeded':'wriggle_right','preempted':'preempted','aborted':'aborted'})
smach.StateMachine.add('wriggle_right', smach_ros.SimpleActionState('/fmExecutors/make_turn', make_turnAction, wriggle_right_goal), transitions={'succeeded':'wriggle_center','preempted':'preempted','aborted':'aborted'})
smach.StateMachine.add('wriggle_center', smach_ros.SimpleActionState('/fmExecutors/make_turn', make_turnAction, wriggle_center_goal), transitions={'succeeded':'go_to_next_search_point','preempted':'preempted','aborted':'aborted'})
smach.StateMachine.add('go_to_next_search_point', smach_ros.SimpleActionState('/fmExecutors/position_planner',positionAction, goal_slots=['x','y']),
transitions={'succeeded':'check_no_mines','preempted':'preempted','aborted':'aborted'},
remapping={'x':'step_next_x','y':'step_next_y'})
smach.StateMachine.add('check_no_mines', smach_ros.MonitorState("/wads", Float64, mine_detect_cb, 1), transitions={'valid':'inspectionDone', 'invalid':'get_next_search_point', 'preempted':'preempted'})
return behaviour
def get_ell_move_height(height, duration, gripper_rot):
goal = EllipsoidMoveGoal()
goal.change_height = height
goal.absolute_height = True
goal.gripper_rot = gripper_rot
goal.duration = duration
return smach_ros.SimpleActionState('ellipsoid_move',
EllipsoidMoveAction,
goal=goal)
def main():
rospy.init_node('smach_example_actionlib')
# Start an action server
server = TestServer('test_action')
sm0 = smach.StateMachine(outcomes=['succeeded', 'aborted', 'preempted'])
with sm0:
smach.StateMachine.add('GOAL_DEFAULT',
smach_ros.SimpleActionState(
'test_action', TestAction),
transitions={'succeeded': 'GOAL_STATIC'})
# Add another simple action state. This will give a goal
# that should abort the action state when it is received, so we
# map 'aborted' for this state onto 'succeeded' for the state machine.
smach.StateMachine.add('GOAL_STATIC',
smach_ros.SimpleActionState(
'test_action',
TestAction,
goal=TestGoal(goal=1)),
transitions={'aborted': 'GOAL_CB'})
# Add another simple action state. This will give a goal
# that should abort the action state when it is received, so we
# map 'aborted' for this state onto 'succeeded' for the state machine.
def goal_callback(userdata, default_goal):
goal = TestGoal()
goal.goal = 2
return goal
smach.StateMachine.add('GOAL_CB',
smach_ros.SimpleActionState(
'test_action',
TestAction,
goal_cb=goal_callback),
transitions={'aborted': 'succeeded'})
outcome = sm0.execute()
rospy.signal_shutdown('All done.')
def get_ell_move_local(delta_lat, delta_lon, delta_hei, gripper_rot):
goal = EllipsoidMoveGoal()
goal.change_latitude = delta_lat
goal.change_longitude = delta_lon
goal.change_height = delta_hei
goal.gripper_rot = gripper_rot
goal.duration = LOCAL_DURATION
return smach_ros.SimpleActionState('ellipsoid_move',
EllipsoidMoveAction,
goal=goal)
def PlaceObject(attempts=3):
""" Place a given object, retrying up to a given number of times """
it = smach.Iterator(
outcomes=['succeeded', 'preempted', 'aborted'],
input_keys=['object_name', 'support_surf', 'place_pose'],
output_keys=[],
it=lambda: range(0, attempts),
it_label='attempt',
exhausted_outcome='aborted')
with it:
sm = smach.StateMachine(
outcomes=['succeeded', 'preempted', 'aborted', 'continue'],
input_keys=['object_name', 'support_surf', 'place_pose'],
output_keys=[])
with sm:
smach.StateMachine.add(
'PlaceObject',
smach_ros.SimpleActionState(
'place_object',
thorp_msgs.PlaceObjectAction,
goal_slots=['object_name', 'support_surf', 'place_pose'],
result_slots=[]),
remapping={
'object_name': 'object_name',
'support_surf': 'support_surf',
'place_pose': 'place_pose'
},
transitions={
'succeeded': 'succeeded',
'preempted': 'preempted',
'aborted': 'ClearOctomap'
})
smach.StateMachine.add('ClearOctomap',
smach_ros.ServiceState(
'clear_octomap', std_srvs.Empty),
transitions={
'succeeded': 'continue',
'preempted': 'preempted',
'aborted': 'aborted'
})
# TODOs:
# - we should open the gripper, in case we have picked an object
# - check error and, if collision between parts of the arm, move a bit the arm --> not enough info
# - this doesn't make too much sense as a loop... better try all our tricks and exit
# - can I reuse the same for place and MoveToTarget???
smach.Iterator.set_contained_state('', sm, loop_outcomes=['continue'])
return it
def get_ell_move_spec_height(duration, gripper_rot):
def goal_cb(userdata, goal_in):
goal = EllipsoidMoveGoal()
goal.change_height = userdata.goal_pose[0][2]
goal.absolute_height = True
goal.gripper_rot = gripper_rot
goal.duration = duration
return goal
return smach_ros.SimpleActionState('ellipsoid_move',
EllipsoidMoveAction,
goal_cb=goal_cb,
input_keys=['goal_pose'])
def FoldArm():
''' Concurrently fold arm and close the gripper '''
sm = smach.Concurrence(outcomes = ['succeeded', 'preempted', 'aborted'],
default_outcome = 'succeeded',
outcome_map = {'succeeded':{'CloseGripper':'succeeded',
'FoldArm':'succeeded'},
'preempted':{'CloseGripper':'preempted',
'FoldArm':'preempted'},
'aborted':{'CloseGripper':'aborted',
'FoldArm':'aborted'}})
with sm:
smach.Concurrence.add('CloseGripper',
smach_ros.SimpleActionState('gripper_controller/gripper_action',
control_msgs.GripperCommandAction,
goal=control_msgs.GripperCommandGoal(control_msgs.GripperCommand(0.005, 0.0))))
smach.Concurrence.add('FoldArm',
smach_ros.SimpleActionState('move_to_target',
thorp_msgs.MoveToTargetAction,
goal=thorp_msgs.MoveToTargetGoal(thorp_msgs.MoveToTargetGoal.NAMED_TARGET,
'resting', None, None)))
return sm