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

rtabmap_odom tests and doc (#1456)

Browse Source 
* rtabmap_odom tests and doc

* opengv note

* added ci checks or humble-latest flaky dep cmake errors

* Added real data tests for rgbd_odom and stereo_odom

* added real data for icp_odometry's deskewing test

* fixing json cmake error on lyrical/rolling

* test 2d icp odom deskewing branch

* first review of existing OdometryROS tests

* testing with imu used as guess

* tested imu arrivals sync

* Fixed odom reset on right pose when guess frame id is used

* fixing header errors in ci >=lyrical

* Added support for input rgbd_image topic with features for odom, added multicam rgbd_odometry test

* Added stereo odom support for features-only frames. Added multicam stereo tests.

* forcing latest rtabmap version

* updated OdometryROS API

* ci: dont build non-latest docker in pull requests

* splitting docker jobs

* doc edit

* Making publish_null_when_lost:=false continous when guess is provided (using guess covariance when we cannot register yet)

* updated stereo doc

* ficing rolling ci (rviz Ogre header)

* Added test coverage of alll rgbd_image callbacks

* fixing rolling ci

* making docker ci build/run the tests on pull requests

* fixing ros2 ci testing

* improved sync callback coverage

* improving stereo_odometry test coverage

* improved icp_odometry test coverage

* lyrical voxel_grid ptr error

* make multicam tests working as well without opengv

* removing deps of missing packages on rolling

* PCL empty cloud  conversion compiler errors fix

* fixing icp_odometry test failure on ci witohut libpointmatcher

* fixing nav2 costmap plugin build on lyrical

* joining thread when exiting

* updating icp test to work the same on pcl 1.15 (lyrical)

* Fix parallel tests seg fault

---------

Co-authored-by: mathieu86 <[email protected]>
This commit is contained in:
matlabbe
2026-09-21 17:02:45 -07:00
committed by GitHub
co-authored by mathieu86
parent 73c98f87a8
commit 11edc01d6a
91 changed files with 9736 additions and 350 deletions
Download Patch File Download Diff File Expand all files Collapse all files
rtabmap_util/src/MapsManager.cpp
+9 -8
View File
@@ -24,6 +24,7 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#include <rtabmap_conversions/PointCloudConversion.h>
#include "rtabmap_util/MapsManager.h"
#include <rtabmap/utilite/ULogger.h>
@@ -1045,7 +1046,7 @@ void MapsManager::publishMaps(
if(cloudGroundPub_->get_subscription_count())
{
sensor_msgs::msg::PointCloud2::UniquePtr cloudMsg(new sensor_msgs::msg::PointCloud2);
pcl::toROSMsg(*assembledGround_, *cloudMsg);
rtabmap_conversions::toPointCloud2Msg(*assembledGround_, *cloudMsg);
cloudMsg->header.stamp = stamp;
cloudMsg->header.frame_id = mapFrameId;
cloudGroundPub_->publish(std::move(cloudMsg));
@@ -1054,7 +1055,7 @@ void MapsManager::publishMaps(
if(cloudObstaclesPub_->get_subscription_count())
{
sensor_msgs::msg::PointCloud2::UniquePtr cloudMsg(new sensor_msgs::msg::PointCloud2);
pcl::toROSMsg(*assembledObstacles_, *cloudMsg);
rtabmap_conversions::toPointCloud2Msg(*assembledObstacles_, *cloudMsg);
cloudMsg->header.stamp = stamp;
cloudMsg->header.frame_id = mapFrameId;
cloudObstaclesPub_->publish(std::move(cloudMsg));
@@ -1064,7 +1065,7 @@ void MapsManager::publishMaps(
{
pcl::PointCloud<pcl::PointXYZRGB> cloud = *assembledObstacles_ + *assembledGround_;
sensor_msgs::msg::PointCloud2::UniquePtr cloudMsg(new sensor_msgs::msg::PointCloud2);
pcl::toROSMsg(cloud, *cloudMsg);
rtabmap_conversions::toPointCloud2Msg(cloud, *cloudMsg);
cloudMsg->header.stamp = stamp;
cloudMsg->header.frame_id = mapFrameId;
@@ -1172,7 +1173,7 @@ void MapsManager::publishMaps(
pcl::PointCloud<pcl::PointXYZRGB> cloudOccupiedSpace;
pcl::IndicesPtr indices = util3d::concatenate(obstacleIndices, groundIndices);
pcl::copyPointCloud(*cloud, *indices, cloudOccupiedSpace);
pcl::toROSMsg(cloudOccupiedSpace, msg);
rtabmap_conversions::toPointCloud2Msg(cloudOccupiedSpace, msg);
msg.header.frame_id = mapFrameId;
msg.header.stamp = stamp;
octoMapCloud_->publish(msg);
@@ -1182,7 +1183,7 @@ void MapsManager::publishMaps(
{
pcl::PointCloud<pcl::PointXYZRGB> cloudFrontier;
pcl::copyPointCloud(*cloud, *frontierIndices, cloudFrontier);
pcl::toROSMsg(cloudFrontier, msg);
rtabmap_conversions::toPointCloud2Msg(cloudFrontier, msg);
msg.header.frame_id = mapFrameId;
msg.header.stamp = stamp;
octoMapFrontierCloud_->publish(msg);
@@ -1192,7 +1193,7 @@ void MapsManager::publishMaps(
{
pcl::PointCloud<pcl::PointXYZRGB> cloudObstacles;
pcl::copyPointCloud(*cloud, *obstacleIndices, cloudObstacles);
pcl::toROSMsg(cloudObstacles, msg);
rtabmap_conversions::toPointCloud2Msg(cloudObstacles, msg);
msg.header.frame_id = mapFrameId;
msg.header.stamp = stamp;
octoMapObstacleCloud_->publish(msg);
@@ -1202,7 +1203,7 @@ void MapsManager::publishMaps(
{
pcl::PointCloud<pcl::PointXYZRGB> cloudGround;
pcl::copyPointCloud(*cloud, *groundIndices, cloudGround);
pcl::toROSMsg(cloudGround, msg);
rtabmap_conversions::toPointCloud2Msg(cloudGround, msg);
msg.header.frame_id = mapFrameId;
msg.header.stamp = stamp;
octoMapGroundCloud_->publish(msg);
@@ -1212,7 +1213,7 @@ void MapsManager::publishMaps(
{
pcl::PointCloud<pcl::PointXYZRGB> cloudEmptySpace;
pcl::copyPointCloud(*cloud, *emptyIndices, cloudEmptySpace);
pcl::toROSMsg(cloudEmptySpace, msg);
rtabmap_conversions::toPointCloud2Msg(cloudEmptySpace, msg);
msg.header.frame_id = mapFrameId;
msg.header.stamp = stamp;
octoMapEmptySpace_->publish(msg);
rtabmap_util/src/nodelets/obstacles_detection.cpp
+5 -4
View File
@@ -25,6 +25,7 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#include <rtabmap_conversions/PointCloudConversion.h>
#include <rtabmap_util/obstacles_detection.hpp>
#include <pcl/point_cloud.h>
@@ -159,7 +160,7 @@ void ObstaclesDetection::callback(const sensor_msgs::msg::PointCloud2::ConstShar
uFormat("data=%d row_step=%d height=%d", cloudMsg->data.size(), cloudMsg->row_step, cloudMsg->height).c_str());
pcl::PointCloud<pcl::PointXYZ>::Ptr inputCloud(new pcl::PointCloud<pcl::PointXYZ>);
pcl::fromROSMsg(*cloudMsg, *inputCloud);
rtabmap_conversions::fromPointCloud2Msg(*cloudMsg, *inputCloud);
if(inputCloud->isOrganized())
{
std::vector<int> indices;
@@ -273,7 +274,7 @@ void ObstaclesDetection::callback(const sensor_msgs::msg::PointCloud2::ConstShar
if(groundPub_->get_subscription_count())
{
sensor_msgs::msg::PointCloud2::UniquePtr rosCloud(new sensor_msgs::msg::PointCloud2);
pcl::toROSMsg(*groundCloud, *rosCloud);
rtabmap_conversions::toPointCloud2Msg(*groundCloud, *rosCloud);
rosCloud->header = cloudMsg->header;
//publish the message
@@ -283,7 +284,7 @@ void ObstaclesDetection::callback(const sensor_msgs::msg::PointCloud2::ConstShar
if(obstaclesPub_->get_subscription_count())
{
sensor_msgs::msg::PointCloud2::UniquePtr rosCloud(new sensor_msgs::msg::PointCloud2);
pcl::toROSMsg(*obstaclesCloud, *rosCloud);
rtabmap_conversions::toPointCloud2Msg(*obstaclesCloud, *rosCloud);
rosCloud->header = cloudMsg->header;
//publish the message
@@ -293,7 +294,7 @@ void ObstaclesDetection::callback(const sensor_msgs::msg::PointCloud2::ConstShar
if(projObstaclesPub_->get_subscription_count())
{
sensor_msgs::msg::PointCloud2::UniquePtr rosCloud(new sensor_msgs::msg::PointCloud2);
pcl::toROSMsg(*obstaclesCloudWithoutFlatSurfaces, *rosCloud);
rtabmap_conversions::toPointCloud2Msg(*obstaclesCloudWithoutFlatSurfaces, *rosCloud);
rosCloud->header.stamp = cloudMsg->header.stamp;
rosCloud->header.frame_id = frameId_;
rtabmap_util/src/nodelets/point_cloud_xyz.cpp
+3 -2
View File
@@ -25,6 +25,7 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#include <rtabmap_conversions/PointCloudConversion.h>
#include <rtabmap_util/point_cloud_xyz.hpp>
#include <rtabmap_conversions/MsgConversion.h>
@@ -345,7 +346,7 @@ if(!pclCloud->empty() && (pclCloud->is_dense || !indices->empty()) && (normalK_
{
pclCloudNormal = rtabmap::util3d::removeNaNNormalsFromPointCloud(pclCloudNormal);
}
pcl::toROSMsg(*pclCloudNormal, *rosCloud);
rtabmap_conversions::toPointCloud2Msg(*pclCloudNormal, *rosCloud);
}
else
{
@@ -353,7 +354,7 @@ if(!pclCloud->empty() && (pclCloud->is_dense || !indices->empty()) && (normalK_
{
pclCloud = rtabmap::util3d::removeNaNFromPointCloud(pclCloud);
}
pcl::toROSMsg(*pclCloud, *rosCloud);
rtabmap_conversions::toPointCloud2Msg(*pclCloud, *rosCloud);
}
rosCloud->header.stamp = header.stamp;
rosCloud->header.frame_id = header.frame_id;
rtabmap_util/src/nodelets/point_cloud_xyzrgb.cpp
+3 -2
View File
@@ -25,6 +25,7 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#include <rtabmap_conversions/PointCloudConversion.h>
#include <rtabmap_util/point_cloud_xyzrgb.hpp>
#include <pcl_conversions/pcl_conversions.h>
@@ -538,7 +539,7 @@ void PointCloudXYZRGB::processAndPublish(
{
pclCloudNormal = rtabmap::util3d::removeNaNNormalsFromPointCloud(pclCloudNormal);
}
pcl::toROSMsg(*pclCloudNormal, *rosCloud);
rtabmap_conversions::toPointCloud2Msg(*pclCloudNormal, *rosCloud);
}
else
{
@@ -546,7 +547,7 @@ void PointCloudXYZRGB::processAndPublish(
{
pclCloud = rtabmap::util3d::removeNaNFromPointCloud(pclCloud);
}
pcl::toROSMsg(*pclCloud, *rosCloud);
rtabmap_conversions::toPointCloud2Msg(*pclCloud, *rosCloud);
}
rosCloud->header.stamp = header.stamp;
rosCloud->header.frame_id = header.frame_id;