mvsim_node_core.h

Go to the documentation of this file.

#ifndef SR_MVSIM_NODE_CORE_H
#define SR_MVSIM_NODE_CORE_H

// ROS includes.
#include <ros/ros.h>
#include <ros/time.h>
#include <rosgraph_msgs/Clock.h>

// Dynamic reconfigure includes.
#include <dynamic_reconfigure/server.h>

#include <tf/transform_broadcaster.h>
#include <tf2_ros/static_transform_broadcaster.h>
#include <visualization_msgs/MarkerArray.h>
#include <thread>

// Auto-generated from cfg/ directory.
#include <mvsim/mvsimNodeConfig.h>

#include <mvsim/mvsim.h>  // the mvsim library
#include <mrpt/utils/CTicTac.h>

#if MRPT_VERSION >= 0x130
#include <mrpt/obs/CObservation.h>
using mrpt::obs::CObservation;
#else
#include <mrpt/slam/CObservation.h>
using mrpt::slam::CObservation;
#endif

class MVSimNode
{
   public:
        MVSimNode(ros::NodeHandle& n);
        ~MVSimNode();

        void loadWorldModel(const std::string& world_xml_file);

        void spin();  

        void configCallback(mvsim::mvsimNodeConfig& config, uint32_t level);

        class MyWorld : public mvsim::World
        {
                MVSimNode& m_parent;

           public:
                MyWorld(MVSimNode& node) : m_parent(node) {}
                virtual void onNewObservation(
                        const mvsim::VehicleBase& veh, const CObservation* obs);

        };  // End of MyWorld

        MyWorld mvsim_world_;  


        double realtime_factor_;  
        int gui_refresh_period_ms_;  
        bool m_show_gui;  
        bool m_do_fake_localization;  



        double m_transform_tolerance;  


   protected:
        ros::NodeHandle& m_n;
        ros::NodeHandle m_localn;

        // === ROS Publishers ====
        ros::Publisher m_pub_map_ros,
                m_pub_map_metadata;  
        ros::Publisher m_pub_clock;
        tf::TransformBroadcaster m_tf_br;  
        tf2_ros::StaticTransformBroadcaster
                m_static_tf_br;  

        struct TPubSubPerVehicle
        {
                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;
        };

        std::vector<TPubSubPerVehicle>
                m_pubsub_vehicles;  



        void initPubSubs(TPubSubPerVehicle& out_pubsubs, mvsim::VehicleBase* veh);

        // === End ROS Publishers ====

        // === ROS Hooks ====
        void onROSMsgCmdVel(
                const geometry_msgs::Twist::ConstPtr& cmd, mvsim::VehicleBase* veh);
        // === End ROS Hooks====

        rosgraph_msgs::Clock m_clockMsg;
        ros::Time m_sim_time;  
        ros::Time
                m_base_last_cmd;  
        ros::Duration m_base_watchdog_timeout;
        const tf::Transform
                m_tfIdentity;  

        struct TThreadParams
        {
                MVSimNode* obj;
                volatile bool closing;
                TThreadParams() : obj(NULL), closing(false) {}
        };
        TThreadParams thread_params_;
        mrpt::utils::CTicTac realtime_tictac_;

        double t_old_;  // = realtime_tictac_.Tac();
        bool world_init_ok_;  


        double m_period_ms_publish_tf;  

        mrpt::utils::CTicTac m_tim_publish_tf;

        double m_period_ms_teleop_refresh;  


        mrpt::utils::CTicTac m_tim_teleop_refresh;

        size_t m_teleop_idx_veh;  

        mvsim::World::TGUIKeyEvent m_gui_key_events;
        std::string m_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 tf::Transform& tx, const ros::Time& stamp);

        struct MVSimVisitor_notifyROSWorldIsUpdated
                : public mvsim::World::VehicleVisitorBase,
                  public mvsim::World::WorldElementVisitorBase
        {
                void visit(mvsim::VehicleBase* obj);
                void visit(mvsim::WorldElementBase* obj);

                MVSimVisitor_notifyROSWorldIsUpdated(MVSimNode& parent)
                        : m_parent(parent)
                {
                }
                MVSimNode& m_parent;
        };

};  // end class

#endif  // SR_MVSIM_NODE_CORE_H