gazebo_bag_plugin.h
Go to the documentation of this file.
1 /*
2  * Copyright 2015 Fadri Furrer, ASL, ETH Zurich, Switzerland
3  * Copyright 2015 Michael Burri, ASL, ETH Zurich, Switzerland
4  * Copyright 2015 Mina Kamel, ASL, ETH Zurich, Switzerland
5  * Copyright 2015 Janosch Nikolic, ASL, ETH Zurich, Switzerland
6  * Copyright 2015 Markus Achtelik, ASL, ETH Zurich, Switzerland
7  * Copyright 2016 Geoffrey Hunter <gbmhunter@gmail.com>
8  *
9  * Licensed under the Apache License, Version 2.0 (the "License");
10  * you may not use this file except in compliance with the License.
11  * You may obtain a copy of the License at
12  *
13  * http://www.apache.org/licenses/LICENSE-2.0
14 
15  * Unless required by applicable law or agreed to in writing, software
16  * distributed under the License is distributed on an "AS IS" BASIS,
17  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
18  * See the License for the specific language governing permissions and
19  * limitations under the License.
20  */
21 
22 
23 #ifndef ROTORS_GAZEBO_PLUGINS_GAZEBO_BAG_PLUGIN_H
24 #define ROTORS_GAZEBO_PLUGINS_GAZEBO_BAG_PLUGIN_H
25 
26 #include <string>
27 
28 #include <gazebo/common/common.hh>
29 #include <gazebo/common/Plugin.hh>
30 #include <gazebo/gazebo.hh>
31 #include <gazebo/physics/physics.hh>
32 #include <geometry_msgs/Point.h>
33 #include <geometry_msgs/PoseStamped.h>
34 #include <geometry_msgs/TwistStamped.h>
35 #include <geometry_msgs/WrenchStamped.h>
36 #include <mav_msgs/Actuators.h>
37 #include <mav_msgs/AttitudeThrust.h>
39 #include <mav_msgs/RateThrust.h>
40 #include <ros/ros.h>
41 #include <rosbag/bag.h>
42 #include <sensor_msgs/Imu.h>
43 #include <std_msgs/Float32.h>
44 #include <trajectory_msgs/MultiDOFJointTrajectory.h>
45 
46 #include "rotors_comm/RecordRosbag.h"
47 #include "rotors_comm/WindSpeed.h"
49 
50 
51 namespace gazebo {
52 
53 // Default values, the rest from common.h
54 static const std::string kDefaultFrameId = "ground_truth_pose";
55 static const std::string kDefaultLinkName = "base_link";
56 static const std::string kDefaultBagFilename_ = "simulator.bag";
57 static const std::string kDefaultRecordingServiceName = "record_rosbag";
58 static constexpr bool kDefaultWaitToRecord = false;
59 static constexpr bool kDefaultIsRecording = false;
60 
64 class GazeboBagPlugin : public ModelPlugin {
65  typedef std::map<const unsigned int, const physics::JointPtr> MotorNumberToJointMap;
66  typedef std::pair<const unsigned int, const physics::JointPtr> MotorNumberToJointPair;
67  public:
69  : ModelPlugin(),
71  // DEFAULT TOPICS
72  ground_truth_pose_topic_(mav_msgs::default_topics::GROUND_TRUTH_POSE),
73  ground_truth_twist_topic_(mav_msgs::default_topics::GROUND_TRUTH_TWIST),
74  imu_topic_(mav_msgs::default_topics::IMU),
75  control_attitude_thrust_topic_(mav_msgs::default_topics::COMMAND_ATTITUDE_THRUST),
76  control_motor_speed_topic_(mav_msgs::default_topics::COMMAND_ACTUATORS),
77  control_rate_thrust_topic_(mav_msgs::default_topics::COMMAND_RATE_THRUST),
78  wind_speed_topic_(mav_msgs::default_topics::WIND_SPEED),
79  motor_topic_(mav_msgs::default_topics::MOTOR_MEASUREMENT),
80  wrench_topic_(mav_msgs::default_topics::WRENCH),
81  external_force_topic_(mav_msgs::default_topics::EXTERNAL_FORCE),
82  waypoint_topic_(mav_msgs::default_topics::COMMAND_TRAJECTORY),
83  command_pose_topic_(mav_msgs::default_topics::COMMAND_POSE),
84  //---------------
85  frame_id_(kDefaultFrameId),
86  link_name_(kDefaultLinkName),
87  bag_filename_(kDefaultBagFilename_),
88  recording_service_name_(kDefaultRecordingServiceName),
90  wait_to_record_(kDefaultWaitToRecord),
91  is_recording_(kDefaultIsRecording),
92  node_handle_(nullptr),
93  contact_mgr_(nullptr) {}
94 
95  virtual ~GazeboBagPlugin();
96 
97  protected:
101  void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf);
102 
105  void OnUpdate(const common::UpdateInfo& /*_info*/);
106 
108  void StartRecording();
109 
111  void StopRecording();
112 
115  void ImuCallback(const sensor_msgs::ImuConstPtr& imu_msg);
116 
119  void ExternalForceCallback(const geometry_msgs::WrenchStampedConstPtr& force_msg);
120 
123  void WaypointCallback(const trajectory_msgs::MultiDOFJointTrajectoryConstPtr& trajectory_msg);
124 
127  void CommandPoseCallback(const geometry_msgs::PoseStampedConstPtr& pose_msg);
128 
131  void AttitudeThrustCallback(const mav_msgs::AttitudeThrustConstPtr& control_msg);
132 
135  void ActuatorsCallback(const mav_msgs::ActuatorsConstPtr& control_msg);
136 
139  void RateThrustCallback(const mav_msgs::RateThrustConstPtr& control_msg);
140 
143  void WindSpeedCallback(const rotors_comm::WindSpeedConstPtr& wind_speed_msg);
144 
147  void LogGroundTruth(const common::Time now);
148 
151  void LogMotorVelocities(const common::Time now);
152 
155  void LogWrenches(const common::Time now);
156 
160  bool RecordingServiceCallback(rotors_comm::RecordRosbag::Request& req,
161  rotors_comm::RecordRosbag::Response& res);
162 
163  private:
165  event::ConnectionPtr update_connection_;
166 
167  physics::WorldPtr world_;
168  physics::ModelPtr model_;
169  physics::LinkPtr link_;
170 
171  physics::Link_V child_links_;
172 
173  MotorNumberToJointMap motor_joints_;
174 
177  physics::ContactManager *contact_mgr_;
178 
179  std::string namespace_;
182  std::string imu_topic_;
184  std::string waypoint_topic_;
185  std::string command_pose_topic_;
189  std::string wind_speed_topic_;
190  std::string wrench_topic_;
191  std::string motor_topic_;
192  std::string frame_id_;
193  std::string link_name_;
194  std::string bag_filename_;
197 
200 
203 
206 
209 
210  // Ros subscribers
219 
220  // Ros service server
222 
223  std::ofstream csvOut;
224 
225  template<class T>
226  void writeBag(const std::string& topic, const ros::Time& time, const T& msg) {
227  boost::mutex::scoped_lock lock(mtx_);
228  try {
229  bag_.write(topic, time, msg);
230  }
231  catch (rosbag::BagIOException& e) {
232  gzerr << "Error while writing to bag " << e.what() << std::endl;
233  }
234  catch (rosbag::BagException& e) {
235  if (time < ros::TIME_MIN) {
236  gzerr<<"Header stamp not set for msg published on topic: "<< topic << ". " << e.what() << std::endl;
237  }
238  else {
239  gzerr << "Error while writing to bag " << e.what() << std::endl;
240  }
241  }
242  }
243 
244  template<class T>
245  void writeBag(const std::string& topic, const ros::Time& time, boost::shared_ptr<T const> const& msg) {
246  boost::mutex::scoped_lock lock(mtx_);
247  try {
248  bag_.write(topic, time, msg);
249  }
250  catch (rosbag::BagIOException& e) {
251  gzerr << "Error while writing to bag " << e.what() << std::endl;
252  }
253  catch (rosbag::BagException& e) {
254  if (time < ros::TIME_MIN) {
255  gzerr<<"Header stamp not set for msg published on topic: "<< topic << ". " << e.what() << std::endl;
256  }
257  else {
258  gzerr << "Error while writing to bag " << e.what() << std::endl;
259  }
260  }
261  }
262 
263 };
264 
265 } // namespace gazebo
266 
267 #endif // ROTORS_GAZEBO_PLUGINS_GAZEBO_BAG_PLUGIN_H
void ExternalForceCallback(const geometry_msgs::WrenchStampedConstPtr &force_msg)
Called when an WrenchStamped message is received.
static const std::string kDefaultFrameId
void WindSpeedCallback(const rotors_comm::WindSpeedConstPtr &wind_speed_msg)
Called when a WindSpeed message is received.
static constexpr double kDefaultRotorVelocitySlowdownSim
Definition: common.h:49
void CommandPoseCallback(const geometry_msgs::PoseStampedConstPtr &pose_msg)
Called when a PoseStamped message is received.
void LogMotorVelocities(const common::Time now)
Log all the motor velocities.
ros::NodeHandle * node_handle_
static const std::string kDefaultLinkName
std::string ground_truth_pose_topic_
void WaypointCallback(const trajectory_msgs::MultiDOFJointTrajectoryConstPtr &trajectory_msg)
Called when a MultiDOFJointTrajectoryPoint message is received.
std::pair< const unsigned int, const physics::JointPtr > MotorNumberToJointPair
const Time TIME_MIN(0, 1)
static constexpr bool kDefaultWaitToRecord
ros::Subscriber control_rate_thrust_sub_
void ActuatorsCallback(const mav_msgs::ActuatorsConstPtr &control_msg)
Called when a Actuators message is received.
void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf)
Load the plugin.
void LogGroundTruth(const common::Time now)
Log the ground truth pose and twist.
ros::Subscriber control_attitude_thrust_sub_
static const std::string kDefaultBagFilename_
void writeBag(const std::string &topic, const ros::Time &time, boost::shared_ptr< T const > const &msg)
static const std::string kDefaultNamespace
Definition: common.h:48
ros::Subscriber waypoint_sub_
physics::ContactManager * contact_mgr_
Pointer to the ContactManager to get all collisions of this link and its children.
std::string control_rate_thrust_topic_
static const std::string kDefaultRecordingServiceName
ros::Subscriber wind_speed_sub_
physics::ModelPtr model_
void AttitudeThrustCallback(const mav_msgs::AttitudeThrustConstPtr &control_msg)
Called when a AttitudeThrust message is received.
ros::Subscriber command_pose_sub_
static constexpr bool kDefaultIsRecording
void StopRecording()
Stop recording the rosbag.
This plugin is used to create rosbag files from within gazebo.
void StartRecording()
Starting recording the rosbag.
ros::Subscriber external_force_sub_
std::string control_motor_speed_topic_
void writeBag(const std::string &topic, const ros::Time &time, const T &msg)
physics::WorldPtr world_
MotorNumberToJointMap motor_joints_
ros::ServiceServer recording_service_
void RateThrustCallback(const mav_msgs::RateThrustConstPtr &control_msg)
Called when a RateThrust message is received.
std::string control_attitude_thrust_topic_
event::ConnectionPtr update_connection_
Pointer to the update event connection.
boost::mutex mtx_
Mutex lock for thread safety of writing bag files.
void OnUpdate(const common::UpdateInfo &)
Called when the world is updated.
std::map< const unsigned int, const physics::JointPtr > MotorNumberToJointMap
std::string ground_truth_twist_topic_
res
ros::Subscriber control_motor_speed_sub_
void ImuCallback(const sensor_msgs::ImuConstPtr &imu_msg)
Called when an IMU message is received.
bool is_recording_
Whether the plugin is currenly recording a rosbag.
void write(std::string const &topic, ros::MessageEvent< T > const &event)
bool RecordingServiceCallback(rotors_comm::RecordRosbag::Request &req, rotors_comm::RecordRosbag::Response &res)
Called when a request to start or stop recording is received.
void LogWrenches(const common::Time now)
Log all the wrenches.
bool wait_to_record_
Whether the plugin should wait for user command to start recording.
std::recursive_mutex mutex
physics::Link_V child_links_


rotors_gazebo_plugins
Author(s): Fadri Furrer, Michael Burri, Mina Kamel, Janosch Nikolic, Markus Achtelik
autogenerated on Mon Feb 28 2022 23:39:03