replace Int32 with Float32MultiArray for ratio-based partial failure

Author: MDEAGEWT

src/realgazebo/models/lc_62.sdf.jinja

@@ -1304,6 +1304,8 @@
       <rotorVelocitySlowdownSim>20</rotorVelocitySlowdownSim>
       <motorType>velocity</motorType>
     </plugin>
+    <plugin filename="MotorFailureROS2" name="custom::MotorFailureROS2System">
+    </plugin>
     <plugin filename="libRealGazebo.so" name="custom::RealGazebo">
       <unreal_ip>{{ unreal_ip }}</unreal_ip>
       <unreal_port>{{ unreal_port }}</unreal_port>

src/realgazebo/plugins/motor_failure_ros2/MotorFailureSystem.cpp

@@ -36,6 +36,7 @@
 
 #include "MotorFailureSystem.hpp"
 
+#include <algorithm>
 #include <gz/plugin/Register.hh>
 #include <gz/sim/components/JointVelocityCmd.hh>
 #include <gz/sim/components/Name.hh>
@@ -98,13 +99,13 @@ void MotorFailureROS2System::Configure(const gz::sim::Entity &_entity,
 	// Get model name to use as namespace
 	std::string model_name = this->model_.Name(_ecm);
 
-	// Get ROS2 topic name for motor failure number subscription
+	// Get ROS2 topic name for motor failure ratios subscription
 	if (_sdf->HasElement("MotorFailureTopic")) {
 		this->ros_topic_ = _sdf->Get<std::string>("MotorFailureTopic");
 
 	} else {
 		// Use model name for topic naming
-		this->ros_topic_ = "/" + model_name + "/motor_failure/motor_number";
+		this->ros_topic_ = "/" + model_name + "/motor_failure/ratios";
 	}
 
 	// Initialize ROS2, if it has not already been initialized
@@ -118,9 +119,9 @@ void MotorFailureROS2System::Configure(const gz::sim::Entity &_entity,
 	this->ros_node_ = rclcpp::Node::make_shared("motor_failure");
 
 	// Create ROS2 subscription
-	this->motor_failure_sub_ = this->ros_node_->create_subscription<std_msgs::msg::Int32>(
+	this->motor_failure_sub_ = this->ros_node_->create_subscription<std_msgs::msg::Float32MultiArray>(
 					   this->ros_topic_, 10,
-					   std::bind(&MotorFailureROS2System::MotorFailureNumberCallback, this, std::placeholders::_1));
+					   std::bind(&MotorFailureROS2System::MotorFailureRatiosCallback, this, std::placeholders::_1));
 
 	gzmsg << "[MotorFailureROS2System] Subscribed to ROS2 topic: " << this->ros_topic_ << std::endl;
 	gzmsg << "[MotorFailureROS2System] Initialized for model: " << model_name << std::endl;
@@ -192,38 +193,63 @@ void MotorFailureROS2System::FindMotorJoints(gz::sim::EntityComponentManager &_e
 //////////////////////////////////////////////////
 void MotorFailureROS2System::ApplyMotorFailure(gz::sim::EntityComponentManager &_ecm)
 {
-	int32_t current_failure;
+	std::vector<float> current_ratios;
 	{
 		std::lock_guard<std::mutex> lock(this->motor_failure_mutex_);
-		current_failure = this->motor_failure_number_;
+		current_ratios = this->motor_failure_ratios_;
 	}
 
 	// Check if failure status changed
-	if (current_failure != this->prev_motor_failure_number_) {
-		if (current_failure > 0) {
-			gzerr << "[MotorFailureROS2System] Motor " << current_failure << " failed!" << std::endl;
+	if (current_ratios != this->prev_motor_failure_ratios_) {
+		for (size_t i = 0; i < current_ratios.size(); ++i) {
+			float ratio = current_ratios[i];
+			float prev = (i < this->prev_motor_failure_ratios_.size()) ? this->prev_motor_failure_ratios_[i] : 0.0f;
 
-		} else if (current_failure == 0 && this->prev_motor_failure_number_ > 0) {
-			gzerr << "[MotorFailureROS2System] Motor " << this->prev_motor_failure_number_
-			      << " recovered!" << std::endl;
+			if (ratio != prev) {
+				if (ratio >= 1.0f) {
+					gzerr << "[MotorFailureROS2System] Motor " << i << " completely failed!" << std::endl;
+
+				} else if (ratio > 0.0f) {
+					gzerr << "[MotorFailureROS2System] Motor " << i << " degraded to "
+					      << (1.0f - ratio) * 100.0f << "% output" << std::endl;
+
+				} else {
+					gzerr << "[MotorFailureROS2System] Motor " << i << " recovered!" << std::endl;
+				}
+			}
+		}
+
+		// Check motors that were in the old list but not in the new (implicitly recovered)
+		for (size_t i = current_ratios.size(); i < this->prev_motor_failure_ratios_.size(); ++i) {
+			if (this->prev_motor_failure_ratios_[i] > 0.0f) {
+				gzerr << "[MotorFailureROS2System] Motor " << i << " recovered!" << std::endl;
+			}
 		}
 
-		this->prev_motor_failure_number_ = current_failure;
+		this->prev_motor_failure_ratios_ = current_ratios;
 	}
 
-	// Apply motor failure if active (1-indexed from ROS2, convert to 0-indexed)
-	if (current_failure > 0 && current_failure <= static_cast<int32_t>(this->motor_joints_.size())) {
-		int motorIdx = current_failure - 1;
-		Entity jointEntity = this->motor_joints_[motorIdx];
+	// Apply per-motor failure ratios
+	for (size_t i = 0; i < this->motor_joints_.size(); ++i) {
+		float ratio = (i < current_ratios.size()) ? current_ratios[i] : 0.0f;
+		ratio = std::clamp(ratio, 0.0f, 1.0f);
 
-		if (jointEntity != kNullEntity) {
-			// Force joint velocity command to 0
-			// This is done in PreUpdate to override MulticopterMotorModel's commands
-			auto jointVelCmd = _ecm.Component<components::JointVelocityCmd>(jointEntity);
+		if (ratio <= 0.0f) {
+			continue;
+		}
 
-			if (jointVelCmd) {
-				*jointVelCmd = components::JointVelocityCmd({0.0});
-			}
+		Entity jointEntity = this->motor_joints_[i];
+
+		if (jointEntity == kNullEntity) {
+			continue;
+		}
+
+		auto jointVelCmd = _ecm.Component<components::JointVelocityCmd>(jointEntity);
+
+		if (jointVelCmd) {
+			double current = jointVelCmd->Data()[0];
+			double degraded = current * (1.0 - static_cast<double>(ratio));
+			*jointVelCmd = components::JointVelocityCmd({degraded});
 		}
 	}
 }
@@ -253,12 +279,12 @@ void MotorFailureROS2System::PreUpdate(const gz::sim::UpdateInfo &_info,
 }
 
 //////////////////////////////////////////////////
-void MotorFailureROS2System::MotorFailureNumberCallback(const std_msgs::msg::Int32::SharedPtr _msg)
+void MotorFailureROS2System::MotorFailureRatiosCallback(const std_msgs::msg::Float32MultiArray::SharedPtr _msg)
 {
 	std::lock_guard<std::mutex> lock(this->motor_failure_mutex_);
-	this->motor_failure_number_ = _msg->data;
-	gzdbg << "[MotorFailureROS2System] Received motor failure number: "
-	      << this->motor_failure_number_ << std::endl;
+	this->motor_failure_ratios_ = _msg->data;
+	gzdbg << "[MotorFailureROS2System] Received motor failure ratios, size: "
+	      << this->motor_failure_ratios_.size() << std::endl;
 }
 
 // Register the plugin

src/realgazebo/plugins/motor_failure_ros2/MotorFailureSystem.hpp

@@ -50,7 +50,7 @@
 
 // ROS2 includes
 #include <rclcpp/rclcpp.hpp>
-#include <std_msgs/msg/int32.hpp>
+#include <std_msgs/msg/float32_multi_array.hpp>
 
 namespace custom
 {
@@ -81,8 +81,8 @@ class MotorFailureROS2System:
 		       gz::sim::EntityComponentManager &_ecm) override;
 
 private:
-	/// \brief Callback for ROS2 motor failure number subscription
-	void MotorFailureNumberCallback(const std_msgs::msg::Int32::SharedPtr _msg);
+	/// \brief Callback for ROS2 motor failure ratios subscription
+	void MotorFailureRatiosCallback(const std_msgs::msg::Float32MultiArray::SharedPtr _msg);
 
 	/// \brief Find all motor joints in the model
 	void FindMotorJoints(gz::sim::EntityComponentManager &_ecm);
@@ -93,8 +93,8 @@ class MotorFailureROS2System:
 	/// \brief ROS2 node for communication
 	rclcpp::Node::SharedPtr ros_node_;
 
-	/// \brief ROS2 subscription for motor failure number
-	rclcpp::Subscription<std_msgs::msg::Int32>::SharedPtr motor_failure_sub_;
+	/// \brief ROS2 subscription for motor failure ratios
+	rclcpp::Subscription<std_msgs::msg::Float32MultiArray>::SharedPtr motor_failure_sub_;
 
 	/// \brief Model entity
 	gz::sim::Entity model_entity_;
@@ -105,15 +105,15 @@ class MotorFailureROS2System:
 	/// \brief Vector of motor joint entities (indexed by motor number)
 	std::vector<gz::sim::Entity> motor_joints_;
 
-	/// \brief Current motor failure number (-1 or 0 means no failure, 1-indexed motor number)
-	int32_t motor_failure_number_{-1};
+	/// \brief Current motor failure ratios (0.0=normal, 1.0=complete failure)
+	std::vector<float> motor_failure_ratios_;
 
-	/// \brief Previous motor failure number to detect changes
-	int32_t prev_motor_failure_number_{-1};
+	/// \brief Previous motor failure ratios to detect changes
+	std::vector<float> prev_motor_failure_ratios_;
 
 	/// \brief ROS2 topic name for subscribing to motor failure commands
-	/// Defaults to /<model_name>/motor_failure/motor_number if not specified in SDF
-	std::string ros_topic_{"/motor_failure/motor_number"};
+	/// Defaults to /<model_name>/motor_failure/ratios if not specified in SDF
+	std::string ros_topic_{"/motor_failure/ratios"};
 
 	/// \brief Mutex to protect motor_failure_number_
 	std::mutex motor_failure_mutex_;

src/realgazebo/plugins/motor_failure_ros2/README.md

@@ -8,19 +8,19 @@ The Motor Failure System plugin subscribes to a ROS2 topic to receive motor fail
 
 ## Features
 
-- ROS2 integration for receiving motor failure commands
+- ROS2 integration for receiving motor failure commands (ratio-based partial failures)
 - Automatic model name detection (no manual namespace configuration needed)
 - 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
+- Direct joint velocity degradation in PreUpdate cycle
+- Thread-safe motor failure ratio handling
 - Configurable topic names via SDF
 
 ## Configuration
 
 ### SDF Parameters
 
 - `<MotorFailureTopic>` (optional): ROS2 topic for receiving motor failure commands
-  - Default: `/<model_name>/motor_failure/motor_number`
+  - Default: `/<model_name>/motor_failure/ratios`
   - The model name is automatically detected from the entity name
 
 ### Example SDF Usage
@@ -55,24 +55,34 @@ The Motor Failure System plugin subscribes to a ROS2 topic to receive motor fail
 
 ### Publishing Motor Failure Commands
 
-To trigger a motor failure, publish a message to the ROS2 topic.
+To trigger a motor failure, publish a `Float32MultiArray` to the ROS2 topic.
+Each element `data[i]` is the failure ratio for motor `i` (0-indexed):
+- `0.0` = normal operation
+- `0.5` = 50% output reduction
+- `1.0` = complete failure
 
 For a vehicle with model name `x500_0`:
 
 ```bash
-# Fail motor 1 (motors are 1-indexed: 1, 2, 3, 4, ...)
-ros2 topic pub --once /x500_0/motor_failure/motor_number std_msgs/msg/Int32 "data: 1"
+# Fail motor 0 by 50% (partial failure)
+ros2 topic pub --once /x500_0/motor_failure/ratios \
+  std_msgs/msg/Float32MultiArray "data: [0.5, 0.0, 0.0, 0.0]"
 
-# Clear motor failure (restore normal operation)
-ros2 topic pub --once /x500_0/motor_failure/motor_number std_msgs/msg/Int32 "data: 0"
+# Completely fail motor 0
+ros2 topic pub --once /x500_0/motor_failure/ratios \
+  std_msgs/msg/Float32MultiArray "data: [1.0, 0.0, 0.0, 0.0]"
+
+# Restore all motors to normal
+ros2 topic pub --once /x500_0/motor_failure/ratios \
+  std_msgs/msg/Float32MultiArray "data: [0.0, 0.0, 0.0, 0.0]"
 ```
 
 **Note**: Replace `x500_0` with your model's name (automatically detected from the entity 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
+**Motor Indexing**:
+- Motors are **0-indexed** matching `rotor_0_joint`, `rotor_1_joint`, etc.
+- Arrays shorter than the motor count leave remaining motors at normal (0.0)
+- Ratios are clamped to `[0.0, 1.0]`
 
 ### Monitoring Motor Failure Status
 
@@ -123,7 +133,7 @@ source install/setup.bash
 ## Notes
 
 - The plugin automatically initializes ROS2 if not already initialized
-- Motor failure number is thread-safe protected with a mutex
-- The plugin applies motor failure in the PreUpdate cycle by setting joint velocity to 0
+- Motor failure ratios are thread-safe protected with a mutex
+- The plugin applies motor failure in the PreUpdate cycle by scaling the current joint velocity command
 - **Plugin Declaration Order**: This plugin must be declared AFTER the MulticopterMotorModel plugin in the SDF file to ensure proper execution order
 - The plugin automatically detects the model name from the entity, eliminating the need for manual namespace configuration