Find correct tag to publish a ros topic

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

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;

WifiRosPlugin::WifiRosPlugin():
    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";
        return;
    }

    // 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";
                return;
            }

            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";

    powerMsg.data = powerVector;

    wirelessPub.publish (powerMsg);
}

GZ_REGISTER_SENSOR_PLUGIN(WifiRosPlugin)

Now i import my sensor plugin into an urdf file

<!-- GAZEBO PLUGIN WIRELESS RECEIVER -->
  <gazebo reference="receiver">
    <sensor name="wirelessReceiver" type="wireless_receiver">
        <plugin name="receiver_plugin" filename="libwifi_ros_plugin.so"/>
        <always_on>1</always_on>
        <update_rate>1</update_rate>
        <visualize>true</visualize>
        <transceiver>
          <min_frequency>2412.0</min_frequency>
          <max_frequency>2484.0</max_frequency>
          <power>14.5</power>
          <gain>2.5</gain>
          <sensitivity>-90.0</sensitivity>
        </transceiver>
      </sensor>
  </gazebo>

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