Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Multiple airspeed sensors in SITL with multi HIL_SENSOR instances #731

Merged
merged 4 commits into from
Mar 22, 2021
Merged

Multiple airspeed sensors in SITL with multi HIL_SENSOR instances #731

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
a5419b2
Add sensor interface
Jaeyoung-Lim Dec 18, 2020
da27604
Enable multiple instances of HIL messages
Jaeyoung-Lim Mar 20, 2021
de0d5c9
Register new HIL sensor instance if id has never been seen
Jaeyoung-Lim Mar 20, 2021
10b1daf
Convert airspeed plugin into a sensor plugin
Jaeyoung-Lim Mar 20, 2021
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
13 changes: 11 additions & 2 deletions include/gazebo_airspeed_plugin.h
View file
Open in desktop
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
Open in desktop
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
Open in desktop
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
Open in desktop
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
Open in desktop
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
Open in desktop
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