22 #include "ConnectGazeboToRosTopic.pb.h" 28 random_generator_(random_device_()),
29 pubs_and_subs_created_(false) {
37 sdf::ElementPtr _sdf) {
39 gzdbg << __FUNCTION__ <<
"() called." << std::endl;
47 if (_sdf->HasElement(
"robotNamespace"))
48 namespace_ = _sdf->GetElement(
"robotNamespace")->Get<std::string>();
50 gzerr <<
"[gazebo_magnetometer_plugin] Please specify a robotNamespace.\n";
52 node_handle_ = gazebo::transport::NodePtr(
new transport::Node());
58 std::string link_name;
59 if (_sdf->HasElement(
"linkName"))
60 link_name = _sdf->GetElement(
"linkName")->Get<std::string>();
62 gzerr <<
"[gazebo_magnetometer_plugin] Please specify a linkName.\n";
66 gzthrow(
"[gazebo_magnetometer_plugin] Couldn't find specified link \"" 67 << link_name <<
"\".");
85 getSdfParam<SdfVector3>(_sdf,
"noiseNormal", noise_normal, zeros3);
86 getSdfParam<SdfVector3>(_sdf,
"noiseUniformInitialBias",
87 noise_uniform_initial_bias, zeros3);
102 noise_uniform_initial_bias.X());
104 noise_uniform_initial_bias.Y());
106 noise_uniform_initial_bias.Z());
117 for (
int i = 0; i < 9; i++) {
120 mag_message_.add_magnetic_field_covariance(noise_normal.X() *
129 mag_message_.add_magnetic_field_covariance(noise_normal.Y() *
138 mag_message_.add_magnetic_field_covariance(noise_normal.Z() *
147 gzdbg << __FUNCTION__ <<
"() called." << std::endl;
156 ignition::math::Pose3d T_W_B =
link_->WorldPose();
157 common::Time current_time =
world_->SimTime();
165 ignition::math::Vector3d field_B = T_W_B.Rot().RotateVectorReverse(
mag_W_ + mag_noise);
168 mag_message_.mutable_header()->mutable_stamp()->set_sec(current_time.sec);
169 mag_message_.mutable_header()->mutable_stamp()->set_nsec(current_time.nsec);
170 mag_message_.mutable_magnetic_field()->set_x(field_B.X());
171 mag_message_.mutable_magnetic_field()->set_y(field_B.Y());
172 mag_message_.mutable_magnetic_field()->set_z(field_B.Z());
180 gazebo::transport::PublisherPtr connect_gazebo_to_ros_topic_pub =
181 node_handle_->Advertise<gz_std_msgs::ConnectGazeboToRosTopic>(
191 gz_std_msgs::ConnectGazeboToRosTopic connect_gazebo_to_ros_topic_msg;
193 connect_gazebo_to_ros_topic_msg.set_gazebo_topic(
"~/" +
namespace_ +
"/" +
194 magnetometer_topic_);
195 connect_gazebo_to_ros_topic_msg.set_ros_topic(
namespace_ +
"/" +
196 magnetometer_topic_);
197 connect_gazebo_to_ros_topic_msg.set_msgtype(
198 gz_std_msgs::ConnectGazeboToRosTopic::MAGNETIC_FIELD);
199 connect_gazebo_to_ros_topic_pub->Publish(connect_gazebo_to_ros_topic_msg,
void CreatePubsAndSubs()
Creates all required publishers and subscribers, incl. routing of messages to/from ROS if required...
gz_sensor_msgs::MagneticField mag_message_
Magnetic field message.
bool pubs_and_subs_created_
Flag that is set to true once CreatePubsAndSubs() is called, used to prevent CreatePubsAndSubs() from...
event::ConnectionPtr updateConnection_
ignition::math::Vector3d mag_W_
virtual ~GazeboMagnetometerPlugin()
void OnUpdate(const common::UpdateInfo &)
static constexpr double kDefaultRefMagEast
NormalDistribution noise_n_[3]
std::normal_distribution NormalDistribution
static const bool kPrintOnPluginLoad
std::uniform_real_distribution UniformDistribution
std::mt19937 random_generator_
static constexpr char MAGNETIC_FIELD[]
static constexpr double kDefaultRefMagDown
gazebo::transport::NodePtr node_handle_
gazebo::transport::PublisherPtr magnetometer_pub_
static const bool kPrintOnUpdates
physics::LinkPtr link_
Pointer to the link.
GZ_REGISTER_MODEL_PLUGIN(GazeboRosP3D)
static const std::string kConnectGazeboToRosSubtopic
GazeboMagnetometerPlugin()
physics::ModelPtr model_
Pointer to the model.
std::string magnetometer_topic_
void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf)
static constexpr double kDefaultRefMagNorth
physics::WorldPtr world_
Pointer to the world.