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

Added planner node

Browse Source 
Added publishing of laserscan data with data_player
Updated for latest changes of the library (some methods moved from util3d)
rtabmapviz not saving GUI config on ctrl-c
This commit is contained in:
Mathieu Labbe
2015-01-23 11:19:19 -05:00
parent 30e3b94eb0
commit 92c30adad8
13 changed files with 441 additions and 27 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/DbPlayerNode.cpp
+18 -1
View File
@@ -30,6 +30,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <sensor_msgs/image_encodings.h>
#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/CameraInfo.h>
#include <pcl_conversions/pcl_conversions.h>
#include <nav_msgs/Odometry.h>
#include <cv_bridge/cv_bridge.h>
#include <image_transport/image_transport.h>
@@ -38,7 +39,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <std_srvs/Empty.h>
#include <rtabmap_ros/MsgConversion.h>
#include <rtabmap/utilite/ULogger.h>
#include <rtabmap/core/util3d.h>
#include <rtabmap/core/DBReader.h>
bool paused = false;
@@ -133,6 +134,7 @@ int main(int argc, char** argv)
ros::Publisher leftCamInfoPub;
ros::Publisher rightCamInfoPub;
ros::Publisher odometryPub;
ros::Publisher scanPub;
tf::TransformBroadcaster tfBroadcaster;
rtabmap::SensorData data = reader.getNextData();
@@ -216,6 +218,11 @@ int main(int argc, char** argv)
camInfoB.height = data.depthOrRightImage().rows;
camInfoB.width = data.depthOrRightImage().cols;
if(!data.laserScan().empty())
{
if(scanPub.getTopic().empty()) scanPub = nh.advertise<sensor_msgs::PointCloud2>("scan_cloud", 1);
}
// publish transforms first
if(publishTf)
{
@@ -339,6 +346,16 @@ int main(int argc, char** argv)
rightCamInfoPub.publish(camInfoB);
}
if(scanPub.getNumSubscribers() && !data.laserScan().empty())
{
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = rtabmap::util3d::laserScanToPointCloud(data.laserScan());
sensor_msgs::PointCloud2 msg;
pcl::toROSMsg(*cloud, msg);
msg.header.frame_id = frameId;
msg.header.stamp = time;
scanPub.publish(msg);
}
ros::spinOnce();
while(ros::ok() && paused)