Skip to content

Commit

Permalink
Revert "Add multi magnetometer capability. Magnetometer plugin now de…
Browse files Browse the repository at this point in the history 
…rived from sensorplugin. Magnetometer topic dervied from naming. Updated all models with magnetometer submodel"

This reverts commit b968405.
  • Loading branch information
@Jaeyoung-Lim
Jaeyoung-Lim committed Feb 22, 2023
1 parent 9343aaf commit 487a789
Show file tree
Hide file tree
Showing 28 changed files with 209 additions and 319 deletions.
12 changes: 3 additions & 9 deletions include/gazebo_magnetometer_plugin.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -58,15 +58,13 @@

#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
Expand All @@ -76,26 +74,22 @@ static constexpr auto kDefaultBiasCorrelationTime = 6.0e+2; // [s]

typedef const boost::shared_ptr<const sensor_msgs::msgs::Groundtruth> GtPtr;

class MagnetometerPlugin : public SensorPlugin {
class MagnetometerPlugin : public ModelPlugin {
public:
MagnetometerPlugin();
virtual ~MagnetometerPlugin();

protected:
virtual void Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf);
virtual void Load(physics::ModelPtr _model, 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_;
Expand Down
3 changes: 1 addition & 2 deletions include/gazebo_mavlink_interface.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -79,7 +79,6 @@ 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 {

Expand Down Expand Up @@ -182,7 +181,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, const int& i);
void MagnetometerCallback(MagnetometerPtr& mag_msg);
void BarometerCallback(BarometerPtr& baro_msg);
void WindVelocityCallback(WindPtr& msg);
void SendSensorMessages();
Expand Down
17 changes: 8 additions & 9 deletions models/believer/believer.sdf
View file
Original file line number Diff line number Diff line change
Expand Up @@ -82,15 +82,6 @@
<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>
Expand Down Expand Up @@ -565,6 +556,14 @@
<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>
Expand Down
17 changes: 8 additions & 9 deletions models/boat/boat.sdf.jinja
View file
Original file line number Diff line number Diff line change
Expand Up @@ -444,15 +444,14 @@
<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='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>
Expand Down
19 changes: 9 additions & 10 deletions models/cloudship/cloudship.sdf.jinja
View file
Original file line number Diff line number Diff line change
Expand Up @@ -383,7 +383,7 @@
</geometry>
</collision>
</link>

{# GPS joint which includes GPS plugin #}
<include>
<uri>model://gps</uri>
Expand Down Expand Up @@ -506,15 +506,14 @@
</joint>

{# Magnetometer #}
<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>
<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>

{# Barometer #}
<plugin name='barometer_plugin' filename='libgazebo_barometer_plugin.so'>
Expand Down
17 changes: 8 additions & 9 deletions models/delta_wing/delta_wing.sdf
View file
Original file line number Diff line number Diff line change
Expand Up @@ -236,18 +236,17 @@
<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>
Expand Down
23 changes: 11 additions & 12 deletions models/glider/glider.sdf
View file
Original file line number Diff line number Diff line change
Expand Up @@ -86,15 +86,6 @@
<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>
Expand Down Expand Up @@ -539,16 +530,24 @@
<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>
Expand Down
17 changes: 8 additions & 9 deletions models/iris/iris.sdf.jinja
View file
Original file line number Diff line number Diff line change
Expand Up @@ -417,18 +417,17 @@
<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>
Expand Down
17 changes: 8 additions & 9 deletions models/iris_hitl/iris_hitl.sdf
View file
Original file line number Diff line number Diff line change
Expand Up @@ -456,18 +456,17 @@
<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>
Expand Down
44 changes: 0 additions & 44 deletions models/magnetometer/magnetometer.sdf
View file

This file was deleted.

9 changes: 0 additions & 9 deletions models/magnetometer/model.config
View file

This file was deleted.

17 changes: 8 additions & 9 deletions models/matrice_100/matrice_100.sdf.jinja
View file
Original file line number Diff line number Diff line change
Expand Up @@ -277,15 +277,14 @@
<robotNamespace/>
</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='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/>
Expand Down
17 changes: 8 additions & 9 deletions models/omnicopter/omnicopter.sdf
View file
Original file line number Diff line number Diff line change
Expand Up @@ -752,15 +752,14 @@
<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='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>
Expand Down
Loading

0 comments on commit 487a789

Please sign in to comment.