diff --git a/CMakeLists.txt b/CMakeLists.txt
index fecf15b3..03e0b11f 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -293,6 +293,11 @@ target_link_libraries(map_optimizer rtabmap_ros ${Libraries})
add_executable(map_assembler src/MapAssemblerNode.cpp)
target_link_libraries(map_assembler rtabmap_ros ${Libraries})
+add_executable(wifi_signal_pub src/WifiSignalPubNode.cpp)
+target_link_libraries(wifi_signal_pub rtabmap_ros ${Libraries})
+add_executable(wifi_signal_sub src/WifiSignalSubNode.cpp)
+target_link_libraries(wifi_signal_sub rtabmap_ros ${Libraries})
+
add_executable(camera src/CameraNode.cpp)
add_dependencies(camera ${${PROJECT_NAME}_EXPORTED_TARGETS})
target_link_libraries(camera ${Libraries})
diff --git a/include/rtabmap_ros/CoreWrapper.h b/include/rtabmap_ros/CoreWrapper.h
index b1f0be5d..b66edf36 100644
--- a/include/rtabmap_ros/CoreWrapper.h
+++ b/include/rtabmap_ros/CoreWrapper.h
@@ -115,6 +115,8 @@ private:
void defaultCallback(const sensor_msgs::ImageConstPtr & imageMsg); // no odom
+ void userDataAsyncCallback(const rtabmap_ros::UserDataConstPtr & dataMsg);
+
void goalCommonCallback(int id, const std::string & label, const rtabmap::Transform & pose, const ros::Time & stamp, double * planningTime = 0);
void goalCallback(const geometry_msgs::PoseStampedConstPtr & msg);
void goalNodeCallback(const rtabmap_ros::GoalConstPtr & msg);
@@ -252,6 +254,9 @@ private:
// for loop closure detection only
image_transport::Subscriber defaultSub_;
+ ros::Subscriber userDataAsyncSub_;
+ cv::Mat userData_;
+
bool stereoToDepth_;
bool odomSensorSync_;
float rate_;
diff --git a/launch/rtabmap.launch b/launch/rtabmap.launch
index 0e231dba..2d49bbaf 100644
--- a/launch/rtabmap.launch
+++ b/launch/rtabmap.launch
@@ -70,6 +70,9 @@
+
+
+
@@ -129,6 +132,7 @@
+
@@ -150,6 +154,8 @@
+
+
diff --git a/src/CoreWrapper.cpp b/src/CoreWrapper.cpp
index 4d614e66..3a082deb 100644
--- a/src/CoreWrapper.cpp
+++ b/src/CoreWrapper.cpp
@@ -501,6 +501,8 @@ void CoreWrapper::onInit()
NODELET_INFO("\n%s subscribed to:\n %s", getName().c_str(), defaultSub_.getTopic().c_str());
}
+
+ userDataAsyncSub_ = nh.subscribe("user_data_async", 1, &CoreWrapper::userDataAsyncCallback, this);
}
CoreWrapper::~CoreWrapper()
@@ -937,6 +939,16 @@ void CoreWrapper::commonDepthCallbackImpl(
if(userDataMsg.get())
{
userData = rtabmap_ros::userDataFromROS(*userDataMsg);
+ if(!userData_.empty())
+ {
+ ROS_WARN("Synchronized and asynchronized user data topics cannot be used at the same time. Async user data dropped!");
+ userData_ = cv::Mat();
+ }
+ }
+ else
+ {
+ userData = userData_;
+ userData_ = cv::Mat();
}
SensorData data(scan,
LaserScanInfo(
@@ -1119,6 +1131,9 @@ void CoreWrapper::commonStereoCallback(
}
}
+ cv::Mat userData = userData_;
+ userData_ = cv::Mat();
+
Transform groundTruthPose;
if(!groundTruthFrameId_.empty())
{
@@ -1134,7 +1149,8 @@ void CoreWrapper::commonStereoCallback(
right,
stereoModel,
lastPoseIntermediate_?-1:leftImageMsg->header.seq,
- rtabmap_ros::timestampFromROS(lastPoseStamp_));
+ rtabmap_ros::timestampFromROS(lastPoseStamp_),
+ userData);
data.setGroundTruth(groundTruthPose);
process(lastPoseStamp_,
@@ -1308,11 +1324,29 @@ void CoreWrapper::process(
NODELET_WARN("Ignoring received image because its sequence ID=0. Please "
"set \"Mem/GenerateIds\"=\"true\" to ignore ros generated sequence id. "
"Use only \"Mem/GenerateIds\"=\"false\" for once-time run of RTAB-Map and "
- "when you need to have IDs output of RTAB-map synchronised with the source "
+ "when you need to have IDs output of RTAB-map synchronized with the source "
"image sequence ID.");
}
}
+void CoreWrapper::userDataAsyncCallback(const rtabmap_ros::UserDataConstPtr & dataMsg)
+{
+ if(!paused_)
+ {
+ if(!userData_.empty())
+ {
+ ROS_WARN("Overwriting previous user data set. Asynchronous user "
+ "data input topic should be used with user data published at "
+ "lower rate than map update rate (current %s=%f).",
+ Parameters::kRtabmapDetectionRate().c_str(), rate_);
+ }
+ else
+ {
+ userData_ = rtabmap_ros::userDataFromROS(*dataMsg);
+ }
+ }
+}
+
void CoreWrapper::goalCommonCallback(
int id,
const std::string & label,
@@ -1527,6 +1561,7 @@ bool CoreWrapper::resetRtabmapCallback(std_srvs::Empty::Request&, std_srvs::Empt
latestNodeWasReached_ = false;
mapsManager_.clear();
previousStamp_ = ros::Time(0);
+ userData_ = cv::Mat();
return true;
}
@@ -1580,6 +1615,7 @@ bool CoreWrapper::backupDatabaseCallback(std_srvs::Empty::Request&, std_srvs::Em
lastPose_.setIdentity();
currentMetricGoal_.setNull();
latestNodeWasReached_ = false;
+ userData_ = cv::Mat();
NODELET_INFO("Backup: Saving \"%s\" to \"%s\"...", databasePath_.c_str(), (databasePath_+".back").c_str());
UFile::copy(databasePath_, databasePath_+".back");
diff --git a/src/WifiSignalPubNode.cpp b/src/WifiSignalPubNode.cpp
new file mode 100644
index 00000000..ae065278
--- /dev/null
+++ b/src/WifiSignalPubNode.cpp
@@ -0,0 +1,149 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+ * Redistributions of source code must retain the above copyright
+ notice, this list of conditions and the following disclaimer.
+ * Redistributions in binary form must reproduce the above copyright
+ notice, this list of conditions and the following disclaimer in the
+ documentation and/or other materials provided with the distribution.
+ * Neither the name of the Universite de Sherbrooke nor the
+ names of its contributors may be used to endorse or promote products
+ derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include
+#include
+
+#include
+#include
+#include
+
+#include
+#include
+
+// Demo:
+// $ roslaunch freenect_launch freenect.launch depth_registration:=true
+// $ roslaunch rtabmap_ros rtabmap.launch rtabmap_args:="--delete_db_on_start" user_data_async_topic:=/wifi_signal rtabmapviz:=false rviz:=true
+// $ rosrun rtabmap_ros wifi_signal_pub interface:="wlan0"
+// $ rosrun rtabmap_ros wifi_signal_sub
+// In RVIZ add PointCloud2 "wifi_signals"
+
+// A percentage value that represents the signal quality
+// of the network. WLAN_SIGNAL_QUALITY is of type ULONG.
+// This member contains a value between 0 and 100. A value
+// of 0 implies an actual RSSI signal strength of -100 dbm.
+// A value of 100 implies an actual RSSI signal strength of -50 dbm.
+// You can calculate the RSSI signal strength value for wlanSignalQuality
+// values between 1 and 99 using linear interpolation.
+inline int quality2dBm(int quality)
+{
+ // Quality to dBm:
+ if(quality <= 0)
+ return -100;
+ else if(quality >= 100)
+ return -50;
+ else
+ return (quality / 2) - 100;
+}
+
+int main(int argc, char** argv)
+{
+ ros::init(argc, argv, "wifi_signal_pub");
+
+ ros::NodeHandle nh;
+ ros::NodeHandle pnh("~");
+
+ std::string interface = "wlan0";
+ double rateHz = 0.5; // Hz
+ std::string frameId = "base_link";
+
+ pnh.param("interface", interface, interface);
+ pnh.param("rate", rateHz, rateHz);
+ pnh.param("frame_id", frameId, frameId);
+
+ ros::Rate rate(rateHz);
+
+ ros::Publisher wifiPub = nh.advertise("wifi_signal", 1);
+
+ while(ros::ok())
+ {
+ int dBm = 0;
+
+ // Code inspired from http://blog.ajhodges.com/2011/10/using-ioctl-to-gather-wifi-information.html
+
+ //have to use a socket for ioctl
+ int sockfd;
+ /* Any old socket will do, and a datagram socket is pretty cheap */
+ if((sockfd = socket(AF_INET, SOCK_DGRAM, 0)) == -1) {
+ ROS_ERROR("Could not create simple datagram socket");
+ return -1;
+ }
+
+ struct iwreq req;
+ struct iw_statistics stats;
+
+ strncpy(req.ifr_name, interface.c_str(), IFNAMSIZ);
+
+ //make room for the iw_statistics object
+ req.u.data.pointer = (caddr_t) &stats;
+ req.u.data.length = sizeof(stats);
+ // clear updated flag
+ req.u.data.flags = 1;
+
+ //this will gather the signal strength
+ if(ioctl(sockfd, SIOCGIWSTATS, &req) == -1)
+ {
+ //die with error, invalid interface
+ ROS_ERROR("Invalid interface (\"%s\"). Tip: Try with sudo!", interface.c_str());
+ }
+ else if(((iw_statistics *)req.u.data.pointer)->qual.updated & IW_QUAL_DBM)
+ {
+ //signal is measured in dBm and is valid for us to use
+ dBm = ((iw_statistics *)req.u.data.pointer)->qual.level - 256;
+ }
+ else
+ {
+ ROS_ERROR("Could not get signal level.");
+ }
+
+ close(sockfd);
+
+ if(dBm != 0)
+ {
+ ros::Time stamp = ros::Time::now();
+
+ // Create user data [level] with the value
+ cv::Mat data(1, 2, CV_64FC1);
+ data.at(0) = double(dBm);
+
+ // we should set stamp in data to be able to
+ // retrieve it from rtabmap map data to get precise
+ // position in the graph afterward
+ data.at(1) = stamp.toSec();
+
+ rtabmap_ros::UserData dataMsg;
+ dataMsg.header.frame_id = frameId;
+ dataMsg.header.stamp = stamp;
+ rtabmap_ros::userDataToROS(data, dataMsg, false);
+ wifiPub.publish(dataMsg);
+ }
+ ros::spinOnce();
+ rate.sleep();
+ }
+
+ return 0;
+}
diff --git a/src/WifiSignalSubNode.cpp b/src/WifiSignalSubNode.cpp
new file mode 100644
index 00000000..08e2420d
--- /dev/null
+++ b/src/WifiSignalSubNode.cpp
@@ -0,0 +1,207 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+ * Redistributions of source code must retain the above copyright
+ notice, this list of conditions and the following disclaimer.
+ * Redistributions in binary form must reproduce the above copyright
+ notice, this list of conditions and the following disclaimer in the
+ documentation and/or other materials provided with the distribution.
+ * Neither the name of the Universite de Sherbrooke nor the
+ names of its contributors may be used to endorse or promote products
+ derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include
+#include
+#include
+
+#include
+#include
+
+#include
+#include
+
+#include
+
+#include
+#include
+#include
+
+// A percentage value that represents the signal quality
+// of the network. WLAN_SIGNAL_QUALITY is of type ULONG.
+// This member contains a value between 0 and 100. A value
+// of 0 implies an actual RSSI signal strength of -100 dbm.
+// A value of 100 implies an actual RSSI signal strength of -50 dbm.
+// You can calculate the RSSI signal strength value for wlanSignalQuality
+// values between 1 and 99 using linear interpolation.
+inline int dBm2Quality(int dBm)
+{
+ // dBm to Quality:
+ if(dBm <= -100)
+ return 0;
+ else if(dBm >= -50)
+ return 100;
+ else
+ return 2 * (dBm + 100);
+}
+
+ros::Publisher wifiSignalCloudPub;
+std::map wifiLevels;
+
+void mapDataCallback(const rtabmap_ros::MapDataConstPtr & mapDataMsg)
+{
+ ROS_INFO("Received map data!");
+
+ rtabmap::Transform mapToOdom;
+ std::map poses;
+ std::multimap links;
+ std::map signatures;
+ rtabmap_ros::mapDataFromROS(*mapDataMsg, poses, links, signatures, mapToOdom);
+
+ std::map nodeStamps; //
+ for(std::map::iterator iter=signatures.begin(); iter!=signatures.end(); ++iter)
+ {
+ cv::Mat data;
+ iter->second.sensorData().uncompressDataConst(0 ,0, 0, &data);
+
+ if(data.type() == CV_64FC1 && data.rows == 1 && data.cols == 2)
+ {
+ // format [int level, double stamp], see wifi_signal_pub_node.cpp
+ int level = data.at(0);
+ double stamp = data.at(1);
+ wifiLevels.insert(std::make_pair(stamp, level));
+ }
+ else if(!data.empty())
+ {
+ ROS_ERROR("Wrong user data format for wifi signal.");
+ }
+
+ // Sort stamps by stamps->id
+ nodeStamps.insert(std::make_pair(iter->second.getStamp(), iter->first));
+ }
+
+ if(wifiLevels.size() == 0)
+ {
+ ROS_WARN("No wifi signal detected yet in user data of map data");
+ }
+
+ //============================
+ // Add WIFI symbols
+ //============================
+
+ pcl::PointCloud::Ptr assembledWifiSignals(new pcl::PointCloud);
+ int id = 0;
+ for(std::map::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;
+ std::map::iterator previousNode = nodeStamps.lower_bound(stampWifi); // lower bound of the stamp
+ if(previousNode!=nodeStamps.end() && previousNode->first > stampWifi && previousNode != nodeStamps.begin())
+ {
+ --previousNode;
+ }
+ std::map::iterator nextNode = nodeStamps.upper_bound(stampWifi); // upper bound of the stamp
+
+ if(previousNode != nodeStamps.end() &&
+ nextNode != nodeStamps.end() &&
+ previousNode->second != nextNode->second &&
+ uContains(poses, previousNode->second) && uContains(poses, nextNode->second))
+ {
+ rtabmap::Transform poseA = poses.at(previousNode->second);
+ rtabmap::Transform poseB = poses.at(nextNode->second);
+ double stampA = previousNode->first;
+ double stampB = nextNode->first;
+ UASSERT(stampWifi>=stampA && stampWifi <=stampB);
+
+ rtabmap::Transform v = poseA.inverse() * poseB;
+ double ratio = (stampWifi-stampA)/(stampB-stampA);
+
+ v.x()*=ratio;
+ v.y()*=ratio;
+ v.z()*=ratio;
+
+ rtabmap::Transform wifiPose = (poseA*v).translation(); // rip off the rotation
+
+ // Make a line with points
+ int quality = dBm2Quality(iter->second)/10;
+ pcl::PointCloud::Ptr cloud(new pcl::PointCloud);
+ for(int i=0; i<10; ++i)
+ {
+ // 2 cm between each points
+ // the number of points depends on the dBm (which varies from -30 (near) to -80 (far))
+ pcl::PointXYZRGB pt;
+ pt.z = float(i+1)*0.02f;
+ if(ipush_back(pt);
+ }
+ pcl::PointXYZRGB anchor(255, 0, 0);
+ cloud->push_back(anchor);
+
+ cloud = rtabmap::util3d::transformPointCloud(cloud, wifiPose);
+
+ if(assembledWifiSignals->size() == 0)
+ {
+ *assembledWifiSignals = *cloud;
+ }
+ else
+ {
+ *assembledWifiSignals += *cloud;
+ }
+ }
+ }
+
+ if(assembledWifiSignals->size())
+ {
+ sensor_msgs::PointCloud2 cloudMsg;
+ pcl::toROSMsg(*assembledWifiSignals, cloudMsg);
+ cloudMsg.header = mapDataMsg->header;
+ wifiSignalCloudPub.publish(cloudMsg);
+ }
+}
+
+int main(int argc, char** argv)
+{
+ ros::init(argc, argv, "wifi_signal_sub");
+
+ ros::NodeHandle nh;
+ ros::NodeHandle pnh("~");
+
+ std::string interface = "wlan0";
+ double rateHz = 0.5; // Hz
+ std::string frameId = "base_link";
+
+ wifiSignalCloudPub = nh.advertise("wifi_signals", 1);
+ ros::Subscriber mapDataSub = nh.subscribe("/rtabmap/mapData", 1, mapDataCallback);
+
+ ros::spin();
+
+ return 0;
+}