diff --git a/include/gazebo_airspeed_plugin.h b/include/gazebo_airspeed_plugin.h index ce3feac752..872965a032 100644 --- a/include/gazebo_airspeed_plugin.h +++ b/include/gazebo_airspeed_plugin.h @@ -59,6 +59,9 @@ #include #include +#include +#include + #include #include @@ -67,15 +70,16 @@ namespace gazebo typedef const boost::shared_ptr 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); @@ -83,17 +87,22 @@ class GAZEBO_VISIBLE AirspeedPlugin : public ModelPlugin 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 standard_normal_distribution_; diff --git a/include/gazebo_mavlink_interface.h b/include/gazebo_mavlink_interface.h index 678cb4c965..6b0d60e07c 100644 --- a/include/gazebo_mavlink_interface.h +++ b/include/gazebo_mavlink_interface.h @@ -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 { @@ -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"; @@ -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(); @@ -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}; @@ -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}; @@ -252,10 +250,6 @@ 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}; @@ -263,17 +257,12 @@ class GazeboMavlinkInterface : public ModelPlugin { 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}; diff --git a/include/mavlink_interface.h b/include/mavlink_interface.h index 0bc674e2ad..349b2a8a07 100644 --- a/include/mavlink_interface.h +++ b/include/mavlink_interface.h @@ -48,6 +48,7 @@ #include #include +#include #include #include "msgbuffer.h" @@ -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(); @@ -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(); @@ -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(); @@ -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_; @@ -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 rx_buf_{}; unsigned int baudrate_{kDefaultBaudRate}; @@ -198,5 +265,6 @@ class MavlinkInterface { bool hil_mode_; bool hil_state_level_; + std::vector> hil_data_; std::atomic gotSigInt_ {false}; }; diff --git a/models/airspeed/airspeed.sdf b/models/airspeed/airspeed.sdf new file mode 100644 index 0000000000..6c4990c0dc --- /dev/null +++ b/models/airspeed/airspeed.sdf @@ -0,0 +1,42 @@ + + + + + 0 0 0 0 0 0 + + 0 0 0 0 0 0 + 0.015 + + 1e-05 + 0 + 0 + 1e-05 + 0 + 1e-05 + + + + + + 0.01 + 0.1 + + + + + + + + 0 0 0 0 0 0 + 5.0 + true + false + + + + + + + diff --git a/models/airspeed/model.config b/models/airspeed/model.config new file mode 100644 index 0000000000..b690f65c82 --- /dev/null +++ b/models/airspeed/model.config @@ -0,0 +1,15 @@ + + + GPS + 1.0 + airspeed.sdf + + + Jaeyoung Lim + jaeyoung@auterion.com + + + + Airspeed sensor model + + diff --git a/models/plane/plane.sdf.jinja b/models/plane/plane.sdf.jinja index e6ab2ae72c..143c76622f 100644 --- a/models/plane/plane.sdf.jinja +++ b/models/plane/plane.sdf.jinja @@ -87,38 +87,14 @@ 1 - + + model://airspeed 0 0 0 0 0 0 - - 0 0 0 0 0 0 - 0.015 - - 1e-05 - 0 - 0 - 1e-05 - 0 - 1e-05 - - - - - plane/airspeed_link + airspeed + + + airspeed::link base_link - - 1 0 0 - - 0 - 0 - 0 - 0 - - - 0 - 0 - - 1 - 0.3 0 0.0 0 1.57 0 @@ -676,10 +652,6 @@ 50 /baro - - - plane/airspeed_link - /imu diff --git a/models/standard_vtol/standard_vtol.sdf.jinja b/models/standard_vtol/standard_vtol.sdf.jinja index a6e98e1179..ffdbc51f82 100644 --- a/models/standard_vtol/standard_vtol.sdf.jinja +++ b/models/standard_vtol/standard_vtol.sdf.jinja @@ -178,38 +178,14 @@ 1 - + + model://airspeed 0 0 0 0 0 0 - - 0 0 0 0 0 0 - 0.015 - - 1e-05 - 0 - 0 - 1e-05 - 0 - 1e-05 - - - - - standard_vtol/airspeed_link + airspeed + + + airspeed::link base_link - - 1 0 0 - - 0 - 0 - 0 - 0 - - - 0 - 0 - - 1 - 0.35 -0.35 0.07 0 0 0 @@ -920,10 +896,6 @@ 50 /baro - - - standard_vtol/airspeed_link - /imu diff --git a/models/standard_vtol_hitl/standard_vtol_hitl.sdf b/models/standard_vtol_hitl/standard_vtol_hitl.sdf index 20253e24a9..3db9efad60 100644 --- a/models/standard_vtol_hitl/standard_vtol_hitl.sdf +++ b/models/standard_vtol_hitl/standard_vtol_hitl.sdf @@ -177,38 +177,14 @@ 1 - + + model://airspeed 0 0 0 0 0 0 - - 0 0 0 0 0 0 - 0.015 - - 1e-05 - 0 - 0 - 1e-05 - 0 - 1e-05 - - - - - standard_vtol_hitl/airspeed_link + airspeed + + + airspeed::link base_link - - 1 0 0 - - 0 - 0 - 0 - 0 - - - 0 - 0 - - 1 - 0.35 -0.35 0.07 0 0 0 @@ -919,10 +895,6 @@ 50 /baro - - - standard_vtol_hitl/airspeed_link - /imu diff --git a/models/tailsitter/tailsitter.sdf b/models/tailsitter/tailsitter.sdf index c996bcb69e..c929461caa 100644 --- a/models/tailsitter/tailsitter.sdf +++ b/models/tailsitter/tailsitter.sdf @@ -85,38 +85,14 @@ 1 - - 0 0 0 0 -1.57 0 - - 0 0 0 0 0 0 - 0.015 - - 1e-05 - 0 - 0 - 1e-05 - 0 - 1e-05 - - - - - tailsitter/airspeed_link + + model://airspeed + 0 0 0 0 0 0 + airspeed + + + airspeed::link base_link - - 1 0 0 - - 0 - 0 - 0 - 0 - - - 0 - 0 - - 1 - 0.3 -0.3 0.4 0 0 0 @@ -662,10 +638,6 @@ 50 /baro - - - tailsitter/airspeed_link - /imu diff --git a/models/techpod/techpod.sdf b/models/techpod/techpod.sdf index e51616d3de..fd46778238 100644 --- a/models/techpod/techpod.sdf +++ b/models/techpod/techpod.sdf @@ -83,39 +83,6 @@ 1 - - 0 0 0 0 0 0 - - 0 0 0 0 0 0 - 0.015 - - 1e-05 - 0 - 0 - 1e-05 - 0 - 1e-05 - - - - - techpod/airspeed_link - base_link - - 1 0 0 - - 0 - 0 - 0 - 0 - - - 0 - 0 - - 1 - - -0.025 0 0.15 0 1.57 0 @@ -497,6 +464,15 @@ gps::link base_link + + model://airspeed + 0 0 0 0 0 0 + airspeed + + + airspeed::link + base_link + 0.05984281113 4.752798721 @@ -677,10 +653,6 @@ 50 /baro - - - techpod/airspeed_link - /imu diff --git a/models/tiltrotor/tiltrotor.sdf.jinja b/models/tiltrotor/tiltrotor.sdf.jinja index effa4893e3..2193325024 100644 --- a/models/tiltrotor/tiltrotor.sdf.jinja +++ b/models/tiltrotor/tiltrotor.sdf.jinja @@ -117,38 +117,14 @@ 1 - + + model://airspeed 0 0 0 0 0 0 - - 0 0 0 0 0 0 - 0.015 - - 1e-05 - 0 - 0 - 1e-05 - 0 - 1e-05 - - - - - tiltrotor/airspeed_link + airspeed + + + airspeed::link base_link - - 1 0 0 - - 0 - 0 - 0 - 0 - - - 0 - 0 - - 1 - 0.35 -0.35 0.02 0 0 0 @@ -1060,10 +1036,6 @@ 50 /baro - - - tiltrotor/airspeed_link - /imu diff --git a/src/gazebo_airspeed_plugin.cpp b/src/gazebo_airspeed_plugin.cpp index 8a02c8fca8..84a37b4bd2 100644 --- a/src/gazebo_airspeed_plugin.cpp +++ b/src/gazebo_airspeed_plugin.cpp @@ -39,11 +39,12 @@ */ #include +#include namespace gazebo { -GZ_REGISTER_MODEL_PLUGIN(AirspeedPlugin) +GZ_REGISTER_SENSOR_PLUGIN(AirspeedPlugin) -AirspeedPlugin::AirspeedPlugin() : ModelPlugin(), +AirspeedPlugin::AirspeedPlugin() : SensorPlugin(), diff_pressure_stddev_(0.01f), temperature_(20.0f) { } @@ -54,11 +55,40 @@ AirspeedPlugin::~AirspeedPlugin() updateConnection_->~Connection(); } -void AirspeedPlugin::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf) +void AirspeedPlugin::Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf) { + // Get then name of the parent sensor + parentSensor_ = std::dynamic_pointer_cast(_parent); + if (!parentSensor_) + gzthrow("AirspeedPlugin requires a Airspeed Sensor as its parent"); + + // Get the root model name + const std::string scopedName = _parent->ParentName(); + link_name_ = scopedName; + std::vector names_splitted; + boost::split(names_splitted, scopedName, boost::is_any_of("::")); + names_splitted.erase(std::remove_if(begin(names_splitted), end(names_splitted), + [](const std::string& name) + { return name.size() == 0; }), end(names_splitted)); + const std::string rootModelName = names_splitted.front(); // The first element is the name of the root model + // the second to the last name is the model name + const std::string parentSensorModelName = names_splitted.rbegin()[1]; + + // store the model name + model_name_ = names_splitted[0]; + + // get gps topic name + if(_sdf->HasElement("topic")) { + airspeed_topic_ = _sdf->GetElement("topic")->Get(); + } else { + // if not set by parameter, get the topic name from the model name + airspeed_topic_ = parentSensorModelName; + gzwarn << "[gazebo_airspeed_plugin]: " + names_splitted.front() + "::" + names_splitted.rbegin()[1] + + " using airspeed topic \"" << parentSensorModelName << "\"\n"; + } + // Store the pointer to the model. - model_ = _model; - world_ = model_->GetWorld(); + world_ = physics::get_world(parentSensor_->WorldName()); if (_sdf->HasElement("robotNamespace")) { namespace_ = _sdf->GetElement("robotNamespace")->Get(); @@ -69,40 +99,39 @@ void AirspeedPlugin::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf) node_handle_ = transport::NodePtr(new transport::Node()); node_handle_->Init(namespace_); - if (_sdf->HasElement("linkName")) - link_name_ = _sdf->GetElement("linkName")->Get(); - else - gzerr << "[gazebo_airspeed_plugin] Please specify a linkName.\n"; - // Get the pointer to the link - link_ = model_->GetLink(link_name_); - if (link_ == NULL) - gzthrow("[gazebo_airspeed_plugin] Couldn't find specified link \"" << link_name_ << "\"."); - + parentSensor_->SetUpdateRate(10.0); + parentSensor_->SetActive(false); + updateSensorConnection_ = parentSensor_->ConnectUpdated(boost::bind(&AirspeedPlugin::OnSensorUpdate, this)); + parentSensor_->SetActive(true); // Listen to the update event. This event is broadcast every // simulation iteration. updateConnection_ = event::Events::ConnectWorldUpdateBegin( boost::bind(&AirspeedPlugin::OnUpdate, this, _1)); - airspeed_pub_ = node_handle_->Advertise("~/" + model_->GetName() + "/airspeed", 10); + airspeed_pub_ = node_handle_->Advertise("~/" + model_name_ + "/link/" + airspeed_topic_, 10); wind_sub_ = node_handle_->Subscribe("~/world_wind", &AirspeedPlugin::WindVelocityCallback, this); - getSdfParam(_sdf, "DiffPressureStdev", diff_pressure_stddev_, diff_pressure_stddev_); getSdfParam(_sdf, "Temperature", temperature_, temperature_); } void AirspeedPlugin::OnUpdate(const common::UpdateInfo&){ +#if GAZEBO_MAJOR_VERSION >= 9 + model_ = world_->ModelByName(model_name_); + physics::EntityPtr parentEntity = world_->EntityByName(link_name_); +#else + model_ = world_->GetModel(model_name_); + physics::EntityPtr parentEntity = world_->GetEntity(link_name_); +#endif + link_ = boost::dynamic_pointer_cast(parentEntity); + if (link_ == NULL) + gzthrow("[gazebo_airspeed_plugin] Couldn't find specified link \"" << link_name_ << "\"."); + #if GAZEBO_MAJOR_VERSION >= 9 common::Time current_time = world_->SimTime(); #else common::Time current_time = world_->GetSimTime(); #endif - const float temperature_msl = 288.0f; // temperature at MSL (Kelvin) - float temperature_local = temperature_ + 273.0f; - const float density_ratio = powf((temperature_msl/temperature_local) , 4.256f); - float rho = 1.225f / density_ratio; - - const float diff_pressure_noise = standard_normal_distribution_(random_generator_) * diff_pressure_stddev_; #if GAZEBO_MAJOR_VERSION >= 9 ignition::math::Pose3d T_W_I = link_->WorldPose(); @@ -112,21 +141,31 @@ void AirspeedPlugin::OnUpdate(const common::UpdateInfo&){ ignition::math::Quaterniond C_W_I = T_W_I.Rot(); #if GAZEBO_MAJOR_VERSION >= 9 - ignition::math::Vector3d vel_a = link_->RelativeLinearVel() - C_W_I.RotateVectorReverse(wind_vel_); + vel_a_ = link_->RelativeLinearVel() - C_W_I.RotateVectorReverse(wind_vel_); #else - ignition::math::Vector3d vel_a = ignitionFromGazeboMath(link_->GetRelativeLinearVel()) - C_W_I.RotateVectorReverse(wind_vel_); + vel_a_ = ignitionFromGazeboMath(link_->GetRelativeLinearVel()) - C_W_I.RotateVectorReverse(wind_vel_); #endif - double diff_pressure = 0.005f * rho * vel_a.X() * vel_a.X() + diff_pressure_noise; + + last_time_ = current_time; +} + +void AirspeedPlugin::OnSensorUpdate() { + const float temperature_msl = 288.0f; // temperature at MSL (Kelvin) + float temperature_local = temperature_ + 273.0f; + const float density_ratio = powf((temperature_msl/temperature_local) , 4.256f); + float rho = 1.225f / density_ratio; + + const float diff_pressure_noise = standard_normal_distribution_(random_generator_) * diff_pressure_stddev_; + double diff_pressure = 0.005f * rho * vel_a_.X() * vel_a_.X() + diff_pressure_noise; // calculate differential pressure in hPa sensor_msgs::msgs::Airspeed airspeed_msg; - airspeed_msg.set_time_usec(current_time.Double() * 1e6); + airspeed_msg.set_time_usec(last_time_.Double() * 1e6); airspeed_msg.set_diff_pressure(diff_pressure); airspeed_pub_->Publish(airspeed_msg); - - last_time_ = current_time; } + void AirspeedPlugin::WindVelocityCallback(WindPtr& msg) { wind_vel_ = ignition::math::Vector3d(msg->velocity().x(), msg->velocity().y(), diff --git a/src/gazebo_magnetometer_plugin.cpp b/src/gazebo_magnetometer_plugin.cpp index afe888abb9..907519fa5c 100644 --- a/src/gazebo_magnetometer_plugin.cpp +++ b/src/gazebo_magnetometer_plugin.cpp @@ -194,17 +194,27 @@ void MagnetometerPlugin::OnUpdate(const common::UpdateInfo&) float X = H * cosf(declination_rad); float Y = H * sinf(declination_rad); + ignition::math::Vector3d magnetic_field_I(X, Y, Z); + +#if GAZEBO_MAJOR_VERSION >= 9 + ignition::math::Pose3d T_W_I = model_->WorldPose(); +#else + ignition::math::Pose3d T_W_I = ignitionFromGazeboMath(model_->GetWorldPose()); +#endif + ignition::math::Quaterniond q_body_to_world = q_ENU_to_NED * T_W_I.Rot() * q_FLU_to_FRD.Inverse(); + + ignition::math::Vector3d magnetic_field_B = q_body_to_world.RotateVectorReverse(magnetic_field_I); // Magnetometer noise - Eigen::Vector3d magnetic_field_I(X, Y, Z); - addNoise(&magnetic_field_I, dt); + Eigen::Vector3d measured_mag(magnetic_field_B.X(), magnetic_field_B.Y(), magnetic_field_B.Z()); + addNoise(&measured_mag, dt); // Fill magnetometer messgae mag_message_.set_time_usec(current_time.Double() * 1e6); gazebo::msgs::Vector3d* magnetic_field = new gazebo::msgs::Vector3d(); - magnetic_field->set_x(magnetic_field_I[0]); - magnetic_field->set_y(magnetic_field_I[1]); - magnetic_field->set_z(magnetic_field_I[2]); + magnetic_field->set_x(measured_mag[0]); + magnetic_field->set_y(measured_mag[1]); + magnetic_field->set_z(measured_mag[2]); mag_message_.set_allocated_magnetic_field(magnetic_field); last_pub_time_ = current_time; diff --git a/src/gazebo_mavlink_interface.cpp b/src/gazebo_mavlink_interface.cpp index bc557f321b..a749d48440 100644 --- a/src/gazebo_mavlink_interface.cpp +++ b/src/gazebo_mavlink_interface.cpp @@ -176,7 +176,6 @@ void GazeboMavlinkInterface::CreateSensorSubscription( auto subscriberPtr = node_handle_->Subscribe("~/" + model_name + "/link/" + sensor_name, &SensorHelperStorage::callback, &callback_entry.first->second); - // Store the SubscriberPtr, sensor ID and sensor orientation sensor_map_.insert(std::pair(subscriberPtr, SensorIdRot_P(sensor_id, sensor_orientation)) @@ -212,7 +211,6 @@ void GazeboMavlinkInterface::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf opticalFlow_sub_topic_, opticalFlow_sub_topic_); getSdfParam(_sdf, "irlockSubTopic", irlock_sub_topic_, irlock_sub_topic_); getSdfParam(_sdf, "magSubTopic", mag_sub_topic_, mag_sub_topic_); - getSdfParam(_sdf, "airspeedSubTopic", airspeed_sub_topic_, airspeed_sub_topic_); getSdfParam(_sdf, "baroSubTopic", baro_sub_topic_, baro_sub_topic_); getSdfParam(_sdf, "groundtruthSubTopic", groundtruth_sub_topic_, groundtruth_sub_topic_); @@ -427,7 +425,6 @@ void GazeboMavlinkInterface::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf groundtruth_sub_ = node_handle_->Subscribe("~/" + model_->GetName() + groundtruth_sub_topic_, &GazeboMavlinkInterface::GroundtruthCallback, this); vision_sub_ = node_handle_->Subscribe("~/" + model_->GetName() + vision_sub_topic_, &GazeboMavlinkInterface::VisionCallback, this); mag_sub_ = node_handle_->Subscribe("~/" + model_->GetName() + mag_sub_topic_, &GazeboMavlinkInterface::MagnetometerCallback, this); - airspeed_sub_ = node_handle_->Subscribe("~/" + model_->GetName() + airspeed_sub_topic_, &GazeboMavlinkInterface::AirspeedCallback, this); baro_sub_ = node_handle_->Subscribe("~/" + model_->GetName() + baro_sub_topic_, &GazeboMavlinkInterface::BarometerCallback, this); wind_sub_ = node_handle_->Subscribe("~/" + wind_sub_topic_, &GazeboMavlinkInterface::WindVelocityCallback, this); @@ -449,6 +446,7 @@ void GazeboMavlinkInterface::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf CreateSensorSubscription(&GazeboMavlinkInterface::LidarCallback, this, joints, nested_model, kDefaultLidarModelNaming); CreateSensorSubscription(&GazeboMavlinkInterface::SonarCallback, this, joints, nested_model, kDefaultSonarModelNaming); CreateSensorSubscription(&GazeboMavlinkInterface::GpsCallback, this, joints, nested_model, kDefaultGPSModelNaming); + CreateSensorSubscription(&GazeboMavlinkInterface::AirspeedCallback, this, joints, nested_model, kDefaultAirspeedModelJointNaming); // Publish gazebo's motor_speed message motor_velocity_reference_pub_ = node_handle_->Advertise("~/" + model_->GetName() + motor_velocity_reference_pub_topic_, 1); @@ -682,11 +680,10 @@ void GazeboMavlinkInterface::SendSensorMessages() } } - mavlink_hil_sensor_t sensor_msg; #if GAZEBO_MAJOR_VERSION >= 9 - sensor_msg.time_usec = std::round(world_->SimTime().Double() * 1e6); + int time_usec = std::round(world_->SimTime().Double() * 1e6); #else - sensor_msg.time_usec = std::round(world_->GetSimTime().Double() * 1e6); + int time_usec = std::round(world_->GetSimTime().Double() * 1e6); #endif // send always accel and gyro data (not dependent of the bitmask) @@ -702,52 +699,12 @@ void GazeboMavlinkInterface::SendSensorMessages() last_imu_message_.angular_velocity().y(), last_imu_message_.angular_velocity().z())); - sensor_msg.xacc = accel_b.X(); - sensor_msg.yacc = accel_b.Y(); - sensor_msg.zacc = accel_b.Z(); - sensor_msg.xgyro = gyro_b.X(); - sensor_msg.ygyro = gyro_b.Y(); - sensor_msg.zgyro = gyro_b.Z(); - - sensor_msg.fields_updated = SensorSource::ACCEL | SensorSource::GYRO; - - // send only mag data - if (mag_updated_) { - ignition::math::Quaterniond q_body_to_world = q_ENU_to_NED * q_gr * q_FLU_to_FRD.Inverse(); - - ignition::math::Vector3d mag_b = q_body_to_world.RotateVectorReverse(mag_n_); - - sensor_msg.xmag = mag_b.X(); - sensor_msg.ymag = mag_b.Y(); - sensor_msg.zmag = mag_b.Z(); - sensor_msg.fields_updated = sensor_msg.fields_updated | SensorSource::MAG; - - mag_updated_ = false; - } - - // send only baro data - if (baro_updated_) { - sensor_msg.temperature = temperature_; - sensor_msg.abs_pressure = abs_pressure_; - sensor_msg.pressure_alt = pressure_alt_; - sensor_msg.fields_updated = sensor_msg.fields_updated | SensorSource::BARO; - - baro_updated_ = false; - } - - // send only diff pressure data - if (diff_press_updated_) { - sensor_msg.diff_pressure = diff_pressure_; - sensor_msg.fields_updated = sensor_msg.fields_updated | SensorSource::DIFF_PRESS; + SensorData::Imu imu_data; + imu_data.accel_b = Eigen::Vector3d(accel_b.X(), accel_b.Y(), accel_b.Z()); + imu_data.gyro_b = Eigen::Vector3d(gyro_b.X(), gyro_b.Y(), gyro_b.Z()); + mavlink_interface_->UpdateIMU(imu_data); - diff_press_updated_ = false; - } - - if (!hil_mode_ || (hil_mode_ && !hil_state_level_)) { - mavlink_message_t msg; - mavlink_msg_hil_sensor_encode_chan(1, 200, MAVLINK_COMM_0, &msg, &sensor_msg); - mavlink_interface_->send_mavlink_message(&msg); - } + mavlink_interface_->SendSensorMessages(time_usec); } void GazeboMavlinkInterface::SendGroundTruth() @@ -822,32 +779,26 @@ void GazeboMavlinkInterface::SendGroundTruth() } void GazeboMavlinkInterface::GpsCallback(GpsPtr& gps_msg, const int& id) { - // fill HIL GPS Mavlink msg - mavlink_hil_gps_t hil_gps_msg; - hil_gps_msg.time_usec = gps_msg->time_utc_usec(); - hil_gps_msg.fix_type = 3; - hil_gps_msg.lat = gps_msg->latitude_deg() * 1e7; - hil_gps_msg.lon = gps_msg->longitude_deg() * 1e7; - hil_gps_msg.alt = gps_msg->altitude() * 1000.0; - hil_gps_msg.eph = gps_msg->eph() * 100.0; - hil_gps_msg.epv = gps_msg->epv() * 100.0; - hil_gps_msg.vel = gps_msg->velocity() * 100.0; - hil_gps_msg.vn = gps_msg->velocity_north() * 100.0; - hil_gps_msg.ve = gps_msg->velocity_east() * 100.0; - hil_gps_msg.vd = -gps_msg->velocity_up() * 100.0; + SensorData::Gps gps_data; + gps_data.time_utc_usec = gps_msg->time_utc_usec(); + gps_data.fix_type = 3; + gps_data.latitude_deg = gps_msg->latitude_deg() * 1e7; + gps_data.longitude_deg = gps_msg->longitude_deg() * 1e7; + gps_data.altitude = gps_msg->altitude() * 1000.0; + gps_data.eph = gps_msg->eph() * 100.0; + gps_data.epv = gps_msg->epv() * 100.0; + gps_data.velocity = gps_msg->velocity() * 100.0; + gps_data.velocity_north = gps_msg->velocity_north() * 100.0; + gps_data.velocity_east = gps_msg->velocity_east() * 100.0; + gps_data.velocity_down = -gps_msg->velocity_up() * 100.0; // MAVLINK_HIL_GPS_T CoG is [0, 360]. math::Angle::Normalize() is [-pi, pi]. ignition::math::Angle cog(atan2(gps_msg->velocity_east(), gps_msg->velocity_north())); cog.Normalize(); - hil_gps_msg.cog = static_cast(GetDegrees360(cog) * 100.0); - hil_gps_msg.satellites_visible = 10; - hil_gps_msg.id = id; + gps_data.cog = static_cast(GetDegrees360(cog) * 100.0); + gps_data.satellites_visible = 10; + gps_data.id = id; - // send HIL_GPS Mavlink msg - if (!hil_mode_ || (hil_mode_ && !hil_state_level_)) { - mavlink_message_t msg; - mavlink_msg_hil_gps_encode_chan(1, 200, MAVLINK_COMM_0, &msg, &hil_gps_msg); - mavlink_interface_->send_mavlink_message(&msg); - } + mavlink_interface_->SendGpsMessages(gps_data); } void GazeboMavlinkInterface::GroundtruthCallback(GtPtr& groundtruth_msg) { @@ -1115,27 +1066,24 @@ void GazeboMavlinkInterface::VisionCallback(OdomPtr& odom_message) { } void GazeboMavlinkInterface::MagnetometerCallback(MagnetometerPtr& mag_msg) { - // update groundtruth magnetometer NED components - mag_n_ = ignition::math::Vector3d( - mag_msg->magnetic_field().x(), - mag_msg->magnetic_field().y(), - mag_msg->magnetic_field().z()); - - mag_updated_ = true; + SensorData::Magnetometer mag_data; + mag_data.mag_b = Eigen::Vector3d(mag_msg->magnetic_field().x(), + mag_msg->magnetic_field().y(), mag_msg->magnetic_field().z()); + mavlink_interface_->UpdateMag(mag_data); } -void GazeboMavlinkInterface::AirspeedCallback(AirspeedPtr& airspeed_msg) { - diff_pressure_ = airspeed_msg->diff_pressure(); - - diff_press_updated_ = true; +void GazeboMavlinkInterface::AirspeedCallback(AirspeedPtr& airspeed_msg, const int& id) { + SensorData::Airspeed airspeed_data; + airspeed_data.diff_pressure = airspeed_msg->diff_pressure(); + mavlink_interface_->UpdateAirspeed(airspeed_data, id); } void GazeboMavlinkInterface::BarometerCallback(BarometerPtr& baro_msg) { - temperature_ = baro_msg->temperature(); - pressure_alt_ = baro_msg->pressure_altitude(); - abs_pressure_ = baro_msg->absolute_pressure(); - - baro_updated_ = true; + SensorData::Barometer baro_data; + baro_data.temperature = baro_msg->temperature(); + baro_data.abs_pressure = baro_msg->absolute_pressure(); + baro_data.pressure_alt = baro_msg->pressure_altitude(); + mavlink_interface_->UpdateBarometer(baro_data); } void GazeboMavlinkInterface::WindVelocityCallback(WindPtr& msg) { diff --git a/src/mavlink_interface.cpp b/src/mavlink_interface.cpp index f6a71d8391..8a1630a0f8 100644 --- a/src/mavlink_interface.cpp +++ b/src/mavlink_interface.cpp @@ -189,6 +189,158 @@ void MavlinkInterface::Load() fds_[CONNECTION_FD].events = POLLIN | POLLOUT; // read/write } } + // hil_data_.resize(1); +} + +void MavlinkInterface::SendSensorMessages(const int &time_usec) { + for (auto& data : hil_data_) { + if (data.baro_updated | data.diff_press_updated | data.mag_updated | data.imu_updated) { + SendSensorMessages(time_usec, data); + } + } +} + +void MavlinkInterface::SendSensorMessages(const int &time_usec, HILData &hil_data) { + const std::lock_guard lock(sensor_msg_mutex_); + + HILData* data = &hil_data; + mavlink_hil_sensor_t sensor_msg; + sensor_msg.id = data->id; + sensor_msg.time_usec = time_usec; + if (data->imu_updated) { + sensor_msg.xacc = data->accel_b[0]; + sensor_msg.yacc = data->accel_b[1]; + sensor_msg.zacc = data->accel_b[2]; + sensor_msg.xgyro = data->gyro_b[0]; + sensor_msg.ygyro = data->gyro_b[1]; + sensor_msg.zgyro = data->gyro_b[2]; + // std::cout <gyro_b[2] << std::endl; + + sensor_msg.fields_updated = (uint16_t)SensorSource::ACCEL | (uint16_t)SensorSource::GYRO; + + data->imu_updated = false; + } + + // send only mag data + if (data->mag_updated) { + sensor_msg.xmag = data->mag_b[0]; + sensor_msg.ymag = data->mag_b[1]; + sensor_msg.zmag = data->mag_b[2]; + sensor_msg.fields_updated = sensor_msg.fields_updated | (uint16_t)SensorSource::MAG; + + data->mag_updated = false; + } + + // send only baro data + if (data->baro_updated) { + sensor_msg.temperature = data->temperature; + sensor_msg.abs_pressure = data->abs_pressure; + sensor_msg.pressure_alt = data->pressure_alt; + sensor_msg.fields_updated = sensor_msg.fields_updated | (uint16_t)SensorSource::BARO; + + data->baro_updated = false; + } + + // send only diff pressure data + if (data->diff_press_updated) { + sensor_msg.diff_pressure = data->diff_pressure; + sensor_msg.fields_updated = sensor_msg.fields_updated | (uint16_t)SensorSource::DIFF_PRESS; + + data->diff_press_updated = false; + } + + if (!hil_mode_ || (hil_mode_ && !hil_state_level_)) { + mavlink_message_t msg; + mavlink_msg_hil_sensor_encode_chan(1, 200, MAVLINK_COMM_0, &msg, &sensor_msg); + send_mavlink_message(&msg); + } +} + +void MavlinkInterface::SendGpsMessages(const SensorData::Gps &data) { + // fill HIL GPS Mavlink msg + mavlink_hil_gps_t hil_gps_msg; + hil_gps_msg.time_usec = data.time_utc_usec; + hil_gps_msg.fix_type = data.fix_type; + hil_gps_msg.lat = data.latitude_deg; + hil_gps_msg.lon = data.longitude_deg; + hil_gps_msg.alt = data.altitude; + hil_gps_msg.eph = data.eph; + hil_gps_msg.epv = data.epv; + hil_gps_msg.vel = data.velocity; + hil_gps_msg.vn = data.velocity_north; + hil_gps_msg.ve = data.velocity_east; + hil_gps_msg.vd = data.velocity_down; + hil_gps_msg.cog = data.cog; + hil_gps_msg.satellites_visible = data.satellites_visible; + hil_gps_msg.id = data.id; + + // send HIL_GPS Mavlink msg + if (!hil_mode_ || (hil_mode_ && !hil_state_level_)) { + mavlink_message_t msg; + mavlink_msg_hil_gps_encode_chan(1, 200, MAVLINK_COMM_0, &msg, &hil_gps_msg); + send_mavlink_message(&msg); + } +} + +void MavlinkInterface::UpdateBarometer(const SensorData::Barometer &data, int id) { + const std::lock_guard lock(sensor_msg_mutex_); + for (auto& instance : hil_data_) { + if (instance.id == id) { + instance.temperature = data.temperature; + instance.abs_pressure = data.abs_pressure; + instance.pressure_alt = data.pressure_alt; + instance.baro_updated = true; + return; + } + } + //Register new HIL instance if we have never seen the id + RegisterNewHILSensorInstance(id); +} + +void MavlinkInterface::UpdateAirspeed(const SensorData::Airspeed &data, int id) { + const std::lock_guard lock(sensor_msg_mutex_); + for (auto& instance : hil_data_) { + if (instance.id == id) { + instance.diff_pressure = data.diff_pressure; + instance.diff_press_updated = true; + return; + } + } + //Register new HIL instance if we have never seen the id + RegisterNewHILSensorInstance(id); +} + +void MavlinkInterface::UpdateIMU(const SensorData::Imu &data, int id) { + const std::lock_guard lock(sensor_msg_mutex_); + for (auto& instance : hil_data_) { + if (instance.id == id) { + instance.accel_b = data.accel_b; + instance.gyro_b = data.gyro_b; + instance.imu_updated = true; + return; + } + } + //Register new HIL instance if we have never seen the id + RegisterNewHILSensorInstance(id); +} + +void MavlinkInterface::UpdateMag(const SensorData::Magnetometer &data, int id) { + const std::lock_guard lock(sensor_msg_mutex_); + for (auto& instance : hil_data_) { + if (instance.id == id) { + instance.mag_b = data.mag_b; + instance.mag_updated = true; + return; + } + } + //Register new HIL instance if we have never seen the id + RegisterNewHILSensorInstance(id); +} + +void MavlinkInterface::RegisterNewHILSensorInstance(int id) { + HILData new_instance; + new_instance.id = id; + hil_data_.push_back(new_instance); } void MavlinkInterface::pollForMAVLinkMessages()