Add warehouse routes for nav2 route server (#5160)

* Added routes for warehouse map.

Signed-off-by: Leander Stephen D'Souza <[email protected]>

* Renamed routes for the depot warehouse to match naming convention.

Signed-off-by: Leander Stephen D'Souza <[email protected]>

* Rename turtlebot4 geojson graphs to remove the turtlebot4 prefix.

Signed-off-by: Leander Stephen D'Souza <[email protected]>

* Restructured warehouse_graph to enable bidirectional navigation.

Signed-off-by: Leander Stephen D'Souza <[email protected]>

* Improved connectivity of the warehouse graph.

Signed-off-by: Leander Stephen D'Souza <[email protected]>

* Added keepout and speed zones to the route example.

Signed-off-by: Leander Stephen D'Souza <[email protected]>

* Added flexibility in setting goals for the route example.

Signed-off-by: Leander Stephen D'Souza <[email protected]>

* Move euler to quaternion function to utils.py.

Signed-off-by: Leander Stephen D'Souza <[email protected]>

* Added additional routes to the warehouse graph.

Signed-off-by: Leander Stephen D'Souza <[email protected]>

* Shift warehouse spawn location to match start of graph node.

Signed-off-by: Leander Stephen D'Souza <[email protected]>

* Added parameters to control enabling of costmap filters.

Signed-off-by: Leander Stephen D'Souza <[email protected]>

* Added node information to the route example launch file.

Signed-off-by: Leander Stephen D'Souza <[email protected]>

---------

Signed-off-by: Leander Stephen D'Souza <[email protected]>

Author: leander-dsouza

nav2_bringup/graphs/turtlebot4_graph.geojson renamed to nav2_bringup/graphs/depot_graph.geojson

@@ -1,6 +1,6 @@
 {
     "type": "FeatureCollection",
-    "name": "turtlebot4_graph",
+    "name": "depot_graph",
     "crs": { "type": "name", "properties": { "name": "urn:ogc:def:crs:EPSG::3857" } },
     "features": [
     { "type": "Feature", "properties": { "id": 0 }, "geometry": { "type": "Point", "coordinates": [ 0.624282608695647, 12.880652173913044 ] } },

nav2_bringup/graphs/warehouse_graph.geojson

@@ -63,7 +63,7 @@ def generate_launch_description() -> LaunchDescription:
 
     declare_graph_file_cmd = DeclareLaunchArgument(
         'graph',
-        default_value=os.path.join(bringup_dir, 'graphs', 'turtlebot4_graph.geojson'),
+        default_value=os.path.join(bringup_dir, 'graphs', 'depot_graph.geojson'),
     )
 
     declare_params_file_cmd = DeclareLaunchArgument(

nav2_bringup/launch/tb4_loopback_simulation_launch.py

@@ -34,7 +34,7 @@
         'R': 0.00, 'P': 0.00, 'Y': 0.00
     },
     'warehouse': {
-        'x': 2.12, 'y': -21.3, 'z': 0.01,
+        'x': 2.00, 'y': -19.65, 'z': 0.01,
         'R': 0.00, 'P': 0.00, 'Y': 1.57
     }
 }
@@ -114,7 +114,7 @@ def generate_launch_description() -> LaunchDescription:
 
     declare_graph_file_cmd = DeclareLaunchArgument(
         'graph',
-        default_value=os.path.join(bringup_dir, 'graphs', 'turtlebot4_graph.geojson'),
+        default_value=os.path.join(bringup_dir, 'graphs', f'{MAP_TYPE}_graph.geojson'),
     )
 
     declare_use_sim_time_cmd = DeclareLaunchArgument(

nav2_bringup/launch/tb4_simulation_launch.py

@@ -26,24 +26,80 @@
 from launch.substitutions import Command, LaunchConfiguration, PythonExpression
 from launch_ros.actions import Node
 
+# Define local map types
+MAP_POSES_DICT = {
+    'depot': {
+        'x': -8.00, 'y': 0.00, 'z': 0.01,
+        'R': 0.00, 'P': 0.00, 'Y': 0.00
+    },
+    'warehouse': {
+        'x': 2.00, 'y': -19.65, 'z': 0.01,
+        'R': 0.00, 'P': 0.00, 'Y': 0.00
+    }
+}
+
+ROUTE_POSES_DICT = {
+    'start': {
+        'depot': {
+            'x': 7.5, 'y': 7.5, 'yaw': 0.00  # 3rd node
+        },
+        'warehouse': {
+            'x': 2.00, 'y': -19.65, 'yaw': 0.00  # 0th node
+        }
+    },
+    'goal': {
+        'depot': {
+            'x': 20.12, 'y': 11.83, 'yaw': 0.00  # 13th node
+        },
+        'warehouse': {
+            'x': -13.5, 'y': -3.15, 'yaw': 0.00  # 61st node
+        }
+    }
+}
+MAP_TYPE = 'depot'  # Change this to 'warehouse' for warehouse map
+
 
 def generate_launch_description() -> LaunchDescription:
     nav2_bringup_dir = get_package_share_directory('nav2_bringup')
     sim_dir = get_package_share_directory('nav2_minimal_tb4_sim')
     desc_dir = get_package_share_directory('nav2_minimal_tb4_description')
 
     robot_sdf = os.path.join(desc_dir, 'urdf', 'standard', 'turtlebot4.urdf.xacro')
-    world = os.path.join(sim_dir, 'worlds', 'depot.sdf')
-    map_yaml_file = os.path.join(nav2_bringup_dir, 'maps', 'depot.yaml')
-    graph_filepath = os.path.join(nav2_bringup_dir, 'graphs', 'turtlebot4_graph.geojson')
+    world = os.path.join(sim_dir, 'worlds', f'{MAP_TYPE}.sdf')
+    map_yaml_file = os.path.join(nav2_bringup_dir, 'maps', f'{MAP_TYPE}.yaml')
+    graph_filepath = os.path.join(
+        nav2_bringup_dir, 'graphs', f'{MAP_TYPE}_graph.geojson')
 
     # Launch configuration variables
     headless = LaunchConfiguration('headless')
+    use_keepout_zones = LaunchConfiguration('use_keepout_zones')
+    use_speed_zones = LaunchConfiguration('use_speed_zones')
+    keepout_mask_yaml_file = LaunchConfiguration('keepout_mask')
+    speed_mask_yaml_file = LaunchConfiguration('speed_mask')
 
     # Declare the launch arguments
     declare_headless_cmd = DeclareLaunchArgument(
         'headless', default_value='False', description='Whether to execute gzclient)'
     )
+    declare_use_keepout_zones_cmd = DeclareLaunchArgument(
+        'use_keepout_zones', default_value='True',
+        description='Whether to enable keepout zones or not'
+    )
+
+    declare_use_speed_zones_cmd = DeclareLaunchArgument(
+        'use_speed_zones', default_value='True',
+        description='Whether to enable speed zones or not'
+    )
+    declare_keepout_mask_yaml_cmd = DeclareLaunchArgument(
+        'keepout_mask',
+        default_value=os.path.join(nav2_bringup_dir, 'maps', f'{MAP_TYPE}_keepout.yaml'),
+        description='Full path to keepout mask file to load',
+    )
+    declare_speed_mask_yaml_cmd = DeclareLaunchArgument(
+        'speed_mask',
+        default_value=os.path.join(nav2_bringup_dir, 'maps', f'{MAP_TYPE}_speed.yaml'),
+        description='Full path to speed mask file to load',
+    )
 
     # start the simulation
     world_sdf = tempfile.mktemp(prefix='nav2_', suffix='.sdf')
@@ -79,12 +135,13 @@ def generate_launch_description() -> LaunchDescription:
             os.path.join(sim_dir, 'launch', 'spawn_tb4.launch.py')),
         launch_arguments={'use_sim_time': 'True',
                           'robot_sdf': robot_sdf,
-                          'x_pose': '-8.0',
-                          'y_pose': '0.0',
-                          'z_pose': '0.0',
-                          'roll': '0.0',
-                          'pitch': '0.0',
-                          'yaw': '0.0'}.items())
+                          'x_pose': str(MAP_POSES_DICT[MAP_TYPE]['x']),
+                          'y_pose': str(MAP_POSES_DICT[MAP_TYPE]['y']),
+                          'z_pose': str(MAP_POSES_DICT[MAP_TYPE]['z']),
+                          'roll': str(MAP_POSES_DICT[MAP_TYPE]['R']),
+                          'pitch': str(MAP_POSES_DICT[MAP_TYPE]['P']),
+                          'yaw': str(MAP_POSES_DICT[MAP_TYPE]['Y']),
+                          }.items())
 
     start_robot_state_publisher_cmd = Node(
         package='robot_state_publisher',
@@ -107,12 +164,31 @@ def generate_launch_description() -> LaunchDescription:
     bringup_cmd = IncludeLaunchDescription(
         PythonLaunchDescriptionSource(
             os.path.join(nav2_bringup_dir, 'launch', 'bringup_launch.py')),
-        launch_arguments={'map': map_yaml_file, 'graph': graph_filepath}.items())
+        launch_arguments={
+            'map': map_yaml_file,
+            'graph': graph_filepath,
+            'use_keepout_zones': use_keepout_zones,
+            'use_speed_zones': use_speed_zones,
+            'keepout_mask': keepout_mask_yaml_file,
+            'speed_mask': speed_mask_yaml_file,
+        }.items())
 
     # start the demo autonomy task
     demo_cmd = Node(
         package='nav2_simple_commander',
         executable='example_route',
+        parameters=[{
+            'start_pose': {
+                'x': ROUTE_POSES_DICT['start'][MAP_TYPE]['x'],
+                'y': ROUTE_POSES_DICT['start'][MAP_TYPE]['y'],
+                'yaw': ROUTE_POSES_DICT['start'][MAP_TYPE]['yaw']
+            },
+            'goal_pose': {
+                'x': ROUTE_POSES_DICT['goal'][MAP_TYPE]['x'],
+                'y': ROUTE_POSES_DICT['goal'][MAP_TYPE]['y'],
+                'yaw': ROUTE_POSES_DICT['goal'][MAP_TYPE]['yaw']
+            },
+        }],
         emulate_tty=True,
         output='screen')
 
@@ -122,6 +198,10 @@ def generate_launch_description() -> LaunchDescription:
 
     ld = LaunchDescription()
     ld.add_action(declare_headless_cmd)
+    ld.add_action(declare_use_keepout_zones_cmd)
+    ld.add_action(declare_use_speed_zones_cmd)
+    ld.add_action(declare_keepout_mask_yaml_cmd)
+    ld.add_action(declare_speed_mask_yaml_cmd)
     ld.add_action(set_env_vars_resources)
     ld.add_action(world_sdf_xacro)
     ld.add_action(remove_temp_sdf_file)

nav2_simple_commander/launch/route_example_launch.py

@@ -15,6 +15,7 @@
 
 from geometry_msgs.msg import Pose, PoseStamped
 from nav2_simple_commander.robot_navigator import BasicNavigator, RunningTask, TaskResult
+from nav2_simple_commander.utils import euler_to_quaternion
 import rclpy
 from std_msgs.msg import Header
 
@@ -34,16 +35,35 @@ def toPoseStamped(pt: Pose, header: Header) -> PoseStamped:
 def main() -> None:
     rclpy.init()
 
+    node = rclpy.create_node('route_example')
+
+    node.declare_parameter('start_pose.x', 0.0)
+    node.declare_parameter('start_pose.y', 0.0)
+    node.declare_parameter('start_pose.yaw', 0.0)
+
+    node.declare_parameter('goal_pose.x', 0.0)
+    node.declare_parameter('goal_pose.y', 0.0)
+    node.declare_parameter('goal_pose.yaw', 0.0)
+
+    start_x = node.get_parameter('start_pose.x').value
+    start_y = node.get_parameter('start_pose.y').value
+    start_yaw = node.get_parameter('start_pose.yaw').value
+
+    goal_x = node.get_parameter('goal_pose.x').value
+    goal_y = node.get_parameter('goal_pose.y').value
+    goal_yaw = node.get_parameter('goal_pose.yaw').value
+
+    node.destroy_node()
+
     navigator = BasicNavigator()
 
     # Set our demo's initial pose
     initial_pose = PoseStamped()
     initial_pose.header.frame_id = 'map'
     initial_pose.header.stamp = navigator.get_clock().now().to_msg()
-    initial_pose.pose.position.x = 7.5
-    initial_pose.pose.position.y = 7.5
-    initial_pose.pose.orientation.z = 0.0
-    initial_pose.pose.orientation.w = 1.0
+    initial_pose.pose.position.x = start_x
+    initial_pose.pose.position.y = start_y
+    initial_pose.pose.orientation = euler_to_quaternion(0.0, 0.0, start_yaw)
     navigator.setInitialPose(initial_pose)
 
     # Activate navigation, if not autostarted. This should be called after setInitialPose()
@@ -66,9 +86,9 @@ def main() -> None:
     goal_pose = PoseStamped()
     goal_pose.header.frame_id = 'map'
     goal_pose.header.stamp = navigator.get_clock().now().to_msg()
-    goal_pose.pose.position.x = 20.12
-    goal_pose.pose.position.y = 11.83
-    goal_pose.pose.orientation.w = 1.0
+    goal_pose.pose.position.x = goal_x
+    goal_pose.pose.position.y = goal_y
+    goal_pose.pose.orientation = euler_to_quaternion(0.0, 0.0, goal_yaw)
 
     # Sanity check a valid route exists using PoseStamped.
     # May also use NodeIDs on the graph if they are known by passing them instead as `int`

nav2_simple_commander/nav2_simple_commander/example_route.py

@@ -16,10 +16,13 @@
 
 """General utility functions."""
 
+import math
 import os
 import signal
 import subprocess
 
+from geometry_msgs.msg import Quaternion
+
 
 def find_os_processes(name: str) -> list[tuple[str, str]]:
     """Find all the processes that are running gz sim."""
@@ -53,3 +56,23 @@ def kill_os_processes(name: str) -> None:
             kill_process(pid)
     else:
         print(f'No processes found starting with {name}')
+
+
+def euler_to_quaternion(
+    roll: float = 0.0, pitch: float = 0.0,
+        yaw: float = 0.0) -> Quaternion:
+    """Convert euler angles to quaternion."""
+    cy = math.cos(yaw * 0.5)
+    sy = math.sin(yaw * 0.5)
+    cp = math.cos(pitch * 0.5)
+    sp = math.sin(pitch * 0.5)
+    cr = math.cos(roll * 0.5)
+    sr = math.sin(roll * 0.5)
+
+    q = Quaternion()
+    q.w = cr * cp * cy + sr * sp * sy
+    q.x = sr * cp * cy - cr * sp * sy
+    q.y = cr * sp * cy + sr * cp * sy
+    q.z = cr * cp * sy - sr * sp * cy
+
+    return q