def test_userdata(self): """Test serial manipulation of userdata.""" pinger_thread = threading.Thread(target=pinger) pinger_thread.start() init_ud = UserData() init_ud.b = 'A' sm = StateMachine(['valid', 'invalid', 'preempted']) sm.set_initial_state(['MON'], userdata=init_ud) assert 'b' in sm.userdata assert sm.userdata.b == 'A' with sm: StateMachine.add( 'MON', MonitorState('/ping', Empty, cond_cb, input_keys=['b'], output_keys=['a'])) outcome = sm.execute() assert outcome == 'invalid' assert 'b' in sm.userdata assert sm.userdata.b == 'A' assert 'a' in sm.userdata assert sm.userdata.a == 'A'
def main(): sm = StateMachine(outcomes=['kill']) with sm: StateMachine.add('IDLE', Idle(), transitions={'explore': 'EXPLORE', \ 'stop': 'kill'}) StateMachine.add('EXPLORE', Explore(), \ transitions={'stop': 'IDLE'}) StateMachine.set_initial_state(sm, ['IDLE']) result = sm.execute() rospy.loginfo(result)
def test_introspection(self): """Test introspection system.""" # Construct state machine sm = StateMachine(['done']) sm2 = StateMachine(['done']) sm3 = StateMachine(['done']) with sm: # Note: the following "Getter" state should fail StateMachine.add('GETTER1', Getter(), {}) StateMachine.add('SM2', sm2, {'done': 'SM3'}) with sm2: StateMachine.add("SETTER", Setter(), {}) StateMachine.add('SM3', sm3, {'done': 'done'}) with sm3: StateMachine.add("SETTER", Setter(), {}) StateMachine.add('GETTER2', Getter(), {'done': 'SM2'}) sm.set_initial_state(['GETTER1']) sm2.set_initial_state(['SETTER']) sm3.set_initial_state(['SETTER']) # Run introspector intro_server = IntrospectionServer('intro_test', sm, '/intro_test') server_thread = threading.Thread(target=intro_server.start) server_thread.start() intro_client = IntrospectionClient() servers = intro_client.get_servers() while '/intro_test' not in servers and not rospy.is_shutdown(): servers = intro_client.get_servers() rospy.loginfo("Smach servers: " + str()) rospy.sleep(0.1) assert '/intro_test' in servers # Set initial state injected_ud = UserData() injected_ud.a = 'A' init_set = intro_client.set_initial_state('intro_test', '/intro_test', ['SM2'], injected_ud, timeout=rospy.Duration(10.0)) assert init_set outcome = sm.execute() assert outcome == 'done'
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 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('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)
def tasker(): rospy.init_node('tasker') wfg = WaitForGoalState() # We don't want multiple subscribers so we need one WaitForGoal state runner = SMACHRunner(rgoap_subclasses) ## sub machines sq_move_to_new_goal = Sequence(outcomes=['succeeded', 'aborted', 'preempted'], connector_outcome='succeeded') with sq_move_to_new_goal: Sequence.add('WAIT_FOR_GOAL', wfg) Sequence.add('MOVE_BASE_RGOAP', MoveBaseRGOAPState(runner)) sq_autonomous_rgoap = Sequence(outcomes=['preempted'], connector_outcome='succeeded') with sq_autonomous_rgoap: Sequence.add('SLEEP_UNTIL_ENABLED', get_sleep_until_smach_enabled_smach()) Sequence.add('AUTONOMOUS_RGOAP', AutonomousRGOAPState(runner), transitions={'succeeded':'SLEEP_UNTIL_ENABLED', 'aborted':'SLEEP'}) Sequence.add('SLEEP', SleepState(5), transitions={'succeeded':'SLEEP_UNTIL_ENABLED'}) ## tasker machine sm_tasker = StateMachine(outcomes=['succeeded', 'aborted', 'preempted', 'field_error', 'undefined_task'], input_keys=['task_goal']) with sm_tasker: ## add all tasks to be available # states using rgoap StateMachine.add('MOVE_TO_NEW_GOAL_RGOAP', sq_move_to_new_goal) StateMachine.add('INCREASE_AWARENESS_RGOAP', IncreaseAwarenessRGOAPState(runner)) StateMachine.add('AUTONOMOUS_RGOAP_CYCLE', sq_autonomous_rgoap) StateMachine.add('AUTONOMOUS_RGOAP_SINGLE_GOAL', AutonomousRGOAPState(runner)) # states from uashh_smach StateMachine.add('LOOK_AROUND', get_lookaround_smach()) StateMachine.add('GLIMPSE_AROUND', get_lookaround_smach(glimpse=True)) StateMachine.add('MOVE_ARM_CRAZY', get_lookaround_smach(crazy=True)) StateMachine.add('MOVE_TO_RANDOM_GOAL', get_random_goal_smach()) StateMachine.add('MOVE_TO_NEW_GOAL_AND_RETURN', task_go_and_return.get_go_and_return_smach()) StateMachine.add('PATROL_TO_NEW_GOAL', task_patrol.get_patrol_smach()) StateMachine.add('MOVE_AROUND', task_move_around.get_move_around_smach()) StateMachine.add('SLEEP_FIVE_SEC', SleepState(5)) ## now the task receiver is created and automatically links to ## all task states added above task_states_labels = sm_tasker.get_children().keys() task_states_labels.sort() # sort alphabetically and then by _RGOAP task_states_labels.sort(key=lambda label: '_RGOAP' in label, reverse=True) task_receiver_transitions = {'undefined_outcome':'undefined_task'} task_receiver_transitions.update({l:l for l in task_states_labels}) StateMachine.add('TASK_RECEIVER', UserDataToOutcomeState(task_states_labels, 'task_goal', lambda ud: ud.task_id), task_receiver_transitions) sm_tasker.set_initial_state(['TASK_RECEIVER']) rospy.loginfo('tasker starting, available tasks: %s', ', '.join(task_states_labels)) pub = rospy.Publisher('/task/available_tasks', String, latch=True) thread.start_new_thread(rostopic.publish_message, (pub, String, [', '.join(task_states_labels)], 1)) asw = ActionServerWrapper('activate_task', TaskActivationAction, wrapped_container=sm_tasker, succeeded_outcomes=['succeeded'], aborted_outcomes=['aborted', 'undefined_task'], preempted_outcomes=['preempted'], goal_key='task_goal' ) # Create and start the introspection server sis = IntrospectionServer('smach_tasker_action', sm_tasker, '/SM_ROOT') sis.start() asw.run_server() rospy.spin() sis.stop()
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)
def main(): rospy.init_node('mini_drc_toplevel') topic_name = '/dyros_jet/smach/transition' joint_init_goal = JointControlGoal() joint_init_goal.command.name = [ 'L_HipYaw', 'L_HipRoll', 'L_HipPitch', 'L_KneePitch', 'L_AnklePitch', 'L_AnkleRoll', 'R_HipYaw', 'R_HipRoll', 'R_HipPitch', 'R_KneePitch', 'R_AnklePitch', 'R_AnkleRoll', 'WaistPitch', 'WaistYaw', 'L_ShoulderPitch', 'L_ShoulderRoll', 'L_ShoulderYaw', 'L_ElbowRoll', 'L_WristYaw', 'L_WristRoll', 'L_HandYaw', 'R_ShoulderPitch', 'R_ShoulderRoll', 'R_ShoulderYaw', 'R_ElbowRoll', 'R_WristYaw', 'R_WristRoll', 'R_HandYaw', 'HeadYaw', 'HeadPitch', 'R_Gripper', 'L_Gripper' ] #msg.enable = [True, True, True, True, True, True, True, True, True, True, True, True, True, True, True, True, True, True, True, True, True, True, True, True, True, True, True, True, False, False, False, False] joint_init_goal.command.position = [ 0, 0.034906585, -0.034906585, 0.733038285, -0.6981317, -0.034906585, 0, -0.034906585, 0.0349065850, -0.733038285, 0.6981317, 0.034906585, 0, 0, 0.6981317008, -1.6580627893, -1.3962634016, -1.9198621771, 0, -1.2217304764, -0.1745329252, -0.6981317008, 1.6580627893, 1.3962634016, 1.9198621771, 0, 1.2217304764, 0.17453292519, 0, 0, 0, 0 ] joint_init_goal.command.duration = [ 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 0, 0, 0, 0 ] # Construct state machine mini_drc_sm = StateMachine(outcomes=['finished', 'aborted', 'preempted']) #run_mode = rospy.get_param('~run_mode', 'simulation') run_mode = rospy.get_param('~run_mode', 'real_robot') print(run_mode) # Set the initial state explicitly if run_mode == 'simulation': mini_drc_sm.set_initial_state(['READY']) elif run_mode == 'mujoco': mini_drc_sm.set_initial_state(['READY']) elif run_mode == 'real_robot': mini_drc_sm.set_initial_state(['POWER_OFF']) else: print 'unknown mode' #request = rt_dynamixel_msgs.srv.ModeSettingRequest(rt_dynamixel_msgs.srv.ModeSettingRequest.SETTING) #print(request) #request = rt_dynamixel_msgs.srv.MotorSettingRequest( #mode=rt_dynamixel_msgs.srv.MotorSettingRequest.SET_TORQUE_ENABLE,value=1) #print(request) with mini_drc_sm: StateMachine.add( 'POWER_OFF', StringTransitionState(topic_name, outcomes=['power_on']), {'power_on': 'SET_DXL_MODE_SETTING_MODE'}) StateMachine.add( 'SET_DXL_MODE_SETTING_MODE', ServiceState( '/rt_dynamixel/mode', rt_dynamixel_msgs.srv.ModeSetting, request=rt_dynamixel_msgs.srv.ModeSettingRequest( rt_dynamixel_msgs.srv.ModeSettingRequest.SETTING)), transitions={ 'succeeded': 'SET_DXL_TORQUE_ON', 'aborted': 'SET_DXL_MODE_SETTING_MODE' }) StateMachine.add( 'SET_DXL_TORQUE_ON', ServiceState('/rt_dynamixel/motor_set', rt_dynamixel_msgs.srv.MotorSetting, request=rt_dynamixel_msgs.srv.MotorSettingRequest( mode=rt_dynamixel_msgs.srv.MotorSettingRequest. SET_TORQUE_ENABLE, value=1)), transitions={ 'succeeded': 'SET_DXL_SYNC_DRIVE_ON', 'aborted': 'SET_DXL_MODE_SETTING_MODE' }) StateMachine.add( 'SET_DXL_SYNC_DRIVE_ON', ServiceState( '/rt_dynamixel/mode', rt_dynamixel_msgs.srv.ModeSetting, request=rt_dynamixel_msgs.srv.ModeSettingRequest( rt_dynamixel_msgs.srv.ModeSettingRequest.CONTROL_RUN)), transitions={ 'succeeded': 'READY', 'aborted': 'SET_DXL_MODE_SETTING_MODE' }) StateMachine.add('READY', StringTransitionState(topic_name, outcomes=['initialize_pose']), transitions={'initialize_pose': 'SET_INIT_POSITION'}) StateMachine.add('SET_INIT_POSITION', SimpleActionState('/dyros_jet/joint_control', JointControlAction, goal=joint_init_goal), transitions={'succeeded': 'READY_TO_MOVE'}) StateMachine.add('READY_TO_MOVE', StringTransitionState(topic_name, outcomes=['Mot1']), transitions={'Mot1': 'Motion1'}) StateMachine.add('Motion1', StringTransitionState(topic_name, outcomes=['Mot2']), transitions={'Mot2': 'Motion2'}) StateMachine.add('Motion2', StringTransitionState( topic_name, outcomes=['stair', 'door', 'initialize_pose1']), transitions={ 'stair': 'finished', 'door': 'finished', 'initialize_pose1': 'SET_INIT_POSITION' }) # Run state machine introspection server intro_server = smach_ros.IntrospectionServer('dyros_jet', mini_drc_sm, '/MINI_DRC') intro_server.start() mini_drc_sm.execute() rospy.spin() intro_server.stop() rospy.signal_shutdown('All done.')