def __init__(self, fleet_config):
super().__init__('fleet_driver_mir')
self.fleet_config = fleet_config
self.robots = {}
self.api_clients = []
self.status_pub = self.create_publisher(FleetState, 'fleet_states', 1)
self.pub_timer = self.create_timer(self.STATUS_PUB_PERIOD,
self.pub_fleet)
self.ref_coordinates_rmf = [[26.95, -20.23], [29.26, -22.38],
[11.4, -16.48], [12.46, -16.99]]
self.ref_coordinates_mir = [[7.2, 16.6], [5.15, 18.35], [23, 12.35],
[22.05, 12.95]]
self.rmf2mir_transform = nudged.estimate(self.ref_coordinates_rmf,
self.ref_coordinates_mir)
self.mir2rmf_transform = nudged.estimate(self.ref_coordinates_mir,
self.ref_coordinates_rmf)
mse = nudged.estimate_error(self.rmf2mir_transform,
self.ref_coordinates_rmf,
self.ref_coordinates_mir)
self.get_logger().info(f'transformation estimate error: {mse}')
for api_client in self.create_all_api_clients(self.fleet_config):
self.get_logger().info(f'initializing robot from \
{api_client.configuration.host}')
robot = Robot(self)
robot.api = mir100_client.DefaultApi(api_client)
# temporary retry configuration to workaround startup race condition while launching
connection_pool_kw = robot.api.api_client.rest_client.pool_manager.connection_pool_kw
orig_retries = connection_pool_kw.get('retries')
retries = urllib3.Retry(10)
retries.backoff_factor = 1
retries.status_forcelist = (404, )
connection_pool_kw['retries'] = retries
mir_status = robot.api.status_get()
robot.name = mir_status.robot_name
self.load_missions(robot)
self.update_positions(robot)
# reset retries
if orig_retries is not None:
connection_pool_kw['retries'] = orig_retries
else:
del connection_pool_kw['retries']
self.robots[robot.name] = robot
self.get_logger().info(f'successfully initialized robot \
{robot.name}')
# Setup fleet driver ROS2 topic subscriptions
self.path_request_sub = self.create_subscription(
PathRequest, '/robot_path_requests', self.on_path_request, 1)
self.mode_sub = self.create_subscription(ModeRequest,
f'/robot_mode_requests',
self.on_robot_mode_request, 1)
def create_robot_command_handles(config, handle_data, dry_run=False):
robots = {}
for robot_name, robot_config in config['robots'].items():
# CONFIGURE MIR =======================================================
mir_config = robot_config['mir_config']
rmf_config = robot_config['rmf_config']
configuration = mir100_client.Configuration()
configuration.username = mir_config['base_url']
configuration.host = mir_config['user']
configuration.password = mir_config['password']
api_client = mir100_client.ApiClient(configuration)
api_client.default_headers['Accept-Language'] = 'en-US'
# CONFIGURE HANDLE ====================================================
robot = MiRCommandHandle(name=robot_name,
node=handle_data['node'],
rmf_graph=handle_data['graph'],
robot_state_update_frequency=rmf_config.get(
'robot_state_update_frequency', 1),
dry_run=dry_run)
robot.mir_api = mir100_client.DefaultApi(api_client)
robot.transforms = handle_data['transforms']
robot.rmf_map_name = rmf_config['start']['map_name']
if not dry_run:
with MiRRetryContext(robot):
_mir_status = robot.mir_api.status_get()
robot.mir_name = _mir_status.robot_name
robot.load_mir_missions()
robot.load_mir_positions()
else:
robot.mir_name = "DUMMY_ROBOT_FOR_DRY_RUN"
robots[robot.name] = robot
# OBTAIN PLAN STARTS ==================================================
start_config = rmf_config['start']
# If the plan start is configured, use it, otherwise, grab it
if (start_config.get('waypoint_index')
and start_config.get('orientation')):
starts = [
plan.Start(handle_data['adapter'].now(),
start_config.get('waypoint_index'),
start_config.get('orientation'))
]
else:
starts = plan.compute_plan_starts(
handle_data['graph'], start_config['map_name'],
robot.get_position(rmf=True, as_dimensions=True),
handle_data['adapter'].now())
assert starts, ("Robot %s can't be placed on the nav graph!" %
robot_name)
# Insert start data into robot
start = starts[0]
if start.lane.has_value: # If the robot is in a lane
robot.rmf_current_lane_index = start.lane.value
robot.rmf_current_waypoint_index = None
robot.rmf_target_waypoint_index = None
else: # Otherwise, the robot is on a waypoint
robot.rmf_current_lane_index = None
robot.rmf_current_waypoint_index = start.waypoint
robot.rmf_target_waypoint_index = None
print("MAP_NAME:", start_config['map_name'])
robot.rmf_map_name = start_config['map_name']
# INSERT UPDATER ======================================================
def updater_inserter(handle_obj, rmf_updater):
"""Insert a RobotUpdateHandle."""
handle_obj.rmf_updater = rmf_updater
handle_data['fleet_handle'].add_robot(robot, robot.name,
handle_data['profile'], starts,
partial(updater_inserter, robot))
handle_data['node'].get_logger().info(
f"successfully initialized robot {robot.name}"
f" (MiR name: {robot.mir_name})")
return robots