Gazebo plugin's topics don't go into namespace
I wrote a plugin and attached it to a robot's urdf, it worked fine for a robot, but when I launch multiple robots within namespaces, the plugin's topics don't go into namespace, I met the same issue with d435i plugin, similar to this. This is my Load function in the plugin class:
void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf)
{
this->model = _model;
this->updateConnection = event::Events::ConnectWorldUpdateBegin(
std::bind(&CrbotSlotcarPlugin::OnUpdate, this));
ROS_WARN("Crbot Slotcar plugin has applied to %s", _model->GetName().c_str());
// Initialize ros, if it has not already bee initialized.
if (!ros::isInitialized())
{
int argc = 0;
char **argv = NULL;
ros::init(argc, argv, "slotcar", ros::init_options::NoSigintHandler);
}
// Create our ROS node. This acts in a similar manner to the Gazebo node
this->rosNode.reset(new ros::NodeHandle("slotcar"));
// Create a named topic, and subscribe to it.
ros::SubscribeOptions so = ros::SubscribeOptions::create<rmf_robot_sim_gazebo_plugins::Waypoint>("goal_pose", 1, boost::bind(&CrbotSlotcarPlugin::OnRosMsg, this, _1), ros::VoidPtr(), &this->rosQueue);
this->rosSub = this->rosNode->subscribe(so);
this->rosPub = this->rosNode->advertise<rmf_robot_sim_gazebo_plugins::RobotState>("robot_state",1);
// Spin up the queue helper thread.
this->rosQueueThread = std::thread(std::bind(&CrbotSlotcarPlugin::QueueThread, this));
This is how I setup launching mutiple robots:
<!-- BEGIN ROBOT 1-->
<group ns="robot1">
<param name="tf_prefix" value="robot1_tf" />
<include file="$(find warehouse_gazebo)/launch/one_robot.launch" >
<arg name="init_pose" value="-x 15 -y -6 -z 0" />
<arg name="robot_name" value="Robot1" />
</include>
</group>
<!-- BEGIN ROBOT 2-->
<group ns="robot2">
<param name="tf_prefix" value="robot2_tf" />
<include file="$(find crbot_warehouse_multi)/launch/one_robot.launch" >
<arg name="init_pose" value="-x -1 -y 1 -z 0" />
<arg name="robot_name" value="Robot2" />
</include>
</group>
Does anyone know how to solve this? Any help would be appreciated. Thanks in advance.