From 517f04259c8b49a98d033d68795ca7e593aba243 Mon Sep 17 00:00:00 2001 From: Kimminkyu <88468818+MDEAGEWT@users.noreply.github.com> Date: Thu, 6 Nov 2025 18:06:42 +0900 Subject: [PATCH] Add Gazebo MotorFailure Plugin (#25776) * Add Gazebo MotorFailure Plugin * switch from ROS2 to Gazebo Transport * Clean up old/dead comments, Refactor variable * gz: submodule update --- Tools/simulation/gz | 2 +- .../simulation/gz_plugins/CMakeLists.txt | 5 +- .../gz_plugins/motor_failure/CMakeLists.txt | 52 +++++ .../motor_failure/MotorFailureSystem.cpp | 221 ++++++++++++++++++ .../motor_failure/MotorFailureSystem.hpp | 123 ++++++++++ .../gz_plugins/motor_failure/README.md | 85 +++++++ 6 files changed, 485 insertions(+), 3 deletions(-) create mode 100644 src/modules/simulation/gz_plugins/motor_failure/CMakeLists.txt create mode 100644 src/modules/simulation/gz_plugins/motor_failure/MotorFailureSystem.cpp create mode 100644 src/modules/simulation/gz_plugins/motor_failure/MotorFailureSystem.hpp create mode 100644 src/modules/simulation/gz_plugins/motor_failure/README.md diff --git a/Tools/simulation/gz b/Tools/simulation/gz index ee3835184c..e34154fd1d 160000 --- a/Tools/simulation/gz +++ b/Tools/simulation/gz @@ -1 +1 @@ -Subproject commit ee3835184c816116887402d186962e13f4b1ff94 +Subproject commit e34154fd1d38fc2a9d50db264870cd2a9a6039a9 diff --git a/src/modules/simulation/gz_plugins/CMakeLists.txt b/src/modules/simulation/gz_plugins/CMakeLists.txt index 7fc65c8b5b..4423a6b139 100644 --- a/src/modules/simulation/gz_plugins/CMakeLists.txt +++ b/src/modules/simulation/gz_plugins/CMakeLists.txt @@ -70,11 +70,12 @@ if (gz-transport_FOUND) add_subdirectory(generic_motor) add_subdirectory(buoyancy) add_subdirectory(spacecraft_thruster) + add_subdirectory(motor_failure) # Add an alias target for each plugin if (TARGET GstCameraSystem) - add_custom_target(px4_gz_plugins ALL DEPENDS OpticalFlowSystem MovingPlatformController TemplatePlugin GenericMotorModelPlugin BuoyancySystemPlugin GstCameraSystem SpacecraftThrusterModelPlugin) + add_custom_target(px4_gz_plugins ALL DEPENDS OpticalFlowSystem MovingPlatformController TemplatePlugin GenericMotorModelPlugin BuoyancySystemPlugin GstCameraSystem SpacecraftThrusterModelPlugin MotorFailurePlugin) else() - add_custom_target(px4_gz_plugins ALL DEPENDS OpticalFlowSystem MovingPlatformController TemplatePlugin GenericMotorModelPlugin BuoyancySystemPlugin SpacecraftThrusterModelPlugin) + add_custom_target(px4_gz_plugins ALL DEPENDS OpticalFlowSystem MovingPlatformController TemplatePlugin GenericMotorModelPlugin BuoyancySystemPlugin SpacecraftThrusterModelPlugin MotorFailurePlugin) endif() endif() diff --git a/src/modules/simulation/gz_plugins/motor_failure/CMakeLists.txt b/src/modules/simulation/gz_plugins/motor_failure/CMakeLists.txt new file mode 100644 index 0000000000..23ced01515 --- /dev/null +++ b/src/modules/simulation/gz_plugins/motor_failure/CMakeLists.txt @@ -0,0 +1,52 @@ +############################################################################ +# +# Copyright (c) 2025 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +project(MotorFailurePlugin) + +add_library(${PROJECT_NAME} SHARED + MotorFailureSystem.cpp +) + +target_link_libraries(${PROJECT_NAME} + PUBLIC px4_gz_msgs + PUBLIC ${GZ_SENSORS_TARGET} + PUBLIC ${GZ_PLUGIN_TARGET} + PUBLIC ${GZ_SIM_TARGET} + PUBLIC ${GZ_TRANSPORT_TARGET} +) + +target_include_directories(${PROJECT_NAME} + PUBLIC ${CMAKE_CURRENT_SOURCE_DIR} + PUBLIC ${CMAKE_CURRENT_BINARY_DIR} + PUBLIC px4_gz_msgs +) diff --git a/src/modules/simulation/gz_plugins/motor_failure/MotorFailureSystem.cpp b/src/modules/simulation/gz_plugins/motor_failure/MotorFailureSystem.cpp new file mode 100644 index 0000000000..e6dd2955d7 --- /dev/null +++ b/src/modules/simulation/gz_plugins/motor_failure/MotorFailureSystem.cpp @@ -0,0 +1,221 @@ +/**************************************************************************** + * + * Copyright (c) 2025 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include "MotorFailureSystem.hpp" + +#include +#include +#include +#include +#include + +using namespace gz; +using namespace sim; +using namespace systems; + +////////////////////////////////////////////////// +void MotorFailureSystem::Configure(const Entity &_entity, + const std::shared_ptr &_sdf, + EntityComponentManager &_ecm, + EventManager &/*_eventMgr*/) +{ + this->_model = Model(_entity); + this->_model_entity = _entity; + + if (!this->_model.Valid(_ecm)) { + gzerr << "[MotorFailureSystem] plugin should be attached to a model " + << "entity. Failed to initialize." << std::endl; + return; + } + + // Get model name to use as namespace + std::string model_name = this->_model.Name(_ecm); + + // Get Gazebo Transport topic name for motor failure number subscription + if (_sdf->HasElement("MotorFailureTopic")) { + this->_gz_topic = _sdf->Get("MotorFailureTopic"); + + } else { + // Use Gazebo model-scoped topic naming convention + this->_gz_topic = "/model/" + model_name + "/motor_failure/motor_number"; + } + + // Subscribe to Gazebo Transport topic + if (!this->_node.Subscribe(this->_gz_topic, &MotorFailureSystem::MotorFailureNumberCallback, this)) { + gzerr << "[MotorFailureSystem] Error subscribing to topic [" << this->_gz_topic << "]" << std::endl; + return; + } + + gzmsg << "[MotorFailureSystem] Subscribed to Gazebo Transport topic: " << this->_gz_topic << std::endl; + gzmsg << "[MotorFailureSystem] Initialized for model: " << model_name << std::endl; +} + +////////////////////////////////////////////////// +void MotorFailureSystem::FindMotorJoints(EntityComponentManager &_ecm) +{ + if (this->_joints_found) { + return; + } + + // Find all joints with "rotor_X_joint" pattern + this->_motor_joints.clear(); + + // Get all joints in the model + auto joints = this->_model.Joints(_ecm); + + // Regular expression to match rotor joints (e.g., "rotor_0_joint", "rotor_1_joint") + std::regex motorPattern("rotor_(\\d+)_joint"); + std::smatch match; + + // Find rotor joints and sort by motor number + std::map motorMap; + + for (const auto &joint : joints) { + auto nameComp = _ecm.Component(joint); + + if (nameComp) { + std::string jointName = nameComp->Data(); + + // Try to match the joint name against the pattern + if (std::regex_match(jointName, match, motorPattern)) { + // Extract motor number from the first capture group + try { + int motorNumber = std::stoi(match[1].str()); + motorMap[motorNumber] = joint; + gzdbg << "[MotorFailureSystem] Found motor " << motorNumber + << ": " << jointName << std::endl; + + } catch (const std::exception &e) { + gzwarn << "[MotorFailureSystem] Failed to parse motor number from: " + << jointName << std::endl; + } + } + } + } + + // Convert map to vector for indexed access + for (const auto &pair : motorMap) { + // Ensure vector is large enough + if (pair.first >= static_cast(this->_motor_joints.size())) { + this->_motor_joints.resize(pair.first + 1, kNullEntity); + } + + this->_motor_joints[pair.first] = pair.second; + } + + if (!this->_motor_joints.empty()) { + gzmsg << "[MotorFailureSystem] Found " << this->_motor_joints.size() + << " motor joints" << std::endl; + this->_joints_found = true; + + } else { + gzwarn << "[MotorFailureSystem] No motor joints found in model" << std::endl; + } +} + +////////////////////////////////////////////////// +void MotorFailureSystem::ApplyMotorFailure(EntityComponentManager &_ecm) +{ + int32_t current_failure; + { + std::lock_guard lock(this->_motor_failure_mutex); + current_failure = this->_motor_failure_number; + } + + // Check if failure status changed + if (current_failure != this->_prev_motor_failure_number) { + if (current_failure > 0) { + gzerr << "[MotorFailureSystem] Motor " << current_failure << " failed!" << std::endl; + + } else if (current_failure == 0 && this->_prev_motor_failure_number > 0) { + gzerr << "[MotorFailureSystem] Motor " << this->_prev_motor_failure_number + << " recovered!" << std::endl; + } + + this->_prev_motor_failure_number = current_failure; + } + + // Apply motor failure if active (1-indexed motor number, convert to 0-indexed) + if (current_failure > 0 && current_failure <= static_cast(this->_motor_joints.size())) { + int motorIdx = current_failure - 1; + Entity jointEntity = this->_motor_joints[motorIdx]; + + if (jointEntity != kNullEntity) { + // Force joint velocity command to 0 + // This is done in PreUpdate to override MulticopterMotorModel's commands + auto jointVelCmd = _ecm.Component(jointEntity); + + if (jointVelCmd) { + *jointVelCmd = components::JointVelocityCmd({0.0}); + } + } + } +} + +////////////////////////////////////////////////// +void MotorFailureSystem::PreUpdate(const UpdateInfo &_info, + EntityComponentManager &_ecm) +{ + // Skip if paused + if (_info.paused) { + return; + } + + // Find motor joints on first update + if (!this->_joints_found) { + this->FindMotorJoints(_ecm); + + } else { + this->ApplyMotorFailure(const_cast(_ecm)); + } +} + +////////////////////////////////////////////////// +void MotorFailureSystem::MotorFailureNumberCallback(const gz::msgs::Int32 &_msg) +{ + std::lock_guard lock(this->_motor_failure_mutex); + this->_motor_failure_number = _msg.data(); + gzdbg << "[MotorFailureSystem] Received motor failure number: " + << this->_motor_failure_number << std::endl; +} + +// Register the plugin +GZ_ADD_PLUGIN( + MotorFailureSystem, + gz::sim::System, + MotorFailureSystem::ISystemConfigure, + MotorFailureSystem::ISystemPreUpdate +) + +GZ_ADD_PLUGIN_ALIAS(MotorFailureSystem, + "gz::sim::systems::MotorFailureSystem") diff --git a/src/modules/simulation/gz_plugins/motor_failure/MotorFailureSystem.hpp b/src/modules/simulation/gz_plugins/motor_failure/MotorFailureSystem.hpp new file mode 100644 index 0000000000..1f3c216128 --- /dev/null +++ b/src/modules/simulation/gz_plugins/motor_failure/MotorFailureSystem.hpp @@ -0,0 +1,123 @@ +/**************************************************************************** + * + * Copyright (c) 2025 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#ifndef GZ_SIM_SYSTEMS_MOTORFAILURESYSTEM_HPP_ +#define GZ_SIM_SYSTEMS_MOTORFAILURESYSTEM_HPP_ + +#include +#include +#include +#include +#include +#include +#include +#include + +// Gazebo Transport includes +#include +#include + +namespace gz +{ +namespace sim +{ +// Inline bracket to help doxygen filtering. +inline namespace GZ_SIM_VERSION_NAMESPACE +{ +namespace systems +{ + +/// \brief This system subscribes to a Gazebo Transport topic to receive motor failure +/// commands and directly controls motor joints to simulate failures. +/// This allows simulating motor failures in multirotor vehicles. +class MotorFailureSystem: + public System, + public ISystemConfigure, + public ISystemPreUpdate +{ +public: + // Documentation inherited + void Configure(const Entity &_entity, + const std::shared_ptr &_sdf, + EntityComponentManager &_ecm, + EventManager &_eventMgr) override; + + // Documentation inherited + void PreUpdate(const gz::sim::UpdateInfo &_info, + gz::sim::EntityComponentManager &_ecm) override; + +private: + /// \brief Callback for Gazebo Transport motor failure number subscription + void MotorFailureNumberCallback(const gz::msgs::Int32 &_msg); + + /// \brief Find all motor joints in the model + void FindMotorJoints(gz::sim::EntityComponentManager &_ecm); + + /// \brief Apply motor failure (set velocity to 0) + void ApplyMotorFailure(gz::sim::EntityComponentManager &_ecm); + + /// \brief Gazebo Transport node for communication + gz::transport::Node _node; + + /// \brief Model entity + gz::sim::Entity _model_entity; + + /// \brief Model interface + gz::sim::Model _model; + + /// \brief Vector of motor joint entities (indexed by motor number) + std::vector _motor_joints; + + /// \brief Current motor failure number (-1 or 0 means no failure, 1-indexed motor number) + int32_t _motor_failure_number{-1}; + + /// \brief Previous motor failure number to detect changes + int32_t _prev_motor_failure_number{-1}; + + /// \brief Gazebo Transport topic name for subscribing to motor failure commands + /// Defaults to /model//motor_failure/motor_number if not specified in SDF + std::string _gz_topic{"/motor_failure/motor_number"}; + + /// \brief Mutex to protect _motor_failure_number + std::mutex _motor_failure_mutex; + + /// \brief Flag to indicate if motor joints have been found + bool _joints_found{false}; +}; + +} // namespace systems +} // namespace GZ_SIM_VERSION_NAMESPACE +} // namespace sim +} // namespace gz + +#endif // GZ_SIM_SYSTEMS_MOTORFAILURESYSTEM_HPP_ diff --git a/src/modules/simulation/gz_plugins/motor_failure/README.md b/src/modules/simulation/gz_plugins/motor_failure/README.md new file mode 100644 index 0000000000..8f1da6ed01 --- /dev/null +++ b/src/modules/simulation/gz_plugins/motor_failure/README.md @@ -0,0 +1,85 @@ +# Motor Failure System Plugin + +This Gazebo plugin enables motor failure simulation for multirotor vehicles in PX4 SITL. + +## Overview + +The Motor Failure System plugin subscribes to a Gazebo Transport topic to receive motor failure commands and directly controls motor joints to simulate failures. This allows simulating motor failures during flight testing. + +## Features + +- Gazebo Transport integration for receiving motor failure commands +- Automatic detection of motor joints (rotor_0_joint, rotor_1_joint, etc.) +- Direct joint velocity override in PreUpdate cycle +- Thread-safe motor failure number handling +- Configurable topic names via SDF + +## Configuration + +### SDF Parameters + +- `` (optional): Gazebo Transport topic for receiving motor failure commands + - Default: `/model//motor_failure/motor_number` + - Follows Gazebo model-scoped topic naming convention + - If specified: Uses the exact topic name provided + +### Example SDF Usage + +**IMPORTANT**: The MotorFailureSystem plugin must be declared **AFTER** the `gz-sim-multicopter-motor-model-system` plugin in your SDF file. This ensures the motor failure override runs after the motor model sets its velocity commands. + +```xml + + + + + + + + + + + + + + /custom/topic/name + +``` + +## Usage + +### Publishing Motor Failure Commands + +To trigger a motor failure, publish a message to the Gazebo Transport topic. + +For a vehicle with namespace `x500_0`: + +```bash +# Fail motor 1 (motors are 1-indexed: 1, 2, 3, 4, ...) +gz topic -t /model/x500_0/motor_failure/motor_number -m gz.msgs.Int32 -p "data: 1" + +# Clear motor failure (restore normal operation) +gz topic -t /model/x500_0/motor_failure/motor_number -m gz.msgs.Int32 -p "data: 0" +``` + +**Note**: Replace `x500_0` with your vehicle's model name. + +**Motor Numbering**: +- Motors are **1-indexed**: 1, 2, 3, 4, etc. +- `data: 0` clears the motor failure +- `data: -1` also clears the motor failure + +### Monitoring Motor Failure Status + +You can monitor the motor failure messages in the Gazebo console output. + + +## Notes + +- The plugin applies motor failure in the PreUpdate cycle by setting joint velocity to 0 +- **Plugin Declaration Order**: This plugin must be declared AFTER the MulticopterMotorModel plugin in the SDF file to ensure proper execution order