def test_group(self):
"""Test adding a bunch of states with group args."""
class DoneState(State):
def __init__(self):
State.__init__(self, outcomes=['done'])
def execute(self, ud=None):
return 'done'
sm = StateMachine(['succeeded', 'done'])
with sm:
StateMachine.add('FAILSAUCE', DoneState())
transitions = {'aborted': 'FAILSAUCE', 'preempted': 'FAILSAUCE'}
with sm:
StateMachine.add(
'FIRST',
SimpleActionState('reference_action', TestAction, goal=g1),
transitions)
StateMachine.add(
'SECOND',
SimpleActionState('reference_action', TestAction, goal=g2),
transitions)
StateMachine.add(
'THIRD',
SimpleActionState('reference_action', TestAction, goal=g1),
transitions)
outcome = sm.execute()
assert outcome == 'done'
def IdentificationSimpleContainer():
sm_identification_simple = StateMachine(['parked', 'aborted', 'preempted'])
with sm_identification_simple:
StateMachine.add('GET_VICTIMS',
SimpleActionState('/navigation/initial_turn',
InitialTurnAction),
transitions={'succeeded': 'GO_TO_VICTIM'})
StateMachine.add('GO_TO_VICTIM',
utils.TargetSelectorContainer('victim'),
transitions={
'target_sent': 'PARK',
'aborted': 'aborted',
'preempted': 'preempted'
})
StateMachine.add('PARK',
SimpleActionState('/navigation/initial_turn',
InitialTurnAction),
transitions={
'succeeded': 'parked',
'aborted': 'aborted',
'preempted': 'preempted'
})
return sm_identification_simple
def __init__(self):
rospy.init_node('move_to_and_inspect_point_sm', anonymous=False)
hsm = StateMachine(outcomes=['finished statemachine'])
with hsm:
StateMachine.add( 'GO_TO_POINT',
SimpleActionState( 'pid_global',
MoveAction,
makeMoveGoal("pid_global_plan", -3.0, 0, -0.5, radius_of_acceptance = 2.0)),
transitions = { "succeeded": 'INSPECT_POINT',
"preempted": 'INSPECT_POINT',
"aborted": 'INSPECT_POINT' })
StateMachine.add( 'INSPECT_POINT',
SimpleActionState( 'inspect_point',
MoveAction,
makeMoveGoal("inspect_point", -3.0, 0.0, -0.5, radius_of_acceptance=2.0)),
transitions = { 'succeeded': 'INSPECT_POINT',
"preempted": 'INSPECT_POINT',
"aborted": 'INSPECT_POINT' })
intro_server = IntrospectionServer(str(rospy.get_name()), hsm,'/SM_ROOT')
intro_server.start()
hsm.execute()
#patrol.execute()
print("State machine execute finished")
intro_server.stop()
def main():
rospy.init_node('tic_crane_state_machine')
sm = smach.StateMachine(outcomes=['succeeded', 'aborted', 'preempted'])
with sm:
deploy_goal = msg.MoveToPositionGoal(position=500)
store_goal = msg.MoveToPositionGoal(position=0)
measure_goal = msg.MeasurementGoal(measurement_duration=7)
sm.add('DEPLOY',
SimpleActionState('move_to_target_pose',
msg.MoveToPositionAction,
goal=deploy_goal),
transitions={'succeeded': 'MEASURE'})
sm.add('MEASURE',
SimpleActionState('measurement',
msg.MeasurementAction,
goal=measure_goal),
transitions={'succeeded': 'STORE'})
sm.add('STORE',
SimpleActionState('move_to_target_pose',
msg.MoveToPositionAction,
goal=store_goal),
transitions={'succeeded': 'succeeded'})
outcome = sm.execute()
def TargetSelectorContainer(target_type):
sm_target_selector = StateMachine(['target_sent', 'aborted', 'preempted'])
with sm_target_selector:
target_selection_goal = targetSelectionGoal(target_type)
StateMachine.add('GET_TARGET',
SimpleActionState('/select_target',
SelectTargetAction,
goal=target_selection_goal,
result_key='target_pose'),
transitions={
'succeeded': 'MOVE_BASE',
'aborted': 'aborted',
'preempted': 'preempted'
},
remapping={'target_pose': 'next_target'})
StateMachine.add('MOVE_BASE',
SimpleActionState('/move_base',
MoveBaseAction,
goal_key='next_target'),
transitions={
'succeeded': 'target_sent',
'aborted': 'GET_TARGET',
'preempted': 'preempted'
})
return sm_target_selector
def __init__(self, input_keys=['tuck_left', 'tuck_right']):
tuck_arms_uri = '/tuck_arms'
SimpleActionState.__init__(self,
tuck_arms_uri,
TuckArmsAction,
goal_cb=self.goal_cb,
input_keys=input_keys)
def __init__(self, text=None, text_cb=None, wait_before_speaking=0, **kwargs):
"""
@param text: the text to speak out aloud (if `text_cb' is None)
@param text_cb: a callback returning the text to speak out
(if `text' is None)
@param wait_before_speaking: how long to wait before speaking;
it may be a number (in seconds) or a rospy.Duration
@param **kwargs: input_keys, output_keys, etc.
"""
if not text and not text_cb:
raise ValueError("Neither `text' nor `text_cb' where set!")
elif text and text_cb:
raise ValueError("You've set both `text' and `text_cb'!")
if not isinstance(wait_before_speaking, rospy.Duration):
wait_before_speaking = rospy.Duration(wait_before_speaking)
def generic_goal_cb(userdata, old_goal):
tts_goal = SoundGoal()
tts_goal.wait_before_speaking = wait_before_speaking
tts_goal.text = text if text else text_cb(userdata)
print "Speaking:", tts_goal.text
return tts_goal
SimpleActionState.__init__(self,
TTS_ACTION_NAME, SoundAction,
goal_cb=generic_goal_cb, **kwargs)
def __init__(self):
SimpleActionState.__init__(self, topics["plan_execution"],
ExecutePlanAction,
goal_cb=self.goal_cb,
result_cb=self.result_cb,
input_keys=['goal'],
output_keys=['result'])
def main():
rospy.init_node("fsm_dummy")
sm = StateMachine(outcomes=['succeeded', 'aborted', 'preempted'])
with sm:
StateMachine.add('INITIAL_TURN',
SimpleActionState('/initial_turn', InitialTurnAction),
transitions={
'succeeded': 'EXPLORATION',
'aborted': 'aborted',
'preempted': 'preempted'
})
StateMachine.add('EXPLORATION',
exploration.ExplorationContainer(),
transitions={
'victim_thermal': 'IDENTIFICATION_SIMPLE',
'victim_camera': 'IDENTIFICATION_TRACKING',
'aborted': 'aborted',
'preempted': 'preempted',
'time_out': 'AGGRESSIVE_EXPLORATION'
})
StateMachine.add('IDENTIFICATION_SIMPLE',
identification.IdentificationSimpleContainer(),
transitions={
'parked': 'succeeded',
'aborted': 'aborted',
'preempted': 'preempted'
})
StateMachine.add('IDENTIFICATION_TRACKING',
identification.IdentificationTrackingContainer(),
transitions={
'identification_finished': 'succeeded',
'aborted': 'aborted',
'preempted': 'preempted'
})
StateMachine.add('AGGRESSIVE_EXPLORATION',
SimpleActionState('/navigation/initial_turn',
InitialTurnAction),
transitions={
'succeeded': 'succeeded',
'aborted': 'aborted',
'preempted': 'preempted'
})
sis = smach_ros.IntrospectionServer('smach_server', sm, '/DUMMY_FSM')
sis.start()
smach_ros.set_preempt_handler(sm)
# Execute SMACH tree in a separate thread so that we can ctrl-c the script
smach_thread = threading.Thread(target=sm.execute)
smach_thread.start()
rospy.spin()
sis.stop()
def __init__(self, frame='/map'):
SimpleActionState.__init__(self,
'move_base',
MoveBaseAction,
input_keys=['x', 'y', 'yaw'],
goal_cb=self.__goal_cb)
self.frame = frame
def __init__(
self,
goal_positon,
travel_speed=0.5,
sphere_of_acceptance=0.5,
action_server="/guidance_interface/los_server",
):
"""A SimpleActionState for traveling to a goal position using LOS guidance.
Args:
goal_positon (geometry_msgs/Point): position drone should travel to.
start_position (geometry_msgs/Point): position drone starts in.
travel_speed (float, optional):
forward velocity drone shoudl travel at. Defaults to 0.5.
sphere_of_acceptance (float, optional):
action returns success when drone is inside this sphere. Defaults to 0.5.
action_server (str, optional):
name of los action server. Defaults to "/guidance_interface/los_server".
"""
goal = LosPathFollowingGoal()
goal.next_waypoint = goal_positon
goal.forward_speed = travel_speed
goal.desired_depth = goal_positon.z
goal.sphereOfAcceptance = sphere_of_acceptance
SimpleActionState.__init__(self, action_server, LosPathFollowingAction,
goal)
def _result_cb(userdata, status, result):
""" This function will analize the status of the response
If the status is aborted, will call the action again with the same
orientation, but with the distance -0,5. If the status now is succeeded,
then call the move_by with x=0.5 in front.
:type status: actionlib.GoalStatus
:type result: MoveBaseResult
"""
if status != GoalStatus.SUCCEEDED:
rospy.loginfo(C.BACKGROUND_YELLOW + "Retrying go to the target goal" + C.NATIVE_COLOR)
new_pose = pose_at_distance(self.nav_goal.target_pose.pose, DISTANCE_TO_RETRY )
self.nav_goal.target_pose.pose = new_pose
self.nav_goal.target_pose.header.stamp = rospy.Time.now()
move_action = SimpleActionState(move_base_action_name, MoveBaseAction, goal=self.nav_goal)
result_status = move_action.execute(userdata)
if result_status == succeeded:
new_goal = MoveBaseGoal()
new_goal.target_pose.header.frame_id = FRAME_BASE_LINK
new_goal.target_pose.pose.position.x = DISTANCE_TO_RETRY
new_goal.target_pose.header.stamp = rospy.Time.now()
move_action = SimpleActionState(MOVE_BY_ACTION_NAME , MoveBaseAction, goal=new_goal)
return move_action.execute(userdata)
def main():
rospy.init_node('smach_usecase_step_04')
# Construct static goals
polygon_big = turtle_actionlib.msg.ShapeGoal(edges = 11, radius = 4.0)
polygon_small = turtle_actionlib.msg.ShapeGoal(edges = 6, radius = 0.5)
# Create a SMACH state machine
sm0 = StateMachine(outcomes=['succeeded','aborted','preempted'])
# Open the container
with sm0:
# Reset turtlesim
StateMachine.add('RESET',
ServiceState('reset', std_srvs.srv.Empty),
{'succeeded':'SPAWN'})
# Create a second turtle
StateMachine.add('SPAWN',
ServiceState('spawn', turtlesim.srv.Spawn,
request = turtlesim.srv.SpawnRequest(0.0,0.0,0.0,'turtle2')),
{'succeeded':'TELEPORT1'})
# Teleport turtle 1
StateMachine.add('TELEPORT1',
ServiceState('turtle1/teleport_absolute', turtlesim.srv.TeleportAbsolute,
request = turtlesim.srv.TeleportAbsoluteRequest(5.0,1.0,0.0)),
{'succeeded':'TELEPORT2'})
# Teleport turtle 2
StateMachine.add('TELEPORT2',
ServiceState('turtle2/teleport_absolute', turtlesim.srv.TeleportAbsolute,
request = turtlesim.srv.TeleportAbsoluteRequest(9.0,5.0,0.0)),
{'succeeded':'BIG'})
# Draw a large polygon with the first turtle
StateMachine.add('BIG',
SimpleActionState('turtle_shape1',turtle_actionlib.msg.ShapeAction,
goal = polygon_big),
{'succeeded':'SMALL'})
# Draw a small polygon with the second turtle
StateMachine.add('SMALL',
SimpleActionState('turtle_shape2',turtle_actionlib.msg.ShapeAction,
goal = polygon_small))
# Attach a SMACH introspection server
sis = IntrospectionServer('smach_usecase_01', sm0, '/USE_CASE')
sis.start()
# Set preempt handler
smach_ros.set_preempt_handler(sm0)
# Execute SMACH tree in a separate thread so that we can ctrl-c the script
smach_thread = threading.Thread(target = sm0.execute)
smach_thread.start()
# Signal handler
rospy.spin()
def __init__(self, target_type):
target_selection_goal = self.targetSelectionGoal(target_type)
SimpleActionState.__init__(self,
select_target_topic,
SelectTargetAction,
goal=target_selection_goal,
result_key='target_pose')
def main():
rospy.init_node('smach_usecase_executive')
sm_root = smach.StateMachine(
outcomes=['succeeded', 'aborted', 'preempted'])
with sm_root:
smach.StateMachine.add('RESET',
ServiceState('reset', std_srvs.srv.Empty),
{'succeeded': 'SPAWN'})
request = turtlesim.srv.SpawnRequest(0.0, 0.0, 0.0, 'turtle2')
smach.StateMachine.add(
'SPAWN', ServiceState('spawn', turtlesim.srv.Spawn, request),
{'succeeded': 'TELEPORT1'})
teleport1 = turtlesim.srv.TeleportAbsoluteRequest(5.0, 1.0, 0.0)
smach.StateMachine.add(
'TELEPORT1',
ServiceState('turtle1/teleport_absolute',
turtlesim.srv.TeleportAbsolute, teleport1),
{'succeeded': 'TELEPORT2'})
teleport2 = turtlesim.srv.TeleportAbsoluteRequest(9.0, 5.0, 0.0)
smach.StateMachine.add(
'TELEPORT2',
ServiceState('turtle2/teleport_absolute',
turtlesim.srv.TeleportAbsolute, teleport2),
{'succeeded': 'SHAPES'})
shapes_concurrence = Concurrence(
outcomes=['succeeded', 'aborted', 'preempted'],
default_outcome='aborted',
outcome_map={
'succeeded': {
'BIG': 'succeeded',
'SMALL': 'succeeded'
}
})
smach.StateMachine.add('SHAPES', shapes_concurrence)
with shapes_concurrence:
Concurrence.add(
'BIG',
SimpleActionState('turtle_shape1', ShapeAction,
ShapeGoal(11, 4.0)), {'succeeded': 'SMALL'})
Concurrence.add(
'SMALL',
SimpleActionState('turtle_shape2', ShapeAction,
ShapeGoal(6, 0.5)))
sis = smach_ros.IntrospectionServer('intro_server', sm_root, '/Intro')
sis.start()
outcome = sm_root.execute()
rospy.spin()
sis.stop()
def __init__(self, vocabulary_list):
if not isinstance(vocabulary_list, list):
rospy.logerr('SetSpeechVocabularyState received something which is not a list')
sys.exit(-1)
elif vocabulary_list == []:
rospy.logwarn('SetSpeechVocabularyState received an empty vocabulary_list')
voc_goal = SetSpeechVocabularyGoal()
voc_goal.words = vocabulary_list
SimpleActionState.__init__(self, '/speech_vocabulary_action', SetSpeechVocabularyAction, goal=voc_goal)
def __init__(self):
SimpleActionState.__init__(
self,
move_kinect_topic,
MoveSensorAction,
goal_cb=self.goal_cb,
outcomes=['succeeded', 'aborted', 'preempted'],
input_keys=['move_end_effector_msg'],
output_keys=['move_end_effector_msg'])
def __init__(self, helper_obj):
StateMachine.__init__(self,
outcomes=['complete', 'preempted', 'aborted'],
input_keys=['mission_data'],
output_keys=['mission_status'])
self.__helper_obj = helper_obj
with self:
# This state will prepare for the mission
StateMachine.add('PREPARE_MISSION',
PrepareMission4(self.__helper_obj),
transitions={
'ready': 'DEFAULT_HEAD_POSITION',
'error': 'complete'
})
# Set up action goal for deafult head position
default_position_pan, default_position_tilt = self.__helper_obj.CameraDefaultPos(
)
head_goal = point_headGoal()
head_goal.absolute = True
head_goal.pan = default_position_pan
head_goal.tilt = default_position_tilt
# Add the default camera position state. Which moves the head to the default position
StateMachine.add('DEFAULT_HEAD_POSITION',
SimpleActionState(
'head_control_node',
point_headAction,
goal=head_goal,
result_cb=self.defaultHeadPositionComplete,
output_keys=['mission_status']),
transitions={
'succeeded': 'MOVE',
'preempted': 'preempted',
'aborted': 'aborted'
})
# This state uses an Action to move the robot to the required goal
StateMachine.add('MOVE',
SimpleActionState('move_base',
MoveBaseAction,
goal_slots=['target_pose'],
result_cb=self.moveComplete,
output_keys=['mission_status']),
transitions={
'succeeded': 'complete',
'preempted': 'preempted',
'aborted': 'aborted'
},
remapping={'target_pose': 'destination'})
def __init__(self, posture_name, speed):
# Assert posture is known
if not posture_name in self.POSTURES:
rospy.logerr('Posture "%s" is an unknown posture! Known postures are: %s. Shutting down node...' % (posture_name, str(self.POSTURES)))
sys.exit(-1)
def body_pose_goal_cb(userdata, goal):
goal = BodyPoseWithSpeedGoal()
goal.posture_name = posture_name
goal.speed = speed
return goal
SimpleActionState.__init__(self, '/body_pose_naoqi', BodyPoseWithSpeedAction, goal_cb=body_pose_goal_cb)
def __init__(self, pose, action_server="/guidance_interface/dp_server"):
"""A SimpleActionState that travels to a goal pose using our DP guidance.
Only use when in close proximity of goal pose.
Args:
pose (geometry_msgs/Pose): Goal pose.
action_server (str, optional):
name of dp action server. Defaults to "/guidance_interface/dp_server".
"""
goal = MoveBaseGoal()
goal.target_pose.pose = pose
SimpleActionState.__init__(self, action_server, MoveBaseAction, goal)
def __init__(self, behavior_name=None):
# Assert behavior is installed
input_keys = []
if not behavior_name:
input_keys = ['behavior_name']
else:
_checkInstalledBehavior(behavior_name)
def behavior_goal_cb(userdata, goal):
goal = RunBehaviorGoal()
goal.behavior = userdata.behavior_name if not behavior_name else behavior_name
return goal
SimpleActionState.__init__(self, '/run_behavior', RunBehaviorAction, goal_cb=behavior_goal_cb, input_keys=input_keys)
def create_action_state(step):
"""Take key value pair of the state of the state and create a SimpleActionState
based on the parameters in the config file"""
action_type = step.keys()[0]
name = step[action_type]
if action_type == "pose":
action_state = SimpleActionState('coordinate_action', CoordinateAction, goal=create_coordinate_goal(name))
elif action_type == "move":
action_state = SimpleActionState('move_base', MoveBaseAction, goal=create_move_goal(name))
else:
raise Exception("action_type pose and move are the only ones implemented")
rospy.logdebug("State with name")
return name.upper(), action_state
def __init__(self, vocabulary_list):
if not isinstance(vocabulary_list, list):
rospy.logerr(
'SetSpeechVocabularyState received something which is not a list'
)
sys.exit(-1)
elif vocabulary_list == []:
rospy.logwarn(
'SetSpeechVocabularyState received an empty vocabulary_list')
voc_goal = SetSpeechVocabularyGoal()
voc_goal.words = vocabulary_list
SimpleActionState.__init__(self,
'/speech_vocabulary_action',
SetSpeechVocabularyAction,
goal=voc_goal)
def __init__(self, object_to_detect_name=None):
smach.StateMachine.__init__(
self,
outcomes=['succeeded', 'preempted', 'aborted'],
input_keys=['object_name'],
output_keys=['object_pose'])
with self:
smach.StateMachine.add('Prepare_data',
Prepare_data(object_to_detect_name),
transitions={'succeeded': 'Prepare_goal'})
smach.StateMachine.add('Prepare_goal',
prepare_object_detection_goal(),
transitions={'succeeded': 'Object_detect'})
def object_detect_result_cb(self, status, object_detect_result):
print('Object Detected: ' + str(object_detect_result))
self.object_pose = object_detect_result
return 'succeeded'
smach.StateMachine.add(
'Object_detect',
SimpleActionState(
objectDetect_topic,
RecognizeAction,
result_cb=object_detect_result_cb,
goal_key='object_detection_goal',
output_keys=['object_pose', 'standard_error']),
transitions={
'succeeded': 'succeeded',
'aborted': 'aborted'
})
def __init__(self, pose_to_place=''):
smach.StateMachine.__init__(
self,
outcomes=['succeeded', 'preempted', 'aborted'],
input_keys=['pose_to_place'],
output_keys=[])
with self:
smach.StateMachine.add("Prepare_data",
Prepare_data(pose_to_place),
transitions={
'succeeded': 'Place_Object_Goal',
'aborted': 'aborted'
})
smach.StateMachine.add('Place_Object_Goal',
Prepare_Place_Goal(),
transitions={'succeeded': 'Place_Object'})
smach.StateMachine.add('Place_Object',
SimpleActionState(
OBJECT_MANIPULATION_TOPIC,
ObjectManipulationAction,
goal_key='object_manipulation_goal',
input_keys=['standard_error'],
output_keys=['standard_error']),
transitions={
'succeeded': 'succeeded',
'aborted': 'aborted'
})
def sm_best_vantage():
sm = smach.StateMachine(outcomes=['succeeded', 'aborted'],
input_keys=['rfid_reads', 'tagid'])
with sm:
smach.StateMachine.add(
'SELECT_BEST_VANTAGE',
BestVantage(),
remapping={
'tagid': 'tagid', # input (string)
'rfid_reads': 'rfid_reads', # input (RecorderReads)
'best_vantage': 'best_vantage', # output (PoseStamped)
'filtered_reads': 'filtered_reads'
}, # output (RecorderReads)
transitions={'succeeded': 'MOVE_BEST_VANTAGE'})
smach.StateMachine.add(
'MOVE_BEST_VANTAGE',
SimpleActionState('/move_base',
MoveBaseAction,
goal_slots=['target_pose']),
remapping={'target_pose': 'best_vantage'}, # input (PoseStamped)
transitions={
'aborted': 'SELECT_BEST_VANTAGE',
'preempted': 'aborted',
'succeeded': 'succeeded'
})
return sm
def los_move(x, y, z=-0.5):
goal = MoveGoal()
goal.controller_name = 'LOS'
goal.target_pose.position = Point(x, y, z)
return SimpleActionState('move', MoveAction, goal=goal)
def execute(self, userdata):
self._goal.planner_service_name = "ompl_planning/plan_kinematic_path"
motion_plan_request = MotionPlanRequest()
motion_plan_request.group_name = "SchunkArm"
motion_plan_request.num_planning_attempts = 1
motion_plan_request.allowed_planning_time = rospy.Duration(5.0)
motion_plan_request.planner_id = ""
self._goal.motion_plan_request = motion_plan_request
desired_pose = SimplePoseConstraint()
desired_pose.header.frame_id = userdata.parent_frame
desired_pose.link_name = "Gripper"
desired_pose.pose.position.x = userdata.x
desired_pose.pose.position.y = userdata.y
desired_pose.pose.position.z = userdata.z
quat = tf.transformations.quaternion_from_euler(-math.pi/2, math.pi/2, userdata.phi)
desired_pose.pose.orientation = Quaternion(*quat)
desired_pose.absolute_position_tolerance.x = 0.02
desired_pose.absolute_position_tolerance.y = 0.02
desired_pose.absolute_position_tolerance.z = 0.02
desired_pose.absolute_roll_tolerance = 0.04
desired_pose.absolute_pitch_tolerance = 0.04
desired_pose.absolute_yaw_tolerance = 0.04
add_goal_constraint_to_move_arm_goal(desired_pose, self._goal)
srv_out = SimpleActionState.execute(self, userdata)
return srv_out
def main():
rospy.init_node('smach_example_state_machine')
# Create a SMACH state machine
sm = smach.StateMachine(['succeeded','aborted','preempted'])
# Open the container
with sm:
# Add states to the container
smach.StateMachine.add('WAIT', Wait(),
transitions={'service':'SERVICE',
'action':'ACTION'})
smach.StateMachine.add('SERVICE',
ServiceState('service_test',
bool_bool),
transitions={'succeeded':'WAIT',
'aborted':'WAIT',
'preempted':'WAIT'})
smach.StateMachine.add('ACTION',
SimpleActionState('find_buoy',
VisionExampleAction),
transitions={'succeeded':'WAIT',
'aborted':'WAIT',
'preempted':'WAIT'})
# Execute SMACH plan
outcome = sm.execute()
def get_nav_approach(self):
def child_term_cb(outcome_map):
return True
def out_cb(outcome_map):
if outcome_map['DETECT_FORWARD_DISTANCE'] == 'reached':
return 'succeeded'
return 'shutdown'
sm_nav_approach_state = smach.Concurrence(
outcomes=['succeeded', 'shutdown'],
default_outcome='shutdown',
child_termination_cb=child_term_cb,
outcome_cb=out_cb,
output_keys=['nav_dist'])
with sm_nav_approach_state:
approach_goal = ApproachGoal()
approach_goal.forward_vel = 0.05
approach_goal.forward_mult = 0.50
smach.Concurrence.add(
'MOVE_FORWARD',
SimpleActionState('/approach_table/move_forward_act',
ApproachAction,
goal=approach_goal))
smach.Concurrence.add(
'DETECT_FORWARD_DISTANCE',
DetectForwardDistance(self.get_transform,
self.nav_approach_dist))
return sm_nav_approach_state
def main():
rospy.init_node('move_gripper_client')
parser = argparse.ArgumentParser(
description='Client for Korus` move gripper action server')
parser.add_argument('gripper_angle', type=float, help='gripper angle')
args = parser.parse_args()
sm = StateMachine(outcomes=['succeeded', 'aborted', 'preempted'])
sm.userdata.gripper_angle = args.gripper_angle
with sm:
def MoveGripperResultCb(userdata, status, result):
rospy.loginfo('action finished in state ' + str(status))
rospy.loginfo('action result: ')
rospy.loginfo(str(result))
smach.StateMachine.add(
'MoveGripper',
SimpleActionState('/korus/move_gripper',
MoveGripperAction,
goal_slots=['gripper_angle'],
result_cb=MoveGripperResultCb))
sm.execute()
def PatrolStateMachine(listOfWaypoints):
tempWaypoint = ()
waypoints = []
#convert waypoints to ros topic format
#waypoints.append([str(0), (listOfWaypoints[-1][1],listOfWaypoints[-1][0]), (0.0, 0.0, 0.0, 1.0)])
for i in range(0, len(listOfWaypoints), 1):
tempWaypoint = (listOfWaypoints[i][1], listOfWaypoints[i][0])
waypoints.append([str(i), tempWaypoint, (0.0, 0.0, 0.0, 1.0)])
#start state machine
patrol = StateMachine(['succeeded', 'aborted', 'preempted'])
with patrol:
for i, w in enumerate(waypoints):
goal_pose = MoveBaseGoal()
goal_pose.target_pose.header.frame_id = 'map'
goal_pose.target_pose.pose.position.x = w[1][0]
goal_pose.target_pose.pose.position.y = w[1][1]
goal_pose.target_pose.pose.position.z = 0.0
goal_pose.target_pose.pose.orientation.x = w[2][0]
goal_pose.target_pose.pose.orientation.y = w[2][1]
goal_pose.target_pose.pose.orientation.z = w[2][2]
goal_pose.target_pose.pose.orientation.w = w[2][3]
StateMachine.add(w[0],
SimpleActionState('move_base',
MoveBaseAction,
goal=goal_pose),
transitions={'succeeded': waypoints[(i + 1) % \
len(waypoints)][0]})
patrol.execute()
def main():
rospy.init_node('smach_example_state_machine')
# Create a SMACH state machine
sm = smach.StateMachine(outcomes=['outcome4', 'outcome5'])
# Open the container
with sm:
# Add states to the container
smach.StateMachine.add('FOO',
Foo(),
transitions={
'outcome1': 'BAR',
'outcome2': 'outcome4'
})
smach.StateMachine.add('BAR', Bar(), transitions={'outcome2': 'FOO'})
def gripper_result_cb(userdata, status, result):
return 'my_outcome'
smach.StateMachine.add('TRIGGER_GRIPPER',
SimpleActionState(
'action_server_namespace',
GetMap,
result_cb=gripper_result_cb,
output_keys=['gripper_output']),
transitions={'succeeded': 'APPROACH_PLUG'},
remapping={'gripper_output': 'userdata_output'})
# Execute SMACH plan
outcome = sm.execute()
def main():
rospy.init_node('smach_example_state_machine')
sm = StateMachine(['exit'])
with sm:
for key, (x, y, next_point) in WAYPOINTS.items():
goal = MoveBaseGoal()
goal.target_pose.header.frame_id = 'map'
goal.target_pose.pose.position.x = x
goal.target_pose.pose.position.y = y
goal.target_pose.pose.orientation.w = 1.
StateMachine.add(key, SimpleActionState('move_base',
MoveBaseAction,
goal=goal),
transitions={'succeeded': next_point, 'aborted': 'exit', 'preempted': 'exit'})
# Create and start the introspection server
sis = IntrospectionServer('strat', sm, '/SM_ROOT')
sis.start()
# Execute the state machine
outcome = sm.execute()
# Wait for ctrl-c to stop the application
rospy.spin()
sis.stop()
def __init__(self, text=None, wait_before_speak=None):
''' If text is used, it will be that text which the robot will say. '''
self._text = text
input_keys = []
if not text:
input_keys = ['text']
self._wait_before_speak = wait_before_speak
# Method to define the goal
def tts_request_cb(ud, goal):
tts_goal = SpeechWithFeedbackGoal()
if (not self._text):
tts_goal.say = ud.text
else:
tts_goal.say = self._text
return tts_goal
SimpleActionState.__init__(self, '/speech_action', SpeechWithFeedbackAction, input_keys=input_keys, goal_cb=tts_request_cb)
def arms_out_of_self_colision(self):
self._print_title("ARMS OUT OF SELF COLISION")
if self.using_the_robot:
if self.__check_action(ARMS_ACTION_NAME) == aborted:
return aborted
else:
self._print_warning("Not checking. ROS_MASTER_URI nor COMPUTER_NAME contains %s" % ROBOTS_NAME)
return aborted
robot = os.environ.get('PAL_ROBOT')
ros_master_uri = os.environ.get('ROS_MASTER_URI')
remotelly_executing = ros_master_uri.rfind('localhost')
rospy.loginfo('remotelly_executing: %s' % remotelly_executing) # FIXME: Remove this line after test in the robot
MOTION_FOLDER_PATH = ''
plan = False
checkSafety = False
if robot == 'rh2c' or robot == 'rh2m' or robot == 'reemh3c' or robot == 'reemh3m' or robot == 'reemh3' or remotelly_executing == -1:
check_for_door = True # FIXME: This variable is not being used.
MOTION_FOLDER_PATH = "/mnt_flash/stacks/reem_alive/alive_engine/config/database/Stopped/interact_1.xml"
if remotelly_executing == -1:
MOTION_FOLDER_PATH = packages.get_pkg_dir('pal_smach_utils') + '/config/interact_1.xml'
plan = True
checkSafety = False
motion_goal = MotionManagerGoal()
motion_goal.plan = plan
motion_goal.filename = MOTION_FOLDER_PATH
motion_goal.checkSafety = checkSafety
motion_goal.repeat = False
motion_manager_action = SimpleActionState(ARMS_ACTION_NAME, MotionManagerAction, goal=motion_goal)
userdata_hacked = {}
status = motion_manager_action.execute(userdata_hacked)
if status == succeeded:
rospy.loginfo(self.colors.GREEN_BOLD + "Arms out of self colisin: OK " + self.colors.NATIVE_COLOR)
else:
self.ALL_OK = False
rospy.loginfo(self.colors.RED_BOLD + "Arms out of self colisin: Failed " + self.colors.NATIVE_COLOR)
def __init__(self, path=None, frame_id='base_footprint'):
# Private attributes
self._path = path
self._frame_id = frame_id
# Method to define the goal
def walk_path_goal_cb(userdata, goal):
walk_path_goal = FollowPathGoal()
path = Path()
path.header.frame_id = self._frame_id
path.header.stamp = rospy.Time.now()
for sub in self._path:
subgoal = PoseStamped()
subgoal.header.frame_id = self._frame_id
subgoal.header.stamp = rospy.Time.now()
subgoal.pose.position = Point(sub[0], sub[1], 0.0)
q = tf.transformations.quaternion_from_euler(0, 0, sub[2])
subgoal.pose.orientation = Quaternion(*q)
path.poses.append(subgoal)
walk_path_goal.path = path
return walk_path_goal
# Class constructor
SimpleActionState.__init__(self, '/walk_path', FollowPathAction, goal_cb=walk_path_goal_cb)
def __init__(self, frame_id="/base_link", move_base_action_name=MOVE_BASE_ACTION_NAME, pose=None, goal_cb=None,
goal_key=None, **kwargs):
"""Constructor for MoveActionState
@type frame_id: string
@param frame_id: The fame_id of the pose object. By default is "/base_link"
@type move_base_action_name: string
@param move_base_action_name: The action name. By default is the action name defined on the MOVE_BASE_ACTION_NAME variable.
@type pose: geometry_msgs.Pose
@param pose: The pose where the robot should move to on frame_id link.
@type goal_cb: callable
@param goal_cb: The function that will be called to get the pose. You can set only one of 'pose, goal_cb and goal_key' variables.
@type goal_key: input_key on userdata.
@param goal_key: The key on userdata variable that contains the pose.
"""
if (pose and goal_cb) or (pose and goal_key) or (goal_cb and goal_key):
raise ValueError("You've set more than one of `pose', " \
"`goal_cb' and `goal_key'!")
if not goal_cb and not goal_key and not pose:
raise ValueError("Neither `pose' nor `goal_cb' nor `goal_key' were set!")
if goal_key:
if not 'input_keys' in kwargs:
kwargs['input_keys'] = []
kwargs['input_keys'].append(goal_key)
def generic_goal_cb(userdata, old_goal):
""" Send a goal to the action """
self.nav_goal = MoveBaseGoal()
self.nav_goal.target_pose.header.stamp = rospy.Time.now()
self.nav_goal.target_pose.header.frame_id = frame_id
if pose:
self.nav_goal.target_pose.pose = pose
return self.nav_goal
elif goal_key:
self.nav_goal.target_pose.pose = getattr(userdata, goal_key)
return self.nav_goal
else:
self.nav_goal.target_pose.pose = Pose()
return goal_cb(userdata, self.nav_goal)
def _result_cb(userdata, status, result):
""" This function will analize the status of the response
If the status is aborted, will call the action again with the same
orientation, but with the distance -0,5. If the status now is succeeded,
then call the move_by with x=0.5 in front.
:type status: actionlib.GoalStatus
:type result: MoveBaseResult
"""
if status != GoalStatus.SUCCEEDED:
rospy.loginfo(C.BACKGROUND_YELLOW + "Retrying go to the target goal" + C.NATIVE_COLOR)
new_pose = pose_at_distance(self.nav_goal.target_pose.pose, DISTANCE_TO_RETRY )
self.nav_goal.target_pose.pose = new_pose
self.nav_goal.target_pose.header.stamp = rospy.Time.now()
move_action = SimpleActionState(move_base_action_name, MoveBaseAction, goal=self.nav_goal)
result_status = move_action.execute(userdata)
if result_status == succeeded:
new_goal = MoveBaseGoal()
new_goal.target_pose.header.frame_id = FRAME_BASE_LINK
new_goal.target_pose.pose.position.x = DISTANCE_TO_RETRY
new_goal.target_pose.header.stamp = rospy.Time.now()
move_action = SimpleActionState(MOVE_BY_ACTION_NAME , MoveBaseAction, goal=new_goal)
return move_action.execute(userdata)
# THE PART BELOW WAS 'REMOVED' BECAUSE FINAL_APPROACH IS NOT RUNNING BY DEFAULT.
# final_approach_goal = FinalApproachPoseRequest()
# final_approach_goal.pose.position.x = DISTANCE_TO_RETRY - 0.1 # -0.1 Just because final_approach don't check if is secure to move
# final_approach_goal.pose.orientation.w = 0.0
# rospy.loginfo("Calling /approachToGoal with message:\n" + str(final_approach_goal))
# final_approach = ServiceState('/approachToGoal', FinalApproachPose, request=final_approach_goal)
# return final_approach.execute(userdata)
SimpleActionState.__init__(self,
move_base_action_name, MoveBaseAction,
goal_cb=generic_goal_cb, result_cb=_result_cb, **kwargs)
def __init__(self, frame='/map'):
SimpleActionState.__init__(self, 'move_base', MoveBaseAction, input_keys=['x', 'y', 'yaw'], goal_cb=self.__goal_cb)
self.frame = frame
def __init__(self, follow_joint_trajectory_topic = '/right_hand_controller/follow_joint_trajectory'):
SimpleActionState.__init__(self, follow_joint_trajectory_topic,
FollowJointTrajectoryAction,
goal_cb=grasp_open_hand_goal_cb2,
result_cb=grasp_hand_result_cb)
def __init__(self, input_keys=['tuck_left', 'tuck_right']):
tuck_arms_uri = '/tuck_arms'
SimpleActionState.__init__(self, tuck_arms_uri, TuckArmsAction, goal_cb=self.goal_cb, input_keys=input_keys)
def __init__(self, frame="/map"):
SimpleActionState.__init__(
self, "move_base", MoveBaseAction, input_keys=["x", "y", "yaw"], goal_cb=self.__goal_cb
)
self.frame = frame
def __init__(self, input_keys=[]):
SimpleActionState.__init__(self, 'recharge', RechargeAction, goal_cb=self.goal_cb, input_keys=input_keys)
def __init__(self):
SimpleActionState.__init__(self, 'move_SchunkArm', MoveArmAction,
input_keys=['x', 'y', 'z', 'phi', 'parent_frame'])
