Add twist-stamper node so nav2 example works with humble

README.md

@@ -96,7 +96,7 @@ Then run the controller using,
 
 Now, using the keyboard keys you can control the drone.
 
-### 2. Obstacle avoidance using Cartographer and Nav2
+### 3. Obstacle avoidance using Cartographer and Nav2
 
 Using the same simulation as before, the nav2 node can be launched to control the copter once it is in the air.
 

config/navigation.yaml

@@ -59,6 +59,8 @@ bt_navigator_navigate_to_pose_rclcpp_node:
 
 controller_server:
   ros__parameters:
+    # TODO: enable when navigation2 supports twist stamped
+    # enable_stamped_cmd_vel: True
     use_sim_time: *default_use_sim_time
     failure_tolerance: 0.3
     progress_checker_plugin: "progress_checker"
@@ -93,7 +95,7 @@ global_costmap:
       global_frame: map
       robot_base_frame: base_link
       use_sim_time: *default_use_sim_time
-      robot_radius: 0.25
+      robot_radius: 0.35
       resolution: 0.05
       lethal_cost_threshold: 50
       always_send_full_costmap: True
@@ -123,7 +125,7 @@ local_costmap:
       width: 5
       height: 5
       resolution: 0.05
-      robot_radius: 0.25
+      robot_radius: 0.35
       always_send_full_costmap: True
       plugins: ["voxel_layer", "inflation_layer"]
       voxel_layer:

launch/navigation.launch.py

@@ -1,3 +1,5 @@
+import os
+
 from launch import LaunchDescription
 from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, GroupAction
 from launch.substitutions import LaunchConfiguration
@@ -15,7 +17,8 @@ def generate_launch_description():
     # Navigation
     navigation = GroupAction(
         actions=[
-            SetRemap(src="/cmd_vel", dst="/ap/cmd_vel"),
+            # TODO: enable when navigation2 supports twist stamped
+            # SetRemap(src="/cmd_vel", dst="/ap/cmd_vel"),
             IncludeLaunchDescription(
                 PythonLaunchDescriptionSource(
                     str(
@@ -38,6 +41,33 @@ def generate_launch_description():
         ]
     )
 
+    # TODO: disable when navigation2 supports twist stamped
+    # Twist stamper.
+    twist_stamper = Node(
+        package="twist_stamper",
+        executable="twist_stamper",
+        parameters=[
+            {"frame_id": "base_link"},
+        ],
+        remappings=[
+            ("cmd_vel_in", "cmd_vel"),
+            ("cmd_vel_out", "ap/cmd_vel"),
+        ],
+    )
+
+    # Robot description.
+
+    # Ensure `SDF_PATH` is populated as `sdformat_urdf`` uses this rather
+    # than `GZ_SIM_RESOURCE_PATH` to locate resources.
+    if "GZ_SIM_RESOURCE_PATH" in os.environ:
+        gz_sim_resource_path = os.environ["GZ_SIM_RESOURCE_PATH"]
+
+        if "SDF_PATH" in os.environ:
+            sdf_path = os.environ["SDF_PATH"]
+            os.environ["SDF_PATH"] = sdf_path + ":" + gz_sim_resource_path
+        else:
+            os.environ["SDF_PATH"] = gz_sim_resource_path
+
     # RViz.
     rviz = Node(
         package="rviz2",
@@ -60,7 +90,8 @@ def generate_launch_description():
             DeclareLaunchArgument(
                 "rviz", default_value="true", description="Open RViz."
             ),
-            rviz,
             navigation,
+            twist_stamper,
+            rviz,
         ]
     )

launch/twist_stamper.launch.py

@@ -0,0 +1,26 @@
+from launch import LaunchDescription
+from launch_ros.actions import Node
+from pathlib import Path
+
+"""Generate a launch description for the twist_stamper node."""
+
+
+def generate_launch_description():
+    # Twist stamper.
+    twist_stamper = Node(
+        package="twist_stamper",
+        executable="twist_stamper",
+        parameters=[
+            {"frame_id": "base_link"},
+        ],
+        remappings=[
+            ("cmd_vel_in", "cmd_vel"),
+            ("cmd_vel_out", "ap/cmd_vel"),
+        ],
+    )
+
+    return LaunchDescription(
+        [
+            twist_stamper,
+        ]
+    )

package.xml

@@ -18,6 +18,7 @@
   <exec_depend>robot_state_publisher</exec_depend>
   <exec_depend>topic_tools</exec_depend>
   <exec_depend>navigation2</exec_depend>
+  <exec_depend>twist_stamper</exec_depend>
   <test_depend>ament_copyright</test_depend>
   <test_depend>ament_flake8</test_depend>
   <test_depend>ament_pep257</test_depend>

rviz/cartographer.rviz

@@ -5,8 +5,8 @@ Panels:
     Property Tree Widget:
       Expanded:
         - /Global Options1
-        - /Iris1
-        - /Iris1/Odometry1/Shape1
+        - /Robot1
+        - /Robot1/Odometry1/Shape1
       Splitter Ratio: 0.5
     Tree Height: 555
   - Class: rviz_common/Selection
@@ -258,7 +258,7 @@ Visualization Manager:
           Use rainbow: true
           Value: true
       Enabled: true
-      Name: Iris
+      Name: Robot
   Enabled: true
   Global Options:
     Background Color: 48; 48; 48