i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-10-05 01:07:49 +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/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_;