Skip to content

Commit

Permalink
Multiple airspeed sensors in SITL with multi HIL_SENSOR instances (#731)
Browse files Browse the repository at this point in the history 
* Add sensor interface

Abstracts sensor interface for the mavlink class
Add sensor msg mutex

* Enable multiple instances of HIL messages

This commit enables having multiple instances of HILdata instances in order to support multi-sensor setups for sensors that are included in the HIL_SENSOR mavlink message

* Register new HIL sensor instance if id has never been seen

This commit enables registering new sensor istances if the ids have never been seen before. This enables using non-sequential instance IDs

* Convert airspeed plugin into a sensor plugin

This commit sets the airspeed plugin into a sensor plugin. This allows the gazebo_mavlink_interface to automatically subscribes to the sensor topics
  • Loading branch information
@Jaeyoung-Lim
Jaeyoung-Lim authored Mar 22, 2021
1 parent 1f339cd commit 3e5fed0
Show file tree
Hide file tree
Showing 15 changed files with 453 additions and 349 deletions.
13 changes: 11 additions & 2 deletions include/gazebo_airspeed_plugin.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -59,6 +59,9 @@
#include <gazebo/physics/physics.hh>
#include <ignition/math.hh>

#include <gazebo/sensors/SensorTypes.hh>
#include <gazebo/sensors/Sensor.hh>

#include <Airspeed.pb.h>
#include <Wind.pb.h>

Expand All @@ -67,33 +70,39 @@ namespace gazebo

typedef const boost::shared_ptr<const physics_msgs::msgs::Wind> WindPtr;

class GAZEBO_VISIBLE AirspeedPlugin : public ModelPlugin
class GAZEBO_VISIBLE AirspeedPlugin : public SensorPlugin
{
public:
AirspeedPlugin();
virtual ~AirspeedPlugin();

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&);
virtual void OnSensorUpdate();

private:
void WindVelocityCallback(WindPtr& msg);

physics::ModelPtr model_;
physics::WorldPtr world_;
physics::LinkPtr link_;
sensors::SensorPtr parentSensor_;

transport::NodePtr node_handle_;
transport::SubscriberPtr wind_sub_;
transport::PublisherPtr airspeed_pub_;
event::ConnectionPtr updateConnection_;
event::ConnectionPtr updateSensorConnection_;

common::Time last_time_;
std::string namespace_;
std::string link_name_;
std::string model_name_;
std::string airspeed_topic_;

ignition::math::Vector3d wind_vel_;
ignition::math::Vector3d vel_a_;

std::default_random_engine random_generator_;
std::normal_distribution<float> standard_normal_distribution_;
Expand Down
17 changes: 3 additions & 14 deletions include/gazebo_mavlink_interface.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -77,6 +77,7 @@
static const std::regex kDefaultLidarModelNaming(".*(lidar|sf10a)(.*)");
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)");

namespace gazebo {

Expand Down Expand Up @@ -109,7 +110,6 @@ static const std::string kDefaultOpticalFlowTopic = "/px4flow/link/opticalFlow";
static const std::string kDefaultIRLockTopic = "/camera/link/irlock";
static const std::string kDefaultVisionTopic = "/vision_odom";
static const std::string kDefaultMagTopic = "/mag";
static const std::string kDefaultAirspeedTopic = "/airspeed";
static const std::string kDefaultBarometerTopic = "/baro";
static const std::string kDefaultWindTopic = "/world_wind";
static const std::string kDefaultGroundtruthTopic = "/groundtruth";
Expand Down Expand Up @@ -176,11 +176,11 @@ class GazeboMavlinkInterface : public ModelPlugin {
void GroundtruthCallback(GtPtr& groundtruth_msg);
void LidarCallback(LidarPtr& lidar_msg, const int& id);
void SonarCallback(SonarPtr& sonar_msg, const int& id);
void AirspeedCallback(AirspeedPtr& airspeed_msg, const int& id);
void OpticalFlowCallback(OpticalFlowPtr& opticalFlow_msg);
void IRLockCallback(IRLockPtr& irlock_msg);
void VisionCallback(OdomPtr& odom_msg);
void MagnetometerCallback(MagnetometerPtr& mag_msg);
void AirspeedCallback(AirspeedPtr& airspeed_msg);
void BarometerCallback(BarometerPtr& baro_msg);
void WindVelocityCallback(WindPtr& msg);
void SendSensorMessages();
Expand Down Expand Up @@ -229,7 +229,6 @@ class GazeboMavlinkInterface : public ModelPlugin {
transport::SubscriberPtr groundtruth_sub_{nullptr};
transport::SubscriberPtr vision_sub_{nullptr};
transport::SubscriberPtr mag_sub_{nullptr};
transport::SubscriberPtr airspeed_sub_{nullptr};
transport::SubscriberPtr baro_sub_{nullptr};
transport::SubscriberPtr wind_sub_{nullptr};

Expand All @@ -241,7 +240,6 @@ class GazeboMavlinkInterface : public ModelPlugin {
std::string groundtruth_sub_topic_{kDefaultGroundtruthTopic};
std::string vision_sub_topic_{kDefaultVisionTopic};
std::string mag_sub_topic_{kDefaultMagTopic};
std::string airspeed_sub_topic_{kDefaultAirspeedTopic};
std::string baro_sub_topic_{kDefaultBarometerTopic};
std::string wind_sub_topic_{kDefaultWindTopic};

Expand All @@ -252,28 +250,19 @@ class GazeboMavlinkInterface : public ModelPlugin {
common::Time last_imu_time_;
common::Time last_actuator_time_;

bool mag_updated_{false};
bool baro_updated_{false};
bool diff_press_updated_{false};

double groundtruth_lat_rad_{0.0};
double groundtruth_lon_rad_{0.0};
double groundtruth_altitude_{0.0};

double imu_update_interval_{0.004}; ///< Used for non-lockstep

ignition::math::Vector3d velocity_prev_W_;
ignition::math::Vector3d mag_n_;
ignition::math::Vector3d wind_vel_;

double temperature_{25.0};
double pressure_alt_{0.0};
double abs_pressure_{0.0};

bool close_conn_{false};

double optflow_distance_{0.0};
double diff_pressure_{0.0};
double sonar_distance;

bool enable_lockstep_{false};
double speed_factor_{1.0};
Expand Down
74 changes: 71 additions & 3 deletions include/mavlink_interface.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -48,6 +48,7 @@
#include <netinet/in.h>

#include <Eigen/Eigen>
#include<Eigen/StdVector>

#include <mavlink/v2.0/common/mavlink.h>
#include "msgbuffer.h"
Expand Down Expand Up @@ -82,8 +83,65 @@ enum class SensorSource {
DIFF_PRESS = 0b10000000000,
};

namespace SensorData {
struct Imu {
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
Eigen::Vector3d accel_b;
Eigen::Vector3d gyro_b;
};

struct Barometer {
double temperature;
double abs_pressure;
double pressure_alt;
};

struct Magnetometer {
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
Eigen::Vector3d mag_b;
};

struct Airspeed {
double diff_pressure;
};

struct Gps {
int time_utc_usec;
int fix_type;
double latitude_deg;
double longitude_deg;
double altitude;
double eph;
double epv;
double velocity;
double velocity_north;
double velocity_east;
double velocity_down;
double cog;
double satellites_visible;
int id;
};
}

struct HILData {
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
int id=-1;
bool baro_updated{false};
bool diff_press_updated{false};
bool mag_updated{false};
bool imu_updated{false};
double temperature;
double pressure_alt;
double abs_pressure;
double diff_pressure;
Eigen::Vector3d mag_b;
Eigen::Vector3d accel_b;
Eigen::Vector3d gyro_b;
};

class MavlinkInterface {
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
MavlinkInterface();
~MavlinkInterface();
void pollForMAVLinkMessages();
Expand All @@ -93,6 +151,13 @@ class MavlinkInterface {
void open();
void close();
void Load();
void SendSensorMessages(const int &time_usec);
void SendSensorMessages(const int &time_usec, HILData &hil_data);
void SendGpsMessages(const SensorData::Gps &data);
void UpdateBarometer(const SensorData::Barometer &data, const int id = 0);
void UpdateAirspeed(const SensorData::Airspeed &data, const int id = 0);
void UpdateIMU(const SensorData::Imu &data, const int id = 0);
void UpdateMag(const SensorData::Magnetometer &data, const int id = 0);
Eigen::VectorXd GetActuatorControls();
bool GetArmedState();
void onSigInt();
Expand Down Expand Up @@ -120,7 +185,8 @@ class MavlinkInterface {

void handle_message(mavlink_message_t *msg);
void acceptConnections();

void RegisterNewHILSensorInstance(int id);

// Serial interface
void open_serial();
void do_serial_read();
Expand All @@ -133,7 +199,7 @@ class MavlinkInterface {
static const unsigned n_out_max = 16;

int input_index_[n_out_max];

struct sockaddr_in local_simulator_addr_;
socklen_t local_simulator_addr_len_;
struct sockaddr_in remote_simulator_addr_;
Expand Down Expand Up @@ -186,9 +252,10 @@ class MavlinkInterface {
mavlink_message_t m_buffer_{};
std::thread io_thread_;
std::string device_{kDefaultDevice};

std::recursive_mutex mutex_;
std::mutex actuator_mutex_;
std::mutex sensor_msg_mutex_;

std::array<uint8_t, MAX_SIZE> rx_buf_{};
unsigned int baudrate_{kDefaultBaudRate};
Expand All @@ -198,5 +265,6 @@ class MavlinkInterface {
bool hil_mode_;
bool hil_state_level_;

std::vector<HILData, Eigen::aligned_allocator<HILData>> hil_data_;
std::atomic<bool> gotSigInt_ {false};
};
42 changes: 42 additions & 0 deletions models/airspeed/airspeed.sdf
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,42 @@
<?xml version="1.0" ?>
<sdf version="1.6">
<model name="airspeed">
<link name="link">
<pose>0 0 0 0 0 0</pose>
<inertial>
<pose>0 0 0 0 0 0</pose>
<mass>0.015</mass>
<inertia>
<ixx>1e-05</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>1e-05</iyy>
<iyz>0</iyz>
<izz>1e-05</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="airspeed" type="gps">
<pose>0 0 0 0 0 0</pose>
<update_rate>5.0</update_rate>
<always_on>true</always_on>
<visualize>false</visualize>
<plugin name='airspeed_plugin' filename='libgazebo_airspeed_plugin.so'>
<robotNamespace/>
</plugin>
</sensor>
</link>
</model>
</sdf>
15 changes: 15 additions & 0 deletions models/airspeed/model.config
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,15 @@
<?xml version="1.0"?>
<model>
<name>GPS</name>
<version>1.0</version>
<sdf version='1.6'>airspeed.sdf</sdf>

<author>
<name>Jaeyoung Lim</name>
<email>[email protected]</email>
</author>

<description>
Airspeed sensor model
</description>
</model>
40 changes: 6 additions & 34 deletions models/plane/plane.sdf.jinja
View file
Original file line number Diff line number Diff line change
Expand Up @@ -87,38 +87,14 @@
<use_parent_model_frame>1</use_parent_model_frame>
</axis>
</joint>
<link name='plane/airspeed_link'>
<include>
<uri>model://airspeed</uri>
<pose>0 0 0 0 0 0</pose>
<inertial>
<pose>0 0 0 0 0 0</pose>
<mass>0.015</mass>
<inertia>
<ixx>1e-05</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>1e-05</iyy>
<iyz>0</iyz>
<izz>1e-05</izz>
</inertia>
</inertial>
</link>
<joint name='plane/airspeed_joint' type='revolute'>
<child>plane/airspeed_link</child>
<name>airspeed</name>
</include>
<joint name='airspeed_joint' type='fixed'>
<child>airspeed::link</child>
<parent>base_link</parent>
<axis>
<xyz>1 0 0</xyz>
<limit>
<lower>0</lower>
<upper>0</upper>
<effort>0</effort>
<velocity>0</velocity>
</limit>
<dynamics>
<spring_reference>0</spring_reference>
<spring_stiffness>0</spring_stiffness>
</dynamics>
<use_parent_model_frame>1</use_parent_model_frame>
</axis>
</joint>
<link name='rotor_puller'>
<pose>0.3 0 0.0 0 1.57 0</pose>
Expand Down Expand Up @@ -676,10 +652,6 @@
<pubRate>50</pubRate>
<baroTopic>/baro</baroTopic>
</plugin>
<plugin name='airspeed_plugin' filename='libgazebo_airspeed_plugin.so'>
<robotNamespace/>
<linkName>plane/airspeed_link</linkName>
</plugin>
<plugin name='mavlink_interface' filename='libgazebo_mavlink_interface.so'>
<robotNamespace></robotNamespace>
<imuSubTopic>/imu</imuSubTopic>
Expand Down
Loading

0 comments on commit 3e5fed0

Please sign in to comment.