Accessing node namespace from model plugin

asked 2014-03-30 09:43:52 -0600

maeisen gravatar image

Hi,

I had a question regarding accessing the namespace of a node from the C++ code of a model plugin. I am launching multiple robots so in my launch file is:

   <!-- BEGIN ROBOT 1 -->
      <group ns="robot1">  
        <param name="tf_prefix" value="robot1_tf" />
        <param name="robotNamespace" value="rob1" />
        <include file="$(find rrbot_gazebo)/launch/one_robot.launch" >
          <arg name="init_pose" value="-x 1 -y 1 -z 0" />
          <arg name="robot_name"  value="Robot1" />
        </include>
      </group>


      <!-- BEGIN ROBOT 2-->
      <group ns="robot2">
        <param name="tf_prefix" value="robot2_tf" />
        <param name="robotNamespace" value="rob2" />
        <include file="$(find rrbot_gazebo)/launch/one_robot.launch" >
          <arg name="init_pose" value="-x -1 -y 1 -z 0" />
          <arg name="robot_name"  value="Robot2" />
        </include>
      </group>

I wish to access the parameter robotNamespace in the plugin so that each robot publishes topics such as "rob2/vars" and "rob1/vars". In my plugin, I am trying to do this like this:

   public: void Load(physics::ModelPtr _parent, sdf::ElementPtr /*_sdf*/)
    {
      // Store the pointer to the model
      this->model = _parent;

      // ROS Nodehandle
      this->node = new ros::NodeHandle("~");

      this->node->getParam("robotNamespace",own_s);
      std::string own_topic = own_s + "/vars";

      // ROS Publisher
      this->pub = this->node->advertise<std_msgs::Float64>(own_topic, 1000);

The problem is that when I create the NodeHandle in the private namespace, it always takes that to be the /gazebo namespace, regardless of the group namespace set in the launch file, so it cannot find the parameter "robotNamespace".

Is there a way to still either access the parameter robot1/robotNamespace or robot2/robotNamespace from the C++ code or initialize the NodeHandle to be in the group namespace?

Any help is greatly appreciated. Thanks!

edit retag flag offensive close merge delete