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)
