Find correct tag to publish a ros topic
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!