class RandomPatrol(): def __init__(self): rospy.init_node('random_patrol', anonymous=False) # Set the shutdown function (stop the robot) rospy.on_shutdown(self.shutdown) # Initialize a number of parameters and variables setup_task_environment(self) # Initialize the patrol state machine self.sm_patrol = StateMachine( outcomes=['succeeded', 'aborted', 'preempted']) # Set the userdata.waypoints variable to the pre-defined waypoints self.sm_patrol.userdata.waypoints = self.waypoints # Add the states to the state machine with the appropriate transitions with self.sm_patrol: StateMachine.add('PICK_WAYPOINT', PickWaypoint(), transitions={'succeeded': 'NAV_WAYPOINT'}, remapping={'waypoint_out': 'patrol_waypoint'}) StateMachine.add('NAV_WAYPOINT', Nav2Waypoint(), transitions={ 'succeeded': 'PICK_WAYPOINT', 'aborted': 'PICK_WAYPOINT', 'preempted': 'PICK_WAYPOINT' }, remapping={'waypoint_in': 'patrol_waypoint'}) # Create and start the SMACH introspection server intro_server = IntrospectionServer('patrol', self.sm_patrol, '/SM_ROOT') intro_server.start() # Execute the state machine sm_outcome = self.sm_patrol.execute() rospy.loginfo('State Machine Outcome: ' + str(sm_outcome)) intro_server.stop() def shutdown(self): rospy.loginfo("Stopping the robot...") self.sm_patrol.request_preempt() self.cmd_vel_pub.publish(Twist()) rospy.sleep(1)
class RandomPatrol(): def __init__(self): rospy.init_node('random_patrol', anonymous=False) # Set the shutdown function (stop the robot) rospy.on_shutdown(self.shutdown) # Initialize a number of parameters and variables setup_task_environment(self) # Initialize the patrol state machine self.sm_patrol = StateMachine(outcomes=['succeeded','aborted','preempted']) # Set the userdata.waypoints variable to the pre-defined waypoints self.sm_patrol.userdata.waypoints = self.waypoints # Add the states to the state machine with the appropriate transitions with self.sm_patrol: StateMachine.add('PICK_WAYPOINT', PickWaypoint(), transitions={'succeeded':'NAV_WAYPOINT'}, remapping={'waypoint_out':'patrol_waypoint'}) StateMachine.add('NAV_WAYPOINT', Nav2Waypoint(), transitions={'succeeded':'PICK_WAYPOINT', 'aborted':'PICK_WAYPOINT', 'preempted':'PICK_WAYPOINT'}, remapping={'waypoint_in':'patrol_waypoint'}) # Create and start the SMACH introspection server intro_server = IntrospectionServer('patrol', self.sm_patrol, '/SM_ROOT') intro_server.start() # Execute the state machine sm_outcome = self.sm_patrol.execute() rospy.loginfo('State Machine Outcome: ' + str(sm_outcome)) intro_server.stop() def shutdown(self): rospy.loginfo("Stopping the robot...") self.sm_patrol.request_preempt() self.cmd_vel_pub.publish(Twist()) rospy.sleep(1)
class main: def __init__(self): rospy.init_node("follow") rospy.on_shutdown(self.shutdown) self.state = None self.sm_follow = StateMachine(outcomes=["succeeded", "aborted", "preempted"]) # 总状态机follow with self.sm_follow: self.co_follow = Concurrence( outcomes=["succeeded", "preempted", "aborted"], default_outcome="succeeded", # outcome_cb = self.co_follow_outcome_cb , # child_termination_cb=self.co_follow_child_cb ) with self.co_follow: Concurrence.add("mo_L", MonitorState("people_position_estimation", PositionMeasurement, self.pos_M_cb)) Concurrence.add("nav", Nav2Waypoint()) # self.sm_nav=StateMachine(outcomes=('succeeded','aborted','preempted')) # with self.sm_nav: # # StateMachine.add('wait',MonitorState('sr_data',Int16,self.wait_cb),transitions={'valid':'wait','invalid':'nav_speech','preempted':'nav_speech'}) # # self.speech_nav=Concurrence(outcomes=['get_in','succeeded'], # default_outcome='succeeded', # outcome_cb=self.speech_nav_outcome_cb, # child_termination_cb=self.speech_nav_child_termination_cb # ) # with self.speech_nav: # Concurrence.add('speech',MonitorState('sr_data',Int16,self.nav_speech_cb)) # Concurrence.add('nav', Nav2Waypoint()) # StateMachine.add('nav_speech',self.speech_nav,transitions={'get_in':'Get_in',}) # self.get_in_speech=Concurrence(outcomes=['get_out','succeeded'], # default_outcome='succeeded', # outcome_cb=self.speech_get_in_outcome_cb, # child_termination_cb=self.speech_get_in_child_termination_cb # # ) # with self.get_in_speech: # Concurrence.add('speech',MonitorState('sr_data',Int16,self.get_in_speech_cb)) # Concurrence.add('get_in',Get_in()) # StateMachine.add('Get_in',self.get_in_speech,transitions={'get_out':'Get_out'}) # StateMachine.add('Get_out',Get_out(), transitions={'succeeded':'nav_speech','preempted':'','aborted':'Get_out' }) # Concurrence.add('Nav',self.sm_nav) StateMachine.add("Nav_top", self.co_follow) a = self.sm_follow.execute() def pos_M_cb(self, userdata, msg): global current_people_pose, updata_flag # lock= threading.Lock() # with lock: if msg.pos is not None: current_people_pose = msg.pos updata_flag += 1 return True def wait_cb(self, userdata, msg): global FOLLOW if msg.data == FOLLOW: return False else: return True def nav_speech_cb(self, userdata, msg): global GET_IN, GET_OUT if msg.data == GET_IN: self.state = "get_in" return False else: return True def get_in_speech_cb(self, userdata, msg): global GET_IN, GET_OUT if msg.data == GET_OUT: self.state = "get_out" return False else: return True def speech_nav_outcome_cb(self, outcome_map): if outcome_map["speech"] == "invalid": if self.state == "get_in": return "get_in" def speech_nav_child_termination_cb(self, outcome_map): if outcome_map["speech"] == "invalid": return True def speech_get_in_outcome_cb(self, outcome_map): if outcome_map["speech"] == "invalid": if self.state == "get_out": return "get_out" def speech_get_in_child_termination_cb(self, outcome_map): if outcome_map["speech"] == "invalid": return True def shutdown(self): rospy.loginfo("Stopping the robot...") self.sm_follow.request_preempt() rospy.sleep(1) pass
class SMACHAI(): def __init__(self): rospy.init_node('petit_smach_ai', anonymous=False) # Set the shutdown function (stop the robot) rospy.on_shutdown(self.shutdown) # Create a list to hold the target quaternions (orientations) quaternions = list() # First define the corner orientations as Euler angles euler_angles = (0, pi, pi/2, -pi/2, pi/4, 0) # Then convert the angles to quaternions for angle in euler_angles: q_angle = quaternion_from_euler(0, 0, angle, axes='sxyz') q = Quaternion(*q_angle) quaternions.append(q) # Create a list to hold the waypoint poses self.waypoints = list() self.square_size = 1.0 # Append each of the four waypoints to the list. Each waypoint # is a pose consisting of a position and orientation in the map frame. self.waypoints.append(Pose(Point(-0.4, 0.65, 0.0), quaternions[0])) self.waypoints.append(Pose(Point(-1.0, 0.50, 0.0), quaternions[1])) self.waypoints.append(Pose(Point(-0.4, 0.35, 0.0), quaternions[2])) self.waypoints.append(Pose(Point(-0.8, 0.65, 0.0), quaternions[3])) self.waypoints.append(Pose(Point(-1.2, 0.30, 0.0), quaternions[4])) self.waypoints.append(Pose(Point(-0.4, 0.657, 0.0), quaternions[5])) # Publisher to manually control the robot (e.g. to stop it) self.cmd_vel_pub = rospy.Publisher('/PETIT/cmd_vel', Twist) self.stopping = False self.recharging = False self.robot_side = 1 # State machine for Actions self.sm_actions = StateMachine(outcomes=['succeeded','aborted','preempted']) with self.sm_actions: # Calib X StateMachine.add('CALIBRATE_X', CalibX(), transitions={'succeeded':'CALIBRATE_Y', 'aborted':'aborted'}) # Calib Y StateMachine.add('CALIBRATE_Y', CalibY(), transitions={'succeeded':'MAX_SPEED_1', 'aborted':'aborted'}) # PICK-UP # Change speed StateMachine.add('MAX_SPEED_1', MaxLinearSpeed(70000), transitions={'succeeded':'GOTO_1_1', 'aborted':'aborted'}) # NavPoint 1 StateMachine.add('GOTO_1_1', Nav2Waypoint(self.waypoints[0]), transitions={'succeeded':'MAX_SPEED_2', 'aborted':'aborted'}) # Change speed StateMachine.add('MAX_SPEED_2', MaxLinearSpeed(40000), transitions={'succeeded':'PAUSE_1', 'aborted':'aborted'}) # Pause StateMachine.add('PAUSE_1', Pause(1.0), transitions={'succeeded':'FORWARD_1', 'aborted':'aborted'}) # Avancer StateMachine.add('FORWARD_1', MoveForward(0.15), transitions={'succeeded':'FORKS_10', 'aborted':'aborted'}) # Monter fourches StateMachine.add('FORKS_10', Forks(90), transitions={'succeeded':'FORKS_11', 'aborted':'aborted'}) StateMachine.add('FORKS_11', Forks(87), transitions={'succeeded':'FORKS_12', 'aborted':'aborted'}) StateMachine.add('FORKS_12', Forks(83), transitions={'succeeded':'BACKWARD_1', 'aborted':'aborted'}) # Reculer StateMachine.add('BACKWARD_1', MoveForward(-0.15), transitions={'succeeded':'ROTATE_1', 'aborted':'aborted'}) # Rotation StateMachine.add('ROTATE_1', MoveRotate(pi), transitions={'succeeded':'MAX_SPEED_3', 'aborted':'aborted'}) # DROP-OFF # Change speed StateMachine.add('MAX_SPEED_3', MaxLinearSpeed(70000), transitions={'succeeded':'GOTO_2_1', 'aborted':'aborted'}) # NavPoint 2 StateMachine.add('GOTO_2_1', Nav2Waypoint(self.waypoints[1]), transitions={'succeeded':'MAX_SPEED_4', 'aborted':'aborted'}) # Change speed StateMachine.add('MAX_SPEED_4', MaxLinearSpeed(40000), transitions={'succeeded':'PAUSE_2', 'aborted':'aborted'}) # Pause StateMachine.add('PAUSE_2', Pause(1.0), transitions={'succeeded':'FORWARD_2', 'aborted':'aborted'}) # Avancer StateMachine.add('FORWARD_2', MoveForward(0.13), transitions={'succeeded':'FORKS_20', 'aborted':'aborted'}) # Baisser fourches StateMachine.add('FORKS_20', Forks(87), transitions={'succeeded':'FORKS_21', 'aborted':'aborted'}) StateMachine.add('FORKS_21', Forks(91), transitions={'succeeded':'FORKS_22', 'aborted':'aborted'}) StateMachine.add('FORKS_22', Forks(95), transitions={'succeeded':'BACKWARD_2', 'aborted':'aborted'}) # Reculer StateMachine.add('BACKWARD_2', MoveForward(-0.15), transitions={'succeeded':'ROTATE_2', 'aborted':'aborted'}) # Rotation StateMachine.add('ROTATE_2', MoveRotate(0), transitions={'succeeded':'GOTO_3_1', 'aborted':'aborted'}) # NavPoint 3 StateMachine.add('GOTO_3_1', Nav2Waypoint(self.waypoints[2]), transitions={'succeeded':'PAUSE_3', 'aborted':'aborted'}) # Pause StateMachine.add('PAUSE_3', Pause(2.0), transitions={'succeeded':'GOTO_4_1', 'aborted':'aborted'}) # NavPoint 4 StateMachine.add('GOTO_4_1', Nav2Waypoint(self.waypoints[3]), transitions={'succeeded':'PAUSE_4', 'aborted':'aborted'}) # Pause StateMachine.add('PAUSE_4', Pause(2.0), transitions={'succeeded':'GOTO_2_2', 'aborted':'aborted'}) # PICK-UP # NavPoint 2 StateMachine.add('GOTO_2_2', Nav2Waypoint(self.waypoints[1]), transitions={'succeeded':'PAUSE_5', 'aborted':'aborted'}) # Pause StateMachine.add('PAUSE_5', Pause(1.0), transitions={'succeeded':'FORWARD_3', 'aborted':'aborted'}) # Avancer StateMachine.add('FORWARD_3', MoveForward(0.15), transitions={'succeeded':'FORKS_30', 'aborted':'aborted'}) # Monter fourches StateMachine.add('FORKS_30', Forks(90), transitions={'succeeded':'FORKS_31', 'aborted':'aborted'}) StateMachine.add('FORKS_31', Forks(87), transitions={'succeeded':'FORKS_32', 'aborted':'aborted'}) StateMachine.add('FORKS_32', Forks(83), transitions={'succeeded':'BACKWARD_3', 'aborted':'aborted'}) # Reculer StateMachine.add('BACKWARD_3', MoveForward(-0.15), transitions={'succeeded':'ROTATE_3', 'aborted':'aborted'}) # Rotation StateMachine.add('ROTATE_3', MoveRotate(0), transitions={'succeeded':'GOTO_1_2', 'aborted':'aborted'}) # DROP-OFF # NavPoint 1 StateMachine.add('GOTO_1_2', Nav2Waypoint(self.waypoints[5]), transitions={'succeeded':'PAUSE_6', 'aborted':'aborted'}) # Pause StateMachine.add('PAUSE_6', Pause(1.0), transitions={'succeeded':'FORWARD_4', 'aborted':'aborted'}) # Avancer StateMachine.add('FORWARD_4', MoveForward(0.13), transitions={'succeeded':'FORKS_40', 'aborted':'aborted'}) # Baisser fourches StateMachine.add('FORKS_40', Forks(87), transitions={'succeeded':'FORKS_41', 'aborted':'aborted'}) StateMachine.add('FORKS_41', Forks(91), transitions={'succeeded':'FORKS_42', 'aborted':'aborted'}) StateMachine.add('FORKS_42', Forks(95), transitions={'succeeded':'BACKWARD_4', 'aborted':'aborted'}) # Reculer StateMachine.add('BACKWARD_4', MoveForward(-0.15), transitions={'succeeded':'ROTATE_4', 'aborted':'aborted'}) # Rotation StateMachine.add('ROTATE_4', MoveRotate(pi), transitions={'succeeded':'GOTO_5_1', 'aborted':'aborted'}) # NavPoint 5 StateMachine.add('GOTO_5_1', Nav2Waypoint(self.waypoints[4]), transitions={'succeeded':'CALIBRATE_X', 'aborted':'aborted'}) # Create the top level state machine self.sm_top = StateMachine(outcomes=['succeeded', 'aborted', 'preempted']) # Add nav_patrol, sm_recharge and a Stop() machine to sm_top with self.sm_top: StateMachine.add('ACTIONS', self.sm_actions, transitions={'succeeded':'ACTIONS', 'aborted':'STOP'}) #StateMachine.add('RECHARGE', self.sm_recharge, transitions={'succeeded':'PATROL'}) StateMachine.add('STOP', Stop(), transitions={'succeeded':''}) # Create and start the SMACH introspection server intro_server = IntrospectionServer('patrol', self.sm_top, '/SM_ROOT') intro_server.start() # Execute the state machine sm_outcome = self.sm_top.execute() rospy.loginfo('State Machine Outcome: ' + str(sm_outcome)) intro_server.stop() def time_cb(self, userdata, msg): if msg.data < 2: self.stopping = True return False else: self.stopping = False return True def start_cb(self, userdata, msg): rospy.loginfo("Start !") return False def color_cb(self, userdata, msg): rospy.loginfo("Color " + str(msg.data)) self.robot_side = msg.data self.sm_action1.userdata.robot_side = self.robot_side self.sm_action2.userdata.robot_side = self.robot_side self.sm_action3.userdata.robot_side = self.robot_side self.sm_action4.userdata.robot_side = self.robot_side self.sm_action5.userdata.robot_side = self.robot_side self.sm_action6.userdata.robot_side = self.robot_side self.sm_action7.userdata.robot_side = self.robot_side self.sm_top.userdata.robot_side = self.robot_side # TODO REMOVE return False def battery_cb(self, userdata, msg): if msg.data < 320: self.recharging = True return False else: self.recharging = False return True def objective_cb(self, userdata, response): #objective_response = GetObjective().Response userdata.waypoint_out = response.goal waypoint_type = response.type.data rospy.loginfo("goal: " + str(response.goal)) if(waypoint_type == 1): return 'action1' if(waypoint_type == 2): return 'action2' if(waypoint_type == 3): return 'action3' if(waypoint_type == 4): return 'action4' if(waypoint_type == 5): return 'action5' if(waypoint_type == 6): return 'action6' if(waypoint_type == 7): return 'action7' return 'aborted' def requestPrioCube_cb(self, userdata, request): rospy.loginfo("Side : " + str(userdata.robot_side)) update_request = UpdatePriorityRequest() update_request.goal.pose.position.x = userdata.robot_side*(1.500 - 1.300) update_request.goal.pose.position.y = 1.000 update_request.prio.data = 100 rospy.loginfo("Request Priority Drop cubes update") return update_request #request.goal.pose.position.x = userdata.robot_side*(1.500 - 1.300) #request.goal.pose.position.y = 1.000 #request.prio = 100 #return request def updatePrioCube_cb(self, userdata, response): rospy.loginfo("Priority Drop cubes updated") return 'aborted' def requestPrioShell_cb(self, userdata, request): rospy.loginfo("Side : " + str(userdata.robot_side)) update_request = UpdatePriorityRequest() update_request.goal.pose.position.x = userdata.robot_side*(1.500 - 0.300) update_request.goal.pose.position.y = 0.800 update_request.prio.data = 100 rospy.loginfo("Request Priority Drop shell update") return update_request #request.goal.pose.position.x = userdata.robot_side*(1.500 - 0.300) #request.goal.pose.position.y = 0.800 #request.prio = 100 #return request def updatePrioShell_cb(self, userdata, response): rospy.loginfo("Priority Drop shell updated") return 'aborted' # Gets called when ANY child state terminates def concurrence_child_termination_cb(self, outcome_map): # If the current navigation task has succeeded, return True if outcome_map['SM_ACTIONS'] == 'succeeded': return True # If the MonitorState state returns False (invalid), store the current nav goal and recharge if outcome_map['MONITOR_TIME'] == 'invalid': rospy.loginfo("LOW TIME! NEED TO STOP...") return True if outcome_map['MONITOR_BATTERY'] == 'invalid': rospy.loginfo("LOW BATTERY! NEED TO STOP...") return True else: return False # Gets called when ALL child states are terminated def concurrence_outcome_cb(self, outcome_map): # If the battery is below threshold, return the 'recharge' outcome if outcome_map['MONITOR_TIME'] == 'invalid': rospy.loginfo("TIME FINISHED !! GOING TO STOP ! ") return 'stop' if outcome_map['MONITOR_BATTERY'] == 'invalid': return 'stop' # Otherwise, if the last nav goal succeeded, return 'succeeded' or 'stop' elif outcome_map['SM_ACTIONS'] == 'succeeded': #self.patrol_count += 1 #rospy.loginfo("FINISHED PATROL LOOP: " + str(self.patrol_count)) # If we have not completed all our patrols, start again at the beginning #if self.n_patrols == -1 or self.patrol_count < self.n_patrols: #self.sm_nav.set_initial_state(['NAV_STATE_0'], UserData()) return 'succeeded' # Otherwise, we are finished patrolling so return 'stop' #else: #self.sm_nav.set_initial_state(['NAV_STATE_4'], UserData()) #return 'stop' # Recharge if all else fails else: return 'recharge' def shutdown(self): rospy.loginfo("Stopping the robot...") self.sm_actions.request_preempt() self.cmd_vel_pub.publish(Twist()) rospy.sleep(1)
class SMACHAI(): def __init__(self): rospy.init_node('petit_smach_ai', anonymous=False) # Set the shutdown function (stop the robot) rospy.on_shutdown(self.shutdown) # Create a list to hold the target quaternions (orientations) quaternions = list() # First define the corner orientations as Euler angles euler_angles = (pi/2, pi, 3*pi/2, 0) # Then convert the angles to quaternions for angle in euler_angles: q_angle = quaternion_from_euler(0, 0, angle, axes='sxyz') q = Quaternion(*q_angle) quaternions.append(q) # Create a list to hold the waypoint poses self.waypoints = list() self.square_size = 1.0 # Append each of the four waypoints to the list. Each waypoint # is a pose consisting of a position and orientation in the map frame. self.waypoints.append(Pose(Point(0.0, 0.0, 0.0), quaternions[3])) self.waypoints.append(Pose(Point(self.square_size, 0.0, 0.0), quaternions[0])) self.waypoints.append(Pose(Point(self.square_size, self.square_size, 0.0), quaternions[1])) self.waypoints.append(Pose(Point(0.0, self.square_size, 0.0), quaternions[2])) # Publisher to manually control the robot (e.g. to stop it) self.cmd_vel_pub = rospy.Publisher('/PETIT/cmd_vel', Twist) self.stopping = False self.recharging = False self.robot_side = 1 # State machine for Action1 self.sm_action1 = StateMachine(outcomes=['succeeded','aborted','preempted'], input_keys=['waypoint_in'], output_keys=['waypoint_out']) self.sm_action1.userdata.mandibles_sleep = 0.1 with self.sm_action1: StateMachine.add('OPEN_MANDIBLES', OpenMandibles(), transitions={'succeeded':'NAV_WAYPOINT', 'aborted':'aborted'}) StateMachine.add('NAV_WAYPOINT', Nav2Waypoint(), transitions={'succeeded':'UPDATE_DROPCUBE_OBJ', 'aborted':'aborted'}) # StateMachine.add('UPDATE_DROPCUBE_OBJ', UpdateObjectiveDropCubes(), # transitions={'succeeded':'CLOSE_MANDIBLES', # 'aborted':'aborted'}) StateMachine.add('UPDATE_DROPCUBE_OBJ', ServiceState('/PETIT/update_priority', UpdatePriority, request_cb=self.requestPrioCube_cb, response_cb=self.updatePrioCube_cb, output_keys=['waypoint_out'], input_keys=['robot_side']), transitions={'succeeded':'HCLOSE_MANDIBLES', 'aborted':'HCLOSE_MANDIBLES'}, remapping={'waypoint_out':'patrol_waypoint'}) StateMachine.add('HCLOSE_MANDIBLES', HalfCloseMandibles(), transitions={'succeeded':'succeeded', 'aborted':'aborted'}) # State machine for Action2 self.sm_action2 = StateMachine(outcomes=['succeeded','aborted','preempted'], input_keys=['waypoint_in'], output_keys=['waypoint_out']) self.sm_action2.userdata.speed = -0.1; self.sm_action2.userdata.distance = 0.3; self.sm_action2.userdata.mandibles_sleep = 0.1 with self.sm_action2: StateMachine.add('NAV_WAYPOINT', Nav2Waypoint(), transitions={'succeeded':'OPEN_MANDIBLES', 'aborted':'aborted'}) StateMachine.add('OPEN_MANDIBLES', OpenMandibles(), transitions={'succeeded':'MOVE_BACKWARD', 'aborted':'aborted'}) StateMachine.add('MOVE_BACKWARD', MoveForward(), transitions={'succeeded':'succeeded', 'aborted':'aborted'}) # State machine for Action3 self.sm_action3 = StateMachine(outcomes=['succeeded','aborted','preempted'], input_keys=['waypoint_in'], output_keys=['waypoint_out']) self.sm_action3.userdata.mandibles_sleep = 0.1 self.sm_action3.userdata.speed = -0.1; self.sm_action3.userdata.distance = 0.2; with self.sm_action3: StateMachine.add('NAV_WAYPOINT', Nav2Waypoint(), transitions={'succeeded':'MOVE_PUSH', 'aborted':'aborted'}) StateMachine.add('MOVE_PUSH', MovePush(), transitions={'succeeded':'succeeded', 'aborted':'aborted'}) # State machine for Action4 self.sm_action4 = StateMachine(outcomes=['succeeded','aborted','preempted'], input_keys=['waypoint_in'], output_keys=['waypoint_out']) self.sm_action4.userdata.mandibles_sleep = 0.1 self.sm_action4.userdata.speed_x = 0.1 self.sm_action4.userdata.speed_y = 0.1 self.sm_action4.userdata.distance = 0.5; with self.sm_action4: StateMachine.add('CLOSE_MANDIBLES', CloseMandibles(), transitions={'succeeded':'NAV_WAYPOINT', 'aborted':'aborted'}) StateMachine.add('NAV_WAYPOINT', Nav2Waypoint(), transitions={'succeeded':'SLIDE', 'aborted':'aborted'}) StateMachine.add('SLIDE', Slide(), transitions={'succeeded':'succeeded', 'aborted':'aborted'}) # State machine for Action5 self.sm_action5 = StateMachine(outcomes=['succeeded','aborted','preempted'], input_keys=['waypoint_in'], output_keys=['waypoint_out']) self.sm_action5.userdata.mandibles_sleep = 0.1 with self.sm_action5: StateMachine.add('OPEN_MANDIBLES', OpenMandibles(), transitions={'succeeded':'NAV_WAYPOINT', 'aborted':'aborted'}) StateMachine.add('NAV_WAYPOINT', Nav2Waypoint(), transitions={'succeeded':'UPDATE_DROPSHELL_OBJ', 'aborted':'aborted'}) # StateMachine.add('UPDATE_DROPSHELL_OBJ', UpdateObjectiveDropShell(), # transitions={'succeeded':'ALMOSTCLOSE_MANDIBLES', # 'aborted':'aborted'}) StateMachine.add('UPDATE_DROPSHELL_OBJ', ServiceState('/PETIT/update_priority', UpdatePriority, request_cb=self.requestPrioShell_cb, response_cb=self.updatePrioShell_cb, output_keys=['waypoint_out'], input_keys=['robot_side']), transitions={'succeeded':'ALMOSTCLOSE_MANDIBLES', 'aborted':'ALMOSTCLOSE_MANDIBLES'}) StateMachine.add('ALMOSTCLOSE_MANDIBLES', AlmostCloseMandibles(), transitions={'succeeded':'succeeded', 'aborted':'aborted'}) # State machine for Action6 self.sm_action6 = StateMachine(outcomes=['succeeded','aborted','preempted'], input_keys=['waypoint_in'], output_keys=['waypoint_out']) self.sm_action6.userdata.mandibles_sleep = 0.1 self.sm_action6.userdata.speed = 0.1; self.sm_action6.userdata.distance = 0.2; with self.sm_action6: StateMachine.add('OPEN_MANDIBLES', OpenMandibles(), transitions={'succeeded':'NAV_WAYPOINT', 'aborted':'aborted'}) StateMachine.add('NAV_WAYPOINT', Nav2Waypoint(), transitions={'succeeded':'FORWARD', 'aborted':'aborted'}) StateMachine.add('FORWARD', MoveForward(), transitions={'succeeded':'UPDATE_DROPSHELL_OBJ', 'aborted':'aborted'}) # StateMachine.add('UPDATE_DROPSHELL_OBJ', UpdateObjectiveDropShell(), # transitions={'succeeded':'HCLOSE_MANDIBLES', # 'aborted':'aborted'}) StateMachine.add('UPDATE_DROPSHELL_OBJ', ServiceState('/PETIT/update_priority', UpdatePriority, request_cb=self.requestPrioShell_cb, response_cb=self.updatePrioShell_cb, output_keys=['waypoint_out'], input_keys=['robot_side']), transitions={'succeeded':'HCLOSE_MANDIBLES', 'aborted':'HCLOSE_MANDIBLES'}) StateMachine.add('HCLOSE_MANDIBLES', HalfCloseMandibles(), transitions={'succeeded':'succeeded', 'aborted':'aborted'}) # State machine for Action7 self.sm_action7 = StateMachine(outcomes=['succeeded','aborted','preempted'], input_keys=['waypoint_in'], output_keys=['waypoint_out']) self.sm_action7.userdata.mandibles_sleep = 0.1 with self.sm_action7: StateMachine.add('NAV_WAYPOINT', Nav2Waypoint(), transitions={'succeeded':'succeeded', 'aborted':'aborted'}) # State machine for Actions self.sm_actions = StateMachine(outcomes=['succeeded','aborted','preempted']) self.sm_actions.userdata.waypoints = self.waypoints with self.sm_actions: StateMachine.add('PICK_WAYPOINT', ServiceState('/PETIT/get_objective', GetObjective, response_cb=self.objective_cb, output_keys=['waypoint_out'], outcomes=['action1','action2','action3','action4','action5','action6','action7','aborted','succeeded','preempted']), transitions={'action1':'SM_ACTION1','action2':'SM_ACTION2','action3':'SM_ACTION3','action4':'SM_ACTION4','action5':'SM_ACTION5','action6':'SM_ACTION6','action7':'SM_ACTION7','aborted':'SM_ACTION1'}, remapping={'waypoint_out':'patrol_waypoint'}) #StateMachine.add('PICK_WAYPOINT', PickWaypoint(), # transitions={'action1':'SM_ACTION1','action2':'SM_ACTION2','action3':'SM_ACTION3','action4':'SM_ACTION4','action5':'SM_ACTION5','action6':'SM_ACTION6','aborted':'SM_ACTION1'}, # remapping={'waypoint_out':'patrol_waypoint'}) StateMachine.add('SM_ACTION1', self.sm_action1, transitions={'succeeded':'REMOVE_OBJECTIVE', 'aborted':'aborted'}, remapping={'waypoint_in':'patrol_waypoint', 'waypoint_out':'remove_waypoint'}) StateMachine.add('SM_ACTION2', self.sm_action2, transitions={'succeeded':'REMOVE_OBJECTIVE', 'aborted':'aborted'}, remapping={'waypoint_in':'patrol_waypoint', 'waypoint_out':'remove_waypoint'}) StateMachine.add('SM_ACTION3', self.sm_action3, transitions={'succeeded':'REMOVE_OBJECTIVE', 'aborted':'aborted'}, remapping={'waypoint_in':'patrol_waypoint', 'waypoint_out':'remove_waypoint'}) StateMachine.add('SM_ACTION4', self.sm_action4, transitions={'succeeded':'REMOVE_OBJECTIVE', 'aborted':'aborted'}, remapping={'waypoint_in':'patrol_waypoint', 'waypoint_out':'remove_waypoint'}) StateMachine.add('SM_ACTION5', self.sm_action5, transitions={'succeeded':'REMOVE_OBJECTIVE', 'aborted':'aborted'}, remapping={'waypoint_in':'patrol_waypoint', 'waypoint_out':'remove_waypoint'}) StateMachine.add('SM_ACTION6', self.sm_action6, transitions={'succeeded':'REMOVE_OBJECTIVE', 'aborted':'aborted'}, remapping={'waypoint_in':'patrol_waypoint', 'waypoint_out':'remove_waypoint'}) StateMachine.add('SM_ACTION7', self.sm_action7, transitions={'succeeded':'REMOVE_OBJECTIVE', 'aborted':'aborted'}, remapping={'waypoint_in':'patrol_waypoint', 'waypoint_out':'remove_waypoint'}) StateMachine.add('REMOVE_OBJECTIVE', RemoveObjective(), transitions={'succeeded':'succeeded', 'aborted':'aborted'}, remapping={'waypoint_in':'remove_waypoint'}) # State machine with concurrence self.sm_concurrent = Concurrence(outcomes=['succeeded', 'stop'], default_outcome='succeeded', child_termination_cb=self.concurrence_child_termination_cb, outcome_cb=self.concurrence_outcome_cb) # Add the sm_actions machine and a battery MonitorState to the nav_patrol machine with self.sm_concurrent: Concurrence.add('SM_ACTIONS', self.sm_actions) Concurrence.add('MONITOR_TIME', MonitorState("/GENERAL/remain", Int32, self.time_cb)) Concurrence.add('MONITOR_BATTERY', MonitorState("/PETIT/adc", Int32, self.battery_cb)) # Create the top level state machine self.sm_top = StateMachine(outcomes=['succeeded', 'aborted', 'preempted']) # Add nav_patrol, sm_recharge and a Stop() machine to sm_top with self.sm_top: # @smach.cb_interface() # def requestPrioCube_cb(userdata, request): # update_request = UpdatePriority().Request # update_request.goal.pose.position.x = userdata.robot_side*(1.500 - 0.300) # update_request.goal.pose.position.y = 0.800 # update_request.prio = 100 # return update_request StateMachine.add('WAIT_COLOR', MonitorState("/GENERAL/color", Int32, self.color_cb), transitions={'valid':'WAIT_START', 'preempted':'WAIT_START', 'invalid':'WAIT_START'}) StateMachine.add('WAIT_START', MonitorState("/GENERAL/start", Empty, self.start_cb), transitions={'valid':'CONCURRENT', 'preempted':'CONCURRENT', 'invalid':'CONCURRENT'}) StateMachine.add('CONCURRENT', self.sm_concurrent, transitions={'succeeded':'CONCURRENT', 'stop':'STOP'}) #StateMachine.add('RECHARGE', self.sm_recharge, transitions={'succeeded':'PATROL'}) StateMachine.add('STOP', Stop(), transitions={'succeeded':''}) # Create and start the SMACH introspection server intro_server = IntrospectionServer('patrol', self.sm_top, '/SM_ROOT') intro_server.start() # Execute the state machine sm_outcome = self.sm_top.execute() rospy.loginfo('State Machine Outcome: ' + str(sm_outcome)) intro_server.stop() def time_cb(self, userdata, msg): if msg.data < 2: self.stopping = True return False else: self.stopping = False return True def start_cb(self, userdata, msg): rospy.loginfo("Start !") return False def color_cb(self, userdata, msg): rospy.loginfo("Color " + str(msg.data)) self.robot_side = msg.data self.sm_action1.userdata.robot_side = self.robot_side self.sm_action2.userdata.robot_side = self.robot_side self.sm_action3.userdata.robot_side = self.robot_side self.sm_action4.userdata.robot_side = self.robot_side self.sm_action5.userdata.robot_side = self.robot_side self.sm_action6.userdata.robot_side = self.robot_side self.sm_action7.userdata.robot_side = self.robot_side self.sm_top.userdata.robot_side = self.robot_side # TODO REMOVE return False def battery_cb(self, userdata, msg): if msg.data < 320: self.recharging = True return False else: self.recharging = False return True def objective_cb(self, userdata, response): #objective_response = GetObjective().Response userdata.waypoint_out = response.goal waypoint_type = response.type.data rospy.loginfo("goal: " + str(response.goal)) if(waypoint_type == 1): return 'action1' if(waypoint_type == 2): return 'action2' if(waypoint_type == 3): return 'action3' if(waypoint_type == 4): return 'action4' if(waypoint_type == 5): return 'action5' if(waypoint_type == 6): return 'action6' if(waypoint_type == 7): return 'action7' return 'aborted' def requestPrioCube_cb(self, userdata, request): rospy.loginfo("Side : " + str(userdata.robot_side)) update_request = UpdatePriorityRequest() update_request.goal.pose.position.x = userdata.robot_side*(1.500 - 1.300) update_request.goal.pose.position.y = 1.000 update_request.prio.data = 100 rospy.loginfo("Request Priority Drop cubes update") return update_request #request.goal.pose.position.x = userdata.robot_side*(1.500 - 1.300) #request.goal.pose.position.y = 1.000 #request.prio = 100 #return request def updatePrioCube_cb(self, userdata, response): rospy.loginfo("Priority Drop cubes updated") return 'aborted' def requestPrioShell_cb(self, userdata, request): rospy.loginfo("Side : " + str(userdata.robot_side)) update_request = UpdatePriorityRequest() update_request.goal.pose.position.x = userdata.robot_side*(1.500 - 0.300) update_request.goal.pose.position.y = 0.800 update_request.prio.data = 100 rospy.loginfo("Request Priority Drop shell update") return update_request #request.goal.pose.position.x = userdata.robot_side*(1.500 - 0.300) #request.goal.pose.position.y = 0.800 #request.prio = 100 #return request def updatePrioShell_cb(self, userdata, response): rospy.loginfo("Priority Drop shell updated") return 'aborted' # Gets called when ANY child state terminates def concurrence_child_termination_cb(self, outcome_map): # If the current navigation task has succeeded, return True if outcome_map['SM_ACTIONS'] == 'succeeded': return True # If the MonitorState state returns False (invalid), store the current nav goal and recharge if outcome_map['MONITOR_TIME'] == 'invalid': rospy.loginfo("LOW TIME! NEED TO STOP...") return True if outcome_map['MONITOR_BATTERY'] == 'invalid': rospy.loginfo("LOW BATTERY! NEED TO STOP...") return True else: return False # Gets called when ALL child states are terminated def concurrence_outcome_cb(self, outcome_map): # If the battery is below threshold, return the 'recharge' outcome if outcome_map['MONITOR_TIME'] == 'invalid': rospy.loginfo("TIME FINISHED !! GOING TO STOP ! ") return 'stop' if outcome_map['MONITOR_BATTERY'] == 'invalid': return 'stop' # Otherwise, if the last nav goal succeeded, return 'succeeded' or 'stop' elif outcome_map['SM_ACTIONS'] == 'succeeded': #self.patrol_count += 1 #rospy.loginfo("FINISHED PATROL LOOP: " + str(self.patrol_count)) # If we have not completed all our patrols, start again at the beginning #if self.n_patrols == -1 or self.patrol_count < self.n_patrols: #self.sm_nav.set_initial_state(['NAV_STATE_0'], UserData()) return 'succeeded' # Otherwise, we are finished patrolling so return 'stop' #else: #self.sm_nav.set_initial_state(['NAV_STATE_4'], UserData()) #return 'stop' # Recharge if all else fails else: return 'recharge' def shutdown(self): rospy.loginfo("Stopping the robot...") self.sm_actions.request_preempt() self.cmd_vel_pub.publish(Twist()) rospy.sleep(1)
class Main(): def __init__(self): rospy.init_node('operating_mode', anonymous=False) global mb mb = MongoBridge() # Set the shutdown function (stop the robot) rospy.on_shutdown(self.shutdown) # Create the top level SMACH state machine self.sm_top = StateMachine(outcomes=['stop']) # Open the container with self.sm_top: # ===== SETUP State ===== StateMachine.add('SETUP', Setup(), transitions={ 'succeeded': 'TOP_CONTROL', 'preempted': 'stop', 'aborted': 'stop' }) StateMachine.add('RECHARGE', Recharge(), transitions={ 'succeeded': 'SETUP', 'preempted': 'stop', 'aborted': 'stop' }) # ===== TOP_CONTROL State ===== self.sm_battery_concurrence = Concurrence( outcomes=[ 'restart', 'recharge', 'preempted', 'aborted', 'stop' ], default_outcome='recharge', child_termination_cb=self.child_termination_cb, outcome_cb=self.outcome_cb) self.sm_battery_concurrence.userdata.mode_change_infos = { 'mode': None, 'change_mode': False } # Open the container with self.sm_battery_concurrence: # Add states to the container Concurrence.add( 'BATTERY_MONITOR', MonitorState("/battery_level", Float32, self.battery_monitor_cb)) Concurrence.add( 'CHANGE_MODE_MONITOR', MonitorState("/change_mode", String, self.change_mode_monitor_cb, input_keys=['mode_change_infos'], output_keys=['mode_change_infos'])) self.operating_mode = StateMachine( outcomes=['aborted', 'preempted', 'standby'], input_keys=['mode_change_infos']) with self.operating_mode: StateMachine.add( 'MODE_MANAGEMENT', OperatingMode(), transitions={'succeeded': 'MODE_MANAGEMENT'}) Concurrence.add('OPERATING_MODE', self.operating_mode) StateMachine.add('TOP_CONTROL', self.sm_battery_concurrence, transitions={ 'restart': 'TOP_CONTROL', 'recharge': 'RECHARGE', 'aborted': 'stop', 'preempted': 'stop' }) # Create and start the SMACH introspection server intro_server = IntrospectionServer('mode', self.sm_top, '/SM_ROOT') intro_server.start() # Execute the state machine sm_outcome = self.sm_top.execute() rospy.loginfo('State Machine Outcome: ' + str(sm_outcome)) intro_server.stop() def child_termination_cb(self, outcome_map): if outcome_map['BATTERY_MONITOR'] == 'invalid' or \ outcome_map['CHANGE_MODE_MONITOR'] == 'invalid' or \ outcome_map['OPERATING_MODE'] == 'aborted' or \ outcome_map['OPERATING_MODE'] == 'preempted': return True else: return False def outcome_cb(self, outcome_map): if outcome_map['BATTERY_MONITOR'] == 'invalid': return 'recharge' elif outcome_map['CHANGE_MODE_MONITOR'] == 'invalid' or \ outcome_map['OPERATING_MODE'] == 'aborted' or \ outcome_map['OPERATING_MODE'] == 'preempted': return 'restart' else: return 'stop' def battery_monitor_cb(self, userdata, msg): if msg.data < 20.0: return False else: return True def change_mode_monitor_cb(self, userdata, msg): if msg.data: data = json.loads(msg.data) if not hasattr(self, '_last_msg_stamp' ) or self._last_msg_stamp != data['stamp']: print('Entrou!') mb.add_data({ 'class_name': 'mode', 'item_name': 'mode', 'data': data }) mb.send_data_and_wait() mode_id = pym.mode.find_one({}, {"_id": 1}, sort=[("data.timestamp", order_des) ])["_id"] if data.has_key( 'default_mode') and data['default_mode'] == True: mb.add_data({ 'class_name': 'mode', 'item_name': 'mode', 'params': { 'default_patrol': mode_id } }) mb.send_data_and_wait() userdata.mode_change_infos['mode'] = data['mode'] userdata.mode_change_infos['mode_id'] = mode_id userdata.mode_change_infos['change_mode'] = True self._last_msg_stamp = data['stamp'] return False else: return True def shutdown(self): rospy.loginfo("Stopping the robot...") self.sm_top.request_preempt() rospy.sleep(1)
class Patrol(): def __init__(self): rospy.init_node('patrol_smach', anonymous=False) # Set the shutdown function (stop the robot) rospy.on_shutdown(self.shutdown) # Initialize a number of parameters and variables setup_task_environment(self) # Track success rate of getting to the goal locations self.n_succeeded = 0 self.n_aborted = 0 self.n_preempted = 0 # A list to hold then navigation waypoints nav_states = list() # Turn the waypoints into SMACH states for waypoint in self.waypoints: nav_goal = MoveBaseGoal() nav_goal.target_pose.header.frame_id = 'map' nav_goal.target_pose.pose = waypoint move_base_state = SimpleActionState('move_base', MoveBaseAction, goal=nav_goal, result_cb=self.move_base_result_cb, exec_timeout=rospy.Duration(10.0), server_wait_timeout=rospy.Duration(10.0)) nav_states.append(move_base_state) # Initialize the patrol state machine self.sm_patrol = StateMachine(outcomes=['succeeded','aborted','preempted']) # Add the states to the state machine with the appropriate transitions with self.sm_patrol: StateMachine.add('NAV_STATE_0', nav_states[0], transitions={'succeeded':'NAV_STATE_1','aborted':'NAV_STATE_1','preempted':'NAV_STATE_1'}) StateMachine.add('NAV_STATE_1', nav_states[1], transitions={'succeeded':'NAV_STATE_2','aborted':'NAV_STATE_2','preempted':'NAV_STATE_2'}) StateMachine.add('NAV_STATE_2', nav_states[2], transitions={'succeeded':'NAV_STATE_3','aborted':'NAV_STATE_3','preempted':'NAV_STATE_3'}) StateMachine.add('NAV_STATE_3', nav_states[3], transitions={'succeeded':'NAV_STATE_4','aborted':'NAV_STATE_4','preempted':'NAV_STATE_4'}) StateMachine.add('NAV_STATE_4', nav_states[0], transitions={'succeeded':'','aborted':'','preempted':''}) # Create and start the SMACH introspection server intro_server = IntrospectionServer('patrol', self.sm_patrol, '/SM_ROOT') intro_server.start() # Execute the state machine for the specified number of patrols while (self.n_patrols == -1 or self.patrol_count < self.n_patrols) and not rospy.is_shutdown(): sm_outcome = self.sm_patrol.execute() self.patrol_count += 1 rospy.loginfo("FINISHED PATROL LOOP: " + str(self.patrol_count)) rospy.loginfo('State Machine Outcome: ' + str(sm_outcome)) intro_server.stop() def move_base_result_cb(self, userdata, status, result): if status == actionlib.GoalStatus.SUCCEEDED: self.n_succeeded += 1 elif status == actionlib.GoalStatus.ABORTED: self.n_aborted += 1 elif status == actionlib.GoalStatus.PREEMPTED: self.n_preempted += 1 try: rospy.loginfo("Success rate: " + str(100.0 * self.n_succeeded / (self.n_succeeded + self.n_aborted + self.n_preempted))) except: pass def shutdown(self): rospy.loginfo("Stopping the robot...") self.sm_patrol.request_preempt() self.cmd_vel_pub.publish(Twist()) rospy.sleep(1)
class History_Smach(): def __init__(self): rospy.init_node('explain_history_concurrence', anonymous=False) # 设置关闭机器人函数(stop the robot) rospy.on_shutdown(self.shutdown) # 初始化一些参数和变量 setup_task_environment(self) # 跟踪到达目标位置的成功率 self.n_succeeded = 0 self.n_aborted = 0 self.n_preempted = 0 # 保存上一个或者当前的导航目标点的变量 self.last_nav_state = None # 指示是否正在充电的标志 self.recharging = False # 保存导航目标点的列表 nav_states = {} # 把waypoints变成状态机的状态 for waypoint in self.room_locations.iterkeys(): nav_goal = MoveBaseGoal() nav_goal.target_pose.header.frame_id = 'map' nav_goal.target_pose.pose = self.room_locations[waypoint] move_base_state = SimpleActionState( 'move_base', MoveBaseAction, goal=nav_goal, result_cb=self.move_base_result_cb, exec_timeout=rospy.Duration(10.0), server_wait_timeout=rospy.Duration(10.0)) # nav_states.append(move_base_state) nav_states[waypoint] = move_base_state # 为扩展底座(docking station)创建一个MoveBaseAction state nav_goal = MoveBaseGoal() nav_goal.target_pose.header.frame_id = 'map' nav_goal.target_pose.pose = self.docking_station_pose nav_docking_station = SimpleActionState( 'move_base', MoveBaseAction, goal=nav_goal, result_cb=self.move_base_result_cb, exec_timeout=rospy.Duration(20.0), server_wait_timeout=rospy.Duration(10.0)) # 为written words子任务创建一个状态机 sm_written_words = StateMachine( outcomes=['succeeded', 'aborted', 'preempted']) # 然后添加子任务 with sm_written_words: StateMachine.add('EXPLAIN_HISTORY', WrittenWords('written_words', 5), transitions={ 'succeeded': '', 'aborted': '', 'preempted': '' }) # 为rule子任务创建一个状态机 sm_rule = StateMachine(outcomes=['succeeded', 'aborted', 'preempted']) # 然后添加子任务 with sm_rule: StateMachine.add('EXPLAIN_HISTORY', Rule('rule', 5), transitions={ 'succeeded': '', 'aborted': '', 'preempted': '' }) # 为life子任务创建一个状态机 sm_life = StateMachine(outcomes=['succeeded', 'aborted', 'preempted']) # 然后添加子任务 with sm_life: StateMachine.add('EXPLAIN_HISTORY', Life('life', 5), transitions={ 'succeeded': '', 'aborted': '', 'preempted': '' }) # 为faith子任务创建一个状态机 sm_faith = StateMachine(outcomes=['succeeded', 'aborted', 'preempted']) # 然后添加子任务 with sm_faith: StateMachine.add('EXPLAIN_HISTORY', Faith('faith', 5), transitions={ 'succeeded': '', 'aborted': '', 'preempted': '' }) # 初始化导航的状态机 self.sm_nav = StateMachine( outcomes=['succeeded', 'aborted', 'preempted']) # 使用transitions将导航的状态添加到状态机 with self.sm_nav: StateMachine.add('START', nav_states['explanatory_text'], transitions={ 'succeeded': 'WRITTEN_WORDS', 'aborted': 'WRITTEN_WORDS', 'preempted': 'WRITTEN_WORDS' }) ''' Add the living room subtask(s) ''' StateMachine.add('WRITTEN_WORDS', nav_states['explanatory_text'], transitions={ 'succeeded': 'WRITTEN_WORDS_TASKS', 'aborted': 'RULE', 'preempted': 'RULE' }) # 当任务完成时, 继续进行kitchen任务 StateMachine.add('WRITTEN_WORDS_TASKS', sm_written_words, transitions={ 'succeeded': 'RULE', 'aborted': 'RULE', 'preempted': 'RULE' }) ''' Add the kitchen subtask(s) ''' StateMachine.add('RULE', nav_states['explain_the_rule'], transitions={ 'succeeded': 'RULE_TASKS', 'aborted': 'LIFE', 'preempted': 'LIFE' }) # 当任务完成时, 继续进行bathroom任务 StateMachine.add('RULE_TASKS', sm_rule, transitions={ 'succeeded': 'LIFE', 'aborted': 'LIFE', 'preempted': 'LIFE' }) ''' Add the bathroom subtask(s) ''' StateMachine.add('LIFE', nav_states['explain_life'], transitions={ 'succeeded': 'LIFE_TASKS', 'aborted': 'FAITH', 'preempted': 'FAITH' }) # 当任务完成时, 继续进行hallway任务 StateMachine.add('LIFE_TASKS', sm_life, transitions={ 'succeeded': 'FAITH', 'aborted': 'FAITH', 'preempted': 'FAITH' }) ''' Add the hallway subtask(s) ''' StateMachine.add('FAITH', nav_states['explain_faith'], transitions={ 'succeeded': 'FAITH_TASKS', 'aborted': '', 'preempted': '' }) # 当任务完成时, stop StateMachine.add('FAITH_TASKS', sm_faith, transitions={ 'succeeded': '', 'aborted': '', 'preempted': '' }) # 在sm_nav状态机中注册一个回调函数以启动状态转换(state transitions) self.sm_nav.register_transition_cb(self.nav_transition_cb, cb_args=[]) # 初始化充电的状态机 self.sm_recharge = StateMachine( outcomes=['succeeded', 'aborted', 'preempted']) with self.sm_recharge: StateMachine.add('NAV_DOCKING_STATION', nav_docking_station, transitions={'succeeded': 'RECHARGE_BATTERY'}) StateMachine.add('RECHARGE_BATTERY', ServiceState( 'battery_simulator/set_battery_level', SetBatteryLevel, 100, response_cb=self.recharge_cb), transitions={'succeeded': ''}) # 使用并发容器(Concurrence container)创建nav_patrol状态机 self.nav_patrol = Concurrence( outcomes=['succeeded', 'recharge', 'stop'], default_outcome='succeeded', child_termination_cb=self.concurrence_child_termination_cb, outcome_cb=self.concurrence_outcome_cb) # 将sm_nav machine和battery MonitorState添加到nav_patrol状态机里面 with self.nav_patrol: Concurrence.add('SM_NAV', self.sm_nav) Concurrence.add( 'MONITOR_BATTERY', MonitorState("battery_level", Float32, self.battery_cb)) # 创建顶层状态机 self.sm_top = StateMachine( outcomes=['succeeded', 'aborted', 'preempted']) # 将nav_patrol,sm_recharge和Stop添加到sm_top状态机 with self.sm_top: StateMachine.add('PATROL', self.nav_patrol, transitions={ 'succeeded': 'PATROL', 'recharge': 'RECHARGE', 'stop': 'STOP' }) StateMachine.add('RECHARGE', self.sm_recharge, transitions={'succeeded': 'PATROL'}) StateMachine.add('STOP', Stop(), transitions={'succeeded': ''}) # 创建并开始SMACH introspection server intro_server = IntrospectionServer('patrol', self.sm_top, '/SM_ROOT') intro_server.start() # 运行状态机 sm_outcome = self.sm_top.execute() rospy.loginfo('State Machine Outcome: ' + str(sm_outcome)) intro_server.stop() def nav_transition_cb(self, userdata, active_states, *cb_args): self.last_nav_state = active_states # 当任何子状态终止时调用 def concurrence_child_termination_cb(self, outcome_map): # 如果当前导航任务完成, return True if outcome_map['SM_NAV'] == 'succeeded': return True # 如果MonitorState状态变成False则储存当前的导航目标点并充电 if outcome_map['MONITOR_BATTERY'] == 'invalid': rospy.loginfo("LOW BATTERY! NEED TO RECHARGE...") if self.last_nav_state is not None: self.sm_nav.set_initial_state(self.last_nav_state, UserData()) return True else: return False # 当任何子状态终止时调用 def concurrence_outcome_cb(self, outcome_map): # 如果电池电量低于设定的阈值,返回'recharge' outcome if outcome_map['MONITOR_BATTERY'] == 'invalid': return 'recharge' # 否则,如果最后一个导航目标点成功,返回'succeeded' 或者 'stop' elif outcome_map['SM_NAV'] == 'succeeded': self.patrol_count += 1 rospy.loginfo("FINISHED PATROL LOOP: " + str(self.patrol_count)) # 如果没有完成所有的巡逻,重新开始导航 if self.n_patrols == -1 or self.patrol_count < self.n_patrols: # self.sm_nav.set_initial_state(['NAV_STATE_0'], UserData()) return 'succeeded' # 否则,完成所有导航并返回 'stop' else: # self.sm_nav.set_initial_state(['NAV_STATE_4'], UserData()) return 'stop' # 如果其他操作失败了,重新充电 else: return 'recharge' def battery_cb(self, userdata, msg): if msg.data < self.low_battery_threshold: self.recharging = True return False else: self.recharging = False return True def recharge_cb(self, userdata, response): return 'succeeded' def move_base_result_cb(self, userdata, status, result): if not self.recharging: if status == actionlib.GoalStatus.SUCCEEDED: self.n_succeeded += 1 elif status == actionlib.GoalStatus.ABORTED: self.n_aborted += 1 elif status == actionlib.GoalStatus.PREEMPTED: self.n_preempted += 1 try: rospy.loginfo("Success rate: " + str(100.0 * self.n_succeeded / (self.n_succeeded + self.n_aborted + self.n_preempted))) except: pass def shutdown(self): rospy.loginfo("Stopping the robot...") self.sm_nav.request_preempt() self.cmd_vel_pub.publish(Twist()) rospy.sleep(1)
class main(): def __init__(self): rospy.init_node('patrol_smach', anonymous=False) # Set the shutdown function (stop the robot) rospy.on_shutdown(self.shutdown) # Initialize a number of parameters and variables self.init() # Subscribe to the move_base action server self.move_base = actionlib.SimpleActionClient("move_base", MoveBaseAction) rospy.loginfo("Waiting for move_base action server...") # Wait up to 60 seconds for the action server to become available self.move_base.wait_for_server(rospy.Duration(60)) rospy.loginfo("Connected to move_base action server") # Track success rate of getting to the goal locations self.n_succeeded = 0 self.n_aborted = 0 self.n_preempted = 0 self.n_patrols = 2 # Turn the waypoints into SMACH states nav_states = list() for waypoint in self.waypoints: nav_goal = MoveBaseGoal() nav_goal.target_pose.header.frame_id = 'map' nav_goal.target_pose.pose = waypoint move_base_state = SimpleActionState( 'move_base', MoveBaseAction, goal=nav_goal, result_cb=self.move_base_result_cb, exec_timeout=rospy.Duration(10.0), server_wait_timeout=rospy.Duration(10.0)) nav_states.append(move_base_state) move_base_state = SimpleActionState( 'move_base', MoveBaseAction, goal=nav_goal, result_cb=self.move_base_result_cb, exec_timeout=rospy.Duration(10.0)) # Initialize the top level state machine self.sm = StateMachine(outcomes=['succeeded', 'aborted', 'preempted']) with self.sm: # Initialize the iterator self.sm_patrol_iterator = Iterator( outcomes=['succeeded', 'preempted', 'aborted'], input_keys=[], it=lambda: range(0, self.n_patrols), output_keys=[], it_label='index', exhausted_outcome='succeeded') with self.sm_patrol_iterator: # Initialize the patrol state machine self.sm_patrol = StateMachine( outcomes=['succeeded', 'aborted', 'preempted', 'continue']) # Add the states to the state machine with the appropriate transitions with self.sm_patrol: StateMachine.add('NAV_STATE_0', nav_states[0], transitions={ 'succeeded': 'NAV_STATE_1', 'aborted': 'NAV_STATE_1', 'preempted': 'NAV_STATE_1' }) StateMachine.add('NAV_STATE_1', nav_states[1], transitions={ 'succeeded': 'NAV_STATE_2', 'aborted': 'NAV_STATE_2', 'preempted': 'NAV_STATE_2' }) StateMachine.add('NAV_STATE_2', nav_states[2], transitions={ 'succeeded': 'NAV_STATE_3', 'aborted': 'NAV_STATE_3', 'preempted': 'NAV_STATE_3' }) StateMachine.add('NAV_STATE_3', nav_states[3], transitions={ 'succeeded': 'NAV_STATE_4', 'aborted': 'NAV_STATE_4', 'preempted': 'NAV_STATE_4' }) StateMachine.add('NAV_STATE_4', nav_states[0], transitions={ 'succeeded': 'continue', 'aborted': 'continue', 'preempted': 'continue' }) # Close the sm_patrol machine and add it to the iterator Iterator.set_contained_state('PATROL_STATE', self.sm_patrol, loop_outcomes=['continue']) # Close the top level state machine StateMachine.add('PATROL_ITERATOR', self.sm_patrol_iterator, { 'succeeded': 'succeeded', 'aborted': 'aborted' }) # Create and start the SMACH introspection server intro_server = IntrospectionServer('patrol', self.sm, '/SM_ROOT') intro_server.start() # Execute the state machine sm_outcome = self.sm.execute() rospy.loginfo('State Machine Outcome: ' + str(sm_outcome)) intro_server.stop() def move_base_result_cb(self, userdata, status, result): if status == actionlib.GoalStatus.SUCCEEDED: self.n_succeeded += 1 elif status == actionlib.GoalStatus.ABORTED: self.n_aborted += 1 elif status == actionlib.GoalStatus.PREEMPTED: self.n_preempted += 1 try: rospy.loginfo( "Success rate: " + str(100.0 * self.n_succeeded / (self.n_succeeded + self.n_aborted + self.n_preempted))) except: pass def init(self): # How big is the square we want the robot to patrol? self.square_size = rospy.get_param("~square_size", 1.0) # meters # How many times should we execute the patrol loop self.n_patrols = rospy.get_param("~n_patrols", 3) # meters # Create a list to hold the target quaternions (orientations) quaternions = list() # First define the corner orientations as Euler angles euler_angles = (pi / 2, pi, 3 * pi / 2, 0) # Then convert the angles to quaternions for angle in euler_angles: q_angle = quaternion_from_euler(0, 0, angle, axes='sxyz') q = Quaternion(*q_angle) quaternions.append(q) # Create a list to hold the waypoint poses self.waypoints = list() # Append each of the four waypoints to the list. Each waypoint # is a pose consisting of a position and orientation in the map frame. self.waypoints.append(Pose(Point(0.0, 0.0, 0.0), quaternions[3])) self.waypoints.append( Pose(Point(self.square_size, 0.0, 0.0), quaternions[0])) self.waypoints.append( Pose(Point(self.square_size, self.square_size, 0.0), quaternions[1])) self.waypoints.append( Pose(Point(0.0, self.square_size, 0.0), quaternions[2])) # Initialize the waypoint visualization markers for RViz self.init_waypoint_markers() # Set a visualization marker at each waypoint for waypoint in self.waypoints: p = Point() p = waypoint.position self.waypoint_markers.points.append(p) # Publisher to manually control the robot (e.g. to stop it) self.cmd_vel_pub = rospy.Publisher('cmd_vel', Twist) rospy.loginfo("Starting SMACH test") # Publish the waypoint markers self.marker_pub.publish(self.waypoint_markers) rospy.sleep(1) self.marker_pub.publish(self.waypoint_markers) def init_waypoint_markers(self): # Set up our waypoint markers marker_scale = 0.2 marker_lifetime = 0 # 0 is forever marker_ns = 'waypoints' marker_id = 0 marker_color = {'r': 1.0, 'g': 0.7, 'b': 1.0, 'a': 1.0} # Define a marker publisher. self.marker_pub = rospy.Publisher('waypoint_markers', Marker) # Initialize the marker points list. self.waypoint_markers = Marker() self.waypoint_markers.ns = marker_ns self.waypoint_markers.id = marker_id self.waypoint_markers.type = Marker.CUBE_LIST self.waypoint_markers.action = Marker.ADD self.waypoint_markers.lifetime = rospy.Duration(marker_lifetime) self.waypoint_markers.scale.x = marker_scale self.waypoint_markers.scale.y = marker_scale self.waypoint_markers.color.r = marker_color['r'] self.waypoint_markers.color.g = marker_color['g'] self.waypoint_markers.color.b = marker_color['b'] self.waypoint_markers.color.a = marker_color['a'] self.waypoint_markers.header.frame_id = 'odom' self.waypoint_markers.header.stamp = rospy.Time.now() self.waypoint_markers.points = list() def shutdown(self): rospy.loginfo("Stopping the robot...") self.sm_patrol.request_preempt() self.cmd_vel_pub.publish(Twist()) rospy.sleep(1)
class RandomPatrol(): def __init__(self): rospy.init_node('random_patrol', anonymous=False) # Set the shutdown function (stop the robot) rospy.on_shutdown(self.shutdown) # Initialize a number of parameters and variables #setup_task_environment(self) # Create a list to hold the target quaternions (orientations) quaternions = list() # First define the corner orientations as Euler angles euler_angles = (pi / 2, pi, 3 * pi / 2, 0) # Then convert the angles to quaternions for angle in euler_angles: q_angle = quaternion_from_euler(0, 0, angle, axes='sxyz') q = Quaternion(*q_angle) quaternions.append(q) # Create a list to hold the waypoint poses self.waypoints = list() self.square_size = 1.0 # Append each of the four waypoints to the list. Each waypoint # is a pose consisting of a position and orientation in the map frame. self.waypoints.append(Pose(Point(0.0, 0.0, 0.0), quaternions[3])) self.waypoints.append( Pose(Point(self.square_size, 0.0, 0.0), quaternions[0])) self.waypoints.append( Pose(Point(self.square_size, self.square_size, 0.0), quaternions[1])) self.waypoints.append( Pose(Point(0.0, self.square_size, 0.0), quaternions[2])) # Publisher to manually control the robot (e.g. to stop it) self.cmd_vel_pub = rospy.Publisher('/PETIT/cmd_vel', Twist) self.stopping = False self.recharging = False # Initialize the patrol state machine self.sm_patrol = StateMachine( outcomes=['succeeded', 'aborted', 'preempted']) # Set the userdata.waypoints variable to the pre-defined waypoints self.sm_patrol.userdata.waypoints = self.waypoints # Add the states to the state machine with the appropriate transitions with self.sm_patrol: StateMachine.add('PICK_WAYPOINT', PickWaypoint(), transitions={ 'succeeded': 'NAV_WAYPOINT', 'aborted': 'NAV_WAYPOINT' }, remapping={'waypoint_out': 'patrol_waypoint'}) StateMachine.add('NAV_WAYPOINT', Nav2Waypoint(), transitions={ 'succeeded': '', 'aborted': '' }, remapping={'waypoint_in': 'patrol_waypoint'}) # Create the nav_patrol state machine using a Concurrence container self.sm_concurrent = Concurrence( outcomes=['succeeded', 'stop'], default_outcome='succeeded', child_termination_cb=self.concurrence_child_termination_cb, outcome_cb=self.concurrence_outcome_cb) # Add the sm_nav machine and a battery MonitorState to the nav_patrol machine with self.sm_concurrent: Concurrence.add('SM_NAV', self.sm_patrol) Concurrence.add( 'MONITOR_TIME', MonitorState("/GENERAL/remain", Int32, self.time_cb)) #Concurrence.add('MONITOR_BATTERY', MonitorState("/PETIT/adc", Int32, self.battery_cb)) # Create the top level state machine self.sm_top = StateMachine( outcomes=['succeeded', 'aborted', 'preempted']) # Add nav_patrol, sm_recharge and a Stop() machine to sm_top with self.sm_top: StateMachine.add('CONCURRENT', self.sm_concurrent, transitions={ 'succeeded': 'CONCURRENT', 'stop': 'STOP' }) #StateMachine.add('RECHARGE', self.sm_recharge, transitions={'succeeded':'PATROL'}) StateMachine.add('STOP', Stop(), transitions={'succeeded': ''}) # Create and start the SMACH introspection server intro_server = IntrospectionServer('patrol', self.sm_top, '/SM_ROOT') intro_server.start() # Execute the state machine sm_outcome = self.sm_top.execute() rospy.loginfo('State Machine Outcome: ' + str(sm_outcome)) intro_server.stop() def time_cb(self, userdata, msg): if msg.data < 2: self.stopping = True return False else: self.stopping = False return True def battery_cb(self, userdata, msg): if msg.data < 320: self.recharging = True return False else: self.recharging = False return True # Gets called when ANY child state terminates def concurrence_child_termination_cb(self, outcome_map): # If the current navigation task has succeeded, return True if outcome_map['SM_NAV'] == 'succeeded': return True # If the MonitorState state returns False (invalid), store the current nav goal and recharge if outcome_map['MONITOR_TIME'] == 'invalid': rospy.loginfo("LOW TIME! NEED TO STOP...") return True else: return False # Gets called when ALL child states are terminated def concurrence_outcome_cb(self, outcome_map): # If the battery is below threshold, return the 'recharge' outcome if outcome_map['MONITOR_TIME'] == 'invalid': rospy.loginfo("TIME FINISHED !! GOING TO STOP ! ") return 'stop' #if outcome_map['MONITOR_BATTERY'] == 'invalid': #return 'recharge' # Otherwise, if the last nav goal succeeded, return 'succeeded' or 'stop' elif outcome_map['SM_NAV'] == 'succeeded': #self.patrol_count += 1 #rospy.loginfo("FINISHED PATROL LOOP: " + str(self.patrol_count)) # If we have not completed all our patrols, start again at the beginning #if self.n_patrols == -1 or self.patrol_count < self.n_patrols: #self.sm_nav.set_initial_state(['NAV_STATE_0'], UserData()) return 'succeeded' # Otherwise, we are finished patrolling so return 'stop' #else: #self.sm_nav.set_initial_state(['NAV_STATE_4'], UserData()) #return 'stop' # Recharge if all else fails else: return 'recharge' def shutdown(self): rospy.loginfo("Stopping the robot...") self.sm_patrol.request_preempt() self.cmd_vel_pub.publish(Twist()) rospy.sleep(1)
class Patrol: def __init__(self): rospy.init_node("patrol_smach", anonymous=False) # Set the shutdown function (stop the robot) rospy.on_shutdown(self.shutdown) # Initialize a number of parameters and variables setup_task_environment(self) # Track success rate of getting to the goal locations self.n_succeeded = 0 self.n_aborted = 0 self.n_preempted = 0 # A list to hold then navigation waypoints nav_states = list() # Turn the waypoints into SMACH states for waypoint in self.waypoints: nav_goal = MoveBaseGoal() nav_goal.target_pose.header.frame_id = "map" nav_goal.target_pose.pose = waypoint move_base_state = SimpleActionState( "move_base", MoveBaseAction, goal=nav_goal, result_cb=self.move_base_result_cb, exec_timeout=rospy.Duration(10.0), server_wait_timeout=rospy.Duration(10.0), ) nav_states.append(move_base_state) # Initialize the patrol state machine self.sm_patrol = StateMachine(outcomes=["succeeded", "aborted", "preempted"]) # Add the states to the state machine with the appropriate transitions with self.sm_patrol: StateMachine.add( "NAV_STATE_0", nav_states[0], transitions={"succeeded": "NAV_STATE_1", "aborted": "NAV_STATE_1", "preempted": "NAV_STATE_1"}, ) StateMachine.add( "NAV_STATE_1", nav_states[1], transitions={"succeeded": "NAV_STATE_2", "aborted": "NAV_STATE_2", "preempted": "NAV_STATE_2"}, ) StateMachine.add( "NAV_STATE_2", nav_states[2], transitions={"succeeded": "NAV_STATE_3", "aborted": "NAV_STATE_3", "preempted": "NAV_STATE_3"}, ) StateMachine.add( "NAV_STATE_3", nav_states[3], transitions={"succeeded": "NAV_STATE_4", "aborted": "NAV_STATE_4", "preempted": "NAV_STATE_4"}, ) StateMachine.add( "NAV_STATE_4", nav_states[0], transitions={"succeeded": "", "aborted": "", "preempted": ""} ) # Create and start the SMACH introspection server intro_server = IntrospectionServer("patrol", self.sm_patrol, "/SM_ROOT") intro_server.start() # Execute the state machine for the specified number of patrols while (self.n_patrols == -1 or self.patrol_count < self.n_patrols) and not rospy.is_shutdown(): sm_outcome = self.sm_patrol.execute() self.patrol_count += 1 rospy.loginfo("FINISHED PATROL LOOP: " + str(self.patrol_count)) rospy.loginfo("State Machine Outcome: " + str(sm_outcome)) intro_server.stop() def move_base_result_cb(self, userdata, status, result): if status == actionlib.GoalStatus.SUCCEEDED: self.n_succeeded += 1 elif status == actionlib.GoalStatus.ABORTED: self.n_aborted += 1 elif status == actionlib.GoalStatus.PREEMPTED: self.n_preempted += 1 try: rospy.loginfo( "Success rate: " + str(100.0 * self.n_succeeded / (self.n_succeeded + self.n_aborted + self.n_preempted)) ) except: pass def shutdown(self): rospy.loginfo("Stopping the robot...") self.sm_patrol.request_preempt() self.cmd_vel_pub.publish(Twist()) rospy.sleep(1)
class Patrol(): def __init__(self): rospy.init_node('patrol_smach_concurrence', anonymous=False) # Set the shutdown function (stop the robot) rospy.on_shutdown(self.shutdown) # Track success rate of getting to the goal locations self.n_succeeded = 0 self.n_aborted = 0 self.n_preempted = 0 self.cmd_vel_pub = rospy.Publisher('cmd_vel', Twist, queue_size=5) self.docking_station_pose = (Pose(Point(-0.5, 0.0, 0.0), Quaternion(0.0, 0.0, 0.0, 1.0))) self.waypoints = list() nav_goal = MoveBaseGoal() nav_goal.target_pose.header.frame_id = 'map' nav_goal.target_pose.pose.position.x = 0.7 nav_goal.target_pose.pose.position.y = 0.0 nav_goal.target_pose.pose.position.z = 0.0 nav_goal.target_pose.pose.orientation.w = 1.0 self.waypoints.append(nav_goal) nav_goal = MoveBaseGoal() nav_goal.target_pose.header.frame_id = 'map' nav_goal.target_pose.pose.position.x = 2.51611566544 nav_goal.target_pose.pose.position.y = 0.100562250018 nav_goal.target_pose.pose.position.z = 0.0 nav_goal.target_pose.pose.orientation.w = 1.0 self.waypoints.append(nav_goal) nav_goal = MoveBaseGoal() nav_goal.target_pose.header.frame_id = 'map' nav_goal.target_pose.pose.position.x = 2.94330668449 nav_goal.target_pose.pose.position.y = -0.77200148106 nav_goal.target_pose.pose.position.z = 0.0 nav_goal.target_pose.pose.orientation.z = -0.585479230542 nav_goal.target_pose.pose.orientation.w = 0.81068740622 self.waypoints.append(nav_goal) nav_goal = MoveBaseGoal() nav_goal.target_pose.header.frame_id = 'map' nav_goal.target_pose.pose.position.x = 2.94330668449 nav_goal.target_pose.pose.position.y = -1.67200148106 nav_goal.target_pose.pose.position.z = 0.0 nav_goal.target_pose.pose.orientation.z = -0.585479230542 nav_goal.target_pose.pose.orientation.w = 0.81068740622 self.waypoints.append(nav_goal) # A variable to hold the last/current navigation goal self.last_nav_state = None # A flag to indicate whether or not we are rechargin self.recharging = False # A list to hold then navigation goa nav_states = list() # Create a MoveBaseAction state for the docking station nav_goal = MoveBaseGoal() nav_goal.target_pose.header.frame_id = 'map' nav_goal.target_pose.pose = self.docking_station_pose nav_docking_station = SimpleActionState( 'move_base', MoveBaseAction, goal=nav_goal, result_cb=self.move_base_result_cb, exec_timeout=rospy.Duration(20.0), server_wait_timeout=rospy.Duration(10.0)) # Initialize the navigation state machine self.sm_nav = StateMachine( outcomes=['succeeded', 'aborted', 'preempted']) # Add the nav states to the state machine with the appropriate transitions self.sm_nav.userdata.waypoints = self.waypoints with self.sm_nav: StateMachine.add('PICK_WAYPOINT', PickWaypoint(), transitions={'succeeded': 'NAV_WAYPOINT'}, remapping={'waypoint_out': 'patrol_waypoint'}) StateMachine.add('NAV_WAYPOINT', Nav2Waypoint(), transitions={ 'succeeded': 'PICK_WAYPOINT', 'aborted': 'PICK_WAYPOINT', }, remapping={'waypoint_in': 'patrol_waypoint'}) # Register a callback function to fire on state transitions within the sm_nav state machine self.sm_nav.register_transition_cb(self.nav_transition_cb, cb_args=[]) # Initialize the recharge state machine self.sm_recharge = StateMachine( outcomes=['succeeded', 'aborted', 'preempted']) with self.sm_recharge: StateMachine.add('NAV_DOCKING_STATION', nav_docking_station, transitions={'succeeded': 'RECHARGE_BATTERY'}) StateMachine.add('RECHARGE_BATTERY', ServiceState('battery/set_battery', SetBattery, 100, response_cb=self.recharge_cb), transitions={'succeeded': ''}) # Create the nav_patrol state machine using a Concurrence container self.nav_patrol = Concurrence( outcomes=['succeeded', 'recharge', 'stop'], default_outcome='succeeded', child_termination_cb=self.concurrence_child_termination_cb, outcome_cb=self.concurrence_outcome_cb) # Add the sm_nav machine and a battery MonitorState to the nav_patrol machine with self.nav_patrol: Concurrence.add('SM_NAV', self.sm_nav) Concurrence.add( 'MONITOR_BATTERY', MonitorState("battery/battery_level", Float32, self.battery_cb)) # Create the top level state machine self.sm_top = StateMachine( outcomes=['succeeded', 'aborted', 'preempted']) # Add nav_patrol, sm_recharge and a Stop() machine to sm_top with self.sm_top: StateMachine.add('PATROL', self.nav_patrol, transitions={ 'succeeded': 'PATROL', 'recharge': 'RECHARGE', 'stop': 'STOP' }) StateMachine.add('RECHARGE', self.sm_recharge, transitions={'succeeded': 'PATROL'}) StateMachine.add('STOP', Stop(), transitions={'succeeded': ''}) # Create and start the SMACH introspection server intro_server = IntrospectionServer('patrol', self.sm_top, '/SM_ROOT') intro_server.start() # Execute the state machine sm_outcome = self.sm_top.execute() rospy.loginfo('State Machine Outcome: ' + str(sm_outcome)) intro_server.stop() def nav_transition_cb(self, userdata, active_states, *cb_args): self.last_nav_state = ['NAV_WAYPOINT'] # Gets called when ANY child state terminates def concurrence_child_termination_cb(self, outcome_map): # If the current navigation task has succeeded, return True if outcome_map['SM_NAV'] == 'succeeded': return True # If the MonitorState state returns False (invalid), store the current nav goal and recharge if outcome_map['MONITOR_BATTERY'] == 'invalid': rospy.loginfo("LOW BATTERY! NEED TO RECHARGE...") if self.last_nav_state is not None: self.sm_nav.set_initial_state(self.last_nav_state, UserData()) return True else: return False # Gets called when ALL child states are terminated def concurrence_outcome_cb(self, outcome_map): # If the battery is below threshold, return the 'recharge' outcome if outcome_map['MONITOR_BATTERY'] == 'invalid': return 'recharge' # Otherwise, if the last nav goal succeeded, return 'succeeded' or 'stop' elif outcome_map['SM_NAV'] == 'succeeded': #self.patrol_count += 1 #rospy.loginfo("FINISHED PATROL LOOP: " + str(self.patrol_count)) # If we have not completed all our patrols, start again at the beginning #if self.n_patrols == -1 or self.patrol_count < self.n_patrols: self.sm_nav.set_initial_state(['NAV_WAYPOINT'], UserData()) return 'succeeded' # Otherwise, we are finished patrolling so return 'stop' #else: # self.sm_nav.set_initial_state(['NAV_STATE_0'], UserData()) # return 'succeeded' # Recharge if all else fails else: return 'recharge' def battery_cb(self, userdata, msg): if msg.data < 30.0: self.recharging = True return False else: self.recharging = False return True def recharge_cb(self, userdata, response): return 'succeeded' def move_base_result_cb(self, userdata, status, result): print status, userdata if not self.recharging: if status == actionlib.GoalStatus.SUCCEEDED: self.n_succeeded += 1 elif status == actionlib.GoalStatus.ABORTED: self.n_aborted += 1 elif status == actionlib.GoalStatus.PREEMPTED: self.n_preempted += 1 try: rospy.loginfo("Success rate: " + str(100.0 * self.n_succeeded / (self.n_succeeded + self.n_aborted + self.n_preempted))) except: pass def shutdown(self): rospy.loginfo("Stopping the robot...") self.sm_nav.request_preempt() self.cmd_vel_pub.publish(Twist()) rospy.sleep(1)
class main(): def __init__(self): rospy.init_node('patrol_smach', anonymous=False) # Set the shutdown function (stop the robot) rospy.on_shutdown(self.shutdown) # Initialize a number of parameters and variables self.init() # Subscribe to the move_base action server self.move_base = actionlib.SimpleActionClient("move_base", MoveBaseAction) rospy.loginfo("Waiting for move_base action server...") # Wait up to 60 seconds for the action server to become available self.move_base.wait_for_server(rospy.Duration(60)) rospy.loginfo("Connected to move_base action server") # Track success rate of getting to the goal locations self.n_succeeded = 0 self.n_aborted = 0 self.n_preempted = 0 self.n_patrols = 2 # Turn the waypoints into SMACH states nav_states = list() for waypoint in self.waypoints: nav_goal = MoveBaseGoal() nav_goal.target_pose.header.frame_id = 'map' nav_goal.target_pose.pose = waypoint move_base_state = SimpleActionState('move_base', MoveBaseAction, goal=nav_goal, result_cb=self.move_base_result_cb, exec_timeout=rospy.Duration(10.0), server_wait_timeout=rospy.Duration(10.0)) nav_states.append(move_base_state) move_base_state = SimpleActionState('move_base', MoveBaseAction, goal=nav_goal, result_cb=self.move_base_result_cb, exec_timeout=rospy.Duration(10.0)) # Initialize the top level state machine self.sm = StateMachine(outcomes=['succeeded','aborted','preempted']) with self.sm: # Initialize the iterator self.sm_patrol_iterator = Iterator(outcomes = ['succeeded','preempted','aborted'], input_keys = [], it = lambda: range(0, self.n_patrols), output_keys = [], it_label = 'index', exhausted_outcome = 'succeeded') with self.sm_patrol_iterator: # Initialize the patrol state machine self.sm_patrol = StateMachine(outcomes=['succeeded','aborted','preempted','continue']) # Add the states to the state machine with the appropriate transitions with self.sm_patrol: StateMachine.add('NAV_STATE_0', nav_states[0], transitions={'succeeded':'NAV_STATE_1','aborted':'NAV_STATE_1','preempted':'NAV_STATE_1'}) StateMachine.add('NAV_STATE_1', nav_states[1], transitions={'succeeded':'NAV_STATE_2','aborted':'NAV_STATE_2','preempted':'NAV_STATE_2'}) StateMachine.add('NAV_STATE_2', nav_states[2], transitions={'succeeded':'NAV_STATE_3','aborted':'NAV_STATE_3','preempted':'NAV_STATE_3'}) StateMachine.add('NAV_STATE_3', nav_states[3], transitions={'succeeded':'NAV_STATE_4','aborted':'NAV_STATE_4','preempted':'NAV_STATE_4'}) StateMachine.add('NAV_STATE_4', nav_states[0], transitions={'succeeded':'continue','aborted':'continue','preempted':'continue'}) # Close the sm_patrol machine and add it to the iterator Iterator.set_contained_state('PATROL_STATE', self.sm_patrol, loop_outcomes=['continue']) # Close the top level state machine StateMachine.add('PATROL_ITERATOR', self.sm_patrol_iterator, {'succeeded':'succeeded', 'aborted':'aborted'}) # Create and start the SMACH introspection server intro_server = IntrospectionServer('patrol', self.sm, '/SM_ROOT') intro_server.start() # Execute the state machine sm_outcome = self.sm.execute() rospy.loginfo('State Machine Outcome: ' + str(sm_outcome)) intro_server.stop() def move_base_result_cb(self, userdata, status, result): if status == actionlib.GoalStatus.SUCCEEDED: self.n_succeeded += 1 elif status == actionlib.GoalStatus.ABORTED: self.n_aborted += 1 elif status == actionlib.GoalStatus.PREEMPTED: self.n_preempted += 1 try: rospy.loginfo("Success rate: " + str(100.0 * self.n_succeeded / (self.n_succeeded + self.n_aborted + self.n_preempted))) except: pass def init(self): # How big is the square we want the robot to patrol? self.square_size = rospy.get_param("~square_size", 1.0) # meters # How many times should we execute the patrol loop self.n_patrols = rospy.get_param("~n_patrols", 3) # meters # Create a list to hold the target quaternions (orientations) quaternions = list() # First define the corner orientations as Euler angles euler_angles = (pi/2, pi, 3*pi/2, 0) # Then convert the angles to quaternions for angle in euler_angles: q_angle = quaternion_from_euler(0, 0, angle, axes='sxyz') q = Quaternion(*q_angle) quaternions.append(q) # Create a list to hold the waypoint poses self.waypoints = list() # Append each of the four waypoints to the list. Each waypoint # is a pose consisting of a position and orientation in the map frame. self.waypoints.append(Pose(Point(0.0, 0.0, 0.0), quaternions[3])) self.waypoints.append(Pose(Point(self.square_size, 0.0, 0.0), quaternions[0])) self.waypoints.append(Pose(Point(self.square_size, self.square_size, 0.0), quaternions[1])) self.waypoints.append(Pose(Point(0.0, self.square_size, 0.0), quaternions[2])) # Initialize the waypoint visualization markers for RViz self.init_waypoint_markers() # Set a visualization marker at each waypoint for waypoint in self.waypoints: p = Point() p = waypoint.position self.waypoint_markers.points.append(p) # Publisher to manually control the robot (e.g. to stop it) self.cmd_vel_pub = rospy.Publisher('cmd_vel', Twist) rospy.loginfo("Starting SMACH test") # Publish the waypoint markers self.marker_pub.publish(self.waypoint_markers) rospy.sleep(1) self.marker_pub.publish(self.waypoint_markers) def init_waypoint_markers(self): # Set up our waypoint markers marker_scale = 0.2 marker_lifetime = 0 # 0 is forever marker_ns = 'waypoints' marker_id = 0 marker_color = {'r': 1.0, 'g': 0.7, 'b': 1.0, 'a': 1.0} # Define a marker publisher. self.marker_pub = rospy.Publisher('waypoint_markers', Marker) # Initialize the marker points list. self.waypoint_markers = Marker() self.waypoint_markers.ns = marker_ns self.waypoint_markers.id = marker_id self.waypoint_markers.type = Marker.CUBE_LIST self.waypoint_markers.action = Marker.ADD self.waypoint_markers.lifetime = rospy.Duration(marker_lifetime) self.waypoint_markers.scale.x = marker_scale self.waypoint_markers.scale.y = marker_scale self.waypoint_markers.color.r = marker_color['r'] self.waypoint_markers.color.g = marker_color['g'] self.waypoint_markers.color.b = marker_color['b'] self.waypoint_markers.color.a = marker_color['a'] self.waypoint_markers.header.frame_id = 'odom' self.waypoint_markers.header.stamp = rospy.Time.now() self.waypoint_markers.points = list() def shutdown(self): rospy.loginfo("Stopping the robot...") self.sm_patrol.request_preempt() self.cmd_vel_pub.publish(Twist()) rospy.sleep(1)
class PatrolController(GenericNode): __waypoints = [] __poser = None __homeGoal = None __actionClient = None __StateMachine = None __keyListener = None __isPatrolling = False __restartFlag = False __shuffleFlag = False def __init__(self): super(PatrolController, self).__init__('PatrolServer') self.__keyListener = rospy.Subscriber('keys', String, self.__keysCallback) #TODO make sure shuffling works and thet the first node stays the same # Or try a flag to block the reissue of patrol self.__waypoints = self.__loadWaypointsFromCsv() self.__homeGoal = self.__getGoal(self.__waypoints[0]) self.__StateMachine = StateMachine(['succeded', 'aborted', 'preempted']) def preparePatrol(self, randomize = False): try: if self.__StateMachine.is_opened(): self.__StateMachine.close() rospy.loginfo('Statemachine is closed') self.__StateMachine.open() rospy.loginfo('Statemachine is now open closed') waypoints = copy(self.__waypoints) if randomize == True: waypoints = random.shuffle(waypoints) rospy.loginfo("Waypoints are now shuffled") for n in range(len(waypoints)): waypoint = waypoints[n] _waypointId = 'WP' + str(n) _waypointActionId = 'WPA' + str(n) _nextWaypointId = 'WP' + str((n+1) % len(waypoints)) _nextWaypointActionId = 'WPA' + str((n+1) % len(waypoints)) StateMachine.add(_waypointId, PatrolNavigateState(waypoint), transitions={'succeeded': _nextWaypointActionId}) StateMachine.add(_waypointActionId, PatrolMarkCheckpointState(waypoint), transitions={'succeeded': _nextWaypointId}) rospy.loginfo('Waypoint {0} set!'.format(str(n))) return True except Exception as ex: rospy.logwarn('PatrolNode.PreparePatrol - ', ex.message) return False def patrol(self): try: self.__isPatrolling = True self.__StateMachine.execute() self.__isPatrolling = False self.__restartFlag = False self.__shuffleFlag = False rospy.logwarn('PatrolNode.patrol - Patrol routine has started') except Exception as ex: self.__isPatrolling = False rospy.logwarn('PatrolNode.patrol - ', ex.message) def endPatrolAndRestart(self, shuffle = False): try: rospy.logwarn('Requested Patrol Restart') self.__StateMachine.request_preempt() self.goHome() if shuffle: self.preparePatrol(True) self.__isPatrolling = False except Exception as ex: rospy.logwarn('PatrolNode.endPatrolAndRestart - ', ex.message) def goHome(self): try: _waypoint = self.__homeGoal _actionClient = actionlib.SimpleActionClient('move_base', MoveBaseAction) _actionClient.wait_for_server() _actionClient.cancel_all_goals() _actionClient.send_goal(_waypoint) _actionClient.wait_for_result() except Exception as ex: rospy.logwarn('PatrolNode.__goHome - ', ex.message) def isPatrolling(self): return self.__isPatrolling def restartRequestIssued(self): return self.__restartFlag def __loadWaypointsFromCsv(self): _retVal = [] try: _filename = pkg_resources.resource_filename('resources', 'test.csv') with open(_filename, 'rb') as f: _reader = csv.reader(f) for _row in _reader: _buffer = [] for _col in range(len(_row)): _buffer.append(float(_row[_col])) _retVal.append(_buffer) except Exception as ex: import sys print "System path= ", sys.path print"Error! --> ", ex.message return _retVal def __keysCallback(self, msg): try: rospy.logwarn('Received keypress "{0}"'.format(msg.data)) if len(msg.data) == 0: rospy.logwarn('Received unmapped keypress') return # unknown key. if msg.data == 'h': self.__restartFlag = True rospy.logwarn('Restart Patrol') elif msg.data == 'r': self.__shuffleFlag == True rospy.logwarn('Start Randomized Patrol') except Exception as ex: rospy.logwarn('PatrolNode.__keysCallback - ', ex.message) def __getGoal(self, data): _waypoint = MoveBaseGoal() _waypoint.target_pose.header.frame_id = 'map' _waypoint.target_pose.pose.position.x = data[0] _waypoint.target_pose.pose.position.y = data[1] _waypoint.target_pose.pose.position.z = data[2] _waypoint.target_pose.pose.orientation.x = data[3] _waypoint.target_pose.pose.orientation.y = data[4] _waypoint.target_pose.pose.orientation.z = data[5] _waypoint.target_pose.pose.orientation.w = data[6] return _waypoint
class RandomPatrol(): def __init__(self): rospy.init_node('random_patrol', anonymous=False) # Set the shutdown function (stop the robot) rospy.on_shutdown(self.shutdown) # Initialize a number of parameters and variables #setup_task_environment(self) # Create a list to hold the target quaternions (orientations) quaternions = list() # First define the corner orientations as Euler angles euler_angles = (pi/2, pi, 3*pi/2, 0) # Then convert the angles to quaternions for angle in euler_angles: q_angle = quaternion_from_euler(0, 0, angle, axes='sxyz') q = Quaternion(*q_angle) quaternions.append(q) # Create a list to hold the waypoint poses self.waypoints = list() self.square_size = 1.0 # Append each of the four waypoints to the list. Each waypoint # is a pose consisting of a position and orientation in the map frame. self.waypoints.append(Pose(Point(0.0, 0.0, 0.0), quaternions[3])) self.waypoints.append(Pose(Point(self.square_size, 0.0, 0.0), quaternions[0])) self.waypoints.append(Pose(Point(self.square_size, self.square_size, 0.0), quaternions[1])) self.waypoints.append(Pose(Point(0.0, self.square_size, 0.0), quaternions[2])) # Publisher to manually control the robot (e.g. to stop it) self.cmd_vel_pub = rospy.Publisher('/PETIT/cmd_vel', Twist) self.stopping = False self.recharging = False # Initialize the patrol state machine self.sm_patrol = StateMachine(outcomes=['succeeded','aborted','preempted']) # Set the userdata.waypoints variable to the pre-defined waypoints self.sm_patrol.userdata.waypoints = self.waypoints # Add the states to the state machine with the appropriate transitions with self.sm_patrol: StateMachine.add('PICK_WAYPOINT', PickWaypoint(), transitions={'succeeded':'NAV_WAYPOINT','aborted':'NAV_WAYPOINT'}, remapping={'waypoint_out':'patrol_waypoint'}) StateMachine.add('NAV_WAYPOINT', Nav2Waypoint(), transitions={'succeeded':'', 'aborted':''}, remapping={'waypoint_in':'patrol_waypoint'}) # Create the nav_patrol state machine using a Concurrence container self.sm_concurrent = Concurrence(outcomes=['succeeded', 'stop'], default_outcome='succeeded', child_termination_cb=self.concurrence_child_termination_cb, outcome_cb=self.concurrence_outcome_cb) # Add the sm_nav machine and a battery MonitorState to the nav_patrol machine with self.sm_concurrent: Concurrence.add('SM_NAV', self.sm_patrol) Concurrence.add('MONITOR_TIME', MonitorState("/GENERAL/remain", Int32, self.time_cb)) #Concurrence.add('MONITOR_BATTERY', MonitorState("/PETIT/adc", Int32, self.battery_cb)) # Create the top level state machine self.sm_top = StateMachine(outcomes=['succeeded', 'aborted', 'preempted']) # Add nav_patrol, sm_recharge and a Stop() machine to sm_top with self.sm_top: StateMachine.add('CONCURRENT', self.sm_concurrent, transitions={'succeeded':'CONCURRENT', 'stop':'STOP'}) #StateMachine.add('RECHARGE', self.sm_recharge, transitions={'succeeded':'PATROL'}) StateMachine.add('STOP', Stop(), transitions={'succeeded':''}) # Create and start the SMACH introspection server intro_server = IntrospectionServer('patrol', self.sm_top, '/SM_ROOT') intro_server.start() # Execute the state machine sm_outcome = self.sm_top.execute() rospy.loginfo('State Machine Outcome: ' + str(sm_outcome)) intro_server.stop() def time_cb(self, userdata, msg): if msg.data < 2: self.stopping = True return False else: self.stopping = False return True def battery_cb(self, userdata, msg): if msg.data < 320: self.recharging = True return False else: self.recharging = False return True # Gets called when ANY child state terminates def concurrence_child_termination_cb(self, outcome_map): # If the current navigation task has succeeded, return True if outcome_map['SM_NAV'] == 'succeeded': return True # If the MonitorState state returns False (invalid), store the current nav goal and recharge if outcome_map['MONITOR_TIME'] == 'invalid': rospy.loginfo("LOW TIME! NEED TO STOP...") return True else: return False # Gets called when ALL child states are terminated def concurrence_outcome_cb(self, outcome_map): # If the battery is below threshold, return the 'recharge' outcome if outcome_map['MONITOR_TIME'] == 'invalid': rospy.loginfo("TIME FINISHED !! GOING TO STOP ! ") return 'stop' #if outcome_map['MONITOR_BATTERY'] == 'invalid': #return 'recharge' # Otherwise, if the last nav goal succeeded, return 'succeeded' or 'stop' elif outcome_map['SM_NAV'] == 'succeeded': #self.patrol_count += 1 #rospy.loginfo("FINISHED PATROL LOOP: " + str(self.patrol_count)) # If we have not completed all our patrols, start again at the beginning #if self.n_patrols == -1 or self.patrol_count < self.n_patrols: #self.sm_nav.set_initial_state(['NAV_STATE_0'], UserData()) return 'succeeded' # Otherwise, we are finished patrolling so return 'stop' #else: #self.sm_nav.set_initial_state(['NAV_STATE_4'], UserData()) #return 'stop' # Recharge if all else fails else: return 'recharge' def shutdown(self): rospy.loginfo("Stopping the robot...") self.sm_patrol.request_preempt() self.cmd_vel_pub.publish(Twist()) rospy.sleep(1)
class Patrol(): def __init__(self): rospy.init_node('patrol_smach', anonymous=False) # Set the shutdown function (stop the robot) rospy.on_shutdown(self.shutdown) # Initialize a number of parameters and variables setup_task_environment(self) # Track success rate of getting to the goal locations self.n_succeeded = 0 self.n_aborted = 0 self.n_preempted = 0 self.patrol_count = 0 self.n_patrols = 1 # A random number generator for use in the transition callback self.rand = Random() # A list to hold then nav_states nav_states = list() # Turn the waypoints into SMACH states for waypoint in self.waypoints: nav_goal = MoveBaseGoal() nav_goal.target_pose.header.frame_id = 'map' nav_goal.target_pose.pose = waypoint move_base_state = SimpleActionState( 'move_base', MoveBaseAction, goal=nav_goal, result_cb=self.move_base_result_cb, exec_timeout=rospy.Duration(10.0), server_wait_timeout=rospy.Duration(10.0)) nav_states.append(move_base_state) # Initialize the patrol state machine self.sm_patrol = StateMachine( outcomes=['succeeded', 'aborted', 'preempted']) # Register a callback function to fire on state transitions within the sm_patrol state machine self.sm_patrol.register_transition_cb(self.transition_cb, cb_args=[]) # Add the states to the state machine with the appropriate transitions with self.sm_patrol: StateMachine.add('NAV_STATE_0', nav_states[0], transitions={ 'succeeded': 'NAV_STATE_1', 'aborted': 'NAV_STATE_1' }) StateMachine.add('NAV_STATE_1', nav_states[1], transitions={ 'succeeded': 'NAV_STATE_2', 'aborted': 'NAV_STATE_2' }) StateMachine.add('NAV_STATE_2', nav_states[2], transitions={ 'succeeded': 'NAV_STATE_3', 'aborted': 'NAV_STATE_3' }) StateMachine.add('NAV_STATE_3', nav_states[3], transitions={ 'succeeded': 'NAV_STATE_4', 'aborted': 'NAV_STATE_4' }) StateMachine.add('NAV_STATE_4', nav_states[0], transitions={ 'succeeded': 'NAV_STATE_0', 'aborted': 'NAV_STATE_0' }) StateMachine.add('CHECK_COUNT', CheckCount(self.n_patrols), transitions={ 'succeeded': 'NAV_STATE_0', 'aborted': '', 'preempted': '' }) # Create and start the SMACH introspection server intro_server = IntrospectionServer('patrol', self.sm_patrol, '/SM_ROOT') intro_server.start() # Execute the state machine self.sm_outcome = self.sm_patrol.execute() rospy.loginfo('State Machine Outcome: ' + str(self.sm_outcome)) intro_server.stop() os._exit(0) def move_base_result_cb(self, userdata, status, result): if status == actionlib.GoalStatus.SUCCEEDED: self.n_succeeded += 1 elif status == actionlib.GoalStatus.ABORTED: self.n_aborted += 1 elif status == actionlib.GoalStatus.PREEMPTED: self.n_preempted += 1 try: rospy.loginfo( "Success rate: " + str(100.0 * self.n_succeeded / (self.n_succeeded + self.n_aborted + self.n_preempted))) except: pass def transition_cb(self, userdata, active_states, *cb_args): if self.rand.randint(0, 3) == 0: rospy.loginfo("Greetings human!") rospy.sleep(1) rospy.loginfo("Is everything OK?") rospy.sleep(1) else: rospy.loginfo("Nobody here.") def shutdown(self): rospy.loginfo("Stopping the robot...") self.sm_patrol.request_preempt() self.cmd_vel_pub.publish(Twist()) rospy.sleep(1)
class Patrol(): def __init__(self): rospy.init_node('Pi_Smach', anonymous=False) rospy.on_shutdown(self.shutdown) setup_environment(self) self.last_nav_state = None self.recharging = False nav_states = list() for waypoint in self.waypoints: nav_goal = MoveBaseGoal() nav_goal.target_pose.header.frame_id = 'map' nav_goal.target_pose.pose = waypoint move_base_state = SimpleActionState( 'move_base', MoveBaseAction, goal=nav_goal, exec_timeout=rospy.Duration(60.0), server_wait_timeout=rospy.Duration(10.0)) nav_states.append(move_base_state) nav_goal = MoveBaseGoal() nav_goal.target_pose.header.frame_id = 'map' nav_goal.target_pose.pose = self.docking_station_pose nav_docking_station = SimpleActionState( 'move_base', MoveBaseAction, goal=nav_goal, exec_timeout=rospy.Duration(60.0), server_wait_timeout=rospy.Duration(10.0)) self.sm_nav = StateMachine( outcomes=['succeeded', 'aborted', 'preempted']) self.sm_nav.userdata.X_input = 0 self.sm_nav.userdata.Y_input = 0 with self.sm_nav: StateMachine.add('NAV_STATE_0', nav_states[0], transitions={ 'succeeded': 'NAV_STATE_1', 'aborted': 'NAV_STATE_1' }) StateMachine.add('NAV_STATE_1', nav_states[1], transitions={ 'succeeded': 'INPUTXY', 'aborted': 'INPUTXY' }) StateMachine.add('INPUTXY', InputXY(), transitions={'succeeded': 'GOCHOSEWAY'}, remapping={ 'X_output': 'X_input', 'Y_output': 'Y_input' }) StateMachine.add('GOCHOSEWAY', ChoseWay(), transitions={ 'succeeded': 'MOVE_ARM', 'aborted': 'MOVE_ARM' }, remapping={ 'X': 'X_input', 'Y': 'Y_input' }) StateMachine.add('MOVE_ARM', Move_Arm(), transitions={'succeeded': 'NAV_STATE_2'}) StateMachine.add('NAV_STATE_2', nav_states[2], transitions={ 'succeeded': '', 'aborted': '' }) self.sm_nav.register_transition_cb(self.nav_transition_cb, cb_args=[]) self.sm_recharge = StateMachine( outcomes=['succeeded', 'aborted', 'preempted']) with self.sm_recharge: StateMachine.add('NAV_DOCKING_STATION', nav_docking_station, transitions={'succeeded': 'RECHARGE_BATTERY'}) StateMachine.add('RECHARGE_BATTERY', ServiceState( 'battery_simulator/set_battery_level', SetBatteryLevel, 100, response_cb=self.recharge_cb), transitions={'succeeded': ''}) self.nav_patrol = Concurrence( outcomes=['succeeded', 'recharge', 'stop'], default_outcome='succeeded', child_termination_cb=self.concurrence_child_termination_cb, outcome_cb=self.concurrence_outcome_cb) with self.nav_patrol: Concurrence.add('SM_NAV', self.sm_nav) Concurrence.add( 'MONITOR_BATTERY', MonitorState("battery_level", Float32, self.battery_cb)) self.sm_top = StateMachine( outcomes=['succeeded', 'aborted', 'preempted']) with self.sm_top: StateMachine.add('PATROL', self.nav_patrol, transitions={ 'succeeded': 'PATROL', 'recharge': 'RECHARGE', 'stop': 'STOP' }) StateMachine.add('RECHARGE', self.sm_recharge, transitions={'succeeded': 'PATROL'}) StateMachine.add('STOP', Stop(), transitions={'succeeded': ''}) intro_server = IntrospectionServer('patrol', self.sm_top, '/SM_ROOT') intro_server.start() sm_outcome = self.sm_top.execute() rospy.loginfo('State Machine Outcome: ' + str(sm_outcome)) intro_server.stop() def nav_transition_cb(self, userdata, active_states, *cb_args): self.last_nav_state = active_states def concurrence_child_termination_cb(self, outcome_map): if outcome_map['SM_NAV'] == 'succeeded': return True if outcome_map['MONITOR_BATTERY'] == 'invalid': rospy.loginfo("LOW BATTERY! NEED TO RECHARGE...") if self.last_nav_state is not None: self.sm_nav.set_initial_state(self.last_nav_state, UserData()) return True else: return False def concurrence_outcome_cb(self, outcome_map): if outcome_map['MONITOR_BATTERY'] == 'invalid': return 'recharge' elif outcome_map['SM_NAV'] == 'succeeded': self.sm_nav.set_initial_state(['NAV_STATE_2'], UserData()) return 'stop' else: return 'recharge' def battery_cb(self, userdata, msg): if msg.data < self.low_battery_threshold: self.recharging = True return False else: self.recharging = False return True def recharge_cb(self, userdata, response): return 'succeeded' def shutdown(self): rospy.loginfo("Stopping the robot...") self.sm_nav.request_preempt() #self.cmd_vel_pub.publish(Twist()) rospy.sleep(1)
class Patrol(): def __init__(self): rospy.init_node('patrol_smach_concurrence', anonymous=False) # Set the shutdown function (stop the robot) rospy.on_shutdown(self.shutdown) # Initialize a number of parameters and variables setup_task_environment(self) # Track success rate of getting to the goal locations self.n_succeeded = 0 self.n_aborted = 0 self.n_preempted = 0 # A variable to hold the last/current navigation goal self.last_nav_state = None # A flag to indicate whether or not we are rechargin self.recharging = False # A list to hold then navigation goa nav_states = list() # Turn the waypoints into SMACH states for waypoint in self.waypoints: nav_goal = MoveBaseGoal() nav_goal.target_pose.header.frame_id = 'map' nav_goal.target_pose.pose = waypoint move_base_state = SimpleActionState('move_base', MoveBaseAction, goal=nav_goal, result_cb=self.move_base_result_cb, exec_timeout=rospy.Duration(10.0), server_wait_timeout=rospy.Duration(10.0)) nav_states.append(move_base_state) # Create a MoveBaseAction state for the docking station nav_goal = MoveBaseGoal() nav_goal.target_pose.header.frame_id = 'map' nav_goal.target_pose.pose = self.docking_station_pose nav_docking_station = SimpleActionState('move_base', MoveBaseAction, goal=nav_goal, result_cb=self.move_base_result_cb, exec_timeout=rospy.Duration(20.0), server_wait_timeout=rospy.Duration(10.0)) # Initialize the navigation state machine self.sm_nav = StateMachine(outcomes=['succeeded', 'aborted', 'preempted']) # Add the nav states to the state machine with the appropriate transitions with self.sm_nav: StateMachine.add('NAV_STATE_0', nav_states[0], transitions={'succeeded':'NAV_STATE_1','aborted':'NAV_STATE_1'}) StateMachine.add('NAV_STATE_1', nav_states[1], transitions={'succeeded':'NAV_STATE_2','aborted':'NAV_STATE_2'}) StateMachine.add('NAV_STATE_2', nav_states[2], transitions={'succeeded':'NAV_STATE_3','aborted':'NAV_STATE_3'}) StateMachine.add('NAV_STATE_3', nav_states[3], transitions={'succeeded':'NAV_STATE_4','aborted':'NAV_STATE_4'}) StateMachine.add('NAV_STATE_4', nav_states[0], transitions={'succeeded':'','aborted':''}) # Register a callback function to fire on state transitions within the sm_nav state machine self.sm_nav.register_transition_cb(self.nav_transition_cb, cb_args=[]) # Initialize the recharge state machine self.sm_recharge = StateMachine(outcomes=['succeeded', 'aborted', 'preempted']) with self.sm_recharge: StateMachine.add('NAV_DOCKING_STATION', nav_docking_station, transitions={'succeeded':'RECHARGE_BATTERY'}) StateMachine.add('RECHARGE_BATTERY', ServiceState('battery_simulator/set_battery_level', SetBatteryLevel, 100, response_cb=self.recharge_cb), transitions={'succeeded':''}) # Create the nav_patrol state machine using a Concurrence container self.nav_patrol = Concurrence(outcomes=['succeeded', 'recharge', 'stop'], default_outcome='succeeded', child_termination_cb=self.concurrence_child_termination_cb, outcome_cb=self.concurrence_outcome_cb) # Add the sm_nav machine and a battery MonitorState to the nav_patrol machine with self.nav_patrol: Concurrence.add('SM_NAV', self.sm_nav) Concurrence.add('MONITOR_BATTERY', MonitorState("battery_level", Float32, self.battery_cb)) # Create the top level state machine self.sm_top = StateMachine(outcomes=['succeeded', 'aborted', 'preempted']) # Add nav_patrol, sm_recharge and a Stop() machine to sm_top with self.sm_top: StateMachine.add('PATROL', self.nav_patrol, transitions={'succeeded':'PATROL', 'recharge':'RECHARGE', 'stop':'STOP'}) StateMachine.add('RECHARGE', self.sm_recharge, transitions={'succeeded':'PATROL'}) StateMachine.add('STOP', Stop(), transitions={'succeeded':''}) # Create and start the SMACH introspection server intro_server = IntrospectionServer('patrol', self.sm_top, '/SM_ROOT') intro_server.start() # Execute the state machine sm_outcome = self.sm_top.execute() rospy.loginfo('State Machine Outcome: ' + str(sm_outcome)) intro_server.stop() def nav_transition_cb(self, userdata, active_states, *cb_args): self.last_nav_state = active_states # Gets called when ANY child state terminates def concurrence_child_termination_cb(self, outcome_map): # If the current navigation task has succeeded, return True if outcome_map['SM_NAV'] == 'succeeded': return True # If the MonitorState state returns False (invalid), store the current nav goal and recharge if outcome_map['MONITOR_BATTERY'] == 'invalid': rospy.loginfo("LOW BATTERY! NEED TO RECHARGE...") if self.last_nav_state is not None: self.sm_nav.set_initial_state(self.last_nav_state, UserData()) return True else: return False # Gets called when ALL child states are terminated def concurrence_outcome_cb(self, outcome_map): # If the battery is below threshold, return the 'recharge' outcome if outcome_map['MONITOR_BATTERY'] == 'invalid': return 'recharge' # Otherwise, if the last nav goal succeeded, return 'succeeded' or 'stop' elif outcome_map['SM_NAV'] == 'succeeded': self.patrol_count += 1 rospy.loginfo("FINISHED PATROL LOOP: " + str(self.patrol_count)) # If we have not completed all our patrols, start again at the beginning if self.n_patrols == -1 or self.patrol_count < self.n_patrols: self.sm_nav.set_initial_state(['NAV_STATE_0'], UserData()) return 'succeeded' # Otherwise, we are finished patrolling so return 'stop' else: self.sm_nav.set_initial_state(['NAV_STATE_4'], UserData()) return 'stop' # Recharge if all else fails else: return 'recharge' def battery_cb(self, userdata, msg): if msg.data < self.low_battery_threshold: self.recharging = True return False else: self.recharging = False return True def recharge_cb(self, userdata, response): return 'succeeded' def move_base_result_cb(self, userdata, status, result): if not self.recharging: if status == actionlib.GoalStatus.SUCCEEDED: self.n_succeeded += 1 elif status == actionlib.GoalStatus.ABORTED: self.n_aborted += 1 elif status == actionlib.GoalStatus.PREEMPTED: self.n_preempted += 1 try: rospy.loginfo("Success rate: " + str(100.0 * self.n_succeeded / (self.n_succeeded + self.n_aborted + self.n_preempted))) except: pass def shutdown(self): rospy.loginfo("Stopping the robot...") self.sm_nav.request_preempt() self.cmd_vel_pub.publish(Twist()) rospy.sleep(1)
class main(): def __init__(self): rospy.init_node('find_treasure', anonymous=False) # Set the shutdown function (stop the robot) rospy.on_shutdown(self.shutdown) # How long do we have to get to each waypoint? self.move_base_timeout = rospy.get_param("~move_base_timeout", 10) #seconds # Initialize the patrol counter self.patrol_count = 0 # Subscribe to the move_base action server self.move_base = actionlib.SimpleActionClient("move_base", MoveBaseAction) rospy.loginfo("Waiting for move_base action server...") # Publisher to manually control the robot (e.g. to stop it) self.cmd_vel_pub = rospy.Publisher('cmd_vel', Twist) # Wait up to 60 seconds for the action server to become available self.move_base.wait_for_server(rospy.Duration(60)) rospy.loginfo("Connected to move_base action server") # Create a list to hold the target quaternions (orientations) quaternions = list() # First define the corner orientations as Euler angles euler_angles = (pi/2, pi, 3*pi/2, 0) # Then convert the angles to quaternions for angle in euler_angles: q_angle = quaternion_from_euler(0, 0, angle, axes='sxyz') q = Quaternion(*q_angle) quaternions.append(q) # Create a list to hold the waypoint poses self.waypoints = list() # Append each of the four waypoints to the list. Each waypoint # is a pose consisting of a position and orientation in the map frame. # -0.163200, 0.044660, 0.193186 self.waypoints.append(Pose(Point(0.9579, 1.8710, 0.0), quaternions[3])) self.waypoints.append(Pose(Point(0.7555, 0.0692, 0.0), quaternions[1])) self.waypoints.append(Pose(Point(-0.72511, 0.4952, 0.0), quaternions[0])) self.waypoints.append(Pose(Point(0.167730, 2.18168, 0.0), quaternions[2])) position_locations = list() position_locations.append(('position1', self.waypoints[0])) position_locations.append(('position2', self.waypoints[1])) position_locations.append(('position3', self.waypoints[2])) position_locations.append(('position4', self.waypoints[3])) self.position_locations = OrderedDict(position_locations) # Publisher to manually control the robot (e.g. to stop it) self.cmd_vel_pub = rospy.Publisher('cmd_vel', Twist) rospy.loginfo("Starting Tasks") # Initialize a number of parameters and variables # setup_task_environment(self) RVizUtils.get_instance().init_primary_waypoint_markers(self.waypoints) ''' Create individual state machines for assigning tasks to each position ''' # Initialize the overall state machine self.sm_find_treasure = StateMachine(outcomes=['succeeded','aborted','preempted']) # Build the find treasure state machine from the nav states and treasure finding states with self.sm_find_treasure: # First State Machine sm_nav = StateMachine(outcomes=['succeeded','aborted','preempted']) sm_nav.userdata.waypoints = self.waypoints sm_nav.userdata.waypoints_primary_count = len(self.waypoints) rospy.loginfo(sm_nav.userdata.waypoints) with sm_nav: # StateMachine.add('PICK_WAYPOINT', PickWaypoint(), # transitions={'succeeded':'NAV_WAYPOINT','aborted':'','preempted':''}, # remapping={'waypoint_out':'patrol_waypoint', # 'waypoints_primary_count':'waypoints_primary_count'}) StateMachine.add('NAV_WAYPOINT', NavState(), transitions={'succeeded':'NAV_WAYPOINT', 'aborted':'', 'preempted':''}, remapping={'waypoint_in':'patrol_waypoint', 'waypoints_primary_count':'waypoints_primary_count'}) # Second State Machine sm_read_tags = StateMachine(outcomes=['valid','invalid','preempted']) sm_read_tags.userdata.waypoints = self.waypoints with sm_read_tags: StateMachine.add('read_tag', ReadTagState('ar_pose_marker', AlvarMarkers), transitions={'invalid':'read_tag', 'valid':'read_tag', 'preempted':''}) # Third State Machine sm_detect_faces = StateMachine(outcomes=['valid','invalid','preempted']) with sm_detect_faces: StateMachine.add('detect_face', FaceDetectState("/camera/rgb/image_color", Image), transitions={'invalid':'detect_face', 'valid':'detect_face', 'preempted':''}) # Forth State Machine sm_recognize_faces = StateMachine(outcomes=['succeeded','aborted','preempted']) with sm_recognize_faces: StateMachine.add('recognize_face', FaceRecognitionState(), transitions={'succeeded':'', 'aborted':'', 'preempted':''}) # goal = FaceRecognitionGoal() # goal.order_id = 1 # goal.order_argument = '' # StateMachine.add('recognize_face', SimpleActionState('face_recognition', # FaceRecognitionAction, goal=goal), transitions={'succeeded':'', 'aborted':'', 'preempted':''}) sm_con = Concurrence(outcomes=['succeeded', 'aborted', 'preempted'],default_outcome='succeeded', child_termination_cb=self.child_term_cb, outcome_cb=self.out_cb) with sm_con: Concurrence.add('SM_NAV', sm_nav) Concurrence.add('SM_READ_TAGS', sm_read_tags) # Concurrence.add('SM_DETECT_FACES', sm_detect_faces) Concurrence.add('SM_RECOGNIZE_FACES', sm_recognize_faces) sm_estimate_position = StateMachine(outcomes=['succeeded','aborted','preempted']) with sm_estimate_position: StateMachine.add('estimate_pose', PoseEstimation('ar_pose_marker', AlvarMarkers), transitions={'succeeded':'', 'aborted':'estimate_pose', 'preempted':''}) StateMachine.add('POSE_ESTIMATE', sm_estimate_position, transitions={'succeeded':'CON','aborted':'CON','preempted':'CON'}) StateMachine.add('CON',sm_con, transitions={'succeeded':'','aborted':'','preempted':''}) # Create and start the SMACH introspection server sm_find_treasure intro_server = IntrospectionServer('find_treasure', self.sm_find_treasure, '/SM_ROOT') intro_server.start() # Execute the state machine sm_outcome = self.sm_find_treasure.execute() rospy.loginfo('the length now is') rospy.loginfo(self.waypoints) intro_server.stop() def shutdown(self): rospy.loginfo("Stopping the robot...") self.sm_find_treasure.request_preempt() self.cmd_vel_pub.publish(Twist()) rospy.sleep(1) def out_cb(self,outcome_map): return 'succeeded' def child_term_cb(self,outcome_map): rospy.loginfo(outcome_map) if 'SM_NAV' in outcome_map and (outcome_map['SM_NAV'] == 'succeeded' or outcome_map['SM_NAV'] == 'aborted'): rospy.loginfo('It visited all points') return True return False
class RodneyMissionsNode: def __init__(self): rospy.on_shutdown(self.ShutdownCallback) # Subscribe to message to cancel missions self.__cancel_sub = rospy.Subscriber('/missions/mission_cancel', Empty, self.CancelCallback) # Create an instance of the missions helper class self.__missions_helper = MissionsHelper() # ------------------------- Top level state machine ------------------------- # Create top level state machine self.__sm = StateMachine(outcomes=['preempted','aborted'], output_keys=['mission_data']) with self.__sm: # Add a state which monitors for a mission to run StateMachine.add('WAITING', MonitorState('/missions/mission_request', String, self.MissionRequestCB, output_keys = ['mission']), transitions={'valid':'WAITING', 'invalid':'PREPARE', 'preempted':'preempted'}) # Add state to prepare the mission StateMachine.add('PREPARE', Prepare(self.__missions_helper), transitions={'mission1':'MISSION1','mission2':'MISSION2','mission4':'MISSION4', 'done_task':'WAITING','head_default':'DEFAULT_HEAD_POSITION', 'move_head':'MOVE_HEAD'}) # Add the reporting state StateMachine.add('REPORT', Report(), transitions={'success':'DEFAULT_HEAD_POSITION'}) # Set up action goal for deafult head position default_position_pan, default_position_tilt = self.__missions_helper.CameraDefaultPos() head_goal = point_headGoal() head_goal.absolute = True head_goal.pan = default_position_pan head_goal.tilt = default_position_tilt # Add the default camera position state. Which moves the head to the default position StateMachine.add('DEFAULT_HEAD_POSITION', SimpleActionState('head_control_node', point_headAction, result_cb = self.__missions_helper.CameraAtDefaultPos, goal = head_goal), transitions={'succeeded':'WAITING','preempted':'WAITING','aborted':'aborted'}) # Add the move head state StateMachine.add('MOVE_HEAD', SimpleActionState('head_control_node', point_headAction, goal_slots=['absolute', 'pan', 'tilt']), transitions={'succeeded':'WAITING', 'preempted':'WAITING', 'aborted':'aborted'}, remapping={'absolute':'user_data_absolute', 'pan':'user_data_pan', 'tilt':'user_data_tilt'}) # ------------------------- Sub State machine for mission 1 --------------------- # Create a sub state machine for mission 1 - take a message to self.__sm_mission1 = missions_lib.Mission1StateMachine(self.__missions_helper) # Now add the sub state machine for mission 1 to the top level one StateMachine.add('MISSION1', self.__sm_mission1, transitions={'complete':'REPORT','preempted':'REPORT','aborted':'REPORT'}) # ------------------------- Sub State machine for mission 2 --------------------- # Create a sub state machine for mission 2 - face detection and greeting self.__sm_mission2 = missions_lib.Mission2StateMachine(self.__missions_helper) # Now add the sub state machine for mission 2 to the top level one StateMachine.add('MISSION2', self.__sm_mission2, transitions={'complete':'REPORT','preempted':'REPORT','aborted':'aborted'}) # ------------------------- Sub State machine for mission 4 --------------------- # Create a sub state machine for mission 4 - Go Home self.__sm_mission4 = missions_lib.Mission4StateMachine(self.__missions_helper) # Now add the sub state machine for mission 4 to the top level one StateMachine.add('MISSION4', self.__sm_mission4, transitions={'complete':'REPORT','preempted':'REPORT','aborted':'REPORT'}) # ------------------------------------------------------------------------------- # Create and start the introspective server so that we can use smach_viewer sis = IntrospectionServer('server_name', self.__sm, '/SM_ROOT') sis.start() self.__sm.execute() # Wait for ctrl-c to stop application rospy.spin() sis.stop() # Monitor State takes /missions/mission_request topic and passes the mission # in user_data to the PREPARE state def MissionRequestCB(self, userdata, msg): # Take the message data and send it to the next state in the userdata userdata.mission = msg.data # Returning False means the state transition will follow the invalid line return False # Callback for cancel mission message def CancelCallback(self, data): # If a sub statemachine for a mission is running then request it be preempted if self.__sm_mission1.is_running(): self.__sm_mission1.request_preempt() elif self.__sm_mission2.is_running(): self.__sm_mission2.request_preempt() elif self.__sm_mission4.is_running(): self.__sm_mission4.request_preempt() def ShutdownCallback(self): self.__sm.request_preempt()
class SMACHAI(): def __init__(self): rospy.init_node('petit_smach_ai', anonymous=False) # Set the shutdown function (stop the robot) rospy.on_shutdown(self.shutdown) # Create a list to hold the target quaternions (orientations) quaternions = list() # First define the corner orientations as Euler angles euler_angles = (0, pi, pi / 2, -pi / 2, pi / 4, 0) # Then convert the angles to quaternions for angle in euler_angles: q_angle = quaternion_from_euler(0, 0, angle, axes='sxyz') q = Quaternion(*q_angle) quaternions.append(q) # Create a list to hold the waypoint poses self.waypoints = list() self.square_size = 1.0 # Append each of the four waypoints to the list. Each waypoint # is a pose consisting of a position and orientation in the map frame. self.waypoints.append(Pose(Point(-0.4, 0.65, 0.0), quaternions[0])) self.waypoints.append(Pose(Point(-1.0, 0.50, 0.0), quaternions[1])) self.waypoints.append(Pose(Point(-0.4, 0.35, 0.0), quaternions[2])) self.waypoints.append(Pose(Point(-0.8, 0.65, 0.0), quaternions[3])) self.waypoints.append(Pose(Point(-1.2, 0.30, 0.0), quaternions[4])) self.waypoints.append(Pose(Point(-0.4, 0.657, 0.0), quaternions[5])) # Publisher to manually control the robot (e.g. to stop it) self.cmd_vel_pub = rospy.Publisher('/PETIT/cmd_vel', Twist) self.stopping = False self.recharging = False self.robot_side = 1 # State machine for Actions self.sm_actions = StateMachine( outcomes=['succeeded', 'aborted', 'preempted']) with self.sm_actions: # Calib X StateMachine.add('CALIBRATE_X', CalibX(), transitions={ 'succeeded': 'CALIBRATE_Y', 'aborted': 'aborted' }) # Calib Y StateMachine.add('CALIBRATE_Y', CalibY(), transitions={ 'succeeded': 'MAX_SPEED_1', 'aborted': 'aborted' }) # PICK-UP # Change speed StateMachine.add('MAX_SPEED_1', MaxLinearSpeed(70000), transitions={ 'succeeded': 'GOTO_1_1', 'aborted': 'aborted' }) # NavPoint 1 StateMachine.add('GOTO_1_1', Nav2Waypoint(self.waypoints[0]), transitions={ 'succeeded': 'MAX_SPEED_2', 'aborted': 'aborted' }) # Change speed StateMachine.add('MAX_SPEED_2', MaxLinearSpeed(40000), transitions={ 'succeeded': 'PAUSE_1', 'aborted': 'aborted' }) # Pause StateMachine.add('PAUSE_1', Pause(1.0), transitions={ 'succeeded': 'FORWARD_1', 'aborted': 'aborted' }) # Avancer StateMachine.add('FORWARD_1', MoveForward(0.15), transitions={ 'succeeded': 'FORKS_10', 'aborted': 'aborted' }) # Monter fourches StateMachine.add('FORKS_10', Forks(90), transitions={ 'succeeded': 'FORKS_11', 'aborted': 'aborted' }) StateMachine.add('FORKS_11', Forks(87), transitions={ 'succeeded': 'FORKS_12', 'aborted': 'aborted' }) StateMachine.add('FORKS_12', Forks(83), transitions={ 'succeeded': 'BACKWARD_1', 'aborted': 'aborted' }) # Reculer StateMachine.add('BACKWARD_1', MoveForward(-0.15), transitions={ 'succeeded': 'ROTATE_1', 'aborted': 'aborted' }) # Rotation StateMachine.add('ROTATE_1', MoveRotate(pi), transitions={ 'succeeded': 'MAX_SPEED_3', 'aborted': 'aborted' }) # DROP-OFF # Change speed StateMachine.add('MAX_SPEED_3', MaxLinearSpeed(70000), transitions={ 'succeeded': 'GOTO_2_1', 'aborted': 'aborted' }) # NavPoint 2 StateMachine.add('GOTO_2_1', Nav2Waypoint(self.waypoints[1]), transitions={ 'succeeded': 'MAX_SPEED_4', 'aborted': 'aborted' }) # Change speed StateMachine.add('MAX_SPEED_4', MaxLinearSpeed(40000), transitions={ 'succeeded': 'PAUSE_2', 'aborted': 'aborted' }) # Pause StateMachine.add('PAUSE_2', Pause(1.0), transitions={ 'succeeded': 'FORWARD_2', 'aborted': 'aborted' }) # Avancer StateMachine.add('FORWARD_2', MoveForward(0.13), transitions={ 'succeeded': 'FORKS_20', 'aborted': 'aborted' }) # Baisser fourches StateMachine.add('FORKS_20', Forks(87), transitions={ 'succeeded': 'FORKS_21', 'aborted': 'aborted' }) StateMachine.add('FORKS_21', Forks(91), transitions={ 'succeeded': 'FORKS_22', 'aborted': 'aborted' }) StateMachine.add('FORKS_22', Forks(95), transitions={ 'succeeded': 'BACKWARD_2', 'aborted': 'aborted' }) # Reculer StateMachine.add('BACKWARD_2', MoveForward(-0.15), transitions={ 'succeeded': 'ROTATE_2', 'aborted': 'aborted' }) # Rotation StateMachine.add('ROTATE_2', MoveRotate(0), transitions={ 'succeeded': 'GOTO_3_1', 'aborted': 'aborted' }) # NavPoint 3 StateMachine.add('GOTO_3_1', Nav2Waypoint(self.waypoints[2]), transitions={ 'succeeded': 'PAUSE_3', 'aborted': 'aborted' }) # Pause StateMachine.add('PAUSE_3', Pause(2.0), transitions={ 'succeeded': 'GOTO_4_1', 'aborted': 'aborted' }) # NavPoint 4 StateMachine.add('GOTO_4_1', Nav2Waypoint(self.waypoints[3]), transitions={ 'succeeded': 'PAUSE_4', 'aborted': 'aborted' }) # Pause StateMachine.add('PAUSE_4', Pause(2.0), transitions={ 'succeeded': 'GOTO_2_2', 'aborted': 'aborted' }) # PICK-UP # NavPoint 2 StateMachine.add('GOTO_2_2', Nav2Waypoint(self.waypoints[1]), transitions={ 'succeeded': 'PAUSE_5', 'aborted': 'aborted' }) # Pause StateMachine.add('PAUSE_5', Pause(1.0), transitions={ 'succeeded': 'FORWARD_3', 'aborted': 'aborted' }) # Avancer StateMachine.add('FORWARD_3', MoveForward(0.15), transitions={ 'succeeded': 'FORKS_30', 'aborted': 'aborted' }) # Monter fourches StateMachine.add('FORKS_30', Forks(90), transitions={ 'succeeded': 'FORKS_31', 'aborted': 'aborted' }) StateMachine.add('FORKS_31', Forks(87), transitions={ 'succeeded': 'FORKS_32', 'aborted': 'aborted' }) StateMachine.add('FORKS_32', Forks(83), transitions={ 'succeeded': 'BACKWARD_3', 'aborted': 'aborted' }) # Reculer StateMachine.add('BACKWARD_3', MoveForward(-0.15), transitions={ 'succeeded': 'ROTATE_3', 'aborted': 'aborted' }) # Rotation StateMachine.add('ROTATE_3', MoveRotate(0), transitions={ 'succeeded': 'GOTO_1_2', 'aborted': 'aborted' }) # DROP-OFF # NavPoint 1 StateMachine.add('GOTO_1_2', Nav2Waypoint(self.waypoints[5]), transitions={ 'succeeded': 'PAUSE_6', 'aborted': 'aborted' }) # Pause StateMachine.add('PAUSE_6', Pause(1.0), transitions={ 'succeeded': 'FORWARD_4', 'aborted': 'aborted' }) # Avancer StateMachine.add('FORWARD_4', MoveForward(0.13), transitions={ 'succeeded': 'FORKS_40', 'aborted': 'aborted' }) # Baisser fourches StateMachine.add('FORKS_40', Forks(87), transitions={ 'succeeded': 'FORKS_41', 'aborted': 'aborted' }) StateMachine.add('FORKS_41', Forks(91), transitions={ 'succeeded': 'FORKS_42', 'aborted': 'aborted' }) StateMachine.add('FORKS_42', Forks(95), transitions={ 'succeeded': 'BACKWARD_4', 'aborted': 'aborted' }) # Reculer StateMachine.add('BACKWARD_4', MoveForward(-0.15), transitions={ 'succeeded': 'ROTATE_4', 'aborted': 'aborted' }) # Rotation StateMachine.add('ROTATE_4', MoveRotate(pi), transitions={ 'succeeded': 'GOTO_5_1', 'aborted': 'aborted' }) # NavPoint 5 StateMachine.add('GOTO_5_1', Nav2Waypoint(self.waypoints[4]), transitions={ 'succeeded': 'CALIBRATE_X', 'aborted': 'aborted' }) # Create the top level state machine self.sm_top = StateMachine( outcomes=['succeeded', 'aborted', 'preempted']) # Add nav_patrol, sm_recharge and a Stop() machine to sm_top with self.sm_top: StateMachine.add('ACTIONS', self.sm_actions, transitions={ 'succeeded': 'ACTIONS', 'aborted': 'STOP' }) #StateMachine.add('RECHARGE', self.sm_recharge, transitions={'succeeded':'PATROL'}) StateMachine.add('STOP', Stop(), transitions={'succeeded': ''}) # Create and start the SMACH introspection server intro_server = IntrospectionServer('patrol', self.sm_top, '/SM_ROOT') intro_server.start() # Execute the state machine sm_outcome = self.sm_top.execute() rospy.loginfo('State Machine Outcome: ' + str(sm_outcome)) intro_server.stop() def time_cb(self, userdata, msg): if msg.data < 2: self.stopping = True return False else: self.stopping = False return True def start_cb(self, userdata, msg): rospy.loginfo("Start !") return False def color_cb(self, userdata, msg): rospy.loginfo("Color " + str(msg.data)) self.robot_side = msg.data self.sm_action1.userdata.robot_side = self.robot_side self.sm_action2.userdata.robot_side = self.robot_side self.sm_action3.userdata.robot_side = self.robot_side self.sm_action4.userdata.robot_side = self.robot_side self.sm_action5.userdata.robot_side = self.robot_side self.sm_action6.userdata.robot_side = self.robot_side self.sm_action7.userdata.robot_side = self.robot_side self.sm_top.userdata.robot_side = self.robot_side # TODO REMOVE return False def battery_cb(self, userdata, msg): if msg.data < 320: self.recharging = True return False else: self.recharging = False return True def objective_cb(self, userdata, response): #objective_response = GetObjective().Response userdata.waypoint_out = response.goal waypoint_type = response.type.data rospy.loginfo("goal: " + str(response.goal)) if (waypoint_type == 1): return 'action1' if (waypoint_type == 2): return 'action2' if (waypoint_type == 3): return 'action3' if (waypoint_type == 4): return 'action4' if (waypoint_type == 5): return 'action5' if (waypoint_type == 6): return 'action6' if (waypoint_type == 7): return 'action7' return 'aborted' def requestPrioCube_cb(self, userdata, request): rospy.loginfo("Side : " + str(userdata.robot_side)) update_request = UpdatePriorityRequest() update_request.goal.pose.position.x = userdata.robot_side * (1.500 - 1.300) update_request.goal.pose.position.y = 1.000 update_request.prio.data = 100 rospy.loginfo("Request Priority Drop cubes update") return update_request #request.goal.pose.position.x = userdata.robot_side*(1.500 - 1.300) #request.goal.pose.position.y = 1.000 #request.prio = 100 #return request def updatePrioCube_cb(self, userdata, response): rospy.loginfo("Priority Drop cubes updated") return 'aborted' def requestPrioShell_cb(self, userdata, request): rospy.loginfo("Side : " + str(userdata.robot_side)) update_request = UpdatePriorityRequest() update_request.goal.pose.position.x = userdata.robot_side * (1.500 - 0.300) update_request.goal.pose.position.y = 0.800 update_request.prio.data = 100 rospy.loginfo("Request Priority Drop shell update") return update_request #request.goal.pose.position.x = userdata.robot_side*(1.500 - 0.300) #request.goal.pose.position.y = 0.800 #request.prio = 100 #return request def updatePrioShell_cb(self, userdata, response): rospy.loginfo("Priority Drop shell updated") return 'aborted' # Gets called when ANY child state terminates def concurrence_child_termination_cb(self, outcome_map): # If the current navigation task has succeeded, return True if outcome_map['SM_ACTIONS'] == 'succeeded': return True # If the MonitorState state returns False (invalid), store the current nav goal and recharge if outcome_map['MONITOR_TIME'] == 'invalid': rospy.loginfo("LOW TIME! NEED TO STOP...") return True if outcome_map['MONITOR_BATTERY'] == 'invalid': rospy.loginfo("LOW BATTERY! NEED TO STOP...") return True else: return False # Gets called when ALL child states are terminated def concurrence_outcome_cb(self, outcome_map): # If the battery is below threshold, return the 'recharge' outcome if outcome_map['MONITOR_TIME'] == 'invalid': rospy.loginfo("TIME FINISHED !! GOING TO STOP ! ") return 'stop' if outcome_map['MONITOR_BATTERY'] == 'invalid': return 'stop' # Otherwise, if the last nav goal succeeded, return 'succeeded' or 'stop' elif outcome_map['SM_ACTIONS'] == 'succeeded': #self.patrol_count += 1 #rospy.loginfo("FINISHED PATROL LOOP: " + str(self.patrol_count)) # If we have not completed all our patrols, start again at the beginning #if self.n_patrols == -1 or self.patrol_count < self.n_patrols: #self.sm_nav.set_initial_state(['NAV_STATE_0'], UserData()) return 'succeeded' # Otherwise, we are finished patrolling so return 'stop' #else: #self.sm_nav.set_initial_state(['NAV_STATE_4'], UserData()) #return 'stop' # Recharge if all else fails else: return 'recharge' def shutdown(self): rospy.loginfo("Stopping the robot...") self.sm_actions.request_preempt() self.cmd_vel_pub.publish(Twist()) rospy.sleep(1)
class Patrol(): def __init__(self): rospy.init_node('patrol_smach_concurrence', anonymous=False) # Set the shutdown function (stop the robot) rospy.on_shutdown(self.shutdown) # Initialize a number of parameters and variables setup_task_environment(self) # Track success rate of getting to the goal locations self.n_succeeded = 0 self.n_aborted = 0 self.n_preempted = 0 # A variable to hold the last/current navigation goal self.last_nav_state = None # A flag to indicate whether or not we are rechargin self.recharging = False # A list to hold then navigation goa nav_states = list() # Turn the waypoints into SMACH states for waypoint in self.waypoints: nav_goal = MoveBaseGoal() nav_goal.target_pose.header.frame_id = 'map' nav_goal.target_pose.pose = waypoint move_base_state = SimpleActionState('move_base', MoveBaseAction, goal=nav_goal, result_cb=self.move_base_result_cb, exec_timeout=rospy.Duration(40.0), server_wait_timeout=rospy.Duration(10.0)) nav_states.append(move_base_state) # Create a MoveBaseAction state for the docking station nav_goal = MoveBaseGoal() nav_goal.target_pose.header.frame_id = 'map' nav_goal.target_pose.pose = self.docking_station_pose nav_docking_station = SimpleActionState('move_base', MoveBaseAction, goal=nav_goal, result_cb=self.move_base_result_cb, exec_timeout=rospy.Duration(40.0), server_wait_timeout=rospy.Duration(10.0)) # Initialize the navigation state machine self.sm_nav = StateMachine(outcomes=['succeeded', 'aborted', 'preempted']) # Add the nav states to the state machine with the appropriate transitions with self.sm_nav: StateMachine.add('NAV_STATE_0', nav_states[0], transitions={'succeeded':'NAV_STATE_1','aborted':'NAV_STATE_1'}) StateMachine.add('NAV_STATE_1', nav_states[1], transitions={'succeeded':'NAV_STATE_2','aborted':'NAV_STATE_2'}) StateMachine.add('NAV_STATE_2', nav_states[2], transitions={'succeeded':'NAV_STATE_3','aborted':'NAV_STATE_3'}) StateMachine.add('NAV_STATE_3', nav_states[3], transitions={'succeeded':'NAV_STATE_4','aborted':'NAV_STATE_4'}) StateMachine.add('NAV_STATE_4', nav_states[0], transitions={'succeeded':'','aborted':''}) # Register a callback function to fire on state transitions within the sm_nav state machine self.sm_nav.register_transition_cb(self.nav_transition_cb, cb_args=[]) # Initialize the recharge state machine self.sm_recharge = StateMachine(outcomes=['succeeded', 'aborted', 'preempted']) with self.sm_recharge: StateMachine.add('NAV_DOCKING_STATION', nav_docking_station, transitions={'succeeded':''}) #StateMachine.add('RECHARGE_BATTERY', ServiceState('battery_simulator/set_battery_level', SetBatteryLevel, 100, response_cb=self.recharge_cb), #transitions={'succeeded':''}) # Create the nav_patrol state machine using a Concurrence container self.nav_patrol = Concurrence(outcomes=['succeeded', 'recharge', 'stop'], default_outcome='succeeded', child_termination_cb=self.concurrence_child_termination_cb, outcome_cb=self.concurrence_outcome_cb) # Add the sm_nav machine and a battery MonitorState to the nav_patrol machine with self.nav_patrol: Concurrence.add('SM_NAV', self.sm_nav) Concurrence.add('MONITOR_BATTERY', MonitorState("battery_level", Float32, self.battery_cb)) # Create the top level state machine self.sm_top = StateMachine(outcomes=['succeeded', 'aborted', 'preempted']) # Add nav_patrol, sm_recharge and a Stop() machine to sm_top with self.sm_top: StateMachine.add('PATROL', self.nav_patrol, transitions={'succeeded':'PATROL', 'recharge':'RECHARGE', 'stop':'STOP'}) StateMachine.add('RECHARGE', self.sm_recharge, transitions={'succeeded':'PATROL'}) StateMachine.add('STOP', Stop(), transitions={'succeeded':''}) # Create and start the SMACH introspection server intro_server = IntrospectionServer('patrol', self.sm_top, '/SM_ROOT') intro_server.start() # Execute the state machine sm_outcome = self.sm_top.execute() rospy.loginfo('State Machine Outcome: ' + str(sm_outcome)) intro_server.stop() def nav_transition_cb(self, userdata, active_states, *cb_args): self.last_nav_state = active_states # Gets called when ANY child state terminates def concurrence_child_termination_cb(self, outcome_map): # If the current navigation task has succeeded, return True if outcome_map['SM_NAV'] == 'succeeded': return True # If the MonitorState state returns False (invalid), store the current nav goal and recharge if outcome_map['MONITOR_BATTERY'] == 'invalid': rospy.loginfo("LOW BATTERY! NEED TO RECHARGE...") if self.last_nav_state is not None: self.sm_nav.set_initial_state(self.last_nav_state, UserData()) return True else: return False # Gets called when ALL child states are terminated def concurrence_outcome_cb(self, outcome_map): # If the battery is below threshold, return the 'recharge' outcome if outcome_map['MONITOR_BATTERY'] == 'invalid': return 'recharge' # Otherwise, if the last nav goal succeeded, return 'succeeded' or 'stop' elif outcome_map['SM_NAV'] == 'succeeded': self.patrol_count += 1 rospy.loginfo("FINISHED PATROL LOOP: " + str(self.patrol_count)) # If we have not completed all our patrols, start again at the beginning if self.n_patrols == -1 or self.patrol_count < self.n_patrols: self.sm_nav.set_initial_state(['NAV_STATE_0'], UserData()) return 'succeeded' # Otherwise, we are finished patrolling so return 'stop' else: self.sm_nav.set_initial_state(['NAV_STATE_4'], UserData()) return 'stop' # Recharge if all else fails else: return 'recharge' def battery_cb(self, userdata, msg): if msg.data < self.low_battery_threshold: self.recharging = True return False else: self.recharging = False return True def recharge_cb(self, userdata, response): return 'succeeded' def move_base_result_cb(self, userdata, status, result): if not self.recharging: if status == actionlib.GoalStatus.SUCCEEDED: self.n_succeeded += 1 elif status == actionlib.GoalStatus.ABORTED: self.n_aborted += 1 elif status == actionlib.GoalStatus.PREEMPTED: self.n_preempted += 1 try: rospy.loginfo("Success rate: " + str(100.0 * self.n_succeeded / (self.n_succeeded + self.n_aborted + self.n_preempted))) except: pass def shutdown(self): rospy.loginfo("Stopping the robot...") self.sm_nav.request_preempt() self.cmd_vel_pub.publish(Twist()) rospy.sleep(1)
class Patrol(): def __init__(self): rospy.init_node('patrol_smach_concurrence', anonymous=False) # Set the shutdown function (stop the robot) rospy.on_shutdown(self.shutdown) # Initialize a number of parameters and variables setup_task_environment(self) # Track success rate of getting to the goal locations self.n_succeeded = 0 self.n_aborted = 0 self.n_preempted = 0 # A variable to hold the last/current navigation goal self.last_nav_state = None # A flag to indicate whether or not we are rechargin self.recharging = False # A list to hold then navigation goa nav_states = list() location_goal = MoveBaseGoal() result_goal = MoveBaseGoal() #recognize_goal = VisionGoal() #flag = 1 class wait(State): def __init__(self): State.__init__(self, outcomes=['succeeded', 'aborted']) def execute(self, userdata): if flag == 0: rospy.loginfo('waitting') return 'aborted' else: return 'succeeded' class MoveItDemo(State): # spin() simply keeps python from exiting until this node is stopped def __init__(self): # Initialize the move_group API State.__init__(self, outcomes=['succeeded', 'aborted']) def execute(self, userdata): moveit_commander.roscpp_initialize(sys.argv) #rospy.init_node('moveit_ik') #rospy.Subscriber("chatter", Pose, callback) # Initialize the move group for the right arm right_arm = moveit_commander.MoveGroupCommander('right_arm') # Get the name of the end-effector link end_effector_link = right_arm.get_end_effector_link() # Set the reference frame for pose targets reference_frame = 'base_footprint' # Set the right arm reference frame accordingly right_arm.set_pose_reference_frame(reference_frame) # Allow replanning to increase the odds of a solution right_arm.allow_replanning(True) # Allow some leeway in position (meters) and orientation (radians) right_arm.set_goal_position_tolerance(0.11) right_arm.set_goal_orientation_tolerance(0.15) # Start the arm in the "resting" pose stored in the SRDF file right_arm.set_named_target('right_arm_init') right_arm.go() rospy.sleep(2) # Set the target pose. This particular pose has the gripper oriented horizontally # 0.85 meters above the ground, 0.10 meters to the right and 0.20 meters ahead of # the center of the robot base. target_pose = PoseStamped() target_pose.header.frame_id = reference_frame target_pose.header.stamp = rospy.Time.now() #global a,b,c,d,e,f,g target_pose.pose.position.x = 0.3 target_pose.pose.position.y = -0.45 target_pose.pose.position.z = 1.35 target_pose.pose.orientation.x = 0 target_pose.pose.orientation.y = 0 target_pose.pose.orientation.z = 0 target_pose.pose.orientation.w = 1 #Set the start state to the current state right_arm.set_start_state_to_current_state() #print a # Set the goal pose of the end effector to the stored pose right_arm.set_pose_target(target_pose, end_effector_link) # Plan the trajectory to the goal traj = right_arm.plan() # Execute the planned trajectory right_arm.execute(traj) # Pause for a second rospy.sleep(5) #right_arm.set_named_target('right_arm_init') right_arm.go() return 'succeeded' class MoveItDemo1(State): # spin() simply keeps python from exiting until this node is stopped def __init__(self): # Initialize the move_group API State.__init__(self, outcomes=['succeeded', 'aborted']) def execute(self, userdata): moveit_commander.roscpp_initialize(sys.argv) #rospy.init_node('moveit_ik') #rospy.Subscriber("chatter", Pose, callback) # Initialize the move group for the right arm right_arm = moveit_commander.MoveGroupCommander('right_arm') # Get the name of the end-effector link end_effector_link = right_arm.get_end_effector_link() # Set the reference frame for pose targets reference_frame = 'base_footprint' # Set the right arm reference frame accordingly right_arm.set_pose_reference_frame(reference_frame) # Allow replanning to increase the odds of a solution right_arm.allow_replanning(True) right_arm.set_named_target('right_arm_init') right_arm.go() return 'succeeded' # Turn the waypoints into SMACH states for waypoint in self.waypoints: nav_goal = MoveBaseGoal() nav_goal.target_pose.header.frame_id = 'map' nav_goal.target_pose.pose = waypoint move_base_state = SimpleActionState( 'move_base', MoveBaseAction, goal=nav_goal, result_cb=self.move_base_result_cb, exec_timeout=rospy.Duration(20.0), server_wait_timeout=rospy.Duration(20.0)) nav_states.append(move_base_state) # Create a MoveBaseAction state for the docking station nav_goal = MoveBaseGoal() nav_goal.target_pose.header.frame_id = 'map' nav_goal.target_pose.pose = self.docking_station_pose nav_docking_station = SimpleActionState( 'move_base', MoveBaseAction, goal=nav_goal, result_cb=self.move_base_result_cb, exec_timeout=rospy.Duration(20.0), server_wait_timeout=rospy.Duration(10.0)) pose_target = geometry_msgs.msg.Pose() pose_target.orientation.w = 0.1 pose_target.position.x = 0.7 pose_target.position.y = -0.0 pose_target.position.z = 1.1 result_goal.target_pose.header.frame_id = 'map' result_goal.target_pose.pose = (Pose(Point(0.5, 1.5, 0.0), Quaternion(0.0, 0.0, 0.0, 1.0))) # Initialize the navigation state machine self.sm_nav = StateMachine( outcomes=['succeeded', 'aborted', 'preempted']) # Add the nav states to the state machine with the appropriate transitions with self.sm_nav: StateMachine.add('WAITTING', wait(), transitions={ 'succeeded': 'NAV_STATE_0', 'aborted': 'WAITTING' }) StateMachine.add('NAV_STATE_0', nav_states[0], transitions={ 'succeeded': 'NAV_STATE_1', 'aborted': 'NAV_STATE_1' }) StateMachine.add('NAV_STATE_1', SimpleActionState('move_base', MoveBaseAction, goal=location_goal), transitions={ 'succeeded': 'ARM', 'aborted': 'ARM' }) #StateMachine.add('VISION', SimpleActionState('drv_action', VisionAction, goal=recognize_goal), #transitions={'succeeded': 'ARM', 'aborted': 'ARM'}) StateMachine.add('ARM', MoveItDemo(), transitions={ 'succeeded': 'NAV_STATE_2', 'aborted': 'NAV_STATE_2' }) StateMachine.add('NAV_STATE_2', nav_states[0], transitions={ 'succeeded': 'ARM1', 'aborted': 'ARM1' }) StateMachine.add('ARM1', MoveItDemo1(), transitions={ 'succeeded': '', 'aborted': '' }) # StateMachine.add('NAV_STATE_2', SimpleActionState('move_base', MoveBaseAction, goal_slots=['target_pose']), # transitions={'succeeded': 'NAV_STATE_0'}, remapping={'target_pose': 'user_data'}) # Register a callback function to fire on state transitions within the sm_nav state machine self.sm_nav.register_transition_cb(self.nav_transition_cb, cb_args=[]) # Initialize the recharge state machine self.sm_recharge = StateMachine( outcomes=['succeeded', 'aborted', 'preempted']) with self.sm_recharge: StateMachine.add('NAV_DOCKING_STATION', nav_docking_station, transitions={'succeeded': 'RECHARGE_BATTERY'}) StateMachine.add('RECHARGE_BATTERY', ServiceState( 'battery_simulator/set_battery_level', SetBatteryLevel, 100, response_cb=self.recharge_cb), transitions={'succeeded': ''}) # Create the nav_patrol state machine using a Concurrence container self.nav_patrol = Concurrence( outcomes=['succeeded', 'recharge', 'stop'], default_outcome='succeeded', child_termination_cb=self.concurrence_child_termination_cb, outcome_cb=self.concurrence_outcome_cb) # Add the sm_nav machine and a battery MonitorState to the nav_patrol machine with self.nav_patrol: Concurrence.add('SM_NAV', self.sm_nav) # Concurrence.add('MONITOR_BATTERY', MonitorState("battery_level", Float32, self.battery_cb)) Concurrence.add( 'LOCATION_GOAL', MonitorState("nav_location_goal", Pose, self.nav_location_goal_cb)) Concurrence.add( 'RECOGNIZE_GOAL', MonitorState("/comm/msg/control/recognize_goal", String, self.recognize_goal_cb)) #Concurrence.add('RESULT', MonitorState("/drv_action/result", VisionActionResult, self.result_goal_cb)) # Create the top level state machine self.sm_top = StateMachine( outcomes=['succeeded', 'aborted', 'preempted']) # Add nav_patrol, sm_recharge and a Stop() machine to sm_top with self.sm_top: StateMachine.add('PATROL', self.nav_patrol, transitions={ 'succeeded': 'PATROL', 'recharge': 'RECHARGE', 'stop': 'STOP' }) StateMachine.add('RECHARGE', self.sm_recharge, transitions={'succeeded': 'PATROL'}) StateMachine.add('STOP', Stop(), transitions={'succeeded': ''}) rospy.loginfo('=============ce shi=============') time.sleep(5) # Create and start the SMACH introspection server intro_server = IntrospectionServer('patrol', self.sm_top, '/SM_ROOT') intro_server.start() # Execute the state machine sm_outcome = self.sm_top.execute() rospy.loginfo('State Machine Outcome: ' + str(sm_outcome)) intro_server.stop() def nav_transition_cb(self, userdata, active_states, *cb_args): self.last_nav_state = active_states # Gets called when ANY child state terminates def concurrence_child_termination_cb(self, outcome_map): # If the current navigation task has succeeded, return True if outcome_map['SM_NAV'] == 'succeeded': return True # If the MonitorState state returns False (invalid), store the current nav goal and recharge if outcome_map['MONITOR_BATTERY'] == 'invalid': rospy.loginfo("LOW BATTERY! NEED TO RECHARGE...") if self.last_nav_state is not None: self.sm_nav.set_initial_state(self.last_nav_state, UserData()) return True else: return False # Gets called when ALL child states are terminated def concurrence_outcome_cb(self, outcome_map): # If the battery is below threshold, return the 'recharge' outcome if outcome_map['MONITOR_BATTERY'] == 'invalid': return 'recharge' # Otherwise, if the last nav goal succeeded, return 'succeeded' or 'stop' elif outcome_map['SM_NAV'] == 'succeeded': self.patrol_count += 1 rospy.loginfo("FINISHED PATROL LOOP: " + str(self.patrol_count)) # If we have not completed all our patrols, start again at the beginning if self.n_patrols == -1 or self.patrol_count < self.n_patrols: self.sm_nav.set_initial_state(['NAV_STATE_0'], UserData()) return 'succeeded' # Otherwise, we are finished patrolling so return 'stop' else: self.sm_nav.set_initial_state(['NAV_STATE_4'], UserData()) return 'stop' # Recharge if all else fails else: return 'recharge' def battery_cb(self, userdata, msg): if msg.data < self.low_battery_threshold: self.recharging = True return False else: self.recharging = False return True def nav_location_goal_cb(self, userdata, msg): #if msg != []: global flag flag = 1 self.nav_patrol._states["SM_NAV"]._states[ "NAV_STATE_1"]._goal.target_pose.header.frame_id = 'map' self.nav_patrol._states["SM_NAV"]._states[ "NAV_STATE_1"]._goal.target_pose.pose = msg return True def recognize_goal_cb(self, userdata, msg): self.nav_patrol._states["SM_NAV"]._states["VISION"]._goal.mode = 1 self.nav_patrol._states["SM_NAV"]._states[ "VISION"]._goal.target_label = msg.data return True #def result_goal_cb(self, userdata, msg): # self.nav_patrol._states["SM_NAV"]._states["VISION"]._goal.mode = 1 #self.nav_patrol._states["SM_NAV"]._states["ARM"]._goal.target_pose.header.frame_id = 'map' #self.nav_patrol._states["SM_NAV"]._states["ARM"]._goal.target_pose.pose = msg.result.target_location.pose #rospy.loginfo(self.nav_patrol._states["SM_NAV"]._states["ARM"]._goal) #return True def recharge_cb(self, userdata, response): return 'succeeded' def move_base_result_cb(self, userdata, status, result): if not self.recharging: if status == alm.GoalStatus.SUCCEEDED: self.n_succeeded += 1 elif status == alm.GoalStatus.ABORTED: self.n_aborted += 1 elif status == alm.GoalStatus.PREEMPTED: self.n_preempted += 1 rospy.loginfo( "Success rate: " + str(100.0 * self.n_succeeded / (self.n_succeeded + self.n_aborted + self.n_preempted))) def shutdown(self): rospy.loginfo("Stopping the robot...") self.sm_nav.request_preempt() self.cmd_vel_pub.publish(Twist()) rospy.sleep(1)
class Patrol(): def __init__(self): rospy.init_node('patrol_smach', anonymous=False) # Set the shutdown function (stop the robot) rospy.on_shutdown(self.shutdown) # Initialize a number of parameters and variables setup_task_environment(self) # Track success rate of getting to the goal locations self.n_succeeded = 0 self.n_aborted = 0 self.n_preempted = 0 self.patrol_count = 0 self.n_patrols = 1 # A random number generator for use in the transition callback self.rand = Random() # A list to hold then nav_states nav_states = list() # Turn the waypoints into SMACH states for waypoint in self.waypoints: nav_goal = MoveBaseGoal() nav_goal.target_pose.header.frame_id = 'map' nav_goal.target_pose.pose = waypoint move_base_state = SimpleActionState('move_base', MoveBaseAction, goal=nav_goal, result_cb=self.move_base_result_cb, exec_timeout=rospy.Duration(10.0), server_wait_timeout=rospy.Duration(10.0)) nav_states.append(move_base_state) # Initialize the patrol state machine self.sm_patrol = StateMachine(outcomes=['succeeded','aborted','preempted']) # Register a callback function to fire on state transitions within the sm_patrol state machine self.sm_patrol.register_transition_cb(self.transition_cb, cb_args=[]) # Add the states to the state machine with the appropriate transitions with self.sm_patrol: StateMachine.add('NAV_STATE_0', nav_states[0], transitions={'succeeded':'NAV_STATE_1','aborted':'NAV_STATE_1'}) StateMachine.add('NAV_STATE_1', nav_states[1], transitions={'succeeded':'NAV_STATE_2','aborted':'NAV_STATE_2'}) StateMachine.add('NAV_STATE_2', nav_states[2], transitions={'succeeded':'NAV_STATE_3','aborted':'NAV_STATE_3'}) StateMachine.add('NAV_STATE_3', nav_states[3], transitions={'succeeded':'NAV_STATE_4','aborted':'NAV_STATE_4'}) StateMachine.add('NAV_STATE_4', nav_states[0], transitions={'succeeded':'NAV_STATE_0','aborted':'NAV_STATE_0'}) StateMachine.add('CHECK_COUNT', CheckCount(self.n_patrols), transitions={'succeeded':'NAV_STATE_0','aborted':'','preempted':''}) # Create and start the SMACH introspection server intro_server = IntrospectionServer('patrol', self.sm_patrol, '/SM_ROOT') intro_server.start() # Execute the state machine self.sm_outcome = self.sm_patrol.execute() rospy.loginfo('State Machine Outcome: ' + str(self.sm_outcome)) intro_server.stop() os._exit(0) def move_base_result_cb(self, userdata, status, result): if status == actionlib.GoalStatus.SUCCEEDED: self.n_succeeded += 1 elif status == actionlib.GoalStatus.ABORTED: self.n_aborted += 1 elif status == actionlib.GoalStatus.PREEMPTED: self.n_preempted += 1 try: rospy.loginfo("Success rate: " + str(100.0 * self.n_succeeded / (self.n_succeeded + self.n_aborted + self.n_preempted))) except: pass def transition_cb(self, userdata, active_states, *cb_args): if self.rand.randint(0, 3) == 0: rospy.loginfo("Greetings human!") rospy.sleep(1) rospy.loginfo("Is everything OK?") rospy.sleep(1) else: rospy.loginfo("Nobody here.") def shutdown(self): rospy.loginfo("Stopping the robot...") self.sm_patrol.request_preempt() self.cmd_vel_pub.publish(Twist()) rospy.sleep(1)