Program Listing for File InputROS2.h

↰ Return to documentation for file (/tmp/ws/src/mola/mola_input_ros2/include/mola_input_ros2/InputROS2.h)

/* -------------------------------------------------------------------------
 *   A Modular Optimization framework for Localization and mApping  (MOLA)
 * Copyright (C) 2018-2024 Jose Luis Blanco, University of Almeria
 * See LICENSE for license information.
 * ------------------------------------------------------------------------- */
#pragma once

#include <mola_kernel/interfaces/RawDataSourceBase.h>
#include <tf2_ros/buffer.h>
#include <tf2_ros/transform_listener.h>

#include <nav_msgs/msg/odometry.hpp>
#include <optional>
#include <sensor_msgs/msg/laser_scan.hpp>
#include <sensor_msgs/msg/point_cloud2.hpp>

namespace mola
{
class InputROS2 : public RawDataSourceBase
{
    DEFINE_MRPT_OBJECT(InputROS2, mola)

   public:
    InputROS2();
    ~InputROS2() override = default;

    // See docs in base class
    void initialize(const Yaml& cfg) override;
    void spinOnce() override;

   private:
    std::thread rosNodeThread_;

    struct Params
    {
        std::string base_link_frame = "base_footprint";

        std::string odom_frame = "odom";

        bool publish_odometry_from_tf = false;

        int wait_for_tf_timeout_milliseconds = 100;
    };

    Params params_;

    // Yaml is NOT a reference on purpose.
    void ros_node_thread_main(Yaml cfg);

    std::shared_ptr<tf2_ros::Buffer>            tf_buffer_;
    std::shared_ptr<tf2_ros::TransformListener> tf_listener_;

    rclcpp::Clock::SharedPtr ros_clock_;
    std::mutex               ros_clock_mtx_;

    std::vector<rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr>
        subsPointCloud_;

    std::vector<rclcpp::Subscription<sensor_msgs::msg::LaserScan>::SharedPtr>
        subsLaserScan_;

    std::vector<rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr>
        subsOdometry_;

    void callbackOnPointCloud2(
        const sensor_msgs::msg::PointCloud2&       o,
        const std::string&                         outSensorLabel,
        const std::optional<mrpt::poses::CPose3D>& fixedSensorPose);

    void callbackOnLaserScan(
        const sensor_msgs::msg::LaserScan& o, const std::string& outSensorLabel,
        const std::optional<mrpt::poses::CPose3D>& fixedSensorPose);

    void callbackOnOdometry(
        const nav_msgs::msg::Odometry& o, const std::string& outSensorLabel);

    bool waitForTransform(
        mrpt::poses::CPose3D& des, const std::string& target_frame,
        const std::string& source_frame, const rclcpp::Time& time,
        const int timeoutMilliseconds, bool printErrors);

    void publishOdometry();
};

}  // namespace mola