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

Increased minimum rtabmap version to 0.20.7. Updated OdomInfo msg with localScanMapFormat (fixed intensity channel wrongly converted to rgb). rtabmap.launch: Show in rviz filtered scan from odometry if icp_odometry is true. rtabmapviz: fixed hanging shutdown after a ctr-c. OdometryROS: can now publish local scan map with intensity channel. icp_odometry: fixed intensity ignored if voxel size is set.

Browse Source
This commit is contained in:
matlabbe
2020-11-28 17:28:45 -05:00
parent 371a826109
commit ada3e7082c
7 changed files with 64 additions and 33 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/GuiNode.cpp
+2 -1
View File
@@ -38,6 +38,7 @@ ros::AsyncSpinner * spinner = 0;
void my_handler(int s){
ROS_INFO("rtabmapviz: ctrl-c catched! Exiting Qt app...");
ros::shutdown();
app->exit(-1);
}
@@ -72,9 +73,9 @@ int main(int argc, char** argv)
int r = app->exec();// MUST be called by the Main Thread
ROS_INFO("rtabmapviz stopping spinner...");
spinner->stop();
delete spinner;
ROS_INFO("rtabmapviz deleting qt stuff...");
delete gui;
delete app;
ROS_INFO("rtabmapviz: All done! Closing...");
src/MsgConversion.cpp
+2 -2
View File
@@ -1437,8 +1437,7 @@ rtabmap::OdometryInfo odomInfoFromROS(const rtabmap_ros::OdomInfo & msg)
info.localMap.insert(std::make_pair(msg.localMapKeys[i], point3fFromROS(msg.localMapValues[i])));
}
info.localScanMap = rtabmap::LaserScan::backwardCompatibility(rtabmap::uncompressData(msg.localScanMap));
info.localScanMap = rtabmap::LaserScan(rtabmap::uncompressData(msg.localScanMap), 0, 0, (rtabmap::LaserScan::Format)msg.localScanMapFormat);
return info;
}
@@ -1495,6 +1494,7 @@ void odomInfoToROS(const rtabmap::OdometryInfo & info, rtabmap_ros::OdomInfo & m
points3fToROS(uValues(info.localMap), msg.localMapValues);
msg.localScanMap = rtabmap::compressData(rtabmap::util3d::transformLaserScan(info.localScanMap, info.localScanMap.localTransform()).data());
msg.localScanMapFormat = info.localScanMap.format();
}
cv::Mat userDataFromROS(const rtabmap_ros::UserData & dataMsg)
src/OdometryROS.cpp
+11 -1
View File
@@ -823,11 +823,21 @@ void OdometryROS::processData(const SensorData & data, const ros::Time & stamp,
if(odomLocalScanMap_.getNumSubscribers() && !info.localScanMap.isEmpty())
{
sensor_msgs::PointCloud2 cloudMsg;
if(info.localScanMap.hasNormals())
if(info.localScanMap.hasNormals() && info.localScanMap.hasIntensity())
{
pcl::PointCloud<pcl::PointXYZINormal>::Ptr cloud = util3d::laserScanToPointCloudINormal(info.localScanMap, info.localScanMap.localTransform());
pcl::toROSMsg(*cloud, cloudMsg);
}
else if(info.localScanMap.hasNormals())
{
pcl::PointCloud<pcl::PointNormal>::Ptr cloud = util3d::laserScanToPointCloudNormal(info.localScanMap, info.localScanMap.localTransform());
pcl::toROSMsg(*cloud, cloudMsg);
}
else if(info.localScanMap.hasIntensity())
{
pcl::PointCloud<pcl::PointXYZI>::Ptr cloud = util3d::laserScanToPointCloudI(info.localScanMap, info.localScanMap.localTransform());
pcl::toROSMsg(*cloud, cloudMsg);
}
else
{
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = util3d::laserScanToPointCloud(info.localScanMap, info.localScanMap.localTransform());
src/nodelets/icp_odometry.cpp
+43 -25
View File
@@ -67,6 +67,7 @@ public:
scanVoxelSize_(0.0),
scanNormalK_(0),
scanNormalRadius_(0.0),
scanNormalGroundUp_(0.0),
plugin_loader_("rtabmap_ros", "rtabmap_ros::PluginInterface"),
scanReceived_(false),
cloudReceived_(false)
@@ -87,10 +88,12 @@ private:
pnh.param("scan_cloud_max_points", scanCloudMaxPoints_, scanCloudMaxPoints_);
pnh.param("scan_downsampling_step", scanDownsamplingStep_, scanDownsamplingStep_);
pnh.param("scan_range_min", scanRangeMin_, scanRangeMin_);
pnh.param("scan_range_max", scanRangeMax_, scanRangeMax_);
pnh.param("scan_voxel_size", scanVoxelSize_, scanVoxelSize_);
pnh.param("scan_normal_k", scanNormalK_, scanNormalK_);
pnh.param("scan_range_min", scanRangeMin_, scanRangeMin_);
pnh.param("scan_range_max", scanRangeMax_, scanRangeMax_);
pnh.param("scan_voxel_size", scanVoxelSize_, scanVoxelSize_);
pnh.param("scan_normal_k", scanNormalK_, scanNormalK_);
pnh.param("scan_normal_radius", scanNormalRadius_, scanNormalRadius_);
pnh.param("scan_normal_ground_up", scanNormalGroundUp_, scanNormalGroundUp_);
if (pnh.hasParam("plugins"))
{
@@ -127,7 +130,6 @@ private:
"The value is still used. Use \"scan_normal_k\" to avoid this warning.");
pnh.param("scan_cloud_normal_k", scanNormalK_, scanNormalK_);
}
pnh.param("scan_normal_radius", scanNormalRadius_, scanNormalRadius_);
NODELET_INFO("IcpOdometry: scan_cloud_max_points = %d", scanCloudMaxPoints_);
NODELET_INFO("IcpOdometry: scan_downsampling_step = %d", scanDownsamplingStep_);
@@ -136,6 +138,7 @@ private:
NODELET_INFO("IcpOdometry: scan_voxel_size = %f m", scanVoxelSize_);
NODELET_INFO("IcpOdometry: scan_normal_k = %d", scanNormalK_);
NODELET_INFO("IcpOdometry: scan_normal_radius = %f m", scanNormalRadius_);
NODELET_INFO("IcpOdometry: scan_normal_ground_up = %f", scanNormalGroundUp_);
scan_sub_ = nh.subscribe("scan", 1, &ICPOdometry::callbackScan, this);
cloud_sub_ = nh.subscribe("scan_cloud", 1, &ICPOdometry::callbackCloud, this);
@@ -252,6 +255,19 @@ private:
}
}
}
iter = parameters.find(Parameters::kIcpPointToPlaneGroundNormalsUp());
if(iter != parameters.end())
{
float value = uStr2Float(iter->second);
if(value != 0.0f)
{
if(!pnh.hasParam("scan_normal_ground_up"))
{
ROS_WARN("IcpOdometry: Transferring value %s of \"%s\" to ros parameter \"scan_normal_ground_up\" for convenience.", iter->second.c_str(), iter->first.c_str());
scanNormalGroundUp_ = value;
}
}
}
}
void callbackScan(const sensor_msgs::LaserScanConstPtr& scanMsg)
@@ -497,37 +513,33 @@ private:
}
else
{
cloudMsg = *pointCloudMsg;
cloudMsg = *pointCloudMsg;
}
cv::Mat scan;
bool hasNormals = false;
bool hasIntensity = false;
if(scanVoxelSize_ == 0.0f)
for(unsigned int i=0; i<cloudMsg.fields.size(); ++i)
{
for(unsigned int i=0; i<cloudMsg.fields.size(); ++i)
if(scanVoxelSize_ == 0.0f && cloudMsg.fields[i].name.compare("normal_x") == 0)
{
if(cloudMsg.fields[i].name.compare("normal_x") == 0)
hasNormals = true;
}
if(cloudMsg.fields[i].name.compare("intensity") == 0)
{
if(cloudMsg.fields[i].datatype == sensor_msgs::PointField::FLOAT32)
{
hasNormals = true;
break;
hasIntensity = true;
}
if(cloudMsg.fields[i].name.compare("intensity") == 0)
else
{
if(cloudMsg.fields[i].datatype == sensor_msgs::PointField::FLOAT32)
static bool warningShown = false;
if(!warningShown)
{
hasIntensity = true;
}
else
{
static bool warningShown = false;
if(!warningShown)
{
ROS_WARN("The input scan cloud has an \"intensity\" field "
"but the datatype (%d) is not supported. Intensity will be ignored. "
"This message is only shown once.", cloudMsg.fields[i].datatype);
warningShown = true;
}
ROS_WARN("The input scan cloud has an \"intensity\" field "
"but the datatype (%d) is not supported. Intensity will be ignored. "
"This message is only shown once.", cloudMsg.fields[i].datatype);
warningShown = true;
}
}
}
@@ -554,6 +566,7 @@ private:
scanCloudMaxPoints_ = cloudMsg.width *cloudMsg.height;
}
int maxLaserScans = scanCloudMaxPoints_;
if(hasNormals && hasIntensity)
{
pcl::PointCloud<pcl::PointXYZINormal>::Ptr pclScan(new pcl::PointCloud<pcl::PointXYZINormal>);
@@ -734,6 +747,10 @@ private:
{
laserScan = util3d::rangeFiltering(laserScan, scanRangeMin_, scanRangeMax_);
}
if(!laserScan.isEmpty() && laserScan.hasNormals() && !laserScan.is2d() && scanNormalGroundUp_)
{
laserScan = util3d::adjustNormalsToViewPoint(laserScan, Eigen::Vector3f(0,0,10), (float)scanNormalGroundUp_);
}
rtabmap::SensorData data(
laserScan,
@@ -763,6 +780,7 @@ private:
double scanVoxelSize_;
int scanNormalK_;
double scanNormalRadius_;
double scanNormalGroundUp_;
std::vector<boost::shared_ptr<rtabmap_ros::PluginInterface> > plugins_;
pluginlib::ClassLoader<rtabmap_ros::PluginInterface> plugin_loader_;
bool scanReceived_ = false;