Program Listing for File BridgeROS2.h
↰ Return to documentation for file (include/mola_bridge_ros2/BridgeROS2.h)
/* _
_ __ ___ ___ | | __ _
| '_ ` _ \ / _ \| |/ _` | Modular Optimization framework for
| | | | | | (_) | | (_| | Localization and mApping (MOLA)
|_| |_| |_|\___/|_|\__,_| https://github.com/MOLAorg/mola
Copyright (C) 2018-2025 Jose Luis Blanco, University of Almeria,
and individual contributors.
SPDX-License-Identifier: GPL-3.0
See LICENSE for full license information.
*/
#pragma once
// MOLA virtual interfaces:
#include <mola_kernel/Georeferencing.h>
#include <mola_kernel/interfaces/ExecutableBase.h>
#include <mola_kernel/interfaces/LocalizationSourceBase.h>
#include <mola_kernel/interfaces/MapServer.h>
#include <mola_kernel/interfaces/MapSourceBase.h>
#include <mola_kernel/interfaces/RawDataConsumer.h>
#include <mola_kernel/interfaces/RawDataSourceBase.h>
#include <mola_kernel/interfaces/Relocalization.h>
// MRPT:
#include <mrpt/maps/COccupancyGridMap2D.h>
#include <mrpt/obs/CObservationGPS.h>
#include <mrpt/obs/CObservationIMU.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>
// MOLA <-> ROS services:
#include <mola_msgs/srv/map_load.hpp>
#include <mola_msgs/srv/map_save.hpp>
#include <mola_msgs/srv/mola_runtime_param_get.hpp>
#include <mola_msgs/srv/mola_runtime_param_set.hpp>
#include <mola_msgs/srv/relocalize_from_state_estimator.hpp>
#include <mola_msgs/srv/relocalize_near_pose.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::ConstPtr& 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 publish_localization_following_rep105 = true;
bool forward_ros_tf_as_mola_odometry_observations = false;
bool publish_odometry_msgs_from_slam = true;
// Which source will be forwarded (empty=any)
std::string publish_odometry_msgs_from_slam_source = {};
bool publish_tf_from_slam = true;
// Which source will be forwarded (empty=any)
std::string publish_tf_from_slam_source = {};
bool publish_tf_from_robot_pose_observations = true;
std::string relocalize_from_topic = "/initialpose";
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_publish_diagnostics = 1.0; // [s]
double period_check_new_mola_subs = 1.0; // [s]
int wait_for_tf_timeout_milliseconds = 100;
std::string georef_map_reference_frame = "map";
std::string georef_map_utm_frame = "utm";
std::string georef_map_enu_frame = "enu";
};
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> subsGNSS_;
rclcpp::Subscription<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr subInitPose_;
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);
void callbackOnRelocalizeTopic(const geometry_msgs::msg::PoseWithCovarianceStamped& o);
bool waitForTransform(
mrpt::poses::CPose3D& des, const std::string& target_frame, const std::string& source_frame,
bool printErrors);
void importRosOdometryToMOLA();
rclcpp::Time myNow(const mrpt::Clock::time_point& observationStamp);
std::map<std::string, rclcpp::PublisherBase::SharedPtr> rosPubs_;
std::recursive_mutex rosPubsMtx_;
template <typename MSG_TYPE>
[[nodiscard]] auto get_publisher(const std::string& topic, const rclcpp::QoS& qos)
-> std::shared_ptr<rclcpp::Publisher<MSG_TYPE>>
{
auto lck = mrpt::lockHelper(rosPubsMtx_);
// Create the publisher the first time an observation arrives:
const bool is_1st_pub = rosPubs_.find(topic) == rosPubs_.end();
auto& pub = rosPubs_[topic];
if (is_1st_pub)
{
pub = rosNode()->create_publisher<MSG_TYPE>(topic, qos);
}
lck.unlock();
auto ret = std::dynamic_pointer_cast<rclcpp::Publisher<MSG_TYPE>>(pub);
ASSERT_(ret);
return ret;
}
struct MolaSubs
{
// MOLA subscribers:
std::set<mola::RawDataSourceBase::Ptr> dataSources;
std::set<std::shared_ptr<mola::LocalizationSourceBase>> locSources;
std::set<std::shared_ptr<mola::MapSourceBase>> mapSources;
std::set<std::shared_ptr<mola::Relocalization>> relocalization;
std::set<std::shared_ptr<mola::MapServer>> mapServers;
};
MolaSubs molaSubs_;
std::mutex molaSubsMtx_;
// ROS services:
rclcpp::Service<mola_msgs::srv::RelocalizeFromStateEstimator>::SharedPtr srvRelocSE_;
rclcpp::Service<mola_msgs::srv::RelocalizeNearPose>::SharedPtr srvRelocPose_;
rclcpp::Service<mola_msgs::srv::MapLoad>::SharedPtr srvMapLoad_;
rclcpp::Service<mola_msgs::srv::MapSave>::SharedPtr srvMapSave_;
rclcpp::Service<mola_msgs::srv::MolaRuntimeParamGet>::SharedPtr srvParamGet_;
rclcpp::Service<mola_msgs::srv::MolaRuntimeParamSet>::SharedPtr srvParamSet_;
void service_relocalize_from_se(
const std::shared_ptr<mola_msgs::srv::RelocalizeFromStateEstimator::Request> request,
std::shared_ptr<mola_msgs::srv::RelocalizeFromStateEstimator::Response> response);
void service_relocalize_near_pose(
const std::shared_ptr<mola_msgs::srv::RelocalizeNearPose::Request> request,
std::shared_ptr<mola_msgs::srv::RelocalizeNearPose::Response> response);
void service_map_load(
const std::shared_ptr<mola_msgs::srv::MapLoad::Request> request,
std::shared_ptr<mola_msgs::srv::MapLoad::Response> response);
void service_map_save(
const std::shared_ptr<mola_msgs::srv::MapSave::Request> request,
std::shared_ptr<mola_msgs::srv::MapSave::Response> response);
void service_param_get(
const std::shared_ptr<mola_msgs::srv::MolaRuntimeParamGet::Request> request,
std::shared_ptr<mola_msgs::srv::MolaRuntimeParamGet::Response> response);
void service_param_set(
const std::shared_ptr<mola_msgs::srv::MolaRuntimeParamSet::Request> request,
std::shared_ptr<mola_msgs::srv::MolaRuntimeParamSet::Response> response);
void onNewLocalization(const mola::LocalizationSourceBase::LocalizationUpdate& l);
void onNewMap(const mola::MapSourceBase::MapUpdate& m);
std::mutex lastLocMapMtx_;
std::vector<mola::LocalizationSourceBase::LocalizationUpdate> lastLocUpdates_;
std::multimap<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::CObservationIMU& obs);
void internalOn(
const mrpt::obs::CObservationPointCloud& obs, bool isSensorTopic,
const std::string& sSensorFrameId);
void internalPublishGridMap(
const mrpt::maps::COccupancyGridMap2D& m, const std::string& sMapTopicName,
const std::string& sReferenceFrame, const mrpt::Clock::time_point& timestamp);
void internalAnalyzeTopicsToSubscribe(const mrpt::containers::yaml& ds_subscribe);
void publishStaticTFs();
void publishDiagnostics();
void publishMetricMapGeoreferencingData(
const mola::Georeferencing& g, const std::string& georefTopic);
};
} // namespace mola