From 055b4eeba7c39243fd6eccdd6489d109b21f1aea Mon Sep 17 00:00:00 2001 From: Alexey Simonov Date: Wed, 2 Sep 2026 10:36:38 +0400 Subject: [PATCH 1/4] Build the Gazebo plugins for Harmonic as well as Fortress (#36). SolarPanelPlugin, RechargeableBatteryPlugin, SensorPowerSystemPlugin and RadioisotopeThermalGeneratorPlugin were written against the Ignition Fortress API and, since #41, only built on Humble. They now target the Gazebo Harmonic (gz-sim 8) API: gz::sim, gz::rendering and gz::sensors namespaces, gz/ header paths, the GZ_ADD_PLUGIN registration macros and the gz* console macros. Each message type gets its own include, as gz-msgs 10 no longer provides them transitively. Fortress ships gz/ redirect headers for all of these namespaces but not for the GZ_ / gz prefixed macros, so plugins/gz_compat.hh maps those onto their Ignition names when they are missing. One set of sources therefore builds on both distributions; CMakeLists.txt and package.xml select the Ignition system packages on Humble and the ROS gz_*_vendor packages otherwise, and the environment hook sets both IGN_GAZEBO_* and GZ_SIM_* paths. --- CMakeLists.txt | 121 +++++------ hooks/simulation.dsv.in | 4 + package.xml | 8 + plugins/RadioisotopeThermalGeneratorPlugin.cc | 46 ++-- plugins/RadioisotopeThermalGeneratorPlugin.hh | 18 +- plugins/RechargeableBatteryPlugin.cc | 205 +++++++++--------- plugins/RechargeableBatteryPlugin.hh | 34 +-- plugins/SensorPowerSystemPlugin.cc | 135 ++++++------ plugins/SensorPowerSystemPlugin.hh | 24 +- plugins/SolarPanelPlugin.cc | 165 +++++++------- plugins/SolarPanelPlugin.hh | 22 +- plugins/gz_compat.hh | 62 ++++++ 12 files changed, 455 insertions(+), 389 deletions(-) create mode 100644 plugins/gz_compat.hh diff --git a/CMakeLists.txt b/CMakeLists.txt index df63da3..328bcf9 100644 --- a/CMakeLists.txt +++ b/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") diff --git a/hooks/simulation.dsv.in b/hooks/simulation.dsv.in index ccc31f6..7e75fae 100644 --- a/hooks/simulation.dsv.in +++ b/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 diff --git a/package.xml b/package.xml index ea52ba9..27ef1e8 100644 --- a/package.xml +++ b/package.xml @@ -10,6 +10,7 @@ ros_environment ament_cmake + ign-cmake2 ign-plugin1 ign-common4 @@ -17,6 +18,13 @@ ign-rendering6 ign-sensors6 + + gz_sim_vendor + gz_plugin_vendor + gz_common_vendor + gz_rendering_vendor + gz_sensors_vendor + xacro diff --git a/plugins/RadioisotopeThermalGeneratorPlugin.cc b/plugins/RadioisotopeThermalGeneratorPlugin.cc index 4e7d056..e23c303 100644 --- a/plugins/RadioisotopeThermalGeneratorPlugin.cc +++ b/plugins/RadioisotopeThermalGeneratorPlugin.cc @@ -16,11 +16,13 @@ */ #include "RadioisotopeThermalGeneratorPlugin.hh" +#include "gz_compat.hh" -#include -#include "ignition/gazebo/Model.hh" +#include +#include "gz/sim/Model.hh" -#include +#include +#include #include @@ -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 &_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("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(validTopic); + this->dataPtr->nominalPowerPub = this->dataPtr->node.Advertise(validTopic); } else { - ignerr << "Radioisotope Thermal Generator plugin should have a element. " + gzerr << "Radioisotope Thermal Generator plugin should have a 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 element. " + gzerr << "Radioisotope Thermal Generator plugin should have a 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") diff --git a/plugins/RadioisotopeThermalGeneratorPlugin.hh b/plugins/RadioisotopeThermalGeneratorPlugin.hh index dd95777..1e6ccec 100644 --- a/plugins/RadioisotopeThermalGeneratorPlugin.hh +++ b/plugins/RadioisotopeThermalGeneratorPlugin.hh @@ -19,7 +19,7 @@ #define Radioisotope_Thermal_Generator_Plugin_HH_ #include -#include +#include namespace simulation { @@ -34,9 +34,9 @@ namespace simulation /// /// - `link_name`: The name of the link where the radioisotope thermal generator is attached. /// - `nominal_power`: The nominal power output of the radioisotope thermal generator. - class RadioisotopeThermalGeneratorPlugin : public ignition::gazebo::System, - public ignition::gazebo::ISystemConfigure, - public ignition::gazebo::ISystemPostUpdate + class RadioisotopeThermalGeneratorPlugin : public gz::sim::System, + public gz::sim::ISystemConfigure, + public gz::sim::ISystemPostUpdate { /// \brief Constructor @@ -49,15 +49,15 @@ namespace simulation /// Documentation inherited public: - void Configure(const ignition::gazebo::Entity &_entity, + void Configure(const gz::sim::Entity &_entity, const std::shared_ptr &_sdf, - ignition::gazebo::EntityComponentManager &_ecm, - ignition::gazebo::EventManager &_eventMgr) override; + gz::sim::EntityComponentManager &_ecm, + gz::sim::EventManager &_eventMgr) override; /// Documentation inherited public: - void PostUpdate(const ignition::gazebo::UpdateInfo &_info, - const ignition::gazebo::EntityComponentManager &_ecm) final; + void PostUpdate(const gz::sim::UpdateInfo &_info, + const gz::sim::EntityComponentManager &_ecm) final; /// \brief Private data pointer private: diff --git a/plugins/RechargeableBatteryPlugin.cc b/plugins/RechargeableBatteryPlugin.cc index 92b03e8..90ff92d 100644 --- a/plugins/RechargeableBatteryPlugin.cc +++ b/plugins/RechargeableBatteryPlugin.cc @@ -16,20 +16,23 @@ */ #include "RechargeableBatteryPlugin.hh" - -#include -#include -#include "ignition/gazebo/components/BatterySoC.hh" -#include -#include -#include +#include "gz_compat.hh" + +#include +#include +#include "gz/sim/components/BatterySoC.hh" +#include +#include +#include #include #include #include -#include -#include +#include +#include +#include +#include #include "gz/sim/Model.hh" -#include +#include using namespace simulation; @@ -70,7 +73,7 @@ class simulation::RechargeableBatteryPluginPrivate public: void OnBatteryDrainingMsg( const char *_data, const size_t _size, - const ignition::transport::MessageInfo &_info); + const gz::transport::MessageInfo &_info); /// \brief Callback connected to additional topics that can stop battery /// draining. @@ -80,30 +83,30 @@ class simulation::RechargeableBatteryPluginPrivate public: void OnBatteryStopDrainingMsg( const char *_data, const size_t _size, - const ignition::transport::MessageInfo &_info); + const gz::transport::MessageInfo &_info); /// \brief Callback connected to power source topics. /// \param[in] _id The id of the power source. /// \param[in] _msg The message containing the power source power. public: void OnPowerSourceMsg(int _id, - const ignition::msgs::Float &_msg); + const gz::msgs::Float &_msg); /// \brief Ignition communication node public: - ignition::transport::Node node; + gz::transport::Node node; /// \brief Battery state of charge message publisher public: - ignition::transport::Node::Publisher statePub; + gz::transport::Node::Publisher statePub; /// \brief Total power supply publisher public: - ignition::transport::Node::Publisher totalPowerSupplyPub; + gz::transport::Node::Publisher totalPowerSupplyPub; /// \brief Total power consumption publisher public: - ignition::transport::Node::Publisher totalPowerConsumptionPub; + gz::transport::Node::Publisher totalPowerConsumptionPub; /// \brief Battery consumer identifier. /// Current implementation limits one consumer (Model) per battery. @@ -124,7 +127,7 @@ class simulation::RechargeableBatteryPluginPrivate /// \brief Battery entity public: - ignition::gazebo::Entity batteryEntity{ignition::gazebo::kNullEntity}; + gz::sim::Entity batteryEntity{gz::sim::kNullEntity}; /// \brief modelName public: @@ -132,7 +135,7 @@ class simulation::RechargeableBatteryPluginPrivate /// \brief Model entity public: - gz::sim::Model model{ignition::gazebo::kNullEntity}; + gz::sim::Model model{gz::sim::kNullEntity}; /// \brief Open-circuit voltage. /// E(t) = e0 + e1 * Q(t) / c @@ -235,16 +238,16 @@ RechargeableBatteryPlugin::~RechargeableBatteryPlugin() } ///////////////////////////////////////////////// -void RechargeableBatteryPlugin::Configure(const ignition::gazebo::Entity &_entity, +void RechargeableBatteryPlugin::Configure(const gz::sim::Entity &_entity, const std::shared_ptr &_sdf, - ignition::gazebo::EntityComponentManager &_ecm, - ignition::gazebo::EventManager &_eventMgr) + gz::sim::EntityComponentManager &_ecm, + gz::sim::EventManager &_eventMgr) { // Store the pointer to the model this battery is under - auto model = ignition::gazebo::Model(_entity); + auto model = gz::sim::Model(_entity); if (!model.Valid(_ecm)) { - ignerr << "Battery plugin should be attached to a model entity. " + gzerr << "Battery plugin should be attached to a model entity. " << "Failed to initialize." << std::endl; return; } @@ -262,7 +265,7 @@ void RechargeableBatteryPlugin::Configure(const ignition::gazebo::Entity &_entit if (this->dataPtr->c <= 0) { - ignerr << "No or incorrect value specified. Capacity should be " + gzerr << "No or incorrect value specified. Capacity should be " << "greater than 0.\n"; return; } @@ -273,11 +276,11 @@ void RechargeableBatteryPlugin::Configure(const ignition::gazebo::Entity &_entit this->dataPtr->q0 = _sdf->Get("initial_charge"); if (this->dataPtr->q0 > this->dataPtr->c || this->dataPtr->q0 < 0) { - ignerr << " value should be between [0, ]." + gzerr << " value should be between [0, ]." << std::endl; this->dataPtr->q0 = std::max(0.0, std::min(this->dataPtr->q0, this->dataPtr->c)); - ignerr << "Setting to [" << this->dataPtr->q0 + gzerr << "Setting to [" << this->dataPtr->q0 << "] instead." << std::endl; } } @@ -292,7 +295,7 @@ void RechargeableBatteryPlugin::Configure(const ignition::gazebo::Entity &_entit this->dataPtr->tau = _sdf->Get("smooth_current_tau"); if (this->dataPtr->tau <= 0) { - ignerr << " value should be positive. " + gzerr << " value should be positive. " << "Using [1] instead." << std::endl; this->dataPtr->tau = 1; } @@ -302,28 +305,28 @@ void RechargeableBatteryPlugin::Configure(const ignition::gazebo::Entity &_entit { this->dataPtr->batteryName = _sdf->Get("battery_name"); auto initVoltage = _sdf->Get("voltage"); - igndbg << "Battery name: " << this->dataPtr->batteryName << std::endl; - igndbg << "Initial voltage: " << initVoltage << std::endl; + gzdbg << "Battery name: " << this->dataPtr->batteryName << std::endl; + gzdbg << "Initial voltage: " << initVoltage << std::endl; // Create battery entity and some components this->dataPtr->batteryEntity = _ecm.CreateEntity(); - _ecm.CreateComponent(this->dataPtr->batteryEntity, ignition::gazebo::components::Name( + _ecm.CreateComponent(this->dataPtr->batteryEntity, gz::sim::components::Name( this->dataPtr->batteryName)); _ecm.SetParentEntity(this->dataPtr->batteryEntity, _entity); // Create actual battery and assign update function - this->dataPtr->battery = std::make_shared( + this->dataPtr->battery = std::make_shared( this->dataPtr->batteryName, initVoltage); this->dataPtr->battery->Init(); // print battery voltage - igndbg << "Battery voltage: " << this->dataPtr->battery->Voltage() << std::endl; + gzdbg << "Battery voltage: " << this->dataPtr->battery->Voltage() << std::endl; this->dataPtr->battery->SetUpdateFunc( std::bind(&RechargeableBatteryPlugin::OnUpdateVoltage, this, std::placeholders::_1)); } else { - ignerr << "No or specified. Both are required.\n"; + gzerr << "No or specified. Both are required.\n"; return; } @@ -335,11 +338,11 @@ void RechargeableBatteryPlugin::Configure(const ignition::gazebo::Entity &_entit bool success = this->dataPtr->battery->SetPowerLoad( this->dataPtr->consumerId, this->dataPtr->initialPowerLoad); if (!success) - ignerr << "Failed to set consumer power load." << std::endl; + gzerr << "Failed to set consumer power load." << std::endl; } else { - ignwarn << "Required attribute power_load missing " + gzwarn << "Required attribute power_load missing " << "in BatteryPlugin SDF" << std::endl; } if (_sdf->HasElement("start_draining")) @@ -356,7 +359,7 @@ void RechargeableBatteryPlugin::Configure(const ignition::gazebo::Entity &_entit std::bind(&RechargeableBatteryPluginPrivate::OnBatteryDrainingMsg, this->dataPtr.get(), std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); - ignmsg << "RechargeableBatteryPlugin subscribes to power draining topic [" + gzmsg << "RechargeableBatteryPlugin subscribes to power draining topic [" << topic << "]." << std::endl; sdfElem = sdfElem->GetNextElement("power_draining_topic"); } @@ -374,7 +377,7 @@ void RechargeableBatteryPlugin::Configure(const ignition::gazebo::Entity &_entit std::bind(&RechargeableBatteryPluginPrivate::OnBatteryStopDrainingMsg, this->dataPtr.get(), std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); - ignmsg << "RechargeableBatteryPlugin subscribes to stop power draining topic [" + gzmsg << "RechargeableBatteryPlugin subscribes to stop power draining topic [" << topic << "]." << std::endl; sdfElem = sdfElem->GetNextElement("power_draining_topic"); } @@ -390,10 +393,10 @@ void RechargeableBatteryPlugin::Configure(const ignition::gazebo::Entity &_entit const auto &topic = powerSourceElem->Get(); std::string stateTopic{"/model/" + this->dataPtr->model.Name(_ecm) + "/" + topic}; - auto validPowerSourceTopic = ignition::transport::TopicUtils::AsValidTopic(stateTopic); + auto validPowerSourceTopic = gz::transport::TopicUtils::AsValidTopic(stateTopic); if (validPowerSourceTopic.empty()) { - ignerr << "Failed to create valid topic. Not valid: [" + gzerr << "Failed to create valid topic. Not valid: [" << topic << "]" << std::endl; return; } @@ -401,10 +404,10 @@ void RechargeableBatteryPlugin::Configure(const ignition::gazebo::Entity &_entit powerSource.id = id; powerSource.nominalPower = 0.0; powerSource.dataUpdated = false; - std::function callback = std::bind(&RechargeableBatteryPluginPrivate::OnPowerSourceMsg, + std::function callback = std::bind(&RechargeableBatteryPluginPrivate::OnPowerSourceMsg, this->dataPtr.get(), id, std::placeholders::_1); this->dataPtr->node.Subscribe(validPowerSourceTopic, callback); - ignmsg << "RechargeableBatteryPlugin subscribes to power source topic [" + gzmsg << "RechargeableBatteryPlugin subscribes to power source topic [" << validPowerSourceTopic << "]." << std::endl; this->dataPtr->powerSourcesInfo.emplace_back(std::move(powerSource)); powerSourceElem = powerSourceElem->GetNextElement("power_source"); @@ -413,79 +416,79 @@ void RechargeableBatteryPlugin::Configure(const ignition::gazebo::Entity &_entit } else { - ignerr << "No power source topic specified." << std::endl; + gzerr << "No power source topic specified." << std::endl; } - ignmsg << "RechargeableBatteryPlugin configured. Battery name: " + gzmsg << "RechargeableBatteryPlugin configured. Battery name: " << this->dataPtr->battery->Name() << std::endl; - igndbg << "Battery initial voltage: " << this->dataPtr->battery->InitVoltage() + gzdbg << "Battery initial voltage: " << this->dataPtr->battery->InitVoltage() << std::endl; this->dataPtr->soc = this->dataPtr->q / this->dataPtr->c; // Initialize battery with initial calculated state of charge _ecm.CreateComponent(this->dataPtr->batteryEntity, - ignition::gazebo::components::BatterySoC(this->dataPtr->soc)); + gz::sim::components::BatterySoC(this->dataPtr->soc)); // Setup battery state topic std::string stateTopic{"/model/" + this->dataPtr->model.Name(_ecm) + "/battery/" + this->dataPtr->battery->Name() + "/state"}; - auto validStateTopic = ignition::transport::TopicUtils::AsValidTopic(stateTopic); + auto validStateTopic = gz::transport::TopicUtils::AsValidTopic(stateTopic); if (validStateTopic.empty()) { - ignerr << "Failed to create valid state topic [" + gzerr << "Failed to create valid state topic [" << stateTopic << "]" << std::endl; return; } - ignition::transport::AdvertiseMessageOptions opts; + gz::transport::AdvertiseMessageOptions opts; opts.SetMsgsPerSec(50); - this->dataPtr->statePub = this->dataPtr->node.Advertise( + this->dataPtr->statePub = this->dataPtr->node.Advertise( validStateTopic, opts); // Setup total power supply topic std::string totalPowerSupplyTopic{"/model/" + this->dataPtr->model.Name(_ecm) + "/battery/" + this->dataPtr->battery->Name() + "/total_power_supply"}; - auto validTotalPowerSupplyTopic = ignition::transport::TopicUtils::AsValidTopic(totalPowerSupplyTopic); + auto validTotalPowerSupplyTopic = gz::transport::TopicUtils::AsValidTopic(totalPowerSupplyTopic); if (validTotalPowerSupplyTopic.empty()) { - ignerr << "Failed to create valid total power supply topic [" + gzerr << "Failed to create valid total power supply topic [" << totalPowerSupplyTopic << "]" << std::endl; return; } - this->dataPtr->totalPowerSupplyPub = this->dataPtr->node.Advertise( + this->dataPtr->totalPowerSupplyPub = this->dataPtr->node.Advertise( validTotalPowerSupplyTopic); // Setup total power consumption topic std::string totalPowerConsumptionTopic{"/model/" + this->dataPtr->model.Name(_ecm) + "/battery/" + this->dataPtr->battery->Name() + "/total_power_consumption"}; - auto validTotalPowerConsumptionTopic = ignition::transport::TopicUtils::AsValidTopic(totalPowerConsumptionTopic); + auto validTotalPowerConsumptionTopic = gz::transport::TopicUtils::AsValidTopic(totalPowerConsumptionTopic); if (validTotalPowerConsumptionTopic.empty()) { - ignerr << "Failed to create valid total power consumption topic [" + gzerr << "Failed to create valid total power consumption topic [" << totalPowerConsumptionTopic << "]" << std::endl; return; } - this->dataPtr->totalPowerConsumptionPub = this->dataPtr->node.Advertise( + this->dataPtr->totalPowerConsumptionPub = this->dataPtr->node.Advertise( validTotalPowerConsumptionTopic); } ///////////////////////////////////////////////// void RechargeableBatteryPlugin::PreUpdate( - const ignition::gazebo::UpdateInfo &_info, - ignition::gazebo::EntityComponentManager &_ecm) + const gz::sim::UpdateInfo &_info, + gz::sim::EntityComponentManager &_ecm) { - IGN_PROFILE("RechargeableBatteryPlugin::PreUpdate"); + GZ_PROFILE("RechargeableBatteryPlugin::PreUpdate"); // Recalculate total power load among all consumers double totalPowerLoad = this->dataPtr->initialPowerLoad; - _ecm.Each( - [&](const ignition::gazebo::Entity & /*_entity*/, - const ignition::gazebo::components::BatteryPowerLoad *_batteryPowerLoadInfo) -> bool + _ecm.Each( + [&](const gz::sim::Entity & /*_entity*/, + const gz::sim::components::BatteryPowerLoad *_batteryPowerLoadInfo) -> bool { if (_batteryPowerLoadInfo->Data().batteryId == this->dataPtr->batteryEntity) @@ -497,26 +500,26 @@ void RechargeableBatteryPlugin::PreUpdate( }); // Publish total power consumption - ignition::msgs::Float totalPowerConsumptionMsg; + gz::msgs::Float totalPowerConsumptionMsg; totalPowerConsumptionMsg.set_data(totalPowerLoad); this->dataPtr->totalPowerConsumptionPub.Publish(totalPowerConsumptionMsg); bool success = this->dataPtr->battery->SetPowerLoad( this->dataPtr->consumerId, totalPowerLoad); if (!success) - ignerr << "Failed to set consumer power load." << std::endl; + gzerr << "Failed to set consumer power load." << std::endl; // start draining the battery if the robot has started moving if (!this->dataPtr->startDraining) { - const std::vector &joints = - _ecm.ChildrenByComponents(this->dataPtr->model.Entity(), ignition::gazebo::components::Joint()); + const std::vector &joints = + _ecm.ChildrenByComponents(this->dataPtr->model.Entity(), gz::sim::components::Joint()); - for (ignition::gazebo::Entity jointEntity : joints) + for (gz::sim::Entity jointEntity : joints) { const auto *jointVelocityCmd = - _ecm.Component(jointEntity); + _ecm.Component(jointEntity); if (jointVelocityCmd) { for (double jointVel : jointVelocityCmd->Data()) @@ -530,7 +533,7 @@ void RechargeableBatteryPlugin::PreUpdate( } const auto *jointForceCmd = - _ecm.Component(jointEntity); + _ecm.Component(jointEntity); if (jointForceCmd) { for (double jointForce : jointForceCmd->Data()) @@ -547,13 +550,13 @@ void RechargeableBatteryPlugin::PreUpdate( } /////////////////////////////////////////////// -void RechargeableBatteryPlugin::Update(const ignition::gazebo::UpdateInfo &_info, - ignition::gazebo::EntityComponentManager &_ecm) +void RechargeableBatteryPlugin::Update(const gz::sim::UpdateInfo &_info, + gz::sim::EntityComponentManager &_ecm) { - IGN_PROFILE("RechargeableBatteryPlugin::Update"); + GZ_PROFILE("RechargeableBatteryPlugin::Update"); if (_info.dt < std::chrono::steady_clock::duration::zero()) { - ignwarn << "Detected jump back in time [" + gzwarn << "Detected jump back in time [" << std::chrono::duration_cast(_info.dt).count() << "s]. System may not work properly." << std::endl; } @@ -575,7 +578,7 @@ void RechargeableBatteryPlugin::Update(const ignition::gazebo::UpdateInfo &_info if (drainTime != this->dataPtr->lastPrintTime) { this->dataPtr->lastPrintTime = drainTime; - igndbg << "[Battery Plugin] Battery drain: " << drainTime << " minutes passed.\n"; + gzdbg << "[Battery Plugin] Battery drain: " << drainTime << " minutes passed.\n"; } // update step size @@ -588,7 +591,7 @@ void RechargeableBatteryPlugin::Update(const ignition::gazebo::UpdateInfo &_info 1e-9; if (this->dataPtr->tau < dt) { - ignerr << " should be in the range [dt, +inf) but is " + gzerr << " should be in the range [dt, +inf) but is " << "configured with [" << this->dataPtr->tau << "]. We'll be using " << "[" << dt << "] instead" << std::endl; this->dataPtr->tau = dt; @@ -598,7 +601,7 @@ void RechargeableBatteryPlugin::Update(const ignition::gazebo::UpdateInfo &_info { // Update battery component this->dataPtr->battery->Update(); - auto *batteryComp = _ecm.Component( + auto *batteryComp = _ecm.Component( this->dataPtr->batteryEntity); batteryComp->Data() = this->dataPtr->StateOfCharge(); @@ -606,17 +609,17 @@ void RechargeableBatteryPlugin::Update(const ignition::gazebo::UpdateInfo &_info } ///////////////////////////////////////////////// -void RechargeableBatteryPlugin::PostUpdate(const ignition::gazebo::UpdateInfo &_info, - const ignition::gazebo::EntityComponentManager &_ecm) +void RechargeableBatteryPlugin::PostUpdate(const gz::sim::UpdateInfo &_info, + const gz::sim::EntityComponentManager &_ecm) { - IGN_PROFILE("RechargeableBatteryPlugin::PostUpdate"); + GZ_PROFILE("RechargeableBatteryPlugin::PostUpdate"); // Nothing left to do if paused or the publisher wasn't created. if (_info.paused || !this->dataPtr->statePub) return; // Publish battery state - ignition::msgs::BatteryState msg; - msg.mutable_header()->mutable_stamp()->CopyFrom(ignition::gazebo::convert(_info.simTime)); + gz::msgs::BatteryState msg; + msg.mutable_header()->mutable_stamp()->CopyFrom(gz::sim::convert(_info.simTime)); msg.set_voltage(this->dataPtr->battery->Voltage()); msg.set_current(this->dataPtr->ismooth); msg.set_charge(this->dataPtr->q); @@ -624,21 +627,21 @@ void RechargeableBatteryPlugin::PostUpdate(const ignition::gazebo::UpdateInfo &_ msg.set_percentage(this->dataPtr->soc * 100); if (this->dataPtr->isCharging) - msg.set_power_supply_status(ignition::msgs::BatteryState::CHARGING); + msg.set_power_supply_status(gz::msgs::BatteryState::CHARGING); else if (this->dataPtr->startDraining) - msg.set_power_supply_status(ignition::msgs::BatteryState::DISCHARGING); + msg.set_power_supply_status(gz::msgs::BatteryState::DISCHARGING); else if (!this->dataPtr->StateOfCharge() > 0.9) - msg.set_power_supply_status(ignition::msgs::BatteryState::FULL); + msg.set_power_supply_status(gz::msgs::BatteryState::FULL); else - msg.set_power_supply_status(ignition::msgs::BatteryState::NOT_CHARGING); + msg.set_power_supply_status(gz::msgs::BatteryState::NOT_CHARGING); this->dataPtr->statePub.Publish(msg); } ///////////////////////////////////////////////// -double RechargeableBatteryPlugin::OnUpdateVoltage(const ignition::common::Battery *_battery) +double RechargeableBatteryPlugin::OnUpdateVoltage(const gz::common::Battery *_battery) { - IGN_ASSERT(_battery != nullptr, "Battery pointer is null"); + GZ_ASSERT(_battery != nullptr, "Battery pointer is null"); if (fabs(_battery->Voltage()) < 1e-3) return 0.0; @@ -675,7 +678,7 @@ double RechargeableBatteryPlugin::OnUpdateVoltage(const ignition::common::Batter } // Publish total power supply - ignition::msgs::Float totalPowerSupplyMsg; + gz::msgs::Float totalPowerSupplyMsg; totalPowerSupplyMsg.set_data(this->dataPtr->totalPowerSupply); this->dataPtr->totalPowerSupplyPub.Publish(totalPowerSupplyMsg); @@ -710,18 +713,18 @@ double RechargeableBatteryPlugin::OnUpdateVoltage(const ignition::common::Batter if (socInt % 10 == 0 && socInt != prevSocInt) { - igndbg << "Battery: " << this->dataPtr->battery->Name() << std::endl; - igndbg << "PowerLoads().size(): " << _battery->PowerLoads().size() + gzdbg << "Battery: " << this->dataPtr->battery->Name() << std::endl; + gzdbg << "PowerLoads().size(): " << _battery->PowerLoads().size() << std::endl; - igndbg << "charging current: " << powerSourceCurrent << std::endl; - igndbg << "voltage: " << voltage << std::endl; - igndbg << "state of charge: " << this->dataPtr->StateOfCharge() + gzdbg << "charging current: " << powerSourceCurrent << std::endl; + gzdbg << "voltage: " << voltage << std::endl; + gzdbg << "state of charge: " << this->dataPtr->StateOfCharge() << " (q " << this->dataPtr->q << ")" << std::endl << std::endl; } if (this->dataPtr->StateOfCharge() < 0 && !this->dataPtr->drainPrinted) { - ignwarn << "Model " << this->dataPtr->modelName << " out of battery.\n"; + gzwarn << "Model " << this->dataPtr->modelName << " out of battery.\n"; this->dataPtr->drainPrinted = true; } @@ -740,21 +743,21 @@ void RechargeableBatteryPluginPrivate::Reset() ////////////////////////////////////////////////// void RechargeableBatteryPluginPrivate::OnBatteryDrainingMsg( - const char *, const size_t, const ignition::transport::MessageInfo &) + const char *, const size_t, const gz::transport::MessageInfo &) { this->startDraining = true; } ////////////////////////////////////////////////// void RechargeableBatteryPluginPrivate::OnBatteryStopDrainingMsg( - const char *, const size_t, const ignition::transport::MessageInfo &) + const char *, const size_t, const gz::transport::MessageInfo &) { this->startDraining = false; } ////////////////////////////////////////////////// void RechargeableBatteryPluginPrivate::OnPowerSourceMsg(int _id, - const ignition::msgs::Float &_msg) + const gz::msgs::Float &_msg) { std::lock_guard lock(*(this->powerSourcesInfo[_id].mutex_ptr)); this->powerSourcesInfo[_id].nominalPower = _msg.data(); @@ -767,13 +770,13 @@ double RechargeableBatteryPluginPrivate::StateOfCharge() const return this->soc; } -#include +#include -IGNITION_ADD_PLUGIN(RechargeableBatteryPlugin, - ignition::gazebo::System, +GZ_ADD_PLUGIN(RechargeableBatteryPlugin, + gz::sim::System, RechargeableBatteryPlugin::ISystemConfigure, RechargeableBatteryPlugin::ISystemPreUpdate, RechargeableBatteryPlugin::ISystemUpdate, RechargeableBatteryPlugin::ISystemPostUpdate) -IGNITION_ADD_PLUGIN_ALIAS(RechargeableBatteryPlugin, "simulation::RechargeableBatteryPlugin") +GZ_ADD_PLUGIN_ALIAS(RechargeableBatteryPlugin, "simulation::RechargeableBatteryPlugin") diff --git a/plugins/RechargeableBatteryPlugin.hh b/plugins/RechargeableBatteryPlugin.hh index 9d0aab1..ff07efc 100644 --- a/plugins/RechargeableBatteryPlugin.hh +++ b/plugins/RechargeableBatteryPlugin.hh @@ -19,8 +19,8 @@ #define RECHARGEABLE_BATTERY_PLUGIN_HH_ #include -#include -#include +#include +#include namespace simulation @@ -47,11 +47,11 @@ namespace simulation /// - ``: This is to subscribe to power sources topics. Repeat as many times as needed with the same name class RechargeableBatteryPlugin - : public ignition::gazebo::System, - public ignition::gazebo::ISystemConfigure, - public ignition::gazebo::ISystemPreUpdate, - public ignition::gazebo::ISystemUpdate, - public ignition::gazebo::ISystemPostUpdate + : public gz::sim::System, + public gz::sim::ISystemConfigure, + public gz::sim::ISystemPreUpdate, + public gz::sim::ISystemUpdate, + public gz::sim::ISystemPostUpdate { /// \brief Constructor public: @@ -63,31 +63,31 @@ namespace simulation /// Documentation Inherited public: - void Configure(const ignition::gazebo::Entity &_entity, + void Configure(const gz::sim::Entity &_entity, const std::shared_ptr &_sdf, - ignition::gazebo::EntityComponentManager &_ecm, - ignition::gazebo::EventManager &_eventMgr) final; + gz::sim::EntityComponentManager &_ecm, + gz::sim::EventManager &_eventMgr) final; /// Documentation Inherited public: - void PreUpdate(const ignition::gazebo::UpdateInfo &_info, - ignition::gazebo::EntityComponentManager &_ecm) override; + void PreUpdate(const gz::sim::UpdateInfo &_info, + gz::sim::EntityComponentManager &_ecm) override; // Documentation Inherited public: - void Update(const ignition::gazebo::UpdateInfo &_info, - ignition::gazebo::EntityComponentManager &_ecm) override; + void Update(const gz::sim::UpdateInfo &_info, + gz::sim::EntityComponentManager &_ecm) override; /// Documentation Inherited public: - void PostUpdate(const ignition::gazebo::UpdateInfo &_info, - const ignition::gazebo::EntityComponentManager &_ecm) override; + void PostUpdate(const gz::sim::UpdateInfo &_info, + const gz::sim::EntityComponentManager &_ecm) override; /// \brief Callback for Battery Update events. /// \param[in] _battery Pointer to the battery that is to be updated. /// \return The new voltage. private: - double OnUpdateVoltage(const ignition::common::Battery *_battery); + double OnUpdateVoltage(const gz::common::Battery *_battery); /// \brief Private data pointer private: diff --git a/plugins/SensorPowerSystemPlugin.cc b/plugins/SensorPowerSystemPlugin.cc index ac3eee8..fcb2361 100644 --- a/plugins/SensorPowerSystemPlugin.cc +++ b/plugins/SensorPowerSystemPlugin.cc @@ -15,19 +15,20 @@ * limitations under the License. */ #include "SensorPowerSystemPlugin.hh" -#include -#include -#include -#include +#include "gz_compat.hh" +#include +#include +#include +#include #include "gz/sim/components/BatterySoC.hh" #include "gz/sim/components/BatteryPowerLoad.hh" #include -#include -#include +#include +#include #include "gz/sim/Model.hh" #include #include -#include "ignition/sensors/Sensor.hh" +#include using namespace simulation; @@ -50,10 +51,10 @@ struct SensorInfo std::string batteryName{""}; /// \brief Battery entity - ignition::gazebo::Entity batteryEntity{ignition::gazebo::kNullEntity}; + gz::sim::Entity batteryEntity{gz::sim::kNullEntity}; /// \brief Battery consumer entity - ignition::gazebo::Entity batteryConsumerEntity{ignition::gazebo::kNullEntity}; + gz::sim::Entity batteryConsumerEntity{gz::sim::kNullEntity}; /// \brief Flag to check if the battery exists bool batteryExist{false}; @@ -75,13 +76,13 @@ class simulation::SensorPowerSystemPrivate /// \param[in] _id The id of the sensor /// \param[in] _msg The message containing the activation state public: - void OnActivateSensor(int _id, const ignition::msgs::Boolean &_msg); + void OnActivateSensor(int _id, const gz::msgs::Boolean &_msg); /// \brief Check if the battery has sufficient charge /// \param[in] _ecm The entity component manager /// \return True if the battery has sufficient charge public: - void HasSufficientBattery(const ignition::gazebo::EntityComponentManager &_ecm); + void HasSufficientBattery(const gz::sim::EntityComponentManager &_ecm); /// \brief Model name public: @@ -89,11 +90,11 @@ class simulation::SensorPowerSystemPrivate /// \brief Model entity public: - gz::sim::Model model{ignition::gazebo::kNullEntity}; + gz::sim::Model model{gz::sim::kNullEntity}; /// \brief Ignition communication node public: - ignition::transport::Node node; + gz::transport::Node node; /// \brief Sensors information public: @@ -114,16 +115,16 @@ SensorPowerSystemPlugin::SensorPowerSystemPlugin() SensorPowerSystemPlugin::~SensorPowerSystemPlugin() = default; ///////////////////////////////////////////////// -void SensorPowerSystemPlugin::Configure(const ignition::gazebo::Entity &_entity, +void SensorPowerSystemPlugin::Configure(const gz::sim::Entity &_entity, const std::shared_ptr &_sdf, - ignition::gazebo::EntityComponentManager &_ecm, - ignition::gazebo::EventManager &_eventMgr) + gz::sim::EntityComponentManager &_ecm, + gz::sim::EventManager &_eventMgr) { // Store the pointer to the model this battery is under - auto model = ignition::gazebo::Model(_entity); + auto model = gz::sim::Model(_entity); if (!model.Valid(_ecm)) { - ignerr << "SensorPowerSystemPlugin plugin should be attached to a model entity. " + gzerr << "SensorPowerSystemPlugin plugin should be attached to a model entity. " << "Failed to initialize." << std::endl; return; } @@ -136,14 +137,14 @@ void SensorPowerSystemPlugin::Configure(const ignition::gazebo::Entity &_entity, // camera sensors int sensorCount = 0; - _ecm.Each( - [&](const ignition::gazebo::Entity &_entity, - const ignition::gazebo::components::Camera *_camera) -> bool + _ecm.Each( + [&](const gz::sim::Entity &_entity, + const gz::sim::components::Camera *_camera) -> bool { - auto cameraName = _ecm.Component(_entity); + auto cameraName = _ecm.Component(_entity); if (cameraName) { - auto cameraPtr = _ecm.Component(_entity); + auto cameraPtr = _ecm.Component(_entity); auto camera = cameraPtr->Data().CameraSensor(); auto parent = camera->Element()->GetParent(); if (parent->HasElement("power_load") && parent->HasElement("battery_name")) @@ -155,12 +156,12 @@ void SensorPowerSystemPlugin::Configure(const ignition::gazebo::Entity &_entity, sensorInfo.batteryName = parent->Get("battery_name"); sensorInfo.enableSensor = true; sensorInfo.dataUpdated = false; - igndbg << "CAMERA: " << sensorInfo.name << " id: " << sensorInfo.id << std::endl; - igndbg << "CAMERA: " << sensorInfo.name << " Power: " << sensorInfo.powerLoad << std::endl; - igndbg << "CAMERA: " << sensorInfo.name << " Battery name: " << sensorInfo.batteryName << std::endl; - igndbg << "CAMERA: " << sensorInfo.name << " is enabled: " << sensorInfo.enableSensor << std::endl; - igndbg << "CAMERA: " << sensorInfo.name << " data updated: " << sensorInfo.dataUpdated << std::endl; - igndbg << "CAMERA id: " << sensorInfo.id << std::endl; + gzdbg << "CAMERA: " << sensorInfo.name << " id: " << sensorInfo.id << std::endl; + gzdbg << "CAMERA: " << sensorInfo.name << " Power: " << sensorInfo.powerLoad << std::endl; + gzdbg << "CAMERA: " << sensorInfo.name << " Battery name: " << sensorInfo.batteryName << std::endl; + gzdbg << "CAMERA: " << sensorInfo.name << " is enabled: " << sensorInfo.enableSensor << std::endl; + gzdbg << "CAMERA: " << sensorInfo.name << " data updated: " << sensorInfo.dataUpdated << std::endl; + gzdbg << "CAMERA id: " << sensorInfo.id << std::endl; this->dataPtr->sensorsInfo.emplace_back(std::move(sensorInfo)); sensorCount++; } @@ -169,15 +170,15 @@ void SensorPowerSystemPlugin::Configure(const ignition::gazebo::Entity &_entity, }); // imu sensors - _ecm.Each( - [&](const ignition::gazebo::Entity &_entity, - const ignition::gazebo::components::Imu *_imu) -> bool + _ecm.Each( + [&](const gz::sim::Entity &_entity, + const gz::sim::components::Imu *_imu) -> bool { // get the imu name - auto imuName = _ecm.Component(_entity); + auto imuName = _ecm.Component(_entity); if (imuName) { - auto imuPtr = _ecm.Component(_entity); + auto imuPtr = _ecm.Component(_entity); auto imu = imuPtr->Data().ImuSensor(); auto parent = imu->Element()->GetParent(); if (parent->HasElement("power_load") && parent->HasElement("battery_name")) @@ -189,11 +190,11 @@ void SensorPowerSystemPlugin::Configure(const ignition::gazebo::Entity &_entity, sensorInfo.batteryName = parent->Get("battery_name"); sensorInfo.enableSensor = true; sensorInfo.dataUpdated = false; - igndbg << "IMU: " << sensorInfo.name << " id: " << sensorInfo.id << std::endl; - igndbg << "IMU: " << sensorInfo.name << " Power: " << sensorInfo.powerLoad << std::endl; - igndbg << "IMU: " << sensorInfo.name << " Battery name: " << sensorInfo.batteryName << std::endl; - igndbg << "IMU: " << sensorInfo.name << " is enabled: " << sensorInfo.enableSensor << std::endl; - igndbg << "IMU: " << sensorInfo.name << " data updated: " << sensorInfo.dataUpdated << std::endl; + gzdbg << "IMU: " << sensorInfo.name << " id: " << sensorInfo.id << std::endl; + gzdbg << "IMU: " << sensorInfo.name << " Power: " << sensorInfo.powerLoad << std::endl; + gzdbg << "IMU: " << sensorInfo.name << " Battery name: " << sensorInfo.batteryName << std::endl; + gzdbg << "IMU: " << sensorInfo.name << " is enabled: " << sensorInfo.enableSensor << std::endl; + gzdbg << "IMU: " << sensorInfo.name << " data updated: " << sensorInfo.dataUpdated << std::endl; this->dataPtr->sensorsInfo.emplace_back(std::move(sensorInfo)); sensorCount++; } @@ -205,22 +206,22 @@ void SensorPowerSystemPlugin::Configure(const ignition::gazebo::Entity &_entity, for (auto &sensor : this->dataPtr->sensorsInfo) { std::string stateTopic{"/model/" + this->dataPtr->model.Name(_ecm) + "/sensor/" + sensor.name + "/activate"}; - auto validSensorTopic = ignition::transport::TopicUtils::AsValidTopic(stateTopic); + auto validSensorTopic = gz::transport::TopicUtils::AsValidTopic(stateTopic); if (validSensorTopic.empty()) { - ignerr << "Failed to create valid topic. Not valid: [" + gzerr << "Failed to create valid topic. Not valid: [" << sensor.name << "]" << std::endl; return; } - std::function callback = std::bind(&SensorPowerSystemPrivate::OnActivateSensor, + std::function callback = std::bind(&SensorPowerSystemPrivate::OnActivateSensor, this->dataPtr.get(), sensor.id, std::placeholders::_1); this->dataPtr->node.Subscribe(validSensorTopic, callback); } } ////////////////////////////////////////////////// -void SensorPowerSystemPlugin::PreUpdate(const ignition::gazebo::UpdateInfo &_info, - ignition::gazebo::EntityComponentManager &_ecm) +void SensorPowerSystemPlugin::PreUpdate(const gz::sim::UpdateInfo &_info, + gz::sim::EntityComponentManager &_ecm) { if (_info.paused) @@ -230,10 +231,10 @@ void SensorPowerSystemPlugin::PreUpdate(const ignition::gazebo::UpdateInfo &_inf if (!this->dataPtr->batteriesInitialized) { this->dataPtr->batteriesInitialized = true; - _ecm.Each( - [&](const ignition::gazebo::Entity &_entity, - const ignition::gazebo::components::BatterySoC *_batterySoc, - const ignition::gazebo::components::Name *_name) -> bool + _ecm.Each( + [&](const gz::sim::Entity &_entity, + const gz::sim::components::BatterySoC *_batterySoc, + const gz::sim::components::Name *_name) -> bool { if (_name) { @@ -241,8 +242,8 @@ void SensorPowerSystemPlugin::PreUpdate(const ignition::gazebo::UpdateInfo &_inf { if (sensor.batteryName == _name->Data()) { - igndbg << "Battery found for sensor: " << sensor.name << std::endl; - igndbg << "Battery name: " << _name->Data() << std::endl; + gzdbg << "Battery found for sensor: " << sensor.name << std::endl; + gzdbg << "Battery name: " << _name->Data() << std::endl; sensor.batteryExist = true; sensor.batteryEntity = _entity; } @@ -255,14 +256,14 @@ void SensorPowerSystemPlugin::PreUpdate(const ignition::gazebo::UpdateInfo &_inf if (sensor.batteryExist) { sensor.batteryConsumerEntity = _ecm.CreateEntity(); - ignition::gazebo::components::BatteryPowerLoadInfo batteryPowerLoad{ + gz::sim::components::BatteryPowerLoadInfo batteryPowerLoad{ sensor.batteryEntity, sensor.powerLoad}; - _ecm.CreateComponent(sensor.batteryConsumerEntity, ignition::gazebo::components::BatteryPowerLoad(batteryPowerLoad)); + _ecm.CreateComponent(sensor.batteryConsumerEntity, gz::sim::components::BatteryPowerLoad(batteryPowerLoad)); _ecm.SetParentEntity(sensor.batteryConsumerEntity, sensor.batteryEntity); } else { - igndbg << "Sensor " << sensor.name << " battery does not exist" << std::endl; + gzdbg << "Sensor " << sensor.name << " battery does not exist" << std::endl; } } } @@ -279,17 +280,17 @@ void SensorPowerSystemPlugin::PreUpdate(const ignition::gazebo::UpdateInfo &_inf } std::lock_guard lock(*sensor.mutex_ptr); sensor.dataUpdated = false; - ignition::gazebo::v6::components::BatteryPowerLoadInfo batteryPowerLoad{ + gz::sim::components::BatteryPowerLoadInfo batteryPowerLoad{ sensor.batteryEntity, setPower}; - _ecm.SetComponentData(sensor.batteryConsumerEntity, batteryPowerLoad); + _ecm.SetComponentData(sensor.batteryConsumerEntity, batteryPowerLoad); } } } } ///////////////////////////////////////////////// -void SensorPowerSystemPlugin::PostUpdate(const ignition::gazebo::UpdateInfo &_info, - const ignition::gazebo::EntityComponentManager &_ecm) +void SensorPowerSystemPlugin::PostUpdate(const gz::sim::UpdateInfo &_info, + const gz::sim::EntityComponentManager &_ecm) { if(_info.paused) @@ -301,13 +302,13 @@ void SensorPowerSystemPlugin::PostUpdate(const ignition::gazebo::UpdateInfo &_in ///////////////////////////////////////////////// void SensorPowerSystemPrivate::HasSufficientBattery( - const ignition::gazebo::EntityComponentManager &_ecm) + const gz::sim::EntityComponentManager &_ecm) { - _ecm.Each([&]( - const ignition::gazebo::Entity &_entity, - const ignition::gazebo::components::BatterySoC *_data + _ecm.Each([&]( + const gz::sim::Entity &_entity, + const gz::sim::components::BatterySoC *_data ){ - auto BatteryName = _ecm.Component(_entity); + auto BatteryName = _ecm.Component(_entity); if(!BatteryName) { return false; @@ -331,19 +332,19 @@ void SensorPowerSystemPrivate::HasSufficientBattery( } ///////////////////////////////////////////////// -void SensorPowerSystemPrivate::OnActivateSensor(int _id, const ignition::msgs::Boolean &_msg) +void SensorPowerSystemPrivate::OnActivateSensor(int _id, const gz::msgs::Boolean &_msg) { std::lock_guard lock(*this->sensorsInfo[_id].mutex_ptr); this->sensorsInfo[_id].enableSensor = _msg.data(); this->sensorsInfo[_id].dataUpdated = true; } -#include +#include -IGNITION_ADD_PLUGIN(SensorPowerSystemPlugin, - ignition::gazebo::System, +GZ_ADD_PLUGIN(SensorPowerSystemPlugin, + gz::sim::System, SensorPowerSystemPlugin::ISystemConfigure, SensorPowerSystemPlugin::ISystemPreUpdate, SensorPowerSystemPlugin::ISystemPostUpdate) -IGNITION_ADD_PLUGIN_ALIAS(SensorPowerSystemPlugin, "simulation::SensorPowerSystemPlugin") +GZ_ADD_PLUGIN_ALIAS(SensorPowerSystemPlugin, "simulation::SensorPowerSystemPlugin") diff --git a/plugins/SensorPowerSystemPlugin.hh b/plugins/SensorPowerSystemPlugin.hh index e91c36d..259007f 100644 --- a/plugins/SensorPowerSystemPlugin.hh +++ b/plugins/SensorPowerSystemPlugin.hh @@ -20,7 +20,7 @@ -#include +#include namespace simulation @@ -44,10 +44,10 @@ namespace simulation /// why this is a separate plugin. class SensorPowerSystemPlugin - : public ignition::gazebo::System, - public ignition::gazebo::ISystemConfigure, - public ignition::gazebo::ISystemPreUpdate, - public ignition::gazebo::ISystemPostUpdate + : public gz::sim::System, + public gz::sim::ISystemConfigure, + public gz::sim::ISystemPreUpdate, + public gz::sim::ISystemPostUpdate { // Constructor public: @@ -59,19 +59,19 @@ class SensorPowerSystemPlugin /// Documentation Inherited public: - void Configure(const ignition::gazebo::Entity &_entity, + void Configure(const gz::sim::Entity &_entity, const std::shared_ptr &_sdf, - ignition::gazebo::EntityComponentManager &_ecm, - ignition::gazebo::EventManager &_eventMgr) final; + gz::sim::EntityComponentManager &_ecm, + gz::sim::EventManager &_eventMgr) final; /// Documentation Inherited public: - void PreUpdate(const ignition::gazebo::UpdateInfo &_info, - ignition::gazebo::EntityComponentManager &_ecm) override; + void PreUpdate(const gz::sim::UpdateInfo &_info, + gz::sim::EntityComponentManager &_ecm) override; /// Documentation Inherited public: - void PostUpdate(const ignition::gazebo::UpdateInfo &_info, - const ignition::gazebo::EntityComponentManager &_ecm) override; + void PostUpdate(const gz::sim::UpdateInfo &_info, + const gz::sim::EntityComponentManager &_ecm) override; /// \brief Private data pointer private: std::unique_ptr dataPtr; diff --git a/plugins/SolarPanelPlugin.cc b/plugins/SolarPanelPlugin.cc index e39cc31..ae3e647 100644 --- a/plugins/SolarPanelPlugin.cc +++ b/plugins/SolarPanelPlugin.cc @@ -16,27 +16,28 @@ */ #include "SolarPanelPlugin.hh" +#include "gz_compat.hh" -#include +#include +#include -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include -#include -#include "ignition/gazebo/Model.hh" -#include -#include +#include +#include "gz/sim/Model.hh" +#include +#include -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include using namespace simulation; @@ -47,29 +48,27 @@ class simulation::SolarPanelPluginPrivate /// \param[in] _ecm Entity component manager public: std::vector GetVisualChildren( - const ignition::gazebo::EntityComponentManager &_ecm); + const gz::sim::EntityComponentManager &_ecm); /// \brief Find the scene public: bool FindScene(); - IGN_COMMON_WARN_IGNORE__DLL_INTERFACE_MISSING /// \brief Event that is used to trigger callbacks when the scene /// is changed /// \param[in] _scene The new scene public: - static ignition::common::EventT + static gz::common::EventT sceneEvent; - IGN_COMMON_WARN_RESUME__DLL_INTERFACE_MISSING /// \brief Pointer to rendering scene /// \param[in] _scene Rendering scene public: - ignition::rendering::ScenePtr scene{nullptr}; + gz::rendering::ScenePtr scene{nullptr}; /// \brief Connection to the Manager's scene change event. public: - ignition::common::ConnectionPtr sceneChangeConnection; + gz::common::ConnectionPtr sceneChangeConnection; /// \brief Just a mutex for thread safety public: @@ -97,19 +96,19 @@ class simulation::SolarPanelPluginPrivate /// \brief Model interface public: - ignition::gazebo::Model model{ignition::gazebo::kNullEntity}; + gz::sim::Model model{gz::sim::kNullEntity}; /// \brief Link entity public: - ignition::gazebo::Entity linkEntity{ignition::gazebo::kNullEntity}; + gz::sim::Entity linkEntity{gz::sim::kNullEntity}; /// \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; }; ////////////////////////////////////////////////// @@ -122,16 +121,16 @@ SolarPanelPlugin::SolarPanelPlugin() SolarPanelPlugin::~SolarPanelPlugin() = default; ////////////////////////////////////////////////// -void SolarPanelPlugin::Configure(const ignition::gazebo::Entity &_entity, +void SolarPanelPlugin::Configure(const gz::sim::Entity &_entity, const std::shared_ptr &_sdf, - ignition::gazebo::EntityComponentManager &_ecm, - ignition::gazebo::EventManager &_eventMgr) + gz::sim::EntityComponentManager &_ecm, + gz::sim::EventManager &_eventMgr) { // Store the pointer to the model the solar panel is under - auto model = ignition::gazebo::Model(_entity); + auto model = gz::sim::Model(_entity); if (!model.Valid(_ecm)) { - ignerr << "Solar panel plugin should be attached to a model entity. " + gzerr << "Solar panel plugin should be attached to a model entity. " << "Failed to initialize." << std::endl; return; } @@ -144,18 +143,18 @@ void SolarPanelPlugin::Configure(const ignition::gazebo::Entity &_entity, { this->dataPtr->linkName = _sdf->Get("link_name"); this->dataPtr->topicName = "/model/" + this->dataPtr->modelName + "/" + this->dataPtr->linkName + "/solar_panel_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(validTopic); + this->dataPtr->nominalPowerPub = this->dataPtr->node.Advertise(validTopic); } else { - ignerr << "Solar panel plugin should have a element. " + gzerr << "Solar panel plugin should have a element. " << "Failed to initialize." << std::endl; return; } @@ -166,7 +165,7 @@ void SolarPanelPlugin::Configure(const ignition::gazebo::Entity &_entity, } else { - ignerr << "Solar panel plugin should have a element. " + gzerr << "Solar panel plugin should have a element. " << "Failed to initialize." << std::endl; return; } @@ -175,8 +174,8 @@ void SolarPanelPlugin::Configure(const ignition::gazebo::Entity &_entity, } ////////////////////////////////////////////////// -void SolarPanelPlugin::PostUpdate(const ignition::gazebo::UpdateInfo &_info, - const ignition::gazebo::EntityComponentManager &_ecm) +void SolarPanelPlugin::PostUpdate(const gz::sim::UpdateInfo &_info, + const gz::sim::EntityComponentManager &_ecm) { if (_info.paused) { @@ -186,15 +185,15 @@ void SolarPanelPlugin::PostUpdate(const ignition::gazebo::UpdateInfo &_info, { if (!this->dataPtr->FindScene()) { - ignwarn << "Rendering scene not available yet" << std::endl; + gzwarn << "Rendering scene not available yet" << std::endl; return; } } - std::shared_ptr rayQuery = this->dataPtr->scene->CreateRayQuery(); + std::shared_ptr rayQuery = this->dataPtr->scene->CreateRayQuery(); if (!rayQuery) { - ignerr << "Failed to create RayQuery" << std::endl; + gzerr << "Failed to create RayQuery" << std::endl; return; } @@ -204,12 +203,12 @@ void SolarPanelPlugin::PostUpdate(const ignition::gazebo::UpdateInfo &_info, } // Get sun entity - ignition::gazebo::Entity sunEntity; - ignition::math::Pose3d sunPose; - _ecm.Each( - [&](const ignition::gazebo::Entity &_entity, - const ignition::gazebo::components::Name *_name, - const ignition::gazebo::components::Pose *_pose) -> bool + gz::sim::Entity sunEntity; + gz::math::Pose3d sunPose; + _ecm.Each( + [&](const gz::sim::Entity &_entity, + const gz::sim::components::Name *_name, + const gz::sim::components::Pose *_pose) -> bool { if (_name->Data() == "sun") { @@ -220,15 +219,15 @@ void SolarPanelPlugin::PostUpdate(const ignition::gazebo::UpdateInfo &_info, return true; }); - if (sunEntity == ignition::gazebo::v6::kNullEntity) + if (sunEntity == gz::sim::kNullEntity) { - ignerr << "Sun entity not found" << std::endl; + gzerr << "Sun entity not found" << std::endl; return; } // Check if sun entity is of type "light" and has a "direction" element - const auto *lightComp = _ecm.Component(sunEntity); - ignition::math::Vector3d direction; + const auto *lightComp = _ecm.Component(sunEntity); + gz::math::Vector3d direction; if (lightComp) { const auto &light = lightComp->Data(); @@ -236,23 +235,23 @@ void SolarPanelPlugin::PostUpdate(const ignition::gazebo::UpdateInfo &_info, } else { - ignerr << "Sun entity is not a light!" << std::endl; + gzerr << "Sun entity is not a light!" << std::endl; return; } // Rotate sun direction according to sun pose orientation - ignition::math::Vector3d sunDirection = sunPose.Rot().RotateVector(direction); + gz::math::Vector3d sunDirection = sunPose.Rot().RotateVector(direction); - if (this->dataPtr->linkEntity == ignition::gazebo::v6::kNullEntity) + if (this->dataPtr->linkEntity == gz::sim::kNullEntity) { this->dataPtr->linkEntity = this->dataPtr->model.LinkByName(_ecm, this->dataPtr->linkName); } - ignition::math::Pose3d linkPose = ignition::gazebo::worldPose(this->dataPtr->linkEntity, _ecm); + gz::math::Pose3d linkPose = gz::sim::worldPose(this->dataPtr->linkEntity, _ecm); // Perform ray cast from link to sun - ignition::math::Vector3d start = linkPose.Pos(); - ignition::math::Vector3d end = sunPose.Pos(); + gz::math::Vector3d start = linkPose.Pos(); + gz::math::Vector3d end = sunPose.Pos(); rayQuery->SetOrigin(end); rayQuery->SetDirection(start - end); @@ -263,7 +262,7 @@ void SolarPanelPlugin::PostUpdate(const ignition::gazebo::UpdateInfo &_info, std::string objectName = "unknown"; bool isInLOS = false; - ignition::rendering::NodePtr node = this->dataPtr->scene->NodeById(result.objectId); + gz::rendering::NodePtr node = this->dataPtr->scene->NodeById(result.objectId); if (node) { objectName = node->Name(); @@ -279,7 +278,7 @@ void SolarPanelPlugin::PostUpdate(const ignition::gazebo::UpdateInfo &_info, // Compute the angle between the link normal and sun direction // Calculate dot product - ignition::math::Vector3d linkNormal = linkPose.Rot().RotateVector(ignition::math::Vector3d::UnitZ); + gz::math::Vector3d linkNormal = linkPose.Rot().RotateVector(gz::math::Vector3d::UnitZ); float dotProduct = linkNormal.Dot(-sunDirection); // Negate sunDirection because it points from sun to scene // Solar panel will not receive any power if angle is more than 90deg (sun rays hitting horizontally or below) @@ -290,8 +289,8 @@ void SolarPanelPlugin::PostUpdate(const ignition::gazebo::UpdateInfo &_info, float magnitude2 = sunDirection.Length(); // Calculate cosine of the angle float cosAngle; - if (ignition::math::equal(magnitude1, 0.0F) || - ignition::math::equal(magnitude2, 0.0F)) + if (gz::math::equal(magnitude1, 0.0F) || + gz::math::equal(magnitude2, 0.0F)) { cosAngle = 1.0F; } @@ -308,16 +307,16 @@ void SolarPanelPlugin::PostUpdate(const ignition::gazebo::UpdateInfo &_info, } // Publish result - ignition::msgs::Float msg; + gz::msgs::Float msg; msg.set_data(currentPower); this->dataPtr->nominalPowerPub.Publish(msg); - igndbg << "Solar Panel Plugin:: Current power output: " << currentPower << " watts" << std::endl; - igndbg << "Solar Panel Plugin:: In line of sight: " << (isInLOS ? "Yes" : "No") << std::endl; + gzdbg << "Solar Panel Plugin:: Current power output: " << currentPower << " watts" << std::endl; + gzdbg << "Solar Panel Plugin:: In line of sight: " << (isInLOS ? "Yes" : "No") << std::endl; } ////////////////////////////////////////////////// -void SolarPanelPlugin::SetScene(ignition::rendering::ScenePtr _scene) +void SolarPanelPlugin::SetScene(gz::rendering::ScenePtr _scene) { std::lock_guard lock(this->dataPtr->mutex); // APIs make it possible for the scene pointer to change @@ -330,10 +329,10 @@ void SolarPanelPlugin::SetScene(ignition::rendering::ScenePtr _scene) ////////////////////////////////////////////////// bool SolarPanelPluginPrivate::FindScene() { - auto loadedEngNames = ignition::rendering::loadedEngines(); + auto loadedEngNames = gz::rendering::loadedEngines(); if (loadedEngNames.empty()) { - ignwarn << "No rendering engine is loaded yet" << std::endl; + gzwarn << "No rendering engine is loaded yet" << std::endl; return false; } @@ -341,20 +340,20 @@ bool SolarPanelPluginPrivate::FindScene() auto engineName = loadedEngNames[0]; if (loadedEngNames.size() > 1) { - ignwarn << "More than one engine is available. " + gzwarn << "More than one engine is available. " << "Using engine [" << engineName << "]" << std::endl; } - auto engine = ignition::rendering::engine(engineName); + auto engine = gz::rendering::engine(engineName); if (!engine) { - ignerr << "Internal error: failed to load engine [" << engineName + gzerr << "Internal error: failed to load engine [" << engineName << "]. Solar panel plugin won't work." << std::endl; return false; } if (engine->SceneCount() == 0) { - igndbg << "No scene has been created yet" << std::endl; + gzdbg << "No scene has been created yet" << std::endl; return false; } @@ -362,13 +361,13 @@ bool SolarPanelPluginPrivate::FindScene() auto scenePtr = engine->SceneByIndex(0); if (nullptr == scenePtr) { - ignerr << "Internal error: scene is null." << std::endl; + gzerr << "Internal error: scene is null." << std::endl; return false; } if (engine->SceneCount() > 1) { - igndbg << "More than one scene is available. " + gzdbg << "More than one scene is available. " << "Using scene [" << scene->Name() << "]" << std::endl; } @@ -383,18 +382,18 @@ bool SolarPanelPluginPrivate::FindScene() ////////////////////////////////////////////////// std::vector SolarPanelPluginPrivate::GetVisualChildren( - const ignition::gazebo::EntityComponentManager &_ecm) + const gz::sim::EntityComponentManager &_ecm) { // Build the prefix for the scoped name std::string scopedPrefix = this->modelName + "::" + this->linkName + "::"; // Find all visual entities that are children of this link std::vector scopedVisualChildren; - _ecm.Each( - [&](const ignition::gazebo::Entity &_entity, - const ignition::gazebo::components::Visual *, - const ignition::gazebo::components::Name *_name, - const ignition::gazebo::components::ParentEntity *_parent) -> bool + _ecm.Each( + [&](const gz::sim::Entity &_entity, + const gz::sim::components::Visual *, + const gz::sim::components::Name *_name, + const gz::sim::components::ParentEntity *_parent) -> bool { if (_parent->Data() == linkEntity) { @@ -407,11 +406,11 @@ std::vector SolarPanelPluginPrivate::GetVisualChildren( return scopedVisualChildren; } -ignition::common::EventT +gz::common::EventT SolarPanelPluginPrivate::sceneEvent; -IGNITION_ADD_PLUGIN(SolarPanelPlugin, ignition::gazebo::System, +GZ_ADD_PLUGIN(SolarPanelPlugin, gz::sim::System, SolarPanelPlugin::ISystemConfigure, SolarPanelPlugin::ISystemPostUpdate) -IGNITION_ADD_PLUGIN_ALIAS(SolarPanelPlugin, "simulation::SolarPanelPlugin") +GZ_ADD_PLUGIN_ALIAS(SolarPanelPlugin, "simulation::SolarPanelPlugin") diff --git a/plugins/SolarPanelPlugin.hh b/plugins/SolarPanelPlugin.hh index 8c3028a..ca266d9 100644 --- a/plugins/SolarPanelPlugin.hh +++ b/plugins/SolarPanelPlugin.hh @@ -22,8 +22,8 @@ #include #include -#include -#include +#include +#include namespace simulation { @@ -32,9 +32,9 @@ namespace simulation /// \brief SolarPanelPlugin SolarPanelPlugin.hh /// \brief A plugin that simulates a solar panel - class SolarPanelPlugin : public ignition::gazebo::System, - public ignition::gazebo::ISystemConfigure, - public ignition::gazebo::ISystemPostUpdate + class SolarPanelPlugin : public gz::sim::System, + public gz::sim::ISystemConfigure, + public gz::sim::ISystemPostUpdate { /// \brief Constructor @@ -47,19 +47,19 @@ namespace simulation /// Documentation inherited public: - void Configure(const ignition::gazebo::Entity &_entity, + void Configure(const gz::sim::Entity &_entity, const std::shared_ptr &_sdf, - ignition::gazebo::EntityComponentManager &_ecm, - ignition::gazebo::EventManager &_eventMgr) override; + gz::sim::EntityComponentManager &_ecm, + gz::sim::EventManager &_eventMgr) override; /// Documentation inherited public: - void PostUpdate(const ignition::gazebo::UpdateInfo &_info, - const ignition::gazebo::EntityComponentManager &_ecm) final; + void PostUpdate(const gz::sim::UpdateInfo &_info, + const gz::sim::EntityComponentManager &_ecm) final; /// \brief Set the scene public: - void SetScene(ignition::rendering::ScenePtr _scene); + void SetScene(gz::rendering::ScenePtr _scene); /// \brief Private data pointer private: diff --git a/plugins/gz_compat.hh b/plugins/gz_compat.hh new file mode 100644 index 0000000..2089829 --- /dev/null +++ b/plugins/gz_compat.hh @@ -0,0 +1,62 @@ +/* + * Copyright (C) 2026 Alexey Simonov + * + * 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. + */ + +#ifndef SIMULATION_GZ_COMPAT_HH_ +#define SIMULATION_GZ_COMPAT_HH_ + +// The plugins are written against the Gazebo Harmonic (gz-sim 8) API. Ignition +// Fortress (gz-sim 6), used on ROS 2 Humble, ships redirect headers under gz/ +// that provide the gz::sim, gz::rendering, gz::sensors, ... namespaces, but +// not the GZ_ / gz prefixed macros. Map those onto their Ignition names when +// they are missing, so one set of sources builds on both. + +#include +#include +#include + +#ifndef GZ_ADD_PLUGIN +#define GZ_ADD_PLUGIN IGNITION_ADD_PLUGIN +#endif + +#ifndef GZ_ADD_PLUGIN_ALIAS +#define GZ_ADD_PLUGIN_ALIAS IGNITION_ADD_PLUGIN_ALIAS +#endif + +#ifndef GZ_PROFILE +#define GZ_PROFILE IGN_PROFILE +#endif + +#ifndef GZ_ASSERT +#define GZ_ASSERT IGN_ASSERT +#endif + +#ifndef gzerr +#define gzerr ignerr +#endif + +#ifndef gzwarn +#define gzwarn ignwarn +#endif + +#ifndef gzmsg +#define gzmsg ignmsg +#endif + +#ifndef gzdbg +#define gzdbg igndbg +#endif + +#endif // SIMULATION_GZ_COMPAT_HH_ From 7237835b2a1c3fd72e70c3c2c46b01990aebf9f1 Mon Sep 17 00:00:00 2001 From: Alexey Simonov Date: Wed, 2 Sep 2026 10:36:38 +0400 Subject: [PATCH 2/4] Run the solar panel line-of-sight ray cast on the render thread (#44). Gazebo runs ISystemPostUpdate systems on parallel worker threads. SolarPanelPlugin created a rendering RayQuery and cast it from PostUpdate, so the three panel instances on the rover hit the Ogre scene concurrently with each other and with the Sensors system's render thread. Under Harmonic that corrupts the heap within seconds of the rover spawning (malloc_consolidate, std::bad_alloc, SIGSEGV or SIGBUS depending on the run). Disabling this one plugin makes the crash go away; disabling either of the other two does not. Move all rendering access into a gz::sim::events::PostRender callback, which runs on the render thread. PostUpdate now only reads the entity state it needs from the ECM, hands the sun-to-panel ray to the render thread under a mutex, takes back the latest line-of-sight result and publishes the power output. The power computation itself is unchanged. The unused static scene event and SetScene hook are removed, and the ray query is created once instead of on every simulation step. --- plugins/SolarPanelPlugin.cc | 200 ++++++++++++++++++++++-------------- plugins/SolarPanelPlugin.hh | 22 ++-- 2 files changed, 134 insertions(+), 88 deletions(-) diff --git a/plugins/SolarPanelPlugin.cc b/plugins/SolarPanelPlugin.cc index ae3e647..896253d 100644 --- a/plugins/SolarPanelPlugin.cc +++ b/plugins/SolarPanelPlugin.cc @@ -1,6 +1,7 @@ /* * Copyright (C) 2024 Robin Baran * Copyright (C) 2024 Stevedan Ogochukwu Omodolor Omodia + * Copyright (C) 2026 Alexey Simonov * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. @@ -18,7 +19,11 @@ #include "SolarPanelPlugin.hh" #include "gz_compat.hh" -#include +#include +#include +#include +#include + #include #include @@ -29,13 +34,15 @@ #include #include -#include "gz/sim/Model.hh" +#include #include #include +#include #include #include #include +#include #include #include @@ -50,27 +57,28 @@ class simulation::SolarPanelPluginPrivate std::vector GetVisualChildren( const gz::sim::EntityComponentManager &_ecm); - /// \brief Find the scene + /// \brief Find the rendering scene. Must be called on the render thread. public: bool FindScene(); - /// \brief Event that is used to trigger callbacks when the scene - /// is changed - /// \param[in] _scene The new scene + /// \brief Ray cast from the sun to the panel and record whether the panel + /// is the first thing the ray hits. Runs on the render thread. public: - static gz::common::EventT - sceneEvent; + void OnPostRender(); /// \brief Pointer to rendering scene - /// \param[in] _scene Rendering scene public: gz::rendering::ScenePtr scene{nullptr}; - /// \brief Connection to the Manager's scene change event. + /// \brief Ray query, created once the scene is available +public: + gz::rendering::RayQueryPtr rayQuery{nullptr}; + + /// \brief Connection to the post-render event public: - gz::common::ConnectionPtr sceneChangeConnection; + gz::common::ConnectionPtr postRenderConn; - /// \brief Just a mutex for thread safety + /// \brief Protects the data shared between the simulation and render threads public: std::mutex mutex; @@ -102,13 +110,30 @@ class simulation::SolarPanelPluginPrivate public: gz::sim::Entity linkEntity{gz::sim::kNullEntity}; - /// \brief Ignition communication node + /// \brief Gazebo communication node public: gz::transport::Node node; - /// \brief Publisher for the radioisotope thermal generator output + /// \brief Publisher for the solar panel output public: gz::transport::Node::Publisher nominalPowerPub; + + /// \brief Ray origin (the sun), written by the simulation thread +public: + gz::math::Vector3d rayOrigin; + + /// \brief Ray direction (sun to panel), written by the simulation thread +public: + gz::math::Vector3d rayDirection; + + /// \brief Whether rayOrigin and rayDirection have been set +public: + bool rayValid{false}; + + /// \brief Whether the panel is in the sun's line of sight, written by the + /// render thread +public: + bool isInLOS{false}; }; ////////////////////////////////////////////////// @@ -131,7 +156,7 @@ void SolarPanelPlugin::Configure(const gz::sim::Entity &_entity, if (!model.Valid(_ecm)) { gzerr << "Solar panel plugin should be attached to a model entity. " - << "Failed to initialize." << std::endl; + << "Failed to initialize." << std::endl; return; } @@ -155,7 +180,7 @@ void SolarPanelPlugin::Configure(const gz::sim::Entity &_entity, else { gzerr << "Solar panel plugin should have a element. " - << "Failed to initialize." << std::endl; + << "Failed to initialize." << std::endl; return; } @@ -166,11 +191,15 @@ void SolarPanelPlugin::Configure(const gz::sim::Entity &_entity, else { gzerr << "Solar panel plugin should have a element. " - << "Failed to initialize." << std::endl; + << "Failed to initialize." << std::endl; return; } - this->dataPtr->sceneChangeConnection = this->dataPtr->sceneEvent.Connect(std::bind(&SolarPanelPlugin::SetScene, this, std::placeholders::_1)); + // The line-of-sight check needs the rendering scene, which may only be used + // from the render thread. PostUpdate runs on parallel worker threads, so the + // ray cast is done from the post-render event instead. + this->dataPtr->postRenderConn = _eventMgr.Connect( + std::bind(&SolarPanelPluginPrivate::OnPostRender, this->dataPtr.get())); } ////////////////////////////////////////////////// @@ -181,29 +210,9 @@ void SolarPanelPlugin::PostUpdate(const gz::sim::UpdateInfo &_info, { return; } - if (!this->dataPtr->scene) - { - if (!this->dataPtr->FindScene()) - { - gzwarn << "Rendering scene not available yet" << std::endl; - return; - } - } - - std::shared_ptr rayQuery = this->dataPtr->scene->CreateRayQuery(); - if (!rayQuery) - { - gzerr << "Failed to create RayQuery" << std::endl; - return; - } - - if (this->dataPtr->scopedVisualChildren.empty()) - { - this->dataPtr->scopedVisualChildren = this->dataPtr->GetVisualChildren(_ecm); - } // Get sun entity - gz::sim::Entity sunEntity; + gz::sim::Entity sunEntity{gz::sim::kNullEntity}; gz::math::Pose3d sunPose; _ecm.Each( [&](const gz::sim::Entity &_entity, @@ -246,31 +255,28 @@ void SolarPanelPlugin::PostUpdate(const gz::sim::UpdateInfo &_info, { this->dataPtr->linkEntity = this->dataPtr->model.LinkByName(_ecm, this->dataPtr->linkName); + if (this->dataPtr->linkEntity == gz::sim::kNullEntity) + { + gzerr << "Link [" << this->dataPtr->linkName << "] not found" << std::endl; + return; + } } gz::math::Pose3d linkPose = gz::sim::worldPose(this->dataPtr->linkEntity, _ecm); - // Perform ray cast from link to sun - gz::math::Vector3d start = linkPose.Pos(); - gz::math::Vector3d end = sunPose.Pos(); - - rayQuery->SetOrigin(end); - rayQuery->SetDirection(start - end); - // Check if ray intersects with any obstacles - auto result = rayQuery->ClosestPoint(); - bool isValid = result; - - std::string objectName = "unknown"; + // Hand the ray from the sun to the panel over to the render thread and take + // back its latest line-of-sight result. bool isInLOS = false; - gz::rendering::NodePtr node = this->dataPtr->scene->NodeById(result.objectId); - if (node) { - objectName = node->Name(); - if (isValid) + std::lock_guard lock(this->dataPtr->mutex); + if (this->dataPtr->scopedVisualChildren.empty()) { - isInLOS = (any_of(this->dataPtr->scopedVisualChildren.begin(), this->dataPtr->scopedVisualChildren.end(), [&](const std::string &elem) - { return elem == objectName; })); + this->dataPtr->scopedVisualChildren = this->dataPtr->GetVisualChildren(_ecm); } + this->dataPtr->rayOrigin = sunPose.Pos(); + this->dataPtr->rayDirection = linkPose.Pos() - sunPose.Pos(); + this->dataPtr->rayValid = true; + isInLOS = this->dataPtr->isInLOS; } // Compute current power output @@ -316,14 +322,60 @@ void SolarPanelPlugin::PostUpdate(const gz::sim::UpdateInfo &_info, } ////////////////////////////////////////////////// -void SolarPanelPlugin::SetScene(gz::rendering::ScenePtr _scene) +void SolarPanelPluginPrivate::OnPostRender() { - std::lock_guard lock(this->dataPtr->mutex); - // APIs make it possible for the scene pointer to change - if (this->dataPtr->scene != _scene) + if (!this->scene) + { + if (!this->FindScene()) + { + return; + } + } + + if (!this->rayQuery) { - this->dataPtr->scene = _scene; + this->rayQuery = this->scene->CreateRayQuery(); + if (!this->rayQuery) + { + gzerr << "Failed to create RayQuery" << std::endl; + return; + } } + + gz::math::Vector3d origin; + gz::math::Vector3d direction; + std::vector visualChildren; + { + std::lock_guard lock(this->mutex); + if (!this->rayValid) + { + return; + } + origin = this->rayOrigin; + direction = this->rayDirection; + visualChildren = this->scopedVisualChildren; + } + + // Perform ray cast from the sun to the panel: the panel is lit if it is + // the first thing the ray hits. + this->rayQuery->SetOrigin(origin); + this->rayQuery->SetDirection(direction); + auto result = this->rayQuery->ClosestPoint(); + + bool inLOS = false; + if (result) + { + gz::rendering::NodePtr node = this->scene->NodeById(result.objectId); + if (node) + { + const std::string objectName = node->Name(); + inLOS = std::any_of(visualChildren.begin(), visualChildren.end(), + [&](const std::string &_elem) { return _elem == objectName; }); + } + } + + std::lock_guard lock(this->mutex); + this->isInLOS = inLOS; } ////////////////////////////////////////////////// @@ -332,7 +384,7 @@ bool SolarPanelPluginPrivate::FindScene() auto loadedEngNames = gz::rendering::loadedEngines(); if (loadedEngNames.empty()) { - gzwarn << "No rendering engine is loaded yet" << std::endl; + gzdbg << "No rendering engine is loaded yet" << std::endl; return false; } @@ -341,13 +393,13 @@ bool SolarPanelPluginPrivate::FindScene() if (loadedEngNames.size() > 1) { gzwarn << "More than one engine is available. " - << "Using engine [" << engineName << "]" << std::endl; + << "Using engine [" << engineName << "]" << std::endl; } auto engine = gz::rendering::engine(engineName); if (!engine) { gzerr << "Internal error: failed to load engine [" << engineName - << "]. Solar panel plugin won't work." << std::endl; + << "]. Solar panel plugin won't work." << std::endl; return false; } @@ -368,7 +420,7 @@ bool SolarPanelPluginPrivate::FindScene() if (engine->SceneCount() > 1) { gzdbg << "More than one scene is available. " - << "Using scene [" << scene->Name() << "]" << std::endl; + << "Using scene [" << scenePtr->Name() << "]" << std::endl; } if (!scenePtr->IsInitialized() || nullptr == scenePtr->RootVisual()) @@ -388,29 +440,25 @@ std::vector SolarPanelPluginPrivate::GetVisualChildren( std::string scopedPrefix = this->modelName + "::" + this->linkName + "::"; // Find all visual entities that are children of this link - std::vector scopedVisualChildren; + std::vector visualChildren; _ecm.Each( - [&](const gz::sim::Entity &_entity, + [&](const gz::sim::Entity &, const gz::sim::components::Visual *, const gz::sim::components::Name *_name, const gz::sim::components::ParentEntity *_parent) -> bool { - if (_parent->Data() == linkEntity) + if (_parent->Data() == this->linkEntity) { - std::string scopedName = scopedPrefix + _name->Data(); - scopedVisualChildren.push_back(scopedName); + visualChildren.push_back(scopedPrefix + _name->Data()); } return true; }); - return scopedVisualChildren; + return visualChildren; } -gz::common::EventT - SolarPanelPluginPrivate::sceneEvent; - GZ_ADD_PLUGIN(SolarPanelPlugin, gz::sim::System, - SolarPanelPlugin::ISystemConfigure, - SolarPanelPlugin::ISystemPostUpdate) + SolarPanelPlugin::ISystemConfigure, + SolarPanelPlugin::ISystemPostUpdate) GZ_ADD_PLUGIN_ALIAS(SolarPanelPlugin, "simulation::SolarPanelPlugin") diff --git a/plugins/SolarPanelPlugin.hh b/plugins/SolarPanelPlugin.hh index ca266d9..91ab64c 100644 --- a/plugins/SolarPanelPlugin.hh +++ b/plugins/SolarPanelPlugin.hh @@ -1,6 +1,7 @@ /* * Copyright (C) 2024 Robin Baran * Copyright (C) 2024 Stevedan Ogochukwu Omodolor Omodia + * Copyright (C) 2026 Alexey Simonov * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. @@ -18,25 +19,25 @@ #ifndef SOLAR_PANEL_PLUGIN_HH_ #define SOLAR_PANEL_PLUGIN_HH_ -#include -#include #include #include -#include namespace simulation { // Forward declaration class SolarPanelPluginPrivate; - /// \brief SolarPanelPlugin SolarPanelPlugin.hh - /// \brief A plugin that simulates a solar panel + /// \brief A plugin that computes the power output of a solar panel from its + /// orientation towards the sun and whether the sun is in its line of sight. + /// + /// The line-of-sight ray cast uses the rendering scene, so it runs on the + /// render thread (gz::sim::events::PostRender). PostUpdate only reads the + /// latest result and publishes the power output. class SolarPanelPlugin : public gz::sim::System, public gz::sim::ISystemConfigure, public gz::sim::ISystemPostUpdate { - /// \brief Constructor public: SolarPanelPlugin(); @@ -45,25 +46,22 @@ namespace simulation public: ~SolarPanelPlugin() override; - /// Documentation inherited + // Documentation inherited public: void Configure(const gz::sim::Entity &_entity, const std::shared_ptr &_sdf, gz::sim::EntityComponentManager &_ecm, gz::sim::EventManager &_eventMgr) override; - /// Documentation inherited + // Documentation inherited public: void PostUpdate(const gz::sim::UpdateInfo &_info, const gz::sim::EntityComponentManager &_ecm) final; - /// \brief Set the scene - public: - void SetScene(gz::rendering::ScenePtr _scene); - /// \brief Private data pointer private: std::unique_ptr dataPtr; }; } + #endif // SOLAR_PANEL_PLUGIN_HH_ From d5c6ad8776f0dfd996de38554b00666d22ebb3fb Mon Sep 17 00:00:00 2001 From: Alexey Simonov Date: Wed, 2 Sep 2026 10:36:38 +0400 Subject: [PATCH 3/4] Make the lunar pole rover model work on Fortress and Harmonic (#36). The ros2_control hardware plugin and the Gazebo system plugins have different names on the two: ign_ros2_control/IgnitionSystem and ignition-gazebo-*-system on Fortress, gz_ros2_control/GazeboSimSystem and gz-sim-*-system on Harmonic. xacro properties keyed on ROS_DISTRO select the right set, so the same model serves the Humble demo and the Jazzy one. The gz_ros2_control plugin also referenced its controller configuration via $(find lunar_pole_exploration_rover), coupling the model to a package in the demos repository. The configuration now lives with the model, as the curiosity rover's does. --- .../lunar_pole_exploration_rover_control.yaml | 51 +++++++++++++++++++ .../urdf/lunar_pole_exploration_rover.gazebo | 22 +++++--- 2 files changed, 67 insertions(+), 6 deletions(-) create mode 100644 models/lunar_pole_exploration_rover/config/lunar_pole_exploration_rover_control.yaml diff --git a/models/lunar_pole_exploration_rover/config/lunar_pole_exploration_rover_control.yaml b/models/lunar_pole_exploration_rover/config/lunar_pole_exploration_rover_control.yaml new file mode 100644 index 0000000..ef4503f --- /dev/null +++ b/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 diff --git a/models/lunar_pole_exploration_rover/urdf/lunar_pole_exploration_rover.gazebo b/models/lunar_pole_exploration_rover/urdf/lunar_pole_exploration_rover.gazebo index 4f81d8c..ae37bb3 100644 --- a/models/lunar_pole_exploration_rover/urdf/lunar_pole_exploration_rover.gazebo +++ b/models/lunar_pole_exploration_rover/urdf/lunar_pole_exploration_rover.gazebo @@ -1,9 +1,19 @@ - + + + + + + + + + + - ign_ros2_control/IgnitionSystem + ${hardware_plugin} @@ -74,19 +84,19 @@ - + robot_description robot_state_publisher - $(find lunar_pole_exploration_rover)/config/lunar_pole_exploration_rover_control.yaml + $(find simulation)/models/lunar_pole_exploration_rover/config/lunar_pole_exploration_rover_control.yaml - + /odom /base_footprint 10 - + ogre2 0.0 0.0 0.0 1 true From dddfea890806e64b49efabcd85dc2ea2f14b348e Mon Sep 17 00:00:00 2001 From: Alexey Simonov Date: Fri, 4 Sep 2026 17:37:51 +0400 Subject: [PATCH 4/4] Drop the leading slashes from the lunar pole rover's odometry frame names (#45). ROS 2 tf2 frame ids must not start with a slash. The demo's odom_tf_publisher copies the frame names from the odometry message into /tf, so the transform went out as /odom -> /base_footprint and only resolved because tf2 strips the slashes on the way in. The curiosity model has the same names; left for a separate change since the space_robots and nav2 demos build on it. --- .../urdf/lunar_pole_exploration_rover.gazebo | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/models/lunar_pole_exploration_rover/urdf/lunar_pole_exploration_rover.gazebo b/models/lunar_pole_exploration_rover/urdf/lunar_pole_exploration_rover.gazebo index ae37bb3..030c5c1 100644 --- a/models/lunar_pole_exploration_rover/urdf/lunar_pole_exploration_rover.gazebo +++ b/models/lunar_pole_exploration_rover/urdf/lunar_pole_exploration_rover.gazebo @@ -91,8 +91,8 @@ - /odom - /base_footprint + odom + base_footprint 10