From e873a8f6c0e3161554a67522397f8458a517a3af Mon Sep 17 00:00:00 2001 From: Aaron Chong Date: Mon, 30 Dec 2024 17:17:01 +0800 Subject: [PATCH] Basic nav1 fleet adapter setup done Signed-off-by: Aaron Chong --- .../free_fleet_adapter/fleet_adapter.py | 37 ++++++++++- .../nav1_tb3_simulation_fleet_config.yaml | 63 +++++++++++++++++++ .../fleet/tb3_simulation_fleet_config.yaml | 1 + ...v1_tb3_simulation_fleet_adapter.launch.xml | 14 +++++ 4 files changed, 112 insertions(+), 3 deletions(-) create mode 100644 free_fleet_examples/config/fleet/nav1_tb3_simulation_fleet_config.yaml create mode 100644 free_fleet_examples/launch/nav1_tb3_simulation_fleet_adapter.launch.xml diff --git a/free_fleet_adapter/free_fleet_adapter/fleet_adapter.py b/free_fleet_adapter/free_fleet_adapter/fleet_adapter.py index ed178d73..5242a2ff 100644 --- a/free_fleet_adapter/free_fleet_adapter/fleet_adapter.py +++ b/free_fleet_adapter/free_fleet_adapter/fleet_adapter.py @@ -20,6 +20,7 @@ import threading import time +from free_fleet_adapter.nav1_robot_adapter import Nav1RobotAdapter from free_fleet_adapter.nav2_robot_adapter import Nav2RobotAdapter import nudged import rclpy @@ -156,9 +157,39 @@ def main(argv=sys.argv): for robot_name in fleet_config.known_robots: robot_config_yaml = config_yaml['rmf_fleet']['robots'][robot_name] robot_config = fleet_config.get_known_robot_configuration(robot_name) - robots[robot_name] = Nav2RobotAdapter( - robot_name, robot_config, robot_config_yaml, node, zenoh_session, - fleet_handle, tf_buffer) + + if 'navigation_stack' not in robot_config_yaml: + error_message = \ + 'Navigation stack must be defined to set up RobotAdapter' + node.get_logger().error(error_message) + raise RuntimeError(error_message) + + nav_stack = robot_config_yaml['navigation_stack'] + if nav_stack == 2: + robots[robot_name] = Nav2RobotAdapter( + robot_name, + robot_config, + robot_config_yaml, + node, + zenoh_session, + fleet_handle, + tf_buffer + ) + elif nav_stack == 1: + robots[robot_name] = Nav1RobotAdapter( + robot_name, + robot_config, + robot_config_yaml, + node, + zenoh_session, + fleet_handle, + tf_buffer + ) + else: + error_message = \ + 'Navigation stack can only be 1 (experimental) or 2' + node.get_logger().error(error_message) + raise RuntimeError(error_message) update_period = 1.0/config_yaml['rmf_fleet'].get( 'robot_state_update_frequency', 10.0 diff --git a/free_fleet_examples/config/fleet/nav1_tb3_simulation_fleet_config.yaml b/free_fleet_examples/config/fleet/nav1_tb3_simulation_fleet_config.yaml new file mode 100644 index 00000000..c63a0050 --- /dev/null +++ b/free_fleet_examples/config/fleet/nav1_tb3_simulation_fleet_config.yaml @@ -0,0 +1,63 @@ +# FLEET CONFIG ================================================================= +# RMF Fleet parameters + +rmf_fleet: + name: "turtlebot3" + limits: + linear: [0.5, 0.75] # velocity, acceleration + angular: [0.6, 2.0] # velocity, acceleration + profile: # Robot profile is modelled as a circle + footprint: 0.3 # radius in m + vicinity: 0.5 # radius in m + reversible: True # whether robots in this fleet can reverse + battery_system: + voltage: 12.0 # V + capacity: 24.0 # Ahr + charging_current: 5.0 # A + mechanical_system: + mass: 20.0 # kg + moment_of_inertia: 10.0 #kgm^2 + friction_coefficient: 0.22 + ambient_system: + power: 20.0 # W + tool_system: + power: 0.0 # W + recharge_threshold: 0.10 # 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 + # actions: ["some_action_here"] + finishing_request: "nothing" # [park, charge, nothing] + responsive_wait: True # Should responsive wait be on/off for the whole fleet by default? False if not specified. + robots: + tb3_0: + charger: "turtlebot3_1_charger" + responsive_wait: False # Should responsive wait be on/off for this specific robot? Overrides the fleet-wide setting. + # For Nav1RobotAdapter + navigation_stack: 1 + initial_map: "L1" + maps: + L1: + map_url: "/opt/ros/jazzy/share/nav2_bringup/maps/turtlebot3_world.yaml" + # initial_pose: [-1.6000019311904907, -0.5031192898750305, 0] + + robot_state_update_frequency: 10.0 # Hz + + +# TRANSFORM CONFIG ============================================================= +# For computing transforms between Robot and RMF coordinate systems + +# Optional +reference_coordinates: + L1: + rmf: [[8.9508, -6.6006], + [7.1006, -9.1508], + [12.3511, -9.2008], + [11.0510, -11.8010]] + robot: [[-1.04555, 2.5456], + [-2.90519, 0.00186], + [2.39611, -0.061773], + [1.08783, -2.59750]] diff --git a/free_fleet_examples/config/fleet/tb3_simulation_fleet_config.yaml b/free_fleet_examples/config/fleet/tb3_simulation_fleet_config.yaml index a0cec1b1..b8bb20ef 100644 --- a/free_fleet_examples/config/fleet/tb3_simulation_fleet_config.yaml +++ b/free_fleet_examples/config/fleet/tb3_simulation_fleet_config.yaml @@ -37,6 +37,7 @@ rmf_fleet: charger: "turtlebot3_1_charger" responsive_wait: False # Should responsive wait be on/off for this specific robot? Overrides the fleet-wide setting. # For Nav2RobotAdapter + navigation_stack: 2 initial_map: "L1" maps: L1: diff --git a/free_fleet_examples/launch/nav1_tb3_simulation_fleet_adapter.launch.xml b/free_fleet_examples/launch/nav1_tb3_simulation_fleet_adapter.launch.xml new file mode 100644 index 00000000..6fad5a53 --- /dev/null +++ b/free_fleet_examples/launch/nav1_tb3_simulation_fleet_adapter.launch.xml @@ -0,0 +1,14 @@ + + + + + + + + + + + + + +