mirror of
https://github.com/introlab/rtabmap_ros.git
synced 2026-10-04 00:37:46 +08:00
matlabbeandmathieu86
11edc01d6a
* rtabmap_odom tests and doc * opengv note * added ci checks or humble-latest flaky dep cmake errors * Added real data tests for rgbd_odom and stereo_odom * added real data for icp_odometry's deskewing test * fixing json cmake error on lyrical/rolling * test 2d icp odom deskewing branch * first review of existing OdometryROS tests * testing with imu used as guess * tested imu arrivals sync * Fixed odom reset on right pose when guess frame id is used * fixing header errors in ci >=lyrical * Added support for input rgbd_image topic with features for odom, added multicam rgbd_odometry test * Added stereo odom support for features-only frames. Added multicam stereo tests. * forcing latest rtabmap version * updated OdometryROS API * ci: dont build non-latest docker in pull requests * splitting docker jobs * doc edit * Making publish_null_when_lost:=false continous when guess is provided (using guess covariance when we cannot register yet) * updated stereo doc * ficing rolling ci (rviz Ogre header) * Added test coverage of alll rgbd_image callbacks * fixing rolling ci * making docker ci build/run the tests on pull requests * fixing ros2 ci testing * improved sync callback coverage * improving stereo_odometry test coverage * improved icp_odometry test coverage * lyrical voxel_grid ptr error * make multicam tests working as well without opengv * removing deps of missing packages on rolling * PCL empty cloud conversion compiler errors fix * fixing icp_odometry test failure on ci witohut libpointmatcher * fixing nav2 costmap plugin build on lyrical * joining thread when exiting * updating icp test to work the same on pcl 1.15 (lyrical) * Fix parallel tests seg fault --------- Co-authored-by: mathieu86 <[email protected]>
80 lines
2.2 KiB
C++
80 lines
2.2 KiB
C++
/*
|
|
Copyright (c) 2010-2026, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
|
|
All rights reserved. (BSD-3-Clause, see the repository root.)
|
|
*/
|
|
|
|
#ifndef RTABMAP_ODOM_BAG_PLAYBACK_HPP_
|
|
#define RTABMAP_ODOM_BAG_PLAYBACK_HPP_
|
|
|
|
#include <rclcpp/serialization.hpp>
|
|
#include <rosbag2_cpp/reader.hpp>
|
|
|
|
#include <string>
|
|
#include <vector>
|
|
|
|
#include "test_data.hpp"
|
|
|
|
/**
|
|
* @file
|
|
* @brief Reads recorded messages out of a bag, for tests that need real sensor input.
|
|
*
|
|
* The messages are read and replayed by the test itself rather than by `ros2 bag play`:
|
|
* no second process, no wall-clock pacing, and the test controls exactly when each
|
|
* message reaches the node. What the recording provides is the part that cannot be
|
|
* written by hand -- a real driver's cloud layout and a dense TF history around it.
|
|
*/
|
|
|
|
namespace rtabmap_odom_test {
|
|
|
|
/**
|
|
* @brief The Ouster recording in test/data/lidar; see that directory's README.
|
|
*
|
|
* Two sweeps half a mast turn apart, from a platform that never moves: the only thing
|
|
* between them is the mast's rotation, which TF describes in full.
|
|
*/
|
|
inline std::string ousterHalfTurnBag()
|
|
{
|
|
return testDataRoot() + "/lidar/ouster_half_turn";
|
|
}
|
|
|
|
/**
|
|
* @brief Every message recorded on @p topic, deserialized.
|
|
*
|
|
* Returns an empty vector if the bag or the topic is missing, which the caller is
|
|
* expected to assert on -- a silently empty fixture would make a test pass for the wrong
|
|
* reason.
|
|
*/
|
|
template <typename MsgT>
|
|
std::vector<MsgT> readBagMessages(const std::string & bagPath, const std::string & topic)
|
|
{
|
|
std::vector<MsgT> messages;
|
|
rosbag2_cpp::Reader reader;
|
|
try
|
|
{
|
|
reader.open(bagPath);
|
|
}
|
|
catch(const std::exception & e)
|
|
{
|
|
return messages;
|
|
}
|
|
|
|
rclcpp::Serialization<MsgT> serialization;
|
|
while(reader.has_next())
|
|
{
|
|
const std::shared_ptr<rosbag2_storage::SerializedBagMessage> message = reader.read_next();
|
|
if(message->topic_name != topic)
|
|
{
|
|
continue;
|
|
}
|
|
rclcpp::SerializedMessage serialized(*message->serialized_data);
|
|
MsgT deserialized;
|
|
serialization.deserialize_message(&serialized, &deserialized);
|
|
messages.push_back(deserialized);
|
|
}
|
|
return messages;
|
|
}
|
|
|
|
} // namespace rtabmap_odom_test
|
|
|
|
#endif /* RTABMAP_ODOM_BAG_PLAYBACK_HPP_ */
|