i am trying to use code similar to unit 6 to realise rmf with my physical robot(s). this is the error i am getting when i run the fleet adapter.
Starting fleet adapter…
[INFO] [1738744154.948739359] [originbot_fleet_adapter]: Parameter [discovery_timeout] set to: 60.000000
Traceback (most recent call last):
File “/home/fyp2024/rmf_ws/install/physical_api/lib/physical_api/fleet_adapter”, line 33, in
sys.exit(load_entry_point(‘physical-api==0.0.0’, ‘console_scripts’, ‘fleet_adapter’)())
File “/home/fyp2024/rmf_ws/install/physical_api/lib/python3.10/site-packages/physical_api/fleet_adapter.py”, line 314, in main
adapter = initialize_fleet(
File “/home/fyp2024/rmf_ws/install/physical_api/lib/python3.10/site-packages/physical_api/fleet_adapter.py”, line 87, in initialize_fleet
assert adapter, ("Unable to initialize fleet adapter. Please ensure "
AssertionError: Unable to initialize fleet adapter. Please ensure RMF Schedule Node is running
[ros2run]: Process exited with failure 1
the following is my robot config file in yaml
# FLEET CONFIG =================================================================
# RMF Fleet parameters
rmf_fleet:
name: "originbot"
fleet_manager:
prefix: "http://127.0.0.1:22022"
user: "some_user"
password: "some_password"
limits:
linear: [0.7, 0.5] # velocity, acceleration
angular: [0.4, 1.0] # velocity, acceleration
profile: # Robot profile is modelled as a circle
footprint: 1.0 # radius in m
vicinity: 1.2 # radius in m
reversible: False # whether robots in this fleet can reverse
# TODO Update battery parameters with actual specs
battery_system:
voltage: 12.0 # V
capacity: 20.0 # Ahr
charging_current: 10.0 # A
mechanical_system:
mass: 70.0 # kg
moment_of_inertia: 40.0 #kgm^2
friction_coefficient: 0.22
ambient_system:
power: 100.0 # W
tool_system:
power: 0.0 # W
recharge_threshold: 0.20 # Battery level below which robots in this fleet will not operate
recharge_soc: 1.0 # Battery level to which robots in this fleet should be charged up to during recharging tasks
publish_fleet_state: 10.0 # Publish frequency for fleet state, ensure that it is same as robot_state_update_frequency
account_for_battery_drain: True
task_capabilities: # Specify the types of RMF Tasks that robots in this fleet are capable of performing
loop: True
delivery: True
clean: False
finishing_request: "park" # [park, charge, nothing]
# Originbot CONFIG =================================================================
robots:
# Here the user is expected to append the configuration for each robot in the
# fleet.
# Configuration for originbot_0
originbot_0:
robot_config:
max_delay: 15.0 # allowed seconds of delay of the current itinerary before it gets interrupted and replanned
rmf_config:
robot_state_update_frequency: 10.0
start:
map_name: "L1"
waypoint: ""
orientation: 0.0 # radians
charger:
waypoint: ""
# Configuration for originbot_1
originbot_1:
robot_config:
max_delay: 15.0 # allowed seconds of delay of the current itinerary before it gets interrupted and replanned
rmf_config:
robot_state_update_frequency: 10.0
start:
map_name: "L1"
waypoint: "originbot_spawn"
orientation: 0.0 # radians
charger:
waypoint: "originbot_spawn"
# TRANSFORM CONFIG =============================================================
# For computing transforms between Robot and RMF coordinate systems
# If its a 1:1 correspondance just place the same numbers
reference_coordinates:
rmf: [[10.2837, -5.6326], # Right corner
[5.3339, -0.3840], # Top Corner
[0.2560, -5.2059], # Left Corner
[4.8645, -9.8143], # Bottom Corner
[3.7977, -4.3524], # Our seats
[3.9684, -6.5713], # Circular Robot Base
[6.2300, -3.4990], # Table leg on the right
[6.8274, -6.2300], # Bottom Right Arm
[8.5769, -8.9182]] # Island
robot: [[8.04443, -1.89213], # Right corner
[2.2344, 4.35452], # Top Corner
[-3.61153, -1.37832], # Left Corner
[1.50039, -692627], # Bottom Corner
[0.462738, -0.356078], # Our seats
[0.684927, -2.9862], # Circular Robot Base
[3.30796, 0.658291], # Table leg on the right
[4.03551, -2.54359], # Bottom Right Arm
[5.95516, -5.64273]] # Island
this is my fleet adapter code (.py)
# Copyright 2021 Open Source Robotics Foundation, Inc.
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
# http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
import sys
import argparse
import yaml
import nudged
import time
import threading
import rclpy
import rclpy.node
from rclpy.parameter import Parameter
import rmf_adapter as adpt
import rmf_adapter.vehicletraits as traits
import rmf_adapter.battery as battery
import rmf_adapter.geometry as geometry
import rmf_adapter.graph as graph
import rmf_adapter.plan as plan
from rmf_task_msgs.msg import TaskProfile, TaskType
from functools import partial
from .RobotCommandHandle import RobotCommandHandle
from .RobotClientAPI import RobotAPI
# ------------------------------------------------------------------------------
# Helper functions
# ------------------------------------------------------------------------------
def initialize_fleet(config_yaml, nav_graph_path, node, use_sim_time, server_uri):
# Profile and traits
fleet_config = config_yaml['rmf_fleet']
profile = traits.Profile(geometry.make_final_convex_circle(
fleet_config['profile']['footprint']),
geometry.make_final_convex_circle(fleet_config['profile']['vicinity']))
vehicle_traits = traits.VehicleTraits(
linear=traits.Limits(*fleet_config['limits']['linear']),
angular=traits.Limits(*fleet_config['limits']['angular']),
profile=profile)
vehicle_traits.differential.reversible = fleet_config['reversible']
# Battery system
voltage = fleet_config['battery_system']['voltage']
capacity = fleet_config['battery_system']['capacity']
charging_current = fleet_config['battery_system']['charging_current']
battery_sys = battery.BatterySystem.make(
voltage, capacity, charging_current)
# Mechanical system
mass = fleet_config['mechanical_system']['mass']
moment = fleet_config['mechanical_system']['moment_of_inertia']
friction = fleet_config['mechanical_system']['friction_coefficient']
mech_sys = battery.MechanicalSystem.make(mass, moment, friction)
# Power systems
ambient_power_sys = battery.PowerSystem.make(
fleet_config['ambient_system']['power'])
tool_power_sys = battery.PowerSystem.make(
fleet_config['tool_system']['power'])
# Power sinks
motion_sink = battery.SimpleMotionPowerSink(battery_sys, mech_sys)
ambient_sink = battery.SimpleDevicePowerSink(
battery_sys, ambient_power_sys)
tool_sink = battery.SimpleDevicePowerSink(battery_sys, tool_power_sys)
nav_graph = graph.parse_graph(nav_graph_path, vehicle_traits)
# Adapter
fleet_name = fleet_config['name']
adapter = adpt.Adapter.make(f'{fleet_name}_fleet_adapter')
if use_sim_time:
adapter.node.use_sim_time()
assert adapter, ("Unable to initialize fleet adapter. Please ensure "
"RMF Schedule Node is running")
adapter.start()
time.sleep(1.0)
fleet_handle = adapter.add_fleet(fleet_name, vehicle_traits, nav_graph, server_uri)
if not fleet_config['publish_fleet_state']:
fleet_handle.fleet_state_publish_period(None)
# Account for battery drain
drain_battery = fleet_config['account_for_battery_drain']
recharge_threshold = fleet_config['recharge_threshold']
recharge_soc = fleet_config['recharge_soc']
finishing_request = fleet_config['task_capabilities']['finishing_request']
node.get_logger().info(f"Finishing request: [{finishing_request}]")
# Set task planner params
ok = fleet_handle.set_task_planner_params(
battery_sys,
motion_sink,
ambient_sink,
tool_sink,
recharge_threshold,
recharge_soc,
drain_battery,
finishing_request)
assert ok, ("Unable to set task planner params")
task_capabilities = []
if fleet_config['task_capabilities']['loop']:
node.get_logger().info(
f"Fleet [{fleet_name}] is configured to perform Loop tasks")
task_capabilities.append(TaskType.TYPE_LOOP)
if fleet_config['task_capabilities']['delivery']:
node.get_logger().info(
f"Fleet [{fleet_name}] is configured to perform Delivery tasks")
task_capabilities.append(TaskType.TYPE_DELIVERY)
if fleet_config['task_capabilities']['clean']:
node.get_logger().info(
f"Fleet [{fleet_name}] is configured to perform Clean tasks")
task_capabilities.append(TaskType.TYPE_CLEAN)
# Callable for validating requests that this fleet can accommodate
def _task_request_check(task_capabilities, msg: TaskProfile):
if msg.description.task_type in task_capabilities:
return True
else:
return False
fleet_handle.accept_task_requests(
partial(_task_request_check, task_capabilities))
# Transforms
rmf_coordinates = config_yaml['reference_coordinates']['rmf']
robot_coordinates = config_yaml['reference_coordinates']['robot']
transforms = {
'rmf_to_robot': nudged.estimate(rmf_coordinates, robot_coordinates),
'robot_to_rmf': nudged.estimate(robot_coordinates, rmf_coordinates)}
transforms['orientation_offset'] = \
transforms['rmf_to_robot'].get_rotation()
mse = nudged.estimate_error(transforms['rmf_to_robot'],
rmf_coordinates,
robot_coordinates)
print(f"Coordinate transformation error: {mse}")
print("RMF to Robot transform:")
print(f" rotation:{transforms['rmf_to_robot'].get_rotation()}")
print(f" scale:{transforms['rmf_to_robot'].get_scale()}")
print(f" trans:{transforms['rmf_to_robot'].get_translation()}")
print("Robot to RMF transform:")
print(f" rotation:{transforms['robot_to_rmf'].get_rotation()}")
print(f" scale:{transforms['robot_to_rmf'].get_scale()}")
print(f" trans:{transforms['robot_to_rmf'].get_translation()}")
def _updater_inserter(cmd_handle, update_handle):
"""Insert a RobotUpdateHandle."""
cmd_handle.update_handle = update_handle
# Initialize robot API for this fleet
api = RobotAPI(
fleet_config['fleet_manager']['prefix'],
fleet_config['fleet_manager']['user'],
fleet_config['fleet_manager']['password'],
fleet_config['name'],
timeout = 5.0,
debug = False)
# Initialize robots for this fleet
missing_robots = config_yaml['robots']
def _add_fleet_robots():
robots = {}
while len(missing_robots) > 0:
time.sleep(0.2)
for robot_name in list(missing_robots.keys()):
node.get_logger().debug(f"Connecting to robot: {robot_name}")
position = api.position(robot_name)
if position is None:
continue
if len(position) > 2:
node.get_logger().info(f"Initializing robot: {robot_name}")
robots_config = config_yaml['robots'][robot_name]
rmf_config = robots_config['rmf_config']
robot_config = robots_config['robot_config']
initial_waypoint = rmf_config['start']['waypoint']
initial_orientation = rmf_config['start']['orientation']
starts = []
time_now = adapter.now()
if (initial_waypoint is not None) and\
(initial_orientation is not None):
node.get_logger().info(
f"Using provided initial waypoint "
"[{initial_waypoint}] "
f"and orientation [{initial_orientation:.2f}] to "
f"initialize starts for robot [{robot_name}]")
# Get the waypoint index for initial_waypoint
initial_waypoint_index = nav_graph.find_waypoint(
initial_waypoint).index
starts = [plan.Start(time_now,
initial_waypoint_index,
initial_orientation)]
else:
node.get_logger().info(
f"Running compute_plan_starts for robot: "
"{robot_name}")
starts = plan.compute_plan_starts(
nav_graph,
rmf_config['start']['map_name'],
position,
time_now)
if starts is None or len(starts) == 0:
node.get_logger().error(
f"Unable to determine StartSet for {robot_name}")
continue
robot = RobotCommandHandle(
name=robot_name,
fleet_name=fleet_name,
config=robot_config,
node=node,
graph=nav_graph,
vehicle_traits=vehicle_traits,
transforms=transforms,
map_name=rmf_config['start']['map_name'],
start=starts[0],
position=position,
charger_waypoint=rmf_config['charger']['waypoint'],
update_frequency=rmf_config.get(
'robot_state_update_frequency', 1),
adapter=adapter,
api=api)
if robot.initialized:
robots[robot_name] = robot
# Add robot to fleet
fleet_handle.add_robot(robot,
robot_name,
profile,
[starts[0]],
partial(_updater_inserter,
robot))
node.get_logger().info(
f"Successfully added new robot: {robot_name}")
else:
node.get_logger().error(
f"Failed to initialize robot: {robot_name}")
del missing_robots[robot_name]
else:
pass
node.get_logger().debug(
f"{robot_name} not found, trying again...")
return
add_robots = threading.Thread(target=_add_fleet_robots, args=())
add_robots.start()
return adapter
# ------------------------------------------------------------------------------
# Main
# ------------------------------------------------------------------------------
def main(argv=sys.argv):
# Init rclpy and adapter
rclpy.init(args=argv)
adpt.init_rclcpp()
args_without_ros = rclpy.utilities.remove_ros_args(argv)
parser = argparse.ArgumentParser(
prog="fleet_adapter",
description="Configure and spin up the fleet adapter")
parser.add_argument("-c", "--config_file", type=str, required=True,
help="Path to the config.yaml file")
parser.add_argument("-n", "--nav_graph", type=str, required=True,
help="Path to the nav_graph for this fleet adapter")
parser.add_argument("-s", "--server_uri", type=str, required=False, default="",
help="URI of the api server to transmit state and task information.")
parser.add_argument("--use_sim_time", action="store_true",
help='Use sim time, default: false')
args = parser.parse_args(args_without_ros[1:])
print(f"Starting fleet adapter...")
config_path = args.config_file
nav_graph_path = args.nav_graph
# Load config and nav graph yamls
with open(config_path, "r") as f:
config_yaml = yaml.safe_load(f)
# ROS 2 node for the command handle
fleet_name = config_yaml['rmf_fleet']['name']
node = rclpy.node.Node(f'{fleet_name}_command_handle')
# Enable sim time for testing offline
if args.use_sim_time:
param = Parameter("use_sim_time", Parameter.Type.BOOL, True)
node.set_parameters([param])
if args.server_uri == "":
server_uri = None
else:
server_uri = args.server_uri
adapter = initialize_fleet(
config_yaml,
nav_graph_path,
node,
args.use_sim_time,
server_uri)
# Create executor for the command handle node
rclpy_executor = rclpy.executors.SingleThreadedExecutor()
rclpy_executor.add_node(node)
# Start the fleet adapter
rclpy_executor.spin()
# Shutdown
node.destroy_node()
rclpy_executor.shutdown()
rclpy.shutdown()
if __name__ == '__main__':
main(sys.argv)