def __init__(self):
node_params = util.get_node_params()
self.odom_frame = node_params.odometry_frame
self.tf_listener = tf.TransformListener()
self.action_server = actionlib.SimpleActionServer('sun_pointing_action',
ComputeAngleAction,
self.run_compute_angle_action,
False)
self.simple_mover = actionlib.SimpleActionClient("simple_move",
samplereturn_msg.SimpleMoveAction)
self.spin_goal = SimpleMoveGoal(type=SimpleMoveGoal.SPIN,
velocity = node_params.spin_velocity)
self.bridge = CvBridge()
self.min_lightness = []
self.img_angles = []
self.turning = False
self.starting_yaw = None
self.current_yaw = None
rospy.Timer(rospy.Duration(node_params.yaw_update_period),
self.update_yaw)
self.action_server.start()
def __init__(self):
self.node_params = util.get_node_params()
self.tf_listener = tf.TransformListener()
rospy.sleep(2.0)
self.strafes = rospy.get_param('strafes')
self.check_publisher = rospy.Publisher('costmap_check',
samplereturn_msg.CostmapCheck,
queue_size=1)
self.debug_map_pub = rospy.Publisher('/test_costmap',
nav_msg.OccupancyGrid,
queue_size=1)
self.costmap_listener = rospy.Subscriber('local_costmap',
nav_msg.OccupancyGrid,
self.handle_costmap)
rospy.spin()
def __init__(self):
rospy.on_shutdown(self.shutdown_cb)
self.node_params = util.get_node_params()
self.tf_listener = tf.TransformListener()
self.executive_frame = self.node_params.executive_frame
self.beacon_approach_point = self.node_params.beacon_approach_point
#calculate beacon approach pose now, in /map
header = std_msg.Header(0, rospy.Time(0), '/map')
point = geometry_msg.Point(self.beacon_approach_point['x'],
self.beacon_approach_point['y'], 0)
quat_array = tf.transformations.quaternion_from_euler(0, 0, math.pi)
pose = geometry_msg.Pose(point, geometry_msg.Quaternion(*quat_array))
self.beacon_approach_pose = geometry_msg.PoseStamped(header, pose)
#interfaces
self.announcer = util.AnnouncerInterface("audio_navigate")
self.move_base = actionlib.SimpleActionClient("planner_move_base",
move_base_msg.MoveBaseAction)
self.CAN_interface = util.CANInterface()
start_time = rospy.get_time()
while not rospy.is_shutdown():
if self.move_base.wait_for_server(rospy.Duration(0.1)):
break #all services up, exit this loop
if (rospy.get_time() - start_time) > 10.0:
start_time = rospy.get_time()
self.announcer.say("Move base not available")
rospy.logwarn('Timeout waiting for move base')
rospy.sleep(0.1)
self.state_machine = smach.StateMachine(
outcomes=['complete', 'preempted', 'aborted'],
input_keys = ['action_goal'],
output_keys = ['action_result'])
#these are important values! master frame id and return timing
self.state_machine.userdata.executive_frame = self.executive_frame
self.state_machine.userdata.start_time = rospy.Time.now()
self.state_machine.userdata.return_time = rospy.Time.now() + \
rospy.Duration(self.node_params.return_time_minutes*60)
self.state_machine.userdata.pre_cached_id = samplereturn_msg.NamedPoint.PRE_CACHED
#beacon approach
self.state_machine.userdata.max_pursuit_error = self.node_params.max_pursuit_error
self.state_machine.userdata.min_pursuit_distance = self.node_params.min_pursuit_distance
self.state_machine.userdata.max_point_lost_time = self.node_params.max_point_lost_time
self.state_machine.userdata.beacon_approach_pose = self.beacon_approach_pose
#search line parameters
self.state_machine.userdata.line_plan_step = self.node_params.line_plan_step
self.state_machine.userdata.line_replan_distance = self.node_params.line_replan_distance
self.state_machine.userdata.line_velocity = self.node_params.line_velocity
self.state_machine.userdata.blocked_check_distance = self.node_params.blocked_check_distance
self.state_machine.userdata.blocked_check_width = self.node_params.blocked_check_width
self.state_machine.userdata.blocked_rotation_min = self.node_params.blocked_rotation_min
self.state_machine.userdata.blocked_rotation_max = self.node_params.blocked_rotation_max
self.state_machine.userdata.motion_check_interval = self.node_params.motion_check_interval
self.state_machine.userdata.min_motion = self.node_params.min_motion
#search line variables
self.state_machine.userdata.next_line_pose = None
self.state_machine.userdata.last_line_pose = None
self.state_machine.userdata.line_yaw = 0 #IN RADIANS!
#subscriber controlled userdata
self.state_machine.userdata.paused = False
self.state_machine.userdata.detected_sample = None
self.state_machine.userdata.beacon_point = None
#use these as booleans in remaps
self.state_machine.userdata.true = True
self.state_machine.userdata.false = False
#motion mode stuff
planner_mode = self.node_params.planner_mode
MODE_PLANNER = getattr(platform_srv.SelectMotionModeRequest, planner_mode)
with self.state_machine:
smach.StateMachine.add('START_LEVEL_TWO',
StartLeveLTwo(input_keys=['line_yaw',
'line_plan_step',
'executive_frame'],
output_keys=['action_result',
'next_line_pose'],
outcomes=['next']),
transitions = {'next':'ANNOUNCE_LEVEL_TWO'})
smach.StateMachine.add('ANNOUNCE_LEVEL_TWO',
AnnounceState(self.announcer,
'Enter ing level two mode'),
transitions = {'next':'SELECT_PLANNER'})
smach.StateMachine.add('SELECT_PLANNER',
SelectMotionMode(self.CAN_interface,
MODE_PLANNER),
transitions = {'next':'SEARCH_LINE',
'failed':'LEVEL_TWO_ABORTED'})
#the concurrence will preempt all other child states if this cb returns True
def search_line_cb(outcome_map):
return True
self.line_manager = smach.Concurrence(outcomes = ['sample_detected',
'move_complete',
'line_blocked',
'timeout',
'return_home',
'preempted', 'aborted'],
default_outcome = 'aborted',
input_keys = ['next_line_pose',
'line_yaw',
'line_velocity',
'line_plan_step',
'line_replan_distance',
'blocked_check_distance',
'blocked_check_width',
'return_time',
'detected_sample',
'motion_check_interval',
'min_motion',
'paused',
'true', 'false'],
output_keys = ['next_line_pose',
'last_line_pose'],
child_termination_cb = search_line_cb,
outcome_map = { 'sample_detected' : {'DRIVE_TO_POSE':'sample_detected'},
'move_complete' : {'DRIVE_TO_POSE':'complete'},
'line_blocked' : {'SEARCH_LINE_MANAGER':'line_blocked'},
'timeout' : {'DRIVE_TO_POSE':'timeout'},
'return_home' : {'SEARCH_LINE_MANAGER':'return_home'},
'preempted' : {'DRIVE_TO_POSE':'preempted',
'SEARCH_LINE_MANAGER':'preempted'} })
with self.line_manager:
smach.Concurrence.add('DRIVE_TO_POSE',
DriveToPoseState(self.move_base, self.tf_listener),
remapping = {'target_pose':'next_line_pose',
'velocity':'line_velocity',
'last_pose':'last_line_pose',
'pursue_samples':'true',
'stop_on_sample':'false'})
smach.Concurrence.add('SEARCH_LINE_MANAGER',
SearchLineManager(self.tf_listener, self.announcer))
#must enter this concurrency with next_line_pose set to valid goal, if it is
#at the current robot position, DriveToPose will return complete immediately
#this is seen as some kind of move_base error, and a new search line will be chosen
smach.StateMachine.add('SEARCH_LINE',
self.line_manager,
transitions = {'sample_detected':'PURSUE_SAMPLE',
'move_complete':'CHOOSE_NEW_LINE',
'line_blocked':'CHOOSE_NEW_LINE',
'timeout':'CHOOSE_NEW_LINE',
'return_home':'ANNOUNCE_RETURN_HOME',
'preempted':'LEVEL_TWO_PREEMPTED',
'aborted':'LEVEL_TWO_ABORTED'})
@smach.cb_interface(input_keys=['detected_sample'])
def get_pursuit_goal_cb(userdata, request):
goal = samplereturn_msg.GeneralExecutiveGoal()
goal.input_point = userdata.detected_sample
goal.input_string = "level_two_pursuit_request"
return goal
smach.StateMachine.add('PURSUE_SAMPLE',
smach_ros.SimpleActionState('pursue_sample',
samplereturn_msg.GeneralExecutiveAction,
goal_cb = get_pursuit_goal_cb),
transitions = {'succeeded':'ANNOUNCE_RETURN_TO_SEARCH',
'aborted':'ANNOUNCE_RETURN_TO_SEARCH'})
smach.StateMachine.add('ANNOUNCE_RETURN_TO_SEARCH',
AnnounceState(self.announcer,
'Return ing to search line'),
transitions = {'next':'RETURN_TO_SEARCH_LINE'})
smach.StateMachine.add('RETURN_TO_SEARCH_LINE',
DriveToPoseState(self.move_base,
self.tf_listener),
transitions = {'complete':'SEARCH_LINE',
'timeout':'ANNOUNCE_RETURN_TO_SEARCH',
'sample_detected':'PURSUE_SAMPLE',
'preempted':'LEVEL_TWO_PREEMPTED'},
remapping = {'target_pose':'last_line_pose',
'velocity':'line_velocity',
'pursue_samples':'true',
'stop_on_sample':'false'})
smach.StateMachine.add('CHOOSE_NEW_LINE',
ChooseNewLine(self.tf_listener),
transitions = {'next':'ROTATE_TO_NEW_LINE',
'preempted':'LEVEL_TWO_PREEMPTED',
'aborted':'LEVEL_TWO_ABORTED'})
smach.StateMachine.add('ROTATE_TO_NEW_LINE',
DriveToPoseState(self.move_base,
self.tf_listener),
transitions = {'complete':'SEARCH_LINE',
'timeout':'CHOOSE_NEW_LINE',
'sample_detected':'LEVEL_TWO_ABORTED',
'preempted':'LEVEL_TWO_PREEMPTED'},
remapping = {'target_pose':'rotate_pose',
'velocity':'line_velocity',
'pursue_samples':'false'})
smach.StateMachine.add('ANNOUNCE_RETURN_HOME',
AnnounceState(self.announcer,
'Moving to beacon approach point'),
transitions = {'next':'DRIVE_TO_BEACON_APPROACH_START'})
smach.StateMachine.add('DRIVE_TO_BEACON_APPROACH_START',
DriveToPoseState(self.move_base,
self.tf_listener),
transitions = {'complete':'ANNOUNCE_SEARCH_FOR_BEACON',
'timeout':'ANNOUNCE_RETURN_HOME',
'sample_detected':'ANNOUNCE_APPROACH_BEACON'},
remapping = {'target_pose':'beacon_approach_pose',
'velocity':'line_velocity',
'detected_sample':'beacon_point',
'pursue_samples':'true',
'stop_on_sample':'false'})
smach.StateMachine.add('ANNOUNCE_APPROACH_BEACON',
AnnounceState(self.announcer,
'Beacon in view approach ing'),
transitions = {'next':'APPROACH_BEACON'})
self.approach_beacon = GetPursueDetectedPointState(self.move_base,
self.tf_listener)
smach.StateMachine.add('APPROACH_BEACON',
self.approach_beacon,
transitions = {'min_distance':'ANNOUNCE_MOUNT_PLATFORM',
'point_lost':'ANNOUNCE_RETURN_HOME',
'complete':'ANNOUNCE_RETURN_HOME',
'timeout':'ANNOUNCE_RETURN_HOME',
'aborted':'LEVEL_TWO_ABORTED'},
remapping = {'velocity':'line_velocity',
'pursuit_point':'beacon_point',
'pursue_samples':'false',
'stop_on_sample':'false'})
smach.StateMachine.add('ANNOUNCE_MOUNT_PLATFORM',
AnnounceState(self.announcer,
'Mount ing platform'),
transitions = {'next':'MOUNT_PLATFORM'})
smach.StateMachine.add('MOUNT_PLATFORM',
DriveToPoseState(self.move_base,
self.tf_listener),
transitions = {'complete':'DESELECT_PLANNER',
'timeout':'ANNOUNCE_RETURN_HOME',
'sample_detected':'LEVEL_TWO_ABORTED'},
remapping = {'velocity':'line_velocity',
'pursue_samples':'false',
'stop_on_sample':'false'})
#ADD BEACON SEARCH!
smach.StateMachine.add('ANNOUNCE_SEARCH_FOR_BEACON',
AnnounceState(self.announcer,
'Beacon not in view, searching'),
transitions = {'next':'LEVEL_TWO_ABORTED'})
MODE_ENABLE = platform_srv.SelectMotionModeRequest.MODE_ENABLE
smach.StateMachine.add('DESELECT_PLANNER',
SelectMotionMode(self.CAN_interface,
MODE_ENABLE),
transitions = {'next':'complete',
'failed':'LEVEL_TWO_ABORTED'})
smach.StateMachine.add('LEVEL_TWO_PREEMPTED',
LevelTwoPreempted(),
transitions = {'complete':'preempted',
'fail':'aborted'})
smach.StateMachine.add('LEVEL_TWO_ABORTED',
LevelTwoAborted(),
transitions = {'recover':'ANNOUNCE_RETURN_HOME',
'fail':'aborted'})
#action server wrapper
level_two_server = smach_ros.ActionServerWrapper(
'level_two', samplereturn_msg.GeneralExecutiveAction,
wrapped_container = self.state_machine,
succeeded_outcomes = ['complete'],
preempted_outcomes = ['preempted'],
aborted_outcomes = ['aborted'],
goal_key = 'action_goal',
result_key = 'action_result')
#introspection server
sls = smach_ros.IntrospectionServer('smach_grab_introspection',
self.state_machine,
'/START_LEVEL_TWO')
rospy.Subscriber('detected_sample_search',
samplereturn_msg.NamedPoint,
self.sample_update)
rospy.Subscriber("beacon_pose",
geometry_msg.PoseStamped,
self.beacon_update)
rospy.Subscriber("pause_state", std_msg.Bool, self.pause_state_update)
#start action servers and services
sls.start()
level_two_server.run_server()
rospy.spin()
sls.stop()
def __init__(self):
rospy.on_shutdown(self.shutdown_cb)
node_params = util.get_node_params()
self.tf_listener = tf.TransformListener()
self.world_fixed_frame = rospy.get_param("world_fixed_frame", "map")
self.odometry_frame = rospy.get_param("odometry_frame", "odom")
#make a Point msg out of beacon_approach_point, and a pose, at that point, facing beacon
header = std_msg.Header(0, rospy.Time(0), self.world_fixed_frame)
point = geometry_msg.Point(node_params.beacon_approach_point['x'],
node_params.beacon_approach_point['y'], 0)
beacon_facing_orientation = geometry_msg.Quaternion(*tf.transformations.quaternion_from_euler(0,0,math.pi))
pose = geometry_msg.Pose(position = point, orientation = beacon_facing_orientation)
self.beacon_approach_pose = geometry_msg.PoseStamped(header = header, pose = pose)
#also need platform point
platform_point = geometry_msg.Point( 0, 0, 0)
self.platform_point = geometry_msg.PointStamped(header, platform_point)
#interfaces
self.announcer = util.AnnouncerInterface("audio_search")
self.CAN_interface = util.CANInterface()
#movement action servers
self.vfh_mover = actionlib.SimpleActionClient("vfh_move",
samplereturn_msg.VFHMoveAction)
self.simple_mover = actionlib.SimpleActionClient("simple_move",
samplereturn_msg.SimpleMoveAction)
#get the star shape
self.spokes = self.get_hollow_star(node_params.spoke_count,
node_params.spoke_length,
node_params.first_spoke_offset,
node_params.star_hub_radius,
node_params.spin_step)
self.state_machine = smach.StateMachine(
outcomes=['complete', 'preempted', 'aborted'],
input_keys = ['action_goal'],
output_keys = ['action_result'])
self.state_machine.userdata.spokes = self.spokes
self.state_machine.userdata.star_hub_radius = node_params.star_hub_radius
#store the intial planned goal in map, check for the need to replan
self.state_machine.userdata.move_point_map = None
self.replan_threshold = node_params.replan_threshold
#these are important values! master frame id and return timing
self.state_machine.userdata.world_fixed_frame = self.world_fixed_frame
self.state_machine.userdata.odometry_frame = self.odometry_frame
self.state_machine.userdata.start_time = rospy.Time.now()
self.state_machine.userdata.return_time = rospy.Time.now() + \
rospy.Duration(node_params.return_time_minutes*60)
self.state_machine.userdata.pre_cached_id = samplereturn_msg.NamedPoint.PRE_CACHED
#dismount move
self.state_machine.userdata.dismount_move = node_params.dismount_move
#beacon approach
self.state_machine.userdata.beacon_approach_pose = self.beacon_approach_pose
self.state_machine.userdata.beacon_mount_step = node_params.beacon_mount_step
self.state_machine.userdata.platform_point = self.platform_point
#search line parameters
self.state_machine.userdata.move_velocity = node_params.move_velocity
self.state_machine.userdata.spin_velocity = node_params.spin_velocity
self.state_machine.userdata.min_spin_radius = node_params.min_spin_radius
self.state_machine.userdata.last_spin_radius = 0
self.state_machine.userdata.spin_step = node_params.spin_step
self.state_machine.userdata.line_yaw = None #IN RADIANS!
self.state_machine.userdata.outbound = False
self.state_machine.userdata.blocked_retry_delay = rospy.Duration(node_params.blocked_retry_delay)
self.state_machine.userdata.blocked_retried = False
self.state_machine.userdata.last_retry_time = rospy.Time.now()
#stop function flags
self.state_machine.userdata.stop_on_sample = False
self.state_machine.userdata.stop_on_beacon = False
#subscriber controlled userdata
self.state_machine.userdata.paused = False
self.state_machine.userdata.detected_sample = None
self.state_machine.userdata.beacon_point = None
#use these as booleans in remaps
self.state_machine.userdata.true = True
self.state_machine.userdata.false = False
#motion mode stuff
planner_mode = node_params.planner_mode
MODE_PLANNER = getattr(platform_srv.SelectMotionModeRequest, planner_mode)
MODE_SERVO = platform_srv.SelectMotionModeRequest.MODE_SERVO
MODE_PAUSE = platform_srv.SelectMotionModeRequest.MODE_PAUSE
with self.state_machine:
smach.StateMachine.add('START_LEVEL_TWO',
StartLeveLTwo(input_keys=['dismount_move'],
output_keys=['action_result',
'simple_move'],
outcomes=['next']),
transitions = {'next':'ANNOUNCE_LEVEL_TWO'})
smach.StateMachine.add('ANNOUNCE_LEVEL_TWO',
AnnounceState(self.announcer,
'Enter ing level two mode.'), # Enable ing search camera.'),
transitions = {'next':'SELECT_PLANNER'})
#smach.StateMachine.add('ENABLE_SEARCH_CAMERA',
# smach_ros.ServiceState('enable_search',
# platform_srv.Enable,
# request = platform_srv.EnableRequest(True)),
# transitions = {'succeeded':'SELECT_PLANNER',
# 'aborted':'LEVEL_TWO_ABORTED'})
smach.StateMachine.add('SELECT_PLANNER',
SelectMotionMode(self.CAN_interface,
MODE_PLANNER),
transitions = {'next':'DISMOUNT_MOVE',
'paused':'WAIT_FOR_UNPAUSE',
'failed':'LEVEL_TWO_ABORTED'})
smach.StateMachine.add('WAIT_FOR_UNPAUSE',
WaitForFlagState('paused',
flag_trigger_value = False,
timeout = 15,
announcer = self.announcer,
start_message ='System is paused. Un pause to continue level two'),
transitions = {'next':'SELECT_PLANNER',
'timeout':'WAIT_FOR_UNPAUSE',
'preempted':'LEVEL_TWO_PREEMPTED'})
smach.StateMachine.add('DISMOUNT_MOVE',
ExecuteSimpleMove(self.simple_mover),
transitions = {'complete':'STAR_MANAGER',
'sample_detected':'STAR_MANAGER',
'preempted':'LEVEL_TWO_PREEMPTED',
'aborted':'LEVEL_TWO_ABORTED'},
remapping = {'stop_on_sample':'false'})
smach.StateMachine.add('STAR_MANAGER',
StarManager(self.tf_listener,
self.announcer),
transitions = {'next_spoke':'LINE_MANAGER',
'return_home':'ANNOUNCE_RETURN_HOME'})
smach.StateMachine.add('LINE_MANAGER',
SearchLineManager(self.tf_listener),
transitions = {'move':'LINE_MOVE',
'line_end':'STAR_MANAGER',
'return_home':'ANNOUNCE_RETURN_HOME',
'preempted':'LEVEL_TWO_PREEMPTED',
'aborted':'LEVEL_TWO_ABORTED'})
smach.StateMachine.add('LINE_MOVE',
ExecuteVFHMove(self.vfh_mover),
transitions = {'complete':'SPIN_MANAGER',
'missed_target':'ANNOUNCE_MISSED_TARGET',
'blocked':'ANNOUNCE_LINE_BLOCKED',
'off_course':'ANNOUNCE_OFF_COURSE',
'sample_detected':'PURSUE_SAMPLE',
'preempted':'LEVEL_TWO_PREEMPTED',
'aborted':'LEVEL_TWO_ABORTED'},
remapping = {'stop_on_sample':'true'})
smach.StateMachine.add('ANNOUNCE_LINE_BLOCKED',
AnnounceState(self.announcer, 'Line Blocked.'),
transitions = {'next':'RETRY_LINE_MOVE'})
smach.StateMachine.add('RETRY_LINE_MOVE',
RetryMove(self.announcer),
transitions = {'continue':'STAR_MANAGER',
'retry':'LINE_MOVE',
'preempted':'LEVEL_TWO_PREEMPTED',
'aborted':'LEVEL_TWO_ABORTED'})
smach.StateMachine.add('ANNOUNCE_OFF_COURSE',
AnnounceState(self.announcer, 'Off Course.'),
transitions = {'next':'STAR_MANAGER'})
smach.StateMachine.add('ANNOUNCE_MISSED_TARGET',
AnnounceState(self.announcer, 'Missed Target'),
transitions = {'next':'SPIN_MANAGER'})
smach.StateMachine.add('SPIN_MANAGER',
SpinManager(self.tf_listener),
transitions = {'spin':'ROTATE',
'move':'LINE_MANAGER',
'preempted':'LEVEL_TWO_PREEMPTED',
'aborted':'LEVEL_TWO_ABORTED'})
smach.StateMachine.add('ROTATE',
ExecuteSimpleMove(self.simple_mover),
transitions = {'complete':'LINE_MANAGER',
'sample_detected':'PURSUE_SAMPLE',
'preempted':'LEVEL_TWO_PREEMPTED',
'aborted':'LEVEL_TWO_ABORTED'},
remapping = {'stop_on_sample':'true'})
@smach.cb_interface(input_keys=['detected_sample'])
def pursuit_goal_cb(userdata, request):
goal = samplereturn_msg.GeneralExecutiveGoal()
goal.input_point = userdata.detected_sample
goal.input_string = "level_two_pursuit_request"
return goal
@smach.cb_interface(input_keys=['line_points','next_line_point'],
output_keys=['line_points','detected_sample'])
def pursuit_result_cb(userdata, status, result):
#clear samples after a pursue action
userdata.detected_sample = None
#reload the next spoke point, line manager will pop it back
#off the array and we will continue to go there
userdata.line_points.appendleft(userdata.next_line_point)
smach.StateMachine.add('PURSUE_SAMPLE',
smach_ros.SimpleActionState('pursue_sample',
samplereturn_msg.GeneralExecutiveAction,
goal_cb = pursuit_goal_cb,
result_cb = pursuit_result_cb),
transitions = {'succeeded':'ANNOUNCE_RETURN_TO_SEARCH',
'aborted':'ANNOUNCE_RETURN_TO_SEARCH'})
smach.StateMachine.add('ANNOUNCE_RETURN_TO_SEARCH',
AnnounceState(self.announcer,
'Return ing to search line'),
transitions = {'next':'LINE_MANAGER'})
smach.StateMachine.add('ANNOUNCE_RETURN_HOME',
AnnounceState(self.announcer,
'Return ing to home platform'),
transitions = {'next':'BEACON_SEARCH'})
smach.StateMachine.add('BEACON_SEARCH',
BeaconSearch(self.tf_listener, self.announcer),
transitions = {'mount':'MOUNT_MANAGER',
'move':'BEACON_SEARCH_MOVE',
'spin':'BEACON_SEARCH_SPIN',
'aborted':'LEVEL_TWO_ABORTED'})
smach.StateMachine.add('BEACON_SEARCH_SPIN',
ExecuteSimpleMove(self.simple_mover),
transitions = {'complete':'BEACON_SEARCH',
'sample_detected':'BEACON_SEARCH',
'preempted':'LEVEL_TWO_PREEMPTED',
'aborted':'LEVEL_TWO_ABORTED'},
remapping = {'stop_on_sample':'stop_on_beacon',
'detected_sample':'beacon_point'})
#return to start along the approach point
#if the path is ever blocked just give up and return to the level_two search
smach.StateMachine.add('BEACON_SEARCH_MOVE',
ExecuteVFHMove(self.vfh_mover),
transitions = {'complete':'BEACON_SEARCH',
'blocked':'BEACON_SEARCH',
'missed_target':'BEACON_SEARCH',
'off_course':'BEACON_SEARCH',
'sample_detected':'BEACON_SEARCH',
'preempted':'BEACON_SEARCH',
'aborted':'LEVEL_TWO_ABORTED'},
remapping = {'stop_on_sample':'stop_on_beacon',
'detected_sample':'beacon_point'})
smach.StateMachine.add('MOUNT_MANAGER',
MountManager(self.tf_listener, self.announcer),
transitions = {'move':'MOUNT_MOVE',
'final':'MOUNT_FINAL',
'aborted':'LEVEL_TWO_ABORTED'})
smach.StateMachine.add('MOUNT_MOVE',
ExecuteSimpleMove(self.simple_mover),
transitions = {'complete':'MOUNT_MANAGER',
'sample_detected':'MOUNT_MANAGER',
'preempted':'LEVEL_TWO_PREEMPTED',
'aborted':'LEVEL_TWO_ABORTED'})
smach.StateMachine.add('MOUNT_FINAL',
ExecuteSimpleMove(self.simple_mover),
transitions = {'complete':'DESELECT_PLANNER',
'sample_detected':'MOUNT_MANAGER',
'preempted':'LEVEL_TWO_PREEMPTED',
'aborted':'LEVEL_TWO_ABORTED'})
smach.StateMachine.add('DESELECT_PLANNER',
SelectMotionMode(self.CAN_interface,
MODE_PAUSE),
transitions = {'next':'complete',
'paused':'LEVEL_TWO_ABORTED',
'failed':'LEVEL_TWO_ABORTED'})
smach.StateMachine.add('LEVEL_TWO_PREEMPTED',
LevelTwoPreempted(),
transitions = {'next':'preempted'})
smach.StateMachine.add('LEVEL_TWO_ABORTED',
LevelTwoAborted(),
transitions = {'next':'aborted'})
#action server wrapper
level_two_server = smach_ros.ActionServerWrapper(
'level_two', samplereturn_msg.GeneralExecutiveAction,
wrapped_container = self.state_machine,
succeeded_outcomes = ['complete'],
preempted_outcomes = ['preempted'],
aborted_outcomes = ['aborted'],
goal_key = 'action_goal',
result_key = 'action_result')
#introspection server
sls = smach_ros.IntrospectionServer('smach_grab_introspection',
self.state_machine,
'/START_LEVEL_TWO')
rospy.Subscriber('detected_sample_search',
samplereturn_msg.NamedPoint,
self.sample_update)
rospy.Subscriber("beacon_pose",
geometry_msg.PoseWithCovarianceStamped,
self.beacon_update)
rospy.Subscriber("pause_state", std_msg.Bool, self.pause_state_update)
#start a timer loop to check for localization updates
rospy.Timer(rospy.Duration(5.0), self.localization_check)
#start action servers and services
sls.start()
level_two_server.run_server()
rospy.spin()
sls.stop()
def __init__(self):
rospy.on_shutdown(self.shutdown_cb)
#stuff namedtuple with joystick parameters
self.node_params = util.get_node_params()
self.joy_state = JoyState(self.node_params)
self.CAN_interface = util.CANInterface()
#basler camera enable publishers
self.search_camera_enable = rospy.Publisher('enable_search',
std_msg.Bool,
queue_size=10)
self.beacon_camera_enable = rospy.Publisher('enable_beacon',
std_msg.Bool,
queue_size=10)
self.announcer = util.AnnouncerInterface("audio_search")
self.tf = tf.TransformListener()
self.odometry_frame = 'odom'
self.last_sample = rospy.Time.now()
self.simple_mover = actionlib.SimpleActionClient("simple_move",
samplereturn_msg.SimpleMoveAction)
self.state_machine = smach.StateMachine(
outcomes=['complete', 'preempted', 'aborted'],
input_keys = ['action_goal'],
output_keys = ['action_result'])
self.state_machine.userdata.button_cancel = self.joy_state.button('BUTTON_CANCEL')
self.state_machine.userdata.detected_sample = None
self.state_machine.userdata.manipulator_sample = None
self.state_machine.userdata.paused = False
self.state_machine.userdata.light_state = False
self.state_machine.userdata.search_camera_state = True
self.state_machine.userdata.announce_sample = False
self.state_machine.userdata.manual_bin = 0
#strafe search settings
self.state_machine.userdata.manipulator_correction = self.node_params.manipulator_correction
self.state_machine.userdata.servo_params = self.node_params.servo_params
#use these as booleans in remaps
self.state_machine.userdata.true = True
self.state_machine.userdata.false = False
with self.state_machine:
MODE_JOYSTICK = platform_srv.SelectMotionModeRequest.MODE_JOYSTICK
MODE_SERVO = platform_srv.SelectMotionModeRequest.MODE_SERVO
MODE_PLANNER = platform_srv.SelectMotionModeRequest.MODE_PLANNER_TWIST
MODE_PAUSE = platform_srv.SelectMotionModeRequest.MODE_PAUSE
MODE_RESUME = platform_srv.SelectMotionModeRequest.MODE_RESUME
MODE_HOME = platform_srv.SelectMotionModeRequest.MODE_HOME
MODE_UNLOCK = platform_srv.SelectMotionModeRequest.MODE_UNLOCK
MODE_LOCK = platform_srv.SelectMotionModeRequest.MODE_LOCK
MODE_ENABLE = platform_srv.SelectMotionModeRequest.MODE_ENABLE
smach.StateMachine.add('START_MANUAL_CONTROL',
ProcessGoal(self.announcer),
transitions = {'valid_goal':'SELECT_JOYSTICK',
'invalid_goal':'MANUAL_ABORTED'})
smach.StateMachine.add('SELECT_JOYSTICK',
SelectMotionMode(self.CAN_interface,
MODE_JOYSTICK),
transitions = {'next':'JOYSTICK_LISTEN',
'paused':'WAIT_FOR_UNPAUSE',
'failed':'MANUAL_ABORTED'})
smach.StateMachine.add('WAIT_FOR_UNPAUSE',
WaitForFlagState('paused',
flag_trigger_value = False,
timeout = 15,
announcer = self.announcer,
start_message ='System is paused. Un pause to allow manual control'),
transitions = {'next':'SELECT_JOYSTICK',
'timeout':'WAIT_FOR_UNPAUSE',
'preempted':'MANUAL_PREEMPTED'})
smach.StateMachine.add('JOYSTICK_LISTEN',
JoystickListen(self.CAN_interface, self.joy_state),
transitions = {'visual_servo_requested':'ENABLE_MANIPULATOR_DETECTOR',
'pursuit_requested':'CONFIRM_SAMPLE_PRESENT',
'manipulator_grab_requested':'ANNOUNCE_GRAB',
'home_wheelpods_requested':'SELECT_HOME',
'lock_wheelpods_requested':'SELECT_PAUSE_FOR_LOCK',
'preempted':'MANUAL_PREEMPTED',
'aborted':'MANUAL_ABORTED'})
smach.StateMachine.add('CONFIRM_SAMPLE_PRESENT',
ConfirmSamplePresent(self.announcer),
transitions = {'no_sample':'JOYSTICK_LISTEN',
'pursue':'SELECT_PURSUE_MODE',
'aborted':'MANUAL_ABORTED'})
smach.StateMachine.add('SELECT_PURSUE_MODE',
SelectMotionMode(self.CAN_interface,
MODE_PLANNER),
transitions = {'next':'PURSUE_SAMPLE',
'paused':'WAIT_FOR_UNPAUSE',
'failed':'SELECT_JOYSTICK'})
@smach.cb_interface(input_keys=['detected_sample'])
def pursuit_goal_cb(userdata, request):
goal = samplereturn_msg.GeneralExecutiveGoal()
goal.input_point = userdata.detected_sample
goal.input_string = "manual_control_pursuit_request"
#disable localization checks while in pursuit
return goal
@smach.cb_interface(output_keys=['detected_sample'])
def pursuit_result_cb(userdata, status, result):
#clear samples after a pursue action
rospy.sleep(2.0) #wait 2 seconds for detector/filter to clear for sure
userdata.detected_sample = None
smach.StateMachine.add('PURSUE_SAMPLE',
smach_ros.SimpleActionState('/processes/executive/pursue_sample',
samplereturn_msg.GeneralExecutiveAction,
goal_cb = pursuit_goal_cb,
result_cb = pursuit_result_cb),
transitions = {'succeeded':'SELECT_JOYSTICK',
'aborted':'SELECT_JOYSTICK'})
@smach.cb_interface(input_keys=['manipulator_sample'])
def enable_detector_cb(userdata, response):
timeout = rospy.Duration(5.0)
start = rospy.Time.now()
while (rospy.Time.now() - start) < timeout:
rospy.sleep(0.1)
if userdata.manipulator_sample is not None:
if userdata.manipulator_sample.header.stamp > start:
break
return 'succeeded'
smach.StateMachine.add('ENABLE_MANIPULATOR_DETECTOR',
smach_ros.ServiceState('enable_manipulator_detector',
samplereturn_srv.Enable,
request = samplereturn_srv.EnableRequest(True)),
transitions = {'succeeded':'ENABLE_MANIPULATOR_PROJECTOR',
'aborted':'SELECT_JOYSTICK'})
smach.StateMachine.add('ENABLE_MANIPULATOR_PROJECTOR',
smach_ros.ServiceState('enable_manipulator_projector',
samplereturn_srv.Enable,
request = samplereturn_srv.EnableRequest(True),
response_cb = enable_detector_cb,
input_keys = ['manipulator_sample']),
transitions = {'succeeded':'SELECT_SERVO_MODE',
'aborted':'SELECT_JOYSTICK'})
smach.StateMachine.add('SELECT_SERVO_MODE',
SelectMotionMode(self.CAN_interface,
MODE_PLANNER),
transitions = {'next':'VISUAL_SERVO',
'paused':'WAIT_FOR_UNPAUSE',
'failed':'SELECT_JOYSTICK'})
#calculate the strafe move to the sample
smach.StateMachine.add('VISUAL_SERVO',
ServoController(self.tf, self.announcer),
transitions = {'move':'SERVO_MOVE',
'complete':'DISABLE_MANIPULATOR_DETECTOR',
'point_lost':'ANNOUNCE_NO_SAMPLE',
'aborted':'DISABLE_MANIPULATOR_DETECTOR'},
remapping = {'detected_sample':'manipulator_sample'})
smach.StateMachine.add('SERVO_MOVE',
ExecuteSimpleMove(self.simple_mover),
transitions = {'complete':'VISUAL_SERVO',
'object_detected':'VISUAL_SERVO',
'aborted':'MANUAL_ABORTED'},
remapping = {'stop_on_detection':'false'})
smach.StateMachine.add('ANNOUNCE_NO_SAMPLE',
AnnounceState(self.announcer,
'Servo canceled.'),
transitions = {'next':'DISABLE_MANIPULATOR_DETECTOR'})
smach.StateMachine.add('ANNOUNCE_SERVO_CANCELED',
AnnounceState(self.announcer,
'Servo canceled.'),
transitions = {'next':'DISABLE_MANIPULATOR_DETECTOR'})
smach.StateMachine.add('DISABLE_MANIPULATOR_DETECTOR',
smach_ros.ServiceState('enable_manipulator_detector',
samplereturn_srv.Enable,
request = samplereturn_srv.EnableRequest(False)),
transitions = {'succeeded':'DISABLE_MANIPULATOR_PROJECTOR',
'aborted':'SELECT_JOYSTICK'})
smach.StateMachine.add('DISABLE_MANIPULATOR_PROJECTOR',
smach_ros.ServiceState('enable_manipulator_projector',
samplereturn_srv.Enable,
request = samplereturn_srv.EnableRequest(False)),
transitions = {'succeeded':'SELECT_JOYSTICK',
'aborted':'SELECT_JOYSTICK'})
smach.StateMachine.add('ANNOUNCE_GRAB',
AnnounceState(self.announcer,
'Start ing, grab.'),
transitions = {'next':'MANIPULATOR_GRAB'})
@smach.cb_interface(input_keys = ['manipulator_sample',
'manual_bin'])
def grab_msg_cb(userdata):
grab_msg = manipulator_msg.ManipulatorGoal()
grab_msg.type = grab_msg.GRAB
grab_msg.grip_torque = 0.7
grab_msg.target_bin = userdata.manual_bin
grab_msg.wrist_angle = 0
if userdata.manipulator_sample is not None:
grab_msg.wrist_angle = userdata.manipulator_sample.grip_angle
rospy.sleep(2.0)
return grab_msg
smach.StateMachine.add('MANIPULATOR_GRAB',
InterruptibleActionClientState(
actionname = 'manipulator_action',
actionspec = manipulator_msg.ManipulatorAction,
goal_cb = grab_msg_cb,
timeout = 30.0),
transitions = {'complete':'ANNOUNCE_GRAB_COMPLETE',
'timeout':'ANNOUNCE_GRAB_CANCELED',
'canceled':'ANNOUNCE_GRAB_CANCELED',
'preempted':'MANUAL_PREEMPTED',
'aborted':'MANUAL_ABORTED'})
smach.StateMachine.add('ANNOUNCE_GRAB_COMPLETE',
AnnounceState(self.announcer,
'Grab complete'),
transitions = {'next':'SELECT_JOYSTICK'})
smach.StateMachine.add('ANNOUNCE_GRAB_CANCELED',
AnnounceState(self.announcer,
'Servo canceled'),
transitions = {'next':'SELECT_JOYSTICK'})
smach.StateMachine.add('MANUAL_PREEMPTED',
ManualPreempted(self.CAN_interface),
transitions = {'complete':'preempted',
'fail':'aborted'})
smach.StateMachine.add('MANUAL_ABORTED',
ManualAborted(self.CAN_interface),
transitions = {'recover':'SELECT_JOYSTICK',
'fail':'aborted'})
smach.StateMachine.add('SELECT_HOME',
SelectMotionMode(self.CAN_interface,
MODE_HOME),
transitions = {'next':'ANNOUNCE_HOMING',
'paused':'WAIT_FOR_UNPAUSE',
'failed':'SELECT_JOYSTICK'})
smach.StateMachine.add('ANNOUNCE_HOMING',
AnnounceState(self.announcer, 'Home ing.'),
transitions = {'next':'PERFORM_HOME'})
home_goal = platform_msg.HomeGoal()
home_goal.home_count = 3
smach.StateMachine.add('PERFORM_HOME',
InterruptibleActionClientState(
actionname = 'home_wheel_pods',
actionspec = platform_msg.HomeAction,
goal = home_goal,
timeout = 30.0),
transitions = {'complete':'SELECT_JOYSTICK',
'timeout':'SELECT_JOYSTICK',
'canceled':'SELECT_JOYSTICK',
'preempted':'MANUAL_PREEMPTED',
'aborted':'MANUAL_ABORTED'})
smach.StateMachine.add('SELECT_PAUSE_FOR_LOCK',
SelectMotionMode(self.CAN_interface,
MODE_PAUSE),
transitions = {'next':'SELECT_LOCK',
'paused':'WAIT_FOR_UNPAUSE',
'failed':'SELECT_LOCK'})
smach.StateMachine.add('SELECT_LOCK',
SelectMotionMode(self.CAN_interface,
MODE_LOCK),
transitions = {'next':'ANNOUNCE_LOCK',
'paused':'WAIT_FOR_UNPAUSE',
'failed':'RESUME_FROM_LOCK'})
smach.StateMachine.add('ANNOUNCE_LOCK',
AnnounceState(self.announcer,
'Wheels locked'),
transitions = {'next':'WAIT_FOR_UNLOCK'})
smach.StateMachine.add('WAIT_FOR_UNLOCK',
WaitForJoystickButton(self.joy_state,
'BUTTON_LOCK',
self.announcer),
transitions = {'pressed':'SELECT_UNLOCK',
'timeout':'SELECT_LOCK',
'preempted':'MANUAL_PREEMPTED'})
smach.StateMachine.add('SELECT_UNLOCK',
SelectMotionMode(self.CAN_interface,
MODE_UNLOCK),
transitions = {'next':'RESUME_FROM_LOCK',
'paused':'WAIT_FOR_UNPAUSE',
'failed':'RESUME_FROM_LOCK'})
smach.StateMachine.add('RESUME_FROM_LOCK',
SelectMotionMode(self.CAN_interface,
MODE_RESUME),
transitions = {'next':'JOYSTICK_LISTEN',
'paused':'WAIT_FOR_UNPAUSE',
'failed':'SELECT_JOYSTICK'})
#end with state_machine
#action server wrapper
manual_control_server = smach_ros.ActionServerWrapper(
'manual_control', platform_msg.ManualControlAction,
wrapped_container = self.state_machine,
succeeded_outcomes = ['complete'],
preempted_outcomes = ['preempted'],
aborted_outcomes = ['aborted'],
goal_key = 'action_goal',
result_key = 'action_result')
sls = smach_ros.IntrospectionServer('smach_grab_introspection',
self.state_machine,
'/START_MANUAL_CONTROL')
sls.start()
rospy.Subscriber("pause_state", std_msg.Bool, self.pause_state_update)
joy_sub = rospy.Subscriber("joy", sensor_msg.Joy, self.joy_callback)
rospy.Subscriber('detected_sample_search',
samplereturn_msg.NamedPoint,
self.sample_update)
rospy.Subscriber('detected_sample_manipulator',
samplereturn_msg.NamedPoint,
self.sample_detection_manipulator)
#start action servers and services
manual_control_server.run_server()
rospy.spin()
def __init__(self):
# ordinary variables
self.last_servo_feedback = None
self.lost_sample = None
self.gpio = None
self.search_sample = None
self.manip_sample = None
self.last_detected_mode = None
self.last_selected_mode = None
self.selected_mode = None
self.preempt_modes = ('LEVEL_ONE_MODE', 'LEVEL_TWO_MODE', 'MANUAL_MODE')
#load all private node parameters
self.node_params = util.get_node_params()
self.GPIO_PIN_LEVEL_ONE = int(self.node_params.GPIO_PIN_LEVEL_ONE, 16)
self.GPIO_PIN_LEVEL_TWO = int(self.node_params.GPIO_PIN_LEVEL_TWO, 16)
# create publishers
self.announcer = util.AnnouncerInterface("audio_search")
self.state_machine = smach.StateMachine(outcomes=['shutdown', 'aborted'],
input_keys = [],
output_keys = [])
#create initial userdata states in this state machine
self.state_machine.userdata.paused = None
self.state_machine.userdata.selected_mode = None
#Beginning of top level state machine declaration.
#In this machine, "preempted" will indicate a normal command
#from another process or node, requesting the state machine to pause
#and handle some input. This will probably be primarily for handling
#mode changes. "aborted" will indicate an internal failure in a state
#that prevents the state from executing properly.
#The "next" outcome will be used for states that have only one possible transition.
with self.state_machine:
smach.StateMachine.add('START_MASTER_EXECUTIVE',
StartState(self.announcer),
transitions = { 'next':'WAIT_FOR_UNPAUSE',
'preempted':'TOP_PREEMPTED',
'aborted':'TOP_ABORTED'})
cam_dict = {'NAV_CENTER' : 'camera_started',
'NAV_PORT' : 'camera_started',
'NAV_STARBOARD' : 'camera_started'}
if not self.node_params.ignore_manipulator_camera:
cam_dict['MANIPULATOR'] = 'camera_started'
if not self.node_params.ignore_search_camera:
cam_dict['SEARCH'] = 'camera_started'
#concurrency container to allow waiting for cameras in parallel
wait_for_cams = smach.Concurrence(outcomes = ['all_started',
'timeout',
'preempted'],
default_outcome = 'timeout',
input_keys = [],
output_keys = [],
outcome_map = { 'all_started' : cam_dict})
with wait_for_cams:
camera_wait_outcomes = [('Ready', 'camera_started', 1 )]
smach.Concurrence.add('NAV_CENTER',
MonitorTopicState('navigation_center_camera_status',
'data',
std_msg.String,
camera_wait_outcomes,
timeout = 10.0))
smach.Concurrence.add('NAV_PORT',
MonitorTopicState('navigation_port_camera_status',
'data',
std_msg.String,
camera_wait_outcomes,
timeout = 10.0))
smach.Concurrence.add('NAV_STARBOARD',
MonitorTopicState('navigation_starboard_camera_status',
'data',
std_msg.String,
camera_wait_outcomes,
timeout = 10.0))
if not self.node_params.ignore_manipulator_camera:
smach.Concurrence.add('MANIPULATOR',
MonitorTopicState('manipulator_camera_status',
'data',
std_msg.String,
camera_wait_outcomes,
timeout = 10.0))
if not self.node_params.ignore_search_camera:
smach.Concurrence.add('SEARCH',
MonitorTopicState('search_camera_status',
'data',
std_msg.String,
camera_wait_outcomes,
timeout = 10.0))
smach.StateMachine.add('CHECK_CAMERAS',
wait_for_cams,
transitions = {'all_started':'CAMERA_SUCCESS',
'timeout':'CAMERA_FAILURE',
'preempted':'TOP_PREEMPTED'})
smach.StateMachine.add('CAMERA_FAILURE',
CameraFailure(output_keys=['camera_status'],
outcomes=['next']),
transitions = {'next':'CHECK_MODE'})
smach.StateMachine.add('CAMERA_SUCCESS',
CameraSuccess(output_keys=['camera_status'],
outcomes=['next']),
transitions = {'next':'CHECK_MODE'})
smach.StateMachine.add('WAIT_FOR_UNPAUSE',
WaitForFlagState('paused',
flag_trigger_value = False,
timeout = 20,
announcer = self.announcer,
start_message ='Waiting for system enable'),
transitions = {'next':'HOME_PLATFORM',
'timeout':'WAIT_FOR_UNPAUSE',
'preempted':'TOP_PREEMPTED'})
smach.StateMachine.add('HOME_PLATFORM',
HomePlatform(self.announcer),
transitions = {'homed':'CHECK_MODE',
'paused':'WAIT_FOR_UNPAUSE',
'preempted':'TOP_PREEMPTED',
'aborted':'TOP_ABORTED'})
smach.StateMachine.add('CHECK_MODE',
CheckMode(self.announcer),
transitions = {'manual':'MANUAL_MODE',
'level_one':'LEVEL_ONE_MODE',
'level_two':'LEVEL_TWO_MODE',
'check_cameras':'CHECK_CAMERAS',
'preempted':'TOP_PREEMPTED',
'aborted':'TOP_ABORTED'})
smach.StateMachine.add('LEVEL_ONE_MODE',
smach_ros.SimpleActionState('level_one',
samplereturn_msg.GeneralExecutiveAction,
goal = samplereturn_msg.GeneralExecutiveGoal()),
transitions = {'succeeded':'WAIT_FOR_MODE_CHANGE',
'preempted':'TOP_PREEMPTED',
'aborted':'TOP_ABORTED'})
smach.StateMachine.add('LEVEL_TWO_MODE',
smach_ros.SimpleActionState('level_two',
samplereturn_msg.GeneralExecutiveAction,
goal = samplereturn_msg.GeneralExecutiveGoal()),
transitions = {'succeeded':'WAIT_FOR_MODE_CHANGE',
'preempted':'TOP_PREEMPTED',
'aborted':'TOP_ABORTED'})
smach.StateMachine.add('WAIT_FOR_MODE_CHANGE',
WaitForFlagState('selected_mode',
flag_trigger_value = 'manual',
timeout = 20,
announcer = self.announcer,
start_message ='Mode complete, switch to manual'),
transitions = {'next':'CHECK_MODE',
'timeout':'WAIT_FOR_MODE_CHANGE',
'preempted':'TOP_PREEMPTED'})
mcg = platform_msg.ManualControlGoal()
mcg.mode = mcg.FULL_CONTROL
smach.StateMachine.add('MANUAL_MODE',
smach_ros.SimpleActionState('manual_control',
platform_msg.ManualControlAction,
goal = mcg),
transitions = {'succeeded':'CHECK_MODE',
'preempted':'TOP_PREEMPTED',
'aborted':'TOP_ABORTED'})
smach.StateMachine.add('TOP_PREEMPTED',
TopPreempted(),
transitions = {'next':'CHECK_MODE'})
smach.StateMachine.add('TOP_ABORTED',
TopAborted(),
transitions = {'recover':'CHECK_MODE',
'fail':'aborted'})
sls = smach_ros.IntrospectionServer('top_introspection',
self.state_machine,
'/START_MASTER_EXECUTIVE')
# subscribe to topics
rospy.Subscriber("gpio_read", platform_msg.GPIO, self.gpio_update)
rospy.Subscriber("pause_state", std_msg.Bool, self.pause_state_update)
#start state machine an introspection server
sls.start()
self.state_machine.execute()
sls.stop()
rospy.logwarn("EXECUTIVE MASTER STATE MACHINE EXIT")
rospy.spin()
def __init__(self):
rospy.on_shutdown(self.shutdown_cb)
self.node_params = util.get_node_params()
self.tf_listener = tf.TransformListener()
self.world_fixed_frame = rospy.get_param("world_fixed_frame", "map")
self.odometry_frame = rospy.get_param("odometry_frame", "odom")
#interfaces
self.announcer = util.AnnouncerInterface("audio_search")
self.result_pub = rospy.Publisher('pursuit_result', samplereturn_msg.PursuitResult, queue_size=1)
self.light_pub = rospy.Publisher('search_lights', std_msg.Bool, queue_size=1)
self.CAN_interface = util.CANInterface()
#get mover action clients
self.vfh_mover = actionlib.SimpleActionClient("vfh_move",
samplereturn_msg.VFHMoveAction)
self.simple_mover = actionlib.SimpleActionClient("simple_move",
samplereturn_msg.SimpleMoveAction)
#for this state machine, there is no preempt path. It either finshes successfully and
#reports success on the PursuitResult topic, or in case of any interupption or failure, it
#exits through the abort path, reporting failure on the topic.
self.state_machine = smach.StateMachine(
outcomes=['complete', 'preempted', 'aborted'],
input_keys = ['action_goal'],
output_keys = ['action_result'])
#tf frames
self.state_machine.userdata.odometry_frame = self.odometry_frame
self.state_machine.userdata.world_fixed_frame = self.world_fixed_frame
#userdata that subscribers fiddle with
self.state_machine.userdata.paused = False
self.state_machine.userdata.detected_sample = None
self.state_machine.userdata.target_sample = None
self.state_machine.userdata.pursuit_goal = None
#latched sample is the sample information returned from the servo controller, which
#is hopefully the identification made by the manipulator detector
self.state_machine.latched_sample = None
#pursuit params
self.max_pursuit_error = self.node_params.max_pursuit_error
self.min_pursuit_distance = self.node_params.min_pursuit_distance
self.state_machine.userdata.simple_pursuit_threshold = self.node_params.simple_pursuit_threshold
self.state_machine.userdata.pursuit_velocity = self.node_params.pursuit_velocity
self.state_machine.userdata.spin_velocity = self.node_params.spin_velocity
self.state_machine.userdata.final_pursuit_step = self.node_params.final_pursuit_step
self.state_machine.userdata.min_pursuit_distance = self.node_params.min_pursuit_distance
self.state_machine.userdata.max_pursuit_error = self.node_params.max_pursuit_error
self.state_machine.userdata.sample_yaw_tolerance = np.pi/4
self.sample_obstacle_check_width = self.node_params.sample_obstacle_check_width
#latched filter id is the id of the hypothesis that triggered this pursuit
#if it ever disappears or becomes impossible to get to, etc, publish failure
self.state_machine.userdata.latched_filter_id = None
self.state_machine.userdata.filter_changed = False
#stop function check flags
self.state_machine.userdata.check_pursuit_distance = False
self.state_machine.userdata.stop_on_detection = False
#area search and servo settings, search_count limits the number of times we can enter the
#sample_lost search pattern
self.state_machine.userdata.search_velocity = self.node_params.search_velocity
self.state_machine.userdata.settle_time = rospy.Duration(self.node_params.settle_time)
self.state_machine.userdata.manipulator_search_angle = math.pi/4
self.state_machine.userdata.search_count = 0
self.state_machine.userdata.search_try_limit = 2
self.state_machine.userdata.grab_count = 0
self.state_machine.userdata.grab_count_limit = 2
self.state_machine.userdata.manipulator_correction = self.node_params.manipulator_correction
self.state_machine.userdata.servo_params = self.node_params.servo_params
spiral_step = self.node_params.spiral_search_step
spiral_search_positions = [ {'x':spiral_step,'y':0},
{'x':spiral_step,'y':spiral_step},
{'x':-spiral_step,'y':spiral_step},
{'x':-spiral_step,'y':-spiral_step},
{'x':2*spiral_step,'y':-spiral_step},
{'x':2*spiral_step,'y':2*spiral_step},
{'x':-2*spiral_step,'y':2*spiral_step},
{'x':-2*spiral_step,'y':-2*spiral_step},
{'x':2*spiral_step,'y':-2*spiral_step} ]
self.state_machine.userdata.spiral_search_positions = spiral_search_positions
#allocate 10 bins from 1 to 10, bin 0 is the closed carousel position
self.state_machine.userdata.available_bins = self.node_params.available_bins
self.state_machine.userdata.active_bin_id = 0
#use these as booleans in remaps
self.state_machine.userdata.true = True
self.state_machine.userdata.false = False
#motion mode stuff
planner_mode = self.node_params.planner_mode
MODE_PLANNER = getattr(platform_srv.SelectMotionModeRequest, planner_mode)
MODE_SERVO = platform_srv.SelectMotionModeRequest.MODE_SERVO
with self.state_machine:
smach.StateMachine.add('START_SAMPLE_PURSUIT',
StartSamplePursuit(self.tf_listener),
transitions = {'goal':'ANNOUNCE_SAMPLE_PURSUIT',
'simple':'ANNOUNCE_SIMPLE_PURSUIT',
'aborted':'PUBLISH_FAILURE'})
smach.StateMachine.add('ANNOUNCE_SAMPLE_PURSUIT',
AnnounceState(self.announcer,
'Sample detected. Pursue ing'),
transitions = {'next':'APPROACH_SAMPLE'})
smach.StateMachine.add('ANNOUNCE_SIMPLE_PURSUIT',
AnnounceState(self.announcer,
'Sample detected. Position ing'),
transitions = {'next':'APPROACH_STRAFE'})
smach.StateMachine.add('APPROACH_STRAFE',
ExecuteSimpleMove(self.simple_mover),
transitions = {'complete':'FACE_SAMPLE',
'object_detected':'FACE_SAMPLE',
'preempted':'PUBLISH_FAILURE',
'aborted':'PUBLISH_FAILURE'},
remapping = {'simple_move':'approach_strafe',
'stop_on_detection':'false'})
smach.StateMachine.add('FACE_SAMPLE',
ExecuteSimpleMove(self.simple_mover),
transitions = {'complete':'ANNOUNCE_MANIPULATOR_APPROACH',
'object_detected':'ANNOUNCE_MANIPULATOR_APPROACH',
'preempted':'PUBLISH_FAILURE',
'aborted':'PUBLISH_FAILURE'},
remapping = {'simple_move':'face_sample',
'stop_on_detection':'false'})
smach.StateMachine.add('APPROACH_SAMPLE',
ExecuteVFHMove(self.vfh_mover),
transitions = {'complete':'ANNOUNCE_MANIPULATOR_APPROACH',
'blocked':'PUBLISH_FAILURE',
'started_blocked':'PUBLISH_FAILURE',
'missed_target':'ANNOUNCE_MANIPULATOR_APPROACH',
'off_course':'PUBLISH_FAILURE',
'object_detected':'PUBLISH_FAILURE',
'preempted':'PUBLISH_FAILURE',
'aborted':'PUBLISH_FAILURE'},
remapping = {'move_goal':'pursuit_goal'})
smach.StateMachine.add('ANNOUNCE_MANIPULATOR_APPROACH',
AnnounceState(self.announcer,
'Prepare ing final approach.'),
transitions = {'next':'CALCULATE_MANIPULATOR_APPROACH'})
#calculate the final strafe move to the sample, this gets us the yaw for the obstacle check
smach.StateMachine.add('CALCULATE_MANIPULATOR_APPROACH',
CalculateManipulatorApproach(self.tf_listener),
transitions = {'move':'OBSTACLE_CHECK',
'retry_approach':'ANNOUNCE_RETRY_APPROACH',
'point_lost':'ANNOUNCE_POINT_LOST',
'aborted':'PURSUE_SAMPLE_ABORTED'})
smach.StateMachine.add('ANNOUNCE_RETRY_APPROACH',
AnnounceState(self.announcer,
'Sample not in tolerance, retry ing.'),
transitions = {'next':'APPROACH_SAMPLE'})
@smach.cb_interface(outcomes=['clear','blocked'])
def obstacle_check_resp(userdata, response):
if response.obstacle:
return 'blocked'
else:
return 'clear'
#request_cb for obstacle check, gets the yaw from userdata
@smach.cb_interface(input_keys=['target_yaw', 'target_distance'])
def obstacle_check_req(userdata, request):
check_distance = self.sample_obstacle_check_width/2. + userdata.target_distance
return samplereturn_srv.ObstacleCheckRequest(yaw = userdata.target_yaw,
width = self.sample_obstacle_check_width,
distance = check_distance)
smach.StateMachine.add('OBSTACLE_CHECK',
smach_ros.ServiceState('obstacle_check', samplereturn_srv.ObstacleCheck,
request_cb = obstacle_check_req,
response_cb = obstacle_check_resp),
transitions = {'blocked':'ANNOUNCE_BLOCKED',
'clear':'ANNOUNCE_CLEAR',
'succeeded':'PUBLISH_FAILURE', #means cb error I think
'aborted':'PUBLISH_FAILURE'})
smach.StateMachine.add('ANNOUNCE_BLOCKED',
AnnounceState(self.announcer,
'Obstacles in sample area.'),
transitions = {'next':'PUBLISH_FAILURE'})
smach.StateMachine.add('ANNOUNCE_CLEAR',
AnnounceState(self.announcer,
'Area clear. Begin ing approach.'),
transitions = {'next':'MANIPULATOR_APPROACH_MOVE'})
smach.StateMachine.add('MANIPULATOR_APPROACH_MOVE',
ExecuteSimpleMove(self.simple_mover),
transitions = {'complete':'SEARCH_LIGHTS_ON',
'object_detected':'SEARCH_LIGHTS_ON',
'aborted':'PUBLISH_FAILURE'},
remapping = {'detection_message':'detected_sample',
'stop_on_detection':'false'})
smach.StateMachine.add('SEARCH_LIGHTS_ON',
PublishMessageState(self.light_pub),
transitions = {'next':'ANNOUNCE_SUN_POINTING'},
remapping = {'message':'true'})
smach.StateMachine.add('ENABLE_MANIPULATOR_DETECTOR',
smach_ros.ServiceState('enable_manipulator_detector',
samplereturn_srv.Enable,
request = samplereturn_srv.EnableRequest(True)),
transitions = {'succeeded':'ENABLE_MANIPULATOR_PROJECTOR',
'aborted':'PUBLISH_FAILURE'})
@smach.cb_interface(input_keys=['detected_sample'],
outcomes=['sample_detected'])
def enable_detector_cb(userdata, response):
timeout = rospy.Duration(2.0)
start = rospy.Time.now()
while (rospy.Time.now() - start) < timeout:
rospy.sleep(0.1)
if userdata.detected_sample is not None:
if userdata.detected_sample.header.stamp > start:
return 'sample_detected'
return 'succeeded'
smach.StateMachine.add('ENABLE_MANIPULATOR_PROJECTOR',
smach_ros.ServiceState('enable_manipulator_projector',
samplereturn_srv.Enable,
request = samplereturn_srv.EnableRequest(True),
response_cb = enable_detector_cb),
transitions = {'sample_detected':'VISUAL_SERVO',
'succeeded':'HANDLE_SEARCH',
'aborted':'PUBLISH_FAILURE'})
smach.StateMachine.add('ANNOUNCE_SUN_POINTING',
AnnounceState(self.announcer,
"Rotate ing to optimal light condition."),
transitions = {'next':'SUN_POINTING_ROTATION'})
smach.StateMachine.add('SUN_POINTING_ROTATION',
smach_ros.SimpleActionState('sun_pointing',
ComputeAngleAction,
goal = ComputeAngleGoal(True)),
transitions = {'succeeded':'ENABLE_MANIPULATOR_DETECTOR',
'preempted':'PUBLISH_FAILURE',
'aborted':'PUBLISH_FAILURE'})
smach.StateMachine.add('HANDLE_SEARCH',
HandleSearch(self.tf_listener, self.announcer),
transitions = {'sample_search':'HANDLE_SEARCH_MOVES',
'optimize_lighting':'ANNOUNCE_SUN_POINTING',
'no_sample':'PUBLISH_FAILURE'})
smach.StateMachine.add('HANDLE_SEARCH_MOVES',
MoveToPoints(self.tf_listener),
transitions = {'next_point':'SEARCH_MOVE',
'complete':'ANNOUNCE_SEARCH_FAILURE',
'aborted':'PUBLISH_FAILURE'},
remapping = {'velocity':'search_velocity'})
smach.StateMachine.add('SEARCH_MOVE',
ExecuteSimpleMove(self.simple_mover),
transitions = {'complete':'HANDLE_SEARCH_MOVES',
'object_detected':'VISUAL_SERVO',
'aborted':'PUBLISH_FAILURE'},
remapping = {'detection_message':'detected_sample',
'stop_on_detection':'true'})
smach.StateMachine.add('ANNOUNCE_SEARCH_FAILURE',
AnnounceState(self.announcer,
"Search complete, no sample found"),
transitions = {'next':'PUBLISH_FAILURE'})
#calculate the strafe move to the sample
smach.StateMachine.add('VISUAL_SERVO',
ServoController(self.tf_listener, self.announcer),
transitions = {'move':'SERVO_MOVE',
'complete':'ANNOUNCE_GRAB',
'point_lost':'HANDLE_SEARCH',
'aborted':'PUBLISH_FAILURE'})
smach.StateMachine.add('SERVO_MOVE',
ExecuteSimpleMove(self.simple_mover),
transitions = {'complete':'VISUAL_SERVO',
'object_detected':'VISUAL_SERVO',
'aborted':'PUBLISH_FAILURE'},
remapping = {'stop_on_detection':'false'})
smach.StateMachine.add('ANNOUNCE_GRAB',
AnnounceState(self.announcer,
"Deploy ing gripper"),
transitions = {'next':'GRAB_SAMPLE'})
@smach.cb_interface(input_keys=['latched_sample',
'available_bins'],
output_keys=['available_bins',
'active_bin_id'])
def grab_goal_cb(userdata, request):
goal = manipulator_msg.ManipulatorGoal()
goal.type = goal.GRAB
goal.wrist_angle = userdata.latched_sample.grip_angle
goal.target_bin = userdata.available_bins[0]
userdata.active_bin_id = goal.target_bin
goal.grip_torque = 0.8
rospy.loginfo("PURSUE_SAMPLE target_bin: {!s}, sample_id: {!s}, goal: {!s}".format(goal.target_bin,
userdata.latched_sample.sample_id,
goal))
rospy.loginfo("PURSUE_SAMPLE available_bins: {!s}".format(userdata.available_bins))
return goal
#if Steve pauses the robot during this action, it returns preempted,
#return to visual servo after pause, in case robot moved
smach.StateMachine.add('GRAB_SAMPLE',
smach_ros.SimpleActionState('manipulator_action',
manipulator_msg.ManipulatorAction,
goal_cb = grab_goal_cb),
transitions = {'succeeded':'CONFIRM_SAMPLE_ACQUIRED',
'preempted':'CHECK_PAUSE_STATE',
'aborted':'PUBLISH_FAILURE'})
smach.StateMachine.add('CONFIRM_SAMPLE_ACQUIRED',
ConfirmSampleAcquired(self.announcer, self.result_pub),
transitions = {'sample_gone':'DISABLE_MANIPULATOR_DETECTOR',
'sample_present':'VISUAL_SERVO',
'aborted':'PUBLISH_FAILURE'})
#if the grab action is preempted by shutdown (or other non-pause reason),
#exit pursue sample. If paused, wait for unpause
smach.StateMachine.add('CHECK_PAUSE_STATE',
WaitForFlagState('paused',
flag_trigger_value = True),
transitions = {'next':'WAIT_FOR_UNPAUSE',
'timeout':'PUBLISH_FAILURE',
'preempted':'PUBLISH_FAILURE'})
smach.StateMachine.add('WAIT_FOR_UNPAUSE',
WaitForFlagState('paused',
flag_trigger_value = False,
timeout = 20,
announcer = self.announcer,
start_message ='Pursuit paused, waiting for un pause'),
transitions = {'next':'VISUAL_SERVO',
'timeout':'WAIT_FOR_UNPAUSE',
'preempted':'PUBLISH_FAILURE'})
smach.StateMachine.add('ANNOUNCE_POINT_LOST',
AnnounceState(self.announcer,
"Sample lost."),
transitions = {'next':'PUBLISH_FAILURE'})
smach.StateMachine.add('PUBLISH_FAILURE',
PublishFailure(self.result_pub),
transitions = {'next':'DISABLE_MANIPULATOR_DETECTOR'})
#beginning of clean exit path
smach.StateMachine.add('DISABLE_MANIPULATOR_DETECTOR',
smach_ros.ServiceState('enable_manipulator_detector',
samplereturn_srv.Enable,
request = samplereturn_srv.EnableRequest(False)),
transitions = {'succeeded':'DISABLE_MANIPULATOR_PROJECTOR',
'aborted':'DISABLE_MANIPULATOR_PROJECTOR'})
smach.StateMachine.add('DISABLE_MANIPULATOR_PROJECTOR',
smach_ros.ServiceState('enable_manipulator_projector',
samplereturn_srv.Enable,
request = samplereturn_srv.EnableRequest(False)),
transitions = {'succeeded':'SEARCH_LIGHTS_OFF',
'aborted':'SEARCH_LIGHTS_OFF'})
smach.StateMachine.add('SEARCH_LIGHTS_OFF',
PublishMessageState(self.light_pub),
transitions = {'next':'ANNOUNCE_CONTINUE'},
remapping = {'message':'false'})
smach.StateMachine.add('ANNOUNCE_CONTINUE',
AnnounceState(self.announcer,
"Exit ing pursuit."),
transitions = {'next':'complete'})
smach.StateMachine.add('ANNOUNCE_RETURN',
AnnounceState(self.announcer,
"Exit ing pursuit."),
transitions = {'next':'complete'})
smach.StateMachine.add('PURSUE_SAMPLE_ABORTED',
PursueSampleAborted(self.result_pub),
transitions = {'next':'aborted'})
#action server wrapper
pursue_sample_server = smach_ros.ActionServerWrapper(
'pursue_sample', samplereturn_msg.GeneralExecutiveAction,
wrapped_container = self.state_machine,
succeeded_outcomes = ['complete'],
preempted_outcomes = ['preempted'],
aborted_outcomes = ['aborted'],
goal_key = 'action_goal',
result_key = 'action_result')
#introspection server
sls = smach_ros.IntrospectionServer('smach_grab_introspection',
self.state_machine,
'/START_SAMPLE_PURSUIT')
#subscribers, need to go after state_machine
rospy.Subscriber('detected_sample_search',
samplereturn_msg.NamedPoint,
self.sample_detection_search)
rospy.Subscriber('detected_sample_manipulator',
samplereturn_msg.NamedPoint,
self.sample_detection_manipulator)
rospy.Subscriber("pause_state", std_msg.Bool, self.pause_state_update)
#start action servers and services
sls.start()
pursue_sample_server.run_server()
rospy.spin()
sls.stop()
def __init__(self):
rospy.on_shutdown(self.shutdown_cb)
self.node_params = util.get_node_params()
self.tf_listener = tf.TransformListener()
self.executive_frame = self.node_params.executive_frame
self.beacon_approach_point = self.node_params.beacon_approach_point
#calculate beacon approach pose now, in /map
header = std_msg.Header(0, rospy.Time(0), '/map')
point = geometry_msg.Point(self.beacon_approach_point['x'],
self.beacon_approach_point['y'], 0)
quat_array = tf.transformations.quaternion_from_euler(0, 0, math.pi)
pose = geometry_msg.Pose(point, geometry_msg.Quaternion(*quat_array))
self.beacon_approach_pose = geometry_msg.PoseStamped(header, pose)
#interfaces
self.announcer = util.AnnouncerInterface("audio_navigate")
self.move_base = actionlib.SimpleActionClient("planner_move_base",
move_base_msg.MoveBaseAction)
self.CAN_interface = util.CANInterface()
start_time = rospy.get_time()
while not rospy.is_shutdown():
start_time = rospy.get_time()
if self.move_base.wait_for_server(rospy.Duration(0.1)):
break #all services up, exit this loop
if (rospy.get_time() - start_time) > 10.0:
self.announcer.say("Move base not available")
rospy.logwarn('Timeout waiting for move base')
rospy.sleep(0.1)
self.state_machine = smach.StateMachine(
outcomes=['complete', 'preempted', 'aborted'],
input_keys = ['action_goal'],
output_keys = ['action_result'])
#beacon approach params
self.state_machine.userdata.max_pursuit_error = self.node_params.max_pursuit_error
self.state_machine.userdata.min_pursuit_distance = self.node_params.min_pursuit_distance
self.state_machine.userdata.max_point_lost_time = self.node_params.max_point_lost_time
self.state_machine.userdata.beacon_approach_pose = self.beacon_approach_pose
#other parameters
self.state_machine.userdata.executive_frame = self.executive_frame
self.state_machine.userdata.motion_check_interval = self.node_params.motion_check_interval
self.state_machine.userdata.min_motion = self.node_params.min_motion
self.state_machine.userdata.search_poses_2d = self.node_params.search_poses_2d
self.state_machine.userdata.velocity = self.node_params.velocity
self.state_machine.userdata.search_velocity = self.node_params.search_velocity
#subscriber controlled userdata
self.state_machine.userdata.paused = False
self.state_machine.userdata.beacon_point = None
self.state_machine.userdata.detected_sample = None
#use these as booleans in remaps
self.state_machine.userdata.true = True
self.state_machine.userdata.false = False
#motion mode stuff
planner_mode = self.node_params.planner_mode
MODE_PLANNER = getattr(platform_srv.SelectMotionModeRequest, planner_mode)
MODE_ENABLE = platform_srv.SelectMotionModeRequest.MODE_ENABLE
with self.state_machine:
smach.StateMachine.add('START_LEVEL_ONE',
StartLevelOne(output_keys=['action_result',
'stop_on_sample',
'pose_list'],
input_keys=['search_poses_2d'],
outcomes=['next']),
transitions = {'next':'ANNOUNCE_LEVEL_ONE'})
smach.StateMachine.add('ANNOUNCE_LEVEL_ONE',
AnnounceState(self.announcer,
'Enter ing level one mode'),
transitions = {'next':'SELECT_PLANNER'})
smach.StateMachine.add('SELECT_PLANNER',
SelectMotionMode(self.CAN_interface,
MODE_PLANNER),
transitions = {'next':'DRIVE_TO_SEARCH_START',
'failed':'LEVEL_ONE_ABORTED'})
smach.StateMachine.add('DRIVE_TO_SEARCH_START',
DriveToSearch(self.announcer),
transitions = {'next_point':'DRIVE_TO_POSE',
'complete':'SEARCH_FOR_SAMPLE',
'preempted':'LEVEL_ONE_PREEMPTED'})
smach.StateMachine.add('DRIVE_TO_POSE',
DriveToPoseState(self.move_base, self.tf_listener),
transitions = {'complete':'DRIVE_TO_SEARCH_START',
'timeout':'DRIVE_TO_SEARCH_START',
'sample_detected':'PURSUE_SAMPLE'},
remapping = {'pursue_samples':'false'})
smach.StateMachine.add('SEARCH_FOR_SAMPLE',
DriveToPoseState(self.move_base, self.tf_listener),
transitions = {'complete':'ANNOUNCE_RETURN_HOME',
'timeout':'ANNOUNCE_RETURN_HOME',
'sample_detected':'PURSUE_SAMPLE'},
remapping = {'velocity':'search_velocity',
'pursue_samples':'true'})
@smach.cb_interface(input_keys=['detected_sample'])
def get_pursuit_goal_cb(userdata, request):
goal = samplereturn_msg.GeneralExecutiveGoal()
goal.input_point = userdata.detected_sample
goal.input_string = "level_one_pursuit_request"
return goal
smach.StateMachine.add('PURSUE_SAMPLE',
smach_ros.SimpleActionState('pursue_sample',
samplereturn_msg.GeneralExecutiveAction,
goal_cb = get_pursuit_goal_cb),
transitions = {'succeeded':'ANNOUNCE_RETURN_HOME',
'aborted':'ANNOUNCE_RETURN_HOME'})
smach.StateMachine.add('ANNOUNCE_RETURN_HOME',
AnnounceState(self.announcer,
'Moving to beacon approach point'),
transitions = {'next':'DRIVE_TO_BEACON_APPROACH_START'})
smach.StateMachine.add('DRIVE_TO_BEACON_APPROACH_START',
DriveToPoseState(self.move_base,
self.tf_listener),
transitions = {'complete':'ANNOUNCE_SEARCH_FOR_BEACON',
'timeout':'ANNOUNCE_RETURN_HOME',
'sample_detected':'ANNOUNCE_APPROACH_BEACON'},
remapping = {'target_pose':'beacon_approach_pose',
'detected_sample':'beacon_point',
'pursue_samples':'true',
'stop_on_sample':'false'})
smach.StateMachine.add('ANNOUNCE_APPROACH_BEACON',
AnnounceState(self.announcer,
'Beacon in view approach ing'),
transitions = {'next':'APPROACH_BEACON'})
self.approach_beacon = GetPursueDetectedPointState(self.move_base,
self.tf_listener)
smach.StateMachine.add('APPROACH_BEACON',
self.approach_beacon,
transitions = {'min_distance':'ANNOUNCE_MOUNT_PLATFORM',
'point_lost':'ANNOUNCE_RETURN_HOME',
'complete':'ANNOUNCE_RETURN_HOME',
'timeout':'ANNOUNCE_RETURN_HOME',
'aborted':'LEVEL_ONE_ABORTED'},
remapping = {'pursuit_point':'beacon_point',
'pursue_samples':'false',
'stop_on_sample':'false'})
smach.StateMachine.add('ANNOUNCE_MOUNT_PLATFORM',
AnnounceState(self.announcer,
'Mount ing platform'),
transitions = {'next':'MOUNT_PLATFORM'})
smach.StateMachine.add('MOUNT_PLATFORM',
DriveToPoseState(self.move_base,
self.tf_listener),
transitions = {'complete':'DESELECT_PLANNER',
'timeout':'ANNOUNCE_RETURN_HOME',
'sample_detected':'LEVEL_ONE_ABORTED'},
remapping = {'velocity':'search_velocity',
'pursue_samples':'false',
'stop_on_sample':'false'})
#ADD BEACON SEARCH!
smach.StateMachine.add('ANNOUNCE_SEARCH_FOR_BEACON',
AnnounceState(self.announcer,
'Beacon not in view, searching'),
transitions = {'next':'LEVEL_ONE_ABORTED'})
smach.StateMachine.add('DESELECT_PLANNER',
SelectMotionMode(self.CAN_interface,
MODE_ENABLE),
transitions = {'next':'complete',
'failed':'LEVEL_ONE_ABORTED'})
smach.StateMachine.add('LEVEL_ONE_PREEMPTED',
LevelOnePreempted(self.CAN_interface),
transitions = {'complete':'preempted',
'fail':'aborted'})
smach.StateMachine.add('LEVEL_ONE_ABORTED',
LevelOneAborted(),
transitions = {'recover':'SEARCH_FOR_SAMPLE',
'fail':'aborted'})
#action server wrapper
level_one_server = smach_ros.ActionServerWrapper(
'level_one', samplereturn_msg.GeneralExecutiveAction,
wrapped_container = self.state_machine,
succeeded_outcomes = ['complete'],
preempted_outcomes = ['preempted'],
aborted_outcomes = ['aborted'],
goal_key = 'action_goal',
result_key = 'action_result')
#introspection server
sls = smach_ros.IntrospectionServer('smach_grab_introspection',
self.state_machine,
'/START_LEVEL_ONE')
#subscribers, need to go after state_machine
rospy.Subscriber('detected_sample_search',
samplereturn_msg.NamedPoint,
self.sample_update)
rospy.Subscriber("beacon_pose",
geometry_msg.PoseStamped,
self.beacon_update)
rospy.Subscriber("pause_state", std_msg.Bool, self.pause_state_update)
#start action servers and services
sls.start()
level_one_server.run_server()
rospy.spin()
sls.stop()
def __init__(self):
rospy.on_shutdown(self.shutdown_cb)
self.shutdown=False
node_params = util.get_node_params()
self.publish_debug = node_params.publish_debug
self.odometry_frame = node_params.odometry_frame
self._mover = SimpleMover("~vfh_motion_params/")
self._tf = tf.TransformListener()
#action server stuff
self._as = actionlib.SimpleActionServer("vfh_move",
VFHMoveAction,
auto_start=False)
self._as.register_goal_callback(self.goal_cb)
self._as.register_preempt_callback(self.preempt_cb)
self._action_outcome = None
#goal flags and params
self._goal_orientation_tolerance = np.radians(node_params.goal_orientation_tolerance)
self._goal_obstacle_radius = node_params.goal_obstacle_radius
self._clear_distance = node_params.clear_distance
self._clear_first_angle = np.radians(node_params.clear_first_angle)
self._default_course_tolerance = node_params.course_tolerance
self._course_tolerance = node_params.course_tolerance
self._goal_odom = None
self._goal_local = None
self._target_point_odom = None
self._start_point = None
self._orient_at_target = False
self._rotate_to_clear = False
#costmap crap
self.costmap = None
self.costmap_listener = rospy.Subscriber('local_costmap',
nav_msg.OccupancyGrid,
self.handle_costmap)
self.costmap_info = None
self.costmap_header = None
#vfh flags
self.vfh_running = False
self.stop_requested = False
self.last_vfh_update = rospy.Time.now()
#VFH algorithm params
self._lethal_threshold = node_params.lethal_threshold
self.sector_angle = np.radians(node_params.sector_angle)
self.active_window = np.radians(node_params.active_window)
#cost coefficients
self.u_goal = node_params.u_goal #cost multiplier for angular difference to target_angle
self.u_current = node_params.u_current #cost multiplier for angular difference from current angle
#constants a and b: vfh guy say they should be set such that a-b*dmax = 0
#so that obstacles produce zero cost at a range of dmax. Seems like b=1 is fine...
#so I am replacing this with a=dmax
self.max_obstacle_distance = node_params.max_obstacle_distance #this is dmax
self.min_obstacle_distance = node_params.min_obstacle_distance #min. dist. to consider for obstacles
#setup sectors
sector_count = int(self.active_window/self.sector_angle) + 1
#offset from first sector in array to the robot zero sector
self.zero_offset = int(self.active_window/(self.sector_angle * 2))
#sectors are free if 0, or blocked if 1, start with all free!
self.vfh_sectors = np.zeros(sector_count, dtype=np.bool)
self.current_sector_index = 0 #start moving straight along the robot's yaw
#thresholds at which to set clear, or blocked, a gap for hysteresis
#I think lethal cells are set to 100 in costmap, so these thresholds will be kinda high
self.threshold_high = node_params.threshold_high
self.threshold_low = node_params.threshold_high
#debug marker stuff
self.debug_marker_pub = rospy.Publisher('vfh_markers',
vis_msg.MarkerArray,
queue_size=3)
self.marker_id = 0
#this loop controls the strafe angle using vfh sauce
rospy.Timer(rospy.Duration(0.2), self.vfh_planner)
if node_params.publish_search_area_check:
#this loop publishes true/false for search area check points
self.search_check_points = node_params.search_check_points
rospy.Timer(rospy.Duration(1.0/node_params.search_check_rate),
self.search_area_check)
self.search_area_check_pub = rospy.Publisher('search_area_check',
SearchAreaCheck,
queue_size=2)
#service to allow other nodes to look at spots in the costmap for obstacles
self.obstacle_check_service = rospy.Service('obstacle_check',
ObstacleCheck,
self.obstacle_check)
self._as.start()
