mvsim_node_core.h

Go to the documentation of this file.

/*+-------------------------------------------------------------------------+
  |                       MultiVehicle simulator (libmvsim)                 |
  |                                                                         |
  | Copyright (C) 2014-2024  Jose Luis Blanco Claraco                       |
  | Copyright (C) 2017  Borys Tymchenko (Odessa Polytechnic University)     |
  | Distributed under 3-clause BSD License                                  |
  |   See COPYING                                                           |
  +-------------------------------------------------------------------------+ */
 
#pragma once
 
#include <mrpt/core/WorkerThreadsPool.h>
#include <mrpt/obs/CObservation.h>
#include <mrpt/obs/CObservation3DRangeScan.h>
#include <mrpt/obs/CObservationGPS.h>
#include <mrpt/obs/CObservationIMU.h>
#include <mrpt/obs/CObservationPointCloud.h>
#include <mrpt/system/CTicTac.h>
#include <mrpt/system/CTimeLogger.h>
#include <mvsim/World.h>
#include <tf2/LinearMath/Transform.h>
 
#if defined(MVSIM_HAS_ZMQ) && defined(MVSIM_HAS_PROTOBUF)
#include <mvsim/Comms/Server.h>
#endif
 
#include <atomic>
#include <thread>
 
#if PACKAGE_ROS_VERSION == 1
#include <dynamic_reconfigure/server.h>
#include <geometry_msgs/Polygon.h>
#include <geometry_msgs/PoseArray.h>
#include <geometry_msgs/PoseWithCovarianceStamped.h>
#include <geometry_msgs/Twist.h>
#include <mvsim/mvsimNodeConfig.h>
#include <nav_msgs/MapMetaData.h>
#include <nav_msgs/OccupancyGrid.h>
#include <nav_msgs/Odometry.h>
#include <ros/ros.h>
#include <ros/time.h>
#include <rosgraph_msgs/Clock.h>
#include <sensor_msgs/CameraInfo.h>
#include <std_msgs/Bool.h>
#include <tf2_msgs/TFMessage.h>
#include <visualization_msgs/MarkerArray.h>
 
// usings:
using ros_Time = ros::Time;
using ros_Duration = ros::Duration;
 
using Msg_Polygon = geometry_msgs::Polygon;
using Msg_PoseArray = geometry_msgs::PoseArray;
using Msg_PoseWithCovarianceStamped = geometry_msgs::PoseWithCovarianceStamped;
using Msg_Twist = geometry_msgs::Twist;
using Msg_Twist_CSPtr = geometry_msgs::Twist::ConstPtr;
using Msg_OccupancyGrid = nav_msgs::OccupancyGrid;
using Msg_Odometry = nav_msgs::Odometry;
using Msg_MapMetaData = nav_msgs::MapMetaData;
using Msg_Bool = std_msgs::Bool;
using Msg_TFMessage = tf2_msgs::TFMessage;
using Msg_MarkerArray = visualization_msgs::MarkerArray;
using Msg_CameraInfo = sensor_msgs::CameraInfo;
#else
#include <geometry_msgs/msg/polygon.hpp>
#include <geometry_msgs/msg/pose_array.hpp>
#include <geometry_msgs/msg/pose_with_covariance_stamped.hpp>
#include <geometry_msgs/msg/twist.hpp>
#include <nav_msgs/msg/map_meta_data.hpp>
#include <nav_msgs/msg/occupancy_grid.hpp>
#include <nav_msgs/msg/odometry.hpp>
#include <rclcpp/clock.hpp>
#include <rclcpp/rclcpp.hpp>
#include <rclcpp/time_source.hpp>
#include <sensor_msgs/msg/camera_info.hpp>
#include <std_msgs/msg/bool.hpp>
#include <tf2_msgs/msg/tf_message.hpp>
#include <visualization_msgs/msg/marker_array.hpp>
 
#include "wrapper/publisher_wrapper.h"
 
// usings:
using ros_Time = rclcpp::Time;
using ros_Duration = rclcpp::Duration;
 
using Msg_Polygon = geometry_msgs::msg::Polygon;
using Msg_PoseArray = geometry_msgs::msg::PoseArray;
using Msg_PoseWithCovarianceStamped = geometry_msgs::msg::PoseWithCovarianceStamped;
using Msg_Twist = geometry_msgs::msg::Twist;
using Msg_Twist_CSPtr = geometry_msgs::msg::Twist::ConstSharedPtr;
using Msg_OccupancyGrid = nav_msgs::msg::OccupancyGrid;
using Msg_Odometry = nav_msgs::msg::Odometry;
using Msg_MapMetaData = nav_msgs::msg::MapMetaData;
using Msg_Bool = std_msgs::msg::Bool;
using Msg_TFMessage = tf2_msgs::msg::TFMessage;
using Msg_MarkerArray = visualization_msgs::msg::MarkerArray;
using Msg_CameraInfo = sensor_msgs::msg::CameraInfo;
#endif
 
namespace mvsim_node
{
#if PACKAGE_ROS_VERSION == 1
template <typename T, typename... Args>
boost::shared_ptr<T> make_shared(Args&&... args)
{
        return boost::make_shared<T>(std::forward<Args>(args)...);
}
 
template <typename T>
using shared_ptr = boost::shared_ptr<T>;
#else
template <typename T, typename... Args>
std::shared_ptr<T> make_shared(Args&&... args)
{
        return std::make_shared<T>(std::forward<Args>(args)...);
}
 
template <typename T>
using shared_ptr = std::shared_ptr<T>;
#endif
}  // namespace mvsim_node
 
class MVSimNode
{
   public:
#if PACKAGE_ROS_VERSION == 1
        MVSimNode(ros::NodeHandle& n);
#else
        MVSimNode(rclcpp::Node::SharedPtr& n);
#endif
 
        ~MVSimNode();
 
        void terminateSimulation();
 
        void launch_mvsim_server();
 
        void loadWorldModel(const std::string& world_xml_file);
 
        void spin();  
 
#if PACKAGE_ROS_VERSION == 1
 
        void configCallback(mvsim::mvsimNodeConfig& config, uint32_t level);
#endif
 
        void onNewObservation(const mvsim::Simulable& veh, const mrpt::obs::CObservation::Ptr& obs);
 
        mvsim_node::shared_ptr<mvsim::World> mvsim_world_ = mvsim_node::make_shared<mvsim::World>();
 
        mrpt::WorkerThreadsPool ros_publisher_workers_{
                4 /*threads*/, mrpt::WorkerThreadsPool::POLICY_FIFO};
 
        double realtime_factor_ = 1.0;
        int gui_refresh_period_ms_ = 50;
        bool headless_ = false;
 
        bool do_fake_localization_ = true;
 
        bool publish_tf_odom2baselink_ = true;
 
        int publisher_history_len_ = 50;
 
   protected:
#if defined(MVSIM_HAS_ZMQ) && defined(MVSIM_HAS_PROTOBUF)
        mvsim_node::shared_ptr<mvsim::Server> mvsim_server_;
#endif
 
#if PACKAGE_ROS_VERSION == 1
        ros::NodeHandle& n_;
        ros::NodeHandle localn_{"~"};
#else
        rclcpp::Node::SharedPtr n_;
#endif
 
        // === ROS Publishers ====
#if PACKAGE_ROS_VERSION == 1
        // mvsim_node::shared_ptr<ros::Publisher> pub_clock_;
#else
        rclcpp::TimeSource ts_{n_};
        rclcpp::Clock::SharedPtr clock_;
#endif
 
        struct WorldPubs
        {
#if PACKAGE_ROS_VERSION == 1
                mvsim_node::shared_ptr<ros::Publisher> pub_map_ros;      
                mvsim_node::shared_ptr<ros::Publisher>
                        pub_map_metadata;  
#else
                rclcpp::Publisher<Msg_OccupancyGrid>::SharedPtr pub_map_ros;
                rclcpp::Publisher<Msg_MapMetaData>::SharedPtr pub_map_metadata;
#endif
        };
 
        WorldPubs worldPubs_;
 
        struct TPubSubPerVehicle
        {
#if PACKAGE_ROS_VERSION == 1
                mvsim_node::shared_ptr<ros::Subscriber>
                        sub_cmd_vel;  
 
                mvsim_node::shared_ptr<ros::Publisher> pub_odom;  
                mvsim_node::shared_ptr<ros::Publisher>
                        pub_ground_truth;  
 
                mvsim_node::shared_ptr<ros::Publisher> pub_amcl_pose;  
                mvsim_node::shared_ptr<ros::Publisher>
                        pub_particlecloud;      
 
                std::map<std::string, mvsim_node::shared_ptr<ros::Publisher>> pub_sensors;
 
                mvsim_node::shared_ptr<ros::Publisher> pub_chassis_markers;      
                mvsim_node::shared_ptr<ros::Publisher> pub_chassis_shape;  
                mvsim_node::shared_ptr<ros::Publisher> pub_collision;  
 
                mvsim_node::shared_ptr<ros::Publisher> pub_tf;  
                mvsim_node::shared_ptr<ros::Publisher> pub_tf_static;  
 
                Msg_MarkerArray chassis_shape_msg;
#else
                rclcpp::Subscription<Msg_Twist>::SharedPtr sub_cmd_vel;
 
                rclcpp::Publisher<Msg_Odometry>::SharedPtr pub_odom;
                rclcpp::Publisher<Msg_Odometry>::SharedPtr pub_ground_truth;
 
                rclcpp::Publisher<Msg_PoseWithCovarianceStamped>::SharedPtr pub_amcl_pose;
                rclcpp::Publisher<Msg_PoseArray>::SharedPtr pub_particlecloud;
 
                std::map<std::string, mvsim_node::shared_ptr<PublisherWrapperBase>> pub_sensors;
 
                rclcpp::Publisher<Msg_MarkerArray>::SharedPtr pub_chassis_markers;
                rclcpp::Publisher<Msg_Polygon>::SharedPtr pub_chassis_shape;
                rclcpp::Publisher<Msg_Bool>::SharedPtr pub_collision;
 
                rclcpp::Publisher<Msg_TFMessage>::SharedPtr pub_tf;
                rclcpp::Publisher<Msg_TFMessage>::SharedPtr pub_tf_static;
 
                Msg_MarkerArray chassis_shape_msg;
#endif
        };
 
        std::vector<TPubSubPerVehicle> pubsub_vehicles_;
        std::mutex pubsub_vehicles_mtx_;
 
        void initPubSubs(TPubSubPerVehicle& out_pubsubs, mvsim::VehicleBase* veh);
 
        // === End ROS Publishers ====
 
        // === ROS Hooks ====
        void onROSMsgCmdVel(Msg_Twist_CSPtr cmd, mvsim::VehicleBase* veh);
        // === End ROS Hooks====
 
#if PACKAGE_ROS_VERSION == 1
        // rosgraph_msgs::Clock clockMsg_;
#endif
        // ros_Time sim_time_;   //!< Current simulation time
        ros_Time base_last_cmd_;  
        ros_Duration base_watchdog_timeout_ = ros_Duration(1, 0);
 
        const tf2::Transform tfIdentity_ = tf2::Transform::getIdentity();
 
        struct TThreadParams
        {
                TThreadParams() = default;
 
                MVSimNode* obj = nullptr;
                std::atomic_bool closing{false};
        };
        TThreadParams thread_params_;
        mrpt::system::CTicTac realtime_tictac_;
 
        double t_old_ = -1;
 
        bool world_init_ok_ = false;
 
        double period_ms_publish_tf_ = 20;
 
        mrpt::system::CTicTac tim_publish_tf_;
 
        bool force_publish_vehicle_namespace_ = false;
 
        double period_ms_teleop_refresh_ = 100;
        mrpt::system::CTicTac tim_teleop_refresh_;
 
        std::map<mvsim::VehicleBase*, double> lastCmdVelTimestamp_;
 
        size_t teleop_idx_veh_ = 0;
        mvsim::World::TGUIKeyEvent gui_key_events_;
        std::string msg2gui_;
 
        std::thread thGUI_;
        static void thread_update_GUI(TThreadParams& thread_params);
 
        void notifyROSWorldIsUpdated();
 
        void spinNotifyROS();
 
        std::string vehVarName(const std::string& sVarName, const mvsim::VehicleBase& veh) const;
 
        ros_Time myNow() const;
        double myNowSec() const;
 
        mrpt::system::CTimeLogger profiler_{true /*enabled*/, "mvsim_node"};
 
        void publishWorldElements(mvsim::WorldElementBase& obj);
        void publishVehicles(mvsim::VehicleBase& veh);
 
        void internalOn(const mvsim::VehicleBase& veh, const mrpt::obs::CObservation2DRangeScan& obs);
        void internalOn(const mvsim::VehicleBase& veh, const mrpt::obs::CObservationImage& obs);
        void internalOn(const mvsim::VehicleBase& veh, const mrpt::obs::CObservation3DRangeScan& obs);
        void internalOn(const mvsim::VehicleBase& veh, const mrpt::obs::CObservationPointCloud& obs);
        void internalOn(const mvsim::VehicleBase& veh, const mrpt::obs::CObservationIMU& obs);
        void internalOn(const mvsim::VehicleBase& veh, const mrpt::obs::CObservationGPS& obs);
 
};      // end class