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

merged master->ros2

Browse Source
This commit is contained in:
matlabbe
2023-04-09 12:32:43 -07:00
parent a430f02cdc fc9ca0e40d
commit 822ba11ef7
17 changed files with 53 additions and 54 deletions
Download Patch File Download Diff File Expand all files Collapse all files
rtabmap_odom/package.xml
+1 -1
View File
@@ -2,7 +2,7 @@
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>rtabmap_odom</name>
<version>0.21.0</version>
<version>0.21.1</version>
<description>RTAB-Map's odometry package.</description>
<maintainer email="[email protected]">Mathieu Labbe</maintainer>
<author>Mathieu Labbe</author>
rtabmap_odom/src/nodelets/icp_odometry.cpp
+20 -14
View File
@@ -49,7 +49,7 @@ namespace rtabmap_odom
ICPOdometry::ICPOdometry(const rclcpp::NodeOptions & options) :
OdometryROS("icp_odometry", options),
scanCloudMaxPoints_(0),
scanCloudMaxPoints_(-1),
scanCloudIs2d_(false),
scanDownsamplingStep_(1),
scanRangeMin_(0),
@@ -68,7 +68,6 @@ ICPOdometry::ICPOdometry(const rclcpp::NodeOptions & options) :
ICPOdometry::~ICPOdometry()
{
//plugins_.clear();
}
void ICPOdometry::onOdomInit()
@@ -624,19 +623,26 @@ void ICPOdometry::callbackCloud(const sensor_msgs::msg::PointCloud2::SharedPtr p
}
}
if(scanCloudMaxPoints_ == 0 && cloudMsg->height > 1)
if(cloudMsg->height > 1) // organized cloud
{
if(scanCloudMaxPoints_ == -1)
{
scanCloudMaxPoints_ = cloudMsg->height * cloudMsg->width;
RCLCPP_WARN(this->get_logger(), "IcpOdometry: \"scan_cloud_max_points\" is not set but input "
"cloud is not dense, for convenience it will be set to %d (%dx%d)",
scanCloudMaxPoints_, cloudMsg->width, cloudMsg->height);
}
else if(scanCloudMaxPoints_ > 0 && scanCloudMaxPoints_ < int(cloudMsg->height * cloudMsg->width))
{
RCLCPP_WARN(this->get_logger(), "IcpOdometry: \"scan_cloud_max_points\" is set to %d but input "
"cloud is not dense and has a size of %d (%dx%d), setting to this later size.",
scanCloudMaxPoints_, cloudMsg->width *cloudMsg->height, cloudMsg->width, cloudMsg->height);
scanCloudMaxPoints_ = cloudMsg->width *cloudMsg->height;
}
}
if(scanCloudMaxPoints_ == -1)
{
scanCloudMaxPoints_ = cloudMsg->height * cloudMsg->width;
RCLCPP_WARN(this->get_logger(), "IcpOdometry: \"scan_cloud_max_points\" is not set but input "
"cloud is not dense, for convenience it will be set to %d (%dx%d)",
scanCloudMaxPoints_, cloudMsg->width, cloudMsg->height);
}
else if(cloudMsg->height > 1 && scanCloudMaxPoints_ < int(cloudMsg->height * cloudMsg->width))
{
RCLCPP_WARN(this->get_logger(), "IcpOdometry: \"scan_cloud_max_points\" is set to %d but input "
"cloud is not dense and has a size of %d (%dx%d), setting to this later size.",
scanCloudMaxPoints_, cloudMsg->width *cloudMsg->height, cloudMsg->width, cloudMsg->height);
scanCloudMaxPoints_ = cloudMsg->width *cloudMsg->height;
scanCloudMaxPoints_ = 0;
}
int maxLaserScans = scanCloudMaxPoints_;