def __init__(self, swarmie_name, app_root_dir):
self.swarmie_name = swarmie_name
self.swarmie_id = RlBehavior.NAME_TO_ID_MAP.index(swarmie_name)
self.emitter = EventEmitter()
self.mode = RlBehavior.MANUAL_MODE
self.tf = None
self.coord_xform = CoordinateTransform(RlBehavior.ARENA_X_RANGE,
RlBehavior.ARENA_Y_RANGE,
RlBehavior.GRID_QUANTIZATION)
self.arena = Arena(len(RlBehavior.NAME_TO_ID_MAP), self.coord_xform,
RlBehavior.NEST_X_TOP_LEFT, RlBehavior.NEST_DIMS,
self.emitter)
self.swarmie_state = SwarmieState(self.swarmie_name, self.swarmie_id,
self.coord_xform, self.emitter)
self.rl_state_rep = StateRepository(
list(range(len(RlBehavior.NAME_TO_ID_MAP))), self.swarmie_id,
RlBehavior.CUBE_COUNT, RlBehavior.GRID_QUANTIZATION, self.emitter)
self.model_file_path = os.path.join(
app_root_dir, 'rl_models', '{}_model.h5'.format(self.swarmie_name))
self.rl_agent = None
self.tag_state = None
self._latest_tag_list = None
self._current_action = None
self._map_center = None
self._map_current = None
# Publishers ---------------------------------------------------------------
self.status_pub = None
self.hb_pub = None
self.infolog_pub = None
self.drive_cmd_pub = None
self.wrist_cmd_pub = None
self.fingers_cmd_pub = None
# Timers -------------------------------------------------------------------
self.status_timer = None
self.timestep_timer = None
self.hb_timer = None
# Subscribers --------------------------------------------------------------
self.map_sub = None
self.odom_sub = None
self.mode_sub = None
self.sonar_left_sub = None
self.sonar_center_sub = None
self.sonar_right_sub = None
self.sonar_sync = None
self.joy_sub = None
self.target_sub = None
self.maint_cmd_sub = None
# the COPYING file for more details.
from events import EventEmitter
if __name__ == '__main__':
print("""Expected output:
a2
b2
[<function a at ...>, <function b at ...>]
b3
3
[]
[]
""")
ee = EventEmitter()
ee.emit('log', 1)
def a(p):
print('a' + repr(p))
def b(p):
print('b' + repr(p))
ee.on('log', a)
ee.on('log', b)
ee.emit('log', 2)
print(ee.listeners('log'))
ee.remove_listener('log', a)
ee.emit('log', 3)
ee.on('log', b)
ee.on('log', b)
print(EventEmitter.listener_count(ee, 'log'))
ee.on('gol', b)
# -*- coding: utf-8 -*-
# Script to do some test for events.py
#
# Copyright (c) 2014 Tristan Cavelier <*****@*****.**>
# This program is free software. It comes without any warranty, to
# the extent permitted by applicable law. You can redistribute it
# and/or modify it under the terms of the Do What The F**k You Want
# To Public License, Version 2, as published by Sam Hocevar. See
# the COPYING file for more details.
from events import EventEmitter
if __name__ == '__main__':
print("""Expected output:
a
b
""")
ee = EventEmitter()
def a():
print('a')
ee.emit('b')
def b():
print('b')
ee.on('a', a)
ee.on('b', b)
ee.emit('a')
def real_loc_in_nest(self, real_coords):
grid_coords = self.coord_xform.from_real_to_grid(real_coords)
return self.grid_loc_in_nest(grid_coords)
def grid_loc_in_nest(self, grid_coords):
return self._nest_mask[grid_coords]
def grid_to_str(self, grid):
return '\n'.join(
map(
lambda row: ''.join(map(lambda cell: '*'
if cell else '.', row)), grid))
if __name__ == '__main__':
from coordxform import CoordinateTransform
from events import EventEmitter
my_emitter = EventEmitter()
my_xform = CoordinateTransform((-7.5, 7.5), (-7.5, 7.5), (
30,
30,
))
my_arena = Arena(3, my_xform, (-2.0, 2.0), (4.0, 4.0), my_emitter)
from events import SwarmieLocEvent
my_arena.swarmie_loc_update(SwarmieLocEvent(0, (-7.5, 7.5)))
my_arena.swarmie_loc_update(SwarmieLocEvent(1, (-7.5, 6.5)))
my_arena.swarmie_loc_update(SwarmieLocEvent(2, (-6.9, 7.5)))
print '{}'.format(my_arena.grid_to_str(my_arena.build_planning_grid()))
# vim: set ts=2 sw=2 expandtab:
class RlBehavior(object):
NAME_TO_ID_MAP = ['achilles', 'aeneas', 'ajax']
HB_PERIOD = rospy.Duration(2, 0)
STATUS_PERIOD = rospy.Duration(1, 0)
MANUAL_MODE = 1
AUTONOMOUS_MODE = 2
MODE_DICT = {
MANUAL_MODE: {
'log_fmt': '{swarmie_name} switching to MANUAL_MODE',
'infolog_fmt': '{swarmie_name} under manual control',
},
AUTONOMOUS_MODE: {
'log_fmt': '{swarmie_name} switching to AUTONOMOUS_MODE',
'infolog_fmt': '{swarmie_name} operating autonomously',
}
}
WARMUP_TIME = rospy.Duration(30, 0)
TIMESTEP_PERIOD = rospy.Duration(0, 100000000)
JOYSTICK_LINEAR_AXIS = 4
JOYSTICK_ANGULAR_AXIS = 3
MAX_MOTOR_CMD = 255.0
INIT_PLACE_RADIUS = 1.3
ARENA_X_RANGE = (-7.5, 7.5)
ARENA_Y_RANGE = (-7.5, 7.5)
GRID_QUANTIZATION = (30, 30)
NEST_X_TOP_LEFT = (-0.78, 0.78)
NEST_DIMS = (1.56, 1.56)
CUBE_ANNOUNCE_THRESHOLD = 0.25 # meters
CUBE_COUNT = 16
ACTION_COUNT = 2
SEARCH_ACTION = 0
FETCH_ACTION = 1
ACTION_NAMES = {SEARCH_ACTION: 'SEARCH', FETCH_ACTION: 'FETCH'}
ODOM_DRIFT_TOLERANCE = 0.5
def __init__(self, swarmie_name, app_root_dir):
self.swarmie_name = swarmie_name
self.swarmie_id = RlBehavior.NAME_TO_ID_MAP.index(swarmie_name)
self.emitter = EventEmitter()
self.mode = RlBehavior.MANUAL_MODE
self.tf = None
self.coord_xform = CoordinateTransform(RlBehavior.ARENA_X_RANGE,
RlBehavior.ARENA_Y_RANGE,
RlBehavior.GRID_QUANTIZATION)
self.arena = Arena(len(RlBehavior.NAME_TO_ID_MAP), self.coord_xform,
RlBehavior.NEST_X_TOP_LEFT, RlBehavior.NEST_DIMS,
self.emitter)
self.swarmie_state = SwarmieState(self.swarmie_name, self.swarmie_id,
self.coord_xform, self.emitter)
self.rl_state_rep = StateRepository(
list(range(len(RlBehavior.NAME_TO_ID_MAP))), self.swarmie_id,
RlBehavior.CUBE_COUNT, RlBehavior.GRID_QUANTIZATION, self.emitter)
self.model_file_path = os.path.join(
app_root_dir, 'rl_models', '{}_model.h5'.format(self.swarmie_name))
self.rl_agent = None
self.tag_state = None
self._latest_tag_list = None
self._current_action = None
self._map_center = None
self._map_current = None
# Publishers ---------------------------------------------------------------
self.status_pub = None
self.hb_pub = None
self.infolog_pub = None
self.drive_cmd_pub = None
self.wrist_cmd_pub = None
self.fingers_cmd_pub = None
# Timers -------------------------------------------------------------------
self.status_timer = None
self.timestep_timer = None
self.hb_timer = None
# Subscribers --------------------------------------------------------------
self.map_sub = None
self.odom_sub = None
self.mode_sub = None
self.sonar_left_sub = None
self.sonar_center_sub = None
self.sonar_right_sub = None
self.sonar_sync = None
self.joy_sub = None
self.target_sub = None
self.maint_cmd_sub = None
def run(self):
print('Welcome to the world of tomorrow {}!'.format(self.swarmie_name))
rospy.init_node('{}_BEHAVIOUR'.format(self.swarmie_name))
self.tf = TransformListener()
self.tag_state = TagState(self.swarmie_name, self.swarmie_state,
self.tf, self.coord_xform, self.arena)
self.rl_agent = DqnAgent(policy=GreedyPolicy(),
state_size=self.rl_state_rep.state_size,
action_size=RlBehavior.ACTION_COUNT,
model_file_path=self.model_file_path)
# --------------------------------------------------------------------------
# Set up all the publishers
#
self.status_pub = rospy.Publisher('{}/swarmie_status'.format(
self.swarmie_name),
String,
queue_size=10)
self.hb_pub = rospy.Publisher('{}/behaviour/heartbeat'.format(
self.swarmie_name),
String,
queue_size=1)
self.infolog_pub = rospy.Publisher('/infoLog', String, queue_size=1)
self.drive_cmd_pub = rospy.Publisher('{}/driveControl'.format(
self.swarmie_name),
Skid,
queue_size=10)
self.wrist_cmd_pub = rospy.Publisher('{}/wristAngle/cmd'.format(
self.swarmie_name),
Float32,
queue_size=1)
self.fingers_cmd_pub = rospy.Publisher('{}/fingerAngle/cmd'.format(
self.swarmie_name),
Float32,
queue_size=1)
# --------------------------------------------------------------------------
# Set up all the subscribers
#
self.map_sub = rospy.Subscriber('{}/odom/ekf'.format(
self.swarmie_name),
Odometry,
callback=self._on_map_update,
queue_size=10)
self.odom_sub = rospy.Subscriber('{}/odom/filtered'.format(
self.swarmie_name),
Odometry,
callback=self._on_odom_update,
queue_size=10)
self.mode_sub = rospy.Subscriber('{}/mode'.format(self.swarmie_name),
UInt8,
callback=self._on_mode_change,
queue_size=10)
self.sonar_left_sub = message_filters.Subscriber(
'{}/sonarLeft'.format(self.swarmie_name), Range)
self.sonar_center_sub = message_filters.Subscriber(
'{}/sonarCenter'.format(self.swarmie_name), Range)
self.sonar_right_sub = message_filters.Subscriber(
'{}/sonarRight'.format(self.swarmie_name), Range)
self.sonar_sync = message_filters.TimeSynchronizer([
self.sonar_left_sub,
self.sonar_center_sub,
self.sonar_right_sub,
],
queue_size=10)
self.sonar_sync.registerCallback(self._on_sonar_update)
self.joy_sub = rospy.Subscriber('{}/joystick'.format(
self.swarmie_name),
Joy,
callback=self._on_joy_cmd,
queue_size=10)
self.target_sub = rospy.Subscriber('/{}/targets'.format(
self.swarmie_name),
AprilTagDetectionArray,
callback=self._on_tags_spotted,
queue_size=10)
self.maint_cmd_sub = rospy.Subscriber('{}/maintCmd'.format(
self.swarmie_name),
String,
callback=self._on_maint_cmd,
queue_size=10)
# --------------------------------------------------------------------------
# Set up all the timers
#
self.hb_timer = rospy.Timer(RlBehavior.HB_PERIOD, self._on_hb_timer)
self.status_timer = rospy.Timer(RlBehavior.STATUS_PERIOD,
self._on_status_timer)
# --------------------------------------------------------------------------
# Away we go!
#
self.rl_state_rep.init_remote()
rospy.Timer(RlBehavior.WARMUP_TIME, self._on_warmup_timer)
rospy.spin()
@property
def initialized(self):
return self.swarmie_state.ready
def _on_hb_timer(self, event):
hb_msg = String()
hb_msg.data = ''
self.hb_pub.publish(hb_msg)
def _on_status_timer(self, event):
status_msg = String()
status_msg.data = 'learners'
self.status_pub.publish(status_msg)
def _on_warmup_timer(self, event):
# Start the timestep evaluation
if self.timestep_timer is None:
self.timestep_timer = rospy.Timer(RlBehavior.TIMESTEP_PERIOD,
self._on_timestep_timer)
def _on_timestep_timer(self, event):
"""
:param event:
:type event: rospy.timer.TimerEvent
"""
time_since_last_step = None
if event.last_real is not None:
time_since_last_step = (rospy.Time.now() - event.last_real)
try:
# ------------------------------------------------------------------------
# Initial timestep
if not self.initialized:
self.infolog_pub.publish(
String(
data='{} willing to learn!'.format(self.swarmie_name)))
self.swarmie_state.initialize_centers(
RlBehavior.INIT_PLACE_RADIUS)
rospy.loginfo(
'{} perceived center in odom frame at ({},{})'.format(
self.swarmie_name, self.swarmie_state.odom_center[0],
self.swarmie_state.odom_center[1]))
self._map_center = (self._map_current[0] +
(RlBehavior.INIT_PLACE_RADIUS *
math.cos(self._map_current[2])),
self._map_current[1] +
(RlBehavior.INIT_PLACE_RADIUS *
math.sin(self._map_current[2])),
self._map_current[2])
if not self.initialized:
rospy.logfatal(
'{} failed initialization with {} {} {}'.format(
self.swarmie_name,
self.swarmie_state._positions_populated,
self.swarmie_state.linear_vel,
self.swarmie_state.angular_vel))
self.hb_timer.shutdown()
self.status_timer.shutdown()
self.timestep_timer.shutdown()
rospy.signal_shutdown(
'Odometry data not available for {}'.format(
self.swarmie_name))
# ------------------------------------------------------------------------
# Evaluate and mitigate any Odometry drift
# timestep_step = 'Evaluating Odometry'
# self._evaluate_odom_drift()
# ------------------------------------------------------------------------
# Update tag information with latest message
if self._latest_tag_list is not None:
self.tag_state.update(self._latest_tag_list).sort()
for a_tag_report in self.tag_state.cube_tags:
if a_tag_report.tag_dist <= RlBehavior.CUBE_ANNOUNCE_THRESHOLD:
continue
self.emitter.emit(
CubeSpottedEvent(swarmie_id=self.swarmie_id,
cube_loc=a_tag_report.grid_coords[0:2]))
# ------------------------------------------------------------------------
# Automatically select an action if in automatic mode
if self.mode == RlBehavior.AUTONOMOUS_MODE:
state_vector = self.rl_state_rep.make_state_vector()
rl_action_id = self.rl_agent.act(state_vector)
rospy.loginfo('Automated agent selected action {}'.format(
RlBehavior.ACTION_NAMES[rl_action_id]))
if rl_action_id == RlBehavior.SEARCH_ACTION and not isinstance(
self._current_action, SearchAction):
if self._current_action is None:
# TEMPORARY SETUP
SEARCH_QUAD_FOR = {
'achilles': SearchAction.ALPHA_QUADRANT,
'aeneas': SearchAction.CHARLIE_QUADRANT,
'ajax': SearchAction.BRAVO_QUADRANT
}
quad_name = SEARCH_QUAD_FOR[self.swarmie_name]
self._current_action = SearchAction(
self.swarmie_name, self.arena, quad_name)
else:
self._current_action = None
elif rl_action_id == RlBehavior.FETCH_ACTION and not isinstance(
self._current_action, FetchAction):
if self._current_action is None and self.rl_state_rep.nearest_cube[
self.swarmie_id] is not None:
nearest_cube = self.rl_state_rep.nearest_cube[
self.swarmie_id]
if nearest_cube is not None:
self._current_action = FetchAction(
self.swarmie_name, self.swarmie_id, self.arena,
self.rl_state_rep, self.tag_state)
else:
self._current_action = None
# ------------------------------------------------------------------------
# Execute current action, should there be one.
action_response = None
if self._current_action is not None:
action_response = self._current_action.update(
swarmie_state=self.swarmie_state,
elapsed_time=time_since_last_step)
if action_response is not None:
if action_response.skid:
self.drive_cmd_pub.publish(action_response.skid)
if action_response.wrist:
self.wrist_cmd_pub.publish(action_response.wrist)
if action_response.fingers:
self.fingers_cmd_pub.publish(action_response.fingers)
else:
self._current_action = None
self.drive_cmd_pub.publish(Skid(left=0., right=0.))
# ------------------------------------------------------------------------
# Age the april tag sightings based on elapsed time
if time_since_last_step is not None:
self.tag_state.dock_age(time_since_last_step)
except Exception:
# ex_info = sys.exc_info()
# file_name = '<unknown>'
# line_num = '<unknown>'
# func_name = '<unknown>'
# if len(ex_info) > 2 and ex_info[2] is not None:
# tb_info = traceback.extract_tb(ex_info[2], 1)
# if len(tb_info) > 0 and len(tb_info[0]) > 3:
# (file_name, line_num, func_name, _) = tb_info[0]
rospy.logwarn('{}'.format(traceback.format_exc()))
def _evaluate_odom_drift(self):
map_pose = PoseStamped()
map_pose.header.stamp = rospy.Time.now()
map_pose.header.frame_id = '{}/map'.format(self.swarmie_name)
(map_pose.pose.orientation.x, map_pose.pose.orientation.y,
map_pose.pose.orientation.z,
map_pose.pose.orientation.w) = quaternion_from_euler(
0., 0., self._map_center[2])
map_pose.pose.position.x = self._map_center[0]
map_pose.pose.position.y = self._map_center[1]
odom_pose = None
try:
self.tf.waitForTransform(map_pose.header.frame_id,
'{}/odom'.format(self.swarmie_name),
map_pose.header.stamp, rospy.Duration(1))
odom_pose = self.tf.transformPose(
'{}/odom'.format(self.swarmie_name), map_pose)
except tf.Exception as ex:
rospy.logwarn('Unable to evaluate odometry drift: {}'.format(
ex.message))
return
x_diff = odom_pose.pose.position.x - self.swarmie_state.odom_center[0]
y_diff = odom_pose.pose.position.y - self.swarmie_state.odom_center[1]
total_diff = math.hypot(x_diff, y_diff)
rospy.loginfo('Odometry drift appears to be {}'.format(total_diff))
if total_diff > RlBehavior.ODOM_DRIFT_TOLERANCE:
new_center = np.array([
self.swarmie_state.odom_center[0] + (x_diff / total_diff),
self.swarmie_state.odom_center[1] + (y_diff / total_diff)
])
rospy.logwarn(
'Odometry drift out of tolerance by {}. Adjust odom_center from {} to {}'
.format(total_diff, self.swarmie_state.odom_center[0:2],
new_center))
def _on_odom_update(self, sample):
"""Position update callback.
Called by ROS whenever a sample arrives via the <swarmie>/odom/filtered
topic, communicating the currently-sensed position of the swarmie using a
local (starting point-centered) reference frame.
:param sample: Sample containing position information.
:type sample: nav_msgs.msg.Odometry
"""
(_, _, yaw) = euler_from_quaternion(
(sample.pose.pose.orientation.x, sample.pose.pose.orientation.y,
sample.pose.pose.orientation.z, sample.pose.pose.orientation.w))
self.swarmie_state.odom_current = np.array(
[sample.pose.pose.position.x, sample.pose.pose.position.y, yaw])
self.swarmie_state.linear_vel = sample.twist.twist.linear.x
self.swarmie_state.angular_vel = sample.twist.twist.angular.z
def _on_map_update(self, sample):
(_, _, yaw) = euler_from_quaternion(
(sample.pose.pose.orientation.x, sample.pose.pose.orientation.y,
sample.pose.pose.orientation.z, sample.pose.pose.orientation.w))
self._map_current = np.array(
[sample.pose.pose.position.x, sample.pose.pose.position.y, yaw])
def _on_mode_change(self, sample):
"""Operating mode update callback.
Called by ROS whenever a sample arrives via the <swarmie>/mode topic,
indicating either a mode initialization or a mode change.
:param sample: UInt8 sample containing the new mode.
:type sample: std_msgs.msg.UInt8
"""
if sample.data != self.mode:
self._current_action = None
if sample.data in RlBehavior.MODE_DICT:
self.mode = sample.data
dict_entry = RlBehavior.MODE_DICT[sample.data]
rospy.loginfo(dict_entry['log_fmt'].format(
swarmie_name=self.swarmie_name))
self.infolog_pub.publish(dict_entry['infolog_fmt'].format(
swarmie_name=self.swarmie_name))
else:
rospy.logwarn('{} cannot switch to unknown mode {}'.format(
self.swarmie_name, sample.data))
def _on_sonar_update(self, left_sample, center_sample, right_sample):
"""Sonar data update callback.
Called by ROS when it is able to deliver a time-homogeneous collection of
samples from the three (3) sonar sensors in the swarmies.
:param left_sample: Sample from the left range finder.
:type left_sample: sensor_msgs.msg.Range
:param center_sample: Sample from the center range finder.
:type center_sample: sensor_msgs.msg.Range
:param right_sample: Sample from the right range finder.
:type right_sample: sensor_msgs.msg.Range
"""
rospy.logdebug('{} sonar readings: L:{} C:{} R:{}'.format(
self.swarmie_name, left_sample.range, center_sample.range,
right_sample.range))
self.swarmie_state.sonar_readings = np.array(
[left_sample.range, center_sample.range, right_sample.range])
def _on_joy_cmd(self, cmd):
"""Joystick command callback.
Called by ROS whenever a Joystick command is emitted in the system.
:param cmd: Joystick command sample.
:type cmd: sensor_msgs.msg.Joy
"""
if self.mode == RlBehavior.MANUAL_MODE:
# ------------------------------------------------------------------------
# Scale linear motor command, enforcing a +/- 0.1 "dead zone"
linear_thresh = math.fabs(
cmd.axes[RlBehavior.JOYSTICK_LINEAR_AXIS]) >= 0.1
linear_cmd = (cmd.axes[RlBehavior.JOYSTICK_LINEAR_AXIS] *
RlBehavior.MAX_MOTOR_CMD)
linear_cmd = 0.0 if not linear_thresh else linear_cmd
# ------------------------------------------------------------------------
# Scale angular motor command, enforcing a +/- 0.1 "dead zone"
angular_thresh = math.fabs(
cmd.axes[RlBehavior.JOYSTICK_ANGULAR_AXIS]) >= 0.1
angular_cmd = (cmd.axes[RlBehavior.JOYSTICK_ANGULAR_AXIS] *
RlBehavior.MAX_MOTOR_CMD)
angular_cmd = 0.0 if not angular_thresh else angular_cmd
# ------------------------------------------------------------------------
# Calculate left robot tracks command, restricting the command value to
# the range [-MAX_MOTOR_CMD, MAX_MOTOR_CMD]
left_cmd = linear_cmd - angular_cmd
left_cmd = min(left_cmd, RlBehavior.MAX_MOTOR_CMD)
left_cmd = max(left_cmd, -1.0 * RlBehavior.MAX_MOTOR_CMD)
# ------------------------------------------------------------------------
# Calculate right robot tracks command, restricting the command value to
# the range [-MAX_MOTOR_CMD, MAX_MOTOR_CMD]
right_cmd = linear_cmd + angular_cmd
right_cmd = min(right_cmd, RlBehavior.MAX_MOTOR_CMD)
right_cmd = max(right_cmd, -1.0 * RlBehavior.MAX_MOTOR_CMD)
self.drive_cmd_pub.publish(Skid(left=left_cmd, right=right_cmd))
def _on_tags_spotted(self, tag_list):
"""April tag detection callback.
Called by ROS whenever the April tag CV task spots tags in images returned
by the swarmie camera.
:param tag_list: List of April tags spotted by CV algorithm.
:type tag_list: apriltags_ros.msg.AprilTagDetectionArray
"""
self._latest_tag_list = tag_list
def _on_maint_cmd(self, the_cmd):
cmd_str = str(the_cmd.data).lower()
if cmd_str == 'approach':
self._current_action = ApproachAction(self.swarmie_name,
self.tag_state)
elif cmd_str == 'turn_east':
self._current_action = TurnAction(self.swarmie_name, 0.)
elif cmd_str == 'turn_west':
self._current_action = TurnAction(self.swarmie_name, math.pi)
elif cmd_str == 'turn_north':
self._current_action = TurnAction(self.swarmie_name,
(math.pi / 2.))
elif cmd_str == 'turn_south':
self._current_action = TurnAction(self.swarmie_name,
-1. * (math.pi / 2.))
elif cmd_str == 'drive_forward':
self._current_action = DriveAction(self.swarmie_name)
elif cmd_str == 'drive_backward':
self._current_action = DriveAction(self.swarmie_name,
forward=False)
elif cmd_str.startswith('move_to_cell'):
parsed_cmd = re.search(
r'move_to_cell\((?P<row>[0-9]+),(?P<col>[0-9]+)\)', cmd_str)
dest_coords = (
int(parsed_cmd.group('row')),
int(parsed_cmd.group('col')),
)
self._current_action = MoveToCellAction(self.swarmie_name,
self.arena, dest_coords)
elif cmd_str.startswith('move_to_real'):
parsed_cmd = re.search(
r'move_to_real\((?P<x>\-?[0-9]+(\.[0-9]+)?),(?P<y>\-?[0-9]+(\.[0-9]+)?)\)',
cmd_str)
dest_coords = (
float(parsed_cmd.group('x')),
float(parsed_cmd.group('y')),
)
self._current_action = MoveToRealAction(self.swarmie_name,
dest_coords)
elif cmd_str == 'pickup':
self._current_action = PickupAction(self.swarmie_name)
elif cmd_str == 'drop_off':
self._current_action = DropOffAction(self.swarmie_name)
elif cmd_str == 'sweep':
self._current_action = SweepAction(self.swarmie_name,
self.tag_state)
elif cmd_str.startswith('search'):
parsed_cmd = re.search(r'search\((?P<quad>[a-z]+)\)', cmd_str)
quad = parsed_cmd.group('quad')
self._current_action = SearchAction(self.swarmie_name, self.arena,
quad)
# elif cmd_str.startswith('fetch'):
# parsed_cmd = re.search(r'fetch\((?P<row>[0-9]+),(?P<col>[0-9]+)\)', cmd_str)
# dest_coords = (
# int(parsed_cmd.group('row')),
# int(parsed_cmd.group('col')),
# )
# self._current_action = FetchAction(self.swarmie_name, self.arena, dest_coords, self.tag_state)
elif cmd_str == 'nest':
self._current_action = MoveToRealAction(self.swarmie_name,
(0.0, 0.0))
elif cmd_str == 'halt':
self._current_action = None
else:
rospy.loginfo(
'Did not recognize maintenance command: {}'.format(cmd_str))
