Copter: fix odometry transform

Update iris_with_lidar model
- Remove PosePublisher plugin from iris_with_lidar model.
- Set base_link as mobile base in odometry publisher.

Update iris_maze world
- Change initial pose to align with world x-axis (east).
- For consistency with cartographer.

Update iris_bridge config, prefix TF topics from Gazebo with /gz.

Update iris_maze launch file
- Update comment for robot_state_publisher in launch.
- Add package dependency on topic_tools.
- Remove tf remapping in robot_state_publisher.
- Add topic_tools node to relay /filter '/gz/tf' for odom transform.
- Add launch argument to enable relay of /gz/tf to /tf (default true).
- Add launch event to delay relay until bridge has started.

Update rviz config
- Remove 'iris' TF prefix from rviz.
- Add laser scan to rviz.

Signed-off-by: Rhys Mainwaring <[email protected]>

ardupilot_gz_bringup/launch/robots/iris.launch.py

@@ -40,14 +40,23 @@
 from ament_index_python.packages import get_package_share_directory
 
 from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
 from launch.actions import IncludeLaunchDescription
+from launch.actions import RegisterEventHandler
+
+from launch.conditions import IfCondition
+
+from launch.event_handlers import OnProcessStart
+
 from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.substitutions import LaunchConfiguration
 from launch.substitutions import PathJoinSubstitution
 
 from launch_ros.actions import Node
 from launch_ros.substitutions import FindPackageShare
 
 
+
 def generate_launch_description():
     """Generate a launch description for a iris quadcopter."""
     pkg_ardupilot_sitl = get_package_share_directory("ardupilot_sitl")
@@ -122,14 +131,15 @@ def generate_launch_description():
         robot_desc = infp.read()
         # print(robot_desc)
 
-    # Remap the /tf and /tf_static under /ignore as the TF conflicts.
+    # Publish /tf and /tf_static.
     robot_state_publisher = Node(
         package="robot_state_publisher",
         executable="robot_state_publisher",
         name="robot_state_publisher",
         output="both",
         parameters=[
             {"robot_description": robot_desc},
+            {"frame_prefix": ""},
         ],
     )
 
@@ -148,10 +158,52 @@ def generate_launch_description():
         output="screen",
     )
 
+    # Transform - use if the model includes "gz::sim::systems::PosePublisher"
+    #             and a filter is required.
+    # topic_tools_tf = Node(
+    #     package="topic_tools",
+    #     executable="transform",
+    #     arguments=[
+    #         "/gz/tf",
+    #         "/tf",
+    #         "tf2_msgs/msg/TFMessage",
+    #         "tf2_msgs.msg.TFMessage(transforms=[x for x in m.transforms if x.header.frame_id == 'odom'])",
+    #         "--import",
+    #         "tf2_msgs",
+    #         "geometry_msgs",
+    #     ],
+    #     output="screen",
+    #     respawn=True,
+    # )
+
+    # Relay - use instead of transform when Gazebo is only publishing odom -> base_link
+    topic_tools_tf = Node(
+        package="topic_tools",
+        executable="relay",
+        arguments=[
+            "/gz/tf",
+            "/tf",
+        ],
+        output="screen",
+        respawn=False,
+        condition=IfCondition(LaunchConfiguration("use_gz_tf")),
+    )
+
     return LaunchDescription(
         [
+            DeclareLaunchArgument(
+                "use_gz_tf", default_value="true", description="Use Gazebo TF."
+            ),
             sitl_dds,
             robot_state_publisher,
             bridge,
+            RegisterEventHandler(
+                OnProcessStart(
+                    target_action=bridge,
+                    on_start=[
+                        topic_tools_tf
+                    ]
+                )
+            ),
         ]
     )

ardupilot_gz_bringup/launch/robots/iris_lidar.launch.py

@@ -40,8 +40,16 @@
 from ament_index_python.packages import get_package_share_directory
 
 from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
 from launch.actions import IncludeLaunchDescription
+from launch.actions import RegisterEventHandler
+
+from launch.conditions import IfCondition
+
+from launch.event_handlers import OnProcessStart
+
 from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.substitutions import LaunchConfiguration
 from launch.substitutions import PathJoinSubstitution
 
 from launch_ros.actions import Node
@@ -125,14 +133,15 @@ def generate_launch_description():
         robot_desc = infp.read()
         # print(robot_desc)
 
-    # Remap the /tf and /tf_static under /ignore as the TF conflicts.
+    # Publish /tf and /tf_static.
     robot_state_publisher = Node(
         package="robot_state_publisher",
         executable="robot_state_publisher",
         name="robot_state_publisher",
         output="both",
         parameters=[
             {"robot_description": robot_desc},
+            {"frame_prefix": ""},
         ],
     )
 
@@ -151,10 +160,52 @@ def generate_launch_description():
         output="screen",
     )
 
+    # Transform - use if the model includes "gz::sim::systems::PosePublisher"
+    #             and a filter is required.
+    # topic_tools_tf = Node(
+    #     package="topic_tools",
+    #     executable="transform",
+    #     arguments=[
+    #         "/gz/tf",
+    #         "/tf",
+    #         "tf2_msgs/msg/TFMessage",
+    #         "tf2_msgs.msg.TFMessage(transforms=[x for x in m.transforms if x.header.frame_id == 'odom'])",
+    #         "--import",
+    #         "tf2_msgs",
+    #         "geometry_msgs",
+    #     ],
+    #     output="screen",
+    #     respawn=True,
+    # )
+
+    # Relay - use instead of transform when Gazebo is only publishing odom -> base_link
+    topic_tools_tf = Node(
+        package="topic_tools",
+        executable="relay",
+        arguments=[
+            "/gz/tf",
+            "/tf",
+        ],
+        output="screen",
+        respawn=False,
+        condition=IfCondition(LaunchConfiguration("use_gz_tf")),
+    )
+
     return LaunchDescription(
         [
+            DeclareLaunchArgument(
+                "use_gz_tf", default_value="true", description="Use Gazebo TF."
+            ),
             sitl_dds,
             robot_state_publisher,
             bridge,
+            RegisterEventHandler(
+                OnProcessStart(
+                    target_action=bridge,
+                    on_start=[
+                        topic_tools_tf
+                    ]
+                )
+            ),
         ]
     )

ardupilot_gz_bringup/package.xml

@@ -16,6 +16,7 @@
   <exec_depend>launch_ros</exec_depend>
   <exec_depend>ros_gz_sim</exec_depend>
   <exec_depend>robot_state_publisher</exec_depend>
+  <exec_depend>topic_tools</exec_depend>
   <test_depend>ament_cmake_pytest</test_depend>
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>

ardupilot_gz_bringup/rviz/iris.rviz

@@ -4,8 +4,8 @@ Panels:
     Name: Displays
     Property Tree Widget:
       Expanded:
+        - /Global Options1
         - /Iris1
-        - /Iris1/TF1
       Splitter Ratio: 0.5
     Tree Height: 288
   - Class: rviz_common/Selection
@@ -122,7 +122,7 @@ Visualization Manager:
             Inertia: false
             Mass: false
           Name: RobotModel
-          TF Prefix: iris
+          TF Prefix: ""
           Update Interval: 0
           Value: true
           Visual Enabled: true
@@ -131,54 +131,49 @@ Visualization Manager:
           Frame Timeout: 15
           Frames:
             All Enabled: true
-            iris:
-              Value: true
-            iris/base_link:
+            base_link:
               Value: true
-            iris/gimbal_link:
+            gimbal_link:
               Value: true
-            iris/imu_link:
+            imu_link:
               Value: true
-            iris/odom:
+            odom:
               Value: true
-            iris/roll_link:
+            roll_link:
               Value: true
-            iris/rotor_0:
+            rotor_0:
               Value: true
-            iris/rotor_1:
+            rotor_1:
               Value: true
-            iris/rotor_2:
+            rotor_2:
               Value: true
-            iris/rotor_3:
+            rotor_3:
               Value: true
-            iris/tilt_link:
+            tilt_link:
               Value: true
           Marker Scale: 1
           Name: TF
           Show Arrows: true
           Show Axes: true
           Show Names: false
           Tree:
-            iris/odom:
-              iris:
-                iris/base_link:
-                  {}
-                iris/gimbal_link:
-                  {}
-                iris/imu_link:
-                  {}
-                iris/roll_link:
-                  {}
-                iris/rotor_0:
-                  {}
-                iris/rotor_1:
-                  {}
-                iris/rotor_2:
-                  {}
-                iris/rotor_3:
-                  {}
-                iris/tilt_link:
-                  {}
+            base_link:
+              gimbal_link:
+                roll_link:
+                  tilt_link:
+                    {}
+              imu_link:
+                {}
+              rotor_0:
+                {}
+              rotor_1:
+                {}
+              rotor_2:
+                {}
+              rotor_3:
+                {}
+            odom:
+              {}
           Update Interval: 0
           Value: true
         - Angle Tolerance: 0.10000000149011612
@@ -239,7 +234,7 @@ Visualization Manager:
   Enabled: true
   Global Options:
     Background Color: 48; 48; 48
-    Fixed Frame: iris/odom
+    Fixed Frame: odom
     Frame Rate: 30
   Name: root
   Tools:
@@ -321,4 +316,4 @@ Window Geometry:
     collapsed: false
   Width: 1200
   X: 0
-  Y: 25
+  Y: 38