mvsim_node_core.h

Go to the documentation of this file.

/*+-------------------------------------------------------------------------+
  |                       MultiVehicle simulator (libmvsim)                 |
  |                                                                         |
  | Copyright (C) 2014-2023  Jose Luis Blanco Claraco                       |
  | Copyright (C) 2017  Borys Tymchenko (Odessa Polytechnic University)     |
  | Distributed under 3-clause BSD License                                  |
  |   See COPYING                                                           |
  +-------------------------------------------------------------------------+ */

#pragma once

#if PACKAGE_ROS_VERSION == 1
#include <dynamic_reconfigure/server.h>
#include <mvsim/mvsimNodeConfig.h>
#endif

#include <mrpt/core/WorkerThreadsPool.h>
#include <mrpt/obs/CObservation.h>
#include <mrpt/obs/obs_frwds.h>
#include <mrpt/system/CTicTac.h>
#include <mrpt/system/CTimeLogger.h>
#include <mvsim/Comms/Server.h>
#include <mvsim/World.h>
#include <tf2/LinearMath/Transform.h>
#include <tf2_ros/static_transform_broadcaster.h>
#include <tf2_ros/transform_broadcaster.h>

#if PACKAGE_ROS_VERSION == 1
#include <geometry_msgs/Twist.h>
#include <ros/ros.h>
#include <ros/time.h>
#include <rosgraph_msgs/Clock.h>
#include <visualization_msgs/MarkerArray.h>
#else
#include <geometry_msgs/msg/polygon.hpp>
#include <geometry_msgs/msg/pose_array.hpp>
#include <geometry_msgs/msg/pose_with_covariance_stamped.hpp>
#include <nav_msgs/msg/map_meta_data.hpp>
#include <nav_msgs/msg/occupancy_grid.hpp>
#include <nav_msgs/msg/odometry.hpp>
#include <visualization_msgs/msg/marker_array.hpp>

#include "rclcpp/clock.hpp"
#include "rclcpp/rclcpp.hpp"
#include "rclcpp/time_source.hpp"
#endif

#include <atomic>
#include <thread>

namespace mrpt
{
namespace obs
{
class CObservationPointCloud;
}
}  // namespace mrpt

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);

        std::shared_ptr<mvsim::World> mvsim_world_ =
                std::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;

        int publisher_history_len_ = 50;

        double transform_tolerance_ = 0.1;

   protected:
        std::shared_ptr<mvsim::Server> mvsim_server_;

#if PACKAGE_ROS_VERSION == 1
        ros::NodeHandle& n_;
        ros::NodeHandle localn_{"~"};
#else
        rclcpp::Node::SharedPtr n_;
#endif

        // === ROS Publishers ====
#if PACKAGE_ROS_VERSION == 1
        ros::Publisher pub_map_ros_, pub_map_metadata_;
        // ros::Publisher pub_clock_;
#else
        rclcpp::Publisher<nav_msgs::msg::OccupancyGrid>::SharedPtr pub_map_ros_;
        rclcpp::Publisher<nav_msgs::msg::MapMetaData>::SharedPtr pub_map_metadata_;
        rclcpp::TimeSource ts_{n_};
        rclcpp::Clock::SharedPtr clock_;
#endif

#if PACKAGE_ROS_VERSION == 1
        tf2_ros::TransformBroadcaster tf_br_;  
        tf2_ros::StaticTransformBroadcaster static_tf_br_;
#else
        tf2_ros::TransformBroadcaster tf_br_{n_};  
        tf2_ros::StaticTransformBroadcaster static_tf_br_{n_};
#endif

        struct TPubSubPerVehicle
        {
#if PACKAGE_ROS_VERSION == 1
                ros::Subscriber sub_cmd_vel;  
                ros::Publisher pub_odom;  
                ros::Publisher pub_ground_truth;  

                ros::Publisher pub_amcl_pose, pub_particlecloud;

                std::map<std::string, ros::Publisher> pub_sensors;

                ros::Publisher pub_chassis_markers;      
                ros::Publisher pub_chassis_shape;  

                visualization_msgs::MarkerArray chassis_shape_msg;
#else
                rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr sub_cmd_vel;
                rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr pub_odom;
                rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr pub_ground_truth;

                rclcpp::Publisher<geometry_msgs::msg::PoseWithCovarianceStamped>::
                        SharedPtr pub_amcl_pose;
                rclcpp::Publisher<geometry_msgs::msg::PoseArray>::SharedPtr
                        pub_particlecloud;

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

                rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr
                        pub_chassis_markers;
                rclcpp::Publisher<geometry_msgs::msg::Polygon>::SharedPtr
                        pub_chassis_shape;

                visualization_msgs::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(
#if PACKAGE_ROS_VERSION == 1
                const geometry_msgs::Twist::ConstPtr& cmd,
#else
                const geometry_msgs::msg::Twist::ConstSharedPtr& cmd,
#endif
                mvsim::VehicleBase* veh);
        // === End ROS Hooks====

#if PACKAGE_ROS_VERSION == 1
        // rosgraph_msgs::Clock clockMsg_;
        // ros::Time sim_time_;  //!< Current simulation time
        ros::Time base_last_cmd_;  
        ros::Duration base_watchdog_timeout_;
#else
        // rclcpp::Time sim_time_;       //!< Current simulation time
        rclcpp::Time base_last_cmd_;  
        rclcpp::Duration base_watchdog_timeout_ = std::chrono::seconds(1);
#endif

        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_;

        double period_ms_teleop_refresh_ = 100;
        mrpt::system::CTicTac tim_teleop_refresh_;

        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;

        void sendStaticTF(
                const std::string& frame_id, const std::string& child_frame_id,
                const tf2::Transform& tx,
#if PACKAGE_ROS_VERSION == 1
                const ros::Time& stamp
#else
                const rclcpp::Time& stamp
#endif
        );

#if PACKAGE_ROS_VERSION == 1
        ros::Time myNow() const;
#else
        rclcpp::Time myNow() const;
#endif

        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);

};      // end class