Find correct tag to publish a ros topic

asked 2019-10-01 09:29:27 -0600

Giaco94 gravatar image

Hi everyone. I create a wireless receiver plugin in order to obtain all the RSSI signals from the wifi transmitter. Here is my plugin:

#include "wifi_ros_plugin.h"

#include <gazebo/physics/physics.hh>
#include <gazebo/transport/transport.hh>
#include <std_msgs/Float64MultiArray.h>

using namespace std;
using namespace gazebo;
using namespace sensors;

    SensorPlugin ()

void WifiRosPlugin::Load (SensorPtr _sensor, sdf::ElementPtr _sdf)
    parentSensor = dynamic_pointer_cast<WirelessReceiver> (_sensor);
    parentLink = dynamic_pointer_cast<physics::Link> (physics::get_world ()->EntityByName (parentSensor->ParentName ()));

    if (!parentSensor) {
        gzerr << "WifiRosPlugin must be instantiate within a Wireless Transmitter Sensor.\n";

    // Init ROS
    if (!ros::isInitialized ()) {
        int argc = 0;
        char **argv = NULL;

        ros::init (argc, argv, "wireless_ros_plugin");

    rosNode = new ros::NodeHandle ("wireless_ros_plugin");
    wirelessPub = rosNode->advertise<std_msgs::Float64MultiArray> ("wireless_power", 1);

    updateConnection = parentSensor->ConnectUpdated (
                bind (&WifiRosPlugin::SensorUpdated, this));

void WifiRosPlugin::SensorUpdated ()
    std_msgs::Float64MultiArray powerMsg;
    vector<double> unorderedPowerVector, powerVector;
    vector<int> indexVector;

    // Find transmitters
    for (SensorPtr currSensor : SensorManager::Instance ()->GetSensors ()) {
        if (currSensor->Type () == "wireless_transmitter") {
            WirelessTransmitterPtr transmitter = dynamic_pointer_cast<WirelessTransmitter> (currSensor);
            int txIndex;
            double signalStrength;

            try {
                txIndex = stoi (transmitter->ESSID ());
            } catch (const invalid_argument &e) {
                gzerr << "ESSID must be numerical";

            signalStrength = transmitter->SignalStrength (parentLink->WorldPose (), parentSensor->Gain ());

            unorderedPowerVector.push_back (signalStrength);
            indexVector.push_back (txIndex);

    // Order power vector
    powerVector.resize (unorderedPowerVector.size ());
    for (int i = 0; i < powerVector.size (); i++)
        powerVector[i] = unorderedPowerVector[indexVector[i]];

    // Build msg
    powerMsg.layout.dim.resize (1);
    powerMsg.layout.dim[0].size = powerVector.size ();
    powerMsg.layout.dim[0].stride = powerVector.size ();
    powerMsg.layout.dim[0].label = "received_powers"; = powerVector;

    wirelessPub.publish (powerMsg);


Now i import my sensor plugin into an urdf file

  <gazebo reference="receiver">
    <sensor name="wirelessReceiver" type="wireless_receiver">
        <plugin name="receiver_plugin" filename=""/>

But when i launch my simulation i see my /wireless_power topic but it does not publish nothing.

When i try using an SDF file i can echo the topic and read all the informations.

So where is the <tag> i need to insert into the <sensor> </sensor> tag in order to publish correctly my topic?

Please help me!

Thanks in advance!

edit retag flag offensive close merge delete