#include <gazebo_imu_plugin.h>
Public Member Functions | |
GazeboImuPlugin () | |
void | InitializeParams () |
void | Publish () |
~GazeboImuPlugin () | |
Protected Member Functions | |
void | AddNoise (Eigen::Vector3d *linear_acceleration, Eigen::Vector3d *angular_velocity, const double dt) |
This method adds noise to acceleration and angular rates for accelerometer and gyroscope measurement simulation. | |
void | Load (physics::ModelPtr _model, sdf::ElementPtr _sdf) |
void | OnUpdate (const common::UpdateInfo &) |
This gets called by the world update start event. | |
Private Member Functions | |
void | CreatePubsAndSubs () |
Creates all required publishers and subscribers, incl. routing of messages to/from ROS if required. | |
Private Attributes | |
Eigen::Vector3d | accelerometer_bias_ |
Eigen::Vector3d | accelerometer_turn_on_bias_ |
std::string | frame_id_ |
ignition::math::Vector3d | gravity_W_ |
Eigen::Vector3d | gyroscope_bias_ |
Eigen::Vector3d | gyroscope_turn_on_bias_ |
gz_sensor_msgs::Imu | imu_message_ |
IMU message. | |
ImuParameters | imu_parameters_ |
transport::PublisherPtr | imu_pub_ |
std::string | imu_topic_ |
common::Time | last_time_ |
physics::LinkPtr | link_ |
Pointer to the link. | |
std::string | link_name_ |
physics::ModelPtr | model_ |
Pointer to the model. | |
std::string | namespace_ |
transport::NodePtr | node_handle_ |
Handle for the Gazebo node. | |
bool | pubs_and_subs_created_ |
Flag that is set to true once CreatePubsAndSubs() is called, used to prevent CreatePubsAndSubs() from be called on every OnUpdate(). | |
std::default_random_engine | random_generator_ |
std::normal_distribution< double > | standard_normal_distribution_ |
event::ConnectionPtr | updateConnection_ |
Pointer to the update event connection. | |
ignition::math::Vector3d | velocity_prev_W_ |
physics::WorldPtr | world_ |
Pointer to the world. |
Definition at line 97 of file gazebo_imu_plugin.h.
Definition at line 40 of file gazebo_imu_plugin.cpp.
Definition at line 46 of file gazebo_imu_plugin.cpp.
void gazebo::GazeboImuPlugin::AddNoise | ( | Eigen::Vector3d * | linear_acceleration, |
Eigen::Vector3d * | angular_velocity, | ||
const double | dt | ||
) | [protected] |
This method adds noise to acceleration and angular rates for accelerometer and gyroscope measurement simulation.
Definition at line 213 of file gazebo_imu_plugin.cpp.
void gazebo::GazeboImuPlugin::CreatePubsAndSubs | ( | ) | [private] |
Creates all required publishers and subscribers, incl. routing of messages to/from ROS if required.
Call this once the first time OnUpdate() is called (can't be called from Load() because there is no guarantee GazeboRosInterfacePlugin has has loaded and listening to ConnectGazeboToRosTopic and ConnectRosToGazeboTopic messages).
Definition at line 340 of file gazebo_imu_plugin.cpp.
void gazebo::GazeboImuPlugin::Load | ( | physics::ModelPtr | _model, |
sdf::ElementPtr | _sdf | ||
) | [protected] |
Definition at line 49 of file gazebo_imu_plugin.cpp.
void gazebo::GazeboImuPlugin::OnUpdate | ( | const common::UpdateInfo & | _info | ) | [protected] |
This gets called by the world update start event.
Calculates IMU parameters and then publishes one IMU message.
Definition at line 266 of file gazebo_imu_plugin.cpp.
void gazebo::GazeboImuPlugin::Publish | ( | ) |
Eigen::Vector3d gazebo::GazeboImuPlugin::accelerometer_bias_ [private] |
Definition at line 170 of file gazebo_imu_plugin.h.
Eigen::Vector3d gazebo::GazeboImuPlugin::accelerometer_turn_on_bias_ [private] |
Definition at line 173 of file gazebo_imu_plugin.h.
Definition at line 141 of file gazebo_imu_plugin.h.
ignition::math::Vector3d gazebo::GazeboImuPlugin::gravity_W_ [private] |
Definition at line 166 of file gazebo_imu_plugin.h.
Eigen::Vector3d gazebo::GazeboImuPlugin::gyroscope_bias_ [private] |
Definition at line 169 of file gazebo_imu_plugin.h.
Eigen::Vector3d gazebo::GazeboImuPlugin::gyroscope_turn_on_bias_ [private] |
Definition at line 172 of file gazebo_imu_plugin.h.
gz_sensor_msgs::Imu gazebo::GazeboImuPlugin::imu_message_ [private] |
IMU message.
This is modified everytime OnUpdate() is called,
Definition at line 164 of file gazebo_imu_plugin.h.
Definition at line 175 of file gazebo_imu_plugin.h.
transport::PublisherPtr gazebo::GazeboImuPlugin::imu_pub_ [private] |
Definition at line 139 of file gazebo_imu_plugin.h.
Definition at line 134 of file gazebo_imu_plugin.h.
common::Time gazebo::GazeboImuPlugin::last_time_ [private] |
Definition at line 159 of file gazebo_imu_plugin.h.
physics::LinkPtr gazebo::GazeboImuPlugin::link_ [private] |
Pointer to the link.
Definition at line 154 of file gazebo_imu_plugin.h.
Definition at line 142 of file gazebo_imu_plugin.h.
physics::ModelPtr gazebo::GazeboImuPlugin::model_ [private] |
Pointer to the model.
Definition at line 151 of file gazebo_imu_plugin.h.
Definition at line 133 of file gazebo_imu_plugin.h.
transport::NodePtr gazebo::GazeboImuPlugin::node_handle_ [private] |
Handle for the Gazebo node.
Definition at line 137 of file gazebo_imu_plugin.h.
bool gazebo::GazeboImuPlugin::pubs_and_subs_created_ [private] |
Flag that is set to true once CreatePubsAndSubs() is called, used to prevent CreatePubsAndSubs() from be called on every OnUpdate().
Definition at line 124 of file gazebo_imu_plugin.h.
std::default_random_engine gazebo::GazeboImuPlugin::random_generator_ [private] |
Definition at line 144 of file gazebo_imu_plugin.h.
std::normal_distribution<double> gazebo::GazeboImuPlugin::standard_normal_distribution_ [private] |
Definition at line 145 of file gazebo_imu_plugin.h.
Pointer to the update event connection.
Definition at line 157 of file gazebo_imu_plugin.h.
ignition::math::Vector3d gazebo::GazeboImuPlugin::velocity_prev_W_ [private] |
Definition at line 167 of file gazebo_imu_plugin.h.
physics::WorldPtr gazebo::GazeboImuPlugin::world_ [private] |
Pointer to the world.
Definition at line 148 of file gazebo_imu_plugin.h.