27 #include <boost/bind.hpp> 36 #include "ConnectGazeboToRosTopic.pb.h" 43 velocity_prev_W_(0, 0, 0),
44 pubs_and_subs_created_(false) {}
51 gzdbg << __FUNCTION__ <<
"() called." << std::endl;
54 gzdbg <<
"_model = " << _model->GetName() << std::endl;
67 if (_sdf->HasElement(
"robotNamespace"))
68 namespace_ = _sdf->GetElement(
"robotNamespace")->Get<std::string>();
70 gzerr <<
"[gazebo_imu_plugin] Please specify a robotNamespace.\n";
73 node_handle_ = transport::NodePtr(
new transport::Node());
78 if (_sdf->HasElement(
"linkName"))
79 link_name_ = _sdf->GetElement(
"linkName")->Get<std::string>();
81 gzerr <<
"[gazebo_imu_plugin] Please specify a linkName.\n";
85 gzthrow(
"[gazebo_imu_plugin] Couldn't find specified link \"" <<
link_name_ 90 getSdfParam<std::string>(_sdf,
"imuTopic",
imu_topic_,
92 getSdfParam<double>(_sdf,
"gyroscopeNoiseDensity",
95 getSdfParam<double>(_sdf,
"gyroscopeBiasRandomWalk",
98 getSdfParam<double>(_sdf,
"gyroscopeBiasCorrelationTime",
102 getSdfParam<double>(_sdf,
"gyroscopeTurnOnBiasSigma",
105 getSdfParam<double>(_sdf,
"accelerometerNoiseDensity",
108 getSdfParam<double>(_sdf,
"accelerometerRandomWalk",
111 getSdfParam<double>(_sdf,
"accelerometerBiasCorrelationTime",
115 getSdfParam<double>(_sdf,
"accelerometerTurnOnBiasSigma",
138 for (
int i = 0; i < 9; i++) {
201 for (
int i = 0; i < 3; ++i) {
214 Eigen::Vector3d* angular_velocity,
216 GZ_ASSERT(linear_acceleration !=
nullptr,
"Linear acceleration was null.");
217 GZ_ASSERT(angular_velocity !=
nullptr,
"Angular velocity was null.");
218 GZ_ASSERT(dt > 0.0,
"Change in time must be greater than 0.");
227 double sigma_b_g_d =
sqrt(-sigma_b_g * sigma_b_g * tau_g / 2.0 *
228 (
exp(-2.0 * dt / tau_g) - 1.0));
230 double phi_g_d =
exp(-1.0 / tau_g * dt);
232 for (
int i = 0; i < 3; ++i) {
236 (*angular_velocity)[i] =
249 double sigma_b_a_d =
sqrt(-sigma_b_a * sigma_b_a * tau_a / 2.0 *
250 (
exp(-2.0 * dt / tau_a) - 1.0));
252 double phi_a_d =
exp(-1.0 / tau_a * dt);
255 for (
int i = 0; i < 3; ++i) {
259 (*linear_acceleration)[i] =
268 gzdbg << __FUNCTION__ <<
"() called." << std::endl;
276 common::Time current_time =
world_->SimTime();
277 double dt = (current_time -
last_time_).Double();
279 double t = current_time.Double();
281 ignition::math::Pose3d T_W_I =
link_->WorldPose();
282 ignition::math::Quaterniond C_W_I = T_W_I.Rot();
284 ignition::math::Vector3d acceleration_I =
287 ignition::math::Vector3d angular_vel_I =
link_->RelativeAngularVel();
289 Eigen::Vector3d linear_acceleration_I(acceleration_I.X(), acceleration_I.Y(),
291 Eigen::Vector3d angular_velocity_I(angular_vel_I.X(), angular_vel_I.Y(),
294 AddNoise(&linear_acceleration_I, &angular_velocity_I, dt);
298 imu_message_.mutable_header()->mutable_stamp()->set_sec(current_time.sec);
301 imu_message_.mutable_header()->mutable_stamp()->set_nsec(current_time.nsec);
315 gazebo::msgs::Quaternion* orientation =
new gazebo::msgs::Quaternion();
316 orientation->set_w(C_W_I.W());
317 orientation->set_x(C_W_I.X());
318 orientation->set_y(C_W_I.Y());
319 orientation->set_z(C_W_I.Z());
322 gazebo::msgs::Vector3d* linear_acceleration =
new gazebo::msgs::Vector3d();
323 linear_acceleration->set_x(linear_acceleration_I[0]);
324 linear_acceleration->set_y(linear_acceleration_I[1]);
325 linear_acceleration->set_z(linear_acceleration_I[2]);
326 imu_message_.set_allocated_linear_acceleration(linear_acceleration);
328 gazebo::msgs::Vector3d* angular_velocity =
new gazebo::msgs::Vector3d();
329 angular_velocity->set_x(angular_velocity_I[0]);
330 angular_velocity->set_y(angular_velocity_I[1]);
331 angular_velocity->set_z(angular_velocity_I[2]);
332 imu_message_.set_allocated_angular_velocity(angular_velocity);
342 gazebo::transport::PublisherPtr connect_gazebo_to_ros_topic_pub =
343 node_handle_->Advertise<gz_std_msgs::ConnectGazeboToRosTopic>(
353 gz_std_msgs::ConnectGazeboToRosTopic connect_gazebo_to_ros_topic_msg;
355 connect_gazebo_to_ros_topic_msg.set_gazebo_topic(
"~/" +
namespace_ +
"/" +
357 connect_gazebo_to_ros_topic_msg.set_ros_topic(
namespace_ +
"/" + imu_topic_);
358 connect_gazebo_to_ros_topic_msg.set_msgtype(
359 gz_std_msgs::ConnectGazeboToRosTopic::IMU);
360 connect_gazebo_to_ros_topic_pub->Publish(connect_gazebo_to_ros_topic_msg,
physics::LinkPtr link_
Pointer to the link.
Eigen::Vector3d gyroscope_bias_
bool pubs_and_subs_created_
Flag that is set to true once CreatePubsAndSubs() is called, used to prevent CreatePubsAndSubs() from...
Eigen::Vector3d accelerometer_turn_on_bias_
void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf)
double gyroscope_random_walk
Gyroscope bias random walk [rad/s/s/sqrt(Hz)].
std::default_random_engine random_generator_
void OnUpdate(const common::UpdateInfo &)
This gets called by the world update start event.
event::ConnectionPtr updateConnection_
Pointer to the update event connection.
physics::ModelPtr model_
Pointer to the model.
physics::WorldPtr world_
Pointer to the world.
gz_sensor_msgs::Imu imu_message_
IMU message.
ignition::math::Vector3d gravity_W_
double accelerometer_turn_on_bias_sigma
Accelerometer turn on bias standard deviation [m/s^2].
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 ...
void CreatePubsAndSubs()
Creates all required publishers and subscribers, incl. routing of messages to/from ROS if required...
double accelerometer_random_walk
Accelerometer bias random walk. [m/s^2/s/sqrt(Hz)].
double gyroscope_bias_correlation_time
Gyroscope bias correlation time constant [s].
static const bool kPrintOnPluginLoad
Eigen::Vector3d gyroscope_turn_on_bias_
INLINE Rall1d< T, V, S > sqrt(const Rall1d< T, V, S > &arg)
double gyroscope_turn_on_bias_sigma
Gyroscope turn on bias standard deviation [rad/s].
double accelerometer_noise_density
Accelerometer noise density (two-sided spectrum) [m/s^2/sqrt(Hz)].
INLINE Rall1d< T, V, S > exp(const Rall1d< T, V, S > &arg)
double gyroscope_noise_density
Gyroscope noise density (two-sided spectrum) [rad/s/sqrt(Hz)].
std::normal_distribution< double > standard_normal_distribution_
transport::NodePtr node_handle_
Handle for the Gazebo node.
static const bool kPrintOnUpdates
static constexpr char IMU[]
Eigen::Vector3d accelerometer_bias_
ImuParameters imu_parameters_
double gravity_magnitude
Norm of the gravitational acceleration [m/s^2].
GZ_REGISTER_MODEL_PLUGIN(GazeboRosP3D)
transport::PublisherPtr imu_pub_
static const std::string kConnectGazeboToRosSubtopic
double accelerometer_bias_correlation_time
Accelerometer bias correlation time constant [s].