mvsim_node_core.h
Go to the documentation of this file.
00001 
00004 #ifndef SR_MVSIM_NODE_CORE_H
00005 #define SR_MVSIM_NODE_CORE_H
00006 
00007 // ROS includes.
00008 #include <ros/ros.h>
00009 #include <ros/time.h>
00010 #include <rosgraph_msgs/Clock.h>
00011 
00012 // Dynamic reconfigure includes.
00013 #include <dynamic_reconfigure/server.h>
00014 
00015 #include <tf/transform_broadcaster.h>
00016 #include <tf2_ros/static_transform_broadcaster.h>
00017 #include <visualization_msgs/MarkerArray.h>
00018 
00019 // Auto-generated from cfg/ directory.
00020 #include <mvsim/mvsimNodeConfig.h>
00021 
00022 #include <mvsim/mvsim.h>  // the mvsim library
00023 #include <mrpt/utils/CTicTac.h>
00024 
00025 #if MRPT_VERSION >= 0x130
00026 #include <mrpt/obs/CObservation.h>
00027 using mrpt::obs::CObservation;
00028 #else
00029 #include <mrpt/slam/CObservation.h>
00030 using mrpt::slam::CObservation;
00031 #endif
00032 
00035 class MVSimNode
00036 {
00037    public:
00039         MVSimNode(ros::NodeHandle& n);
00041         ~MVSimNode();
00042 
00043         void loadWorldModel(const std::string& world_xml_file);
00044 
00045         void spin();  
00046 
00048         void configCallback(mvsim::mvsimNodeConfig& config, uint32_t level);
00049 
00051         class MyWorld : public mvsim::World
00052         {
00053                 MVSimNode& m_parent;
00054 
00055            public:
00056                 MyWorld(MVSimNode& node) : m_parent(node) {}
00057                 virtual void onNewObservation(
00058                         const mvsim::VehicleBase& veh, const CObservation* obs);
00059 
00060         };  // End of MyWorld
00061 
00062         MyWorld mvsim_world_;  
00063 
00064 
00065         double realtime_factor_;  
00066         int gui_refresh_period_ms_;  
00067         bool m_show_gui;  
00068         bool m_do_fake_localization;  
00069 
00070 
00071 
00072         double m_transform_tolerance;  
00073 
00074 
00075    protected:
00076         ros::NodeHandle& m_n;
00077         ros::NodeHandle m_localn;
00078 
00079         // === ROS Publishers ====
00080         ros::Publisher m_pub_map_ros,
00081                 m_pub_map_metadata;  
00082         ros::Publisher m_pub_clock;
00083         tf::TransformBroadcaster m_tf_br;  
00084         tf2_ros::StaticTransformBroadcaster
00085                 m_static_tf_br;  
00086 
00087         struct TPubSubPerVehicle
00088         {
00089                 ros::Subscriber
00090                         sub_cmd_vel;  
00091                 ros::Publisher pub_odom;  
00092                 ros::Publisher
00093                         pub_ground_truth;  
00094                 ros::Publisher pub_amcl_pose,
00095                         pub_particlecloud;  
00096                 std::map<std::string, ros::Publisher>
00097                         pub_sensors;  
00098                 ros::Publisher pub_chassis_markers;  
00099                 ros::Publisher pub_chassis_shape;  
00100                 visualization_msgs::MarkerArray chassis_shape_msg;
00101         };
00102 
00103         std::vector<TPubSubPerVehicle>
00104                 m_pubsub_vehicles;  
00105 
00106 
00107 
00110         void initPubSubs(TPubSubPerVehicle& out_pubsubs, mvsim::VehicleBase* veh);
00111 
00112         // === End ROS Publishers ====
00113 
00114         // === ROS Hooks ====
00115         void onROSMsgCmdVel(
00116                 const geometry_msgs::Twist::ConstPtr& cmd, mvsim::VehicleBase* veh);
00117         // === End ROS Hooks====
00118 
00119         rosgraph_msgs::Clock m_clockMsg;
00120         ros::Time m_sim_time;  
00121         ros::Time
00122                 m_base_last_cmd;  
00123         ros::Duration m_base_watchdog_timeout;
00124         const tf::Transform
00125                 m_tfIdentity;  
00126 
00127         struct TThreadParams
00128         {
00129                 MVSimNode* obj;
00130                 volatile bool closing;
00131                 TThreadParams() : obj(NULL), closing(false) {}
00132         };
00133         TThreadParams thread_params_;
00134         mrpt::utils::CTicTac realtime_tictac_;
00135 
00136         double t_old_;  // = realtime_tictac_.Tac();
00137         bool world_init_ok_;  
00138 
00139 
00140         double m_period_ms_publish_tf;  
00141 
00142         mrpt::utils::CTicTac m_tim_publish_tf;
00143 
00144         double m_period_ms_teleop_refresh;  
00145 
00146 
00147         mrpt::utils::CTicTac m_tim_teleop_refresh;
00148 
00149         size_t m_teleop_idx_veh;  
00150 
00151         mvsim::World::TGUIKeyEvent m_gui_key_events;
00152         std::string m_msg2gui;
00153 
00154         std::thread thGUI_;
00155         static void thread_update_GUI(TThreadParams& thread_params);
00156 
00159         void notifyROSWorldIsUpdated();
00160 
00162         void spinNotifyROS();
00163 
00166         std::string vehVarName(
00167                 const std::string& sVarName, const mvsim::VehicleBase* veh) const;
00168 
00169         void sendStaticTF(
00170                 const std::string& frame_id, const std::string& child_frame_id,
00171                 const tf::Transform& tx, const ros::Time& stamp);
00172 
00173         struct MVSimVisitor_notifyROSWorldIsUpdated
00174                 : public mvsim::World::VehicleVisitorBase,
00175                   public mvsim::World::WorldElementVisitorBase
00176         {
00177                 void visit(mvsim::VehicleBase* obj);
00178                 void visit(mvsim::WorldElementBase* obj);
00179 
00180                 MVSimVisitor_notifyROSWorldIsUpdated(MVSimNode& parent)
00181                         : m_parent(parent)
00182                 {
00183                 }
00184                 MVSimNode& m_parent;
00185         };
00186 
00187 };  // end class
00188 
00189 #endif  // SR_MVSIM_NODE_CORE_H


mvsim
Author(s):
autogenerated on Thu Sep 7 2017 09:27:48