Problem setting namespace for custom force plugin to multiple drones.

edit

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 "/motor_speed_cmd" with the prefix "/drone_1" for example. But, when I spawn 2 or more drones with only a single drone controller publishing to their respective topic (ie /drone_1/motor_speed_cmd), all the drones are being controlled (ie "/drone_2/motor_speed_cmd" and "/drone_3/motor_speed_cmd"). 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 "-robot_namespace" 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 "motor_speed_cmd" 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 ...