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()