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

rtabmap: added mapPath output topic

Browse Source
This commit is contained in:
matlabbe
2017-08-18 15:07:56 -04:00
parent 03a5d2e96f
commit 960d9718e9
3 changed files with 130 additions and 77 deletions
Download Patch File Download Diff File Expand all files Collapse all files
include/rtabmap_ros/CommonDataSubscriber.h
+1
View File
@@ -66,6 +66,7 @@ public:
bool isSubscribedToScan3d() const {return subscribedToScan3d_;}
bool isSubscribedToOdomInfo() const {return subscribedToOdomInfo_;}
bool isDataSubscribed() const {return isSubscribedToDepth() || isSubscribedToStereo() || isSubscribedToRGBD();}
int rgbdCameras() const {return isSubscribedToRGBD()?(int)rgbdSubs_.size():0;}
int getQueueSize() const {return queueSize_;}
bool isApproxSync() const {return approxSync_;}
include/rtabmap_ros/CoreWrapper.h
+1
View File
@@ -209,6 +209,7 @@ private:
ros::Publisher mapDataPub_;
ros::Publisher mapGraphPub_;
ros::Publisher labelsPub_;
ros::Publisher mapPathPub_;
//Planning stuff
ros::Subscriber goalSub_;
src/CoreWrapper.cpp
+128 -77
View File
@@ -196,6 +196,7 @@ void CoreWrapper::onInit()
mapDataPub_ = nh.advertise<rtabmap_ros::MapData>("mapData", 1);
mapGraphPub_ = nh.advertise<rtabmap_ros::MapGraph>("mapGraph", 1);
labelsPub_ = nh.advertise<visualization_msgs::MarkerArray>("labels", 1);
mapPathPub_ = nh.advertise<nav_msgs::Path>("mapPath", 1);
// planning topics
goalSub_ = nh.subscribe("goal", 1, &CoreWrapper::goalCallback, this);
@@ -1954,7 +1955,7 @@ bool CoreWrapper::publishMapCallback(rtabmap_ros::PublishMap::Request& req, rtab
if(mapDataPub_.getNumSubscribers() ||
(!req.graphOnly && mapsManager_.hasSubscribers()) ||
(req.graphOnly && labelsPub_.getNumSubscribers()))
(req.graphOnly && (labelsPub_.getNumSubscribers() || mapGraphPub_.getNumSubscribers() || mapPathPub_.getNumSubscribers())))
{
std::map<int, Transform> poses;
std::multimap<int, rtabmap::Link> constraints;
@@ -2033,11 +2034,19 @@ bool CoreWrapper::publishMapCallback(rtabmap_ros::PublishMap::Request& req, rtab
mapsManager_.publishMaps(filteredPoses, now, mapFrameId_);
}
if(labelsPub_.getNumSubscribers())
bool pubLabels = labelsPub_.getNumSubscribers();
bool pubPath = mapPathPub_.getNumSubscribers();
if(pubLabels || pubPath)
{
if(poses.size() && signatures.size())
{
visualization_msgs::MarkerArray markers;
nav_msgs::Path path;
if(pubPath)
{
path.poses.resize(poses.size());
}
int oi=0;
for(std::map<int, Signature>::const_iterator iter=signatures.begin();
iter!=signatures.end();
++iter)
@@ -2045,14 +2054,43 @@ bool CoreWrapper::publishMapCallback(rtabmap_ros::PublishMap::Request& req, rtab
std::map<int, Transform>::const_iterator poseIter= poses.find(iter->first);
if(poseIter!=poses.end())
{
// Add labels
if(!iter->second.getLabel().empty())
if(pubLabels)
{
// Add labels
if(!iter->second.getLabel().empty())
{
visualization_msgs::Marker marker;
marker.header.frame_id = mapFrameId_;
marker.header.stamp = now;
marker.ns = "labels";
marker.id = -iter->first;
marker.action = visualization_msgs::Marker::ADD;
marker.pose.position.x = poseIter->second.x();
marker.pose.position.y = poseIter->second.y();
marker.pose.position.z = poseIter->second.z();
marker.pose.orientation.x = 0.0;
marker.pose.orientation.y = 0.0;
marker.pose.orientation.z = 0.0;
marker.pose.orientation.w = 1.0;
marker.scale.x = 1;
marker.scale.y = 1;
marker.scale.z = 0.5;
marker.color.a = 0.7;
marker.color.r = 1.0;
marker.color.g = 0.0;
marker.color.b = 0.0;
marker.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
marker.text = iter->second.getLabel();
markers.markers.push_back(marker);
}
// Add node ids
visualization_msgs::Marker marker;
marker.header.frame_id = mapFrameId_;
marker.header.stamp = now;
marker.ns = "labels";
marker.id = -iter->first;
marker.ns = "ids";
marker.id = iter->first;
marker.action = visualization_msgs::Marker::ADD;
marker.pose.position.x = poseIter->second.x();
marker.pose.position.y = poseIter->second.y();
@@ -2063,51 +2101,39 @@ bool CoreWrapper::publishMapCallback(rtabmap_ros::PublishMap::Request& req, rtab
marker.pose.orientation.w = 1.0;
marker.scale.x = 1;
marker.scale.y = 1;
marker.scale.z = 0.5;
marker.color.a = 0.7;
marker.scale.z = 0.2;
marker.color.a = 0.5;
marker.color.r = 1.0;
marker.color.g = 0.0;
marker.color.b = 0.0;
marker.color.g = 1.0;
marker.color.b = 1.0;
marker.lifetime = ros::Duration(2.0f/rate_);
marker.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
marker.text = iter->second.getLabel();
marker.text = uNumber2Str(iter->first);
markers.markers.push_back(marker);
}
// Add node ids
visualization_msgs::Marker marker;
marker.header.frame_id = mapFrameId_;
marker.header.stamp = now;
marker.ns = "ids";
marker.id = iter->first;
marker.action = visualization_msgs::Marker::ADD;
marker.pose.position.x = poseIter->second.x();
marker.pose.position.y = poseIter->second.y();
marker.pose.position.z = poseIter->second.z();
marker.pose.orientation.x = 0.0;
marker.pose.orientation.y = 0.0;
marker.pose.orientation.z = 0.0;
marker.pose.orientation.w = 1.0;
marker.scale.x = 1;
marker.scale.y = 1;
marker.scale.z = 0.2;
marker.color.a = 0.5;
marker.color.r = 1.0;
marker.color.g = 1.0;
marker.color.b = 1.0;
marker.lifetime = ros::Duration(2.0f/rate_);
marker.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
marker.text = uNumber2Str(iter->first);
markers.markers.push_back(marker);
if(pubPath)
{
rtabmap_ros::transformToPoseMsg(poseIter->second, path.poses.at(oi).pose);
path.poses.at(oi).header.frame_id = mapFrameId_;
path.poses.at(oi).header.stamp = ros::Time(iter->second.getStamp());
++oi;
}
}
}
if(markers.markers.size())
if(pubLabels && markers.markers.size())
{
labelsPub_.publish(markers);
}
if(pubPath && oi)
{
path.header.frame_id = mapFrameId_;
path.header.stamp = now;
path.poses.resize(oi);
mapPathPub_.publish(path);
}
}
}
}
@@ -2244,11 +2270,19 @@ void CoreWrapper::publishStats(const ros::Time & stamp)
mapGraphPub_.publish(msg);
}
if(labelsPub_.getNumSubscribers())
bool pubLabels = labelsPub_.getNumSubscribers();
bool pubPath = mapPathPub_.getNumSubscribers();
if(pubLabels || pubPath)
{
if(stats.poses().size() && stats.getSignatures().size())
{
visualization_msgs::MarkerArray markers;
nav_msgs::Path path;
if(pubPath)
{
path.poses.resize(stats.poses().size());
}
int oi = 0;
for(std::map<int, Signature>::const_iterator iter=stats.getSignatures().begin();
iter!=stats.getSignatures().end();
++iter)
@@ -2256,14 +2290,43 @@ void CoreWrapper::publishStats(const ros::Time & stamp)
std::map<int, Transform>::const_iterator poseIter= stats.poses().find(iter->first);
if(poseIter!=stats.poses().end())
{
// Add labels
if(!iter->second.getLabel().empty())
if(pubLabels)
{
// Add labels
if(!iter->second.getLabel().empty())
{
visualization_msgs::Marker marker;
marker.header.frame_id = mapFrameId_;
marker.header.stamp = stamp;
marker.ns = "labels";
marker.id = -iter->first;
marker.action = visualization_msgs::Marker::ADD;
marker.pose.position.x = poseIter->second.x();
marker.pose.position.y = poseIter->second.y();
marker.pose.position.z = poseIter->second.z();
marker.pose.orientation.x = 0.0;
marker.pose.orientation.y = 0.0;
marker.pose.orientation.z = 0.0;
marker.pose.orientation.w = 1.0;
marker.scale.x = 1;
marker.scale.y = 1;
marker.scale.z = 0.5;
marker.color.a = 0.7;
marker.color.r = 1.0;
marker.color.g = 0.0;
marker.color.b = 0.0;
marker.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
marker.text = iter->second.getLabel();
markers.markers.push_back(marker);
}
// Add node ids
visualization_msgs::Marker marker;
marker.header.frame_id = mapFrameId_;
marker.header.stamp = stamp;
marker.ns = "labels";
marker.id = -iter->first;
marker.ns = "ids";
marker.id = iter->first;
marker.action = visualization_msgs::Marker::ADD;
marker.pose.position.x = poseIter->second.x();
marker.pose.position.y = poseIter->second.y();
@@ -2274,51 +2337,39 @@ void CoreWrapper::publishStats(const ros::Time & stamp)
marker.pose.orientation.w = 1.0;
marker.scale.x = 1;
marker.scale.y = 1;
marker.scale.z = 0.5;
marker.color.a = 0.7;
marker.scale.z = 0.2;
marker.color.a = 0.5;
marker.color.r = 1.0;
marker.color.g = 0.0;
marker.color.b = 0.0;
marker.color.g = 1.0;
marker.color.b = 1.0;
marker.lifetime = ros::Duration(2.0f/rate_);
marker.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
marker.text = iter->second.getLabel();
marker.text = uNumber2Str(iter->first);
markers.markers.push_back(marker);
}
// Add node ids
visualization_msgs::Marker marker;
marker.header.frame_id = mapFrameId_;
marker.header.stamp = stamp;
marker.ns = "ids";
marker.id = iter->first;
marker.action = visualization_msgs::Marker::ADD;
marker.pose.position.x = poseIter->second.x();
marker.pose.position.y = poseIter->second.y();
marker.pose.position.z = poseIter->second.z();
marker.pose.orientation.x = 0.0;
marker.pose.orientation.y = 0.0;
marker.pose.orientation.z = 0.0;
marker.pose.orientation.w = 1.0;
marker.scale.x = 1;
marker.scale.y = 1;
marker.scale.z = 0.2;
marker.color.a = 0.5;
marker.color.r = 1.0;
marker.color.g = 1.0;
marker.color.b = 1.0;
marker.lifetime = ros::Duration(2.0f/rate_);
marker.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
marker.text = uNumber2Str(iter->first);
markers.markers.push_back(marker);
if(pubPath)
{
rtabmap_ros::transformToPoseMsg(poseIter->second, path.poses.at(oi).pose);
path.poses.at(oi).header.frame_id = mapFrameId_;
path.poses.at(oi).header.stamp = ros::Time(iter->second.getStamp());
++oi;
}
}
}
if(markers.markers.size())
if(pubLabels && markers.markers.size())
{
labelsPub_.publish(markers);
}
if(pubPath && oi)
{
path.header.frame_id = mapFrameId_;
path.header.stamp = stamp;
path.poses.resize(oi);
mapPathPub_.publish(path);
}
}
}
}