23 #ifndef ROTORS_GAZEBO_PLUGINS_GAZEBO_BAG_PLUGIN_H 24 #define ROTORS_GAZEBO_PLUGINS_GAZEBO_BAG_PLUGIN_H 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> 42 #include <sensor_msgs/Imu.h> 43 #include <std_msgs/Float32.h> 44 #include <trajectory_msgs/MultiDOFJointTrajectory.h> 46 #include "rotors_comm/RecordRosbag.h" 47 #include "rotors_comm/WindSpeed.h" 101 void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf);
105 void OnUpdate(
const common::UpdateInfo& );
115 void ImuCallback(
const sensor_msgs::ImuConstPtr& imu_msg);
123 void WaypointCallback(
const trajectory_msgs::MultiDOFJointTrajectoryConstPtr& trajectory_msg);
161 rotors_comm::RecordRosbag::Response&
res);
227 boost::mutex::scoped_lock lock(mtx_);
229 bag_.
write(topic, time, msg);
232 gzerr <<
"Error while writing to bag " << e.what() << std::endl;
236 gzerr<<
"Header stamp not set for msg published on topic: "<< topic <<
". " << e.what() << std::endl;
239 gzerr <<
"Error while writing to bag " << e.what() << std::endl;
246 boost::mutex::scoped_lock lock(mtx_);
248 bag_.
write(topic, time, msg);
251 gzerr <<
"Error while writing to bag " << e.what() << std::endl;
255 gzerr<<
"Header stamp not set for msg published on topic: "<< topic <<
". " << e.what() << std::endl;
258 gzerr <<
"Error while writing to bag " << e.what() << std::endl;
267 #endif // ROTORS_GAZEBO_PLUGINS_GAZEBO_BAG_PLUGIN_H virtual ~GazeboBagPlugin()
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
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
std::string command_pose_topic_
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
ros::Subscriber waypoint_sub_
physics::ContactManager * contact_mgr_
Pointer to the ContactManager to get all collisions of this link and its children.
std::string recording_service_name_
std::string control_rate_thrust_topic_
static const std::string kDefaultRecordingServiceName
ros::Subscriber wind_speed_sub_
std::string bag_filename_
void AttitudeThrustCallback(const mav_msgs::AttitudeThrustConstPtr &control_msg)
Called when a AttitudeThrust message is received.
std::string external_force_topic_
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)
std::string wind_speed_topic_
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.
std::string wrench_topic_
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_
ros::Subscriber control_motor_speed_sub_
void ImuCallback(const sensor_msgs::ImuConstPtr &imu_msg)
Called when an IMU message is received.
std::string waypoint_topic_
bool is_recording_
Whether the plugin is currenly recording a rosbag.
double rotor_velocity_slowdown_sim_
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_