Public Member Functions | Protected Member Functions | Private Types | Private Member Functions | Private Attributes | List of all members
gazebo::GazeboBagPlugin Class Reference

This plugin is used to create rosbag files from within gazebo. More...

#include <gazebo_bag_plugin.h>

Inheritance diagram for gazebo::GazeboBagPlugin:
Inheritance graph
[legend]

Public Member Functions

 GazeboBagPlugin ()
 
virtual ~GazeboBagPlugin ()
 

Protected Member Functions

void ActuatorsCallback (const mav_msgs::ActuatorsConstPtr &control_msg)
 Called when a Actuators message is received. More...
 
void AttitudeThrustCallback (const mav_msgs::AttitudeThrustConstPtr &control_msg)
 Called when a AttitudeThrust message is received. More...
 
void CommandPoseCallback (const geometry_msgs::PoseStampedConstPtr &pose_msg)
 Called when a PoseStamped message is received. More...
 
void ExternalForceCallback (const geometry_msgs::WrenchStampedConstPtr &force_msg)
 Called when an WrenchStamped message is received. More...
 
void ImuCallback (const sensor_msgs::ImuConstPtr &imu_msg)
 Called when an IMU message is received. More...
 
void Load (physics::ModelPtr _model, sdf::ElementPtr _sdf)
 Load the plugin. More...
 
void LogGroundTruth (const common::Time now)
 Log the ground truth pose and twist. More...
 
void LogMotorVelocities (const common::Time now)
 Log all the motor velocities. More...
 
void LogWrenches (const common::Time now)
 Log all the wrenches. More...
 
void OnUpdate (const common::UpdateInfo &)
 Called when the world is updated. More...
 
void RateThrustCallback (const mav_msgs::RateThrustConstPtr &control_msg)
 Called when a RateThrust message is received. More...
 
bool RecordingServiceCallback (rotors_comm::RecordRosbag::Request &req, rotors_comm::RecordRosbag::Response &res)
 Called when a request to start or stop recording is received. More...
 
void StartRecording ()
 Starting recording the rosbag. More...
 
void StopRecording ()
 Stop recording the rosbag. More...
 
void WaypointCallback (const trajectory_msgs::MultiDOFJointTrajectoryConstPtr &trajectory_msg)
 Called when a MultiDOFJointTrajectoryPoint message is received. More...
 
void WindSpeedCallback (const rotors_comm::WindSpeedConstPtr &wind_speed_msg)
 Called when a WindSpeed message is received. More...
 

Private Types

typedef std::map< const unsigned int, const physics::JointPtr > MotorNumberToJointMap
 
typedef std::pair< const unsigned int, const physics::JointPtr > MotorNumberToJointPair
 

Private Member Functions

template<class T >
void writeBag (const std::string &topic, const ros::Time &time, const T &msg)
 
template<class T >
void writeBag (const std::string &topic, const ros::Time &time, boost::shared_ptr< T const > const &msg)
 

Private Attributes

rosbag::Bag bag_
 
std::string bag_filename_
 
physics::Link_V child_links_
 
ros::Subscriber command_pose_sub_
 
std::string command_pose_topic_
 
physics::ContactManager * contact_mgr_
 Pointer to the ContactManager to get all collisions of this link and its children. More...
 
ros::Subscriber control_attitude_thrust_sub_
 
std::string control_attitude_thrust_topic_
 
ros::Subscriber control_motor_speed_sub_
 
std::string control_motor_speed_topic_
 
ros::Subscriber control_rate_thrust_sub_
 
std::string control_rate_thrust_topic_
 
std::ofstream csvOut
 
ros::Subscriber external_force_sub_
 
std::string external_force_topic_
 
std::string frame_id_
 
std::string ground_truth_pose_topic_
 
std::string ground_truth_twist_topic_
 
ros::Subscriber imu_sub_
 
std::string imu_topic_
 
bool is_recording_
 Whether the plugin is currenly recording a rosbag. More...
 
physics::LinkPtr link_
 
std::string link_name_
 
physics::ModelPtr model_
 
MotorNumberToJointMap motor_joints_
 
std::string motor_topic_
 
boost::mutex mtx_
 Mutex lock for thread safety of writing bag files. More...
 
std::string namespace_
 
ros::NodeHandlenode_handle_
 
ros::ServiceServer recording_service_
 
std::string recording_service_name_
 
double rotor_velocity_slowdown_sim_
 
event::ConnectionPtr update_connection_
 Pointer to the update event connection. More...
 
bool wait_to_record_
 Whether the plugin should wait for user command to start recording. More...
 
ros::Subscriber waypoint_sub_
 
std::string waypoint_topic_
 
ros::Subscriber wind_speed_sub_
 
std::string wind_speed_topic_
 
physics::WorldPtr world_
 
std::string wrench_topic_
 

Detailed Description

This plugin is used to create rosbag files from within gazebo.

This plugin is ROS dependent, and is not built if NO_ROS=TRUE is provided to CMakeLists.txt (as in the case of a PX4/Firmware build).

Definition at line 64 of file gazebo_bag_plugin.h.

Member Typedef Documentation

◆ MotorNumberToJointMap

typedef std::map<const unsigned int, const physics::JointPtr> gazebo::GazeboBagPlugin::MotorNumberToJointMap
private

Definition at line 65 of file gazebo_bag_plugin.h.

◆ MotorNumberToJointPair

typedef std::pair<const unsigned int, const physics::JointPtr> gazebo::GazeboBagPlugin::MotorNumberToJointPair
private

Definition at line 66 of file gazebo_bag_plugin.h.

Constructor & Destructor Documentation

◆ GazeboBagPlugin()

gazebo::GazeboBagPlugin::GazeboBagPlugin ( )
inline

Definition at line 68 of file gazebo_bag_plugin.h.

◆ ~GazeboBagPlugin()

gazebo::GazeboBagPlugin::~GazeboBagPlugin ( )
virtual

Definition at line 30 of file gazebo_bag_plugin.cpp.

Member Function Documentation

◆ ActuatorsCallback()

void gazebo::GazeboBagPlugin::ActuatorsCallback ( const mav_msgs::ActuatorsConstPtr &  control_msg)
protected

Called when a Actuators message is received.

Parameters
[in]control_msgA Actuators message from mav_msgs.

Definition at line 293 of file gazebo_bag_plugin.cpp.

◆ AttitudeThrustCallback()

void gazebo::GazeboBagPlugin::AttitudeThrustCallback ( const mav_msgs::AttitudeThrustConstPtr &  control_msg)
protected

Called when a AttitudeThrust message is received.

Parameters
[in]control_msgA AttitudeThrust message from mav_msgs.

Definition at line 285 of file gazebo_bag_plugin.cpp.

◆ CommandPoseCallback()

void gazebo::GazeboBagPlugin::CommandPoseCallback ( const geometry_msgs::PoseStampedConstPtr &  pose_msg)
protected

Called when a PoseStamped message is received.

Parameters
[in]pose_msgA PoseStamped message from geometry_msgs.

Definition at line 278 of file gazebo_bag_plugin.cpp.

◆ ExternalForceCallback()

void gazebo::GazeboBagPlugin::ExternalForceCallback ( const geometry_msgs::WrenchStampedConstPtr &  force_msg)
protected

Called when an WrenchStamped message is received.

Parameters
[in]force_msgA WrenchStamped message from geometry_msgs.

Definition at line 264 of file gazebo_bag_plugin.cpp.

◆ ImuCallback()

void gazebo::GazeboBagPlugin::ImuCallback ( const sensor_msgs::ImuConstPtr &  imu_msg)
protected

Called when an IMU message is received.

Parameters
[in]imu_msgA IMU message from sensor_msgs.

Definition at line 258 of file gazebo_bag_plugin.cpp.

◆ Load()

void gazebo::GazeboBagPlugin::Load ( physics::ModelPtr  _model,
sdf::ElementPtr  _sdf 
)
protected

Load the plugin.

Parameters
[in]_modelPointer to the model that loaded this plugin.
[in]_sdfSDF element that describes the plugin.

Definition at line 39 of file gazebo_bag_plugin.cpp.

◆ LogGroundTruth()

void gazebo::GazeboBagPlugin::LogGroundTruth ( const common::Time  now)
protected

Log the ground truth pose and twist.

Parameters
[in]nowThe current gazebo common::Time

Definition at line 332 of file gazebo_bag_plugin.cpp.

◆ LogMotorVelocities()

void gazebo::GazeboBagPlugin::LogMotorVelocities ( const common::Time  now)
protected

Log all the motor velocities.

Parameters
[in]nowThe current gazebo common::Time

Definition at line 314 of file gazebo_bag_plugin.cpp.

◆ LogWrenches()

void gazebo::GazeboBagPlugin::LogWrenches ( const common::Time  now)
protected

Log all the wrenches.

Parameters
[in]nowThe current gazebo common::Time

Definition at line 369 of file gazebo_bag_plugin.cpp.

◆ OnUpdate()

void gazebo::GazeboBagPlugin::OnUpdate ( const common::UpdateInfo &  _info)
protected

Called when the world is updated.

Parameters
[in]_infoUpdate timing information.

Definition at line 154 of file gazebo_bag_plugin.cpp.

◆ RateThrustCallback()

void gazebo::GazeboBagPlugin::RateThrustCallback ( const mav_msgs::RateThrustConstPtr &  control_msg)
protected

Called when a RateThrust message is received.

Parameters
[in]control_msgA RateThrust message from mav_msgs.

Definition at line 300 of file gazebo_bag_plugin.cpp.

◆ RecordingServiceCallback()

bool gazebo::GazeboBagPlugin::RecordingServiceCallback ( rotors_comm::RecordRosbag::Request &  req,
rotors_comm::RecordRosbag::Response &  res 
)
protected

Called when a request to start or stop recording is received.

Parameters
[in]reqThe request to start or stop recording.
[out]resThe response to be sent back to the client.

Definition at line 398 of file gazebo_bag_plugin.cpp.

◆ StartRecording()

void gazebo::GazeboBagPlugin::StartRecording ( )
protected

Starting recording the rosbag.

Definition at line 166 of file gazebo_bag_plugin.cpp.

◆ StopRecording()

void gazebo::GazeboBagPlugin::StopRecording ( )
protected

Stop recording the rosbag.

Definition at line 235 of file gazebo_bag_plugin.cpp.

◆ WaypointCallback()

void gazebo::GazeboBagPlugin::WaypointCallback ( const trajectory_msgs::MultiDOFJointTrajectoryConstPtr &  trajectory_msg)
protected

Called when a MultiDOFJointTrajectoryPoint message is received.

Parameters
[in]trajectory_msgA MultiDOFJointTrajectoryPoint message from trajectory_msgs.

Definition at line 271 of file gazebo_bag_plugin.cpp.

◆ WindSpeedCallback()

void gazebo::GazeboBagPlugin::WindSpeedCallback ( const rotors_comm::WindSpeedConstPtr &  wind_speed_msg)
protected

Called when a WindSpeed message is received.

Parameters
[in]wind_speed_msgA WindSpeed message from rotors_comm.

Definition at line 307 of file gazebo_bag_plugin.cpp.

◆ writeBag() [1/2]

template<class T >
void gazebo::GazeboBagPlugin::writeBag ( const std::string &  topic,
const ros::Time time,
const T &  msg 
)
inlineprivate

Definition at line 226 of file gazebo_bag_plugin.h.

◆ writeBag() [2/2]

template<class T >
void gazebo::GazeboBagPlugin::writeBag ( const std::string &  topic,
const ros::Time time,
boost::shared_ptr< T const > const &  msg 
)
inlineprivate

Definition at line 245 of file gazebo_bag_plugin.h.

Member Data Documentation

◆ bag_

rosbag::Bag gazebo::GazeboBagPlugin::bag_
private

Definition at line 207 of file gazebo_bag_plugin.h.

◆ bag_filename_

std::string gazebo::GazeboBagPlugin::bag_filename_
private

Definition at line 194 of file gazebo_bag_plugin.h.

◆ child_links_

physics::Link_V gazebo::GazeboBagPlugin::child_links_
private

Definition at line 171 of file gazebo_bag_plugin.h.

◆ command_pose_sub_

ros::Subscriber gazebo::GazeboBagPlugin::command_pose_sub_
private

Definition at line 218 of file gazebo_bag_plugin.h.

◆ command_pose_topic_

std::string gazebo::GazeboBagPlugin::command_pose_topic_
private

Definition at line 185 of file gazebo_bag_plugin.h.

◆ contact_mgr_

physics::ContactManager* gazebo::GazeboBagPlugin::contact_mgr_
private

Pointer to the ContactManager to get all collisions of this link and its children.

Definition at line 177 of file gazebo_bag_plugin.h.

◆ control_attitude_thrust_sub_

ros::Subscriber gazebo::GazeboBagPlugin::control_attitude_thrust_sub_
private

Definition at line 214 of file gazebo_bag_plugin.h.

◆ control_attitude_thrust_topic_

std::string gazebo::GazeboBagPlugin::control_attitude_thrust_topic_
private

Definition at line 186 of file gazebo_bag_plugin.h.

◆ control_motor_speed_sub_

ros::Subscriber gazebo::GazeboBagPlugin::control_motor_speed_sub_
private

Definition at line 215 of file gazebo_bag_plugin.h.

◆ control_motor_speed_topic_

std::string gazebo::GazeboBagPlugin::control_motor_speed_topic_
private

Definition at line 187 of file gazebo_bag_plugin.h.

◆ control_rate_thrust_sub_

ros::Subscriber gazebo::GazeboBagPlugin::control_rate_thrust_sub_
private

Definition at line 216 of file gazebo_bag_plugin.h.

◆ control_rate_thrust_topic_

std::string gazebo::GazeboBagPlugin::control_rate_thrust_topic_
private

Definition at line 188 of file gazebo_bag_plugin.h.

◆ csvOut

std::ofstream gazebo::GazeboBagPlugin::csvOut
private

Definition at line 223 of file gazebo_bag_plugin.h.

◆ external_force_sub_

ros::Subscriber gazebo::GazeboBagPlugin::external_force_sub_
private

Definition at line 212 of file gazebo_bag_plugin.h.

◆ external_force_topic_

std::string gazebo::GazeboBagPlugin::external_force_topic_
private

Definition at line 183 of file gazebo_bag_plugin.h.

◆ frame_id_

std::string gazebo::GazeboBagPlugin::frame_id_
private

Definition at line 192 of file gazebo_bag_plugin.h.

◆ ground_truth_pose_topic_

std::string gazebo::GazeboBagPlugin::ground_truth_pose_topic_
private

Definition at line 180 of file gazebo_bag_plugin.h.

◆ ground_truth_twist_topic_

std::string gazebo::GazeboBagPlugin::ground_truth_twist_topic_
private

Definition at line 181 of file gazebo_bag_plugin.h.

◆ imu_sub_

ros::Subscriber gazebo::GazeboBagPlugin::imu_sub_
private

Definition at line 211 of file gazebo_bag_plugin.h.

◆ imu_topic_

std::string gazebo::GazeboBagPlugin::imu_topic_
private

Definition at line 182 of file gazebo_bag_plugin.h.

◆ is_recording_

bool gazebo::GazeboBagPlugin::is_recording_
private

Whether the plugin is currenly recording a rosbag.

Definition at line 205 of file gazebo_bag_plugin.h.

◆ link_

physics::LinkPtr gazebo::GazeboBagPlugin::link_
private

Definition at line 169 of file gazebo_bag_plugin.h.

◆ link_name_

std::string gazebo::GazeboBagPlugin::link_name_
private

Definition at line 193 of file gazebo_bag_plugin.h.

◆ model_

physics::ModelPtr gazebo::GazeboBagPlugin::model_
private

Definition at line 168 of file gazebo_bag_plugin.h.

◆ motor_joints_

MotorNumberToJointMap gazebo::GazeboBagPlugin::motor_joints_
private

Definition at line 173 of file gazebo_bag_plugin.h.

◆ motor_topic_

std::string gazebo::GazeboBagPlugin::motor_topic_
private

Definition at line 191 of file gazebo_bag_plugin.h.

◆ mtx_

boost::mutex gazebo::GazeboBagPlugin::mtx_
private

Mutex lock for thread safety of writing bag files.

Definition at line 199 of file gazebo_bag_plugin.h.

◆ namespace_

std::string gazebo::GazeboBagPlugin::namespace_
private

Definition at line 179 of file gazebo_bag_plugin.h.

◆ node_handle_

ros::NodeHandle* gazebo::GazeboBagPlugin::node_handle_
private

Definition at line 208 of file gazebo_bag_plugin.h.

◆ recording_service_

ros::ServiceServer gazebo::GazeboBagPlugin::recording_service_
private

Definition at line 221 of file gazebo_bag_plugin.h.

◆ recording_service_name_

std::string gazebo::GazeboBagPlugin::recording_service_name_
private

Definition at line 195 of file gazebo_bag_plugin.h.

◆ rotor_velocity_slowdown_sim_

double gazebo::GazeboBagPlugin::rotor_velocity_slowdown_sim_
private

Definition at line 196 of file gazebo_bag_plugin.h.

◆ update_connection_

event::ConnectionPtr gazebo::GazeboBagPlugin::update_connection_
private

Pointer to the update event connection.

Definition at line 165 of file gazebo_bag_plugin.h.

◆ wait_to_record_

bool gazebo::GazeboBagPlugin::wait_to_record_
private

Whether the plugin should wait for user command to start recording.

Definition at line 202 of file gazebo_bag_plugin.h.

◆ waypoint_sub_

ros::Subscriber gazebo::GazeboBagPlugin::waypoint_sub_
private

Definition at line 213 of file gazebo_bag_plugin.h.

◆ waypoint_topic_

std::string gazebo::GazeboBagPlugin::waypoint_topic_
private

Definition at line 184 of file gazebo_bag_plugin.h.

◆ wind_speed_sub_

ros::Subscriber gazebo::GazeboBagPlugin::wind_speed_sub_
private

Definition at line 217 of file gazebo_bag_plugin.h.

◆ wind_speed_topic_

std::string gazebo::GazeboBagPlugin::wind_speed_topic_
private

Definition at line 189 of file gazebo_bag_plugin.h.

◆ world_

physics::WorldPtr gazebo::GazeboBagPlugin::world_
private

Definition at line 167 of file gazebo_bag_plugin.h.

◆ wrench_topic_

std::string gazebo::GazeboBagPlugin::wrench_topic_
private

Definition at line 190 of file gazebo_bag_plugin.h.


The documentation for this class was generated from the following files:


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