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

Updated ros-pkg for RTAB-Map 0.8.0

Browse Source 
Moved all nodelets and rviz plugins in "rtabmap_ros" namespace instead of "rtabmap"
Refactored rtabmap_ros messages (added convenient conversion methods in rtabmap_ros/MsgConversion.h)
Added noise filtering parameters for map_assembler node
Added variance parameter for map_optimizer node
Odometry nodes publish covariance matrices in odometry messages. Publish rtambap_ros::OdomInfo topic too.
This commit is contained in:
Mathieu Labbe
2014-12-14 16:44:13 -05:00
parent c91586ea57
commit dfe5cff0c0
74 changed files with 915 additions and 1088 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/rviz/InfoDisplay.cpp
+4 -4
View File
@@ -28,7 +28,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "InfoDisplay.h"
#include "rtabmap_ros/MsgConversion.h"
namespace rtabmap
namespace rtabmap_ros
{
InfoDisplay::InfoDisplay()
@@ -75,7 +75,7 @@ void InfoDisplay::processMessage( const rtabmap_ros::InfoConstPtr& msg )
{
info_ = "";
}
loopTransform_ = transformFromGeometryMsg(msg->loopClosureTransform);
loopTransform_ = rtabmap_ros::transformFromGeometryMsg(msg->loopClosureTransform);
}
this->emitTimeSignal(msg->header.stamp);
@@ -114,7 +114,7 @@ void InfoDisplay::reset()
}
}
} // namespace rtabmap
} // namespace rtabmap_ros
#include <pluginlib/class_list_macros.h>
PLUGINLIB_EXPORT_CLASS( rtabmap::InfoDisplay, rviz::Display )
PLUGINLIB_EXPORT_CLASS( rtabmap_ros::InfoDisplay, rviz::Display )
src/rviz/InfoDisplay.h
+3 -3
View File
@@ -33,7 +33,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <rviz/message_filter_display.h>
#include <rtabmap/core/Transform.h>
namespace rtabmap
namespace rtabmap_ros
{
class InfoDisplay: public rviz::MessageFilterDisplay<rtabmap_ros::Info>
@@ -60,10 +60,10 @@ private:
QString info_;
int globalCount_;
int localCount_;
Transform loopTransform_;
rtabmap::Transform loopTransform_;
boost::mutex info_mutex_;
};
} // namespace rtabmap
} // namespace rtabmap_ros
#endif
src/rviz/MapCloudDisplay.cpp
+19 -19
View File
@@ -55,13 +55,13 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <rtabmap_ros/GetMap.h>
namespace rtabmap
namespace rtabmap_ros
{
MapCloudDisplay::CloudInfo::CloudInfo() :
manager_(0),
pose_(Transform::getIdentity()),
pose_(rtabmap::Transform::getIdentity()),
scene_node_(0)
{}
@@ -258,8 +258,8 @@ void MapCloudDisplay::processMapData(const rtabmap_ros::MapData& map)
float cy = map.nodes[i].cy;
//uncompress data
util3d::CompressionThread ctImage(compressedMatFromBytes(map.nodes[i].image.bytes, false), true);
util3d::CompressionThread ctDepth(compressedMatFromBytes(map.nodes[i].depth.bytes, false), true);
rtabmap::util3d::CompressionThread ctImage(compressedMatFromBytes(map.nodes[i].image, false), true);
rtabmap::util3d::CompressionThread ctDepth(compressedMatFromBytes(map.nodes[i].depth, false), true);
ctImage.start();
ctDepth.start();
ctImage.join();
@@ -272,27 +272,27 @@ void MapCloudDisplay::processMapData(const rtabmap_ros::MapData& map)
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
if(depth.type() == CV_8UC1)
{
cloud = util3d::cloudFromStereoImages(image, depth, cx, cy, fx, fy, cloud_decimation_->getInt());
cloud = rtabmap::util3d::cloudFromStereoImages(image, depth, cx, cy, fx, fy, cloud_decimation_->getInt());
}
else
{
cloud = util3d::cloudFromDepthRGB(image, depth, cx, cy, fx, fy, cloud_decimation_->getInt());
cloud = rtabmap::util3d::cloudFromDepthRGB(image, depth, cx, cy, fx, fy, cloud_decimation_->getInt());
}
if(cloud_max_depth_->getFloat() > 0.0f)
{
cloud = util3d::passThrough<pcl::PointXYZRGB>(cloud, "z", 0, cloud_max_depth_->getFloat());
cloud = rtabmap::util3d::passThrough<pcl::PointXYZRGB>(cloud, "z", 0, cloud_max_depth_->getFloat());
}
if(cloud_voxel_size_->getFloat() > 0.0f)
{
cloud = util3d::voxelize<pcl::PointXYZRGB>(cloud, cloud_voxel_size_->getFloat());
cloud = rtabmap::util3d::voxelize<pcl::PointXYZRGB>(cloud, cloud_voxel_size_->getFloat());
}
cloud = util3d::transformPointCloud<pcl::PointXYZRGB>(cloud, localTransform);
cloud = rtabmap::util3d::transformPointCloud<pcl::PointXYZRGB>(cloud, localTransform);
// do it after local transform
if(cloud_filter_floor_height_->getFloat() > 0.0f)
{
cloud = util3d::passThrough<pcl::PointXYZRGB>(cloud, "z", cloud_filter_floor_height_->getFloat(), 999.0f);
cloud = rtabmap::util3d::passThrough<pcl::PointXYZRGB>(cloud, "z", cloud_filter_floor_height_->getFloat(), 999.0f);
}
sensor_msgs::PointCloud2::Ptr cloudMsg(new sensor_msgs::PointCloud2);
@@ -315,15 +315,15 @@ void MapCloudDisplay::processMapData(const rtabmap_ros::MapData& map)
}
// Update graph
std::map<int, Transform> poses;
for(unsigned int i=0; i<map.poseIDs.size() && i<map.poses.size(); ++i)
std::map<int, rtabmap::Transform> poses;
for(unsigned int i=0; i<map.graph.nodeIds.size() && i<map.graph.poses.size(); ++i)
{
poses.insert(std::make_pair(map.poseIDs[i], transformFromPoseMsg(map.poses[i])));
poses.insert(std::make_pair(map.graph.nodeIds[i], rtabmap_ros::transformFromPoseMsg(map.graph.poses[i])));
}
if(node_filtering_angle_->getFloat() > 0.0f && node_filtering_radius_->getFloat() > 0.0f)
{
poses = util3d::radiusPosesFiltering(poses,
poses = rtabmap::util3d::radiusPosesFiltering(poses,
node_filtering_radius_->getFloat(),
node_filtering_angle_->getFloat()*CV_PI/180.0);
}
@@ -499,12 +499,12 @@ void MapCloudDisplay::downloadMap()
else
{
messageBox->setText(tr("Creating all clouds (%1 poses and %2 clouds downloaded)...")
.arg(getMapSrv.response.data.poses.size()).arg(getMapSrv.response.data.nodes.size()));
.arg(getMapSrv.response.data.graph.poses.size()).arg(getMapSrv.response.data.nodes.size()));
QApplication::processEvents();
this->reset();
processMapData(getMapSrv.response.data);
messageBox->setText(tr("Creating all clouds (%1 poses and %2 clouds downloaded)... done!")
.arg(getMapSrv.response.data.poses.size()).arg(getMapSrv.response.data.nodes.size()));
.arg(getMapSrv.response.data.graph.poses.size()).arg(getMapSrv.response.data.nodes.size()));
QTimer::singleShot(1000, messageBox, SLOT(close()));
}
@@ -556,10 +556,10 @@ void MapCloudDisplay::downloadGraph()
}
else
{
messageBox->setText(tr("Updating the map (%1 nodes downloaded)...").arg(getMapSrv.response.data.poses.size()));
messageBox->setText(tr("Updating the map (%1 nodes downloaded)...").arg(getMapSrv.response.data.graph.poses.size()));
QApplication::processEvents();
processMapData(getMapSrv.response.data);
messageBox->setText(tr("Updating the map (%1 nodes downloaded)... done!").arg(getMapSrv.response.data.poses.size()));
messageBox->setText(tr("Updating the map (%1 nodes downloaded)... done!").arg(getMapSrv.response.data.graph.poses.size()));
QTimer::singleShot(1000, messageBox, SLOT(close()));
}
@@ -885,4 +885,4 @@ bool MapCloudDisplay::transformCloud(const CloudInfoPtr& cloud_info, bool update
} // namespace rtabmap
#include <pluginlib/class_list_macros.h>
PLUGINLIB_EXPORT_CLASS( rtabmap::MapCloudDisplay, rviz::Display )
PLUGINLIB_EXPORT_CLASS( rtabmap_ros::MapCloudDisplay, rviz::Display )
src/rviz/MapCloudDisplay.h
+2 -2
View File
@@ -54,7 +54,7 @@ typedef std::vector<std::string> V_string;
using namespace rviz;
namespace rtabmap
namespace rtabmap_ros
{
class PointCloudCommon;
@@ -185,6 +185,6 @@ private:
pluginlib::ClassLoader<rviz::PointCloudTransformer>* transformer_class_loader_;
};
} // namespace rtabmap
} // namespace rtabmap_ros
#endif
src/rviz/MapGraphDisplay.cpp
+9 -9
View File
@@ -43,7 +43,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "MapGraphDisplay.h"
namespace rtabmap
namespace rtabmap_ros
{
MapGraphDisplay::MapGraphDisplay()
@@ -84,22 +84,22 @@ void MapGraphDisplay::destroyObjects()
void MapGraphDisplay::processMessage( const rtabmap_ros::MapData::ConstPtr& msg )
{
if(!(msg->maps.size() == msg->poseIDs.size() && msg->poses.size() == msg->poseIDs.size()))
if(!(msg->graph.mapIds.size() == msg->graph.nodeIds.size() && msg->graph.poses.size() == msg->graph.nodeIds.size()))
{
ROS_ERROR("rtambap::MapGraph: Error map ids, pose ids and poses must have all the same size.");
ROS_ERROR("rtabmap_ros::MapGraph: Error map ids, pose ids and poses must have all the same size.");
return;
}
// Find all graphs
std::map<int, std::map<int, geometry_msgs::Point> > graphs;
for(unsigned int i=0; i<msg->poses.size(); ++i)
for(unsigned int i=0; i<msg->graph.poses.size(); ++i)
{
std::map<int, std::map<int, geometry_msgs::Point> >::iterator iter = graphs.find(msg->maps[i]);
std::map<int, std::map<int, geometry_msgs::Point> >::iterator iter = graphs.find(msg->graph.mapIds[i]);
if(iter == graphs.end())
{
iter = graphs.insert(std::make_pair(msg->maps[i], std::map<int, geometry_msgs::Point>())).first;
iter = graphs.insert(std::make_pair(msg->graph.mapIds[i], std::map<int, geometry_msgs::Point>())).first;
}
iter->second.insert(std::make_pair(msg->poseIDs[i], msg->poses[i].position));
iter->second.insert(std::make_pair(msg->graph.nodeIds[i], msg->graph.poses[i].position));
}
destroyObjects();
@@ -142,7 +142,7 @@ void MapGraphDisplay::processMessage( const rtabmap_ros::MapData::ConstPtr& msg
}
}
} // namespace rviz
} // namespace rtabmap_ros
#include <pluginlib/class_list_macros.h>
PLUGINLIB_EXPORT_CLASS( rtabmap::MapGraphDisplay, rviz::Display )
PLUGINLIB_EXPORT_CLASS( rtabmap_ros::MapGraphDisplay, rviz::Display )
src/rviz/MapGraphDisplay.h
+5 -5
View File
@@ -26,8 +26,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#ifndef RVIZ_PATH_DISPLAY_H
#define RVIZ_PATH_DISPLAY_H
#ifndef MAP_GRAPH_DISPLAY_H
#define MAP_GRAPH_DISPLAY_H
#include <rtabmap_ros/MapData.h>
@@ -46,7 +46,7 @@ class FloatProperty;
using namespace rviz;
namespace rtabmap
namespace rtabmap_ros
{
/**
@@ -79,7 +79,7 @@ private:
FloatProperty* alpha_property_;
};
} // namespace rviz
} // namespace rtabmap_ros
#endif /* RVIZ_PATH_DISPLAY_H */
#endif /* MAP_GRAPH_DISPLAY_H */
src/rviz/OrbitOrientedViewController.cpp
+2 -2
View File
@@ -38,7 +38,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "rviz/properties/vector_property.h"
#include "rviz/ogre_helpers/shape.h"
namespace rtabmap
namespace rtabmap_ros
{
void OrbitOrientedViewController::updateCamera()
@@ -73,4 +73,4 @@ void OrbitOrientedViewController::updateCamera()
}
#include <pluginlib/class_list_macros.h>
PLUGINLIB_EXPORT_CLASS( rtabmap::OrbitOrientedViewController, rviz::ViewController )
PLUGINLIB_EXPORT_CLASS( rtabmap_ros::OrbitOrientedViewController, rviz::ViewController )
src/rviz/OrbitOrientedViewController.h
+1 -1
View File
@@ -30,7 +30,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "rviz/default_plugin/view_controllers/orbit_view_controller.h"
namespace rtabmap
namespace rtabmap_ros
{
class OrbitOrientedViewController: public rviz::OrbitViewController