mirror of
https://github.com/introlab/rtabmap_ros.git
synced 2026-10-05 01:07:49 +08:00
rtabmap_odom tests and doc (#1456)
* rtabmap_odom tests and doc * opengv note * added ci checks or humble-latest flaky dep cmake errors * Added real data tests for rgbd_odom and stereo_odom * added real data for icp_odometry's deskewing test * fixing json cmake error on lyrical/rolling * test 2d icp odom deskewing branch * first review of existing OdometryROS tests * testing with imu used as guess * tested imu arrivals sync * Fixed odom reset on right pose when guess frame id is used * fixing header errors in ci >=lyrical * Added support for input rgbd_image topic with features for odom, added multicam rgbd_odometry test * Added stereo odom support for features-only frames. Added multicam stereo tests. * forcing latest rtabmap version * updated OdometryROS API * ci: dont build non-latest docker in pull requests * splitting docker jobs * doc edit * Making publish_null_when_lost:=false continous when guess is provided (using guess covariance when we cannot register yet) * updated stereo doc * ficing rolling ci (rviz Ogre header) * Added test coverage of alll rgbd_image callbacks * fixing rolling ci * making docker ci build/run the tests on pull requests * fixing ros2 ci testing * improved sync callback coverage * improving stereo_odometry test coverage * improved icp_odometry test coverage * lyrical voxel_grid ptr error * make multicam tests working as well without opengv * removing deps of missing packages on rolling * PCL empty cloud conversion compiler errors fix * fixing icp_odometry test failure on ci witohut libpointmatcher * fixing nav2 costmap plugin build on lyrical * joining thread when exiting * updating icp test to work the same on pcl 1.15 (lyrical) * Fix parallel tests seg fault --------- Co-authored-by: mathieu86 <[email protected]>
This commit is contained in:
matlabbe
@@ -58,57 +58,144 @@ namespace rtabmap {
|
||||
class Odometry;
|
||||
}
|
||||
|
||||
/**
|
||||
* @file
|
||||
* @brief The node the three odometry nodes of this package are built on.
|
||||
*/
|
||||
|
||||
namespace rtabmap_odom {
|
||||
|
||||
/**
|
||||
* @brief Runs RTAB-Map's odometry as a ROS node: everything but the subscriptions.
|
||||
*
|
||||
* `rgbd_odometry`, `stereo_odometry` and `icp_odometry` differ only in what they listen
|
||||
* to. Each turns its own topics into a rtabmap::SensorData and hands it to processData();
|
||||
* from there on this class does the work -- registration, pose integration, the `odom`
|
||||
* topic and its TF, the IMU intake, the services, the diagnostics, the reset policy when
|
||||
* tracking is lost. That is why the three nodes share nearly all of their parameters and
|
||||
* publish the same topics.
|
||||
*
|
||||
* A subclass is expected to:
|
||||
* - call init() from its constructor, saying which families of RTAB-Map parameters it
|
||||
* accepts, which decides both the defaults and what the node will accept being set;
|
||||
* - create its subscriptions in onOdomInit() and describe them with initDiagnosticMsg();
|
||||
* - call tick() when a message arrives and processData() once a frame is complete;
|
||||
* - implement flushCallbacks(), so that a reset can drop whatever its synchronizer holds.
|
||||
*
|
||||
* The class is also a UThread. By default the frame handed to processData() is passed to
|
||||
* that thread and the callback returns at once, so a slow registration cannot block the
|
||||
* executor; a frame arriving while the thread is busy is dropped rather than queued. With
|
||||
* `always_process_most_recent_frame:=false` it is registered on the calling thread
|
||||
* instead, which keeps every frame at the cost of holding up the executor.
|
||||
*
|
||||
* @see the package README for the parameters and topics these nodes have in common.
|
||||
*/
|
||||
class OdometryROS : public rclcpp::Node, public UThread
|
||||
{
|
||||
|
||||
public:
|
||||
/// Constructs the node under its default name.
|
||||
explicit OdometryROS(const rclcpp::NodeOptions & options);
|
||||
/// Constructs the node under @p name, which is what the three nodes use.
|
||||
explicit OdometryROS(const std::string & name, const rclcpp::NodeOptions & options);
|
||||
virtual ~OdometryROS();
|
||||
|
||||
/**
|
||||
* @brief Hands a complete frame to the odometry; called by a subclass's callback.
|
||||
* @param[in,out] data the frame to register, which comes back carrying the
|
||||
* features the odometry ended up using
|
||||
* @param[in] header stamp and frame of the data, used to publish the result
|
||||
*
|
||||
* The frame is either queued for the worker thread or registered right here,
|
||||
* depending on `always_process_most_recent_frame`. Either way, a frame that arrives
|
||||
* while the previous one is still being registered is dropped: the odometry stays on
|
||||
* the newest data rather than falling behind.
|
||||
*/
|
||||
void processData(rtabmap::SensorData & data, const std_msgs::msg::Header & header);
|
||||
|
||||
/// `reset_odom` service: starts a new map at the origin, or at the guess frame's pose.
|
||||
void resetOdom(const std::shared_ptr<rmw_request_id_t>, const std::shared_ptr<std_srvs::srv::Empty::Request>, std::shared_ptr<std_srvs::srv::Empty::Response>);
|
||||
/// `reset_odom_to_pose` service: starts a new map at the pose given in the request.
|
||||
void resetToPose(const std::shared_ptr<rmw_request_id_t>, const std::shared_ptr<rtabmap_msgs::srv::ResetPose::Request>, std::shared_ptr<rtabmap_msgs::srv::ResetPose::Response>);
|
||||
/// `pause_odom` service: keeps the subscriptions but stops registering what arrives.
|
||||
void pause(const std::shared_ptr<rmw_request_id_t>, const std::shared_ptr<std_srvs::srv::Empty::Request>, std::shared_ptr<std_srvs::srv::Empty::Response>);
|
||||
/// `resume_odom` service: registers again, starting from the next frame.
|
||||
void resume(const std::shared_ptr<rmw_request_id_t>, const std::shared_ptr<std_srvs::srv::Empty::Request>, std::shared_ptr<std_srvs::srv::Empty::Response>);
|
||||
/// `log_debug` service: raises RTAB-Map's own log level to debug at runtime.
|
||||
void setLogDebug(const std::shared_ptr<rmw_request_id_t>, const std::shared_ptr<std_srvs::srv::Empty::Request>, std::shared_ptr<std_srvs::srv::Empty::Response>);
|
||||
/// `log_info` service; see setLogDebug().
|
||||
void setLogInfo(const std::shared_ptr<rmw_request_id_t>, const std::shared_ptr<std_srvs::srv::Empty::Request>, std::shared_ptr<std_srvs::srv::Empty::Response>);
|
||||
/// `log_warning` service; see setLogDebug().
|
||||
void setLogWarn(const std::shared_ptr<rmw_request_id_t>, const std::shared_ptr<std_srvs::srv::Empty::Request>, std::shared_ptr<std_srvs::srv::Empty::Response>);
|
||||
/// `log_error` service; see setLogDebug().
|
||||
void setLogError(const std::shared_ptr<rmw_request_id_t>, const std::shared_ptr<std_srvs::srv::Empty::Request>, std::shared_ptr<std_srvs::srv::Empty::Response>);
|
||||
|
||||
/// The robot frame the odometry is computed for, `frame_id`.
|
||||
const std::string & frameId() const {return frameId_;}
|
||||
/// The frame the estimated poses are expressed in, `odom_frame_id`.
|
||||
const std::string & odomFrameId() const {return odomFrameId_;}
|
||||
/// The frame an external motion guess is read from, `guess_frame_id`; empty if unused.
|
||||
const std::string & guessFrameId() const {return guessFrameId_;}
|
||||
/// The RTAB-Map parameters this node was configured with, defaults included.
|
||||
const rtabmap::ParametersMap & parameters() const {return parameters_;}
|
||||
/// Whether the `pause_odom` service has been called and not resumed since.
|
||||
bool isPaused() const {return paused_;}
|
||||
|
||||
protected:
|
||||
/**
|
||||
* @brief Declares the node's parameters and creates the odometry; call it last in the
|
||||
* subclass constructor.
|
||||
* @param[in] stereoParams true if the node takes RTAB-Map's stereo parameters
|
||||
* @param[in] visParams true if it takes the visual registration ones
|
||||
* @param[in] icpParams true if it takes the scan matching ones
|
||||
*
|
||||
* The three flags decide which RTAB-Map parameters the node declares, and so which
|
||||
* ones it accepts being set: `icp_odometry` refuses a `Vis/` parameter and the other
|
||||
* two refuse an `Icp/` one. onOdomInit() is called at the end, for the subclass to
|
||||
* create its subscriptions.
|
||||
*/
|
||||
void init(bool stereoParams, bool visParams, bool icpParams);
|
||||
/// The reliability the subclass should give its own subscriptions, from `qos`.
|
||||
rmw_qos_reliability_policy_t qos() const {return qos_;}
|
||||
/**
|
||||
* @brief Starts the diagnostics, once the subclass knows what it subscribed to.
|
||||
* @param[in] subscribedTopicsMsg the human readable list logged at startup and
|
||||
* repeated in the "no data received" warning
|
||||
* @param[in] approxSync whether the subclass matches stamps approximately,
|
||||
* which that warning mentions as a likely cause
|
||||
* @param[in] subscribedTopic the one topic whose rate is watched, if any
|
||||
*/
|
||||
void initDiagnosticMsg(const std::string & subscribedTopicsMsg, bool approxSync, const std::string & subscribedTopic = "");
|
||||
|
||||
/// Drops whatever the subclass's synchronizer holds; called when the odometry resets.
|
||||
virtual void flushCallbacks() {};
|
||||
/// The node's TF buffer, for the subclass to look up its sensors' frames.
|
||||
tf2_ros::Buffer & tfBuffer() {return *tfBuffer_;}
|
||||
/// How long a TF lookup may block, from `wait_for_transform`.
|
||||
const double & waitForTransform() const {return waitForTransform_;}
|
||||
/// The velocity of the last registered frame, null when there is no estimate yet.
|
||||
rtabmap::Transform velocityGuess() const;
|
||||
/// Stamp of the last registered frame, 0 before the first one.
|
||||
double previousStamp() const {return previousStamp_;}
|
||||
/// Called after a frame has been registered and published, for a subclass to add to it.
|
||||
virtual void postProcessData(const rtabmap::SensorData & /*data*/, const std_msgs::msg::Header & /*header*/) const {}
|
||||
|
||||
private:
|
||||
void processData();
|
||||
virtual void mainLoop();
|
||||
virtual void mainLoopKill();
|
||||
/// Lets a subclass adjust the RTAB-Map parameters before the odometry is created.
|
||||
virtual void updateParameters(rtabmap::ParametersMap &) {}
|
||||
/// Called at the end of init(), where a subclass creates its subscriptions.
|
||||
virtual void onOdomInit() {}
|
||||
|
||||
void callbackIMU(const sensor_msgs::msg::Imu::SharedPtr msg);
|
||||
void reset(const rtabmap::Transform & pose = rtabmap::Transform::getIdentity());
|
||||
|
||||
protected:
|
||||
/// The callback group the subclass's sensor subscriptions belong to.
|
||||
rclcpp::CallbackGroup::SharedPtr dataCallbackGroup_;
|
||||
/// Reports the arrival of an input message to the diagnostics, before anything else.
|
||||
void tick(const rclcpp::Time & stamp);
|
||||
|
||||
private:
|
||||
|
||||
@@ -44,6 +44,17 @@ using namespace rtabmap;
|
||||
namespace rtabmap_odom
|
||||
{
|
||||
|
||||
/**
|
||||
* @brief Odometry from a laser scanner, 2D or 3D, by scan matching.
|
||||
*
|
||||
* Takes a sensor_msgs::msg::LaserScan or a sensor_msgs::msg::PointCloud2 and registers
|
||||
* each scan against the previous ones with ICP. A cloud whose points carry their own
|
||||
* timestamps is deskewed first, using TF or the last known velocity, since a scan taken
|
||||
* while the robot moves is not one rigid observation.
|
||||
*
|
||||
* @see doc/icp_odometry.md for the topics, the parameters and the shapes it can and
|
||||
* cannot constrain.
|
||||
*/
|
||||
class ICPOdometry : public rtabmap_odom::OdometryROS
|
||||
{
|
||||
public:
|
||||
|
||||
@@ -38,6 +38,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
#include <image_transport/subscriber_filter.hpp>
|
||||
|
||||
#include <sensor_msgs/msg/image.hpp>
|
||||
#include <rtabmap_msgs/msg/key_point.hpp>
|
||||
#include <rtabmap_msgs/msg/point3f.hpp>
|
||||
#include <rtabmap_msgs/msg/rgbd_image.hpp>
|
||||
#include <rtabmap_msgs/msg/rgbd_images.hpp>
|
||||
|
||||
@@ -50,6 +52,18 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
namespace rtabmap_odom
|
||||
{
|
||||
|
||||
/**
|
||||
* @brief Odometry from an RGB-D camera, or from several on one rig.
|
||||
*
|
||||
* Takes either the three raw topics of a camera (`rgb/image`, `depth/image`,
|
||||
* `rgb/camera_info`) or pre-synchronized rtabmap_msgs::msg::RGBDImage messages, one per
|
||||
* camera, and registers each frame's visual features against a local feature map. A
|
||||
* frame that arrives with its own keypoints, 3D points and descriptors is registered
|
||||
* with those rather than having them extracted again.
|
||||
*
|
||||
* @see doc/rgbd_odometry.md for the topics, the parameters and what to do when it loses
|
||||
* tracking.
|
||||
*/
|
||||
class RGBDOdometry : public rtabmap_odom::OdometryROS
|
||||
{
|
||||
public:
|
||||
@@ -61,10 +75,19 @@ private:
|
||||
virtual void updateParameters(rtabmap::ParametersMap & parameters);
|
||||
virtual void onOdomInit();
|
||||
|
||||
/**
|
||||
* Local features, when the input topic carries them, are indexed per camera like
|
||||
* the images are: one entry per camera, in the same order. They are optional, and
|
||||
* a frame that comes without them is processed exactly as before, the features
|
||||
* being extracted from the images downstream.
|
||||
*/
|
||||
void commonCallback(
|
||||
const std::vector<cv_bridge::CvImageConstPtr> & rgbImages,
|
||||
const std::vector<cv_bridge::CvImageConstPtr> & depthImages,
|
||||
const std::vector<sensor_msgs::msg::CameraInfo>& cameraInfos);
|
||||
const std::vector<sensor_msgs::msg::CameraInfo>& cameraInfos,
|
||||
const std::vector<std::vector<rtabmap_msgs::msg::KeyPoint> > & localKeyPointsMsgs = {},
|
||||
const std::vector<std::vector<rtabmap_msgs::msg::Point3f> > & localPoints3dMsgs = {},
|
||||
const std::vector<cv::Mat> & localDescriptorsMsgs = {});
|
||||
|
||||
void callback(
|
||||
const sensor_msgs::msg::Image::ConstSharedPtr image,
|
||||
|
||||
@@ -43,12 +43,25 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
#include <cv_bridge/cv_bridge.hpp>
|
||||
#endif
|
||||
#include <sensor_msgs/msg/image.hpp>
|
||||
#include <rtabmap_msgs/msg/key_point.hpp>
|
||||
#include <rtabmap_msgs/msg/point3f.hpp>
|
||||
#include <rtabmap_msgs/msg/rgbd_image.hpp>
|
||||
#include <rtabmap_msgs/msg/rgbd_images.hpp>
|
||||
|
||||
namespace rtabmap_odom
|
||||
{
|
||||
|
||||
/**
|
||||
* @brief Odometry from a stereo pair, or from several on one rig.
|
||||
*
|
||||
* Takes either the four raw topics of a stereo camera (left and right image plus their
|
||||
* calibrations) or pre-synchronized rtabmap_msgs::msg::RGBDImage messages carrying the
|
||||
* pair, one per camera. The right camera's `P(0,3)` is what gives the trajectory its
|
||||
* scale. A frame that arrives with its own keypoints, 3D points and descriptors is
|
||||
* registered with those rather than having them extracted again.
|
||||
*
|
||||
* @see doc/stereo_odometry.md for the topics, the parameters and the scale it depends on.
|
||||
*/
|
||||
class StereoOdometry : public rtabmap_odom::OdometryROS
|
||||
{
|
||||
public:
|
||||
@@ -60,11 +73,20 @@ private:
|
||||
virtual void updateParameters(rtabmap::ParametersMap & parameters);
|
||||
virtual void onOdomInit();
|
||||
|
||||
/**
|
||||
* Local features, when the input topic carries them, are indexed per camera like
|
||||
* the images are: one entry per camera, in the same order, and placed in that
|
||||
* camera's left image. They are optional, and a frame that comes without them is
|
||||
* processed exactly as before, the features being extracted downstream.
|
||||
*/
|
||||
void commonCallback(
|
||||
const std::vector<cv_bridge::CvImageConstPtr> & leftImages,
|
||||
const std::vector<cv_bridge::CvImageConstPtr> & rightImages,
|
||||
const std::vector<sensor_msgs::msg::CameraInfo>& leftCameraInfos,
|
||||
const std::vector<sensor_msgs::msg::CameraInfo>& rightCameraInfos);
|
||||
const std::vector<sensor_msgs::msg::CameraInfo>& rightCameraInfos,
|
||||
const std::vector<std::vector<rtabmap_msgs::msg::KeyPoint> > & localKeyPointsMsgs = {},
|
||||
const std::vector<std::vector<rtabmap_msgs::msg::Point3f> > & localPoints3dMsgs = {},
|
||||
const std::vector<cv::Mat> & localDescriptorsMsgs = {});
|
||||
|
||||
void callback(
|
||||
const sensor_msgs::msg::Image::ConstSharedPtr imageRectLeft,
|
||||
|
||||
Reference in New Issue
Block a user