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

updated find-object with markers demo and wifi demo to correctly handle the case where only last signature data is published

Browse Source
This commit is contained in:
matlabbe
2018-11-29 14:42:25 -05:00
parent e08f49b092
commit 1559d9ba8e
3 changed files with 68 additions and 32 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/CoreWrapper.cpp
+7 -4
View File
@@ -2068,12 +2068,15 @@ void CoreWrapper::userDataAsyncCallback(const rtabmap_ros::UserDataConstPtr & da
if(!paused_)
{
UScopeMutex lock(userDataMutex_);
if(!userData_.empty())
static bool warningShow = false;
if(!userData_.empty() && !warningShow)
{
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).",
ROS_WARN("Overwriting previous user data set. When asynchronous user "
"data input topic rate is higher than "
"map update rate (current %s=%f), only latest data is saved "
"in the next node created. This message will is shown only once.",
Parameters::kRtabmapDetectionRate().c_str(), rate_);
warningShow = true;
}
userData_ = rtabmap_ros::userDataFromROS(*dataMsg);
}
src/SaveObjectsExample.cpp
+27 -12
View File
@@ -99,27 +99,41 @@ public:
// from rtabmap to rviz visualization
void mapDataCallback(const rtabmap_ros::MapDataConstPtr & msg)
{
std::map<double, int> nodeStamps; // <stamp, id>
std::map<int, rtabmap::Signature> signatures;
std::map<int, rtabmap::Transform> poses;
std::multimap<int, rtabmap::Link> links;
rtabmap::Transform mapToOdom;
rtabmap_ros::mapDataFromROS(*msg, poses, links, signatures, mapToOdom);
// handle the case where we can receive only latest data, or if all data are published
for(std::map<int, rtabmap::Signature>::iterator iter=signatures.begin(); iter!=signatures.end(); ++iter)
{
int id = iter->first;
rtabmap::Signature & node = iter->second;
if(!node.sensorData().userDataCompressed().empty() && nodeToObjects_.find(id)==nodeToObjects_.end())
{
cv::Mat data = rtabmap::uncompressData(node.sensorData().userDataCompressed());
ROS_ASSERT(data.cols == 9 && data.type() == CV_64FC1);
ROS_INFO("Node %d has %d object(s)", id, data.rows);
nodeToObjects_.insert(std::make_pair(id, data));
}
// Sort stamps by stamps->id
nodeStamps.insert(std::make_pair(iter->second.getStamp(), iter->first));
int id = iter->first;
rtabmap::Signature & node = iter->second;
nodeStamps_.insert(std::make_pair(node.getStamp(), node.id()));
if(!node.sensorData().userDataCompressed().empty() && nodeToObjects_.find(id)==nodeToObjects_.end())
{
cv::Mat data = rtabmap::uncompressData(node.sensorData().userDataCompressed());
ROS_ASSERT(data.cols == 9 && data.type() == CV_64FC1);
ROS_INFO("Node %d has %d object(s)", id, data.rows);
nodeToObjects_.insert(std::make_pair(id, data));
}
}
// for the logic below, we should keep only stamps for
// nodes still in the graph (in case nodes are ignored when not moving)
std::map<double, int> nodeStamps;
for(std::map<double, int>::iterator iter=nodeStamps_.begin(); iter!=nodeStamps_.end(); ++iter)
{
std::map<int, rtabmap::Transform>::const_iterator jter = poses.find(iter->second);
if(jter != poses.end())
{
nodeStamps.insert(*iter);
}
}
// Publish markers accordingly to current optimized graph
std::map<int, float> objectsAdded;
visualization_msgs::MarkerArray markers;
@@ -244,6 +258,7 @@ private:
ros::Publisher pub_;
ros::Publisher pubMarkers_;
std::map<int, cv::Mat> nodeToObjects_;
std::map<double, int> nodeStamps_; // <stamp, id>
std::string frameId_;
};
src/WifiSignalSubNode.cpp
+34 -16
View File
@@ -62,6 +62,7 @@ inline int dBm2Quality(int dBm)
ros::Publisher wifiSignalCloudPub;
std::map<double, int> wifiLevels;
std::map<double, int> nodeStamps_;
void mapDataCallback(const rtabmap_ros::MapDataConstPtr & mapDataMsg)
{
@@ -73,26 +74,43 @@ void mapDataCallback(const rtabmap_ros::MapDataConstPtr & mapDataMsg)
std::map<int, rtabmap::Signature> signatures;
rtabmap_ros::mapDataFromROS(*mapDataMsg, poses, links, signatures, mapToOdom);
std::map<double, int> nodeStamps; // <stamp, id>
// handle the case where we can receive only latest data, or if all data are published
for(std::map<int, rtabmap::Signature>::iterator iter=signatures.begin(); iter!=signatures.end(); ++iter)
{
cv::Mat data;
iter->second.sensorData().uncompressDataConst(0 ,0, 0, &data);
int id = iter->first;
rtabmap::Signature & node = iter->second;
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<double>(0);
double stamp = data.at<double>(1);
wifiLevels.insert(std::make_pair(stamp, level));
}
else if(!data.empty())
{
ROS_ERROR("Wrong user data format for wifi signal.");
}
nodeStamps_.insert(std::make_pair(node.getStamp(), node.id()));
// Sort stamps by stamps->id
nodeStamps.insert(std::make_pair(iter->second.getStamp(), iter->first));
if(!node.sensorData().userDataCompressed().empty())
{
cv::Mat data;
node.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<double>(0);
double stamp = data.at<double>(1);
wifiLevels.insert(std::make_pair(stamp, level));
}
else if(!data.empty())
{
ROS_ERROR("Wrong user data format for wifi signal.");
}
}
}
// for the logic below, we should keep only stamps for
// nodes still in the graph (in case nodes are ignored when not moving)
std::map<double, int> nodeStamps;
for(std::map<double, int>::iterator iter=nodeStamps_.begin(); iter!=nodeStamps_.end(); ++iter)
{
std::map<int, rtabmap::Transform>::const_iterator jter = poses.find(iter->second);
if(jter != poses.end())
{
nodeStamps.insert(*iter);
}
}
if(wifiLevels.size() == 0)