Rover: add rover example

README.md

@@ -81,7 +81,7 @@ source ~/ros2_ws/install/setup.sh
 #### 2. Launch the simulation
 
 ```bash
-ros2 launch ardupilot_gz_bringup iris_runway.launch.py
+ros2 launch ardupilot_gz_bringup iris_runway.launch.py rviz:=true use_gz_tf:=true
 ```
 
 #### 3. Launch a GCS (MAVPorxy)
@@ -120,9 +120,19 @@ Here's a list of all the additional use cases from this repository:
 This simulation has an iris copter equipped with a 360 degrees 2d lidar in a maze world.
 
 ```bash
-ros2 launch ardupilot_gz_bringup iris_maze.launch.py
+ros2 launch ardupilot_gz_bringup iris_maze.launch.py rviz:=true use_gz_tf:=true
 ```
 
+### 2. Wild Thumper playpen
+
+This simulation includes a skid-steer rover equipped with a 2d lidar in a playpen world.
+
+
+```bash
+ros2 launch ardupilot_gz_bringup wildthumper_playpen.launch.py rviz:=true use_gz_tf:=true
+```
+
+
 ## Notes
 
 ### 1. Additional dependencies

ardupilot_gz_bringup/config/wildthumper_bridge.yaml

@@ -0,0 +1,50 @@
+---
+- ros_topic_name: "clock"
+  gz_topic_name: "/clock"
+  ros_type_name: "rosgraph_msgs/msg/Clock"
+  gz_type_name: "gz.msgs.Clock"
+  direction: GZ_TO_ROS
+- ros_topic_name: "joint_states"
+  gz_topic_name: "/world/playpen/model/wildthumper/joint_state"
+  ros_type_name: "sensor_msgs/msg/JointState"
+  gz_type_name: "gz.msgs.Model"
+  direction: GZ_TO_ROS
+- ros_topic_name: "odometry"
+  gz_topic_name: "/model/wildthumper/odometry"
+  ros_type_name: "nav_msgs/msg/Odometry"
+  gz_type_name: "gz.msgs.Odometry"
+  direction: GZ_TO_ROS
+- ros_topic_name: "gz/tf"
+  gz_topic_name: "/model/wildthumper/pose"
+  ros_type_name: "tf2_msgs/msg/TFMessage"
+  gz_type_name: "gz.msgs.Pose_V"
+  direction: GZ_TO_ROS
+- ros_topic_name: "gz/tf_static"
+  gz_topic_name: "/model/wildthumper/pose_static"
+  ros_type_name: "tf2_msgs/msg/TFMessage"
+  gz_type_name: "gz.msgs.Pose_V"
+  direction: GZ_TO_ROS
+
+- ros_topic_name: "imu"
+  gz_topic_name: "/world/playpen/model/wildthumper/link/imu_link/sensor/imu_sensor/imu"
+  ros_type_name: "sensor_msgs/msg/Imu"
+  gz_type_name: "gz.msgs.IMU"
+  direction: GZ_TO_ROS
+
+- ros_topic_name: "magnetometer"
+  gz_topic_name: "/world/playpen/model/wildthumper/link/base_link/sensor/magnetometer_sensor/magnetometer"
+  ros_type_name: "sensor_msgs/msg/MagneticField"
+  gz_type_name: "gz.msgs.Magnetometer"
+  direction: GZ_TO_ROS
+
+- ros_topic_name: "navsat"
+  gz_topic_name: "/world/playpen/model/wildthumper/link/base_link/sensor/navsat_sensor/navsat"
+  ros_type_name: "sensor_msgs/msg/NavSatFix"
+  gz_type_name: "gz.msgs.NavSat"
+  direction: GZ_TO_ROS
+
+- ros_topic_name: "battery"
+  gz_topic_name: "/model/wildthumper/battery/linear_battery/state"
+  ros_type_name: "sensor_msgs/msg/BatteryState"
+  gz_type_name: "gz.msgs.BatteryState"
+  direction: GZ_TO_ROS

ardupilot_gz_bringup/launch/robots/wildthumper.launch.py

@@ -0,0 +1,198 @@
+# Copyright 2023 ArduPilot.org.
+#
+# This program is free software: you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation, either version 3 of the License, or
+# (at your option) any later version.
+#
+# This program is distributed in the hope that it will be useful,
+# but WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+# GNU General Public License for more details.
+#
+# You should have received a copy of the GNU General Public License
+# along with this program. If not, see <https://www.gnu.org/licenses/>.
+
+"""
+Launch an wild thumper rover in Gazebo and Rviz.
+
+ros2 launch ardupilot_sitl sitl_dds_udp.launch.py
+transport:=udp4
+refs:=$(ros2 pkg prefix ardupilot_sitl)
+      /share/ardupilot_sitl/config/dds_xrce_profile.xml
+port:=2019
+synthetic_clock:=True
+wipe:=False
+model:=json
+speedup:=1
+slave:=0
+instance:=0
+defaults:=$(ros2 pkg prefix ardupilot_sitl)
+          /share/ardupilot_sitl/config/default_params/rover-skid.parm,
+          $(ros2 pkg prefix ardupilot_sitl)
+          /share/ardupilot_sitl/config/default_params/dds_udp.parm
+sim_address:=127.0.0.1
+master:=tcp:127.0.0.1:5760
+sitl:=127.0.0.1:5501
+"""
+import os
+
+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 wild thumper rover."""
+    pkg_ardupilot_sitl = get_package_share_directory("ardupilot_sitl")
+    pkg_ardupilot_sitl_models = get_package_share_directory("ardupilot_sitl_models")
+    pkg_project_bringup = get_package_share_directory("ardupilot_gz_bringup")
+
+    # Include component launch files.
+    sitl_dds = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            [
+                PathJoinSubstitution(
+                    [
+                        FindPackageShare("ardupilot_sitl"),
+                        "launch",
+                        "sitl_dds_udp.launch.py",
+                    ]
+                ),
+            ]
+        ),
+        launch_arguments={
+            "transport": "udp4",
+            "refs": PathJoinSubstitution(
+                [
+                    FindPackageShare("ardupilot_sitl"),
+                    "config",
+                    "dds_xrce_profile.xml",
+                ]
+            ),
+            "port": "2019",
+            "command": "ardurover",
+            "synthetic_clock": "True",
+            "wipe": "False",
+            "model": "json",
+            "speedup": "1",
+            "slave": "0",
+            "instance": "0",
+            "defaults": os.path.join(
+                pkg_ardupilot_sitl,
+                "config",
+                "default_params",
+                "rover-skid.parm",
+            )
+            + ","
+            + os.path.join(
+                pkg_ardupilot_sitl,
+                "config",
+                "default_params",
+                "dds_udp.parm",
+            ),
+            "sim_address": "127.0.0.1",
+            "master": "tcp:127.0.0.1:5760",
+            "sitl": "127.0.0.1:5501",
+        }.items(),
+    )
+
+    # 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
+
+    # Load SDF file.
+    sdf_file = os.path.join(
+        pkg_ardupilot_sitl_models, "models", "wildthumper", "model.sdf"
+    )
+    with open(sdf_file, "r") as infp:
+        robot_desc = infp.read()
+
+        # substitute `models://` with `package://ardupilot_sitl_models/models/`
+        # for sdformat_urdf plugin used by robot_state_publisher
+        robot_desc = robot_desc.replace(
+            "model://wildthumper",
+            "package://ardupilot_sitl_models/models/wildthumper")
+
+    # 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": ""},
+        ],
+    )
+
+    # Bridge.
+    bridge = Node(
+        package="ros_gz_bridge",
+        executable="parameter_bridge",
+        parameters=[
+            {
+                "config_file": os.path.join(
+                    pkg_project_bringup, "config", "wildthumper_bridge.yaml"
+                ),
+                "qos_overrides./tf_static.publisher.durability": "transient_local",
+            }
+        ],
+        output="screen",
+    )
+
+    # 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/wildthumper_playpen.launch.py

@@ -0,0 +1,108 @@
+# Copyright 2023 ArduPilot.org.
+#
+# This program is free software: you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation, either version 3 of the License, or
+# (at your option) any later version.
+#
+# This program is distributed in the hope that it will be useful,
+# but WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+# GNU General Public License for more details.
+#
+# You should have received a copy of the GNU General Public License
+# along with this program. If not, see <https://www.gnu.org/licenses/>.
+
+# Adapted from https://github.com/gazebosim/ros_gz_project_template
+#
+# Copyright 2019 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+"""Launch a wildthumper rover in the playpen world."""
+import os
+
+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.conditions import IfCondition
+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 wildthumper rover."""
+    pkg_ardupilot_sitl_models = get_package_share_directory("ardupilot_sitl_models")
+    pkg_project_bringup = get_package_share_directory("ardupilot_gz_bringup")
+    pkg_project_gazebo = get_package_share_directory("ardupilot_gz_gazebo")
+    pkg_ros_gz_sim = get_package_share_directory("ros_gz_sim")
+
+    # Wild Thumper rover.
+    rover = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            [
+                PathJoinSubstitution(
+                    [
+                        FindPackageShare("ardupilot_gz_bringup"),
+                        "launch",
+                        "robots",
+                        "wildthumper.launch.py",
+                    ]
+                ),
+            ]
+        )
+    )
+
+    # Gazebo.
+    gz_sim_server = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(pkg_ros_gz_sim, "launch", "gz_sim.launch.py")
+        ),
+        launch_arguments={
+            "gz_args": "-v4 -s -r "
+            + os.path.join(pkg_ardupilot_sitl_models, "worlds", "wildthumper_playpen.sdf")
+        }.items(),
+    )
+
+    gz_sim_gui = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(pkg_ros_gz_sim, "launch", "gz_sim.launch.py")
+        ),
+        launch_arguments={"gz_args": "-v4 -g"}.items(),
+    )
+
+    # RViz.
+    rviz = Node(
+        package="rviz2",
+        executable="rviz2",
+        arguments=["-d", os.path.join(pkg_project_bringup, "rviz", "wildthumper.rviz")],
+        condition=IfCondition(LaunchConfiguration("rviz")),
+    )
+
+    return LaunchDescription(
+        [
+            DeclareLaunchArgument(
+                "rviz", default_value="true", description="Open RViz."
+            ),
+            gz_sim_server,
+            gz_sim_gui,
+            rover,
+            rviz,
+        ]
+    )