Program Listing for File BridgeROS2.h

↰ Return to documentation for file (include/mola_bridge_ros2/BridgeROS2.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

// MOLA virtual interfaces:
#include <mola_kernel/interfaces/ExecutableBase.h>
#include <mola_kernel/interfaces/LocalizationSourceBase.h>
#include <mola_kernel/interfaces/MapSourceBase.h>
#include <mola_kernel/interfaces/RawDataConsumer.h>
#include <mola_kernel/interfaces/RawDataSourceBase.h>

// MRPT:
#include <mrpt/obs/CObservationGPS.h>
#include <mrpt/obs/CObservationImage.h>
#include <mrpt/obs/CObservationPointCloud.h>

// ROS & others
#include <tf2_ros/buffer.h>
#include <tf2_ros/static_transform_broadcaster.h>
#include <tf2_ros/transform_broadcaster.h>
#include <tf2_ros/transform_listener.h>

#include <nav_msgs/msg/odometry.hpp>
#include <optional>
#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>

namespace mola
{
class BridgeROS2 : public RawDataSourceBase, public mola::RawDataConsumer
{
    DEFINE_MRPT_OBJECT(BridgeROS2, mola)

   public:
    BridgeROS2();
    ~BridgeROS2() override;

    // See docs in base class
    void spinOnce() override;

    // RawDataConsumer implementation:
    void onNewObservation(const CObservation::Ptr& o) override;

   protected:
    // See docs in base class
    void initialize_rds(const Yaml& cfg) override;

   private:
    std::thread rosNodeThread_;

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

        std::string base_footprint_frame;  // Disabled by default

        mrpt::math::TPose3D base_footprint_to_base_link_tf = {0, 0, 0, 0, 0, 0};

        std::string odom_frame = "odom";

        std::string reference_frame = "map";

        bool forward_ros_tf_as_mola_odometry_observations = false;
        bool publish_odometry_msgs_from_slam              = true;

        bool publish_tf_from_robot_pose_observations = true;

        bool publish_in_sim_time = false;

        double period_publish_new_localization = 0.2;  // [s]
        double period_publish_new_map          = 5.0;  // [s]
        double period_publish_static_tfs       = 1.0;  // [s]

        double period_check_new_mola_subs = 1.0;  // [s]

        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::BufferCore>            tf_buffer_;
    std::shared_ptr<tf2_ros::TransformListener> tf_listener_;

    std::shared_ptr<tf2_ros::TransformBroadcaster>       tf_bc_;
    std::shared_ptr<tf2_ros::StaticTransformBroadcaster> tf_static_bc_;

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

    std::shared_ptr<rclcpp::Node> rosNode_;
    std::mutex                    rosNodeMtx_;

    std::shared_ptr<rclcpp::Node> rosNode()
    {
        auto lck = mrpt::lockHelper(rosNodeMtx_);
        return rosNode_;
    }
    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_;

    std::vector<rclcpp::Subscription<sensor_msgs::msg::Imu>::SharedPtr>
        subsImu_;

    std::vector<rclcpp::Subscription<sensor_msgs::msg::NavSatFix>::SharedPtr>
        subsGNNS_;

    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 callbackOnImu(
        const sensor_msgs::msg::Imu& o, const std::string& outSensorLabel,
        const std::optional<mrpt::poses::CPose3D>& fixedSensorPose);

    void callbackOnNavSatFix(
        const sensor_msgs::msg::NavSatFix& 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, bool printErrors);

    void publishOdometry();

    rclcpp::Time myNow(const mrpt::Clock::time_point& observationStamp);

    struct RosPubs
    {
        std::map<
            std::string, rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr>
            pub_poses;

        std::map<std::string, rclcpp::PublisherBase::SharedPtr> pub_sensors;

        // rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr
        // pub_markers;
    };

    RosPubs    rosPubs_;
    std::mutex rosPubsMtx_;

    struct MolaSubs
    {
        std::set<mola::RawDataSourceBase::Ptr>                  dataSources;
        std::set<std::shared_ptr<mola::LocalizationSourceBase>> locSources;
        std::set<std::shared_ptr<mola::MapSourceBase>>          mapSources;
    };

    MolaSubs   molaSubs_;
    std::mutex molaSubsMtx_;

    void onNewLocalization(
        const mola::LocalizationSourceBase::LocalizationUpdate& l);

    void onNewMap(const mola::MapSourceBase::MapUpdate& m);

    std::mutex lastLocMapMtx_;
    std::optional<mola::LocalizationSourceBase::LocalizationUpdate> lastLoc_;
    std::map<std::string /*map_name*/, mola::MapSourceBase::MapUpdate>
        lastMaps_;

    void timerPubLocalization();
    void timerPubMap();

    double lastTimeCheckMolaSubs_ = 0;
    void   doLookForNewMolaSubs();

    void internalOn(const mrpt::obs::CObservationImage& obs);
    void internalOn(const mrpt::obs::CObservation2DRangeScan& obs);
    void internalOn(const mrpt::obs::CObservationPointCloud& obs);
    void internalOn(const mrpt::obs::CObservationRobotPose& obs);
    void internalOn(const mrpt::obs::CObservationGPS& obs);

    void internalOn(
        const mrpt::obs::CObservationPointCloud& obs, bool isSensorTopic,
        const std::string& sSensorFrameId);

    void internalAnalyzeTopicsToSubscribe(
        const mrpt::containers::yaml& ds_subscribe);

    void publishStaticTFs();
};

}  // namespace mola