WifiSignalSubNode.cpp
Go to the documentation of this file.
1 /*
2 Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
3 All rights reserved.
4 
5 Redistribution and use in source and binary forms, with or without
6 modification, are permitted provided that the following conditions are met:
7  * Redistributions of source code must retain the above copyright
8  notice, this list of conditions and the following disclaimer.
9  * Redistributions in binary form must reproduce the above copyright
10  notice, this list of conditions and the following disclaimer in the
11  documentation and/or other materials provided with the distribution.
12  * Neither the name of the Universite de Sherbrooke nor the
13  names of its contributors may be used to endorse or promote products
14  derived from this software without specific prior written permission.
15 
16 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
17 ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
18 WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
19 DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
20 DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
21 (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
22 LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
23 ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
24 (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
25 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
26 */
27 
28 #include <ros/ros.h>
29 #include <ros/publisher.h>
30 #include <ros/subscriber.h>
31 
32 #include <rtabmap/utilite/UStl.h>
35 
36 #include <rtabmap_ros/MapData.h>
38 
39 #include <sensor_msgs/PointCloud2.h>
40 
41 #include <pcl/point_cloud.h>
42 #include <pcl/point_types.h>
44 
45 bool hueSymbol = false;
46 int min_dbm = -100;
47 int max_dbm = -50;
48 bool autoScale = false;
49 
50 // A percentage value that represents the signal quality
51 // of the network. WLAN_SIGNAL_QUALITY is of type ULONG.
52 // This member contains a value between 0 and 100. A value
53 // of 0 implies an actual RSSI signal strength of -100 dbm.
54 // A value of 100 implies an actual RSSI signal strength of -50 dbm.
55 // You can calculate the RSSI signal strength value for wlanSignalQuality
56 // values between 1 and 99 using linear interpolation.
57 inline int dBm2Quality(int dBm)
58 {
59  // dBm to Quality:
60  if(dBm <= min_dbm)
61  return 0;
62  else if(dBm >= max_dbm)
63  return 100;
64  else
65  {
66  return -(dBm-min_dbm)*100/(min_dbm-max_dbm);
67  }
68 }
69 
70 void HSVtoRGB( float *r, float *g, float *b, float h, float s, float v )
71 {
72  int i;
73  float f, p, q, t;
74  if( s == 0 ) {
75  // achromatic (grey)
76  *r = *g = *b = v;
77  return;
78  }
79  h /= 60; // sector 0 to 5
80  i = floor( h );
81  f = h - i; // factorial part of h
82  p = v * ( 1 - s );
83  q = v * ( 1 - s * f );
84  t = v * ( 1 - s * ( 1 - f ) );
85  switch( i ) {
86  case 0:
87  *r = v;
88  *g = t;
89  *b = p;
90  break;
91  case 1:
92  *r = q;
93  *g = v;
94  *b = p;
95  break;
96  case 2:
97  *r = p;
98  *g = v;
99  *b = t;
100  break;
101  case 3:
102  *r = p;
103  *g = q;
104  *b = v;
105  break;
106  case 4:
107  *r = t;
108  *g = p;
109  *b = v;
110  break;
111  default: // case 5:
112  *r = v;
113  *g = p;
114  *b = q;
115  break;
116  }
117 }
118 
120 std::map<double, int> wifiLevels;
121 std::map<double, int> nodeStamps_;
122 
123 void mapDataCallback(const rtabmap_ros::MapDataConstPtr & mapDataMsg)
124 {
125  ROS_INFO("Received map data!");
126 
127  rtabmap::Transform mapToOdom;
128  std::map<int, rtabmap::Transform> poses;
129  std::multimap<int, rtabmap::Link> links;
130  std::map<int, rtabmap::Signature> signatures;
131  rtabmap_ros::mapDataFromROS(*mapDataMsg, poses, links, signatures, mapToOdom);
132 
133  // handle the case where we can receive only latest data, or if all data are published
134  for(std::map<int, rtabmap::Signature>::iterator iter=signatures.begin(); iter!=signatures.end(); ++iter)
135  {
136  int id = iter->first;
137  rtabmap::Signature & node = iter->second;
138 
139  nodeStamps_.insert(std::make_pair(node.getStamp(), node.id()));
140 
142  {
144  wifiLevels.insert(std::make_pair(sensor.stamp()>0.0?sensor.stamp():iter->second.getStamp(), sensor.value()));
145  }
146  else if(!node.sensorData().userDataCompressed().empty())
147  {
148  cv::Mat data;
149  node.sensorData().uncompressDataConst(0 ,0, 0, &data);
150 
151  if(data.type() == CV_64FC1 && data.rows == 1 && data.cols == 2)
152  {
153  // format [int level, double stamp], see wifi_signal_pub_node.cpp
154  int level = data.at<double>(0);
155  double stamp = data.at<double>(1);
156  wifiLevels.insert(std::make_pair(stamp, level));
157  }
158  else if(!data.empty())
159  {
160  ROS_ERROR("Wrong user data format for wifi signal.");
161  }
162  }
163  }
164 
165  // for the logic below, we should keep only stamps for
166  // nodes still in the graph (in case nodes are ignored when not moving)
167  std::map<double, int> nodeStamps;
168  for(std::map<double, int>::iterator iter=nodeStamps_.begin(); iter!=nodeStamps_.end(); ++iter)
169  {
170  std::map<int, rtabmap::Transform>::const_iterator jter = poses.find(iter->second);
171  if(jter != poses.end())
172  {
173  nodeStamps.insert(*iter);
174  }
175  }
176 
177  if(wifiLevels.size() == 0)
178  {
179  ROS_WARN("No wifi signal detected yet in user data of map data");
180  }
181 
182  //============================
183  // Add WIFI symbols
184  //============================
185 
186  pcl::PointCloud<pcl::PointXYZRGB>::Ptr assembledWifiSignals(new pcl::PointCloud<pcl::PointXYZRGB>);
187  int id = 0;
188  float min=0,max=0;
189  for(std::map<double, int>::iterator iter=wifiLevels.begin(); iter!=wifiLevels.end(); ++iter, ++id)
190  {
191  if(min == 0.0f || min > iter->second)
192  {
193  min = iter->second;
194  }
195  if(max == 0.0f || max < iter->second)
196  {
197  max = iter->second;
198  }
199  }
200  ROS_INFO("Min/Max dBm = %f %f", min, max);
201  if(autoScale && min<0 && min < max)
202  {
203  min_dbm = min;
204  max_dbm = max;
205  }
206  for(std::map<double, int>::iterator iter=wifiLevels.begin(); iter!=wifiLevels.end(); ++iter, ++id)
207  {
208  // The Wifi value may be taken between two nodes, interpolate its position.
209  double stampWifi = iter->first;
210  std::map<double, int>::iterator previousNode = nodeStamps.lower_bound(stampWifi); // lower bound of the stamp
211  if(previousNode!=nodeStamps.end() && previousNode->first > stampWifi && previousNode != nodeStamps.begin())
212  {
213  --previousNode;
214  }
215  std::map<double, int>::iterator nextNode = nodeStamps.upper_bound(stampWifi); // upper bound of the stamp
216 
217  if(previousNode != nodeStamps.end() &&
218  nextNode != nodeStamps.end() &&
219  previousNode->second != nextNode->second &&
220  uContains(poses, previousNode->second) && uContains(poses, nextNode->second))
221  {
222  rtabmap::Transform poseA = poses.at(previousNode->second);
223  rtabmap::Transform poseB = poses.at(nextNode->second);
224  double stampA = previousNode->first;
225  double stampB = nextNode->first;
226  UASSERT(stampWifi>=stampA && stampWifi <=stampB);
227 
228  rtabmap::Transform v = poseA.inverse() * poseB;
229  double ratio = (stampWifi-stampA)/(stampB-stampA);
230 
231  v.x()*=ratio;
232  v.y()*=ratio;
233  v.z()*=ratio;
234 
235  rtabmap::Transform wifiPose = (poseA*v).translation(); // rip off the rotation
236 
237  pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
238  if(hueSymbol)
239  {
240  // scale between red -> yellow -> green
241  int quality = dBm2Quality(iter->second)*120/100;
242  float r,g,b;
243  HSVtoRGB(&r,&g,&b,quality,1,1);
244  pcl::PointXYZRGB anchor;
245  anchor.r = r*255;
246  anchor.g = g*255;
247  anchor.b = b*255;
248  cloud->push_back(anchor);
249  }
250  else
251  {
252 
253  // Make a line with points
254  int quality = dBm2Quality(iter->second)/10;
255  for(int i=0; i<10; ++i)
256  {
257  // 2 cm between each points
258  // the number of points depends on the dBm (which varies from -30 (near) to -80 (far))
259  pcl::PointXYZRGB pt;
260  pt.z = float(i+1)*0.02f;
261  if(i<quality)
262  {
263  // green
264  pt.g = 255;
265  if(i<7)
266  {
267  // yellow
268  pt.r = 255;
269  }
270  }
271  else
272  {
273  // gray
274  pt.r = pt.g = pt.b = 100;
275  }
276  cloud->push_back(pt);
277  }
278  pcl::PointXYZRGB anchor;
279  anchor.r = 255;
280  cloud->push_back(anchor);
281  }
282 
283  cloud = rtabmap::util3d::transformPointCloud(cloud, wifiPose);
284 
285  if(assembledWifiSignals->size() == 0)
286  {
287  *assembledWifiSignals = *cloud;
288  }
289  else
290  {
291  *assembledWifiSignals += *cloud;
292  }
293  }
294  }
295 
296  if(assembledWifiSignals->size())
297  {
298  sensor_msgs::PointCloud2 cloudMsg;
299  pcl::toROSMsg(*assembledWifiSignals, cloudMsg);
300  cloudMsg.header = mapDataMsg->header;
301  wifiSignalCloudPub.publish(cloudMsg);
302  }
303 }
304 
305 int main(int argc, char** argv)
306 {
307  ros::init(argc, argv, "wifi_signal_sub");
308 
309  ros::NodeHandle nh;
310  ros::NodeHandle pnh("~");
311 
312  pnh.param("hue_symbol", hueSymbol, hueSymbol);
313  pnh.param("min", min_dbm, min_dbm);
314  pnh.param("max", max_dbm, max_dbm);
315  pnh.param("auto", autoScale, autoScale);
316 
317  wifiSignalCloudPub = nh.advertise<sensor_msgs::PointCloud2>("wifi_signals", 1);
318  ros::Subscriber mapDataSub = nh.subscribe("/rtabmap/mapData", 1, mapDataCallback);
319 
320  ros::spin();
321 
322  return 0;
323 }
GLM_FUNC_DECL genType min(genType const &x, genType const &y)
pcl::PointCloud< pcl::PointXYZ >::Ptr RTABMAP_EXP transformPointCloud(const pcl::PointCloud< pcl::PointXYZ >::Ptr &cloud, const Transform &transform)
void HSVtoRGB(float *r, float *g, float *b, float h, float s, float v)
f
std::map< double, int > wifiLevels
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
data
void mapDataCallback(const rtabmap_ros::MapDataConstPtr &mapDataMsg)
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
#define ROS_WARN(...)
geometry_msgs::TransformStamped t
bool hueSymbol
#define UASSERT(condition)
bool param(const std::string &param_name, T &param_val, const T &default_val) const
void publish(const boost::shared_ptr< M > &message) const
void mapDataFromROS(const rtabmap_ros::MapData &msg, std::map< int, rtabmap::Transform > &poses, std::multimap< int, rtabmap::Link > &links, std::map< int, rtabmap::Signature > &signatures, rtabmap::Transform &mapToOdom)
GLM_FUNC_DECL genType floor(genType const &x)
#define ROS_INFO(...)
const EnvSensors & envSensors() const
translation
int min_dbm
std::map< double, int > nodeStamps_
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
ROSCPP_DECL void spin()
bool uContains(const std::list< V > &list, const V &value)
bool autoScale
GLM_FUNC_DECL genType max(genType const &x, genType const &y)
SensorData & sensorData()
int max_dbm
const cv::Mat & userDataCompressed() const
void toROSMsg(const sensor_msgs::PointCloud2 &cloud, sensor_msgs::Image &image)
const double & stamp() const
double getStamp() const
int main(int argc, char **argv)
ros::Publisher wifiSignalCloudPub
int dBm2Quality(int dBm)
r
#define ROS_ERROR(...)
const double & value() const
void uncompressDataConst(cv::Mat *imageRaw, cv::Mat *depthOrRightRaw, LaserScan *laserScanRaw=0, cv::Mat *userDataRaw=0, cv::Mat *groundCellsRaw=0, cv::Mat *obstacleCellsRaw=0, cv::Mat *emptyCellsRaw=0) const
Transform inverse() const
q


rtabmap_ros
Author(s): Mathieu Labbe
autogenerated on Tue Jan 24 2023 04:04:40