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 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)
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 TaskManager(): def __init__(self): # init node rospy.init_node('pool_patrol', anonymous=False) # Set the shutdown fuction (stop the robot) rospy.on_shutdown(self.shutdown) # Initilalize the mission parameters and variables setup_task_environment(self) # A variable to hold the last/current mission goal/state self.last_mission_state = None # A flag to indicate whether or not we are recharging self.recharging = False # Turn the target locations into SMACH MoveBase and LosPathFollowing action states nav_terminal_states = {} nav_transit_states = {} # DP controller for target in self.pool_locations.iterkeys(): nav_goal = MoveBaseGoal() nav_goal.target_pose.header.frame_id = 'odom' nav_goal.target_pose.pose = self.pool_locations[target] move_base_state = SimpleActionState( 'move_base', MoveBaseAction, goal=nav_goal, result_cb=self.nav_result_cb, exec_timeout=self.nav_timeout, server_wait_timeout=rospy.Duration(10.0)) nav_terminal_states[target] = move_base_state # Path following for target in self.pool_locations.iterkeys(): nav_goal = LosPathFollowingGoal() #nav_goal.prev_waypoint = navigation.vehicle_pose.position nav_goal.next_waypoint = self.pool_locations[target].position nav_goal.forward_speed.linear.x = self.transit_speed nav_goal.desired_depth.z = self.search_depth nav_goal.sphereOfAcceptance = self.search_area_size los_path_state = SimpleActionState( 'los_path', LosPathFollowingAction, goal=nav_goal, result_cb=self.nav_result_cb, exec_timeout=self.nav_timeout, server_wait_timeout=rospy.Duration(10.0)) nav_transit_states[target] = los_path_state """ Create individual state machines for assigning tasks to each target zone """ # Create a state machine container for the orienting towards the gate subtask(s) self.sm_gate_tasks = StateMachine(outcomes=[ 'found', 'unseen', 'missed', 'passed', 'aborted', 'preempted' ]) # Then add the subtask(s) with self.sm_gate_tasks: # if gate is found, pass pixel info onto TrackTarget. If gate is not found, look again StateMachine.add('SCANNING_OBJECTS', SearchForTarget('gate'), transitions={ 'found': 'CAMERA_CENTERING', 'unseen': 'BROADEN_SEARCH', 'passed': '', 'missed': '' }, remapping={ 'px_output': 'object_px', 'fx_output': 'object_fx', 'search_output': 'object_search', 'search_confidence_output': 'object_confidence' }) StateMachine.add('CAMERA_CENTERING', TrackTarget('gate', self.pool_locations['gate'].position), transitions={'succeeded': 'SCANNING_OBJECTS'}, remapping={ 'px_input': 'object_px', 'fx_input': 'object_fx', 'search_input': 'object_search', 'search_confidence_input': 'object_confidence' }) StateMachine.add('BROADEN_SEARCH', TrackTarget('gate', self.pool_locations['gate'].position), transitions={'succeeded': 'SCANNING_OBJECTS'}, remapping={ 'px_input': 'object_px', 'fx_input': 'object_fx', 'search_input': 'object_search', 'search_confidence_input': 'object_confidence' }) # Create a state machine container for returning to dock self.sm_docking = StateMachine( outcomes=['succeeded', 'aborted', 'preempted']) # Add states to container with self.sm_docking: StateMachine.add('RETURN_TO_DOCK', nav_transit_states['docking'], transitions={ 'succeeded': 'DOCKING_SECTOR', 'aborted': '', 'preempted': 'RETURN_TO_DOCK' }) StateMachine.add('DOCKING_SECTOR', ControlMode(POSE_HEADING_HOLD), transitions={ 'succeeded': 'DOCKING_PROCEEDURE', 'aborted': '', 'preempted': '' }) StateMachine.add('DOCKING_PROCEEDURE', nav_terminal_states['docking'], transitions={ 'succeeded': '', 'aborted': '', 'preempted': '' }) """ Assemble a Hierarchical State Machine """ # Initialize the HSM self.sm_mission = StateMachine(outcomes=[ 'succeeded', 'aborted', 'preempted', 'passed', 'missed', 'unseen', 'found' ]) # Build the HSM from nav states and target states with self.sm_mission: """ Navigate to GATE in TERMINAL mode """ StateMachine.add('TRANSIT_TO_GATE', nav_transit_states['gate'], transitions={ 'succeeded': 'GATE_SEARCH', 'aborted': 'DOCKING', 'preempted': 'TRANSIT_TO_GATE' }) """ When in GATE sector""" StateMachine.add('GATE_SEARCH', self.sm_gate_tasks, transitions={ 'passed': 'GATE_PASSED', 'missed': 'TRANSIT_TO_GATE', 'aborted': 'DOCKING' }) """ Transiting to gate """ StateMachine.add('GATE_PASSED', ControlMode(OPEN_LOOP), transitions={ 'succeeded': 'TRANSIT_TO_POLE', 'aborted': 'DOCKING', 'preempted': 'TRANSIT_TO_GATE' }) StateMachine.add('TRANSIT_TO_POLE', nav_transit_states['pole'], transitions={ 'succeeded': '', 'aborted': '', 'preempted': '' }) """ When in POLE sector""" #StateMachine.add('POLE_PASSING_TASK', sm_pole_tasks, transitions={'passed':'POLE_PASSING_TASK','missed':'RETURN_TO_DOCK','aborted':'RETURN_TO_DOCK'}) """ When aborted, return to docking """ StateMachine.add('DOCKING', self.sm_docking, transitions={'succeeded': ''}) # Register a callback function to fire on state transitions within the sm_mission state machine self.sm_mission.register_transition_cb(self.mission_transition_cb, cb_args=[]) # Initialize the recharge state machine self.sm_recharge = StateMachine( outcomes=['succeeded', 'aborted', 'preempted']) # Add states to the container with self.sm_recharge: """ To be able to dock we have to be in the recharging area """ StateMachine.add('DOCKING', self.sm_docking, transitions={'succeeded': 'RECHARGE_BATTERY'}) """ Batteru is recharged by using the set_battery_level service to 100 percent battery level """ StateMachine.add('RECHARGE_BATTERY', ServiceState( 'battery_simulator/set_battery_level', SetBatteryLevel, 100, response_cb=self.recharge_cb), transitions={'succeeded': 'RECHARGE_FINISHED'}) """ when recharge is finished, we have to set the DP controller in open loop so we can use the transit controller """ StateMachine.add('RECHARGE_FINISHED', ControlMode(OPEN_LOOP), transitions={'succeeded': ''}) # Create the sm_concurrence state machine using a concurrence container self.sm_concurrence = 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_mission and a battery MonitorState to the sm_concurrence container with self.sm_concurrence: Concurrence.add('SM_MISSION', self.sm_mission) Concurrence.add( 'MONITOR_BATTERY', MonitorState("/manta/battery_level", Float32, self.battery_cb)) # Create the top level state machine self.sm_top = StateMachine( outcomes=['succeeded', 'aborted', 'preempted']) # Add sm_concurrence, sm_recharge and a Stop() objective to sm_top with self.sm_top: StateMachine.add('PATROL', self.sm_concurrence, transitions={ 'succeeded': '', '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(str(rospy.get_name()), self.sm_top, '/SM_ROOT') intro_server.start() # Execute the state machine hsm_outcome = self.sm_top.execute() intro_server.stop() def mission_transition_cb(self, userdata, active_states, *cb_args): self.last_mission_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_MISSION'] == 'succeeded': return True # If the MonitorState state returns False (invalid), store the current mission goal and recharge if outcome_map['MONITOR_BATTERY'] == 'invalid': rospy.loginfo("LOW BATTERY! NEED TO RECHARGE...") if self.last_mission_state is not None: self.sm_mission.set_initial_state(self.last_mission_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 mission goal succeeded, return 'succeeded' or 'stop' elif outcome_map['SM_MISSION'] == 'succeeded': return 'succeeded' 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 nav_result_cb(self, userdata, status, result): if status == GoalStatus.PREEMPTED: rospy.loginfo("Waypoint preempted") if status == GoalStatus.SUCCEEDED: rospy.loginfo("Waypoint succeeded") def shutdown(self): rospy.loginfo("stopping the AUV...") #sm_nav.request_preempt() rospy.sleep(10)
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() # 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 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 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)