Rover: add lidar to wild thumper

Signed-off-by: Rhys Mainwaring <[email protected]> Rover: add SITL_Models to ros2 repos Signed-off-by: Rhys Mainwaring <[email protected]>
ArduPilot · Oct 26, 2023 · 4c1a946 · 4c1a946
1 parent 6ecc691
commit 4c1a946
Show file tree

Hide file tree

Showing 7 changed files with 136 additions and 3 deletions.
diff --git a/ardupilot_gz_bringup/config/wildthumper_bridge.yaml b/ardupilot_gz_bringup/config/wildthumper_bridge.yaml
@@ -48,3 +48,9 @@
   ros_type_name: "sensor_msgs/msg/BatteryState"
   gz_type_name: "gz.msgs.BatteryState"
   direction: GZ_TO_ROS
+
+- ros_topic_name: "scan"
+  gz_topic_name: "/lidar"
+  ros_type_name: "sensor_msgs/msg/LaserScan"
+  gz_type_name: "gz.msgs.LaserScan"
+  direction: GZ_TO_ROS
diff --git a/ardupilot_gz_bringup/launch/robots/wildthumper.launch.py b/ardupilot_gz_bringup/launch/robots/wildthumper.launch.py
@@ -127,7 +127,7 @@ def generate_launch_description():
 
     # Load SDF file.
     sdf_file = os.path.join(
-        pkg_ardupilot_sitl_models, "models", "wildthumper", "model.sdf"
+        pkg_ardupilot_sitl_models, "models", "wildthumper_with_lidar", "model.sdf"
     )
     with open(sdf_file, "r") as infp:
         robot_desc = infp.read()
@@ -138,6 +138,10 @@ def generate_launch_description():
             "model://wildthumper",
             "package://ardupilot_sitl_models/models/wildthumper")
 
+        robot_desc = robot_desc.replace(
+            "model://wildthumper_with_lidar",
+            "package://ardupilot_sitl_models/models/wildthumper_with_lidar")
+
     # Publish /tf and /tf_static.
     robot_state_publisher = Node(
         package="robot_state_publisher",

diff --git a/ardupilot_gz_bringup/launch/wildthumper_playpen.launch.py b/ardupilot_gz_bringup/launch/wildthumper_playpen.launch.py
@@ -48,7 +48,6 @@
 
 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")
@@ -76,7 +75,7 @@ def generate_launch_description():
         ),
         launch_arguments={
             "gz_args": "-v4 -s -r "
-            + os.path.join(pkg_ardupilot_sitl_models, "worlds", "wildthumper_playpen.sdf")
+            + os.path.join(pkg_project_gazebo, "worlds", "wildthumper_playpen.sdf")
         }.items(),
     )
 

diff --git a/ardupilot_gz_bringup/rviz/wildthumper.rviz b/ardupilot_gz_bringup/rviz/wildthumper.rviz
@@ -114,6 +114,11 @@ Visualization Manager:
               Alpha: 1
               Show Axes: false
               Show Trail: false
+            lidar_link:
+              Alpha: 1
+              Show Axes: false
+              Show Trail: false
+              Value: true
             mid_left_motor_asm_link:
               Alpha: 1
               Show Axes: false
@@ -203,6 +208,8 @@ Visualization Manager:
               Value: true
             imu_link:
               Value: true
+            lidar_link:
+              Value: true
             mid_left_motor_asm_link:
               Value: true
             mid_left_wheel_link:
@@ -248,6 +255,8 @@ Visualization Manager:
                       {}
                 imu_link:
                   {}
+                lidar_link:
+                  {}
                 mid_wheel_chassis_long_link:
                   mid_left_motor_asm_link:
                     mid_left_wheel_link:
@@ -307,6 +316,40 @@ Visualization Manager:
             Reliability Policy: Reliable
             Value: /odometry
           Value: true
+        - Alpha: 1
+          Autocompute Intensity Bounds: true
+          Autocompute Value Bounds:
+            Max Value: 10
+            Min Value: -10
+            Value: true
+          Axis: Z
+          Channel Name: intensity
+          Class: rviz_default_plugins/LaserScan
+          Color: 255; 255; 255
+          Color Transformer: ""
+          Decay Time: 0
+          Enabled: true
+          Invert Rainbow: false
+          Max Color: 255; 255; 255
+          Max Intensity: 4096
+          Min Color: 0; 0; 0
+          Min Intensity: 0
+          Name: LaserScan
+          Position Transformer: ""
+          Selectable: true
+          Size (Pixels): 3
+          Size (m): 0.009999999776482582
+          Style: Flat Squares
+          Topic:
+            Depth: 5
+            Durability Policy: Volatile
+            Filter size: 10
+            History Policy: Keep Last
+            Reliability Policy: Reliable
+            Value: /scan
+          Use Fixed Frame: true
+          Use rainbow: true
+          Value: true
       Enabled: true
       Name: WildThumper
   Enabled: true

diff --git a/ardupilot_gz_gazebo/worlds/wildthumper_playpen.sdf b/ardupilot_gz_gazebo/worlds/wildthumper_playpen.sdf
@@ -0,0 +1,73 @@
+<?xml version="1.0" ?>
+<sdf version="1.9">
+  <world name="playpen">
+    <physics name="1ms" type="ignore">
+      <max_step_size>0.001</max_step_size>
+      <real_time_factor>1.0</real_time_factor>
+    </physics>
+
+    <plugin filename="gz-sim-physics-system"
+      name="gz::sim::systems::Physics">
+    </plugin>
+    <plugin
+      filename="gz-sim-sensors-system"
+      name="gz::sim::systems::Sensors">
+      <render_engine>ogre2</render_engine>
+    </plugin>
+    <plugin filename="gz-sim-user-commands-system"
+      name="gz::sim::systems::UserCommands">
+    </plugin>
+    <plugin filename="gz-sim-scene-broadcaster-system"
+      name="gz::sim::systems::SceneBroadcaster">
+    </plugin>
+    <plugin filename="gz-sim-imu-system"
+      name="gz::sim::systems::Imu">
+    </plugin>
+    <plugin filename="gz-sim-magnetometer-system"
+      name="gz::sim::systems::Magnetometer">
+    </plugin>
+    <plugin filename="gz-sim-navsat-system"
+      name="gz::sim::systems::NavSat">
+    </plugin>
+
+    <scene>
+      <ambient>1.0 1.0 1.0</ambient>
+      <background>0.8 0.8 0.8</background>
+      <sky></sky>
+    </scene>
+
+    <light type="directional" name="sun">
+      <cast_shadows>true</cast_shadows>
+      <pose>0 0 10 0 0 0</pose>
+      <diffuse>0.8 0.8 0.8 1</diffuse>
+      <specular>0.8 0.8 0.8 1</specular>
+      <attenuation>
+        <range>1000</range>
+        <constant>0.9</constant>
+        <linear>0.01</linear>
+        <quadratic>0.001</quadratic>
+      </attenuation>
+      <direction>-0.5 0.1 -0.9</direction>
+    </light>
+
+    <spherical_coordinates>
+      <latitude_deg>-35.3632621</latitude_deg>
+      <longitude_deg>149.1652374</longitude_deg>
+      <elevation>10.0</elevation>
+      <heading_deg>0</heading_deg>
+      <surface_model>EARTH_WGS84</surface_model>
+    </spherical_coordinates>
+
+    <include>
+      <pose degrees="true">0 0 0 0 0 0</pose>
+      <uri>model://rover_playpen</uri>
+    </include>
+
+    <include>
+      <pose degrees="true">0 0 0.15 0 0 0</pose>
+      <uri>model://wildthumper_with_lidar</uri>
+      <name>wildthumper</name>
+    </include>
+
+  </world>
+</sdf>
diff --git a/ros2_gz.repos b/ros2_gz.repos
@@ -11,6 +11,10 @@ repositories:
     type: git
     url: https://github.com/ArduPilot/ardupilot_gz.git
     version: main
+  ardupilot_sitl_models:
+    type: git
+    url: https://github.com/ArduPilot/SITL_Models.git
+    version: main
   micro_ros_agent:
     type: git
     url: https://github.com/micro-ROS/micro-ROS-Agent.git

diff --git a/ros2_gz_macos.repos b/ros2_gz_macos.repos
@@ -11,6 +11,10 @@ repositories:
     type: git
     url: https://github.com/ArduPilot/ardupilot_gz.git
     version: main
+  ardupilot_sitl_models:
+    type: git
+    url: https://github.com/ArduPilot/SITL_Models.git
+    version: main
   gps_umd:
     type: git
     url: https://github.com/swri-robotics/gps_umd.git