mirror of
https://github.com/introlab/rtabmap_ros.git
synced 2026-10-08 18:57:46 +08:00
rtabmap_odom tests and doc (#1456)
* 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
@@ -30,7 +30,7 @@ find_package(tf2_eigen REQUIRED)
|
||||
find_package(tf2_geometry_msgs REQUIRED)
|
||||
find_package(tf2_ros REQUIRED)
|
||||
|
||||
find_package(RTABMap 0.23.10 REQUIRED)
|
||||
find_package(RTABMap 0.23.12 REQUIRED)
|
||||
|
||||
# libraries
|
||||
SET(Libraries
|
||||
|
||||
@@ -6,6 +6,13 @@ This package is a library only — it contains no nodes, no launch files and no
|
||||
|
||||
You only need it directly if you are writing your own node against RTAB-Map's C++ API and want to publish or subscribe to `rtabmap_msgs`.
|
||||
|
||||
## Contents
|
||||
|
||||
- [Usage](#usage)
|
||||
- [What it covers](#what-it-covers)
|
||||
- [Conventions worth knowing](#conventions-worth-knowing)
|
||||
- [License](#license)
|
||||
|
||||
## Usage
|
||||
|
||||
Add the dependency to your `package.xml` and `CMakeLists.txt`:
|
||||
@@ -56,14 +63,6 @@ These cut across the whole API and are not obvious from the signatures. Per-func
|
||||
|
||||
**`CameraInfo` matrices are fixed-size arrays.** `k`, `r` and `p` are `std::array`, so they are never "empty" — an unset matrix is all zeros. `cameraModelFromROS()` treats a zero `k[0]`/`p[0]` (the focal length) as absent.
|
||||
|
||||
## Building and testing
|
||||
|
||||
```bash
|
||||
colcon build --packages-select rtabmap_conversions
|
||||
colcon test --packages-select rtabmap_conversions
|
||||
colcon test-result --verbose
|
||||
```
|
||||
|
||||
## License
|
||||
|
||||
BSD-3-Clause. See the [repository root](https://github.com/introlab/rtabmap_ros#license).
|
||||
|
||||
@@ -0,0 +1,75 @@
|
||||
/*
|
||||
Copyright (c) 2010-2026, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
|
||||
All rights reserved. (BSD-3-Clause, see the repository root.)
|
||||
*/
|
||||
|
||||
#ifndef RTABMAP_CONVERSIONS_POINTCLOUDCONVERSION_H_
|
||||
#define RTABMAP_CONVERSIONS_POINTCLOUDCONVERSION_H_
|
||||
|
||||
#include <sensor_msgs/msg/point_cloud2.hpp>
|
||||
|
||||
#include <pcl/point_cloud.h>
|
||||
#include <pcl_conversions/pcl_conversions.h>
|
||||
|
||||
/**
|
||||
* @file
|
||||
* @brief pcl::toROSMsg and pcl::fromROSMsg, minus their empty-cloud crash.
|
||||
*
|
||||
* Both take the address of the first point, and of the first byte of the output, before
|
||||
* checking that there is one (see pcl/conversions.h): for an empty cloud that indexes
|
||||
* past the end of an empty vector. Nothing notices while the standard library does not
|
||||
* check, which is why it went unseen for years -- Ubuntu enables those checks from
|
||||
* resolute on, and then the process aborts outright.
|
||||
*
|
||||
* An empty cloud is ordinary here rather than exceptional: a scan whose points were all
|
||||
* filtered out, a frame with no obstacles in it, an occupancy grid with nothing new. Each
|
||||
* of those still has to be published, so the conversions are used through this.
|
||||
*/
|
||||
|
||||
namespace rtabmap_conversions {
|
||||
|
||||
/**
|
||||
* @brief @p cloud as a PointCloud2 message.
|
||||
*
|
||||
* An empty cloud is converted as a single point and emptied afterwards, so the message
|
||||
* still carries the field layout the installed PCL would have given it.
|
||||
*/
|
||||
template<typename PointT>
|
||||
void toPointCloud2Msg(
|
||||
const pcl::PointCloud<PointT> & cloud, sensor_msgs::msg::PointCloud2 & msg)
|
||||
{
|
||||
if(!cloud.empty())
|
||||
{
|
||||
pcl::toROSMsg(cloud, msg);
|
||||
return;
|
||||
}
|
||||
|
||||
pcl::PointCloud<PointT> onePoint;
|
||||
onePoint.header = cloud.header;
|
||||
onePoint.is_dense = cloud.is_dense;
|
||||
onePoint.push_back(PointT());
|
||||
pcl::toROSMsg(onePoint, msg);
|
||||
msg.width = 0;
|
||||
msg.height = 1;
|
||||
msg.row_step = 0;
|
||||
msg.data.clear();
|
||||
}
|
||||
|
||||
/// @brief @p msg as a point cloud, an empty message included.
|
||||
template<typename PointT>
|
||||
void fromPointCloud2Msg(
|
||||
const sensor_msgs::msg::PointCloud2 & msg, pcl::PointCloud<PointT> & cloud)
|
||||
{
|
||||
if(msg.data.empty())
|
||||
{
|
||||
cloud.clear();
|
||||
cloud.is_dense = msg.is_dense;
|
||||
pcl_conversions::toPCL(msg.header, cloud.header);
|
||||
return;
|
||||
}
|
||||
pcl::fromROSMsg(msg, cloud);
|
||||
}
|
||||
|
||||
} // namespace rtabmap_conversions
|
||||
|
||||
#endif /* RTABMAP_CONVERSIONS_POINTCLOUDCONVERSION_H_ */
|
||||
@@ -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_conversions/MsgConversion.h"
|
||||
|
||||
#include <cmath>
|
||||
@@ -2808,7 +2809,7 @@ bool convertScanMsg(
|
||||
if(hasIntensity)
|
||||
{
|
||||
pcl::PointCloud<pcl::PointXYZI>::Ptr pclScan(new pcl::PointCloud<pcl::PointXYZI>);
|
||||
pcl::fromROSMsg(scanOut, *pclScan);
|
||||
rtabmap_conversions::fromPointCloud2Msg(scanOut, *pclScan);
|
||||
pclScan->is_dense = true;
|
||||
data = rtabmap::util3d::laserScan2dFromPointCloud(*pclScan, laserToOdom).data(); // put back in laser frame
|
||||
format = rtabmap::LaserScan::kXYI;
|
||||
@@ -2816,7 +2817,7 @@ bool convertScanMsg(
|
||||
else
|
||||
{
|
||||
pcl::PointCloud<pcl::PointXYZ>::Ptr pclScan(new pcl::PointCloud<pcl::PointXYZ>);
|
||||
pcl::fromROSMsg(scanOut, *pclScan);
|
||||
rtabmap_conversions::fromPointCloud2Msg(scanOut, *pclScan);
|
||||
pclScan->is_dense = true;
|
||||
data = rtabmap::util3d::laserScan2dFromPointCloud(*pclScan, laserToOdom).data(); // put back in laser frame
|
||||
format = rtabmap::LaserScan::kXY;
|
||||
|
||||
Reference in New Issue
Block a user