Currently running ROS2 Foxy (with Python) with Gazebo 11.11

I currently have a custom force plugin to move my drone model in Gazebo. I am trying to apply a positive force at the location of each propeller of the respective drone. This plugin successfully does so, and when I check the ROS2 topic list, I see only the topic "/motorspeedcmd" with the prefix "/drone1" for example. But, when I spawn 2 or more drones with only a single drone controller publishing to their respective topic (ie /drone1/motorspeedcmd), all the drones are being controlled (ie "/drone2/motorspeedcmd" and "/drone3/motorspeedcmd"). Additionally, it almost seems as if the IMU plugin data may also be shared despite the distinct separation of topics by namespaces, because angular position error seems to affect all drones, but GPS position is consistent.

To implement the namespaces for each drone, from the launch file, the spawner was passed its namespace (drone_ns) and it seems the gazebo plugins take care of the rest (IMU and GPS plugins).

As for the custom force plugin, I had help implementing a workaround where from the launch file I pass to the spawner the "-robotnamespace" which is a variable I define in the plugin to prefix the topic name in the plugin. In the plugin "ns" contains the namespace prefix and is applied to my topic "motorspeedcmd" to subscribe to, then the publisher "pose" to apply to the Gazebo model.

Any insights to this problem or how to fix it is greatly appreciated. Also guides to resources would also be appreciated. C++ is definitely not my strong suit and this has been quite challenging and time consuming to try and figure out.

The following code is the spawner launch file:

from launch import LaunchDescription
from launch_ros.actions import Node
from launch_ros.substitutions import FindPackageShare
from launch.substitutions import LaunchConfiguration

name_pkg = "drone_description"
pkg_share = FindPackageShare(package=name_pkg).find(name_pkg)

def generate_launch_description():    
    robot_description_topic_name = "robot_description"

    drone_ns = LaunchConfiguration('drone_ns', default='-')
    drone_spawn_x = LaunchConfiguration('drone_spawn_x', default='-')
    drone_spawn_y = LaunchConfiguration('drone_spawn_y', default='-')
    drone_spawn_z = LaunchConfiguration('drone_spawn_z', default='-')
    drone_spawn_yaw = LaunchConfiguration('drone_spawn_yaw', default='-')

    return LaunchDescription([

        Node(
            package='gazebo_ros',
            executable='spawn_entity.py',
            output='screen',
            arguments=[
                '-entity', drone_ns,
                '-topic', robot_description_topic_name,
                "-robot_namespace", drone_ns,
                '-x', drone_spawn_x,
                '-y', drone_spawn_y,
                '-z', drone_spawn_z,
                '-Y', drone_spawn_yaw
            ],                    
        ),
    ])

The following code is the include file for the plugin:

# ifndef DRONE_PLUGIN__GAZEBO_ROS_DRONE_FORCE_HPP_
# define DRONE_PLUGIN__GAZEBO_ROS_DRONE_FORCE_HPP_

# include <gazebo/common/Plugin.hh>
# include <tf2_ros/transform_broadcaster.h>
# include <memory>

namespace gazebo_plugins
{
class GazeboRosDroneForcePrivate;

/// A ackermann drive plugin for car like robots. Subscribes to geometry_msgs/twist


class GazeboRosDroneForce : public gazebo::ModelPlugin
{
public:
  /// Constructor
  GazeboRosDroneForce();

  /// Destructor
  ~GazeboRosDroneForce();

  // Documentation inherited
  void Load(gazebo::physics::ModelPtr _model, sdf::ElementPtr _sdf) override;

  void RosThread();

  void PublishDronePose();
protected:
  // /// Optional callback to be called at every simulation iteration.
  // virtual void OnUpdate();
  // void onMotorSpeedsMsg(const geometry_msgs::msg::Twist::SharedPtr _msg);

private:
  /// Private data pointer
  std::unique_ptr<GazeboRosDroneForcePrivate> impl_;

  std::thread ros_thread;

  std::mutex pose_mtx;
  std::mutex cmd_mtx;
  std::string _ns;
  double _rate;
  double _rotor_thrust_coeff;
  double _rotor_torque_coeff;
  bool _publish_tf;
  std::shared_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;
};
}  // namespace gazebo_plugins

#endif  // GAZEBO_PLUGINS__GAZEBO_ROS_ACKERMANN_DRIVE_HPP_

The following code is the custom plugin:

#include <gazebo/gazebo.hh>
#include <gazebo/physics/Model.hh>
#include <gazebo/physics/Link.hh>
#include <drone_plugin/gazebo_ros_drone_force.hpp>

#include <gazebo_ros/node.hpp>
#include <drone_common/msg/pose.hpp>
#include <drone_common/msg/motor_speed.hpp>

#include <rclcpp/rclcpp.hpp>

#include <tf2/LinearMath/Quaternion.h>
#include <tf2/LinearMath/Matrix3x3.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <geometry_msgs/msg/transform_stamped.h>

#include <memory>
#include <mutex>
#include <thread>

namespace gazebo_plugins
{
  ignition::math::Pose3d _pose;
  std::mutex cmd_mtx;
  std::mutex pose_mtx;
  drone_common::msg::MotorSpeed motor_speed_msg;
  // std::string ns_; //tried implementing namespace variable in xacro but I couldnt get it to work
/// Class to hold private data members (PIMPL pattern)
class GazeboRosDroneForcePrivate
{
public:
  /// Pointer to model.
  gazebo::physics::ModelPtr model_;
  double _rotor_thrust_coeff;
  double _rotor_torque_coeff;
  bool _publish_tf;

  /// Node for ROS communication.
  //gazebo_ros::Node::SharedPtr ros_node_; 
  rclcpp::Node::SharedPtr ros_node_;

  /// Pose publisher
  rclcpp::Publisher<drone_common::msg::Pose>::SharedPtr pose_pub_; 
  /// Command subscriber
  rclcpp::Subscription<drone_common::msg::MotorSpeed>::SharedPtr cmd_sub_; 

  /// Connection to event called at every world iteration.
  gazebo::event::ConnectionPtr update_connection_;

  void OnUpdate();
  void onMotorSpeedsMsg(const drone_common::msg::MotorSpeed::SharedPtr _msg);

  void UpdateThrust();

  double CalculateThrust(double w);
  double CalculateTorque(double w);
  double CalculateBodyTorque(double q);
};

GazeboRosDroneForce::GazeboRosDroneForce()
: impl_(std::make_unique<GazeboRosDroneForcePrivate>())
{
  std::cout << "drone_plugin Started" << std::endl;
}

GazeboRosDroneForce::~GazeboRosDroneForce()
{
  std::cout << "drone_plugin Closed" << std::endl;
}

void GazeboRosDroneForce::Load(gazebo::physics::ModelPtr _model, sdf::ElementPtr _sdf)
{
  std::string originalString = _model->GetName().c_str();
  _ns = "/" + originalString + "/";

  if (_sdf->HasElement("updateRate")) {
      _rate = _sdf->GetElement("updateRate")->Get<double>();
    } else {
      _rate = 100.0;
    }
  if (_sdf->HasElement("rotorThrustCoeff")) {
      _rotor_thrust_coeff = _sdf->GetElement("rotorThrustCoeff")->Get<double>();
    } else {
      _rotor_thrust_coeff = 0.01037;
    }
  impl_->_rotor_thrust_coeff = _rotor_thrust_coeff;
  if (_sdf->HasElement("rotorTorqueCoeff")) {
      _rotor_torque_coeff = _sdf->GetElement("rotorTorqueCoeff")->Get<double>();
    } else {
      _rotor_torque_coeff = 0.0000074;
    }
  impl_->_rotor_torque_coeff = _rotor_torque_coeff;
  if (_sdf->HasElement("publishTf")) {
      _publish_tf = _sdf->GetElement("publishTf")->Get<bool>();
    } else {
      _publish_tf = true;
    }
  impl_->_publish_tf = _publish_tf;


  // Create a GazeboRos node instead of a common ROS node.
  // Pass it SDF parameters so common options like namespace and remapping
  // can be handled.
  impl_->ros_node_ = rclcpp::Node::make_shared("drone_force");
  // Get QoS profiles
  // const gazebo_ros::QoS & qos = impl_->ros_node_->get_qos();
  impl_->model_ = _model;

  // The model pointer gives you direct access to the physics object,
  // for example:
  RCLCPP_INFO(impl_->ros_node_->get_logger(), _model->GetName().c_str());

  impl_->pose_pub_ = impl_->ros_node_->create_publisher<drone_common::msg::Pose>(
       **_ns** + "pose", 10
      );

  impl_->cmd_sub_ = impl_->ros_node_->create_subscription<drone_common::msg::MotorSpeed>(
      **_ns** + "motor_speed_cmd", //add namespace in front of topic name
      10,
      std::bind(&GazeboRosDroneForcePrivate::onMotorSpeedsMsg, impl_.get(), std::placeholders::_1)
      );
  ros_thread = std::thread(std::bind(&GazeboRosDroneForce::RosThread, this));

  tf_broadcaster_ = std::make_shared<tf2_ros::TransformBroadcaster>(impl_->ros_node_);

  impl_->update_connection_ = gazebo::event::Events::ConnectWorldUpdateBegin(
      std::bind(&GazeboRosDroneForcePrivate::OnUpdate, impl_.get())
      );
}

void GazeboRosDroneForcePrivate::OnUpdate()
{
  pose_mtx.lock();
  _pose = model_->WorldPose();
  pose_mtx.unlock();

  GazeboRosDroneForcePrivate::UpdateThrust();
}

void GazeboRosDroneForcePrivate::onMotorSpeedsMsg(const drone_common::msg::MotorSpeed::SharedPtr _msg) {
  cmd_mtx.lock();
  motor_speed_msg = *_msg;
  cmd_mtx.unlock();

}

void GazeboRosDroneForce::RosThread() {
    rclcpp::Rate rate(_rate);
    while (rclcpp::ok()) {
      GazeboRosDroneForce::PublishDronePose();
      rclcpp::spin_some(impl_->ros_node_);
      rate.sleep();
    }
}

void GazeboRosDroneForce::PublishDronePose() {
    pose_mtx.lock();
    ignition::math::Pose3d pose = _pose;
    pose_mtx.unlock();

    // ignition::math::Vector3 rpy = pose.Rot.GetAsEuler();
    tf2::Quaternion q(pose.Rot().Roll(), pose.Rot().Pitch(), pose.Rot().Yaw(), pose.Rot().W());
    tf2::Matrix3x3 m(q);
    double roll, pitch, yaw;
    m.getRPY(roll, pitch, yaw);

    drone_common::msg::Pose pose_msg;
    pose_msg.x = pose.Pos().X();
    pose_msg.y = pose.Pos().Y();
    pose_msg.z = pose.Pos().Z();
    pose_msg.roll = roll;
    pose_msg.pitch = -pitch;
    pose_msg.yaw = yaw;
    impl_->pose_pub_->publish(pose_msg);

    if (_publish_tf) {
      tf2::Transform T;
      T.setOrigin(tf2::Vector3(pose.Pos().X(), pose.Pos().Y(), pose.Pos().Z()));
      T.setRotation(tf2::Quaternion(q));

      geometry_msgs::msg::TransformStamped transformStamped;
      transformStamped.header.stamp = impl_->ros_node_->now();
      transformStamped.header.frame_id = "world";
      transformStamped.child_frame_id = "drone";
      transformStamped.transform.translation.x = T.getOrigin().x();
      transformStamped.transform.translation.y = T.getOrigin().y();
      transformStamped.transform.translation.z = T.getOrigin().z();
      transformStamped.transform.rotation.x = T.getRotation().x();
      transformStamped.transform.rotation.y = T.getRotation().y();
      transformStamped.transform.rotation.z = T.getRotation().z();
      transformStamped.transform.rotation.w = T.getRotation().w();
      // tf_broadcaster_->sendTransform(transformStamped);
    }
  }

  void GazeboRosDroneForcePrivate::UpdateThrust() { 
    cmd_mtx.lock();
    drone_common::msg::MotorSpeed cmd = motor_speed_msg;
    cmd_mtx.unlock();

    int n = cmd.name.size();
    for (int i = 0; i < n; ++i) {
      double thrust = GazeboRosDroneForcePrivate::CalculateThrust(cmd.velocity[i]);
      double torque = GazeboRosDroneForcePrivate::CalculateTorque(cmd.velocity[i]);
      //ROS_INFO("torque: %f", torque);
      gazebo::physics::LinkPtr link_ = model_->GetLink(cmd.name[i]);
      if (link_ != NULL) {
        link_->AddLinkForce(ignition::math::Vector3d(0, 0, thrust));
        link_->AddRelativeTorque(ignition::math::Vector3d(0, 0, torque));
      }
    }

    int m = cmd.torque_body_name.size();
    for (int i = 0; i < m; ++i) {
      double body_moment = GazeboRosDroneForcePrivate::CalculateBodyTorque(cmd.body_torque[i]);
      //ROS_INFO("torque: %f", torque);
      gazebo::physics::LinkPtr link_ = model_->GetLink(cmd.torque_body_name[i]);
      if (link_ != NULL) {
        link_->AddRelativeForce(ignition::math::Vector3d(0, body_moment, 0));

      }
    }
  }


  //apply thrust data from DJI E2000 powertrain data () [equation of force is per rotor]
  double GazeboRosDroneForcePrivate::CalculateThrust(double w) {
    double thrust = (0.0002*w*w) - 0.0083*abs(w) + 1.0069; //_rotor_thrust_coeff * w * w;
    return thrust;
  }

  //apply torque given from powertrain data [equation of force is per rotor]
  double GazeboRosDroneForcePrivate::CalculateTorque(double w) {
    double torque = 0; //copysign(100*(538.54*pow(abs(w),-2.282))*(1.2041*0.223459*0.266701*pow((abs(w)*0.266701),2)), w);//new formula:  (2.7488*pow(w,-0.282))  //OLD FORMULA: (538.54*pow(abs(w),-2.282))*(1.2041*0.223459*0.266701*pow((abs(w)*0.266701),2)) //copysign(_rotor_torque_coeff * w * w, w);
    return torque;
  }

  //apply torque given from powertrain data [equation of force is per rotor]
  double GazeboRosDroneForcePrivate::CalculateBodyTorque(double q) {
    double force = q/0.4475; //torque divided by the distance from the body center (see drone.xacro cylinder radius)
    return force;
  }

// Register this plugin with the simulator
GZ_REGISTER_MODEL_PLUGIN(GazeboRosDroneForce)
}  // namespace gazebo_plugins

import xacro

robot_description_config = xacro.process_file(urdf_file_path, mappings={'robot_name_arg': args.drone_name}) #remap link/joint namespaces in xacro file
robot_desc = robot_description_config.toxml() #convert xacro to xml string