Add multi magnetometer capability.

Magnetometer plugin now derived from sensorplugin. Magnetometer topic dervied from naming. Updated all models with magnetometer submodel
junwoo091400 · Sep 5, 2022 · b968405 · b968405
1 parent 3312423
commit b968405
Show file tree

Hide file tree

Showing 28 changed files with 319 additions and 208 deletions.
diff --git a/include/gazebo_magnetometer_plugin.h b/include/gazebo_magnetometer_plugin.h
@@ -58,13 +58,15 @@
 
 #include <ignition/math.hh>
 
+#include <gazebo/sensors/SensorTypes.hh>
+#include <gazebo/sensors/MagnetometerSensor.hh>
+
 #include <common.h>
 
 #include <geo_mag_declination.h>
 
 namespace gazebo {
 
-static constexpr auto kDefaultMagnetometerTopic = "mag";
 static constexpr auto kDefaultPubRate = 100.0; // [Hz]. Note: corresponds to most of the mag devices supported in PX4
 
 // Default values for use with ADIS16448 IMU
@@ -74,22 +76,26 @@ static constexpr auto kDefaultBiasCorrelationTime = 6.0e+2; // [s]
 
 typedef const boost::shared_ptr<const sensor_msgs::msgs::Groundtruth> GtPtr;
 
-class MagnetometerPlugin : public ModelPlugin {
+class MagnetometerPlugin : public SensorPlugin {
 public:
   MagnetometerPlugin();
   virtual ~MagnetometerPlugin();
 
 protected:
-  virtual void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf);
+  virtual void Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf);
   virtual void OnUpdate(const common::UpdateInfo&);
   void addNoise(Eigen::Vector3d* magnetic_field, const double dt);
   void GroundtruthCallback(GtPtr&);
   void getSdfParams(sdf::ElementPtr sdf);
 
 private:
   std::string namespace_;
+  sensors::MagnetometerSensorPtr parentSensor_;
+  std::string model_name_;
   physics::ModelPtr model_;
   physics::WorldPtr world_;
+  physics::LinkPtr link_;
+
   std::string mag_topic_;
   transport::NodePtr node_handle_;
   transport::PublisherPtr pub_mag_;

diff --git a/include/gazebo_mavlink_interface.h b/include/gazebo_mavlink_interface.h
@@ -79,6 +79,7 @@ static const std::regex kDefaultSonarModelNaming(".*(sonar|mb1240-xl-ez4)(.*)");
 static const std::regex kDefaultGPSModelNaming(".*(gps|ublox-neo-7M)(.*)");
 static const std::regex kDefaultAirspeedModelJointNaming(".*(airspeed)(.*_joint)");
 static const std::regex kDefaultImuModelJointNaming(".*(imu)(\\d*_joint)");
+static const std::regex kDefaultMagModelJointNaming(".*(mag)(\\d*_joint)");
 
 namespace gazebo {
 
@@ -181,7 +182,7 @@ class GazeboMavlinkInterface : public ModelPlugin {
   void OpticalFlowCallback(OpticalFlowPtr& opticalFlow_msg);
   void IRLockCallback(IRLockPtr& irlock_msg);
   void VisionCallback(OdomPtr& odom_msg);
-  void MagnetometerCallback(MagnetometerPtr& mag_msg);
+  void MagnetometerCallback(MagnetometerPtr& mag_msg, const int& i);
   void BarometerCallback(BarometerPtr& baro_msg);
   void WindVelocityCallback(WindPtr& msg);
   void SendSensorMessages();

diff --git a/models/believer/believer.sdf b/models/believer/believer.sdf
@@ -82,6 +82,15 @@
       <child>airspeed::link</child>
       <parent>base_link</parent>
     </joint>
+    <include>
+      <uri>model://magnetometer</uri>
+      <pose>0 0 0 0 0 0</pose>
+      <name>mag</name>
+    </include>
+    <joint name='mag_joint' type='fixed'>
+      <child>mag::link</child>
+      <parent>base_link</parent>
+    </joint>
     <link name='rotor_right'>
       <pose>0.05 -0.25 0.05 0 1.57 0</pose>
       <inertial>
@@ -556,14 +565,6 @@
     <plugin name='groundtruth_plugin' filename='libgazebo_groundtruth_plugin.so'>
       <robotNamespace/>
     </plugin>
-    <plugin name='magnetometer_plugin' filename='libgazebo_magnetometer_plugin.so'>
-      <robotNamespace/>
-      <pubRate>100</pubRate>
-      <noiseDensity>0.0004</noiseDensity>
-      <randomWalk>6.4e-06</randomWalk>
-      <biasCorrelationTime>600</biasCorrelationTime>
-      <magTopic>/mag</magTopic>
-    </plugin>
     <plugin name='barometer_plugin' filename='libgazebo_barometer_plugin.so'>
       <robotNamespace/>
       <pubRate>50</pubRate>

diff --git a/models/boat/boat.sdf.jinja b/models/boat/boat.sdf.jinja
@@ -444,13 +444,15 @@
       <child>gps::link</child>
       <parent>base_link</parent>
     </joint>
-    <plugin name='magnetometer_plugin' filename='libgazebo_magnetometer_plugin.so'>
-      <robotNamespace/>
-      <pubRate>100</pubRate>
-      <noiseDensity>0.0004</noiseDensity>
-      <randomWalk>6.4e-06</randomWalk>
-      <biasCorrelationTime>600</biasCorrelationTime>
-      <magTopic>/mag</magTopic>
+    <include>
+      <uri>model://magnetometer</uri>
+      <pose>0 0 0 0 0 0</pose>
+      <name>mag</name>
+    </include>
+    <joint name='mag_joint' type='fixed'>
+      <child>mag::link</child>
+      <parent>base_link</parent>
+    </joint>
     </plugin>
     <plugin name='barometer_plugin' filename='libgazebo_barometer_plugin.so'>
       <robotNamespace/>

diff --git a/models/cloudship/cloudship.sdf.jinja b/models/cloudship/cloudship.sdf.jinja
@@ -383,7 +383,7 @@
         </geometry>
       </collision>
     </link>
-    
+
     {# GPS joint which includes GPS plugin #}
     <include>
       <uri>model://gps</uri>
@@ -506,14 +506,15 @@
     </joint>
 
     {# Magnetometer #}
-    <plugin name='magnetometer_plugin' filename='libgazebo_magnetometer_plugin.so'>
-      <robotNamespace/>
-      <pubRate>20</pubRate>
-      <noiseDensity>0.0004</noiseDensity>
-      <randomWalk>6.4e-06</randomWalk>
-      <biasCorrelationTime>600</biasCorrelationTime>
-      <magTopic>/mag</magTopic>
-    </plugin>
+    <include>
+      <uri>model://magnetometer</uri>
+      <pose>0 0 0 0 0 0</pose>
+      <name>mag</name>
+    </include>
+    <joint name='mag_joint' type='fixed'>
+      <child>mag::link</child>
+      <parent>hull</parent>
+    </joint>
 
     {# Barometer #}
     <plugin name='barometer_plugin' filename='libgazebo_barometer_plugin.so'>

diff --git a/models/delta_wing/delta_wing.sdf b/models/delta_wing/delta_wing.sdf
@@ -236,17 +236,18 @@
       <child>gps::link</child>
       <parent>base_link</parent>
     </joint>
+    <include>
+      <uri>model://magnetometer</uri>
+      <pose>0 0 0 0 0 0</pose>
+      <name>mag</name>
+    </include>
+    <joint name='mag_joint' type='fixed'>
+      <child>mag::link</child>
+      <parent>base_link</parent>
+    </joint>
     <plugin name='groundtruth_plugin' filename='libgazebo_groundtruth_plugin.so'>
       <robotNamespace/>
     </plugin>
-    <plugin name='magnetometer_plugin' filename='libgazebo_magnetometer_plugin.so'>
-      <robotNamespace/>
-      <pubRate>100</pubRate>
-      <noiseDensity>0.0004</noiseDensity>
-      <randomWalk>6.4e-06</randomWalk>
-      <biasCorrelationTime>600</biasCorrelationTime>
-      <magTopic>/mag</magTopic>
-    </plugin>
     <plugin name='barometer_plugin' filename='libgazebo_barometer_plugin.so'>
       <robotNamespace/>
       <pubRate>50</pubRate>

diff --git a/models/glider/glider.sdf b/models/glider/glider.sdf
@@ -86,6 +86,15 @@
       <child>airspeed::link</child>
       <parent>base_link</parent>
     </joint>
+    <include>
+      <uri>model://magnetometer</uri>
+      <pose>0 0 0 0 0 0</pose>
+      <name>mag</name>
+    </include>
+    <joint name='mag_joint' type='fixed'>
+      <child>mag::link</child>
+      <parent>base_link</parent>
+    </joint>
     <link name="left_elevon">
       <inertial>
         <mass>0.00000001</mass>
@@ -530,24 +539,16 @@
       <forward>1 0 0</forward>
       <upward>0 1 0</upward>
       <link_name>base_link</link_name>
-      <control_joint_name> 
-         rudder_joint 
-      </control_joint_name> 
+      <control_joint_name>
+         rudder_joint
+      </control_joint_name>
       <control_joint_rad_to_cl>4.0</control_joint_rad_to_cl>
       <robotNamespace></robotNamespace>
       <windSubTopic>world_wind</windSubTopic>
     </plugin>
     <plugin name='groundtruth_plugin' filename='libgazebo_groundtruth_plugin.so'>
       <robotNamespace/>
     </plugin>
-    <plugin name='magnetometer_plugin' filename='libgazebo_magnetometer_plugin.so'>
-      <robotNamespace/>
-      <pubRate>100</pubRate>
-      <noiseDensity>0.0004</noiseDensity>
-      <randomWalk>6.4e-06</randomWalk>
-      <biasCorrelationTime>600</biasCorrelationTime>
-      <magTopic>/mag</magTopic>
-    </plugin>
     <plugin name='barometer_plugin' filename='libgazebo_barometer_plugin.so'>
       <robotNamespace/>
       <pubRate>50</pubRate>

diff --git a/models/iris/iris.sdf.jinja b/models/iris/iris.sdf.jinja
@@ -417,17 +417,18 @@
       <child>gps0::link</child>
       <parent>base_link</parent>
     </joint>
+    <include>
+      <uri>model://magnetometer</uri>
+      <pose>0 0 0 0 0 0</pose>
+      <name>mag</name>
+    </include>
+    <joint name='mag_joint' type='fixed'>
+      <child>mag::link</child>
+      <parent>base_link</parent>
+    </joint>
     <plugin name='groundtruth_plugin' filename='libgazebo_groundtruth_plugin.so'>
       <robotNamespace/>
     </plugin>
-    <plugin name='magnetometer_plugin' filename='libgazebo_magnetometer_plugin.so'>
-      <robotNamespace/>
-      <pubRate>100</pubRate>
-      <noiseDensity>0.0004</noiseDensity>
-      <randomWalk>6.4e-06</randomWalk>
-      <biasCorrelationTime>600</biasCorrelationTime>
-      <magTopic>/mag</magTopic>
-    </plugin>
     <plugin name='barometer_plugin' filename='libgazebo_barometer_plugin.so'>
       <robotNamespace/>
       <pubRate>50</pubRate>

diff --git a/models/iris_hitl/iris_hitl.sdf b/models/iris_hitl/iris_hitl.sdf
@@ -456,17 +456,18 @@
       <parent>base_link</parent>
       <child>gps0::link</child>
     </joint>
+    <include>
+      <uri>model://magnetometer</uri>
+      <pose>0 0 0 0 0 0</pose>
+      <name>mag</name>
+    </include>
+    <joint name='mag_joint' type='fixed'>
+      <child>mag::link</child>
+      <parent>base_link</parent>
+    </joint>
     <plugin name='groundtruth_plugin' filename='libgazebo_groundtruth_plugin.so'>
       <robotNamespace/>
     </plugin>
-    <plugin name='magnetometer_plugin' filename='libgazebo_magnetometer_plugin.so'>
-      <robotNamespace/>
-      <pubRate>100</pubRate>
-      <noiseDensity>0.0004</noiseDensity>
-      <randomWalk>6.4e-06</randomWalk>
-      <biasCorrelationTime>600</biasCorrelationTime>
-      <magTopic>/mag</magTopic>
-    </plugin>
     <plugin name='barometer_plugin' filename='libgazebo_barometer_plugin.so'>
       <robotNamespace/>
       <pubRate>50</pubRate>

diff --git a/models/magnetometer/magnetometer.sdf b/models/magnetometer/magnetometer.sdf
@@ -0,0 +1,44 @@
+<?xml version="1.0" ?>
+<sdf version="1.5">
+  <model name="magnetometer">
+    <pose>0 0 0 0 0 0</pose>
+    <link name="link">
+      <inertial>
+        <mass>0.001</mass>
+        <inertia>
+          <ixx>1e-07</ixx>
+          <ixy>0</ixy>
+          <ixz>0</ixz>
+          <iyy>1e-07</iyy>
+          <iyz>0</iyz>
+          <izz>1e-07</izz>
+        </inertia>
+      </inertial>
+      <visual name="visual">
+        <geometry>
+          <cylinder>
+            <radius>0.01</radius>
+            <length>0.1</length>
+          </cylinder>
+        </geometry>
+        <material>
+          <script>
+            <name>Gazebo/Black</name>
+          </script>
+        </material>
+      </visual>
+      <sensor name="magnetometer" type="magnetometer">
+        <pose>0 0 0 0 0 0</pose>
+        <always_on>true</always_on>
+        <visualize>false</visualize>
+        <plugin name='magnetometer_plugin' filename='libgazebo_magnetometer_plugin.so'>
+          <robotNamespace/>
+          <pubRate>100</pubRate>
+          <noiseDensity>0.0004</noiseDensity>
+          <randomWalk>6.4e-06</randomWalk>
+          <biasCorrelationTime>600</biasCorrelationTime>
+        </plugin>
+      </sensor>
+    </link>
+  </model>
+</sdf>
diff --git a/models/magnetometer/model.config b/models/magnetometer/model.config
@@ -0,0 +1,9 @@
+<model>
+  <name>Magnetometer</name>
+  <version>1.0</version>
+  <sdf version="1.5">magnetometer.sdf</sdf>
+
+  <description>
+    Common magnetometer model.
+  </description>
+</model>
diff --git a/models/matrice_100/matrice_100.sdf.jinja b/models/matrice_100/matrice_100.sdf.jinja
@@ -277,14 +277,15 @@
       <robotNamespace/>
     </plugin>
 
-    <plugin name='magnetometer_plugin' filename='libgazebo_magnetometer_plugin.so'>
-        <robotNamespace/>
-        <pubRate>100</pubRate>
-        <noiseDensity>0.0004</noiseDensity>
-        <randomWalk>6.4e-06</randomWalk>
-        <biasCorrelationTime>600</biasCorrelationTime>
-        <magTopic>/mag</magTopic>
-    </plugin>
+    <include>
+      <uri>model://magnetometer</uri>
+      <pose>0 0 0 0 0 0</pose>
+      <name>mag</name>
+    </include>
+    <joint name='mag_joint' type='fixed'>
+      <child>mag::link</child>
+      <parent>fuselage</parent>
+    </joint>
 
     <plugin name='barometer_plugin' filename='libgazebo_barometer_plugin.so'>
       <robotNamespace/>

diff --git a/models/omnicopter/omnicopter.sdf b/models/omnicopter/omnicopter.sdf
@@ -752,14 +752,15 @@
       <child>gps::link</child>
       <parent>base_link</parent>
     </joint>
-    <plugin name='magnetometer_plugin' filename='libgazebo_magnetometer_plugin.so'>
-      <robotNamespace/>
-      <pubRate>100</pubRate>
-      <noiseDensity>0.0004</noiseDensity>
-      <randomWalk>6.4e-06</randomWalk>
-      <biasCorrelationTime>600</biasCorrelationTime>
-      <magTopic>/mag</magTopic>
-    </plugin>
+    <include>
+      <uri>model://magnetometer</uri>
+      <pose>0 0 0 0 0 0</pose>
+      <name>mag</name>
+    </include>
+    <joint name='mag_joint' type='fixed'>
+      <child>mag::link</child>
+      <parent>base_link</parent>
+    </joint>
     <plugin name='barometer_plugin' filename='libgazebo_barometer_plugin.so'>
       <robotNamespace/>
       <pubRate>50</pubRate>