Port the lunar pole exploration rover plugins and model to Gazebo Harmonic, keeping Fortress (closes #47)

CMakeLists.txt

@@ -5,76 +5,65 @@ if(DEFINED ENV{ROS_DISTRO})
     set(ROS_DISTRO $ENV{ROS_DISTRO})
 endif()
 
-# Find required packages
 find_package(ament_cmake REQUIRED)
 
+# The plugins are written against the Gazebo Harmonic API and build unchanged
+# against Ignition Fortress through its gz/ redirect headers (see
+# plugins/gz_compat.hh). Only the packages and link targets differ.
 if(ROS_DISTRO STREQUAL "humble")
-  # Find required packages
+  # Ignition Fortress, from the system packages.
   find_package(ignition-plugin1 REQUIRED COMPONENTS register)
   find_package(ignition-gazebo6 REQUIRED)
-  find_package(ignition-physics5 REQUIRED)
-  find_package(ignition-common4 REQUIRED)
+  find_package(ignition-common4 REQUIRED COMPONENTS profiler)
   find_package(ignition-rendering6 REQUIRED)
+  find_package(ignition-sensors6 REQUIRED)
+
+  set(SIM_PLUGIN_LIB ignition-plugin${ignition-plugin1_VERSION_MAJOR}::ignition-plugin${ignition-plugin1_VERSION_MAJOR})
+  set(SIM_SIM_LIB ignition-gazebo${ignition-gazebo6_VERSION_MAJOR}::core)
+  set(SIM_COMMON_LIB ignition-common${ignition-common4_VERSION_MAJOR}::ignition-common${ignition-common4_VERSION_MAJOR})
+  set(SIM_PROFILER_LIB ignition-common${ignition-common4_VERSION_MAJOR}::profiler)
+  set(SIM_RENDERING_LIB ignition-rendering${ignition-rendering6_VERSION_MAJOR}::ignition-rendering${ignition-rendering6_VERSION_MAJOR})
+  set(SIM_SENSORS_LIB ignition-sensors${ignition-sensors6_VERSION_MAJOR}::ignition-sensors${ignition-sensors6_VERSION_MAJOR})
+else()
+  # Gazebo Harmonic, through the ROS vendor packages so the plugins link
+  # against the same libraries as ros_gz and gz_ros2_control.
+  find_package(gz_sim_vendor REQUIRED)
+  find_package(gz-sim REQUIRED)
+  find_package(gz_plugin_vendor REQUIRED)
+  find_package(gz-plugin REQUIRED COMPONENTS register)
+  find_package(gz_common_vendor REQUIRED)
+  find_package(gz-common REQUIRED COMPONENTS profiler)
+  find_package(gz_rendering_vendor REQUIRED)
+  find_package(gz-rendering REQUIRED)
+  find_package(gz_sensors_vendor REQUIRED)
+  find_package(gz-sensors REQUIRED)
+
+  set(SIM_PLUGIN_LIB gz-plugin${gz-plugin_VERSION_MAJOR}::gz-plugin${gz-plugin_VERSION_MAJOR})
+  set(SIM_SIM_LIB gz-sim${gz-sim_VERSION_MAJOR}::gz-sim${gz-sim_VERSION_MAJOR})
+  set(SIM_COMMON_LIB gz-common${gz-common_VERSION_MAJOR}::gz-common${gz-common_VERSION_MAJOR})
+  set(SIM_PROFILER_LIB gz-common${gz-common_VERSION_MAJOR}::profiler)
+  set(SIM_RENDERING_LIB gz-rendering${gz-rendering_VERSION_MAJOR}::gz-rendering${gz-rendering_VERSION_MAJOR})
+  set(SIM_SENSORS_LIB gz-sensors${gz-sensors_VERSION_MAJOR}::gz-sensors${gz-sensors_VERSION_MAJOR})
+endif()
 
-  # Set Gazebo plugin and sim versions
-  set(IGNITION_PLUGIN_VER ${ignition-plugin1_VERSION_MAJOR})
-  set(IGNITION_SIM_VER ${ignition-gazebo6_VERSION_MAJOR})
-  set(IGNITION_COMMON_VER ${ignition-common4_VERSION_MAJOR})
-  set(IGNITION_RENDERING_VER ${ignition-rendering6_VERSION_MAJOR})
-  set(IGNITION_SENSORS_VER ${ignition-sensors6_VERSION_MAJOR})
-
-  # Add the plugins directory to the include path
-  include_directories(${CMAKE_CURRENT_SOURCE_DIR}/Plugins)
-
-  # Add the SolarPanelPlugin library
-  add_library(SolarPanelPlugin SHARED
-    plugins/SolarPanelPlugin.cc
-  )
-
-  # Link the SolarPanelPlugin library with required dependencies
-  target_link_libraries(SolarPanelPlugin
-    PRIVATE ignition-plugin${IGNITION_PLUGIN_VER}::ignition-plugin${IGNITION_PLUGIN_VER}
-    PRIVATE ignition-common${IGNITION_COMMON_VER}::ignition-common${IGNITION_COMMON_VER}
-    PRIVATE ignition-rendering${IGNITION_RENDERING_VER}::ignition-rendering${IGNITION_RENDERING_VER}
-    PRIVATE ignition-gazebo${IGNITION_SIM_VER}::core
-  )
-
-  # Add the RadioisotopeThermalGeneratorPlugin library
-  add_library(RadioisotopeThermalGeneratorPlugin SHARED
-    plugins/RadioisotopeThermalGeneratorPlugin.cc
-  )
-
-  # Link the RadioisotopeThermalGeneratorPlugin library with required dependencies
-  target_link_libraries(RadioisotopeThermalGeneratorPlugin
-    PRIVATE ignition-plugin${IGNITION_PLUGIN_VER}::ignition-plugin${IGNITION_PLUGIN_VER}
-    PRIVATE ignition-gazebo${IGNITION_SIM_VER}::core
-  )
-
-  # Add the RechargeableBatteryPlugin library
-  add_library(RechargeableBatteryPlugin SHARED
-    plugins/RechargeableBatteryPlugin.cc
-  )
+add_library(SolarPanelPlugin SHARED plugins/SolarPanelPlugin.cc)
+target_link_libraries(SolarPanelPlugin PRIVATE
+  ${SIM_SIM_LIB} ${SIM_PLUGIN_LIB} ${SIM_COMMON_LIB} ${SIM_RENDERING_LIB})
 
-  # Link the RechargeableBatteryPlugin library with required dependencies
-  target_link_libraries(RechargeableBatteryPlugin
-    PRIVATE ignition-plugin${IGNITION_PLUGIN_VER}::ignition-plugin${IGNITION_PLUGIN_VER}
-    PRIVATE ignition-gazebo${IGNITION_SIM_VER}::core
-  )
+add_library(RadioisotopeThermalGeneratorPlugin SHARED plugins/RadioisotopeThermalGeneratorPlugin.cc)
+target_link_libraries(RadioisotopeThermalGeneratorPlugin PRIVATE
+  ${SIM_SIM_LIB} ${SIM_PLUGIN_LIB} ${SIM_PROFILER_LIB})
 
-  # add the SensorPowerSystemPlugin library
-  add_library(SensorPowerSystemPlugin SHARED
-    plugins/SensorPowerSystemPlugin.cc
-  )
+add_library(RechargeableBatteryPlugin SHARED plugins/RechargeableBatteryPlugin.cc)
+target_link_libraries(RechargeableBatteryPlugin PRIVATE
+  ${SIM_SIM_LIB} ${SIM_PLUGIN_LIB} ${SIM_COMMON_LIB} ${SIM_PROFILER_LIB})
 
-  # Link the SensorPowerSystemPlugin library with required dependencies
-  target_link_libraries(SensorPowerSystemPlugin
-    PRIVATE ignition-plugin${IGNITION_PLUGIN_VER}::ignition-plugin${IGNITION_PLUGIN_VER}
-    PRIVATE ignition-gazebo${IGNITION_SIM_VER}::core
-    PRIVATE ignition-gazebo${IGNITION_SIM_VER}::ignition-gazebo${IGNITION_SIM_VER}
-    PRIVATE ignition-sensors${IGNITION_SENSORS_VER}::ignition-sensors${IGNITION_SENSORS_VER}
-  )
+add_library(SensorPowerSystemPlugin SHARED plugins/SensorPowerSystemPlugin.cc)
+target_link_libraries(SensorPowerSystemPlugin PRIVATE
+  ${SIM_SIM_LIB} ${SIM_PLUGIN_LIB} ${SIM_SENSORS_LIB})
 
-endif()
+set_property(TARGET SolarPanelPlugin RadioisotopeThermalGeneratorPlugin
+  RechargeableBatteryPlugin SensorPowerSystemPlugin PROPERTY CXX_STANDARD 17)
 
 # Create the models directory
 file(MAKE_DIRECTORY ${CMAKE_CURRENT_BINARY_DIR}/models/curiosity_path)
@@ -86,14 +75,12 @@ install(DIRECTORY
   DESTINATION share/${PROJECT_NAME}/
 )
 
-if(ROS_DISTRO STREQUAL "humble")
-  # Install the plugin library
-  install(TARGETS SolarPanelPlugin RadioisotopeThermalGeneratorPlugin RechargeableBatteryPlugin SensorPowerSystemPlugin
-    ARCHIVE DESTINATION lib
-    LIBRARY DESTINATION lib
-    RUNTIME DESTINATION bin
-  )
-endif()
+install(TARGETS SolarPanelPlugin RadioisotopeThermalGeneratorPlugin
+  RechargeableBatteryPlugin SensorPowerSystemPlugin
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+  RUNTIME DESTINATION bin
+)
 
 ament_environment_hooks("${CMAKE_CURRENT_SOURCE_DIR}/hooks/${PROJECT_NAME}.dsv.in")
 

hooks/simulation.dsv.in

@@ -1,2 +1,6 @@
 prepend-non-duplicate;IGN_GAZEBO_RESOURCE_PATH;share
 prepend-non-duplicate;IGN_GAZEBO_RESOURCE_PATH;share/simulation/models
+prepend-non-duplicate;IGN_GAZEBO_SYSTEM_PLUGIN_PATH;lib
+prepend-non-duplicate;GZ_SIM_RESOURCE_PATH;share
+prepend-non-duplicate;GZ_SIM_RESOURCE_PATH;share/simulation/models
+prepend-non-duplicate;GZ_SIM_SYSTEM_PLUGIN_PATH;lib

models/lunar_pole_exploration_rover/config/lunar_pole_exploration_rover_control.yaml

@@ -0,0 +1,51 @@
+controller_manager:
+  ros__parameters:
+    update_rate: 100
+
+    mast_camera_joint_trajectory_controller:
+      type: joint_trajectory_controller/JointTrajectoryController
+
+    wheel_velocity_controller:
+      type: velocity_controllers/JointGroupVelocityController
+
+    steer_position_controller:
+      type: joint_trajectory_controller/JointTrajectoryController
+
+    joint_state_broadcaster:
+      type: joint_state_broadcaster/JointStateBroadcaster
+
+
+mast_camera_joint_trajectory_controller:
+  ros__parameters:
+    joints:
+      - mast_head_pivot_joint
+      - mast_camera_joint
+    interface_name: position
+    command_interfaces:
+      - position
+    state_interfaces:
+      - position
+      - velocity
+
+wheel_velocity_controller:
+  ros__parameters:
+    joints:
+      - front_left_wheel_joint
+      - rear_left_wheel_joint
+      - front_right_wheel_joint
+      - rear_right_wheel_joint
+    interface_name: velocity
+
+steer_position_controller:
+  ros__parameters:
+    joints:
+      - front_left_wheel_axle_joint
+      - rear_left_wheel_axle_joint
+      - front_right_wheel_axle_joint
+      - rear_right_wheel_axle_joint
+    interface_name: position
+    command_interfaces:
+      - position
+    state_interfaces:
+      - position
+      - velocity

models/lunar_pole_exploration_rover/urdf/lunar_pole_exploration_rover.gazebo

@@ -1,9 +1,19 @@
 <?xml version="1.0"?>
 <robot xmlns:xacro="http://wiki.ros.org/xacro">
 
-    <ros2_control name="IgnitionSystem" type="system">
+    <!-- ROS 2 Humble runs Ignition Fortress, later distributions Gazebo Harmonic;
+         the plugin and system names differ. -->
+    <xacro:property name="humble" value="${'$(optenv ROS_DISTRO jazzy)' == 'humble'}" />
+    <xacro:property name="hardware_name" value="${'IgnitionSystem' if humble else 'GazeboSystem'}" />
+    <xacro:property name="hardware_plugin" value="${'ign_ros2_control/IgnitionSystem' if humble else 'gz_ros2_control/GazeboSimSystem'}" />
+    <xacro:property name="ros2_control_system_file" value="${'libign_ros2_control-system' if humble else 'gz_ros2_control-system'}" />
+    <xacro:property name="ros2_control_system_name" value="${'ign_ros2_control::IgnitionROS2ControlPlugin' if humble else 'gz_ros2_control::GazeboSimROS2ControlPlugin'}" />
+    <xacro:property name="system_prefix" value="${'ignition-gazebo' if humble else 'gz-sim'}" />
+    <xacro:property name="system_namespace" value="${'ignition::gazebo' if humble else 'gz::sim'}" />
+
+    <ros2_control name="${hardware_name}" type="system">
         <hardware>
-            <plugin>ign_ros2_control/IgnitionSystem</plugin>
+            <plugin>${hardware_plugin}</plugin>
         </hardware>
 
          <!-- Mast Joints Interfaces-->
@@ -74,19 +84,19 @@
     </ros2_control>
 
     <gazebo>
-        <plugin filename="libign_ros2_control-system" name="ign_ros2_control::IgnitionROS2ControlPlugin">
+        <plugin filename="${ros2_control_system_file}" name="${ros2_control_system_name}">
             <robot_param>robot_description</robot_param>
             <robot_param_node>robot_state_publisher</robot_param_node>
-            <parameters>$(find lunar_pole_exploration_rover)/config/lunar_pole_exploration_rover_control.yaml</parameters>
+            <parameters>$(find simulation)/models/lunar_pole_exploration_rover/config/lunar_pole_exploration_rover_control.yaml</parameters>
         </plugin>
 
-        <plugin filename="ignition-gazebo-odometry-publisher-system" name="ignition::gazebo::systems::OdometryPublisher">
+        <plugin filename="${system_prefix}-odometry-publisher-system" name="${system_namespace}::systems::OdometryPublisher">
             <odom_frame>/odom</odom_frame>
             <robot_base_frame>/base_footprint</robot_base_frame>
             <odom_publish_frequency>10</odom_publish_frequency>
         </plugin>
 
-        <plugin filename="ignition-gazebo-sensors-system" name="ignition::gazebo::systems::Sensors">
+        <plugin filename="${system_prefix}-sensors-system" name="${system_namespace}::systems::Sensors">
             <render_engine>ogre2</render_engine>
             <background_color>0.0 0.0 0.0 1</background_color>
             <disable_on_drained_battery>true</disable_on_drained_battery>

package.xml

@@ -10,13 +10,21 @@
   <buildtool_depend>ros_environment</buildtool_depend>
   <buildtool_depend>ament_cmake</buildtool_depend>
 
+  <!-- ROS 2 Humble: Ignition Fortress from the system packages -->
   <depend condition="$ROS_DISTRO == 'humble'">ign-cmake2</depend>
   <depend condition="$ROS_DISTRO == 'humble'">ign-plugin1</depend>
   <depend condition="$ROS_DISTRO == 'humble'">ign-common4</depend>
   <depend condition="$ROS_DISTRO == 'humble'">ign-gazebo6</depend>
   <depend condition="$ROS_DISTRO == 'humble'">ign-rendering6</depend>
   <depend condition="$ROS_DISTRO == 'humble'">ign-sensors6</depend>
 
+  <!-- ROS 2 Jazzy and later: Gazebo Harmonic through the ROS vendor packages -->
+  <depend condition="$ROS_DISTRO != 'humble'">gz_sim_vendor</depend>
+  <depend condition="$ROS_DISTRO != 'humble'">gz_plugin_vendor</depend>
+  <depend condition="$ROS_DISTRO != 'humble'">gz_common_vendor</depend>
+  <depend condition="$ROS_DISTRO != 'humble'">gz_rendering_vendor</depend>
+  <depend condition="$ROS_DISTRO != 'humble'">gz_sensors_vendor</depend>
+
   <exec_depend>xacro</exec_depend>
 
   <export>

plugins/RadioisotopeThermalGeneratorPlugin.cc

@@ -16,11 +16,13 @@
  */
 
 #include "RadioisotopeThermalGeneratorPlugin.hh"
+#include "gz_compat.hh"
 
-#include <ignition/transport/Node.hh>
-#include "ignition/gazebo/Model.hh"
+#include <gz/transport/Node.hh>
+#include "gz/sim/Model.hh"
 
-#include <ignition/msgs/double.pb.h>
+#include <gz/msgs/double.pb.h>
+#include <gz/msgs/float.pb.h>
 
 #include <gz/common/Profiler.hh>
 
@@ -49,11 +51,11 @@ class simulation::RadioisotopeThermalGeneratorPluginPrivate
 
   /// \brief Ignition communication node
 public:
-  ignition::transport::Node node;
+  gz::transport::Node node;
 
   /// \brief Publisher for the radioisotope thermal generator output
 public:
-  ignition::transport::Node::Publisher nominalPowerPub;
+  gz::transport::Node::Publisher nominalPowerPub;
 };
 
 //////////////////////////////////////////////////
@@ -66,16 +68,16 @@ RadioisotopeThermalGeneratorPlugin::RadioisotopeThermalGeneratorPlugin()
 RadioisotopeThermalGeneratorPlugin::~RadioisotopeThermalGeneratorPlugin() = default;
 
 //////////////////////////////////////////////////
-void RadioisotopeThermalGeneratorPlugin::Configure(const ignition::gazebo::Entity &_entity,
+void RadioisotopeThermalGeneratorPlugin::Configure(const gz::sim::Entity &_entity,
                                                    const std::shared_ptr<const sdf::Element> &_sdf,
-                                                   ignition::gazebo::EntityComponentManager &_ecm,
-                                                   ignition::gazebo::EventManager &_eventMgr)
+                                                   gz::sim::EntityComponentManager &_ecm,
+                                                   gz::sim::EventManager &_eventMgr)
 {
   // Store the pointer to the model the RTG is under
-  auto model = ignition::gazebo::Model(_entity);
+  auto model = gz::sim::Model(_entity);
   if (!model.Valid(_ecm))
   {
-    ignerr << "Radioisotope Thermal Generator plugin should be attached to a model entity. "
+    gzerr << "Radioisotope Thermal Generator plugin should be attached to a model entity. "
            << "Failed to initialize." << std::endl;
     return;
   }
@@ -87,18 +89,18 @@ void RadioisotopeThermalGeneratorPlugin::Configure(const ignition::gazebo::Entit
   {
     this->dataPtr->linkName = _sdf->Get<std::string>("link_name");
     this->dataPtr->topicName = "/model/" + this->dataPtr->modelName + "/" + this->dataPtr->linkName + "/radioisotope_thermal_generator_output";
-    auto validTopic = ignition::transport::TopicUtils::AsValidTopic(this->dataPtr->topicName);
+    auto validTopic = gz::transport::TopicUtils::AsValidTopic(this->dataPtr->topicName);
     if (validTopic.empty())
     {
-      ignerr << "Failed to create valid topic [" << this->dataPtr->topicName << "]" << std::endl;
+      gzerr << "Failed to create valid topic [" << this->dataPtr->topicName << "]" << std::endl;
       return;
     }
     // Advertise topic where data will be published
-    this->dataPtr->nominalPowerPub = this->dataPtr->node.Advertise<ignition::msgs::Float>(validTopic);
+    this->dataPtr->nominalPowerPub = this->dataPtr->node.Advertise<gz::msgs::Float>(validTopic);
   }
   else
   {
-    ignerr << "Radioisotope Thermal Generator plugin should have a <link_name> element. "
+    gzerr << "Radioisotope Thermal Generator plugin should have a <link_name> element. "
            << "Failed to initialize." << std::endl;
     return;
   }
@@ -109,30 +111,30 @@ void RadioisotopeThermalGeneratorPlugin::Configure(const ignition::gazebo::Entit
   }
   else
   {
-    ignerr << "Radioisotope Thermal Generator plugin should have a <nominal_power> element. "
+    gzerr << "Radioisotope Thermal Generator plugin should have a <nominal_power> element. "
            << "Failed to initialize." << std::endl;
     return;
   }
 }
 
 //////////////////////////////////////////////////
-void RadioisotopeThermalGeneratorPlugin::PostUpdate(const ignition::gazebo::UpdateInfo &_info,
-                                                    const ignition::gazebo::EntityComponentManager &_ecm)
+void RadioisotopeThermalGeneratorPlugin::PostUpdate(const gz::sim::UpdateInfo &_info,
+                                                    const gz::sim::EntityComponentManager &_ecm)
 {
-  IGN_PROFILE("RadioisotopeThermalGeneratorPlugin::PostUpdate");
+  GZ_PROFILE("RadioisotopeThermalGeneratorPlugin::PostUpdate");
   if (_info.paused)
   {
     return;
   }
   // Publish result
-  ignition::msgs::Float msg;
+  gz::msgs::Float msg;
   msg.set_data(this->dataPtr->nominalPower);
   this->dataPtr->nominalPowerPub.Publish(msg);
-  igndbg << "Published RTG output: " << this->dataPtr->nominalPower << std::endl;
+  gzdbg << "Published RTG output: " << this->dataPtr->nominalPower << std::endl;
 }
 
-IGNITION_ADD_PLUGIN(RadioisotopeThermalGeneratorPlugin, ignition::gazebo::System,
+GZ_ADD_PLUGIN(RadioisotopeThermalGeneratorPlugin, gz::sim::System,
                     RadioisotopeThermalGeneratorPlugin::ISystemConfigure,
                     RadioisotopeThermalGeneratorPlugin::ISystemPostUpdate)
 
-IGNITION_ADD_PLUGIN_ALIAS(RadioisotopeThermalGeneratorPlugin, "simulation::RadioisotopeThermalGeneratorPlugin")
+GZ_ADD_PLUGIN_ALIAS(RadioisotopeThermalGeneratorPlugin, "simulation::RadioisotopeThermalGeneratorPlugin")