Change Topic Name of Created Plugin

asked 2018-03-29 03:34:10 -0500

matirc gravatar image

Hello there,

I created a light sensor plugin with the topic called : /light_sensor_plugin/light. When I launch one robot I do not find any issue. However, as this is a constant topic name, when launching several robots in their respective <group> tags, there is only one topic for the light sensor for all of them. This leads to make all of them to publish in that topic.

How can I change the topic name in my plugin so that it takes the prefix/name of each robot creating a different light sensor topic for each robot?

Here is my code: PLUGINS:

HEADER: enter code here


#include <string>
#include <iostream>
#include <stdio.h>
// library for processing camera data for gazebo / ros conversions
#include <gazebo/plugins/CameraPlugin.hh>

#include <gazebo_plugins/gazebo_ros_camera_utils.h>

namespace gazebo
  class GazeboRosLight : public CameraPlugin, GazeboRosCameraUtils
    /// \brief Constructor
    /// \param parent The parent entity, must be a Model or a Sensor
    public: GazeboRosLight();

    /// \brief Destructor
    public: ~GazeboRosLight();

    /// \brief Load the plugin
    /// \param take in SDF root element
    public: void Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf);

    /// \brief Update the controller
    protected: virtual void OnNewFrame(const unsigned char *_image,
    unsigned int _width, unsigned int _height,
    unsigned int _depth, const std::string &_format);

    ros::NodeHandle _nh;
    ros::Publisher _sensorPublisher;

    double _fov;
    double _range;

MAIN enter code here

#include <gazebo/common/Plugin.hh>
#include <ros/ros.h>
#include <gazebo_light_sensor_plugin/light_sensor_plugin.h>

#include <gazebo_plugins/gazebo_ros_camera.h>

#include <string>
#include <iostream>
#include <stdio.h>
#include <gazebo/sensors/Sensor.hh>
#include <gazebo/sensors/CameraSensor.hh>
#include <gazebo/sensors/SensorTypes.hh>

#include <sensor_msgs/Illuminance.h>

namespace gazebo
  // Register this plugin with the simulator

  // Constructor

    _sensorPublisher = _nh.advertise<sensor_msgs::Illuminance>("light", 1);

  // Destructor

  void GazeboRosLight::Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf)
    // Make sure the ROS node for Gazebo has already been initialized
    if (!ros::isInitialized())
      ROS_FATAL_STREAM("A ROS node for Gazebo has not been initialized, unable to load plugin. "
        << "Load the Gazebo system plugin '' in the gazebo_ros package)");

    CameraPlugin::Load(_parent, _sdf);
    // copying from CameraPlugin into GazeboRosCameraUtils
    this->parentSensor_ = this->parentSensor;
    this->width_ = this->width;
    this->height_ = this->height;
    this->depth_ = this->depth;
    this->format_ = this->format;
    this->camera_ = this->camera;

    GazeboRosCameraUtils::Load(_parent, _sdf);



  // Update the controller
  void GazeboRosLight::OnNewFrame(const unsigned char *_image,
    unsigned int _width, unsigned int _height, unsigned int _depth,
    const std::string &_format)
    static int seq=0;

    this->sensor_update_time_ = this->parentSensor_->LastUpdateTime();

    if (!this->parentSensor->IsActive())
      if ((*this->image_connect_count_) > 0)
      // do this first so there's chance for sensor to run once after activated
      if ((*this->image_connect_count_) > 0)
        common::Time cur_time = this->world_->SimTime();
        if (cur_time - this->last_update_time_ >= this->update_period_)
          this->last_update_time_ = cur_time;

          sensor_msgs::Illuminance msg;
          msg.header.stamp = ros::Time::now();
          msg.header.frame_id = "";
          msg.header.seq = seq;

          int startingPix = _width * ( (int)(_height/2) - (int)( _fov/2)) - (int)(_fov/2);

          double illum = 0;
          for (int i=0; i ...
edit retag flag offensive close merge delete