i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-10-04 00:37:46 +08:00
Code Issues Packages Projects Releases Wiki Activity

WifiSignalSubNode: added auto parameter

Browse Source
This commit is contained in:
matlabbe
2020-07-22 15:35:27 -04:00
parent d5182859a5
commit f318436647
1 changed files with 19 additions and 10 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/WifiSignalSubNode.cpp
+19 -10
View File
@@ -45,6 +45,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
bool hueSymbol = false;
int min_dbm = -100;
int max_dbm = -50;
bool autoScale = false;
// A percentage value that represents the signal quality
// of the network. WLAN_SIGNAL_QUALITY is of type ULONG.
@@ -186,6 +187,23 @@ void mapDataCallback(const rtabmap_ros::MapDataConstPtr & mapDataMsg)
int id = 0;
float min=0,max=0;
for(std::map<double, int>::iterator iter=wifiLevels.begin(); iter!=wifiLevels.end(); ++iter, ++id)
{
if(min == 0.0f || min > iter->second)
{
min = iter->second;
}
if(max == 0.0f || max < iter->second)
{
max = iter->second;
}
}
ROS_INFO("Min/Max dBm = %f %f", min, max);
if(autoScale && min<0 && min < max)
{
min_dbm = min;
max_dbm = max;
}
for(std::map<double, int>::iterator iter=wifiLevels.begin(); iter!=wifiLevels.end(); ++iter, ++id)
{
// The Wifi value may be taken between two nodes, interpolate its position.
double stampWifi = iter->first;
@@ -216,15 +234,6 @@ void mapDataCallback(const rtabmap_ros::MapDataConstPtr & mapDataMsg)
rtabmap::Transform wifiPose = (poseA*v).translation(); // rip off the rotation
if(min == 0.0f || min > iter->second)
{
min = iter->second;
}
if(max == 0.0f || max < iter->second)
{
max = iter->second;
}
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
if(hueSymbol)
{
@@ -279,7 +288,6 @@ void mapDataCallback(const rtabmap_ros::MapDataConstPtr & mapDataMsg)
}
}
}
ROS_INFO("Min/Max dBm = %f %f", min, max);
if(assembledWifiSignals->size())
{
@@ -300,6 +308,7 @@ int main(int argc, char** argv)
pnh.param("hue_symbol", hueSymbol, hueSymbol);
pnh.param("min", min_dbm, min_dbm);
pnh.param("max", max_dbm, max_dbm);
pnh.param("auto", autoScale, autoScale);
wifiSignalCloudPub = nh.advertise<sensor_msgs::PointCloud2>("wifi_signals", 1);
ros::Subscriber mapDataSub = nh.subscribe("/rtabmap/mapData", 1, mapDataCallback);