mirror of
https://github.com/introlab/rtabmap_ros.git
synced 2026-10-04 08:47:45 +08:00
rtabmap_slam tests and doc (#1460)
* 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
@@ -0,0 +1,431 @@
|
||||
/*
|
||||
Copyright (c) 2010-2026, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
|
||||
All rights reserved. (BSD-3-Clause, see the repository root.)
|
||||
*/
|
||||
|
||||
#ifndef RTABMAP_SLAM_CORE_WRAPPER_FIXTURE_HPP_
|
||||
#define RTABMAP_SLAM_CORE_WRAPPER_FIXTURE_HPP_
|
||||
|
||||
#include <gtest/gtest.h>
|
||||
|
||||
#include <tf2_msgs/msg/tf_message.hpp>
|
||||
#include <tf2_ros/static_transform_broadcaster.hpp>
|
||||
#include <std_srvs/srv/empty.hpp>
|
||||
|
||||
#include <rtabmap_msgs/msg/info.hpp>
|
||||
#include <rtabmap_msgs/msg/map_graph.hpp>
|
||||
#include <rtabmap_msgs/srv/get_node_data.hpp>
|
||||
#include <rtabmap_msgs/srv/get_map.hpp>
|
||||
|
||||
#include <rtabmap/utilite/UDirectory.h>
|
||||
#include <rtabmap/utilite/UFile.h>
|
||||
|
||||
#include <rtabmap_slam/CoreWrapper.h>
|
||||
|
||||
#include "msg_builders.hpp"
|
||||
#include "node_test_utils.hpp"
|
||||
|
||||
#include <unistd.h>
|
||||
|
||||
#include <atomic>
|
||||
#include <chrono>
|
||||
#include <cstdlib>
|
||||
#include <memory>
|
||||
#include <string>
|
||||
#include <thread>
|
||||
#include <vector>
|
||||
|
||||
namespace rtabmap_slam_test {
|
||||
|
||||
/**
|
||||
* @brief Drives one `rtabmap` node over real ROS topics, against its own database.
|
||||
*
|
||||
* Every test gets a fresh directory for the database and the working directory, so no
|
||||
* test reads another's map and nothing lands in ~/.ros. The node is destroyed before the
|
||||
* directory is removed: its destructor is what saves the database, and a test that wants
|
||||
* to reopen a map does it by destroying the node itself and building another one.
|
||||
*
|
||||
* By default the node subscribes to odometry only (`subscribe_depth` and `subscribe_rgb`
|
||||
* off), the cheapest input that still builds a graph: each odometry message is a node.
|
||||
* `Rtabmap/DetectionRate` is 0 so every message is processed rather than one per second.
|
||||
*/
|
||||
class CoreWrapperTest : public NodeTest
|
||||
{
|
||||
protected:
|
||||
void SetUp() override
|
||||
{
|
||||
NodeTest::SetUp();
|
||||
static std::atomic<int> counter(0);
|
||||
const char * tmp = std::getenv("TMPDIR");
|
||||
dir_ = std::string(tmp && *tmp ? tmp : "/tmp") + "/rtabmap_slam_test_" +
|
||||
std::to_string(::getpid()) + "_" + std::to_string(counter++);
|
||||
UDirectory::makeDir(dir_);
|
||||
}
|
||||
|
||||
void TearDown() override
|
||||
{
|
||||
// Removes the node from the executor first: the destructor then runs with nothing
|
||||
// left to call back into it.
|
||||
stopNodeThreads();
|
||||
NodeTest::TearDown();
|
||||
node_.reset();
|
||||
staticTf_.reset();
|
||||
tfPub_.reset();
|
||||
removeDir(dir_);
|
||||
}
|
||||
|
||||
/// Where this test's database lives.
|
||||
std::string databasePath() const { return dir_ + "/rtabmap.db"; }
|
||||
const std::string & dir() const { return dir_; }
|
||||
|
||||
/// The parameters every test starts from; @p params are applied on top.
|
||||
std::vector<rclcpp::Parameter> defaultParameters(
|
||||
const std::vector<rclcpp::Parameter> & params = {}) const
|
||||
{
|
||||
std::vector<rclcpp::Parameter> all = {
|
||||
rclcpp::Parameter("database_path", databasePath()),
|
||||
rclcpp::Parameter("Rtabmap/WorkingDirectory", dir_),
|
||||
rclcpp::Parameter("subscribe_depth", false),
|
||||
rclcpp::Parameter("subscribe_rgb", false),
|
||||
rclcpp::Parameter("Rtabmap/DetectionRate", "0"),
|
||||
};
|
||||
for(const rclcpp::Parameter & p : params)
|
||||
{
|
||||
bool replaced = false;
|
||||
for(rclcpp::Parameter & q : all)
|
||||
{
|
||||
if(q.get_name() == p.get_name())
|
||||
{
|
||||
q = p;
|
||||
replaced = true;
|
||||
}
|
||||
}
|
||||
if(!replaced)
|
||||
{
|
||||
all.push_back(p);
|
||||
}
|
||||
}
|
||||
return all;
|
||||
}
|
||||
|
||||
/// Builds the node under test with defaultParameters() plus @p params.
|
||||
std::shared_ptr<rtabmap_slam::CoreWrapper> makeNode(
|
||||
const std::vector<rclcpp::Parameter> & params = {},
|
||||
const std::vector<std::string> & arguments = {})
|
||||
{
|
||||
rclcpp::NodeOptions options;
|
||||
options.parameter_overrides(defaultParameters(params));
|
||||
if(!arguments.empty())
|
||||
{
|
||||
options.arguments(arguments);
|
||||
}
|
||||
node_ = addNode(std::make_shared<rtabmap_slam::CoreWrapper>(options));
|
||||
return node_;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Builds the node under test like makeNode(), but spins it on a multi-threaded
|
||||
* executor of its own, in the background, as the `rtabmap` executable does.
|
||||
*
|
||||
* The node's mutexes are recursive: on the shared single-threaded executor, a mutex a
|
||||
* callback leaves locked is simply taken again by the next callback, on the same thread,
|
||||
* and nothing shows. With callbacks on several threads, the next one is blocked or skips
|
||||
* its update, as in the real node. The helper node keeps spinning on the shared executor.
|
||||
*/
|
||||
std::shared_ptr<rtabmap_slam::CoreWrapper> makeMultiThreadedNode(
|
||||
const std::vector<rclcpp::Parameter> & params = {})
|
||||
{
|
||||
rclcpp::NodeOptions options;
|
||||
options.parameter_overrides(defaultParameters(params));
|
||||
node_ = std::make_shared<rtabmap_slam::CoreWrapper>(options);
|
||||
nodeExecutor_ = std::make_shared<rclcpp::executors::MultiThreadedExecutor>(
|
||||
rclcpp::ExecutorOptions(), 4);
|
||||
nodeExecutor_->add_node(node_);
|
||||
nodeThread_ = std::thread([this]() { nodeExecutor_->spin(); });
|
||||
spinFor(std::chrono::milliseconds(50)); // see NodeTest::addNode()
|
||||
return node_;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Destroys the node under test, which is what saves its database.
|
||||
*
|
||||
* The executor and the helper node are rebuilt along with it, so publishers and
|
||||
* collectors made before this call are dead afterwards. Reusing the executor is not
|
||||
* an option: on Humble, one that had a node removed from it still holds that node's
|
||||
* guard condition and dereferences it on the next spin.
|
||||
*/
|
||||
void destroyNode()
|
||||
{
|
||||
stopNodeThreads();
|
||||
staticTf_.reset();
|
||||
tfPub_.reset();
|
||||
NodeTest::TearDown();
|
||||
node_.reset();
|
||||
NodeTest::SetUp();
|
||||
}
|
||||
|
||||
/// A parameter of the node under test, as the string RTAB-Map stores it as.
|
||||
std::string param(const std::string & name) const
|
||||
{
|
||||
return node_->get_parameter(name).as_string();
|
||||
}
|
||||
|
||||
/// Latches base_link -> @p child as a static transform, as a URDF would.
|
||||
void publishStaticTf(const std::string & child, double x = 0.0, double y = 0.0,
|
||||
double z = 0.0, const std::string & parent = "base_link")
|
||||
{
|
||||
geometry_msgs::msg::TransformStamped tf = makeTransform(parent, child, 0.0, x, y);
|
||||
tf.header.stamp = helper()->now();
|
||||
tf.transform.translation.z = z;
|
||||
publishStaticTf(tf);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief base_link -> @p child as a camera optical frame, @p z meters up.
|
||||
*
|
||||
* An optical frame looks along its own +z, with +x to the right of the image: rotated
|
||||
* here so the camera looks along the robot's +x, as mounted on the front of a robot.
|
||||
*/
|
||||
static geometry_msgs::msg::TransformStamped opticalTransform(
|
||||
const std::string & child, double z = 0.0)
|
||||
{
|
||||
geometry_msgs::msg::TransformStamped tf = makeTransform("base_link", child, 0.0);
|
||||
tf.transform.translation.z = z;
|
||||
tf.transform.rotation.x = -0.5;
|
||||
tf.transform.rotation.y = 0.5;
|
||||
tf.transform.rotation.z = -0.5;
|
||||
tf.transform.rotation.w = 0.5;
|
||||
return tf;
|
||||
}
|
||||
|
||||
/// Latches opticalTransform() as a static transform.
|
||||
void publishOpticalTf(const std::string & child, double z = 0.0)
|
||||
{
|
||||
geometry_msgs::msg::TransformStamped tf = opticalTransform(child, z);
|
||||
tf.header.stamp = helper()->now();
|
||||
publishStaticTf(tf);
|
||||
}
|
||||
|
||||
/// Latches @p tf as a static transform.
|
||||
void publishStaticTf(const geometry_msgs::msg::TransformStamped & tf)
|
||||
{
|
||||
if(!staticTf_)
|
||||
{
|
||||
staticTf_ = std::make_shared<tf2_ros::StaticTransformBroadcaster>(*helper());
|
||||
}
|
||||
staticTf_->sendTransform(tf);
|
||||
spinFor(std::chrono::milliseconds(100));
|
||||
}
|
||||
|
||||
/// Publishes one transform on /tf, as a moving odometry source does.
|
||||
void publishTf(const geometry_msgs::msg::TransformStamped & tf)
|
||||
{
|
||||
if(!tfPub_)
|
||||
{
|
||||
tfPub_ = helper()->create_publisher<tf2_msgs::msg::TFMessage>("/tf", rclcpp::QoS(100));
|
||||
// The node's listener and nothing else: publishing before it is matched loses
|
||||
// the transform.
|
||||
waitForSubscriber(tfPub_);
|
||||
}
|
||||
tf2_msgs::msg::TFMessage msg;
|
||||
msg.transforms.push_back(tf);
|
||||
tfPub_->publish(msg);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Publishes one odometry update the way an odometry node does: TF, then topic.
|
||||
*
|
||||
* The node looks odom -> base_link up in TF at the message's stamp and prefers it to
|
||||
* the pose in the message, so the two are published together and agree.
|
||||
*/
|
||||
void sendOdom(
|
||||
const rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr & pub,
|
||||
double stamp, double x, double y = 0.0, double yaw = 0.0,
|
||||
double variance = 0.001)
|
||||
{
|
||||
publishTf(makeTransform("odom", "base_link", stamp, x, y, yaw));
|
||||
pub->publish(makeOdometry(stamp, x, y, yaw, variance));
|
||||
}
|
||||
|
||||
rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odomPublisher()
|
||||
{
|
||||
rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr pub =
|
||||
helper()->create_publisher<nav_msgs::msg::Odometry>("odom", 10);
|
||||
EXPECT_TRUE(waitForSubscriber(pub));
|
||||
return pub;
|
||||
}
|
||||
|
||||
/// Subscribes to `info`, which the node publishes once per processed update.
|
||||
std::shared_ptr<Collector<rtabmap_msgs::msg::Info>> collectInfo()
|
||||
{
|
||||
std::shared_ptr<Collector<rtabmap_msgs::msg::Info>> info =
|
||||
collect<rtabmap_msgs::msg::Info>("info");
|
||||
EXPECT_TRUE(waitForPublisher(info->subscription));
|
||||
return info;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Sends @p count odometry updates @p step meters apart along x, one second apart.
|
||||
*
|
||||
* Waits for each to come out on @p info before sending the next, so the node never has
|
||||
* one queued while it is still processing the previous: the processing timer only
|
||||
* takes a new update once the last one is done, and drops what arrives in between.
|
||||
*/
|
||||
void driveStraight(
|
||||
const rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr & pub,
|
||||
const std::shared_ptr<Collector<rtabmap_msgs::msg::Info>> & info,
|
||||
int count, double step = 0.5, double firstStamp = 1.0, double firstX = 0.0)
|
||||
{
|
||||
for(int i=0; i<count; ++i)
|
||||
{
|
||||
const size_t before = info->size();
|
||||
sendOdom(pub, firstStamp + double(i), firstX + step*double(i));
|
||||
ASSERT_TRUE(spinUntil([&]() { return info->size() > before; }))
|
||||
<< "update " << i << " was not processed";
|
||||
}
|
||||
}
|
||||
|
||||
/// Calls @p service on the node and returns its response, or null if it never came.
|
||||
template <typename SrvT>
|
||||
typename SrvT::Response::SharedPtr call(
|
||||
const std::string & service,
|
||||
typename SrvT::Request::SharedPtr request = std::make_shared<typename SrvT::Request>(),
|
||||
std::chrono::milliseconds timeout = std::chrono::milliseconds(10000))
|
||||
{
|
||||
// The node advertises its services under its own name: /rtabmap/reset, not /reset.
|
||||
typename rclcpp::Client<SrvT>::SharedPtr client =
|
||||
helper()->create_client<SrvT>("/rtabmap/" + service);
|
||||
if(!spinUntil([&]() { return client->service_is_ready(); }))
|
||||
{
|
||||
return typename SrvT::Response::SharedPtr();
|
||||
}
|
||||
auto future = client->async_send_request(request).future.share();
|
||||
if(!spinUntil([&]() {
|
||||
return future.wait_for(std::chrono::seconds(0)) == std::future_status::ready; },
|
||||
timeout))
|
||||
{
|
||||
return typename SrvT::Response::SharedPtr();
|
||||
}
|
||||
return future.get();
|
||||
}
|
||||
|
||||
bool callEmpty(const std::string & service)
|
||||
{
|
||||
return call<std_srvs::srv::Empty>(service).get() != nullptr;
|
||||
}
|
||||
|
||||
/// The whole graph, as `get_map_data` returns it, graph only.
|
||||
rtabmap_msgs::msg::MapData getGraph(bool global = true, bool optimized = true)
|
||||
{
|
||||
rtabmap_msgs::srv::GetMap::Request::SharedPtr req =
|
||||
std::make_shared<rtabmap_msgs::srv::GetMap::Request>();
|
||||
req->global_map = global;
|
||||
req->optimized = optimized;
|
||||
req->graph_only = true;
|
||||
rtabmap_msgs::srv::GetMap::Response::SharedPtr res =
|
||||
call<rtabmap_msgs::srv::GetMap>("get_map_data", req);
|
||||
EXPECT_TRUE(res.get() != nullptr);
|
||||
return res ? res->data : rtabmap_msgs::msg::MapData();
|
||||
}
|
||||
|
||||
/// Node @p id with everything it stores; its `id` is 0 if the node does not exist.
|
||||
rtabmap_msgs::msg::Node getNode(int id)
|
||||
{
|
||||
rtabmap_msgs::srv::GetNodeData::Request::SharedPtr req =
|
||||
std::make_shared<rtabmap_msgs::srv::GetNodeData::Request>();
|
||||
req->ids.push_back(id);
|
||||
req->images = true;
|
||||
req->scan = true;
|
||||
req->grid = true;
|
||||
req->user_data = true;
|
||||
rtabmap_msgs::srv::GetNodeData::Response::SharedPtr res =
|
||||
call<rtabmap_msgs::srv::GetNodeData>("get_node_data", req);
|
||||
EXPECT_TRUE(res.get() != nullptr);
|
||||
return res && !res->data.empty() ? res->data.front() : rtabmap_msgs::msg::Node();
|
||||
}
|
||||
|
||||
/// The map ids of every node in the graph, in node id order.
|
||||
std::vector<int> mapIds()
|
||||
{
|
||||
std::vector<int> ids;
|
||||
rtabmap_msgs::srv::GetMap::Request::SharedPtr req =
|
||||
std::make_shared<rtabmap_msgs::srv::GetMap::Request>();
|
||||
req->global_map = true;
|
||||
req->optimized = false;
|
||||
req->graph_only = false;
|
||||
rtabmap_msgs::srv::GetMap::Response::SharedPtr res =
|
||||
call<rtabmap_msgs::srv::GetMap>("get_map_data", req);
|
||||
if(res)
|
||||
{
|
||||
for(const rtabmap_msgs::msg::Node & n : res->data.nodes)
|
||||
{
|
||||
ids.push_back(n.map_id);
|
||||
}
|
||||
}
|
||||
return ids;
|
||||
}
|
||||
|
||||
/// The value of RTAB-Map statistic @p key in @p info, or @p fallback if absent.
|
||||
static float stat(const rtabmap_msgs::msg::Info & info, const std::string & key,
|
||||
float fallback = -1.0f)
|
||||
{
|
||||
for(size_t i=0; i<info.stats_keys.size(); ++i)
|
||||
{
|
||||
if(info.stats_keys[i] == key)
|
||||
{
|
||||
return info.stats_values[i];
|
||||
}
|
||||
}
|
||||
return fallback;
|
||||
}
|
||||
|
||||
std::shared_ptr<rtabmap_slam::CoreWrapper> node_;
|
||||
|
||||
private:
|
||||
/**
|
||||
* Stops the executor of makeMultiThreadedNode(), if any. After a failure, a callback may
|
||||
* be blocked for good on a leaked lock, and joining would hang the binary instead of
|
||||
* reporting it: the thread and the node are then abandoned, to die with the process.
|
||||
*/
|
||||
void stopNodeThreads()
|
||||
{
|
||||
if(!nodeExecutor_)
|
||||
{
|
||||
return;
|
||||
}
|
||||
nodeExecutor_->cancel();
|
||||
if(HasFailure())
|
||||
{
|
||||
nodeThread_.detach();
|
||||
new std::shared_ptr<rtabmap_slam::CoreWrapper>(node_); // never destroyed
|
||||
new std::shared_ptr<rclcpp::executors::MultiThreadedExecutor>(nodeExecutor_);
|
||||
}
|
||||
else
|
||||
{
|
||||
nodeThread_.join();
|
||||
nodeExecutor_->remove_node(node_);
|
||||
}
|
||||
nodeExecutor_.reset();
|
||||
}
|
||||
|
||||
rclcpp::executors::MultiThreadedExecutor::SharedPtr nodeExecutor_;
|
||||
std::thread nodeThread_;
|
||||
|
||||
static void removeDir(const std::string & dir)
|
||||
{
|
||||
UDirectory d(dir);
|
||||
for(std::string f = d.getNextFilePath(); !f.empty(); f = d.getNextFilePath())
|
||||
{
|
||||
UFile::erase(f);
|
||||
}
|
||||
UDirectory::removeDir(dir);
|
||||
}
|
||||
|
||||
std::string dir_;
|
||||
std::shared_ptr<tf2_ros::StaticTransformBroadcaster> staticTf_;
|
||||
rclcpp::Publisher<tf2_msgs::msg::TFMessage>::SharedPtr tfPub_;
|
||||
};
|
||||
|
||||
} // namespace rtabmap_slam_test
|
||||
|
||||
#endif /* RTABMAP_SLAM_CORE_WRAPPER_FIXTURE_HPP_ */
|
||||
@@ -0,0 +1,348 @@
|
||||
/*
|
||||
Copyright (c) 2010-2026, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
|
||||
All rights reserved. (BSD-3-Clause, see the repository root.)
|
||||
*/
|
||||
|
||||
#ifndef RTABMAP_SLAM_MSG_BUILDERS_HPP_
|
||||
#define RTABMAP_SLAM_MSG_BUILDERS_HPP_
|
||||
|
||||
#include <rclcpp/rclcpp.hpp>
|
||||
#include <geometry_msgs/msg/pose_with_covariance_stamped.hpp>
|
||||
#include <geometry_msgs/msg/transform_stamped.hpp>
|
||||
#include <nav_msgs/msg/odometry.hpp>
|
||||
#include <sensor_msgs/msg/camera_info.hpp>
|
||||
#include <sensor_msgs/msg/image.hpp>
|
||||
#include <sensor_msgs/msg/imu.hpp>
|
||||
#include <sensor_msgs/msg/laser_scan.hpp>
|
||||
#include <sensor_msgs/msg/nav_sat_fix.hpp>
|
||||
#include <sensor_msgs/msg/point_cloud2.hpp>
|
||||
#include <rtabmap_msgs/msg/landmark_detection.hpp>
|
||||
#include <rtabmap_msgs/msg/user_data.hpp>
|
||||
|
||||
#include <opencv2/core/core.hpp>
|
||||
#include <pcl_conversions/pcl_conversions.h>
|
||||
#include <rtabmap/core/LaserScan.h>
|
||||
#include <rtabmap/core/Transform.h>
|
||||
#include <rtabmap/core/util3d.h>
|
||||
#include <rtabmap/core/util3d_transforms.h>
|
||||
#ifdef PRE_ROS_IRON
|
||||
#include <cv_bridge/cv_bridge.h>
|
||||
#else
|
||||
#include <cv_bridge/cv_bridge.hpp>
|
||||
#endif
|
||||
|
||||
#include <algorithm>
|
||||
#include <cmath>
|
||||
#include <limits>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
||||
namespace rtabmap_slam_test {
|
||||
|
||||
/// A ROS time from a double, the way sensor stamps are written throughout these tests.
|
||||
inline rclcpp::Time stampOf(double seconds)
|
||||
{
|
||||
return rclcpp::Time(
|
||||
int32_t(seconds), uint32_t((seconds - int32_t(seconds)) * 1e9), RCL_ROS_TIME);
|
||||
}
|
||||
|
||||
/// A planar pose as a TF: @p x, @p y in meters and @p yaw in radians.
|
||||
inline geometry_msgs::msg::TransformStamped makeTransform(
|
||||
const std::string & parent, const std::string & child, double stamp,
|
||||
double x = 0.0, double y = 0.0, double yaw = 0.0)
|
||||
{
|
||||
geometry_msgs::msg::TransformStamped tf;
|
||||
tf.header.frame_id = parent;
|
||||
tf.header.stamp = stampOf(stamp);
|
||||
tf.child_frame_id = child;
|
||||
tf.transform.translation.x = x;
|
||||
tf.transform.translation.y = y;
|
||||
tf.transform.rotation.z = std::sin(yaw/2.0);
|
||||
tf.transform.rotation.w = std::cos(yaw/2.0);
|
||||
return tf;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief An odometry message at (@p x, @p y, @p yaw), with a small valid covariance.
|
||||
*
|
||||
* The covariance matters to rtabmap: 9999 on both diagonals, or an identity pose after a
|
||||
* non-identity one, is read as an odometry reset and starts a new map.
|
||||
*/
|
||||
inline nav_msgs::msg::Odometry makeOdometry(
|
||||
double stamp, double x = 0.0, double y = 0.0, double yaw = 0.0,
|
||||
double variance = 0.001,
|
||||
const std::string & frameId = "odom", const std::string & childFrameId = "base_link")
|
||||
{
|
||||
nav_msgs::msg::Odometry msg;
|
||||
msg.header.frame_id = frameId;
|
||||
msg.header.stamp = stampOf(stamp);
|
||||
msg.child_frame_id = childFrameId;
|
||||
msg.pose.pose.position.x = x;
|
||||
msg.pose.pose.position.y = y;
|
||||
msg.pose.pose.orientation.z = std::sin(yaw/2.0);
|
||||
msg.pose.pose.orientation.w = std::cos(yaw/2.0);
|
||||
for(int i=0; i<6; ++i)
|
||||
{
|
||||
msg.pose.covariance[i*7] = variance;
|
||||
msg.twist.covariance[i*7] = variance;
|
||||
}
|
||||
return msg;
|
||||
}
|
||||
|
||||
/// What an odometry node publishes when it is lost or has just been reset.
|
||||
inline nav_msgs::msg::Odometry makeResetOdometry(double stamp)
|
||||
{
|
||||
nav_msgs::msg::Odometry msg = makeOdometry(stamp, 0.0, 0.0, 0.0, 9999.0);
|
||||
return msg;
|
||||
}
|
||||
|
||||
/**
|
||||
* @name The room the tests' robot drives in
|
||||
*
|
||||
* A rectangle fixed in the world (the odom and map frames coincide in these tests), with
|
||||
* walls 1 m high. Scans and clouds are generated from the sensor's actual pose in it, so
|
||||
* every node sees the same walls wherever the robot is -- which is what makes the
|
||||
* assembled map, and the occupancy grid in particular, comparable to the room.
|
||||
* @{
|
||||
*/
|
||||
constexpr double kRoomXMin = -1.5;
|
||||
constexpr double kRoomXMax = 3.5;
|
||||
constexpr double kRoomYMin = -2.0;
|
||||
constexpr double kRoomYMax = 2.0;
|
||||
constexpr double kRoomHeight = 1.0;
|
||||
|
||||
/// Distance from (@p x, @p y), inside the room, to its walls along direction @p theta.
|
||||
inline double rayToRoom(double x, double y, double theta)
|
||||
{
|
||||
const double dx = std::cos(theta);
|
||||
const double dy = std::sin(theta);
|
||||
double t = std::numeric_limits<double>::infinity();
|
||||
if(dx > 1e-9) { t = std::min(t, (kRoomXMax - x) / dx); }
|
||||
else if(dx < -1e-9) { t = std::min(t, (kRoomXMin - x) / dx); }
|
||||
if(dy > 1e-9) { t = std::min(t, (kRoomYMax - y) / dy); }
|
||||
else if(dy < -1e-9) { t = std::min(t, (kRoomYMin - y) / dy); }
|
||||
return t;
|
||||
}
|
||||
|
||||
/// A 360 degree LaserScan of the room from a laser at (@p x, @p y, @p yaw) in the world.
|
||||
inline sensor_msgs::msg::LaserScan makeRoomScan(
|
||||
const std::string & frameId, double stamp,
|
||||
double x, double y = 0.0, double yaw = 0.0, size_t count = 720)
|
||||
{
|
||||
sensor_msgs::msg::LaserScan scan;
|
||||
scan.header.frame_id = frameId;
|
||||
scan.header.stamp = stampOf(stamp);
|
||||
scan.angle_increment = float(2.0 * M_PI / double(count));
|
||||
scan.angle_min = float(-M_PI);
|
||||
scan.angle_max = scan.angle_min + scan.angle_increment * float(count - 1);
|
||||
scan.time_increment = 0.0f;
|
||||
scan.scan_time = 0.1f;
|
||||
scan.range_min = 0.1f;
|
||||
scan.range_max = 10.0f;
|
||||
scan.ranges.resize(count);
|
||||
for(size_t i=0; i<count; ++i)
|
||||
{
|
||||
scan.ranges[i] = float(rayToRoom(x, y, yaw + scan.angle_min + scan.angle_increment * double(i)));
|
||||
}
|
||||
return scan;
|
||||
}
|
||||
|
||||
/**
|
||||
* The room as a 3D scan in the world frame: its walls, floor to top, every 0.05 m along
|
||||
* them and every 0.1 m up, dense enough for the grid's obstacle clustering
|
||||
* (Grid/ClusterRadius); and its floor every 0.025 m, so that each 0.05 m grid cell gets
|
||||
* ground points. Built once: only its pose relative to the sensor changes.
|
||||
*/
|
||||
inline const rtabmap::LaserScan & roomPoints()
|
||||
{
|
||||
static const rtabmap::LaserScan room = []() {
|
||||
const double step = 0.05;
|
||||
std::vector<cv::Vec3f> points;
|
||||
for(double fx=kRoomXMin + 0.025; fx<kRoomXMax - 1e-6; fx+=0.025)
|
||||
{
|
||||
for(double fy=kRoomYMin + 0.025; fy<kRoomYMax - 1e-6; fy+=0.025)
|
||||
{
|
||||
points.push_back(cv::Vec3f(float(fx), float(fy), 0.0f));
|
||||
}
|
||||
}
|
||||
for(double h=0.0; h<=kRoomHeight + 1e-6; h+=0.1)
|
||||
{
|
||||
for(double t=kRoomXMin; t<=kRoomXMax + 1e-6; t+=step)
|
||||
{
|
||||
points.push_back(cv::Vec3f(float(t), float(kRoomYMin), float(h)));
|
||||
points.push_back(cv::Vec3f(float(t), float(kRoomYMax), float(h)));
|
||||
}
|
||||
for(double t=kRoomYMin + step; t<kRoomYMax - 1e-6; t+=step)
|
||||
{
|
||||
points.push_back(cv::Vec3f(float(kRoomXMin), float(t), float(h)));
|
||||
points.push_back(cv::Vec3f(float(kRoomXMax), float(t), float(h)));
|
||||
}
|
||||
}
|
||||
return rtabmap::LaserScan(cv::Mat(points, true).reshape(3, 1), int(points.size()),
|
||||
0.0f, rtabmap::LaserScan::kXYZ);
|
||||
}();
|
||||
return room;
|
||||
}
|
||||
|
||||
/**
|
||||
* The room as seen by a 3D lidar at (@p x, @p y, @p z) in the world, facing @p yaw, in the
|
||||
* lidar's frame. Every point of the room is visible from anywhere inside it, so moving the
|
||||
* room is all it takes -- unlike a 2D LaserScan message, whose ranges are per angle from
|
||||
* the sensor and are ray-cast again from each pose by makeRoomScan().
|
||||
*/
|
||||
inline rtabmap::LaserScan roomScan3d(double x, double y = 0.0, double z = 0.0, double yaw = 0.0)
|
||||
{
|
||||
return rtabmap::util3d::transformLaserScan(
|
||||
roomPoints(), rtabmap::Transform(x, y, z, 0, 0, yaw).inverse());
|
||||
}
|
||||
|
||||
/// @p scan as the PointCloud2 a 3D lidar driver would publish.
|
||||
inline sensor_msgs::msg::PointCloud2 makeCloud(
|
||||
const std::string & frameId, double stamp, const rtabmap::LaserScan & scan)
|
||||
{
|
||||
sensor_msgs::msg::PointCloud2 msg;
|
||||
pcl_conversions::moveFromPCL(*rtabmap::util3d::laserScanToPointCloud2(scan), msg);
|
||||
msg.header.frame_id = frameId;
|
||||
msg.header.stamp = stampOf(stamp);
|
||||
return msg;
|
||||
}
|
||||
/** @} */
|
||||
|
||||
/// A rectified pinhole CameraInfo.
|
||||
inline sensor_msgs::msg::CameraInfo makeCameraInfo(
|
||||
const std::string & frameId, double stamp, int width = 64, int height = 48,
|
||||
double fx = 50.0)
|
||||
{
|
||||
sensor_msgs::msg::CameraInfo info;
|
||||
info.header.frame_id = frameId;
|
||||
info.header.stamp = stampOf(stamp);
|
||||
info.width = width;
|
||||
info.height = height;
|
||||
info.distortion_model = "plumb_bob";
|
||||
info.d = {0.0, 0.0, 0.0, 0.0, 0.0};
|
||||
info.k = {fx, 0.0, width/2.0, 0.0, fx, height/2.0, 0.0, 0.0, 1.0};
|
||||
info.r = {1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0};
|
||||
info.p = {fx, 0.0, width/2.0, 0.0, 0.0, fx, height/2.0, 0.0, 0.0, 0.0, 1.0, 0.0};
|
||||
return info;
|
||||
}
|
||||
|
||||
inline sensor_msgs::msg::Image makeImage(
|
||||
const std::string & frameId, double stamp,
|
||||
const cv::Mat & image, const std::string & encoding)
|
||||
{
|
||||
std_msgs::msg::Header header;
|
||||
header.frame_id = frameId;
|
||||
header.stamp = stampOf(stamp);
|
||||
sensor_msgs::msg::Image msg;
|
||||
cv_bridge::CvImage(header, encoding, image).toImageMsg(msg);
|
||||
return msg;
|
||||
}
|
||||
|
||||
/// A random bgr8 texture: something a feature detector finds corners in.
|
||||
inline cv::Mat texturedImage(int width = 64, int height = 48, uint64_t seed = 42)
|
||||
{
|
||||
cv::Mat image(height, width, CV_8UC3);
|
||||
cv::RNG rng(seed);
|
||||
rng.fill(image, cv::RNG::UNIFORM, 0, 255);
|
||||
return image;
|
||||
}
|
||||
|
||||
inline sensor_msgs::msg::Image makeTexturedImage(
|
||||
const std::string & frameId, double stamp, int width = 64, int height = 48,
|
||||
uint64_t seed = 42)
|
||||
{
|
||||
return makeImage(frameId, stamp, texturedImage(width, height, seed), "bgr8");
|
||||
}
|
||||
|
||||
/// A 16UC1 depth image of a flat wall @p millimeters away.
|
||||
inline cv::Mat depthImage(int width = 64, int height = 48, uint16_t millimeters = 1500)
|
||||
{
|
||||
return cv::Mat(height, width, CV_16UC1, cv::Scalar(millimeters));
|
||||
}
|
||||
|
||||
inline sensor_msgs::msg::Image makeDepthImage(
|
||||
const std::string & frameId, double stamp, int width = 64, int height = 48,
|
||||
uint16_t millimeters = 1500)
|
||||
{
|
||||
return makeImage(frameId, stamp, depthImage(width, height, millimeters), "16UC1");
|
||||
}
|
||||
|
||||
/// An uncompressed 2x2 user data matrix.
|
||||
inline rtabmap_msgs::msg::UserData makeUserData(double stamp, uint8_t first = 1)
|
||||
{
|
||||
rtabmap_msgs::msg::UserData msg;
|
||||
msg.header.stamp = stampOf(stamp);
|
||||
msg.rows = 2;
|
||||
msg.cols = 2;
|
||||
msg.type = CV_8UC1;
|
||||
msg.data = {first, 2, 3, 4};
|
||||
return msg;
|
||||
}
|
||||
|
||||
inline sensor_msgs::msg::NavSatFix makeGpsFix(
|
||||
double stamp, double latitude, double longitude, double altitude = 100.0,
|
||||
double variance = 4.0)
|
||||
{
|
||||
sensor_msgs::msg::NavSatFix msg;
|
||||
msg.header.frame_id = "gps";
|
||||
msg.header.stamp = stampOf(stamp);
|
||||
msg.latitude = latitude;
|
||||
msg.longitude = longitude;
|
||||
msg.altitude = altitude;
|
||||
msg.position_covariance = {variance, 0, 0, 0, variance, 0, 0, 0, variance};
|
||||
msg.position_covariance_type = sensor_msgs::msg::NavSatFix::COVARIANCE_TYPE_DIAGONAL_KNOWN;
|
||||
return msg;
|
||||
}
|
||||
|
||||
/// An IMU carrying only an orientation, rolled by @p roll radians.
|
||||
inline sensor_msgs::msg::Imu makeImu(
|
||||
const std::string & frameId, double stamp, double roll = 0.0)
|
||||
{
|
||||
sensor_msgs::msg::Imu msg;
|
||||
msg.header.frame_id = frameId;
|
||||
msg.header.stamp = stampOf(stamp);
|
||||
msg.orientation.x = std::sin(roll/2.0);
|
||||
msg.orientation.w = std::cos(roll/2.0);
|
||||
return msg;
|
||||
}
|
||||
|
||||
/// A landmark (fiducial) detected @p x meters in front of @p frameId.
|
||||
inline rtabmap_msgs::msg::LandmarkDetection makeLandmark(
|
||||
const std::string & frameId, double stamp, int id, double x = 1.0)
|
||||
{
|
||||
rtabmap_msgs::msg::LandmarkDetection msg;
|
||||
msg.header.frame_id = frameId;
|
||||
msg.header.stamp = stampOf(stamp);
|
||||
msg.landmark_frame_id = "tag_" + std::to_string(id);
|
||||
msg.id = id;
|
||||
msg.size = 0.1f;
|
||||
msg.pose.pose.position.x = x;
|
||||
msg.pose.pose.orientation.w = 1.0;
|
||||
for(int i=0; i<6; ++i)
|
||||
{
|
||||
msg.pose.covariance[i*7] = 0.01;
|
||||
}
|
||||
return msg;
|
||||
}
|
||||
|
||||
inline geometry_msgs::msg::PoseWithCovarianceStamped makePoseWithCovariance(
|
||||
const std::string & frameId, double stamp, double x, double y = 0.0,
|
||||
double yaw = 0.0, double variance = 0.01)
|
||||
{
|
||||
geometry_msgs::msg::PoseWithCovarianceStamped msg;
|
||||
msg.header.frame_id = frameId;
|
||||
msg.header.stamp = stampOf(stamp);
|
||||
msg.pose.pose.position.x = x;
|
||||
msg.pose.pose.position.y = y;
|
||||
msg.pose.pose.orientation.z = std::sin(yaw/2.0);
|
||||
msg.pose.pose.orientation.w = std::cos(yaw/2.0);
|
||||
for(int i=0; i<6; ++i)
|
||||
{
|
||||
msg.pose.covariance[i*7] = variance;
|
||||
}
|
||||
return msg;
|
||||
}
|
||||
|
||||
} // namespace rtabmap_slam_test
|
||||
|
||||
#endif /* RTABMAP_SLAM_MSG_BUILDERS_HPP_ */
|
||||
@@ -0,0 +1,256 @@
|
||||
/*
|
||||
Copyright (c) 2010-2026, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
|
||||
All rights reserved.
|
||||
|
||||
Redistribution and use in source and binary forms, with or without
|
||||
modification, are permitted provided that the following conditions are met:
|
||||
* Redistributions of source code must retain the above copyright
|
||||
notice, this list of conditions and the following disclaimer.
|
||||
* Redistributions in binary form must reproduce the above copyright
|
||||
notice, this list of conditions and the following disclaimer in the
|
||||
documentation and/or other materials provided with the distribution.
|
||||
* Neither the name of the Universite de Sherbrooke nor the
|
||||
names of its contributors may be used to endorse or promote products
|
||||
derived from this software without specific prior written permission.
|
||||
|
||||
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
|
||||
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
|
||||
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
|
||||
DISCLAIMED. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY
|
||||
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
|
||||
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
|
||||
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
|
||||
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
|
||||
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
*/
|
||||
|
||||
#ifndef RTABMAP_SLAM_NODE_TEST_UTILS_HPP_
|
||||
#define RTABMAP_SLAM_NODE_TEST_UTILS_HPP_
|
||||
|
||||
#include <gtest/gtest.h>
|
||||
|
||||
#include <rclcpp/rclcpp.hpp>
|
||||
|
||||
#include <chrono>
|
||||
#include <functional>
|
||||
#include <memory>
|
||||
#include <stdexcept>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
||||
namespace rtabmap_slam_test {
|
||||
|
||||
/**
|
||||
* @brief Brings rclcpp up once for the whole test binary.
|
||||
*
|
||||
* Registered as a gtest global environment so it runs before the first test and shuts
|
||||
* down after the last one, which keeps gtest_main usable.
|
||||
*/
|
||||
class RclcppEnvironment : public ::testing::Environment
|
||||
{
|
||||
public:
|
||||
void SetUp() override
|
||||
{
|
||||
if(!rclcpp::ok())
|
||||
{
|
||||
rclcpp::init(0, nullptr);
|
||||
}
|
||||
}
|
||||
void TearDown() override
|
||||
{
|
||||
if(rclcpp::ok())
|
||||
{
|
||||
rclcpp::shutdown();
|
||||
}
|
||||
}
|
||||
};
|
||||
|
||||
/// Registers RclcppEnvironment. Call once at file scope in each test binary.
|
||||
inline ::testing::Environment * registerRclcppEnvironment()
|
||||
{
|
||||
static ::testing::Environment * const env =
|
||||
::testing::AddGlobalTestEnvironment(new RclcppEnvironment);
|
||||
return env;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Base fixture for driving a node under test over real ROS topics.
|
||||
*
|
||||
* The node under test and a helper node share one single-threaded executor, so
|
||||
* publishing, the node's callback and the assertion all happen on the same thread and
|
||||
* the tests stay deterministic. No launch files and no separate processes are involved:
|
||||
* everything runs in the gtest binary.
|
||||
*/
|
||||
class NodeTest : public ::testing::Test
|
||||
{
|
||||
protected:
|
||||
void SetUp() override
|
||||
{
|
||||
executor_ = std::make_shared<rclcpp::executors::SingleThreadedExecutor>();
|
||||
helper_ = std::make_shared<rclcpp::Node>("rtabmap_slam_test_helper");
|
||||
executor_->add_node(helper_);
|
||||
}
|
||||
|
||||
void TearDown() override
|
||||
{
|
||||
for(const rclcpp::Node::SharedPtr & node : nodes_)
|
||||
{
|
||||
executor_->remove_node(node);
|
||||
}
|
||||
nodes_.clear();
|
||||
executor_->remove_node(helper_);
|
||||
helper_.reset();
|
||||
executor_.reset();
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Adds a node under test to the shared executor and keeps it alive for the test.
|
||||
*
|
||||
* The wait is for tf2_ros, not for anything the test does with the node.
|
||||
* ~TransformListener cancels its worker's executor and joins it without ordering the
|
||||
* cancel after the worker reached spin(), so a node dropped microseconds after it was
|
||||
* built -- which a test that only reads a parameter back does -- hangs the binary for
|
||||
* good (ros2/geometry2#517). The window is a few instructions wide and nothing here
|
||||
* can observe that thread, so this buys time instead. Drop it once #752 lands.
|
||||
*/
|
||||
template <typename NodeT>
|
||||
std::shared_ptr<NodeT> addNode(const std::shared_ptr<NodeT> & node)
|
||||
{
|
||||
executor_->add_node(node);
|
||||
nodes_.push_back(node);
|
||||
spinFor(std::chrono::milliseconds(50));
|
||||
return node;
|
||||
}
|
||||
|
||||
/// The helper node, used to publish inputs and subscribe to outputs.
|
||||
rclcpp::Node::SharedPtr helper() { return helper_; }
|
||||
|
||||
/**
|
||||
* @brief Spins until @p done returns true, or the timeout elapses.
|
||||
* @return true if @p done became true
|
||||
*/
|
||||
bool spinUntil(
|
||||
const std::function<bool()> & done,
|
||||
std::chrono::milliseconds timeout = std::chrono::milliseconds(5000))
|
||||
{
|
||||
const std::chrono::steady_clock::time_point deadline =
|
||||
std::chrono::steady_clock::now() + timeout;
|
||||
while(rclcpp::ok() && std::chrono::steady_clock::now() < deadline)
|
||||
{
|
||||
if(done())
|
||||
{
|
||||
return true;
|
||||
}
|
||||
executor_->spin_once(std::chrono::milliseconds(10));
|
||||
}
|
||||
return done();
|
||||
}
|
||||
|
||||
/// Spins for a fixed duration, for the "nothing should happen" assertions.
|
||||
void spinFor(std::chrono::milliseconds duration)
|
||||
{
|
||||
const std::chrono::steady_clock::time_point deadline =
|
||||
std::chrono::steady_clock::now() + duration;
|
||||
while(rclcpp::ok() && std::chrono::steady_clock::now() < deadline)
|
||||
{
|
||||
executor_->spin_once(std::chrono::milliseconds(10));
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Waits until @p publisher has at least @p count matched subscriptions.
|
||||
*
|
||||
* Publishing before the node under test has discovered the topic silently drops the
|
||||
* message, which is the most common cause of a flaky in-process node test.
|
||||
*/
|
||||
template <typename PublisherT>
|
||||
bool waitForSubscriber(const PublisherT & publisher, size_t count = 1)
|
||||
{
|
||||
return spinUntil([&]() { return publisher->get_subscription_count() >= count; });
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Waits until @p subscription sees at least one publisher.
|
||||
*
|
||||
* Every node here publishes only when it has subscribers, so the test's subscription
|
||||
* has to be discovered before the input is sent.
|
||||
*/
|
||||
template <typename SubscriptionT>
|
||||
bool waitForPublisher(const SubscriptionT & subscription, size_t count = 1)
|
||||
{
|
||||
return spinUntil([&]() { return subscription->get_publisher_count() >= count; });
|
||||
}
|
||||
|
||||
/// Collects every message received on @p topic, for later assertions.
|
||||
template <typename MsgT>
|
||||
struct Collector
|
||||
{
|
||||
typename rclcpp::Subscription<MsgT>::SharedPtr subscription;
|
||||
std::vector<typename MsgT::ConstSharedPtr> messages;
|
||||
size_t size() const { return messages.size(); }
|
||||
bool empty() const { return messages.empty(); }
|
||||
|
||||
/**
|
||||
* @brief The last (first) message received.
|
||||
*
|
||||
* A test that reads these without having waited for the topic it is reading --
|
||||
* having waited for a different one, say -- gets a legible failure rather than a
|
||||
* segmentation fault: std::vector::back() on an empty vector dereferences
|
||||
* nullptr-1, which crashes the whole binary and takes the rest of its tests with
|
||||
* it. gtest turns the exception into a failure of the test that threw it.
|
||||
*/
|
||||
const MsgT & back() const { return *checked(messages.empty()?0:&messages.back()); }
|
||||
const MsgT & front() const { return *checked(messages.empty()?0:&messages.front()); }
|
||||
|
||||
private:
|
||||
const typename MsgT::ConstSharedPtr & checked(
|
||||
const typename MsgT::ConstSharedPtr * msg) const
|
||||
{
|
||||
if(msg == 0)
|
||||
{
|
||||
throw std::out_of_range(
|
||||
std::string("nothing was received on \"") +
|
||||
(subscription?subscription->get_topic_name():"?") +
|
||||
"\", so there is no message to read: wait for it to arrive first");
|
||||
}
|
||||
return *msg;
|
||||
}
|
||||
};
|
||||
|
||||
/**
|
||||
* @brief Subscribes the helper node to @p topic and records everything it receives.
|
||||
*
|
||||
* The callback holds the collector weakly. Capturing it by shared_ptr would close a
|
||||
* cycle -- collector owns the subscription, the subscription owns the callback, the
|
||||
* callback owns the collector -- and neither would ever be freed. A subscription that
|
||||
* outlives its test keeps the helper node's rcl handle alive with it, which leaves the
|
||||
* node's rosout publisher registered and greets the next test with "Publisher already
|
||||
* registered for node name: 'rtabmap_slam_test_helper'".
|
||||
*/
|
||||
template <typename MsgT>
|
||||
std::shared_ptr<Collector<MsgT>> collect(
|
||||
const std::string & topic, const rclcpp::QoS & qos = rclcpp::QoS(10))
|
||||
{
|
||||
std::shared_ptr<Collector<MsgT>> collector = std::make_shared<Collector<MsgT>>();
|
||||
std::weak_ptr<Collector<MsgT>> weak = collector;
|
||||
collector->subscription = helper_->create_subscription<MsgT>(
|
||||
topic, qos,
|
||||
[weak](const typename MsgT::ConstSharedPtr msg) {
|
||||
if(std::shared_ptr<Collector<MsgT>> collector = weak.lock())
|
||||
{
|
||||
collector->messages.push_back(msg);
|
||||
}
|
||||
});
|
||||
return collector;
|
||||
}
|
||||
|
||||
private:
|
||||
rclcpp::executors::SingleThreadedExecutor::SharedPtr executor_;
|
||||
rclcpp::Node::SharedPtr helper_;
|
||||
std::vector<rclcpp::Node::SharedPtr> nodes_;
|
||||
};
|
||||
|
||||
} // namespace rtabmap_slam_test
|
||||
|
||||
#endif /* RTABMAP_SLAM_NODE_TEST_UTILS_HPP_ */
|
||||
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,428 @@
|
||||
/*
|
||||
Copyright (c) 2010-2026, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
|
||||
All rights reserved. (BSD-3-Clause, see the repository root.)
|
||||
*/
|
||||
|
||||
#include <gtest/gtest.h>
|
||||
|
||||
#include <geometry_msgs/msg/pose_with_covariance_stamped.hpp>
|
||||
#include <nav_msgs/msg/path.hpp>
|
||||
|
||||
#include <rtabmap/core/Parameters.h>
|
||||
|
||||
#include "core_wrapper_fixture.hpp"
|
||||
|
||||
namespace rtabmap_slam_test {
|
||||
|
||||
namespace {
|
||||
|
||||
::testing::Environment * const kRclcppEnv = registerRclcppEnvironment();
|
||||
|
||||
using rtabmap::Parameters;
|
||||
|
||||
class CoreWrapperMappingTest : public CoreWrapperTest
|
||||
{
|
||||
protected:
|
||||
/// The x of every pose in @p graph, in node id order.
|
||||
static std::vector<double> xs(const rtabmap_msgs::msg::MapGraph & graph)
|
||||
{
|
||||
std::vector<double> out;
|
||||
for(const geometry_msgs::msg::Pose & p : graph.poses)
|
||||
{
|
||||
out.push_back(p.position.x);
|
||||
}
|
||||
return out;
|
||||
}
|
||||
|
||||
/// The neighbor link between @p from and @p to, or one with from_id 0 if none.
|
||||
static rtabmap_msgs::msg::Link neighborLink(
|
||||
const rtabmap_msgs::msg::MapGraph & graph, int from, int to)
|
||||
{
|
||||
for(const rtabmap_msgs::msg::Link & l : graph.links)
|
||||
{
|
||||
if(l.type == 0 && ((l.from_id == from && l.to_id == to) ||
|
||||
(l.from_id == to && l.to_id == from)))
|
||||
{
|
||||
return l;
|
||||
}
|
||||
}
|
||||
return rtabmap_msgs::msg::Link();
|
||||
}
|
||||
|
||||
/// Counts the map -> odom transforms seen on /tf.
|
||||
static size_t countTransforms(
|
||||
const std::shared_ptr<Collector<tf2_msgs::msg::TFMessage>> & tf,
|
||||
const std::string & parent, const std::string & child)
|
||||
{
|
||||
size_t found = 0;
|
||||
for(const tf2_msgs::msg::TFMessage::ConstSharedPtr & msg : tf->messages)
|
||||
{
|
||||
for(const geometry_msgs::msg::TransformStamped & t : msg->transforms)
|
||||
{
|
||||
found += (t.header.frame_id == parent && t.child_frame_id == child) ? 1 : 0;
|
||||
}
|
||||
}
|
||||
return found;
|
||||
}
|
||||
};
|
||||
|
||||
/**
|
||||
* The simplest input rtabmap accepts is odometry alone, and every update that moved far
|
||||
* enough becomes a node, linked to the previous one by the odometry between them.
|
||||
*/
|
||||
TEST_F(CoreWrapperMappingTest, adds_a_node_per_odometry_update)
|
||||
{
|
||||
makeNode();
|
||||
std::shared_ptr<Collector<rtabmap_msgs::msg::Info>> info = collectInfo();
|
||||
rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odom = odomPublisher();
|
||||
|
||||
driveStraight(odom, info, 3);
|
||||
|
||||
ASSERT_EQ(3u, info->size());
|
||||
EXPECT_EQ(1, info->messages[0]->ref_id);
|
||||
EXPECT_EQ(2, info->messages[1]->ref_id);
|
||||
EXPECT_EQ(3, info->messages[2]->ref_id);
|
||||
|
||||
rtabmap_msgs::msg::MapData map = getGraph();
|
||||
ASSERT_EQ(3u, map.graph.poses_id.size());
|
||||
std::vector<double> x = xs(map.graph);
|
||||
EXPECT_NEAR(0.0, x[0], 1e-4);
|
||||
EXPECT_NEAR(0.5, x[1], 1e-4);
|
||||
EXPECT_NEAR(1.0, x[2], 1e-4);
|
||||
EXPECT_NE(0, neighborLink(map.graph, 1, 2).from_id);
|
||||
EXPECT_NE(0, neighborLink(map.graph, 2, 3).from_id);
|
||||
}
|
||||
|
||||
/**
|
||||
* An update that did not move at least RGBD/LinearUpdate (or turn RGBD/AngularUpdate)
|
||||
* since the last node is not added: a robot standing still does not grow the map.
|
||||
*/
|
||||
TEST_F(CoreWrapperMappingTest, does_not_add_nodes_while_standing_still)
|
||||
{
|
||||
makeNode({rclcpp::Parameter(Parameters::kRGBDLinearUpdate(), "0.1")});
|
||||
std::shared_ptr<Collector<rtabmap_msgs::msg::Info>> info = collectInfo();
|
||||
rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odom = odomPublisher();
|
||||
|
||||
driveStraight(odom, info, 4, 0.02);
|
||||
|
||||
EXPECT_EQ(1u, getGraph().graph.poses_id.size());
|
||||
}
|
||||
|
||||
/**
|
||||
* Rtabmap/DetectionRate throttles the updates by their stamps, not by when they arrive:
|
||||
* one closer than 1/rate to the last one processed is dropped.
|
||||
*/
|
||||
TEST_F(CoreWrapperMappingTest, throttles_updates_to_the_detection_rate)
|
||||
{
|
||||
makeNode({rclcpp::Parameter(Parameters::kRtabmapDetectionRate(), "1")});
|
||||
std::shared_ptr<Collector<rtabmap_msgs::msg::Info>> info = collectInfo();
|
||||
rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odom = odomPublisher();
|
||||
|
||||
for(int i=0; i<6; ++i)
|
||||
{
|
||||
sendOdom(odom, 1.0 + 0.25*i, 0.5*i);
|
||||
spinFor(std::chrono::milliseconds(150));
|
||||
}
|
||||
spinFor(std::chrono::milliseconds(300));
|
||||
|
||||
// 1.0 and 2.0 are a full period apart; 1.25, 1.5, 1.75 and 2.25 are not.
|
||||
EXPECT_EQ(2u, info->size());
|
||||
EXPECT_EQ(2u, getGraph().graph.poses_id.size());
|
||||
}
|
||||
|
||||
/**
|
||||
* With Rtabmap/CreateIntermediateNodes, the updates the detection rate would have dropped
|
||||
* are kept as intermediate nodes instead: poses in the graph, without the sensor data or
|
||||
* the loop closure detection. They do not publish `info`.
|
||||
*/
|
||||
TEST_F(CoreWrapperMappingTest, keeps_throttled_updates_as_intermediate_nodes)
|
||||
{
|
||||
makeNode({rclcpp::Parameter(Parameters::kRtabmapDetectionRate(), "1"),
|
||||
rclcpp::Parameter(Parameters::kRtabmapCreateIntermediateNodes(), "true")});
|
||||
std::shared_ptr<Collector<rtabmap_msgs::msg::Info>> info = collectInfo();
|
||||
rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odom = odomPublisher();
|
||||
|
||||
for(int i=0; i<5; ++i)
|
||||
{
|
||||
sendOdom(odom, 1.0 + 0.25*i, 0.5*i);
|
||||
spinFor(std::chrono::milliseconds(150));
|
||||
}
|
||||
spinFor(std::chrono::milliseconds(300));
|
||||
|
||||
EXPECT_EQ(2u, info->size()) << "only the updates at 1.0 and 2.0 are full nodes";
|
||||
rtabmap_msgs::msg::MapData map = getGraph();
|
||||
EXPECT_EQ(5u, map.graph.poses_id.size());
|
||||
}
|
||||
|
||||
/**
|
||||
* An odometry that resets -- an identity pose after a non-identity one, or 9999 on both
|
||||
* covariance diagonals -- starts a new map in the same database, rather than tearing the
|
||||
* graph across a jump the robot never made.
|
||||
*/
|
||||
TEST_F(CoreWrapperMappingTest, starts_a_new_map_when_odometry_resets)
|
||||
{
|
||||
makeNode();
|
||||
std::shared_ptr<Collector<rtabmap_msgs::msg::Info>> info = collectInfo();
|
||||
rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odom = odomPublisher();
|
||||
|
||||
driveStraight(odom, info, 2);
|
||||
{
|
||||
const size_t before = info->size();
|
||||
publishTf(makeTransform("odom", "base_link", 3.0));
|
||||
odom->publish(makeResetOdometry(3.0));
|
||||
ASSERT_TRUE(spinUntil([&]() { return info->size() > before; }));
|
||||
}
|
||||
driveStraight(odom, info, 2, 0.5, 4.0, 0.5);
|
||||
|
||||
EXPECT_EQ(std::vector<int>({0, 0, 1, 1, 1}), mapIds());
|
||||
}
|
||||
|
||||
/**
|
||||
* staleness_factor: when the gap between two updates exceeds that many detection
|
||||
* periods, the odometry is not trusted across it and a new map is started, as if it had
|
||||
* reset.
|
||||
*/
|
||||
TEST_F(CoreWrapperMappingTest, starts_a_new_map_after_a_stale_gap)
|
||||
{
|
||||
makeNode({rclcpp::Parameter(Parameters::kRtabmapDetectionRate(), "1"),
|
||||
rclcpp::Parameter("staleness_factor", 2.0)});
|
||||
std::shared_ptr<Collector<rtabmap_msgs::msg::Info>> info = collectInfo();
|
||||
rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odom = odomPublisher();
|
||||
|
||||
driveStraight(odom, info, 2); // stamps 1 and 2
|
||||
driveStraight(odom, info, 2, 0.5, 6.0, 1.0); // 4 s later: more than 2 periods
|
||||
|
||||
EXPECT_EQ(std::vector<int>({0, 0, 1, 1}), mapIds());
|
||||
}
|
||||
|
||||
/// Values of staleness_factor between 0 and 1 make no sense and disable it.
|
||||
TEST_F(CoreWrapperMappingTest, ignores_a_staleness_factor_below_one)
|
||||
{
|
||||
makeNode({rclcpp::Parameter(Parameters::kRtabmapDetectionRate(), "1"),
|
||||
rclcpp::Parameter("staleness_factor", 0.5)});
|
||||
std::shared_ptr<Collector<rtabmap_msgs::msg::Info>> info = collectInfo();
|
||||
rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odom = odomPublisher();
|
||||
|
||||
driveStraight(odom, info, 2);
|
||||
driveStraight(odom, info, 2, 0.5, 6.0, 1.0);
|
||||
|
||||
EXPECT_EQ(std::vector<int>({0, 0, 0, 0}), mapIds());
|
||||
}
|
||||
|
||||
/// A message with a zero stamp cannot be placed in time and is dropped.
|
||||
TEST_F(CoreWrapperMappingTest, drops_updates_with_a_null_stamp)
|
||||
{
|
||||
makeNode();
|
||||
std::shared_ptr<Collector<rtabmap_msgs::msg::Info>> info = collectInfo();
|
||||
rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odom = odomPublisher();
|
||||
|
||||
odom->publish(makeOdometry(0.0, 1.0));
|
||||
spinFor(std::chrono::milliseconds(500));
|
||||
|
||||
EXPECT_TRUE(info->empty());
|
||||
}
|
||||
|
||||
/**
|
||||
* The odometry's covariance becomes the information matrix of the link between two nodes
|
||||
* (its inverse). The twist covariance is preferred, since it is the uncertainty of the
|
||||
* motion between the two rather than accumulated since the start.
|
||||
*/
|
||||
TEST_F(CoreWrapperMappingTest, weights_links_with_the_odometry_covariance)
|
||||
{
|
||||
makeNode();
|
||||
std::shared_ptr<Collector<rtabmap_msgs::msg::Info>> info = collectInfo();
|
||||
rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odom = odomPublisher();
|
||||
|
||||
sendOdom(odom, 1.0, 0.0, 0.0, 0.0, 0.01);
|
||||
ASSERT_TRUE(spinUntil([&]() { return info->size() == 1; }));
|
||||
sendOdom(odom, 2.0, 0.5, 0.0, 0.0, 0.01);
|
||||
ASSERT_TRUE(spinUntil([&]() { return info->size() == 2; }));
|
||||
|
||||
rtabmap_msgs::msg::Link link = neighborLink(getGraph().graph, 1, 2);
|
||||
ASSERT_NE(0, link.from_id);
|
||||
EXPECT_NEAR(100.0, link.information[0], 1e-3);
|
||||
EXPECT_NEAR(100.0, link.information[35], 1e-3);
|
||||
}
|
||||
|
||||
/**
|
||||
* An odometry with no covariance -- all zeros, as many drivers publish -- gets
|
||||
* odom_tf_linear_variance and odom_tf_angular_variance instead of an infinitely
|
||||
* confident link.
|
||||
*/
|
||||
TEST_F(CoreWrapperMappingTest, falls_back_to_default_variances_without_covariance)
|
||||
{
|
||||
makeNode({rclcpp::Parameter("odom_tf_linear_variance", 0.04),
|
||||
rclcpp::Parameter("odom_tf_angular_variance", 0.25)});
|
||||
std::shared_ptr<Collector<rtabmap_msgs::msg::Info>> info = collectInfo();
|
||||
rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odom = odomPublisher();
|
||||
|
||||
sendOdom(odom, 1.0, 0.0, 0.0, 0.0, 0.0);
|
||||
ASSERT_TRUE(spinUntil([&]() { return info->size() == 1; }));
|
||||
sendOdom(odom, 2.0, 0.5, 0.0, 0.0, 0.0);
|
||||
ASSERT_TRUE(spinUntil([&]() { return info->size() == 2; }));
|
||||
|
||||
rtabmap_msgs::msg::Link link = neighborLink(getGraph().graph, 1, 2);
|
||||
ASSERT_NE(0, link.from_id);
|
||||
EXPECT_NEAR(25.0, link.information[0], 1e-3);
|
||||
EXPECT_NEAR(4.0, link.information[35], 1e-3);
|
||||
}
|
||||
|
||||
/**
|
||||
* The node's job on TF is map -> odom: the correction that puts the odometry frame where
|
||||
* the optimized graph says it is. Identity until a loop closure moves it. It is published
|
||||
* from its own thread once the odometry frame is known, at a rate of 1/tf_delay.
|
||||
*/
|
||||
TEST_F(CoreWrapperMappingTest, publishes_map_to_odom_on_tf)
|
||||
{
|
||||
makeNode();
|
||||
std::shared_ptr<Collector<tf2_msgs::msg::TFMessage>> tf =
|
||||
collect<tf2_msgs::msg::TFMessage>("/tf", rclcpp::QoS(100));
|
||||
std::shared_ptr<Collector<rtabmap_msgs::msg::Info>> info = collectInfo();
|
||||
rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odom = odomPublisher();
|
||||
|
||||
spinFor(std::chrono::milliseconds(300));
|
||||
EXPECT_EQ(0u, countTransforms(tf, "map", "odom"))
|
||||
<< "the odometry frame is not known before the first update";
|
||||
|
||||
driveStraight(odom, info, 1);
|
||||
ASSERT_TRUE(spinUntil([&]() { return countTransforms(tf, "map", "odom") >= 3; }));
|
||||
}
|
||||
|
||||
/// odom_frame_id_init publishes map -> odom from the start, before any odometry arrives.
|
||||
TEST_F(CoreWrapperMappingTest, odom_frame_id_init_publishes_tf_before_the_first_update)
|
||||
{
|
||||
makeNode({rclcpp::Parameter("odom_frame_id_init", "odom")});
|
||||
std::shared_ptr<Collector<tf2_msgs::msg::TFMessage>> tf =
|
||||
collect<tf2_msgs::msg::TFMessage>("/tf", rclcpp::QoS(100));
|
||||
|
||||
EXPECT_TRUE(spinUntil([&]() { return countTransforms(tf, "map", "odom") >= 3; }));
|
||||
}
|
||||
|
||||
TEST_F(CoreWrapperMappingTest, publish_tf_false_publishes_no_tf)
|
||||
{
|
||||
makeNode({rclcpp::Parameter("publish_tf", false)});
|
||||
std::shared_ptr<Collector<tf2_msgs::msg::TFMessage>> tf =
|
||||
collect<tf2_msgs::msg::TFMessage>("/tf", rclcpp::QoS(100));
|
||||
std::shared_ptr<Collector<rtabmap_msgs::msg::Info>> info = collectInfo();
|
||||
rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odom = odomPublisher();
|
||||
|
||||
driveStraight(odom, info, 2);
|
||||
spinFor(std::chrono::milliseconds(300));
|
||||
|
||||
EXPECT_EQ(0u, countTransforms(tf, "map", "odom"));
|
||||
}
|
||||
|
||||
/// map_frame_id renames the map frame everywhere: TF and every map-frame topic.
|
||||
TEST_F(CoreWrapperMappingTest, map_frame_id_renames_the_map_frame)
|
||||
{
|
||||
makeNode({rclcpp::Parameter("map_frame_id", "world")});
|
||||
std::shared_ptr<Collector<tf2_msgs::msg::TFMessage>> tf =
|
||||
collect<tf2_msgs::msg::TFMessage>("/tf", rclcpp::QoS(100));
|
||||
std::shared_ptr<Collector<nav_msgs::msg::Path>> path =
|
||||
collect<nav_msgs::msg::Path>("mapPath");
|
||||
std::shared_ptr<Collector<rtabmap_msgs::msg::Info>> info = collectInfo();
|
||||
rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odom = odomPublisher();
|
||||
ASSERT_TRUE(waitForPublisher(path->subscription));
|
||||
|
||||
driveStraight(odom, info, 2);
|
||||
|
||||
ASSERT_TRUE(spinUntil([&]() { return countTransforms(tf, "world", "odom") > 0; }));
|
||||
EXPECT_EQ(0u, countTransforms(tf, "map", "odom"));
|
||||
ASSERT_TRUE(spinUntil([&]() { return !path->empty(); }));
|
||||
EXPECT_EQ("world", path->back().header.frame_id);
|
||||
EXPECT_EQ("world", info->back().header.frame_id);
|
||||
}
|
||||
|
||||
/**
|
||||
* mapPath and mapGraph carry the optimized graph after every update: the trajectory for
|
||||
* display, and the graph with its links for the nodes that assemble maps from it.
|
||||
*/
|
||||
TEST_F(CoreWrapperMappingTest, publishes_the_graph_after_every_update)
|
||||
{
|
||||
makeNode();
|
||||
std::shared_ptr<Collector<nav_msgs::msg::Path>> path =
|
||||
collect<nav_msgs::msg::Path>("mapPath");
|
||||
std::shared_ptr<Collector<rtabmap_msgs::msg::MapGraph>> graph =
|
||||
collect<rtabmap_msgs::msg::MapGraph>("mapGraph",
|
||||
rclcpp::QoS(1).reliable().transient_local());
|
||||
std::shared_ptr<Collector<rtabmap_msgs::msg::Info>> info = collectInfo();
|
||||
rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odom = odomPublisher();
|
||||
ASSERT_TRUE(waitForPublisher(path->subscription));
|
||||
ASSERT_TRUE(waitForPublisher(graph->subscription));
|
||||
|
||||
driveStraight(odom, info, 3);
|
||||
|
||||
ASSERT_TRUE(spinUntil([&]() {
|
||||
return !path->empty() && path->back().poses.size() == 3 &&
|
||||
!graph->empty() && graph->back().poses_id.size() == 3; }));
|
||||
EXPECT_EQ("map", path->back().header.frame_id);
|
||||
EXPECT_NEAR(1.0, path->back().poses[2].pose.position.x, 1e-4);
|
||||
EXPECT_EQ(2u, graph->back().links.size());
|
||||
}
|
||||
|
||||
/**
|
||||
* localization_pose is the robot's pose in the map frame -- map -> odom composed with
|
||||
* the odometry -- published on every update. While mapping, its covariance is the
|
||||
* odometry's accumulated along the graph, so it grows with distance until a loop closure
|
||||
* brings it back down.
|
||||
*/
|
||||
TEST_F(CoreWrapperMappingTest, publishes_the_pose_in_the_map_frame)
|
||||
{
|
||||
makeNode();
|
||||
std::shared_ptr<Collector<geometry_msgs::msg::PoseWithCovarianceStamped>> pose =
|
||||
collect<geometry_msgs::msg::PoseWithCovarianceStamped>("localization_pose");
|
||||
std::shared_ptr<Collector<rtabmap_msgs::msg::Info>> info = collectInfo();
|
||||
rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odom = odomPublisher();
|
||||
ASSERT_TRUE(waitForPublisher(pose->subscription));
|
||||
|
||||
driveStraight(odom, info, 2);
|
||||
|
||||
ASSERT_TRUE(spinUntil([&]() { return pose->size() == 2; }));
|
||||
EXPECT_EQ("map", pose->back().header.frame_id);
|
||||
EXPECT_NEAR(0.5, pose->back().pose.pose.position.x, 1e-4);
|
||||
EXPECT_GT(pose->back().pose.covariance[0], 0.0);
|
||||
EXPECT_LT(pose->back().pose.covariance[0], 1.0) << "not the 9999 of an unknown pose";
|
||||
}
|
||||
|
||||
/// pub_loc_pose_only_when_localizing holds it back until a loop closure has localized.
|
||||
TEST_F(CoreWrapperMappingTest, pub_loc_pose_only_when_localizing_holds_back_the_pose)
|
||||
{
|
||||
makeNode({rclcpp::Parameter("pub_loc_pose_only_when_localizing", true)});
|
||||
std::shared_ptr<Collector<geometry_msgs::msg::PoseWithCovarianceStamped>> pose =
|
||||
collect<geometry_msgs::msg::PoseWithCovarianceStamped>("localization_pose");
|
||||
std::shared_ptr<Collector<rtabmap_msgs::msg::Info>> info = collectInfo();
|
||||
rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odom = odomPublisher();
|
||||
ASSERT_TRUE(waitForPublisher(pose->subscription));
|
||||
|
||||
driveStraight(odom, info, 2);
|
||||
spinFor(std::chrono::milliseconds(200));
|
||||
|
||||
EXPECT_TRUE(pose->empty());
|
||||
}
|
||||
|
||||
/**
|
||||
* The map survives a restart: the database saved on shutdown is reopened, the next update
|
||||
* starts a new session in it, and the new nodes carry on numbering after the old ones.
|
||||
*/
|
||||
TEST_F(CoreWrapperMappingTest, continues_the_saved_map_after_a_restart)
|
||||
{
|
||||
{
|
||||
makeNode();
|
||||
std::shared_ptr<Collector<rtabmap_msgs::msg::Info>> info = collectInfo();
|
||||
rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odom = odomPublisher();
|
||||
driveStraight(odom, info, 2);
|
||||
}
|
||||
destroyNode();
|
||||
|
||||
makeNode();
|
||||
std::shared_ptr<Collector<rtabmap_msgs::msg::Info>> info = collectInfo();
|
||||
rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odom = odomPublisher();
|
||||
driveStraight(odom, info, 2, 0.5, 10.0);
|
||||
|
||||
EXPECT_EQ(3, info->front().ref_id);
|
||||
EXPECT_EQ(std::vector<int>({0, 0, 1, 1}), mapIds());
|
||||
}
|
||||
|
||||
} // namespace
|
||||
|
||||
} // namespace rtabmap_slam_test
|
||||
@@ -0,0 +1,308 @@
|
||||
/*
|
||||
Copyright (c) 2010-2026, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
|
||||
All rights reserved. (BSD-3-Clause, see the repository root.)
|
||||
*/
|
||||
|
||||
#include <gtest/gtest.h>
|
||||
|
||||
#include <fstream>
|
||||
#include <sstream>
|
||||
|
||||
#include <rtabmap/core/Parameters.h>
|
||||
|
||||
#include "core_wrapper_fixture.hpp"
|
||||
|
||||
namespace rtabmap_slam_test {
|
||||
|
||||
namespace {
|
||||
|
||||
::testing::Environment * const kRclcppEnv = registerRclcppEnvironment();
|
||||
|
||||
using rtabmap::Parameters;
|
||||
|
||||
class CoreWrapperParametersTest : public CoreWrapperTest
|
||||
{
|
||||
protected:
|
||||
/// RTAB-Map's own default for @p key, spelled the way the node stores it.
|
||||
static std::string rtabmapDefault(const std::string & key)
|
||||
{
|
||||
return Parameters::getDefaultParameters().at(key);
|
||||
}
|
||||
|
||||
static std::string readFile(const std::string & path)
|
||||
{
|
||||
std::ifstream in(path);
|
||||
std::stringstream s;
|
||||
s << in.rdbuf();
|
||||
return s.str();
|
||||
}
|
||||
};
|
||||
|
||||
/**
|
||||
* Every RTAB-Map parameter is a ROS parameter under its own name, declared as a string --
|
||||
* that is how RTAB-Map's own parameter map stores them, whatever the value looks like.
|
||||
* The odometry ones are left out: they belong to the odometry nodes, and declaring them
|
||||
* here would suggest that setting them on rtabmap does something.
|
||||
*/
|
||||
TEST_F(CoreWrapperParametersTest, declares_rtabmap_parameters_as_strings_except_odometry)
|
||||
{
|
||||
makeNode();
|
||||
|
||||
ASSERT_TRUE(node_->has_parameter(Parameters::kRtabmapDetectionRate()));
|
||||
EXPECT_EQ(rclcpp::ParameterType::PARAMETER_STRING,
|
||||
node_->get_parameter(Parameters::kRtabmapDetectionRate()).get_type());
|
||||
EXPECT_EQ(rclcpp::ParameterType::PARAMETER_STRING,
|
||||
node_->get_parameter(Parameters::kMemIncrementalMemory()).get_type());
|
||||
|
||||
EXPECT_FALSE(node_->has_parameter(Parameters::kOdomStrategy()));
|
||||
EXPECT_FALSE(node_->has_parameter(Parameters::kOdomResetCountdown()));
|
||||
EXPECT_FALSE(node_->has_parameter(Parameters::kOdomF2MMaxSize()));
|
||||
}
|
||||
|
||||
/**
|
||||
* Two defaults differ from RTAB-Map's own: the occupancy grid is built by default, since
|
||||
* on a robot that is what the map is for, and the working directory is ~/.ros, or
|
||||
* $ROS_HOME, instead of RTAB-Map's own.
|
||||
*/
|
||||
TEST_F(CoreWrapperParametersTest, builds_the_occupancy_grid_by_default)
|
||||
{
|
||||
ASSERT_FALSE(Parameters::defaultRGBDCreateOccupancyGrid())
|
||||
<< "RTAB-Map's own default changed: this test no longer shows a difference";
|
||||
|
||||
makeNode();
|
||||
|
||||
EXPECT_EQ("true", param(Parameters::kRGBDCreateOccupancyGrid()));
|
||||
EXPECT_EQ(dir(), param(Parameters::kRtabmapWorkingDirectory()));
|
||||
}
|
||||
|
||||
TEST_F(CoreWrapperParametersTest, applies_rtabmap_parameters_set_as_ros_parameters)
|
||||
{
|
||||
makeNode({rclcpp::Parameter(Parameters::kMemRehearsalSimilarity(), "0.45"),
|
||||
rclcpp::Parameter(Parameters::kRGBDLinearUpdate(), "0.3")});
|
||||
|
||||
EXPECT_EQ("0.45", param(Parameters::kMemRehearsalSimilarity()));
|
||||
EXPECT_EQ("0.3", param(Parameters::kRGBDLinearUpdate()));
|
||||
}
|
||||
|
||||
/**
|
||||
* The declared type is string, so a value given with its natural type is refused at
|
||||
* construction rather than silently converted: the quoting in `-p "Rtabmap/DetectionRate:='2'"`
|
||||
* is not optional.
|
||||
*/
|
||||
TEST_F(CoreWrapperParametersTest, refuses_a_rtabmap_parameter_given_as_a_number)
|
||||
{
|
||||
rclcpp::NodeOptions options;
|
||||
options.parameter_overrides(defaultParameters(
|
||||
{rclcpp::Parameter(Parameters::kRGBDLinearUpdate(), 0.3)}));
|
||||
EXPECT_ANY_THROW(std::make_shared<rtabmap_slam::CoreWrapper>(options));
|
||||
}
|
||||
|
||||
TEST_F(CoreWrapperParametersTest, applies_rtabmap_parameters_passed_as_arguments)
|
||||
{
|
||||
makeNode({}, {"--Mem/RehearsalSimilarity", "0.21"});
|
||||
|
||||
EXPECT_EQ("0.21", param(Parameters::kMemRehearsalSimilarity()));
|
||||
}
|
||||
|
||||
/**
|
||||
* config_path is an INI file of RTAB-Map parameters, read at startup. The odometry
|
||||
* parameters in it are ignored, like everywhere else on this node, and ROS parameters
|
||||
* set explicitly win over the file.
|
||||
*/
|
||||
TEST_F(CoreWrapperParametersTest, loads_parameters_from_config_path)
|
||||
{
|
||||
const std::string ini = dir() + "/config.ini";
|
||||
{
|
||||
std::ofstream out(ini);
|
||||
out << "[Core]\n"
|
||||
<< "Mem/RehearsalSimilarity = 0.44\n"
|
||||
<< "RGBD/LinearUpdate = 0.7\n"
|
||||
<< "Odom/Strategy = 1\n";
|
||||
}
|
||||
|
||||
makeNode({rclcpp::Parameter("config_path", ini),
|
||||
rclcpp::Parameter(Parameters::kRGBDLinearUpdate(), "0.2")});
|
||||
|
||||
EXPECT_EQ("0.44", param(Parameters::kMemRehearsalSimilarity()));
|
||||
EXPECT_EQ("0.2", param(Parameters::kRGBDLinearUpdate()));
|
||||
EXPECT_FALSE(node_->has_parameter(Parameters::kOdomStrategy()));
|
||||
}
|
||||
|
||||
/// The node writes its parameters back to config_path when it shuts down.
|
||||
TEST_F(CoreWrapperParametersTest, saves_parameters_to_config_path_on_shutdown)
|
||||
{
|
||||
const std::string ini = dir() + "/generated.ini";
|
||||
ASSERT_FALSE(UFile::exists(ini));
|
||||
|
||||
makeNode({rclcpp::Parameter("config_path", ini),
|
||||
rclcpp::Parameter(Parameters::kMemRehearsalSimilarity(), "0.37")});
|
||||
destroyNode();
|
||||
|
||||
ASSERT_TRUE(UFile::exists(ini));
|
||||
rtabmap::ParametersMap saved;
|
||||
Parameters::readINI(ini, saved);
|
||||
ASSERT_TRUE(saved.count(Parameters::kMemRehearsalSimilarity()));
|
||||
EXPECT_EQ("0.37", saved.at(Parameters::kMemRehearsalSimilarity()));
|
||||
}
|
||||
|
||||
/**
|
||||
* A database remembers the parameters it was built with, and reopening it without
|
||||
* setting them again brings them back: a map made with a given configuration is reopened
|
||||
* with that configuration. What is set explicitly still wins.
|
||||
*/
|
||||
TEST_F(CoreWrapperParametersTest, reuses_the_parameters_stored_in_the_database)
|
||||
{
|
||||
makeNode({rclcpp::Parameter(Parameters::kMemRehearsalSimilarity(), "0.33"),
|
||||
rclcpp::Parameter(Parameters::kRGBDLinearUpdate(), "0.25")});
|
||||
destroyNode();
|
||||
ASSERT_TRUE(UFile::exists(databasePath()));
|
||||
|
||||
makeNode({rclcpp::Parameter(Parameters::kRGBDLinearUpdate(), "0.15")});
|
||||
|
||||
EXPECT_EQ("0.33", param(Parameters::kMemRehearsalSimilarity()));
|
||||
EXPECT_EQ("0.15", param(Parameters::kRGBDLinearUpdate()));
|
||||
}
|
||||
|
||||
/// delete_db_on_start starts over: a new, empty database, with none of the old parameters.
|
||||
TEST_F(CoreWrapperParametersTest, delete_db_on_start_forgets_the_stored_parameters)
|
||||
{
|
||||
makeNode({rclcpp::Parameter(Parameters::kMemRehearsalSimilarity(), "0.33")});
|
||||
destroyNode();
|
||||
|
||||
makeNode({rclcpp::Parameter("delete_db_on_start", true)});
|
||||
|
||||
EXPECT_EQ(rtabmapDefault(Parameters::kMemRehearsalSimilarity()),
|
||||
param(Parameters::kMemRehearsalSimilarity()));
|
||||
}
|
||||
|
||||
/// `-d` and `--delete_db_on_start` as arguments do the same, the form launch files used.
|
||||
TEST_F(CoreWrapperParametersTest, delete_db_on_start_can_be_passed_as_an_argument)
|
||||
{
|
||||
makeNode({rclcpp::Parameter(Parameters::kMemRehearsalSimilarity(), "0.33")});
|
||||
destroyNode();
|
||||
|
||||
makeNode({}, {"-d"});
|
||||
|
||||
EXPECT_EQ(rtabmapDefault(Parameters::kMemRehearsalSimilarity()),
|
||||
param(Parameters::kMemRehearsalSimilarity()));
|
||||
}
|
||||
|
||||
/**
|
||||
* With no camera subscribed, there is nothing to extract visual words from: bag-of-words
|
||||
* loop closure detection is switched off rather than left to fail on every frame.
|
||||
*/
|
||||
TEST_F(CoreWrapperParametersTest, odometry_only_input_disables_bag_of_words)
|
||||
{
|
||||
makeNode();
|
||||
|
||||
EXPECT_EQ("-1", param(Parameters::kKpMaxFeatures()));
|
||||
EXPECT_EQ(rtabmapDefault(Parameters::kRegStrategy()), param(Parameters::kRegStrategy()));
|
||||
}
|
||||
|
||||
/**
|
||||
* With a 2D lidar and no camera, the node reconfigures itself for it: the grid is built
|
||||
* from the scan without a range limit, loop closures are registered with ICP, proximity
|
||||
* detection merges the last 10 scans, and bag-of-words is off.
|
||||
*/
|
||||
TEST_F(CoreWrapperParametersTest, laser_scan_input_switches_to_icp_and_scan_grid)
|
||||
{
|
||||
makeNode({rclcpp::Parameter("subscribe_scan", true)});
|
||||
|
||||
EXPECT_EQ("0", param(Parameters::kGridSensor()));
|
||||
EXPECT_EQ("0", param(Parameters::kGridRangeMax()));
|
||||
EXPECT_EQ("1", param(Parameters::kRegStrategy()));
|
||||
EXPECT_EQ("10", param(Parameters::kRGBDProximityPathMaxNeighbors()));
|
||||
EXPECT_EQ("-1", param(Parameters::kKpMaxFeatures()));
|
||||
}
|
||||
|
||||
/// None of those adjustments overrides a value set explicitly.
|
||||
TEST_F(CoreWrapperParametersTest, laser_scan_adjustments_keep_explicit_values)
|
||||
{
|
||||
makeNode({rclcpp::Parameter("subscribe_scan", true),
|
||||
rclcpp::Parameter(Parameters::kGridSensor(), "1"),
|
||||
rclcpp::Parameter(Parameters::kRGBDProximityPathMaxNeighbors(), "3")});
|
||||
|
||||
EXPECT_EQ("1", param(Parameters::kGridSensor()));
|
||||
EXPECT_EQ(rtabmapDefault(Parameters::kGridRangeMax()), param(Parameters::kGridRangeMax()))
|
||||
<< "the range limit is only lifted for a grid built from the scan";
|
||||
EXPECT_EQ("3", param(Parameters::kRGBDProximityPathMaxNeighbors()));
|
||||
}
|
||||
|
||||
/**
|
||||
* A 3D lidar gets the same treatment, with one difference: proximity detection registers
|
||||
* against the single nearest scan rather than merging ten.
|
||||
*/
|
||||
TEST_F(CoreWrapperParametersTest, scan_cloud_input_switches_to_icp)
|
||||
{
|
||||
makeNode({rclcpp::Parameter("subscribe_scan_cloud", true)});
|
||||
|
||||
EXPECT_EQ("0", param(Parameters::kGridSensor()));
|
||||
EXPECT_EQ("1", param(Parameters::kRegStrategy()));
|
||||
EXPECT_EQ("1", param(Parameters::kRGBDProximityPathMaxNeighbors()));
|
||||
EXPECT_EQ("-1", param(Parameters::kKpMaxFeatures()));
|
||||
}
|
||||
|
||||
/**
|
||||
* A cloud flagged as 2D -- a 2D lidar published as a cloud -- is treated like a laser
|
||||
* scan, merging ten, whether ICP was selected explicitly or by the node's own switch to it
|
||||
* for lack of a camera.
|
||||
*/
|
||||
TEST_F(CoreWrapperParametersTest, scan_cloud_flagged_2d_merges_scans_like_a_laser_scan)
|
||||
{
|
||||
makeNode({rclcpp::Parameter("subscribe_scan_cloud", true),
|
||||
rclcpp::Parameter("scan_cloud_is_2d", true),
|
||||
rclcpp::Parameter(Parameters::kRegStrategy(), "1")});
|
||||
EXPECT_EQ("10", param(Parameters::kRGBDProximityPathMaxNeighbors())) << "ICP selected explicitly";
|
||||
destroyNode();
|
||||
|
||||
makeNode({rclcpp::Parameter("subscribe_scan_cloud", true),
|
||||
rclcpp::Parameter("scan_cloud_is_2d", true),
|
||||
rclcpp::Parameter("delete_db_on_start", true)});
|
||||
EXPECT_EQ("1", param(Parameters::kRegStrategy())) << "switched to ICP by the node";
|
||||
EXPECT_EQ("10", param(Parameters::kRGBDProximityPathMaxNeighbors()));
|
||||
}
|
||||
|
||||
/**
|
||||
* A parameter RTAB-Map has renamed is still honoured under its old name, with a warning,
|
||||
* so that an old launch file keeps working. Old names are never declared, so this is only
|
||||
* possible by reading them from the overrides.
|
||||
*/
|
||||
TEST_F(CoreWrapperParametersTest, migrates_a_renamed_parameter)
|
||||
{
|
||||
// g2o/PixelVariance became Optimizer/PixelVariance.
|
||||
ASSERT_TRUE(Parameters::getRemovedParameters().count("g2o/PixelVariance"));
|
||||
makeNode({rclcpp::Parameter("g2o/PixelVariance", "2.5")});
|
||||
|
||||
EXPECT_EQ("2.5", param(Parameters::kOptimizerPixelVariance()));
|
||||
}
|
||||
|
||||
/**
|
||||
* Loaded in a component container with intra-process communication on, the node must
|
||||
* still start. Intra-process communication doesn't support transient local durability,
|
||||
* so the latched publishers (`latch`, on by default) opt out of it, while the others keep
|
||||
* the container's setting.
|
||||
*/
|
||||
TEST_F(CoreWrapperParametersTest, starts_with_intra_process_comms_whether_latching_or_not)
|
||||
{
|
||||
for(bool latch : {true, false})
|
||||
{
|
||||
SCOPED_TRACE(latch ? "latch" : "no latch");
|
||||
rclcpp::NodeOptions options;
|
||||
options.use_intra_process_comms(true);
|
||||
options.parameter_overrides(defaultParameters({rclcpp::Parameter("latch", latch)}));
|
||||
ASSERT_NO_THROW(node_ = addNode(std::make_shared<rtabmap_slam::CoreWrapper>(options)));
|
||||
|
||||
for(const std::string & topic : {std::string("mapGraph"), std::string("map")})
|
||||
{
|
||||
auto infos = node_->get_publishers_info_by_topic(node_->get_node_topics_interface()->resolve_topic_name(topic));
|
||||
ASSERT_EQ(1u, infos.size()) << topic;
|
||||
EXPECT_EQ(latch ? rclcpp::DurabilityPolicy::TransientLocal : rclcpp::DurabilityPolicy::Volatile,
|
||||
infos[0].qos_profile().durability()) << topic;
|
||||
}
|
||||
destroyNode();
|
||||
}
|
||||
}
|
||||
|
||||
} // namespace
|
||||
|
||||
} // namespace rtabmap_slam_test
|
||||
@@ -0,0 +1,400 @@
|
||||
/*
|
||||
Copyright (c) 2010-2026, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
|
||||
All rights reserved. (BSD-3-Clause, see the repository root.)
|
||||
*/
|
||||
|
||||
#include <gtest/gtest.h>
|
||||
|
||||
#include <geometry_msgs/msg/pose_stamped.hpp>
|
||||
#include <nav_msgs/msg/path.hpp>
|
||||
#include <nav_msgs/srv/get_plan.hpp>
|
||||
#include <rosgraph_msgs/msg/clock.hpp>
|
||||
#include <std_msgs/msg/bool.hpp>
|
||||
|
||||
#include <rtabmap_msgs/msg/goal.hpp>
|
||||
#include <rtabmap_msgs/msg/path.hpp>
|
||||
#include <rtabmap_msgs/srv/get_plan.hpp>
|
||||
#include <rtabmap_msgs/srv/set_goal.hpp>
|
||||
#include <rtabmap_msgs/srv/set_label.hpp>
|
||||
|
||||
#include <rtabmap/core/Parameters.h>
|
||||
|
||||
#include "core_wrapper_fixture.hpp"
|
||||
|
||||
namespace rtabmap_slam_test {
|
||||
|
||||
namespace {
|
||||
|
||||
::testing::Environment * const kRclcppEnv = registerRclcppEnvironment();
|
||||
|
||||
/**
|
||||
* Planning happens on the graph: a goal is a node (or a pose near one), the plan is the
|
||||
* chain of nodes leading to it, and the node hands the next one to reach to a local
|
||||
* planner on goal_out. These tests drive a straight corridor, x = 0 to 2 m in 0.5 m steps,
|
||||
* and plan back along it.
|
||||
*/
|
||||
class CoreWrapperPlanningTest : public CoreWrapperTest
|
||||
{
|
||||
protected:
|
||||
void SetUp() override
|
||||
{
|
||||
CoreWrapperTest::SetUp();
|
||||
makeNode(nodeParameters());
|
||||
goalOut_ = collect<geometry_msgs::msg::PoseStamped>("goal_out");
|
||||
goalReached_ = collect<std_msgs::msg::Bool>("goal_reached");
|
||||
globalPath_ = collect<nav_msgs::msg::Path>("global_path");
|
||||
globalPathNodes_ = collect<rtabmap_msgs::msg::Path>("global_path_nodes");
|
||||
info_ = collectInfo();
|
||||
odom_ = odomPublisher();
|
||||
ASSERT_TRUE(waitForPublisher(goalOut_->subscription));
|
||||
ASSERT_TRUE(waitForPublisher(goalReached_->subscription));
|
||||
ASSERT_TRUE(waitForPublisher(globalPath_->subscription));
|
||||
ASSERT_TRUE(waitForPublisher(globalPathNodes_->subscription));
|
||||
driveStraight(odom_, info_, 5); // nodes 1..5 at x = 0, 0.5, 1.0, 1.5, 2.0
|
||||
nextStamp_ = 6.0;
|
||||
}
|
||||
|
||||
rtabmap_msgs::srv::SetGoal::Response::SharedPtr setGoal(int id, const std::string & label = "")
|
||||
{
|
||||
rtabmap_msgs::srv::SetGoal::Request::SharedPtr req =
|
||||
std::make_shared<rtabmap_msgs::srv::SetGoal::Request>();
|
||||
req->node_id = id;
|
||||
req->node_label = label;
|
||||
return call<rtabmap_msgs::srv::SetGoal>("set_goal", req);
|
||||
}
|
||||
|
||||
virtual std::vector<rclcpp::Parameter> nodeParameters() { return {}; }
|
||||
|
||||
/// Moves the robot to @p x and waits for the update to be processed.
|
||||
bool moveTo(double x)
|
||||
{
|
||||
const size_t before = info_->size();
|
||||
sendOdom(odom_, nextStamp_, x);
|
||||
nextStamp_ += 1.0;
|
||||
return spinUntil([&]() { return info_->size() > before; });
|
||||
}
|
||||
|
||||
std::shared_ptr<Collector<geometry_msgs::msg::PoseStamped>> goalOut_;
|
||||
std::shared_ptr<Collector<std_msgs::msg::Bool>> goalReached_;
|
||||
std::shared_ptr<Collector<nav_msgs::msg::Path>> globalPath_;
|
||||
std::shared_ptr<Collector<rtabmap_msgs::msg::Path>> globalPathNodes_;
|
||||
std::shared_ptr<Collector<rtabmap_msgs::msg::Info>> info_;
|
||||
rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odom_;
|
||||
double nextStamp_ = 0.0;
|
||||
};
|
||||
|
||||
/**
|
||||
* set_goal plans to a node and returns the path; the next node to reach goes out on
|
||||
* goal_out, and the whole plan on global_path and global_path_nodes.
|
||||
*/
|
||||
TEST_F(CoreWrapperPlanningTest, set_goal_plans_to_a_node)
|
||||
{
|
||||
rtabmap_msgs::srv::SetGoal::Response::SharedPtr res = setGoal(1);
|
||||
|
||||
ASSERT_TRUE(res.get() != nullptr);
|
||||
ASSERT_FALSE(res->path_ids.empty());
|
||||
EXPECT_EQ(1, res->path_ids.back());
|
||||
EXPECT_EQ(res->path_ids.size(), res->path_poses.size());
|
||||
EXPECT_NEAR(0.0, res->path_poses.back().position.x, 1e-4);
|
||||
|
||||
ASSERT_TRUE(spinUntil([&]() { return !goalOut_->empty(); }));
|
||||
EXPECT_EQ("map", goalOut_->back().header.frame_id);
|
||||
ASSERT_TRUE(spinUntil([&]() { return !globalPath_->empty() && !globalPathNodes_->empty(); }));
|
||||
EXPECT_EQ(res->path_ids.size(), globalPath_->back().poses.size());
|
||||
EXPECT_EQ(res->path_ids, globalPathNodes_->back().node_ids);
|
||||
}
|
||||
|
||||
/// The goal can be named by its label instead of its id.
|
||||
TEST_F(CoreWrapperPlanningTest, set_goal_plans_to_a_label)
|
||||
{
|
||||
rtabmap_msgs::srv::SetLabel::Request::SharedPtr label =
|
||||
std::make_shared<rtabmap_msgs::srv::SetLabel::Request>();
|
||||
label->node_id = 2;
|
||||
label->node_label = "kitchen";
|
||||
ASSERT_TRUE(call<rtabmap_msgs::srv::SetLabel>("set_label", label).get() != nullptr);
|
||||
|
||||
rtabmap_msgs::srv::SetGoal::Response::SharedPtr res = setGoal(0, "kitchen");
|
||||
|
||||
ASSERT_TRUE(res.get() != nullptr);
|
||||
ASSERT_FALSE(res->path_ids.empty());
|
||||
EXPECT_EQ(2, res->path_ids.back());
|
||||
}
|
||||
|
||||
/// A goal on a node that does not exist fails, and says so on goal_reached.
|
||||
TEST_F(CoreWrapperPlanningTest, reports_failure_for_an_unknown_node)
|
||||
{
|
||||
rtabmap_msgs::srv::SetGoal::Response::SharedPtr res = setGoal(42);
|
||||
|
||||
ASSERT_TRUE(res.get() != nullptr);
|
||||
EXPECT_TRUE(res->path_ids.empty());
|
||||
ASSERT_TRUE(spinUntil([&]() { return !goalReached_->empty(); }));
|
||||
EXPECT_FALSE(goalReached_->back().data);
|
||||
}
|
||||
|
||||
TEST_F(CoreWrapperPlanningTest, reports_failure_for_an_unknown_label)
|
||||
{
|
||||
rtabmap_msgs::srv::SetGoal::Response::SharedPtr res = setGoal(0, "nowhere");
|
||||
|
||||
ASSERT_TRUE(res.get() != nullptr);
|
||||
EXPECT_TRUE(res->path_ids.empty());
|
||||
ASSERT_TRUE(spinUntil([&]() { return !goalReached_->empty(); }));
|
||||
EXPECT_FALSE(goalReached_->back().data);
|
||||
}
|
||||
|
||||
/// A goal on the node the robot is already at is reached straight away.
|
||||
TEST_F(CoreWrapperPlanningTest, reports_a_goal_already_reached)
|
||||
{
|
||||
rtabmap_msgs::srv::SetGoal::Response::SharedPtr res = setGoal(5);
|
||||
|
||||
ASSERT_TRUE(res.get() != nullptr);
|
||||
ASSERT_TRUE(spinUntil([&]() { return !goalReached_->empty(); }));
|
||||
EXPECT_TRUE(goalReached_->back().data);
|
||||
}
|
||||
|
||||
/**
|
||||
* The plan is followed as the robot moves: once it is back at the goal node,
|
||||
* goal_reached says so and the goal is cleared.
|
||||
*
|
||||
* The last step stops 5 cm short of the origin on purpose: an odometry pose of exactly
|
||||
* identity after a non-identity one is how an odometry reset looks, and would start a new
|
||||
* map instead.
|
||||
*/
|
||||
TEST_F(CoreWrapperPlanningTest, reports_the_goal_reached_when_the_robot_gets_there)
|
||||
{
|
||||
ASSERT_TRUE(setGoal(1).get() != nullptr);
|
||||
ASSERT_TRUE(spinUntil([&]() { return !goalOut_->empty(); }));
|
||||
ASSERT_TRUE(goalReached_->empty());
|
||||
|
||||
for(double x : {1.5, 1.0, 0.5, 0.05})
|
||||
{
|
||||
ASSERT_TRUE(moveTo(x));
|
||||
}
|
||||
|
||||
ASSERT_TRUE(spinUntil([&]() { return !goalReached_->empty(); }));
|
||||
EXPECT_TRUE(goalReached_->back().data);
|
||||
}
|
||||
|
||||
/// cancel_goal abandons the plan, which counts as not reaching it.
|
||||
TEST_F(CoreWrapperPlanningTest, cancel_goal_abandons_the_plan)
|
||||
{
|
||||
ASSERT_TRUE(setGoal(1).get() != nullptr);
|
||||
ASSERT_TRUE(spinUntil([&]() { return !goalOut_->empty(); }));
|
||||
|
||||
ASSERT_TRUE(callEmpty("cancel_goal"));
|
||||
|
||||
ASSERT_TRUE(spinUntil([&]() { return !goalReached_->empty(); }));
|
||||
EXPECT_FALSE(goalReached_->back().data);
|
||||
const size_t sent = goalOut_->size();
|
||||
ASSERT_TRUE(moveTo(1.5));
|
||||
spinFor(std::chrono::milliseconds(200));
|
||||
EXPECT_EQ(sent, goalOut_->size()) << "no new goal after cancelling";
|
||||
}
|
||||
|
||||
/**
|
||||
* A pose on the goal topic within RGBD/LocalRadius of the robot is not planned through
|
||||
* the graph at all: the plan is the node the robot is at, followed by the pose itself as
|
||||
* a last waypoint with node id 0, and it is left to the local planner to get there.
|
||||
*/
|
||||
TEST_F(CoreWrapperPlanningTest, plans_to_a_pose_on_the_goal_topic)
|
||||
{
|
||||
rclcpp::Publisher<geometry_msgs::msg::PoseStamped>::SharedPtr goal =
|
||||
helper()->create_publisher<geometry_msgs::msg::PoseStamped>("goal", 1);
|
||||
ASSERT_TRUE(waitForSubscriber(goal));
|
||||
|
||||
geometry_msgs::msg::PoseStamped pose;
|
||||
pose.header.frame_id = "map";
|
||||
pose.pose.position.x = 0.1;
|
||||
pose.pose.orientation.w = 1.0;
|
||||
goal->publish(pose);
|
||||
|
||||
ASSERT_TRUE(spinUntil([&]() { return !globalPathNodes_->empty(); }));
|
||||
EXPECT_EQ(std::vector<int>({5, 0}), globalPathNodes_->back().node_ids);
|
||||
EXPECT_NEAR(0.1, globalPathNodes_->back().poses.back().position.x, 1e-4);
|
||||
ASSERT_TRUE(spinUntil([&]() { return !goalOut_->empty(); }));
|
||||
}
|
||||
|
||||
/**
|
||||
* Beyond RGBD/LocalRadius, a pose goal is planned through the graph to the node nearest
|
||||
* to it, and the pose is appended after that node.
|
||||
*/
|
||||
TEST_F(CoreWrapperPlanningTest, plans_through_the_graph_beyond_the_local_radius)
|
||||
{
|
||||
ASSERT_TRUE(node_->set_parameter(
|
||||
rclcpp::Parameter(rtabmap::Parameters::kRGBDLocalRadius(), "1.0")).successful);
|
||||
spinFor(std::chrono::milliseconds(300)); // applied on the parameter event
|
||||
rclcpp::Publisher<geometry_msgs::msg::PoseStamped>::SharedPtr goal =
|
||||
helper()->create_publisher<geometry_msgs::msg::PoseStamped>("goal", 1);
|
||||
ASSERT_TRUE(waitForSubscriber(goal));
|
||||
|
||||
geometry_msgs::msg::PoseStamped pose;
|
||||
pose.header.frame_id = "map";
|
||||
pose.pose.position.x = 0.1;
|
||||
pose.pose.orientation.w = 1.0;
|
||||
goal->publish(pose);
|
||||
|
||||
ASSERT_TRUE(spinUntil([&]() { return !globalPathNodes_->empty(); }));
|
||||
EXPECT_EQ(std::vector<int>({5, 4, 3, 2, 1, 0}), globalPathNodes_->back().node_ids);
|
||||
EXPECT_NEAR(0.1, globalPathNodes_->back().poses.back().position.x, 1e-4);
|
||||
}
|
||||
|
||||
/**
|
||||
* A goal in a frame the node cannot transform to the map frame is refused rather than
|
||||
* taken as a map-frame pose.
|
||||
*/
|
||||
/**
|
||||
* The same corridor, with the node on the tests' own clock (use_sim_time): map -> odom is
|
||||
* then stamped in the odometry's time base, and with tf_tolerance at 0, exactly at the
|
||||
* clock's time. Only for tests whose TF lookups never have to wait: with a clock that only
|
||||
* moves when told to, a lookup waiting for a transform that is not there -- a goal in an
|
||||
* unknown frame, say -- would wait forever.
|
||||
*/
|
||||
class CoreWrapperPlanningSimTimeTest : public CoreWrapperPlanningTest
|
||||
{
|
||||
protected:
|
||||
std::vector<rclcpp::Parameter> nodeParameters() override
|
||||
{
|
||||
return {rclcpp::Parameter("use_sim_time", true),
|
||||
rclcpp::Parameter("tf_tolerance", 0.0)};
|
||||
}
|
||||
|
||||
/// Sets the node's clock to @p seconds.
|
||||
void setClock(double seconds)
|
||||
{
|
||||
if(!clock_)
|
||||
{
|
||||
clock_ = helper()->create_publisher<rosgraph_msgs::msg::Clock>("/clock", rclcpp::ClockQoS());
|
||||
ASSERT_TRUE(waitForSubscriber(clock_));
|
||||
}
|
||||
rosgraph_msgs::msg::Clock msg;
|
||||
msg.clock = stampOf(seconds);
|
||||
clock_->publish(msg);
|
||||
spinFor(std::chrono::milliseconds(50));
|
||||
}
|
||||
|
||||
rclcpp::Publisher<rosgraph_msgs::msg::Clock>::SharedPtr clock_;
|
||||
};
|
||||
|
||||
TEST_F(CoreWrapperPlanningSimTimeTest, transforms_a_goal_in_the_robot_frame_to_the_map_frame)
|
||||
{
|
||||
// Turn the robot to face +y where it stands, at x = 2.
|
||||
const size_t before = info_->size();
|
||||
sendOdom(odom_, nextStamp_, 2.0, 0.0, M_PI/2.0);
|
||||
ASSERT_TRUE(spinUntil([&]() { return info_->size() > before; }));
|
||||
|
||||
rclcpp::Publisher<geometry_msgs::msg::PoseStamped>::SharedPtr goal =
|
||||
helper()->create_publisher<geometry_msgs::msg::PoseStamped>("goal", 1);
|
||||
ASSERT_TRUE(waitForSubscriber(goal));
|
||||
|
||||
// The goal is looked up through map -> odom -> base_link at its stamp: bring the clock
|
||||
// to the turn's stamp, and wait for map -> odom to be published at it.
|
||||
const double stamp = nextStamp_;
|
||||
std::shared_ptr<Collector<tf2_msgs::msg::TFMessage>> tf =
|
||||
collect<tf2_msgs::msg::TFMessage>("/tf", rclcpp::QoS(100));
|
||||
setClock(stamp);
|
||||
ASSERT_TRUE(spinUntil([&]() {
|
||||
for(const tf2_msgs::msg::TFMessage::ConstSharedPtr & msg : tf->messages)
|
||||
{
|
||||
for(const geometry_msgs::msg::TransformStamped & t : msg->transforms)
|
||||
{
|
||||
if(t.child_frame_id == "odom" && rclcpp::Time(t.header.stamp) == stampOf(stamp))
|
||||
{
|
||||
return true;
|
||||
}
|
||||
}
|
||||
}
|
||||
return false; }));
|
||||
|
||||
// 1 m straight ahead of the robot, facing where it faces.
|
||||
geometry_msgs::msg::PoseStamped pose;
|
||||
pose.header.frame_id = "base_link";
|
||||
pose.header.stamp = stampOf(stamp);
|
||||
pose.pose.position.x = 1.0;
|
||||
pose.pose.orientation.w = 1.0;
|
||||
goal->publish(pose);
|
||||
|
||||
ASSERT_TRUE(spinUntil([&]() { return !globalPathNodes_->empty(); }));
|
||||
const geometry_msgs::msg::Pose & target = globalPathNodes_->back().poses.back();
|
||||
EXPECT_EQ(0, globalPathNodes_->back().node_ids.back()) << "the pose itself, last";
|
||||
EXPECT_NEAR(2.0, target.position.x, 1e-3);
|
||||
EXPECT_NEAR(1.0, target.position.y, 1e-3);
|
||||
EXPECT_NEAR(std::sin(M_PI/4.0), target.orientation.z, 1e-3) << "facing +y";
|
||||
EXPECT_NEAR(std::cos(M_PI/4.0), target.orientation.w, 1e-3);
|
||||
EXPECT_TRUE(goalReached_->empty());
|
||||
}
|
||||
|
||||
TEST_F(CoreWrapperPlanningTest, refuses_a_goal_in_an_unknown_frame)
|
||||
{
|
||||
rclcpp::Publisher<geometry_msgs::msg::PoseStamped>::SharedPtr goal =
|
||||
helper()->create_publisher<geometry_msgs::msg::PoseStamped>("goal", 1);
|
||||
ASSERT_TRUE(waitForSubscriber(goal));
|
||||
|
||||
geometry_msgs::msg::PoseStamped pose;
|
||||
pose.header.frame_id = "nowhere";
|
||||
pose.header.stamp = stampOf(5.0);
|
||||
pose.pose.orientation.w = 1.0;
|
||||
goal->publish(pose);
|
||||
|
||||
ASSERT_TRUE(spinUntil([&]() { return !goalReached_->empty(); }));
|
||||
EXPECT_FALSE(goalReached_->back().data);
|
||||
EXPECT_TRUE(goalOut_->empty());
|
||||
}
|
||||
|
||||
/// goal_node takes a node id or a label, and refuses a message with neither.
|
||||
TEST_F(CoreWrapperPlanningTest, goal_node_topic_plans_to_a_node)
|
||||
{
|
||||
rclcpp::Publisher<rtabmap_msgs::msg::Goal>::SharedPtr goal =
|
||||
helper()->create_publisher<rtabmap_msgs::msg::Goal>("goal_node", 1);
|
||||
ASSERT_TRUE(waitForSubscriber(goal));
|
||||
|
||||
rtabmap_msgs::msg::Goal msg;
|
||||
goal->publish(msg);
|
||||
ASSERT_TRUE(spinUntil([&]() { return !goalReached_->empty(); }));
|
||||
EXPECT_FALSE(goalReached_->back().data);
|
||||
|
||||
msg.node_id = 2;
|
||||
goal->publish(msg);
|
||||
ASSERT_TRUE(spinUntil([&]() { return !globalPathNodes_->empty(); }));
|
||||
EXPECT_EQ(2, globalPathNodes_->back().node_ids.back());
|
||||
}
|
||||
|
||||
/**
|
||||
* get_plan only computes a plan -- nothing is followed and nothing is published -- and
|
||||
* returns it in the goal's frame.
|
||||
*/
|
||||
TEST_F(CoreWrapperPlanningTest, get_plan_computes_without_following)
|
||||
{
|
||||
nav_msgs::srv::GetPlan::Request::SharedPtr req =
|
||||
std::make_shared<nav_msgs::srv::GetPlan::Request>();
|
||||
req->goal.header.frame_id = "map";
|
||||
req->goal.pose.position.x = 0.0;
|
||||
req->goal.pose.orientation.w = 1.0;
|
||||
nav_msgs::srv::GetPlan::Response::SharedPtr res =
|
||||
call<nav_msgs::srv::GetPlan>("get_plan", req);
|
||||
|
||||
ASSERT_TRUE(res.get() != nullptr);
|
||||
ASSERT_FALSE(res->plan.poses.empty());
|
||||
EXPECT_EQ("map", res->plan.header.frame_id);
|
||||
EXPECT_NEAR(0.0, res->plan.poses.back().pose.position.x, 1e-4);
|
||||
spinFor(std::chrono::milliseconds(200));
|
||||
EXPECT_TRUE(goalOut_->empty());
|
||||
}
|
||||
|
||||
/// get_plan_nodes is the same, with the node ids along the plan, to a node or a pose.
|
||||
TEST_F(CoreWrapperPlanningTest, get_plan_nodes_returns_the_node_ids)
|
||||
{
|
||||
rtabmap_msgs::srv::GetPlan::Request::SharedPtr req =
|
||||
std::make_shared<rtabmap_msgs::srv::GetPlan::Request>();
|
||||
req->goal_node = 2;
|
||||
rtabmap_msgs::srv::GetPlan::Response::SharedPtr res =
|
||||
call<rtabmap_msgs::srv::GetPlan>("get_plan_nodes", req);
|
||||
|
||||
ASSERT_TRUE(res.get() != nullptr);
|
||||
ASSERT_FALSE(res->plan.node_ids.empty());
|
||||
EXPECT_EQ(2, res->plan.node_ids.back());
|
||||
EXPECT_EQ(res->plan.node_ids.size(), res->plan.poses.size());
|
||||
EXPECT_TRUE(goalOut_->empty());
|
||||
}
|
||||
|
||||
} // namespace
|
||||
|
||||
} // namespace rtabmap_slam_test
|
||||
@@ -0,0 +1,892 @@
|
||||
/*
|
||||
Copyright (c) 2010-2026, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
|
||||
All rights reserved. (BSD-3-Clause, see the repository root.)
|
||||
*/
|
||||
|
||||
#include <gtest/gtest.h>
|
||||
|
||||
#include <algorithm>
|
||||
#include <tuple>
|
||||
#include <utility>
|
||||
|
||||
#include <rtabmap_msgs/msg/rgbd_image.hpp>
|
||||
#include <rtabmap_msgs/msg/sensor_data.hpp>
|
||||
#include <rtabmap_msgs/srv/add_link.hpp>
|
||||
#include <rtabmap_msgs/srv/get_map2.hpp>
|
||||
#include <rtabmap_msgs/srv/get_nodes_in_radius.hpp>
|
||||
#include <rtabmap_msgs/srv/list_labels.hpp>
|
||||
#include <rtabmap_msgs/srv/load_database.hpp>
|
||||
#include <rtabmap_msgs/srv/publish_map.hpp>
|
||||
#include <rtabmap_msgs/srv/remove_label.hpp>
|
||||
#include <rtabmap_msgs/srv/set_label.hpp>
|
||||
|
||||
#include <rtabmap/core/Compression.h>
|
||||
#include <rtabmap/core/GlobalDescriptor.h>
|
||||
#include <rtabmap/core/SensorData.h>
|
||||
#include <rtabmap/core/Link.h>
|
||||
#include <rtabmap/core/Parameters.h>
|
||||
#include <rtabmap/utilite/ULogger.h>
|
||||
|
||||
#include <rtabmap_conversions/MsgConversion.h>
|
||||
#include <tf2_ros/buffer.hpp>
|
||||
|
||||
#include "core_wrapper_fixture.hpp"
|
||||
|
||||
namespace rtabmap_slam_test {
|
||||
|
||||
namespace {
|
||||
|
||||
::testing::Environment * const kRclcppEnv = registerRclcppEnvironment();
|
||||
|
||||
using rtabmap::Parameters;
|
||||
|
||||
class CoreWrapperServicesTest : public CoreWrapperTest
|
||||
{
|
||||
protected:
|
||||
/// A node with @p count nodes already in its map, 0.5 m apart along x.
|
||||
void makeMap(int count = 3, const std::vector<rclcpp::Parameter> & params = {})
|
||||
{
|
||||
makeNode(params);
|
||||
info_ = collectInfo();
|
||||
odom_ = odomPublisher();
|
||||
driveStraight(odom_, info_, count);
|
||||
}
|
||||
|
||||
/// Sends one more update, @p x meters along, and waits for it to be processed.
|
||||
bool updateAt(double stamp, double x)
|
||||
{
|
||||
const size_t before = info_->size();
|
||||
sendOdom(odom_, stamp, x);
|
||||
return spinUntil([&]() { return info_->size() > before; });
|
||||
}
|
||||
|
||||
rtabmap_msgs::srv::ListLabels::Response::SharedPtr listLabels()
|
||||
{
|
||||
return call<rtabmap_msgs::srv::ListLabels>("list_labels");
|
||||
}
|
||||
|
||||
bool setLabel(int id, const std::string & label)
|
||||
{
|
||||
rtabmap_msgs::srv::SetLabel::Request::SharedPtr req =
|
||||
std::make_shared<rtabmap_msgs::srv::SetLabel::Request>();
|
||||
req->node_id = id;
|
||||
req->node_label = label;
|
||||
return call<rtabmap_msgs::srv::SetLabel>("set_label", req).get() != nullptr;
|
||||
}
|
||||
|
||||
std::shared_ptr<Collector<rtabmap_msgs::msg::Info>> info_;
|
||||
rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odom_;
|
||||
};
|
||||
|
||||
/// Every service the node offers is advertised under its own name, /rtabmap/<service>.
|
||||
TEST_F(CoreWrapperServicesTest, advertises_its_services_under_its_name)
|
||||
{
|
||||
makeNode();
|
||||
const std::vector<std::string> expected = {
|
||||
"update_parameters", "reset", "pause", "resume", "load_database",
|
||||
"trigger_new_map", "backup", "detect_more_loop_closures", "global_bundle_adjustment",
|
||||
"cleanup_local_grids", "set_mode_localization", "set_mode_mapping", "get_node_data",
|
||||
"get_map_data", "get_map_data2", "get_map", "get_prob_map", "publish_map",
|
||||
"get_plan", "get_plan_nodes", "set_goal", "cancel_goal", "set_label", "list_labels",
|
||||
"remove_label", "add_link", "get_nodes_in_radius",
|
||||
"log_debug", "log_info", "log_warning", "log_error"};
|
||||
|
||||
std::map<std::string, std::vector<std::string>> advertised;
|
||||
ASSERT_TRUE(spinUntil([&]() {
|
||||
advertised = helper()->get_service_names_and_types_by_node("rtabmap", "/");
|
||||
return advertised.size() >= expected.size(); }));
|
||||
for(const std::string & name : expected)
|
||||
{
|
||||
EXPECT_TRUE(advertised.count("/rtabmap/" + name)) << "/rtabmap/" << name;
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* pause stops the node from taking any input at all -- the odometry is dropped, not
|
||||
* queued -- and resume picks up from the next message. The state is mirrored in the
|
||||
* is_rtabmap_paused parameter.
|
||||
*/
|
||||
TEST_F(CoreWrapperServicesTest, pause_drops_input_until_resume)
|
||||
{
|
||||
makeMap(1);
|
||||
|
||||
ASSERT_TRUE(callEmpty("pause"));
|
||||
EXPECT_TRUE(node_->get_parameter("is_rtabmap_paused").as_bool());
|
||||
sendOdom(odom_, 2.0, 0.5);
|
||||
spinFor(std::chrono::milliseconds(500));
|
||||
EXPECT_EQ(1u, info_->size());
|
||||
|
||||
ASSERT_TRUE(callEmpty("resume"));
|
||||
EXPECT_FALSE(node_->get_parameter("is_rtabmap_paused").as_bool());
|
||||
EXPECT_TRUE(updateAt(3.0, 1.0));
|
||||
EXPECT_EQ(2u, getGraph().graph.poses_id.size());
|
||||
}
|
||||
|
||||
/// is_rtabmap_paused starts the node paused, waiting for a resume.
|
||||
TEST_F(CoreWrapperServicesTest, is_rtabmap_paused_starts_paused)
|
||||
{
|
||||
makeNode({rclcpp::Parameter("is_rtabmap_paused", true)});
|
||||
info_ = collectInfo();
|
||||
odom_ = odomPublisher();
|
||||
|
||||
sendOdom(odom_, 1.0, 0.0);
|
||||
spinFor(std::chrono::milliseconds(500));
|
||||
EXPECT_TRUE(info_->empty());
|
||||
|
||||
ASSERT_TRUE(callEmpty("resume"));
|
||||
EXPECT_TRUE(updateAt(2.0, 0.5));
|
||||
}
|
||||
|
||||
/// reset erases the map, in memory and in the database, and numbering starts over.
|
||||
TEST_F(CoreWrapperServicesTest, reset_erases_the_map)
|
||||
{
|
||||
makeMap(3);
|
||||
|
||||
ASSERT_TRUE(callEmpty("reset"));
|
||||
EXPECT_TRUE(getGraph().graph.poses_id.empty());
|
||||
|
||||
ASSERT_TRUE(updateAt(10.0, 5.0));
|
||||
EXPECT_EQ(1, info_->back().ref_id);
|
||||
}
|
||||
|
||||
/// trigger_new_map starts a new session in the same database; the old one is kept.
|
||||
TEST_F(CoreWrapperServicesTest, trigger_new_map_starts_a_new_session)
|
||||
{
|
||||
makeMap(2);
|
||||
|
||||
ASSERT_TRUE(callEmpty("trigger_new_map"));
|
||||
ASSERT_TRUE(updateAt(10.0, 1.0));
|
||||
|
||||
EXPECT_EQ(std::vector<int>({0, 0, 1}), mapIds());
|
||||
}
|
||||
|
||||
/**
|
||||
* Labels name nodes, so a goal can be given as "kitchen" rather than as an id. Node 0
|
||||
* means the latest node.
|
||||
*/
|
||||
TEST_F(CoreWrapperServicesTest, labels_nodes)
|
||||
{
|
||||
makeMap(3);
|
||||
|
||||
ASSERT_TRUE(setLabel(1, "kitchen"));
|
||||
ASSERT_TRUE(setLabel(0, "door"));
|
||||
|
||||
rtabmap_msgs::srv::ListLabels::Response::SharedPtr labels = listLabels();
|
||||
ASSERT_TRUE(labels.get() != nullptr);
|
||||
ASSERT_EQ(2u, labels->ids.size());
|
||||
EXPECT_EQ(1, labels->ids[0]);
|
||||
EXPECT_EQ("kitchen", labels->labels[0]);
|
||||
EXPECT_EQ(3, labels->ids[1]);
|
||||
EXPECT_EQ("door", labels->labels[1]);
|
||||
EXPECT_EQ("kitchen", getNode(1).label);
|
||||
}
|
||||
|
||||
TEST_F(CoreWrapperServicesTest, removes_a_label)
|
||||
{
|
||||
makeMap(2);
|
||||
ASSERT_TRUE(setLabel(1, "kitchen"));
|
||||
ASSERT_TRUE(setLabel(2, "door"));
|
||||
|
||||
rtabmap_msgs::srv::RemoveLabel::Request::SharedPtr req =
|
||||
std::make_shared<rtabmap_msgs::srv::RemoveLabel::Request>();
|
||||
req->label = "kitchen";
|
||||
ASSERT_TRUE(call<rtabmap_msgs::srv::RemoveLabel>("remove_label", req).get() != nullptr);
|
||||
|
||||
rtabmap_msgs::srv::ListLabels::Response::SharedPtr labels = listLabels();
|
||||
ASSERT_TRUE(labels.get() != nullptr);
|
||||
EXPECT_EQ(std::vector<std::string>({"door"}), labels->labels);
|
||||
}
|
||||
|
||||
/// A label is unique in the map: setting it on another node is refused.
|
||||
TEST_F(CoreWrapperServicesTest, refuses_a_duplicate_label)
|
||||
{
|
||||
makeMap(2);
|
||||
ASSERT_TRUE(setLabel(1, "kitchen"));
|
||||
ASSERT_TRUE(setLabel(2, "kitchen"));
|
||||
|
||||
rtabmap_msgs::srv::ListLabels::Response::SharedPtr labels = listLabels();
|
||||
ASSERT_TRUE(labels.get() != nullptr);
|
||||
EXPECT_EQ(std::vector<int>({1}), labels->ids);
|
||||
}
|
||||
|
||||
/// get_node_data with no id returns the latest node.
|
||||
TEST_F(CoreWrapperServicesTest, get_node_data_defaults_to_the_latest_node)
|
||||
{
|
||||
makeMap(3);
|
||||
|
||||
rtabmap_msgs::srv::GetNodeData::Response::SharedPtr res =
|
||||
call<rtabmap_msgs::srv::GetNodeData>("get_node_data");
|
||||
ASSERT_TRUE(res.get() != nullptr);
|
||||
ASSERT_EQ(1u, res->data.size());
|
||||
EXPECT_EQ(3, res->data[0].id);
|
||||
EXPECT_NEAR(1.0, res->data[0].pose.position.x, 1e-4);
|
||||
}
|
||||
|
||||
//==========================================================================================
|
||||
// What each map service returns, payload by payload
|
||||
//==========================================================================================
|
||||
|
||||
/**
|
||||
* Every kind of data a node can hold, as one of the map services returned it for node 1.
|
||||
* The graph itself (poses, links) is returned whatever is asked for.
|
||||
*/
|
||||
struct Payloads
|
||||
{
|
||||
bool images = false;
|
||||
bool scans = false;
|
||||
bool userData = false;
|
||||
bool grids = false;
|
||||
bool words = false;
|
||||
bool globalDescriptors = false;
|
||||
|
||||
static Payloads of(const rtabmap_msgs::msg::Node & node)
|
||||
{
|
||||
Payloads p;
|
||||
p.images = !node.data.left_compressed.empty() && !node.data.right_compressed.empty();
|
||||
p.scans = !node.data.laser_scan_compressed.empty();
|
||||
p.userData = !node.data.user_data.empty();
|
||||
p.grids = !node.data.grid_obstacles.empty() || !node.data.grid_empty_cells.empty();
|
||||
p.words = !node.word_id_keys.empty();
|
||||
p.globalDescriptors = !node.data.global_descriptors.empty();
|
||||
return p;
|
||||
}
|
||||
|
||||
bool operator==(const Payloads & o) const
|
||||
{
|
||||
return images == o.images && scans == o.scans && userData == o.userData &&
|
||||
grids == o.grids && words == o.words && globalDescriptors == o.globalDescriptors;
|
||||
}
|
||||
};
|
||||
|
||||
std::ostream & operator<<(std::ostream & os, const Payloads & p)
|
||||
{
|
||||
return os << "{images=" << p.images << " scans=" << p.scans << " user_data=" << p.userData
|
||||
<< " grids=" << p.grids << " words=" << p.words
|
||||
<< " global_descriptors=" << p.globalDescriptors << "}";
|
||||
}
|
||||
|
||||
/// How the sensor data reaches the node.
|
||||
enum class MapInput
|
||||
{
|
||||
RgbdAndScan, ///< rgbd_image and scan, synchronized with odom; user data on user_data_async
|
||||
SensorData ///< the same data packed in one rtabmap_msgs/SensorData, user data included
|
||||
};
|
||||
|
||||
std::string toString(MapInput input)
|
||||
{
|
||||
return input == MapInput::RgbdAndScan ? "rgbd_and_scan" : "sensor_data";
|
||||
}
|
||||
|
||||
/**
|
||||
* A map whose nodes carry everything at once: an RGB-D camera, from which visual words
|
||||
* are extracted, with a global descriptor, a 2D lidar, from which the local occupancy
|
||||
* grid is built, and user data. Built from either input, with the same data.
|
||||
*/
|
||||
class CoreWrapperMapPayloadsBase : public CoreWrapperServicesTest
|
||||
{
|
||||
protected:
|
||||
static constexpr int kWidth = 320;
|
||||
static constexpr int kHeight = 240;
|
||||
static constexpr double kCameraHeight = 0.3;
|
||||
|
||||
void buildMap(MapInput input, const std::vector<rclcpp::Parameter> & extra = {})
|
||||
{
|
||||
publishStaticTf("laser", 0.1);
|
||||
publishOpticalTf("camera", kCameraHeight);
|
||||
|
||||
std::vector<rclcpp::Parameter> params = {
|
||||
rclcpp::Parameter("subscribe_depth", false),
|
||||
rclcpp::Parameter("subscribe_rgb", false),
|
||||
// Set explicitly: the node switches the grid to the scan by itself only when a
|
||||
// scan topic is subscribed, not for a scan inside sensor_data.
|
||||
rclcpp::Parameter(Parameters::kGridSensor(), "0"),
|
||||
rclcpp::Parameter(Parameters::kGridRangeMax(), "0")};
|
||||
if(input == MapInput::RgbdAndScan)
|
||||
{
|
||||
params.push_back(rclcpp::Parameter("subscribe_rgbd", true));
|
||||
params.push_back(rclcpp::Parameter("subscribe_scan", true));
|
||||
}
|
||||
else
|
||||
{
|
||||
params.push_back(rclcpp::Parameter("subscribe_sensor_data", true));
|
||||
}
|
||||
params.insert(params.end(), extra.begin(), extra.end());
|
||||
makeNode(params);
|
||||
info_ = collectInfo();
|
||||
odom_ = odomPublisher();
|
||||
|
||||
rclcpp::Publisher<rtabmap_msgs::msg::RGBDImage>::SharedPtr rgbd;
|
||||
rclcpp::Publisher<sensor_msgs::msg::LaserScan>::SharedPtr scan;
|
||||
rclcpp::Publisher<rtabmap_msgs::msg::UserData>::SharedPtr userData;
|
||||
rclcpp::Publisher<rtabmap_msgs::msg::SensorData>::SharedPtr sensorData;
|
||||
if(input == MapInput::RgbdAndScan)
|
||||
{
|
||||
rgbd = helper()->create_publisher<rtabmap_msgs::msg::RGBDImage>("rgbd_image", 10);
|
||||
scan = helper()->create_publisher<sensor_msgs::msg::LaserScan>("scan", 10);
|
||||
userData = helper()->create_publisher<rtabmap_msgs::msg::UserData>("user_data_async", 1);
|
||||
ASSERT_TRUE(waitForSubscriber(rgbd));
|
||||
ASSERT_TRUE(waitForSubscriber(scan));
|
||||
ASSERT_TRUE(waitForSubscriber(userData));
|
||||
}
|
||||
else
|
||||
{
|
||||
sensorData = helper()->create_publisher<rtabmap_msgs::msg::SensorData>("sensor_data", 10);
|
||||
ASSERT_TRUE(waitForSubscriber(sensorData));
|
||||
}
|
||||
|
||||
// For packing the scan the way the node converts it: in base_link, from the laser.
|
||||
tf2_ros::Buffer tfBuffer(helper()->get_clock());
|
||||
tfBuffer.setUsingDedicatedThread(true); // static transform set below, nothing to wait for
|
||||
geometry_msgs::msg::TransformStamped laserTf = makeTransform("base_link", "laser", 0.0, 0.1);
|
||||
tfBuffer.setTransform(laserTf, "test", true);
|
||||
|
||||
for(int i=0; i<2; ++i)
|
||||
{
|
||||
const double stamp = 1.0 + i;
|
||||
const cv::Mat rgb = texturedImage(kWidth, kHeight, 7 + i);
|
||||
const cv::Mat depth = depthImage(kWidth, kHeight);
|
||||
const sensor_msgs::msg::CameraInfo cameraInfo =
|
||||
makeCameraInfo("camera", stamp, kWidth, kHeight, 250.0);
|
||||
const sensor_msgs::msg::LaserScan scanMsg = makeRoomScan("laser", stamp, 0.5*i + 0.1);
|
||||
const rtabmap_msgs::msg::UserData userDataMsg = makeUserData(stamp);
|
||||
const cv::Mat descriptor = cv::Mat::ones(1, 8, CV_32FC1);
|
||||
|
||||
const size_t before = info_->size();
|
||||
if(input == MapInput::RgbdAndScan)
|
||||
{
|
||||
userData->publish(userDataMsg);
|
||||
spinFor(std::chrono::milliseconds(50));
|
||||
|
||||
rtabmap_msgs::msg::RGBDImage msg;
|
||||
msg.header.frame_id = "camera";
|
||||
msg.header.stamp = stampOf(stamp);
|
||||
msg.rgb = makeImage("camera", stamp, rgb, "bgr8");
|
||||
msg.depth = makeImage("camera", stamp, depth, "16UC1");
|
||||
msg.rgb_camera_info = cameraInfo;
|
||||
msg.depth_camera_info = cameraInfo;
|
||||
msg.global_descriptor.header = msg.header;
|
||||
msg.global_descriptor.data = rtabmap::compressData(descriptor);
|
||||
|
||||
sendOdom(odom_, stamp, 0.5*i);
|
||||
rgbd->publish(msg);
|
||||
scan->publish(scanMsg);
|
||||
}
|
||||
else
|
||||
{
|
||||
// Packed with the node's own conversions, as the odometry nodes republish
|
||||
// what they processed on odom_sensor_data/raw.
|
||||
rtabmap::LaserScan laserScan;
|
||||
ASSERT_TRUE(rtabmap_conversions::convertScanMsg(
|
||||
scanMsg, "base_link", "", stampOf(stamp), laserScan, tfBuffer, 0.0));
|
||||
rtabmap::SensorData data(
|
||||
laserScan, rgb, depth,
|
||||
rtabmap_conversions::cameraModelFromROS(cameraInfo,
|
||||
rtabmap_conversions::transformFromGeometryMsg(
|
||||
opticalTransform("camera", kCameraHeight).transform)),
|
||||
0, stamp,
|
||||
rtabmap_conversions::userDataFromROS(userDataMsg));
|
||||
data.setGlobalDescriptors(std::vector<rtabmap::GlobalDescriptor>(
|
||||
1, rtabmap::GlobalDescriptor(0, descriptor)));
|
||||
rtabmap_msgs::msg::SensorData msg;
|
||||
rtabmap_conversions::sensorDataToROS(data, msg, "base_link", true);
|
||||
|
||||
sendOdom(odom_, stamp, 0.5*i);
|
||||
sensorData->publish(msg);
|
||||
}
|
||||
ASSERT_TRUE(spinUntil([&]() { return info_->size() > before; }))
|
||||
<< "update " << i << " was not processed";
|
||||
}
|
||||
}
|
||||
|
||||
rtabmap_msgs::msg::MapData getMapData2(const Payloads & asked)
|
||||
{
|
||||
rtabmap_msgs::srv::GetMap2::Request::SharedPtr req =
|
||||
std::make_shared<rtabmap_msgs::srv::GetMap2::Request>();
|
||||
req->global_map = true;
|
||||
req->optimized = true;
|
||||
req->with_images = asked.images;
|
||||
req->with_scans = asked.scans;
|
||||
req->with_user_data = asked.userData;
|
||||
req->with_grids = asked.grids;
|
||||
req->with_words = asked.words;
|
||||
req->with_global_descriptors = asked.globalDescriptors;
|
||||
rtabmap_msgs::srv::GetMap2::Response::SharedPtr res =
|
||||
call<rtabmap_msgs::srv::GetMap2>("get_map_data2", req);
|
||||
EXPECT_TRUE(res.get() != nullptr);
|
||||
return res ? res->data : rtabmap_msgs::msg::MapData();
|
||||
}
|
||||
|
||||
rtabmap_msgs::msg::MapData getMapData(bool graphOnly)
|
||||
{
|
||||
rtabmap_msgs::srv::GetMap::Request::SharedPtr req =
|
||||
std::make_shared<rtabmap_msgs::srv::GetMap::Request>();
|
||||
req->global_map = true;
|
||||
req->optimized = true;
|
||||
req->graph_only = graphOnly;
|
||||
rtabmap_msgs::srv::GetMap::Response::SharedPtr res =
|
||||
call<rtabmap_msgs::srv::GetMap>("get_map_data", req);
|
||||
EXPECT_TRUE(res.get() != nullptr);
|
||||
return res ? res->data : rtabmap_msgs::msg::MapData();
|
||||
}
|
||||
|
||||
/// Node 1 of @p map, with the graph checked to be complete whatever was asked for.
|
||||
static rtabmap_msgs::msg::Node node1(const rtabmap_msgs::msg::MapData & map)
|
||||
{
|
||||
EXPECT_EQ(2u, map.graph.poses_id.size());
|
||||
EXPECT_EQ("map", map.header.frame_id);
|
||||
for(const rtabmap_msgs::msg::Node & n : map.nodes)
|
||||
{
|
||||
if(n.id == 1)
|
||||
{
|
||||
return n;
|
||||
}
|
||||
}
|
||||
ADD_FAILURE() << "node 1 is missing";
|
||||
return rtabmap_msgs::msg::Node();
|
||||
}
|
||||
|
||||
/// All six kinds of payload.
|
||||
static Payloads all()
|
||||
{
|
||||
Payloads p;
|
||||
p.images = p.scans = p.userData = p.grids = p.words = p.globalDescriptors = true;
|
||||
return p;
|
||||
}
|
||||
};
|
||||
|
||||
/// The payload tests below, run once per input.
|
||||
class CoreWrapperMapPayloadsTest :
|
||||
public CoreWrapperMapPayloadsBase,
|
||||
public ::testing::WithParamInterface<MapInput>
|
||||
{
|
||||
protected:
|
||||
void SetUp() override
|
||||
{
|
||||
CoreWrapperMapPayloadsBase::SetUp();
|
||||
buildMap(GetParam());
|
||||
}
|
||||
};
|
||||
|
||||
INSTANTIATE_TEST_SUITE_P(Inputs, CoreWrapperMapPayloadsTest,
|
||||
::testing::Values(MapInput::RgbdAndScan, MapInput::SensorData),
|
||||
[](const ::testing::TestParamInfo<MapInput> & info) { return toString(info.param); });
|
||||
|
||||
/// The map these tests build does hold every kind of payload, or the tests below prove nothing.
|
||||
TEST_P(CoreWrapperMapPayloadsTest, the_map_holds_every_payload)
|
||||
{
|
||||
EXPECT_EQ(all(), Payloads::of(node1(getMapData2(all()))));
|
||||
}
|
||||
|
||||
/**
|
||||
* get_map_data2 returns each kind of payload only when asked for it, so a client that
|
||||
* only needs, say, the scans does not download the images too.
|
||||
*/
|
||||
TEST_P(CoreWrapperMapPayloadsTest, get_map_data2_returns_only_the_payloads_asked_for)
|
||||
{
|
||||
EXPECT_EQ(Payloads(), Payloads::of(node1(getMapData2(Payloads()))));
|
||||
|
||||
const std::vector<std::pair<std::string, bool Payloads::*>> flags = {
|
||||
{"with_images", &Payloads::images},
|
||||
{"with_scans", &Payloads::scans},
|
||||
{"with_user_data", &Payloads::userData},
|
||||
{"with_grids", &Payloads::grids},
|
||||
{"with_words", &Payloads::words},
|
||||
{"with_global_descriptors", &Payloads::globalDescriptors}};
|
||||
for(const auto & flag : flags)
|
||||
{
|
||||
SCOPED_TRACE(flag.first);
|
||||
Payloads asked;
|
||||
asked.*(flag.second) = true;
|
||||
EXPECT_EQ(asked, Payloads::of(node1(getMapData2(asked))));
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* get_map_data is get_map_data2 with a single switch: everything, or with graph_only,
|
||||
* nothing but the graph and the nodes' metadata.
|
||||
*/
|
||||
TEST_P(CoreWrapperMapPayloadsTest, get_map_data_returns_everything_unless_graph_only)
|
||||
{
|
||||
EXPECT_EQ(all(), Payloads::of(node1(getMapData(false))));
|
||||
|
||||
rtabmap_msgs::msg::MapData graphOnly = getMapData(true);
|
||||
rtabmap_msgs::msg::Node node = node1(graphOnly);
|
||||
EXPECT_EQ(Payloads(), Payloads::of(node));
|
||||
EXPECT_NEAR(0.0, node.pose.position.x, 1e-4) << "the nodes are still there, without data";
|
||||
}
|
||||
|
||||
/**
|
||||
* get_node_data selects the images, the scan, the grid and the user data separately. The
|
||||
* visual words and the global descriptors have no switch: they always come along.
|
||||
*/
|
||||
TEST_P(CoreWrapperMapPayloadsTest, get_node_data_returns_only_the_payloads_asked_for)
|
||||
{
|
||||
const auto getNodeData = [&](bool images, bool scan, bool grid, bool userData) {
|
||||
rtabmap_msgs::srv::GetNodeData::Request::SharedPtr req =
|
||||
std::make_shared<rtabmap_msgs::srv::GetNodeData::Request>();
|
||||
req->ids = {1};
|
||||
req->images = images;
|
||||
req->scan = scan;
|
||||
req->grid = grid;
|
||||
req->user_data = userData;
|
||||
rtabmap_msgs::srv::GetNodeData::Response::SharedPtr res =
|
||||
call<rtabmap_msgs::srv::GetNodeData>("get_node_data", req);
|
||||
EXPECT_TRUE(res.get() != nullptr);
|
||||
EXPECT_TRUE(res && res->data.size() == 1u);
|
||||
return res && !res->data.empty() ? Payloads::of(res->data[0]) : Payloads();
|
||||
};
|
||||
|
||||
Payloads alwaysThere;
|
||||
alwaysThere.words = true;
|
||||
alwaysThere.globalDescriptors = true;
|
||||
|
||||
EXPECT_EQ(alwaysThere, getNodeData(false, false, false, false));
|
||||
{
|
||||
SCOPED_TRACE("images");
|
||||
Payloads expected = alwaysThere;
|
||||
expected.images = true;
|
||||
EXPECT_EQ(expected, getNodeData(true, false, false, false));
|
||||
}
|
||||
{
|
||||
SCOPED_TRACE("scan");
|
||||
Payloads expected = alwaysThere;
|
||||
expected.scans = true;
|
||||
EXPECT_EQ(expected, getNodeData(false, true, false, false));
|
||||
}
|
||||
{
|
||||
SCOPED_TRACE("grid");
|
||||
Payloads expected = alwaysThere;
|
||||
expected.grids = true;
|
||||
EXPECT_EQ(expected, getNodeData(false, false, true, false));
|
||||
}
|
||||
{
|
||||
SCOPED_TRACE("user_data");
|
||||
Payloads expected = alwaysThere;
|
||||
expected.userData = true;
|
||||
EXPECT_EQ(expected, getNodeData(false, false, false, true));
|
||||
}
|
||||
EXPECT_EQ(all(), getNodeData(true, true, true, true));
|
||||
}
|
||||
|
||||
class CoreWrapperMapInputsTest : public CoreWrapperMapPayloadsBase
|
||||
{
|
||||
protected:
|
||||
static void expectSameMat(const cv::Mat & a, const cv::Mat & b, const std::string & what,
|
||||
bool mayBeEmpty = false, double tolerance = 0.0)
|
||||
{
|
||||
if(!mayBeEmpty)
|
||||
{
|
||||
EXPECT_FALSE(a.empty()) << what << " is empty, so comparing it proves nothing";
|
||||
}
|
||||
ASSERT_EQ(a.empty(), b.empty()) << what;
|
||||
if(a.empty())
|
||||
{
|
||||
return;
|
||||
}
|
||||
ASSERT_EQ(a.size(), b.size()) << what;
|
||||
ASSERT_EQ(a.type(), b.type()) << what;
|
||||
EXPECT_LE(cv::norm(a, b, cv::NORM_INF), tolerance) << what;
|
||||
}
|
||||
|
||||
/// The words' keypoints of @p node, in pixels, sorted.
|
||||
static std::vector<std::pair<float, float>> keypoints(const rtabmap_msgs::msg::Node & node)
|
||||
{
|
||||
std::vector<std::pair<float, float>> out;
|
||||
for(const rtabmap_msgs::msg::KeyPoint & k : node.word_kpts)
|
||||
{
|
||||
out.push_back(std::make_pair(k.pt.x, k.pt.y));
|
||||
}
|
||||
std::sort(out.begin(), out.end());
|
||||
return out;
|
||||
}
|
||||
|
||||
/// The words' 3D points of @p node, sorted.
|
||||
static std::vector<std::tuple<float, float, float>> points(const rtabmap_msgs::msg::Node & node)
|
||||
{
|
||||
std::vector<std::tuple<float, float, float>> out;
|
||||
for(const rtabmap_msgs::msg::Point3f & p : node.word_pts)
|
||||
{
|
||||
out.push_back(std::make_tuple(p.x, p.y, p.z));
|
||||
}
|
||||
std::sort(out.begin(), out.end());
|
||||
return out;
|
||||
}
|
||||
|
||||
/// Node @p a and node @p b hold the same data, down to the pixel and the point.
|
||||
static void expectSameNode(const rtabmap_msgs::msg::Node & a, const rtabmap_msgs::msg::Node & b)
|
||||
{
|
||||
SCOPED_TRACE("node " + std::to_string(a.id));
|
||||
EXPECT_EQ(a.map_id, b.map_id);
|
||||
EXPECT_DOUBLE_EQ(a.stamp, b.stamp);
|
||||
EXPECT_NEAR(a.pose.position.x, b.pose.position.x, 1e-6);
|
||||
|
||||
rtabmap::SensorData da = rtabmap_conversions::sensorDataFromROS(a.data);
|
||||
rtabmap::SensorData db = rtabmap_conversions::sensorDataFromROS(b.data);
|
||||
cv::Mat rgbA, depthA, userA, groundA, obstaclesA, emptyA;
|
||||
cv::Mat rgbB, depthB, userB, groundB, obstaclesB, emptyB;
|
||||
rtabmap::LaserScan scanA, scanB;
|
||||
da.uncompressData(&rgbA, &depthA, &scanA, &userA, &groundA, &obstaclesA, &emptyA);
|
||||
db.uncompressData(&rgbB, &depthB, &scanB, &userB, &groundB, &obstaclesB, &emptyB);
|
||||
|
||||
expectSameMat(rgbA, rgbB, "rgb");
|
||||
expectSameMat(depthA, depthB, "depth");
|
||||
ASSERT_EQ(1u, da.cameraModels().size());
|
||||
ASSERT_EQ(1u, db.cameraModels().size());
|
||||
EXPECT_DOUBLE_EQ(da.cameraModels()[0].fx(), db.cameraModels()[0].fx());
|
||||
EXPECT_DOUBLE_EQ(da.cameraModels()[0].cx(), db.cameraModels()[0].cx());
|
||||
EXPECT_EQ(da.cameraModels()[0].imageSize(), db.cameraModels()[0].imageSize());
|
||||
EXPECT_EQ(da.cameraModels()[0].localTransform().prettyPrint(),
|
||||
db.cameraModels()[0].localTransform().prettyPrint());
|
||||
|
||||
// Converted through the odometry frame on one side (odom_sensor_sync) and straight
|
||||
// into base_link on the other: the same points, to float rounding.
|
||||
expectSameMat(scanA.data(), scanB.data(), "scan", false, 1e-5);
|
||||
EXPECT_EQ(scanA.format(), scanB.format());
|
||||
EXPECT_EQ(scanA.maxPoints(), scanB.maxPoints());
|
||||
EXPECT_FLOAT_EQ(scanA.rangeMax(), scanB.rangeMax());
|
||||
EXPECT_EQ(scanA.localTransform().prettyPrint(), scanB.localTransform().prettyPrint());
|
||||
|
||||
expectSameMat(userA, userB, "user data");
|
||||
|
||||
EXPECT_FLOAT_EQ(da.gridCellSize(), db.gridCellSize());
|
||||
expectSameMat(obstaclesA, obstaclesB, "grid obstacles", false, 1e-5);
|
||||
expectSameMat(emptyA, emptyB, "grid empty cells", false, 1e-5);
|
||||
expectSameMat(groundA, groundB, "grid ground", true, 1e-5);
|
||||
|
||||
// The same features are extracted, but not necessarily given the same word ids:
|
||||
// matching them against the dictionary is approximate, and the latest node's ids
|
||||
// differ from one run to the next even with the same input. So the keypoints and
|
||||
// their 3D points are compared, as sets.
|
||||
EXPECT_FALSE(a.word_kpts.empty());
|
||||
EXPECT_EQ(a.word_id_keys.size(), b.word_id_keys.size());
|
||||
EXPECT_EQ(keypoints(a), keypoints(b));
|
||||
EXPECT_EQ(points(a), points(b));
|
||||
|
||||
ASSERT_EQ(1u, a.data.global_descriptors.size());
|
||||
ASSERT_EQ(1u, b.data.global_descriptors.size());
|
||||
EXPECT_EQ(a.data.global_descriptors[0].type, b.data.global_descriptors[0].type);
|
||||
EXPECT_EQ(a.data.global_descriptors[0].data, b.data.global_descriptors[0].data);
|
||||
}
|
||||
};
|
||||
|
||||
/**
|
||||
* sensor_data is the same map as rgbd_image and scan, given the same data: every node
|
||||
* stores the same images, calibration, scan, user data, grid, visual words and global
|
||||
* descriptor, whichever way it arrived.
|
||||
*/
|
||||
TEST_F(CoreWrapperMapInputsTest, sensor_data_maps_like_rgbd_and_scan)
|
||||
{
|
||||
buildMap(MapInput::RgbdAndScan);
|
||||
const rtabmap_msgs::msg::MapData viaTopics = getMapData2(all());
|
||||
destroyNode();
|
||||
|
||||
buildMap(MapInput::SensorData, {rclcpp::Parameter("delete_db_on_start", true)});
|
||||
const rtabmap_msgs::msg::MapData viaSensorData = getMapData2(all());
|
||||
|
||||
ASSERT_EQ(2u, viaTopics.nodes.size());
|
||||
ASSERT_EQ(viaTopics.nodes.size(), viaSensorData.nodes.size());
|
||||
for(size_t i=0; i<viaTopics.nodes.size(); ++i)
|
||||
{
|
||||
ASSERT_EQ(viaTopics.nodes[i].id, viaSensorData.nodes[i].id);
|
||||
expectSameNode(viaTopics.nodes[i], viaSensorData.nodes[i]);
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* get_nodes_in_radius finds the nodes within a radius of either a node -- not counting
|
||||
* that node -- or a position, which is used when node_id is 0 and it is not the origin.
|
||||
*/
|
||||
TEST_F(CoreWrapperServicesTest, finds_nodes_in_a_radius)
|
||||
{
|
||||
makeMap(4); // x = 0, 0.5, 1.0, 1.5
|
||||
|
||||
rtabmap_msgs::srv::GetNodesInRadius::Request::SharedPtr req =
|
||||
std::make_shared<rtabmap_msgs::srv::GetNodesInRadius::Request>();
|
||||
req->node_id = 1;
|
||||
req->radius = 0.6f;
|
||||
rtabmap_msgs::srv::GetNodesInRadius::Response::SharedPtr res =
|
||||
call<rtabmap_msgs::srv::GetNodesInRadius>("get_nodes_in_radius", req);
|
||||
ASSERT_TRUE(res.get() != nullptr);
|
||||
std::vector<int> ids = res->ids;
|
||||
EXPECT_EQ(std::vector<int>({2}), ids);
|
||||
ASSERT_EQ(1u, res->dists_sqr.size());
|
||||
EXPECT_NEAR(0.25, res->dists_sqr[0], 1e-4);
|
||||
|
||||
req->node_id = 0;
|
||||
req->x = 1.4f;
|
||||
res = call<rtabmap_msgs::srv::GetNodesInRadius>("get_nodes_in_radius", req);
|
||||
ASSERT_TRUE(res.get() != nullptr);
|
||||
ids = res->ids;
|
||||
std::sort(ids.begin(), ids.end());
|
||||
EXPECT_EQ(std::vector<int>({3, 4}), ids);
|
||||
}
|
||||
|
||||
/**
|
||||
* In localization mode the map is not extended: updates are localized against it and
|
||||
* then forgotten. set_mode_mapping goes back to extending it, in a new session, since
|
||||
* nothing links where the robot is now to the map it left. Both are mirrored in the
|
||||
* Mem/IncrementalMemory parameter.
|
||||
*/
|
||||
TEST_F(CoreWrapperServicesTest, localization_mode_stops_extending_the_map)
|
||||
{
|
||||
makeMap(2);
|
||||
|
||||
ASSERT_TRUE(callEmpty("set_mode_localization"));
|
||||
EXPECT_EQ("false", param(Parameters::kMemIncrementalMemory()));
|
||||
ASSERT_TRUE(updateAt(10.0, 3.0));
|
||||
ASSERT_TRUE(updateAt(11.0, 3.5));
|
||||
EXPECT_EQ(2u, getGraph().graph.poses_id.size());
|
||||
|
||||
ASSERT_TRUE(callEmpty("set_mode_mapping"));
|
||||
EXPECT_EQ("true", param(Parameters::kMemIncrementalMemory()));
|
||||
ASSERT_TRUE(updateAt(12.0, 4.0));
|
||||
EXPECT_EQ(std::vector<int>({0, 0, 1}), mapIds());
|
||||
}
|
||||
|
||||
/**
|
||||
* RTAB-Map parameters can be changed while the node runs, with `ros2 param set`: the
|
||||
* node applies them as soon as they change.
|
||||
*/
|
||||
TEST_F(CoreWrapperServicesTest, applies_parameters_changed_at_runtime)
|
||||
{
|
||||
makeMap(1);
|
||||
|
||||
ASSERT_TRUE(node_->set_parameter(
|
||||
rclcpp::Parameter(Parameters::kRGBDLinearUpdate(), "2.0")).successful);
|
||||
spinFor(std::chrono::milliseconds(300)); // the change arrives as a parameter event
|
||||
ASSERT_TRUE(updateAt(2.0, 0.5));
|
||||
ASSERT_TRUE(updateAt(3.0, 1.0));
|
||||
|
||||
EXPECT_EQ(1u, getGraph().graph.poses_id.size())
|
||||
<< "0.5 m steps are now below the 2 m linear update";
|
||||
}
|
||||
|
||||
/**
|
||||
* backup saves the database as it is now to <database_path>.back, reloads it, and carries
|
||||
* on in a new session, as after a restart.
|
||||
*/
|
||||
TEST_F(CoreWrapperServicesTest, backup_copies_the_database)
|
||||
{
|
||||
makeMap(2);
|
||||
|
||||
ASSERT_TRUE(callEmpty("backup"));
|
||||
|
||||
EXPECT_TRUE(UFile::exists(databasePath() + ".back"));
|
||||
EXPECT_EQ(2u, getGraph().graph.poses_id.size());
|
||||
ASSERT_TRUE(updateAt(10.0, 2.0));
|
||||
EXPECT_EQ(std::vector<int>({0, 0, 1}), mapIds());
|
||||
}
|
||||
|
||||
/**
|
||||
* load_database saves the current map and switches to another database -- a new one, or
|
||||
* one whose map is reloaded. clear starts the target over.
|
||||
*/
|
||||
TEST_F(CoreWrapperServicesTest, load_database_switches_maps)
|
||||
{
|
||||
makeMap(2);
|
||||
|
||||
rtabmap_msgs::srv::LoadDatabase::Request::SharedPtr req =
|
||||
std::make_shared<rtabmap_msgs::srv::LoadDatabase::Request>();
|
||||
req->database_path = dir() + "/other.db";
|
||||
req->clear = true;
|
||||
ASSERT_TRUE(call<rtabmap_msgs::srv::LoadDatabase>("load_database", req).get() != nullptr);
|
||||
EXPECT_TRUE(getGraph().graph.poses_id.empty());
|
||||
ASSERT_TRUE(updateAt(10.0, 0.0));
|
||||
EXPECT_EQ(1, info_->back().ref_id);
|
||||
|
||||
req->database_path = databasePath();
|
||||
req->clear = false;
|
||||
ASSERT_TRUE(call<rtabmap_msgs::srv::LoadDatabase>("load_database", req).get() != nullptr);
|
||||
EXPECT_EQ(2u, getGraph().graph.poses_id.size());
|
||||
EXPECT_TRUE(UFile::exists(dir() + "/other.db"));
|
||||
}
|
||||
|
||||
/// A database path in a directory that does not exist is refused, and the map is kept.
|
||||
TEST_F(CoreWrapperServicesTest, load_database_refuses_a_missing_directory)
|
||||
{
|
||||
makeMap(2);
|
||||
|
||||
rtabmap_msgs::srv::LoadDatabase::Request::SharedPtr req =
|
||||
std::make_shared<rtabmap_msgs::srv::LoadDatabase::Request>();
|
||||
req->database_path = dir() + "/no/such/dir/other.db";
|
||||
ASSERT_TRUE(call<rtabmap_msgs::srv::LoadDatabase>("load_database", req).get() != nullptr);
|
||||
|
||||
EXPECT_EQ(2u, getGraph().graph.poses_id.size());
|
||||
}
|
||||
|
||||
/**
|
||||
* publish_map republishes the map on demand to whatever is subscribed -- the whole
|
||||
* database's with global_map, and just the graph with graph_only.
|
||||
*/
|
||||
TEST_F(CoreWrapperServicesTest, publish_map_republishes_on_demand)
|
||||
{
|
||||
makeMap(3);
|
||||
std::shared_ptr<Collector<rtabmap_msgs::msg::MapGraph>> graph =
|
||||
collect<rtabmap_msgs::msg::MapGraph>("mapGraph",
|
||||
rclcpp::QoS(1).reliable().transient_local());
|
||||
ASSERT_TRUE(waitForPublisher(graph->subscription));
|
||||
spinFor(std::chrono::milliseconds(200));
|
||||
const size_t before = graph->size();
|
||||
|
||||
rtabmap_msgs::srv::PublishMap::Request::SharedPtr req =
|
||||
std::make_shared<rtabmap_msgs::srv::PublishMap::Request>();
|
||||
req->global_map = true;
|
||||
req->optimized = true;
|
||||
req->graph_only = true;
|
||||
ASSERT_TRUE(call<rtabmap_msgs::srv::PublishMap>("publish_map", req).get() != nullptr);
|
||||
|
||||
ASSERT_TRUE(spinUntil([&]() { return graph->size() > before; }));
|
||||
EXPECT_EQ(3u, graph->back().poses_id.size());
|
||||
}
|
||||
|
||||
/**
|
||||
* add_link adds a constraint from outside -- a loop closure found by another process,
|
||||
* say -- to the graph, which is then optimized with it.
|
||||
*/
|
||||
TEST_F(CoreWrapperServicesTest, add_link_adds_a_constraint)
|
||||
{
|
||||
makeMap(3);
|
||||
|
||||
rtabmap_msgs::srv::AddLink::Request::SharedPtr req =
|
||||
std::make_shared<rtabmap_msgs::srv::AddLink::Request>();
|
||||
req->link.from_id = 3;
|
||||
req->link.to_id = 1;
|
||||
req->link.type = rtabmap::Link::kUserClosure;
|
||||
req->link.transform.translation.x = -1.0;
|
||||
req->link.transform.rotation.w = 1.0;
|
||||
for(int i=0; i<6; ++i)
|
||||
{
|
||||
req->link.information[i*7] = 100.0;
|
||||
}
|
||||
ASSERT_TRUE(call<rtabmap_msgs::srv::AddLink>("add_link", req).get() != nullptr);
|
||||
|
||||
bool found = false;
|
||||
for(const rtabmap_msgs::msg::Link & l : getGraph().graph.links)
|
||||
{
|
||||
found = found || (l.type == rtabmap::Link::kUserClosure &&
|
||||
((l.from_id == 3 && l.to_id == 1) || (l.from_id == 1 && l.to_id == 3)));
|
||||
}
|
||||
EXPECT_TRUE(found);
|
||||
}
|
||||
|
||||
/// The log_* services set RTAB-Map's own log level, independently from ROS's.
|
||||
TEST_F(CoreWrapperServicesTest, log_services_set_rtabmap_log_level)
|
||||
{
|
||||
makeNode();
|
||||
const ULogger::Level initial = ULogger::level();
|
||||
|
||||
ASSERT_TRUE(callEmpty("log_debug"));
|
||||
EXPECT_EQ(ULogger::kDebug, ULogger::level());
|
||||
ASSERT_TRUE(callEmpty("log_info"));
|
||||
EXPECT_EQ(ULogger::kInfo, ULogger::level());
|
||||
ASSERT_TRUE(callEmpty("log_error"));
|
||||
EXPECT_EQ(ULogger::kError, ULogger::level());
|
||||
ASSERT_TRUE(callEmpty("log_warning"));
|
||||
EXPECT_EQ(ULogger::kWarning, ULogger::level());
|
||||
|
||||
ULogger::setLevel(initial);
|
||||
}
|
||||
|
||||
} // namespace
|
||||
|
||||
} // namespace rtabmap_slam_test
|
||||
Reference in New Issue
Block a user