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