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