mvsim_node_core.h
Go to the documentation of this file.
1 
4 #ifndef SR_MVSIM_NODE_CORE_H
5 #define SR_MVSIM_NODE_CORE_H
6 
7 // ROS includes.
8 #include <ros/ros.h>
9 #include <ros/time.h>
10 #include <rosgraph_msgs/Clock.h>
11 
12 // Dynamic reconfigure includes.
13 #include <dynamic_reconfigure/server.h>
14 
17 #include <visualization_msgs/MarkerArray.h>
18 #include <thread>
19 
20 // Auto-generated from cfg/ directory.
21 #include <mvsim/mvsimNodeConfig.h>
22 
23 #include <mrpt/utils/CTicTac.h>
24 #include <mvsim/mvsim.h> // the mvsim library
25 
26 #if MRPT_VERSION >= 0x130
27 #include <mrpt/obs/CObservation.h>
29 #else
30 #include <mrpt/slam/CObservation.h>
31 using mrpt::slam::CObservation;
32 #endif
33 
36 class MVSimNode
37 {
38  public:
42  ~MVSimNode();
43 
44  void loadWorldModel(const std::string& world_xml_file);
45 
46  void spin();
47 
49  void configCallback(mvsim::mvsimNodeConfig& config, uint32_t level);
50 
52  class MyWorld : public mvsim::World
53  {
55 
56  public:
57  MyWorld(MVSimNode& node) : m_parent(node) {}
58  virtual void onNewObservation(
59  const mvsim::VehicleBase& veh, const CObservation* obs);
60 
61  }; // End of MyWorld
62 
64 
68  bool m_show_gui;
70  double m_transform_tolerance;
74 
76  protected:
79 
80  // === ROS Publishers ====
87 
89  {
95  ros::Publisher pub_amcl_pose,
97  std::map<std::string, ros::Publisher>
101  visualization_msgs::MarkerArray chassis_shape_msg;
102  };
103 
104  std::vector<TPubSubPerVehicle>
106 
111  void initPubSubs(TPubSubPerVehicle& out_pubsubs, mvsim::VehicleBase* veh);
112 
113  // === End ROS Publishers ====
114 
115  // === ROS Hooks ====
116  void onROSMsgCmdVel(
117  const geometry_msgs::Twist::ConstPtr& cmd, mvsim::VehicleBase* veh);
118  // === End ROS Hooks====
119 
120  rosgraph_msgs::Clock m_clockMsg;
122  ros::Time
125  const tf::Transform
127 
129  {
131  volatile bool closing;
132  TThreadParams() : obj(NULL), closing(false) {}
133  };
135  mrpt::system::CTicTac realtime_tictac_;
136 
137  double t_old_; // = realtime_tictac_.Tac();
139 
142  mrpt::system::CTicTac m_tim_publish_tf;
144 
146  mrpt::system::CTicTac m_tim_teleop_refresh;
149 
153  std::string m_msg2gui;
154 
155  std::thread thGUI_;
156  static void thread_update_GUI(TThreadParams& thread_params);
157 
161 
163  void spinNotifyROS();
164 
167  std::string vehVarName(
168  const std::string& sVarName, const mvsim::VehicleBase* veh) const;
169 
170  void sendStaticTF(
171  const std::string& frame_id, const std::string& child_frame_id,
172  const tf::Transform& tx, const ros::Time& stamp);
173 
177  {
178  void visit(mvsim::VehicleBase* obj);
179  void visit(mvsim::WorldElementBase* obj);
180 
182  : m_parent(parent)
183  {
184  }
186  };
187 
188 }; // end class
189 
190 #endif // SR_MVSIM_NODE_CORE_H
ros::Publisher pub_ground_truth
Publisher of "base_pose_ground_truth" topic.
static void thread_update_GUI(TThreadParams &thread_params)
Definition: mvsim_node.cpp:231
std::string vehVarName(const std::string &sVarName, const mvsim::VehicleBase *veh) const
Definition: mvsim_node.cpp:722
mrpt::system::CTicTac m_tim_publish_tf
TF transforms & /*/odom topics (In ms)
double m_period_ms_teleop_refresh
ros::NodeHandle m_localn
ros::Subscriber sub_cmd_vel
Subscribers for each vehicle&#39;s "cmd_vel" topic.
std::string m_msg2gui
double m_transform_tolerance
mrpt::system::CTicTac realtime_tictac_
MyWorld(MVSimNode &node)
ros::Time m_base_last_cmd
Last time we received a vel_cmd (for watchdog)
ros::Publisher m_pub_map_ros
virtual void onNewObservation(const mvsim::VehicleBase &veh, const CObservation *obs)
Definition: mvsim_node.cpp:654
unsigned int uint32_t
rosgraph_msgs::Clock m_clockMsg
mvsim::World::TGUIKeyEvent m_gui_key_events
"focused" vehicle)
std::function< void(VehicleBase &)> vehicle_visitor_t
Definition: World.h:222
mrpt::system::CTicTac m_tim_teleop_refresh
ros::Duration m_base_watchdog_timeout
const tf::Transform m_tfIdentity
Unit transform (const, created only once)
void onROSMsgCmdVel(const geometry_msgs::Twist::ConstPtr &cmd, mvsim::VehicleBase *veh)
Definition: mvsim_node.cpp:454
tf::TransformBroadcaster m_tf_br
Use to send data to TF.
bool world_init_ok_
GLsizei n
void loadWorldModel(const std::string &world_xml_file)
Definition: mvsim_node.cpp:80
void spin()
Process pending msgs, run real-time simulation, etc.
Definition: mvsim_node.cpp:125
void configCallback(mvsim::mvsimNodeConfig &config, uint32_t level)
Definition: mvsim_node.cpp:113
obs
int gui_refresh_period_ms_
Default:25.
size_t m_teleop_idx_veh
GLhandleARB obj
tf2_ros::StaticTransformBroadcaster m_static_tf_br
For sending STATIC tf.
void spinNotifyROS()
Definition: mvsim_node.cpp:472
MVSimNode & m_parent
double m_period_ms_publish_tf
loadWorldModel()
ros::Publisher pub_chassis_shape
"<VEH>/chassis_shape"
std::function< void(WorldElementBase &)> world_element_visitor_t
Definition: World.h:223
ros::NodeHandle & m_n
published TFs
void sendStaticTF(const std::string &frame_id, const std::string &child_frame_id, const tf::Transform &tx, const ros::Time &stamp)
Definition: mvsim_node.cpp:312
ros::Publisher pub_chassis_markers
"<VEH>/chassis_markers"
TThreadParams thread_params_
GLint level
TCLAP::CmdLine cmd("mvsim", ' ',"version", false)
ros::Publisher pub_particlecloud
Publishers for "fake_localization" topics.
ros::Publisher pub_odom
Publisher of "odom" topic.
std::thread thGUI_
void initPubSubs(TPubSubPerVehicle &out_pubsubs, mvsim::VehicleBase *veh)
Definition: mvsim_node.cpp:326
ros::Publisher m_pub_clock
std::vector< TPubSubPerVehicle > m_pubsub_vehicles
MVSimNode(ros::NodeHandle &n)
Definition: mvsim_node.cpp:29
std::map< std::string, ros::Publisher > pub_sensors
Map <sensor_label> => publisher.
void notifyROSWorldIsUpdated()
Definition: mvsim_node.cpp:288
bool m_show_gui
Default= true.
ros::Time m_sim_time
Current simulation time.
MyWorld mvsim_world_
bool m_do_fake_localization
visualization_msgs::MarkerArray chassis_shape_msg
double realtime_factor_
everything: vehicles, obstacles, etc.)
ros::Publisher m_pub_map_metadata
used for simul_map publication


mvsim
Author(s):
autogenerated on Fri May 7 2021 03:05:51