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

rtabmap_slam tests and doc (#1460)

Browse Source 
* rtabmap_slam tests and doc

* another round of review of the doc

* Disable by default use_intra_process_comms on latched/transient publishers

* Added test to catch not unlocked mutex from early error exit

* updated coverage settings

* Using UScopeMutex on all tryLock()
This commit is contained in:
matlabbe
2026-09-28 13:09:51 -07:00
committed by GitHub
parent 853a434fe9
commit 5207dab7c2
30 changed files with 5832 additions and 118 deletions
Download Patch File Download Diff File Expand all files Collapse all files
rtabmap_conversions/CMakeLists.txt
+1 -1
View File
@@ -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.12 REQUIRED)
find_package(RTABMap 0.23.13 REQUIRED)
# libraries
SET(Libraries
rtabmap_conversions/include/rtabmap_conversions/MsgConversion.h
+6 -4
View File
@@ -832,9 +832,9 @@ bool convertStereoMsg(
* @param odomStamp stamp the scan is synchronized to
* @param[out] scan the converted scan
* @param tfBuffer must contain @p frameId -> the laser frame at the scan stamp,
* and the laser frame relative to @p odomFrameId (or @p frameId
* when that is empty) across the whole sweep, since the points
* are projected through it
* and, to deskew, the laser frame relative to @p odomFrameId
* across the whole sweep, since the points are projected through
* it
* @param waitForTransform seconds to wait for TF, 0 to not wait
* @param outputInFrameId express the points in @p frameId rather than the laser frame
* @return false if the scan is malformed (zero angle increment, inverted range or angle
@@ -846,7 +846,9 @@ bool convertStereoMsg(
* target is a fixed frame, i.e. if @p odomFrameId is set — with it empty the
* target is @p frameId, which does not move relative to itself. This is also why
* the laser frame must be known across the whole sweep, which the function checks
* up front.
* up front. When it is not known relative to @p odomFrameId -- odometry not
* published on TF -- the scan is converted as with @p odomFrameId empty, neither
* deskewed nor synchronized, with a warning shown once, rather than refused.
* @note The odometry correction is applied only when the scan stamp differs from
* @p odomStamp; a failed correction lookup warns and leaves the pose uncorrected.
*/
rtabmap_conversions/src/MsgConversion.cpp
+40 -11
View File
@@ -1305,7 +1305,8 @@ rtabmap::SensorData sensorDataFromROS(const rtabmap_msgs::msg::SensorData & msg)
pcl::PCLPointCloud2 cloud;
pcl_conversions::toPCL(msg.laser_scan, cloud);
s.setLaserScan(rtabmap::LaserScan(
rtabmap::util3d::laserScanFromPointCloud(cloud),
rtabmap::util3d::laserScanFromPointCloud(cloud, true,
rtabmap::LaserScan::isScan2d((rtabmap::LaserScan::Format)msg.laser_scan_format)),
msg.laser_scan_max_pts,
msg.laser_scan_max_range,
transformFromGeometryMsg(msg.laser_scan_local_transform)),
@@ -2714,13 +2715,41 @@ bool convertScanMsg(
}
// make sure the frame of the laser is updated during the whole scan time
rtabmap::Transform tmpT = getMovingTransform(
scan2dMsg.header.frame_id,
odomFrameId.empty()?frameId:odomFrameId,
rclcpp::Time(scan2dMsg.header.stamp.sec, scan2dMsg.header.stamp.nanosec),
rclcpp::Time(scan2dMsg.header.stamp.sec, scan2dMsg.header.stamp.nanosec) + rclcpp::Duration::from_seconds((scan2dMsg.ranges.empty()?0:scan2dMsg.ranges.size()-1)*scan2dMsg.time_increment),
tfBuffer,
waitForTransform);
const rclcpp::Time scanStart(scan2dMsg.header.stamp);
const rclcpp::Time scanEnd = scanStart + rclcpp::Duration::from_seconds((scan2dMsg.ranges.empty()?0:scan2dMsg.ranges.size()-1)*scan2dMsg.time_increment);
std::string fixedFrameId = odomFrameId.empty()?frameId:odomFrameId;
rtabmap::Transform tmpT;
if(fixedFrameId == frameId || tfBuffer._frameExists(fixedFrameId)) // don't wait for a frame never published
{
tmpT = getMovingTransform(
scan2dMsg.header.frame_id,
fixedFrameId,
scanStart,
scanEnd,
tfBuffer,
waitForTransform);
}
if(tmpT.isNull() && fixedFrameId != frameId)
{
// Odometry not in TF: use the scan as it is rather than dropping it.
static bool warned = false;
if(!warned)
{
UWARN("Could not get laser frame \"%s\" relative to odometry frame \"%s\" over the scan "
"(%fs to %fs). Laser scans are used without deskewing nor synchronization with "
"odometry. Publish odometry on TF to have them deskewed. This message is only shown once.",
scan2dMsg.header.frame_id.c_str(), odomFrameId.c_str(), scanStart.seconds(), scanEnd.seconds());
warned = true;
}
fixedFrameId = frameId;
tmpT = getMovingTransform(
scan2dMsg.header.frame_id,
fixedFrameId,
scanStart,
scanEnd,
tfBuffer,
waitForTransform);
}
if(tmpT.isNull())
{
return false;
@@ -2740,12 +2769,12 @@ bool convertScanMsg(
//transform in frameId_ frame
sensor_msgs::msg::PointCloud2 scanOut;
laser_geometry::LaserProjection projection;
projection.transformLaserScanToPointCloud(odomFrameId.empty()?frameId:odomFrameId, scan2dMsg, scanOut, tfBuffer);
projection.transformLaserScanToPointCloud(fixedFrameId, scan2dMsg, scanOut, tfBuffer);
//transform back in laser frame
rtabmap::Transform laserToOdom = getTransform(
scan2dMsg.header.frame_id,
odomFrameId.empty()?frameId:odomFrameId,
fixedFrameId,
scan2dMsg.header.stamp,
tfBuffer,
waitForTransform);
@@ -2755,7 +2784,7 @@ bool convertScanMsg(
}
// sync with odometry stamp
if(!odomFrameId.empty() && odomStamp != scan2dMsg.header.stamp)
if(fixedFrameId != frameId && odomStamp != scan2dMsg.header.stamp)
{
rtabmap::Transform sensorT = getMovingTransform(
frameId,
rtabmap_conversions/test/test_msg_conversion.cpp
+139
View File
@@ -2360,6 +2360,76 @@ TEST(MsgConversion, sensorDataLaserScanRoundTrip)
expectTransformNear(outScan.localTransform(), localTransform, 1e-4f);
}
/**
* A raw scan survives the round trip through a SensorData message in every format: it
* goes out as a cloud with one field per channel, and comes back in laser_scan_format
* with the same values. A 2D scan in particular goes out as an x/y/z cloud with z at 0,
* which alone cannot tell it was 2D -- the odometry nodes' odom_sensor_data/raw does this
* for a 2D lidar -- and must come back 2D rather than as a 3D scan that fails the format
* check.
*/
TEST(MsgConversion, sensorDataLaserScanRoundTripEveryFormat)
{
for(int f = rtabmap::LaserScan::kXY; f <= rtabmap::LaserScan::kXYZIRT; ++f)
{
const rtabmap::LaserScan::Format format = (rtabmap::LaserScan::Format)f;
SCOPED_TRACE(rtabmap::LaserScan::formatName(format));
const int channels = rtabmap::LaserScan::channels(format);
ASSERT_GT(channels, 0);
// Small integers in every channel: exact through float and through the integer
// fields some formats use, like the ring.
cv::Mat points(1, 3, CV_32FC(channels));
for(int i = 0; i < points.cols; ++i)
{
float * p = points.ptr<float>(0, i);
for(int c = 0; c < channels; ++c)
{
p[c] = float(1 + i + c);
}
}
const rtabmap::Transform localTransform(0.1f, 0.0f, 0.2f, 0.0f, 0.0f, 0.0f);
const rtabmap::LaserScan scan(points, /*maxPoints=*/360, /*maxRange=*/10.0f, format, localTransform);
if(scan.hasRGB())
{
// A packed 0x00RRGGBB color, as PCL stores it.
for(int i = 0; i < points.cols; ++i)
{
const uint32_t rgb = 0x00102030u + i;
memcpy(points.ptr<float>(0, i) + scan.getRGBOffset(), &rgb, sizeof(float));
}
}
rtabmap::SensorData in;
in.setStamp(1000.0);
in.setLaserScan(scan);
rtabmap_msgs::msg::SensorData msg;
sensorDataToROS(in, msg, "base_link", /*copyRawData=*/true);
ASSERT_FALSE(msg.laser_scan.data.empty());
EXPECT_EQ(msg.laser_scan_format, (int)format);
rtabmap::SensorData out;
bool converted = false;
EXPECT_NO_THROW({ out = sensorDataFromROS(msg); converted = true; });
if(!converted)
{
continue; // reported above; carry on so every failing format is listed
}
const rtabmap::LaserScan & outScan = out.laserScanRaw();
ASSERT_FALSE(outScan.isEmpty());
EXPECT_EQ(outScan.format(), format);
EXPECT_EQ(outScan.is2d(), scan.is2d());
EXPECT_EQ(outScan.maxPoints(), scan.maxPoints());
EXPECT_FLOAT_EQ(outScan.rangeMax(), scan.rangeMax());
expectTransformNear(outScan.localTransform(), localTransform, 1e-4f);
ASSERT_EQ(outScan.data().size(), scan.data().size());
ASSERT_EQ(outScan.data().type(), scan.data().type());
EXPECT_EQ(0, memcmp(outScan.data().data, scan.data().data,
scan.data().total() * scan.data().elemSize())) << "the values changed";
}
}
TEST(MsgConversion, sensorDataStereoModelRoundTrip)
{
const double fx = 525.0;
@@ -3290,6 +3360,75 @@ TEST(MsgConversion, convertScanMsgSyncsToOdomStamp)
EXPECT_NEAR(scan.localTransform().x(), 1.2, 1e-3) << "0.2 base->laser plus 1.0 motion";
}
namespace {
/// 21 rays of 5 m over +-1 rad, swept in 0.5 s, from "laser" at @p stamp.
sensor_msgs::msg::LaserScan makeSweep(double stamp)
{
sensor_msgs::msg::LaserScan msg;
msg.header.stamp = timestampToROS(stamp);
msg.header.frame_id = "laser";
msg.angle_min = -1.0f;
msg.angle_max = 1.0f;
msg.angle_increment = 0.1f;
msg.time_increment = 0.5f / 20.0f;
msg.range_min = 0.1f;
msg.range_max = 30.0f;
msg.ranges.assign(21, 5.0f);
return msg;
}
} // namespace
/**
* With the odometry frame on TF, each ray is placed where the robot was when it was
* measured: at 1 m/s over a 0.5 s sweep, the last ray lands 0.5 m further than it would
* from the pose at the scan's stamp, the first one not at all.
*/
TEST(MsgConversion, convertScanMsgDeskewsWithOdometryTf)
{
const std::shared_ptr<tf2_ros::Buffer> buffer = makeTfBuffer();
addTf(*buffer, "base_link", "laser", rtabmap::Transform::getIdentity(), 1000.0);
addOdomMotion(*buffer);
const sensor_msgs::msg::LaserScan msg = makeSweep(1000.0);
rtabmap::LaserScan skewed, deskewed;
ASSERT_TRUE(convertScanMsg(msg, "base_link", "", timestampToROS(1000.0), skewed, *buffer, 0.0));
ASSERT_TRUE(convertScanMsg(msg, "base_link", "odom", timestampToROS(1000.0), deskewed, *buffer, 0.0));
ASSERT_EQ(skewed.size(), deskewed.size());
ASSERT_EQ(21, deskewed.size());
const float * first = deskewed.data().ptr<float>(0, 0);
const float * firstSkewed = skewed.data().ptr<float>(0, 0);
EXPECT_NEAR(firstSkewed[0], first[0], 1e-4);
EXPECT_NEAR(firstSkewed[1], first[1], 1e-4);
const float * last = deskewed.data().ptr<float>(0, 20);
const float * lastSkewed = skewed.data().ptr<float>(0, 20);
EXPECT_NEAR(lastSkewed[0] + 0.5f, last[0], 1e-3) << "moved by the robot's 0.5 m during the sweep";
EXPECT_NEAR(lastSkewed[1], last[1], 1e-3);
}
/**
* Without the odometry frame on TF -- odometry published as a topic only -- the scan is
* still converted, as it would be without an odometry frame: not deskewed, but not refused
* either.
*/
TEST(MsgConversion, convertScanMsgUsesTheScanAsItIsWithoutOdometryTf)
{
const std::shared_ptr<tf2_ros::Buffer> buffer = makeTfBuffer();
const rtabmap::Transform baseToLaser(0.2f, 0.0f, 0.1f, 0.0f, 0.0f, 0.0f);
addTf(*buffer, "base_link", "laser", baseToLaser, 1000.0);
const sensor_msgs::msg::LaserScan msg = makeSweep(1000.0);
rtabmap::LaserScan withoutOdom, withMissingOdom;
ASSERT_TRUE(convertScanMsg(msg, "base_link", "", timestampToROS(1000.0), withoutOdom, *buffer, 0.0));
ASSERT_TRUE(convertScanMsg(msg, "base_link", "odom", timestampToROS(1000.0), withMissingOdom, *buffer, 0.0));
expectTransformNear(withMissingOdom.localTransform(), baseToLaser, 1e-4f);
ASSERT_EQ(withoutOdom.data().size(), withMissingOdom.data().size());
EXPECT_EQ(0.0, cv::norm(withoutOdom.data(), withMissingOdom.data(), cv::NORM_INF));
}
TEST(MsgConversion, convertRGBDMsgsRejectsBadEncoding)
{
const std::shared_ptr<tf2_ros::Buffer> buffer = makeTfBuffer();