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