Basic nav1 fleet adapter setup done

Signed-off-by: Aaron Chong <[email protected]>

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

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]]

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:

free_fleet_examples/launch/nav1_tb3_simulation_fleet_adapter.launch.xml

@@ -0,0 +1,14 @@
+<?xml version='1.0' ?>
+
+<launch>
+  <arg name="server_uri" default="" description="The URI of the api server to transmit state and task information."/>
+
+  <group>
+    <include file="$(find-pkg-share free_fleet_adapter)/fleet_adapter.launch.xml">
+      <arg name="use_sim_time" value="false"/>
+      <arg name="nav_graph_file" value="$(find-pkg-share free_fleet_examples)/maps/turtlebot3_world/nav_graphs/0.yaml" />
+      <arg name="config_file" value="$(find-pkg-share free_fleet_examples)/config/fleet/nav1_tb3_simulation_fleet_config.yaml"/>
+      <arg name="server_uri" value="$(var server_uri)"/>
+    </include>
+  </group>
+</launch>