From 5f318e31a808d343fd256f11b91090f7c572eedd Mon Sep 17 00:00:00 2001 From: Henry Kotze Date: Mon, 31 Jan 2022 12:22:59 +0200 Subject: [PATCH 1/9] FT Technologies Wind Sensor Plugin Added - Wind Sensor added which outputs wind direction and wind speed - Since the wind sensor is mounted ontop of the aircraft it measures the combined affect of wind and the velocity of the aircraft. - PX4 received the measurements produced by the Sensor Model --- CMakeLists.txt | 10 +- include/gazebo_mavlink_interface.h | 4 + include/gazebo_windsensor_plugin.h | 115 ++++++++++++++++++ include/mavlink_interface.h | 10 ++ models/windsensor/model.config | 15 +++ models/windsensor/windsensor.sdf | 30 +++++ msgs/WindSensor.proto | 9 ++ src/gazebo_mavlink_interface.cpp | 8 ++ src/gazebo_windsensor_plugin.cpp | 182 +++++++++++++++++++++++++++++ src/mavlink_interface.cpp | 23 ++++ 10 files changed, 404 insertions(+), 2 deletions(-) create mode 100644 include/gazebo_windsensor_plugin.h create mode 100644 models/windsensor/model.config create mode 100644 models/windsensor/windsensor.sdf create mode 100644 msgs/WindSensor.proto create mode 100644 src/gazebo_windsensor_plugin.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index be7fda8008..9b8b6e4f16 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -330,6 +330,8 @@ set(sensor_msgs msgs/OpticalFlow.proto msgs/MagneticField.proto msgs/Pressure.proto + # Cloudline Additions + msgs/WindSensor.proto ) PROTOBUF_GENERATE_CPP(MAV_PROTO_SRCS MAV_PROTO_HDRS ${mav_msgs}) @@ -374,8 +376,10 @@ add_library(gazebo_barometer_plugin SHARED src/gazebo_barometer_plugin.cpp) add_library(gazebo_catapult_plugin SHARED src/gazebo_catapult_plugin.cpp) add_library(gazebo_usv_dynamics_plugin SHARED src/gazebo_usv_dynamics_plugin.cpp) add_library(gazebo_parachute_plugin SHARED src/gazebo_parachute_plugin.cpp) -add_library(gazebo_airship_dynamics_plugin SHARED src/gazebo_airship_dynamics_plugin.cpp) add_library(gazebo_drop_plugin SHARED src/gazebo_drop_plugin.cpp) +# Cloudline Additions +add_library(gazebo_windsensor_plugin SHARED src/gazebo_windsensor_plugin.cpp) +add_library(gazebo_airship_dynamics_plugin SHARED src/gazebo_airship_dynamics_plugin.cpp) set(plugins gazebo_airspeed_plugin @@ -401,8 +405,10 @@ set(plugins gazebo_catapult_plugin gazebo_usv_dynamics_plugin gazebo_parachute_plugin - gazebo_airship_dynamics_plugin gazebo_drop_plugin + # Cloudline Additions + gazebo_windsensor_plugin + gazebo_airship_dynamics_plugin ) foreach(plugin ${plugins}) diff --git a/include/gazebo_mavlink_interface.h b/include/gazebo_mavlink_interface.h index aec4a4d80d..d77dc46a97 100644 --- a/include/gazebo_mavlink_interface.h +++ b/include/gazebo_mavlink_interface.h @@ -57,6 +57,7 @@ #include #include #include +#include #include #include #include @@ -78,6 +79,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)"); +static const std::regex kDefaultWindSensorModelJointNaming(".*(windsensor)(.*_joint)"); static const std::regex kDefaultImuModelJointNaming(".*(imu)(\\d*_joint)"); static const std::regex kDefaultMagModelJointNaming(".*(mag)(\\d*_joint)"); @@ -86,6 +88,7 @@ namespace gazebo { typedef const boost::shared_ptr CommandMotorSpeedPtr; typedef const boost::shared_ptr OdomPtr; typedef const boost::shared_ptr AirspeedPtr; +typedef const boost::shared_ptr WindSensorPtr; typedef const boost::shared_ptr GtPtr; typedef const boost::shared_ptr ImuPtr; typedef const boost::shared_ptr IRLockPtr; @@ -179,6 +182,7 @@ class GazeboMavlinkInterface : public ModelPlugin { 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 WindSensorCallback(WindSensorPtr& windsensor_msg, const int& id); void OpticalFlowCallback(OpticalFlowPtr& opticalFlow_msg); void IRLockCallback(IRLockPtr& irlock_msg); void VisionCallback(OdomPtr& odom_msg); diff --git a/include/gazebo_windsensor_plugin.h b/include/gazebo_windsensor_plugin.h new file mode 100644 index 0000000000..6e63c26ee7 --- /dev/null +++ b/include/gazebo_windsensor_plugin.h @@ -0,0 +1,115 @@ +/**************************************************************************** + * + * Copyright (c) 2020 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ +/** + * @brief WindSensor Plugin + * + * This plugin publishes WindSensor sensor data + * + * @author Henry Kotze + */ + +#ifndef _GAZEBO_WINDSENSOR_PLUGIN_HH_ +#define _GAZEBO_WINDSENSOR_PLUGIN_HH_ + +#include +#include +#include +#include +#include + +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include +#include + +namespace gazebo +{ + +typedef const boost::shared_ptr WindPtr; + +class GAZEBO_VISIBLE WindSensorPlugin : public SensorPlugin +{ +public: + WindSensorPlugin(); + virtual ~WindSensorPlugin(); + +protected: + 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 windsensor_pub_; + event::ConnectionPtr updateSensorConnection_; + event::ConnectionPtr updateConnection_; + + // linear velocity + ignition::math::Vector3d vel_a_; + // angular velocity + ignition::math::Vector3d ang_a_; + + common::Time last_time_; + std::string namespace_; + std::string link_name_; + std::string model_name_; + std::string windsensor_topic_; + + ignition::math::Vector3d wind_; + float wind_direction_; + float wind_speed_; + +}; // class GAZEBO_VISIBLE WindSensorPlugin +} // namespace gazebo +#endif // _GAZEBO_WINDSENSOR_PLUGIN_HH_ diff --git a/include/mavlink_interface.h b/include/mavlink_interface.h index 40223231f8..338ed2c329 100644 --- a/include/mavlink_interface.h +++ b/include/mavlink_interface.h @@ -82,6 +82,7 @@ enum class SensorSource { MAG = 0b111000000, BARO = 0b1101000000000, DIFF_PRESS = 0b10000000000, + WIND_SENSOR = 0b10000000011 }; namespace SensorData { @@ -106,6 +107,11 @@ namespace SensorData { double diff_pressure; }; + struct WindSensor { + double wind_speed; + double wind_direction; + }; + struct Gps { uint64_t time_utc_usec; int fix_type; @@ -129,12 +135,15 @@ struct HILData { int id=-1; bool baro_updated{false}; bool diff_press_updated{false}; + bool windsensor_updated{false}; bool mag_updated{false}; bool imu_updated{false}; double temperature; double pressure_alt; double abs_pressure; double diff_pressure; + double wind_direction; + double wind_speed; Eigen::Vector3d mag_b; Eigen::Vector3d accel_b; Eigen::Vector3d gyro_b; @@ -158,6 +167,7 @@ class MavlinkInterface { 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 UpdateWindSensor(const SensorData::WindSensor &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(); diff --git a/models/windsensor/model.config b/models/windsensor/model.config new file mode 100644 index 0000000000..6c2628d012 --- /dev/null +++ b/models/windsensor/model.config @@ -0,0 +1,15 @@ + + + windsensor + 1.0 + windsensor.sdf + + + Henry Kotze + henry@flycloudline.com + + + + FT Technologies ultra sonic wind sensor model + + diff --git a/models/windsensor/windsensor.sdf b/models/windsensor/windsensor.sdf new file mode 100644 index 0000000000..7d8a64e00d --- /dev/null +++ b/models/windsensor/windsensor.sdf @@ -0,0 +1,30 @@ + + + + + 0 0 0 0 0 0 + + + + 0.01 + 0.1 + + + + + + + + 0 0 0 0 0 0 + 5.0 + true + false + + + + + + + diff --git a/msgs/WindSensor.proto b/msgs/WindSensor.proto new file mode 100644 index 0000000000..6d8ca9dccd --- /dev/null +++ b/msgs/WindSensor.proto @@ -0,0 +1,9 @@ +syntax = "proto2"; +package sensor_msgs.msgs; + +message WindSensor +{ + required int64 time_usec = 1; + required double wind_speed = 2; + required double wind_direction = 3; +} diff --git a/src/gazebo_mavlink_interface.cpp b/src/gazebo_mavlink_interface.cpp index 42d34a7902..add32f8720 100644 --- a/src/gazebo_mavlink_interface.cpp +++ b/src/gazebo_mavlink_interface.cpp @@ -443,6 +443,7 @@ void GazeboMavlinkInterface::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf CreateSensorSubscription(&GazeboMavlinkInterface::SonarCallback, this, joints, nested_model, kDefaultSonarModelNaming); CreateSensorSubscription(&GazeboMavlinkInterface::GpsCallback, this, joints, nested_model, kDefaultGPSModelNaming); CreateSensorSubscription(&GazeboMavlinkInterface::AirspeedCallback, this, joints, nested_model, kDefaultAirspeedModelJointNaming); + CreateSensorSubscription(&GazeboMavlinkInterface::WindSensorCallback, this, joints, nested_model, kDefaultWindSensorModelJointNaming); CreateSensorSubscription(&GazeboMavlinkInterface::ImuCallback, this, joints, nested_model, kDefaultImuModelJointNaming); CreateSensorSubscription(&GazeboMavlinkInterface::MagnetometerCallback, this, joints, nested_model, kDefaultMagModelJointNaming); @@ -1092,6 +1093,13 @@ void GazeboMavlinkInterface::AirspeedCallback(AirspeedPtr& airspeed_msg, const i mavlink_interface_->UpdateAirspeed(airspeed_data, id); } +void GazeboMavlinkInterface::WindSensorCallback(WindSensorPtr& windsensor_msg, const int& id) { + SensorData::WindSensor windsensor_data; + windsensor_data.wind_speed = windsensor_msg->wind_speed(); + windsensor_data.wind_direction = windsensor_msg->wind_direction(); + mavlink_interface_->UpdateWindSensor(windsensor_data, id); +} + void GazeboMavlinkInterface::BarometerCallback(BarometerPtr& baro_msg) { SensorData::Barometer baro_data; baro_data.temperature = baro_msg->temperature(); diff --git a/src/gazebo_windsensor_plugin.cpp b/src/gazebo_windsensor_plugin.cpp new file mode 100644 index 0000000000..439a3c3f0d --- /dev/null +++ b/src/gazebo_windsensor_plugin.cpp @@ -0,0 +1,182 @@ +/**************************************************************************** + * + * Copyright (c) 2020 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ +/** + * @brief WindSensor Plugin + * + * This plugin publishes Windsensor + * + * @author Henry Kotze + */ + +#include +#include + +namespace gazebo { +GZ_REGISTER_SENSOR_PLUGIN(WindSensorPlugin) + +WindSensorPlugin::WindSensorPlugin() : SensorPlugin(), + wind_direction_(0.0), + wind_speed_(0.0f) +{ } + +WindSensorPlugin::~WindSensorPlugin() +{ + if (updateConnection_) + updateConnection_->~Connection(); +} + +void WindSensorPlugin::Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf) +{ + // Get then name of the parent sensor + this->parentSensor_ = std::dynamic_pointer_cast(_parent); + if (!parentSensor_) + gzthrow("WindSensorPlugin requires a Wind 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 windsensor topic name + if(_sdf->HasElement("topic")) { + windsensor_topic_ = _sdf->GetElement("topic")->Get(); + } else { + // if not set by parameter, get the topic name from the model name + windsensor_topic_ = parentSensorModelName; + gzwarn << "[gazebo_windsensor_plugin]: " + names_splitted.front() + "::" + names_splitted.rbegin()[1] + + " using windsensor topic \"" << parentSensorModelName << "\"\n"; + } + + + // Store the pointer to the model. + world_ = physics::get_world(parentSensor_->WorldName()); + + if (_sdf->HasElement("robotNamespace")) { + namespace_ = _sdf->GetElement("robotNamespace")->Get(); + } else { + gzerr << "[gazebo_windsensor_plugin] Please specify a robotNamespace.\n"; + } + + this->node_handle_ = transport::NodePtr(new transport::Node()); + node_handle_->Init(namespace_); + + this->parentSensor_->SetUpdateRate(10.0); + this->parentSensor_->SetActive(false); + updateSensorConnection_ = parentSensor_->ConnectUpdated(boost::bind(&WindSensorPlugin::OnSensorUpdate, this)); + this->parentSensor_->SetActive(true); + + // Listen to the update event. This event is broadcast every + // simulation iteration. + updateConnection_ = event::Events::ConnectWorldUpdateBegin(boost::bind(&WindSensorPlugin::OnUpdate, this, _1)); + + windsensor_pub_ = node_handle_->Advertise("~/" + model_name_ + "/link/" + windsensor_topic_, 10); + wind_sub_ = node_handle_->Subscribe("~/world_wind", &WindSensorPlugin::WindVelocityCallback, this); + + +} + +void WindSensorPlugin::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 + +#if GAZEBO_MAJOR_VERSION >= 9 + ignition::math::Pose3d T_W_I = link_->WorldPose(); +#else + ignition::math::Pose3d T_W_I = ignitionFromGazeboMath(link_->GetWorldPose()); +#endif + ignition::math::Quaterniond C_W_I = T_W_I.Rot(); + +#if GAZEBO_MAJOR_VERSION >= 9 + vel_a_ = link_->RelativeLinearVel() - C_W_I.RotateVectorReverse(wind_); + ang_a_ = link_->RelativeLinearVel() - C_W_I.RotateVectorReverse(wind_); +#else + vel_a_ = ignitionFromGazeboMath(link_->GetRelativeLinearVel()) - C_W_I.RotateVectorReverse(wind_); + ang_a_ = ignitionFromGazeboMath(link_->GetRelativeAngularVel()); +#endif + + last_time_ = current_time; + + wind_direction_ = atan2(vel_a_.X(), vel_a_.Y()); + wind_speed_ = vel_a_.Length(); +} + +void WindSensorPlugin::OnSensorUpdate() { + + + sensor_msgs::msgs::WindSensor windsensor_msg; + windsensor_msg.set_time_usec(last_time_.Double() * 1e6); + windsensor_msg.set_wind_speed(wind_speed_); + windsensor_msg.set_wind_direction(wind_direction_); + windsensor_pub_->Publish(windsensor_msg); + +} + +void WindSensorPlugin::WindVelocityCallback(WindPtr& wind) { + + // Get world wind velocity. + ignition::math::Vector3d wind_world = ignition::math::Vector3d(wind->velocity().x(), wind->velocity().y(), wind->velocity().z()); + + // Rotate to body frame + #if GAZEBO_MAJOR_VERSION >= 9 + wind_ = link_->WorldPose().Rot().Inverse().RotateVector(wind_world); + #else + wind_ = ignitionFromGazeboMath(link_->GetWorldPose()).Rot().Inverse().RotateVector(wind_world); + #endif + +} +} // namespace gazebo diff --git a/src/mavlink_interface.cpp b/src/mavlink_interface.cpp index 43bab08b03..6d928f4ac6 100644 --- a/src/mavlink_interface.cpp +++ b/src/mavlink_interface.cpp @@ -274,6 +274,15 @@ void MavlinkInterface::SendSensorMessages(uint64_t time_usec, HILData &hil_data) data->diff_press_updated = false; } + // send only windsensor data + if (data->windsensor_updated) { + sensor_msg.wind_direction = data->wind_direction; + sensor_msg.wind_speed = data->wind_speed; + sensor_msg.fields_updated = sensor_msg.fields_updated | (uint16_t)SensorSource::WIND_SENSOR; + + data->windsensor_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); @@ -335,6 +344,20 @@ void MavlinkInterface::UpdateAirspeed(const SensorData::Airspeed &data, int id) RegisterNewHILSensorInstance(id); } +void MavlinkInterface::UpdateWindSensor(const SensorData::WindSensor &data, int id) { + const std::lock_guard lock(sensor_msg_mutex_); + for (auto& instance : hil_data_) { + if (instance.id == id) { + instance.wind_direction = data.wind_direction; + instance.wind_speed = data.wind_speed; + instance.windsensor_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_) { From 5e27a9e194334094a6ba22834c59b9cd2d719554 Mon Sep 17 00:00:00 2001 From: Henry Kotze Date: Thu, 18 Aug 2022 14:47:02 +0200 Subject: [PATCH 2/9] Windsensor bitmask correction in HIL_SENSOR - Windsensor bitmask in HIL_SENSOR message corrected --- include/mavlink_interface.h | 2 +- src/gazebo_windsensor_plugin.cpp | 5 +++++ src/mavlink_interface.cpp | 2 +- 3 files changed, 7 insertions(+), 2 deletions(-) diff --git a/include/mavlink_interface.h b/include/mavlink_interface.h index 338ed2c329..5210a269f2 100644 --- a/include/mavlink_interface.h +++ b/include/mavlink_interface.h @@ -82,7 +82,7 @@ enum class SensorSource { MAG = 0b111000000, BARO = 0b1101000000000, DIFF_PRESS = 0b10000000000, - WIND_SENSOR = 0b10000000011 + WIND_SENSOR = 0b10000000000000 }; namespace SensorData { diff --git a/src/gazebo_windsensor_plugin.cpp b/src/gazebo_windsensor_plugin.cpp index 439a3c3f0d..a00a1d40c4 100644 --- a/src/gazebo_windsensor_plugin.cpp +++ b/src/gazebo_windsensor_plugin.cpp @@ -113,6 +113,11 @@ void WindSensorPlugin::Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf) wind_sub_ = node_handle_->Subscribe("~/world_wind", &WindSensorPlugin::WindVelocityCallback, this); + // Set initial wind as zero + wind_.X() = 0; + wind_.Y() = 0; + wind_.Z() = 0; + } void WindSensorPlugin::OnUpdate(const common::UpdateInfo&){ diff --git a/src/mavlink_interface.cpp b/src/mavlink_interface.cpp index 6d928f4ac6..e380af4c54 100644 --- a/src/mavlink_interface.cpp +++ b/src/mavlink_interface.cpp @@ -204,7 +204,7 @@ void MavlinkInterface::Load() void MavlinkInterface::SendSensorMessages(uint64_t time_usec) { for (auto& data : hil_data_) { - if (data.baro_updated | data.diff_press_updated | data.mag_updated | data.imu_updated) { + if (data.baro_updated | data.diff_press_updated | data.mag_updated | data.imu_updated | data.windsensor_updated) { SendSensorMessages(time_usec, data); } } From 565c9dc8feaab4e678f9c2582ec0ff0e37b7123b Mon Sep 17 00:00:00 2001 From: Henry Kotze Date: Fri, 2 Sep 2022 16:01:58 +0200 Subject: [PATCH 3/9] Noise added to wind sensor model --- .vscode/settings.json | 3 +++ CMakeLists.txt | 3 --- include/gazebo_windsensor_plugin.h | 4 ++++ src/gazebo_windsensor_plugin.cpp | 16 +++++++++++----- 4 files changed, 18 insertions(+), 8 deletions(-) create mode 100644 .vscode/settings.json diff --git a/.vscode/settings.json b/.vscode/settings.json new file mode 100644 index 0000000000..22f60ba451 --- /dev/null +++ b/.vscode/settings.json @@ -0,0 +1,3 @@ +{ + "ros.distro": "foxy" +} \ No newline at end of file diff --git a/CMakeLists.txt b/CMakeLists.txt index 9b8b6e4f16..6245e8bba5 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -330,7 +330,6 @@ set(sensor_msgs msgs/OpticalFlow.proto msgs/MagneticField.proto msgs/Pressure.proto - # Cloudline Additions msgs/WindSensor.proto ) @@ -377,7 +376,6 @@ add_library(gazebo_catapult_plugin SHARED src/gazebo_catapult_plugin.cpp) add_library(gazebo_usv_dynamics_plugin SHARED src/gazebo_usv_dynamics_plugin.cpp) add_library(gazebo_parachute_plugin SHARED src/gazebo_parachute_plugin.cpp) add_library(gazebo_drop_plugin SHARED src/gazebo_drop_plugin.cpp) -# Cloudline Additions add_library(gazebo_windsensor_plugin SHARED src/gazebo_windsensor_plugin.cpp) add_library(gazebo_airship_dynamics_plugin SHARED src/gazebo_airship_dynamics_plugin.cpp) @@ -406,7 +404,6 @@ set(plugins gazebo_usv_dynamics_plugin gazebo_parachute_plugin gazebo_drop_plugin - # Cloudline Additions gazebo_windsensor_plugin gazebo_airship_dynamics_plugin ) diff --git a/include/gazebo_windsensor_plugin.h b/include/gazebo_windsensor_plugin.h index 6e63c26ee7..6933af7b6b 100644 --- a/include/gazebo_windsensor_plugin.h +++ b/include/gazebo_windsensor_plugin.h @@ -106,6 +106,10 @@ class GAZEBO_VISIBLE WindSensorPlugin : public SensorPlugin std::string model_name_; std::string windsensor_topic_; + std::normal_distribution gauss_dir_; + std::normal_distribution gauss_speed_; + std::default_random_engine generator_; + ignition::math::Vector3d wind_; float wind_direction_; float wind_speed_; diff --git a/src/gazebo_windsensor_plugin.cpp b/src/gazebo_windsensor_plugin.cpp index a00a1d40c4..3e8fab2bc6 100644 --- a/src/gazebo_windsensor_plugin.cpp +++ b/src/gazebo_windsensor_plugin.cpp @@ -46,7 +46,9 @@ GZ_REGISTER_SENSOR_PLUGIN(WindSensorPlugin) WindSensorPlugin::WindSensorPlugin() : SensorPlugin(), wind_direction_(0.0), - wind_speed_(0.0f) + wind_speed_(0.0f), + gauss_dir_(0.0, 4*(M_PI/180.0)), + gauss_speed_(0.0, 0.3) { } WindSensorPlugin::~WindSensorPlugin() @@ -112,7 +114,6 @@ void WindSensorPlugin::Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf) windsensor_pub_ = node_handle_->Advertise("~/" + model_name_ + "/link/" + windsensor_topic_, 10); wind_sub_ = node_handle_->Subscribe("~/world_wind", &WindSensorPlugin::WindVelocityCallback, this); - // Set initial wind as zero wind_.X() = 0; wind_.Y() = 0; @@ -155,9 +156,14 @@ void WindSensorPlugin::OnUpdate(const common::UpdateInfo&){ #endif last_time_ = current_time; - - wind_direction_ = atan2(vel_a_.X(), vel_a_.Y()); - wind_speed_ = vel_a_.Length(); + wind_direction_ = atan2f(vel_a_.Y(),vel_a_.X()) * (180.0/M_PI); + // resolution of 1 degree + wind_direction_ = round(wind_direction_) * (M_PI/180.0) + gauss_dir_(generator_); + // wind sensor cannot measure wind in the z-direction + vel_a_.Z() = 0; + wind_speed_ = vel_a_.Length()*10.0; + // resolution of 0.1m/s + wind_speed_ = round(wind_speed_)/10.0 + gauss_speed_(generator_); } void WindSensorPlugin::OnSensorUpdate() { From d1fa5a4c941d9f9a629acf51878a838c68bbaab6 Mon Sep 17 00:00:00 2001 From: Henry Kotze Date: Sat, 10 Sep 2022 12:27:01 +0200 Subject: [PATCH 4/9] Added .vscode to gitignore --- .gitignore | 1 + .vscode/settings.json | 3 --- 2 files changed, 1 insertion(+), 3 deletions(-) delete mode 100644 .vscode/settings.json diff --git a/.gitignore b/.gitignore index f1dd958cd6..cff3424568 100644 --- a/.gitignore +++ b/.gitignore @@ -1,5 +1,6 @@ Build/ build/ +.vscode/ scripts/schemas .DS_Store *~ diff --git a/.vscode/settings.json b/.vscode/settings.json deleted file mode 100644 index 22f60ba451..0000000000 --- a/.vscode/settings.json +++ /dev/null @@ -1,3 +0,0 @@ -{ - "ros.distro": "foxy" -} \ No newline at end of file From a692768779c92887ddf909a003486cf0367b6233 Mon Sep 17 00:00:00 2001 From: Henry Kotze Date: Sat, 10 Sep 2022 20:16:08 +0200 Subject: [PATCH 5/9] Uncommented section when sending it over mavlink --- src/mavlink_interface.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/mavlink_interface.cpp b/src/mavlink_interface.cpp index e380af4c54..75c2a1a215 100644 --- a/src/mavlink_interface.cpp +++ b/src/mavlink_interface.cpp @@ -275,6 +275,9 @@ void MavlinkInterface::SendSensorMessages(uint64_t time_usec, HILData &hil_data) } // send only windsensor data + + /* Commented to ensure succesfull building until mavlink message definitions are + updated if (data->windsensor_updated) { sensor_msg.wind_direction = data->wind_direction; sensor_msg.wind_speed = data->wind_speed; @@ -282,6 +285,7 @@ void MavlinkInterface::SendSensorMessages(uint64_t time_usec, HILData &hil_data) data->windsensor_updated = false; } + */ if (!hil_mode_ || (hil_mode_ && !hil_state_level_)) { mavlink_message_t msg; From 47a91a17917600cd977a7a90cabd1f6eae7f9edf Mon Sep 17 00:00:00 2001 From: Henry Kotze Date: Mon, 12 Sep 2022 11:27:56 +0200 Subject: [PATCH 6/9] Send windsensor data over mavlink --- src/mavlink_interface.cpp | 3 --- 1 file changed, 3 deletions(-) diff --git a/src/mavlink_interface.cpp b/src/mavlink_interface.cpp index 75c2a1a215..26d2ad3ef0 100644 --- a/src/mavlink_interface.cpp +++ b/src/mavlink_interface.cpp @@ -276,8 +276,6 @@ void MavlinkInterface::SendSensorMessages(uint64_t time_usec, HILData &hil_data) // send only windsensor data - /* Commented to ensure succesfull building until mavlink message definitions are - updated if (data->windsensor_updated) { sensor_msg.wind_direction = data->wind_direction; sensor_msg.wind_speed = data->wind_speed; @@ -285,7 +283,6 @@ void MavlinkInterface::SendSensorMessages(uint64_t time_usec, HILData &hil_data) data->windsensor_updated = false; } - */ if (!hil_mode_ || (hil_mode_ && !hil_state_level_)) { mavlink_message_t msg; From 94b81330a5cb762842f207301106229eb6701818 Mon Sep 17 00:00:00 2001 From: Henry Kotze Date: Wed, 21 Sep 2022 15:07:01 +0200 Subject: [PATCH 7/9] Windsensor naming changed to airflow sensor --- CMakeLists.txt | 15 +- ...plugin.h => gazebo_airflowsensor_plugin.h} | 30 +- include/gazebo_mavlink_interface.h | 5 +- include/mavlink_interface.h | 13 +- .../airflowsensor.sdf} | 6 +- .../model.config | 4 +- models/cloudship2/cloudship2.sdf.jinja | 905 ++++++++++++++++++ msgs/Airspeed.proto | 2 + msgs/WindSensor.proto | 9 - ...in.cpp => gazebo_airflowsensor_plugin.cpp} | 81 +- src/gazebo_mavlink_interface.cpp | 11 +- src/mavlink_interface.cpp | 58 +- 12 files changed, 1024 insertions(+), 115 deletions(-) rename include/{gazebo_windsensor_plugin.h => gazebo_airflowsensor_plugin.h} (84%) rename models/{windsensor/windsensor.sdf => airflowsensor/airflowsensor.sdf} (78%) rename models/{windsensor => airflowsensor}/model.config (76%) create mode 100644 models/cloudship2/cloudship2.sdf.jinja delete mode 100644 msgs/WindSensor.proto rename src/{gazebo_windsensor_plugin.cpp => gazebo_airflowsensor_plugin.cpp} (67%) diff --git a/CMakeLists.txt b/CMakeLists.txt index 6245e8bba5..5b4b1bf09a 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -330,7 +330,15 @@ set(sensor_msgs msgs/OpticalFlow.proto msgs/MagneticField.proto msgs/Pressure.proto +<<<<<<< HEAD msgs/WindSensor.proto +======= + # Cloudline Additions + msgs/AirshipDynamicForces.proto + ) +set(PX4_msgs + msgs/HrtAbsolute.proto +>>>>>>> 3d7464a... Windsensor naming changed to airflow sensor ) PROTOBUF_GENERATE_CPP(MAV_PROTO_SRCS MAV_PROTO_HDRS ${mav_msgs}) @@ -376,7 +384,7 @@ add_library(gazebo_catapult_plugin SHARED src/gazebo_catapult_plugin.cpp) add_library(gazebo_usv_dynamics_plugin SHARED src/gazebo_usv_dynamics_plugin.cpp) add_library(gazebo_parachute_plugin SHARED src/gazebo_parachute_plugin.cpp) add_library(gazebo_drop_plugin SHARED src/gazebo_drop_plugin.cpp) -add_library(gazebo_windsensor_plugin SHARED src/gazebo_windsensor_plugin.cpp) +add_library(gazebo_airflowsensor_plugin SHARED src/gazebo_airflowsensor_plugin.cpp) add_library(gazebo_airship_dynamics_plugin SHARED src/gazebo_airship_dynamics_plugin.cpp) set(plugins @@ -404,7 +412,12 @@ set(plugins gazebo_usv_dynamics_plugin gazebo_parachute_plugin gazebo_drop_plugin +<<<<<<< HEAD gazebo_windsensor_plugin +======= + # Cloudline Additions + gazebo_airflowsensor_plugin +>>>>>>> 3d7464a... Windsensor naming changed to airflow sensor gazebo_airship_dynamics_plugin ) diff --git a/include/gazebo_windsensor_plugin.h b/include/gazebo_airflowsensor_plugin.h similarity index 84% rename from include/gazebo_windsensor_plugin.h rename to include/gazebo_airflowsensor_plugin.h index 6933af7b6b..790bd3f839 100644 --- a/include/gazebo_windsensor_plugin.h +++ b/include/gazebo_airflowsensor_plugin.h @@ -31,15 +31,15 @@ * ****************************************************************************/ /** - * @brief WindSensor Plugin + * @brief AirflowSensor Plugin * - * This plugin publishes WindSensor sensor data + * This plugin publishes Airflow sensor data * * @author Henry Kotze */ -#ifndef _GAZEBO_WINDSENSOR_PLUGIN_HH_ -#define _GAZEBO_WINDSENSOR_PLUGIN_HH_ +#ifndef _GAZEBO_AIRFLOWSENSOR_PLUGIN_HH_ +#define _GAZEBO_AIRFLOWSENSOR_PLUGIN_HH_ #include #include @@ -62,7 +62,7 @@ #include #include -#include +#include #include namespace gazebo @@ -70,11 +70,11 @@ namespace gazebo typedef const boost::shared_ptr WindPtr; -class GAZEBO_VISIBLE WindSensorPlugin : public SensorPlugin +class GAZEBO_VISIBLE AirflowSensorPlugin : public SensorPlugin { public: - WindSensorPlugin(); - virtual ~WindSensorPlugin(); + AirflowSensorPlugin(); + virtual ~AirflowSensorPlugin(); protected: virtual void Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf); @@ -91,7 +91,7 @@ class GAZEBO_VISIBLE WindSensorPlugin : public SensorPlugin transport::NodePtr node_handle_; transport::SubscriberPtr wind_sub_; - transport::PublisherPtr windsensor_pub_; + transport::PublisherPtr airflow_sensor_pub_; event::ConnectionPtr updateSensorConnection_; event::ConnectionPtr updateConnection_; @@ -104,16 +104,18 @@ class GAZEBO_VISIBLE WindSensorPlugin : public SensorPlugin std::string namespace_; std::string link_name_; std::string model_name_; - std::string windsensor_topic_; + std::string airflowsensor_topic_; std::normal_distribution gauss_dir_; std::normal_distribution gauss_speed_; std::default_random_engine generator_; ignition::math::Vector3d wind_; - float wind_direction_; - float wind_speed_; + ignition::math::Vector3d body_wind_; + ignition::math::Vector3d body_vel_; + float airflow_direction_; + float airflow_speed_; -}; // class GAZEBO_VISIBLE WindSensorPlugin +}; // class GAZEBO_VISIBLE AirflowSensorPlugin } // namespace gazebo -#endif // _GAZEBO_WINDSENSOR_PLUGIN_HH_ +#endif // _GAZEBO_AIRFLOWSENSOR_PLUGIN_HH_ diff --git a/include/gazebo_mavlink_interface.h b/include/gazebo_mavlink_interface.h index d77dc46a97..9d21c80b36 100644 --- a/include/gazebo_mavlink_interface.h +++ b/include/gazebo_mavlink_interface.h @@ -57,7 +57,6 @@ #include #include #include -#include #include #include #include @@ -79,16 +78,15 @@ 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)"); -static const std::regex kDefaultWindSensorModelJointNaming(".*(windsensor)(.*_joint)"); static const std::regex kDefaultImuModelJointNaming(".*(imu)(\\d*_joint)"); static const std::regex kDefaultMagModelJointNaming(".*(mag)(\\d*_joint)"); +static const std::regex kDefaultAirflowSensorModelJointNaming(".*(airflowsensor)(.*_joint)"); namespace gazebo { typedef const boost::shared_ptr CommandMotorSpeedPtr; typedef const boost::shared_ptr OdomPtr; typedef const boost::shared_ptr AirspeedPtr; -typedef const boost::shared_ptr WindSensorPtr; typedef const boost::shared_ptr GtPtr; typedef const boost::shared_ptr ImuPtr; typedef const boost::shared_ptr IRLockPtr; @@ -182,7 +180,6 @@ class GazeboMavlinkInterface : public ModelPlugin { 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 WindSensorCallback(WindSensorPtr& windsensor_msg, const int& id); void OpticalFlowCallback(OpticalFlowPtr& opticalFlow_msg); void IRLockCallback(IRLockPtr& irlock_msg); void VisionCallback(OdomPtr& odom_msg); diff --git a/include/mavlink_interface.h b/include/mavlink_interface.h index 5210a269f2..cb18553bd2 100644 --- a/include/mavlink_interface.h +++ b/include/mavlink_interface.h @@ -81,8 +81,7 @@ enum class SensorSource { GYRO = 0b111000, MAG = 0b111000000, BARO = 0b1101000000000, - DIFF_PRESS = 0b10000000000, - WIND_SENSOR = 0b10000000000000 + DIFF_PRESS = 0b10000000000 }; namespace SensorData { @@ -105,6 +104,8 @@ namespace SensorData { struct Airspeed { double diff_pressure; + double speed; + double direction; }; struct WindSensor { @@ -135,15 +136,15 @@ struct HILData { int id=-1; bool baro_updated{false}; bool diff_press_updated{false}; - bool windsensor_updated{false}; + bool airflow_updated{false}; bool mag_updated{false}; bool imu_updated{false}; double temperature; double pressure_alt; double abs_pressure; double diff_pressure; - double wind_direction; - double wind_speed; + double airflow_speed; + double airflow_direction; Eigen::Vector3d mag_b; Eigen::Vector3d accel_b; Eigen::Vector3d gyro_b; @@ -164,6 +165,8 @@ class MavlinkInterface { void SendHeartbeat(); void SendSensorMessages(const uint64_t time_usec); void SendSensorMessages(const uint64_t time_usec, HILData &hil_data); + void SendAirflowSensorMessages(uint64_t 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); diff --git a/models/windsensor/windsensor.sdf b/models/airflowsensor/airflowsensor.sdf similarity index 78% rename from models/windsensor/windsensor.sdf rename to models/airflowsensor/airflowsensor.sdf index 7d8a64e00d..f3edbd4ec9 100644 --- a/models/windsensor/windsensor.sdf +++ b/models/airflowsensor/airflowsensor.sdf @@ -1,6 +1,6 @@ - + 0 0 0 0 0 0 @@ -16,12 +16,12 @@ - + 0 0 0 0 0 0 5.0 true false - + diff --git a/models/windsensor/model.config b/models/airflowsensor/model.config similarity index 76% rename from models/windsensor/model.config rename to models/airflowsensor/model.config index 6c2628d012..dd091c7ee3 100644 --- a/models/windsensor/model.config +++ b/models/airflowsensor/model.config @@ -1,8 +1,8 @@ - windsensor + airflowsensor 1.0 - windsensor.sdf + airflowsensor.sdf Henry Kotze diff --git a/models/cloudship2/cloudship2.sdf.jinja b/models/cloudship2/cloudship2.sdf.jinja new file mode 100644 index 0000000000..d5c49315c3 --- /dev/null +++ b/models/cloudship2/cloudship2.sdf.jinja @@ -0,0 +1,905 @@ +{# ---------------------------------------------------------------- #} +{# general geometry and properties#} +{# ---------------------------------------------------------------- #} +{%- set log_airship_dynamics = 1 -%} +{%- set log_motor_forces = 1 -%} +{%- set log_fin_forces = 1 -%} +{%- set plot_aerodynamics_forces = 0 -%} + +{# geometry #} +{# hull geometry #} +{%- set hull_length = 14.356 -%} +{%- set hull_max_diameter = 3.338 -%} + +{# hull volume #} +{%- set hull_volume = 87.98 -%} + +{# fineness Ratio #} +{%- set fineness_ratio = 4.5 -%} + +{# center of volume as measured from the nose #} +{%- set hull_cv = 6.798245698288959 -%} + +{# center of gravity as measured from the CV #} +{%- set hull_cg_x = 0.0211 -%} +{%- set hull_cg_y = 0.000 -%} +{%- set hull_cg_z = -0.936 -%} + +{# mass and moment of inertia #} +{%- set mass = 102.85 -%} + +{%- set ixx = 558.8664089 -%} +{%- set iyy = 617.39743597 -%} +{%- set izz = 764.31756395 -%} + +{# material properties #} +{# air density (at MSL 15 deg) #} +{%- set air_density = 1.169 -%} +{# gas density (Helium) #} +{%- set gas_density = 0.167 -%} + +{# actuator properties #} +{# maximum thruster rotational velocity (rad/s) #} +{%- set mot_max_vel = 655 -%} +{# maximum thruster lift (kg) #} +{%- set mot_max_thrust_kg = 20 -%} +{# main thrusters time constant #} +{%- set mot_tau = 0.07 -%} +{# maximum thruster angle (rad) #} +{%- set mot_max_angle = 360 * np.pi / 180 -%} +{# maximum control surface angle (rad) #} +{%- set fin_cs_max_angle = 120 * np.pi / 180 -%} +{# simulation motor slowdown #} +{%- set sim_rotor_slow = 20 -%} +{# maximum tail thruster rotational velocity (rad/s) #} +{%- set tail_mot_max_vel = 655 -%} +{# maximum tail thruster lift (kg) #} +{%- set tail_mot_max_thrust_kg = 15 -%} +{# main tail thrusters time constant #} +{%- set tail_mot_tau = 0.07 -%} +{# thruster motor constant #} +{%- set mot_coeff = (mot_max_thrust_kg * 9.81)/(mot_max_vel**2) -%} +{# tail thruster motor constant #} +{%- set tail_mot_coeff = (tail_mot_max_thrust_kg * 9.81)/(tail_mot_max_vel**2) -%} + +{# actuator geometry #} +{# motor distance from gondola #} +{%- set d_mot_y = 1.5 -%} +{# thruster rod radius #} +{%- set mot_rod_r = 0.03 -%} +{# thruster prop radius (16") #} +{%- set mot_prop_r = 0.2032 -%} +{# tail thruster prop radius (16") #} +{%- set tail_mot_prop_r = 0.2032 -%} +{# thruster rod radius inertia factor #} +{%- set mot_rod_r_inertia_factor = 20 -%} +{# thruster prop height #} +{%- set mot_prop_h = 0.02 -%} + +{# fins #} +{%- set fin_root_chord = 1.872 -%} +{%- set fin_span = 0.98 -%} +{%- set fin_thickness = 0.02 -%} +{%- set fin_cs_chord = 0.410 -%} +{%- set fin_cs_gap = 0.001 -%} + +{# center of fin distance from nose and distance from center of hull #} +{%- set d_fin_x_c = 10.77 -%} +{%- set r_fin_rad = 0.41 -%} +{%- set r_fin_cop_x = 5.10653559 -%} +{%- set r_fin_cop_z = 1.737608 -%} +{%- set angle_fin = 20.13833019 -%} +{%- set angle_cs_diff_fin = 7 -%} + +{# aerodynamic properties #} +{# added mass #} +{%- set m11 = 7.565 -%} +{%- set m22 = 96.366 -%} +{%- set m26 = 32.88 -%} +{%- set m33 = m22 -%} +{%- set m35 = -m26 -%} +{%- set m44 = 13.03 -%} +{%- set m53 = -m26 -%} +{%- set m55 = 236.657 -%} +{%- set m62 = m26 -%} +{%- set m66 = m55 -%} + +{# aerodynamic coefficients #} +{%- set dist_potential_flow = hull_cv - 12.34245577700727 -%} +{%- set force_hull_inviscid_flow_coeff = -3.106982155301005 -%} +{%- set force_hull_viscous_flow_coeff = 2.1059728145144914 -%} +{%- set moment_hull_inviscid_flow_coeff = 20.24930722567859 -%} +{%- set moment_hull_viscous_flow_coeff = -13.341622825927411 -%} +{%- set fin_normal_force_coeff = 1.2981061370780032 -%} +{%- set fin_cs_rad_to_cl = 2*np.pi*0.8*0.75*1.2 -%} +{%- set fin_stall_angle = np.rad2deg(0.6997924350094509) -%} +{%- set axial_drag_coeff = 0.001 -%} + +{# ---------------------------------------------------------------- #} +{# positions #} +{# ---------------------------------------------------------------- #} +{# hull center of volume #} +{%- set origin_x = hull_cv -%} +{%- set origin_y = 0 -%} +{%- set origin_z = 0 -%} + +{# ---------------------------------------------------------------- #} +{# macros #} +{# ---------------------------------------------------------------- #} +{# inertial #} +{%- macro inertial(m, ixx, iyy, izz) -%} + + {{m}} + + {{ixx}} + {{iyy}} + {{izz}} + + +{%- endmacro -%} + +{# inertial offset #} +{%- macro inertial_offset(m, ixx, iyy, izz, x, y, z) -%} + + {{ x }} {{ y }} {{ z }} 0 0 0 + {{m}} + + {{ixx}} + {{iyy}} + {{izz}} + + +{%- endmacro -%} + +{# cylinder #} +{%- macro cylinder(r, h) -%} + + + {{r}} + {{h}} + + +{%- endmacro -%} + +{# box #} +{%- macro box(x, y, z) -%} + + + {{x}} {{y}} {{z}} + + +{%- endmacro -%} + +{# fin plugin #} +{%- macro fin_plugin(name, cop_y, cop_z, u_y, u_z, roll) -%} + + + {{ -r_fin_cop_x }} {{ cop_y }} {{ cop_z }} 0 0 0 + {{ inertial(0.001, 0.001, 0.001, 0.001) }} + + 0 0 0 {{ roll }} 0 0 + {{ box(fin_root_chord, fin_thickness, fin_span) }} + + + + {% if plot_aerodynamics_forces == 1 %} + + {{ name }}_liftforce_visual + Gazebo/Blue + + {% endif %} + + + + + hull + {{ name }} + {{ fin_root_chord/2 }} 0 0 0 0 0 + + 1 0 0 + + -{{ 0 }} + {{ 0 }} + + + 1.0 + + + + + 1 + + + + + + 0.0 + {{ fin_normal_force_coeff / (fin_root_chord * fin_span) }} + 0.0 + 0.0 + {{ np.radians(fin_stall_angle) }} + 0.0 + 0.0 + 0.0 + {{ -r_fin_cop_x }} {{ cop_y }} {{ cop_z }} + {{ fin_root_chord * fin_span }} + {{ air_density }} + 1 0 0 + 0 {{ u_y }} {{ u_z }} + hull + {{ name }}_joint + {% if plot_aerodynamics_forces == 1 %} + {{ name }}_liftforce_visual + {% endif %} + + + +{%- endmacro -%} + +{# control surfaces #} +{%- macro control_surface(name, y, z, roll, pitch, yaw, axis_y, axis_z, u_y, u_z, i_span, i_thickness, i_chord, direction) -%} + + {%- set fin_cs_mass = 0.02 -%} + + {{ hull_cv - d_fin_x_c - fin_root_chord - fin_cs_chord/2 - fin_cs_gap }} {{ y }} {{ z }} 0 {{ pitch }} {{ yaw }} + {{ inertial(fin_cs_mass, fin_cs_mass * (1/12)*(i_span**2 + i_thickness**2), fin_cs_mass * (1/12)*(i_chord**2 + i_span**2), fin_cs_mass * (1/12)*(i_chord**2 + i_thickness**2))|indent(6) }} + + true + false + + + + 0 0 0 {{ roll }} 0 0 + {{ box(fin_cs_chord, fin_thickness, fin_span) }} + + + + {% if plot_aerodynamics_forces == 1 %} + + {{ name }}_liftforce_visual + Gazebo/Blue + + {% endif %} + + + + 0 0 0 {{ roll }} 0 0 + {{ box(fin_cs_chord, fin_thickness, fin_span) }} + + + + + hull + {{ name }} + {{ fin_cs_chord/2 }} 0 0 0 0 0 + + 0 {{ -axis_y }} {{ axis_z }} + + -{{ fin_cs_max_angle/2 }} + {{ fin_cs_max_angle/2 }} + + + 0.1 + + + + + 1 + + + + + + 0.0 + 0.0 + 0.0 + 0.0 + {{ np.radians(fin_stall_angle) }} + 0.0 + 0.0 + 0.0 + {{ hull_cv - d_fin_x_c - fin_root_chord - fin_cs_gap }} {{ y }} {{ z }} + {{ fin_cs_chord * fin_span }} + {{ air_density }} + 1 0 0 + 0 {{ u_y }} {{ u_z }} + hull + + {{ name }}_joint + + {{ direction * fin_cs_rad_to_cl }} + {% if plot_aerodynamics_forces == 1 %} + {{ name }}_liftforce_visual + {% endif %} + + +{%- endmacro -%} + +{# Tilt thruster #} +{%- macro tilt_thruster(motor_num, direction, parent, mot_prop_r, mot_tau, mot_max_vel, mot_coeff, reversible, dist_x, dist_y, dist_z, roll) -%} + + {{ dist_x }} {{ dist_y }} {{ dist_z }} {{ 0 }} {{ 0 }} {{ 0 }} + + 0 0 0 0 0 0 + 0.05 + + 0.0166704 + 0 + 0 + 0.0166704 + 0 + 0.0167604 + + + + 0 0 0 0 0 0 + + + 0.01 + 0.01 + + + + + + + + + + + + + 0 0 0 0 0 0 + + + 0.1 + 0.1 + + + + + + + 1 + + 0 + + + + {{ parent }} + motor_{{ motor_num }} + + 0 1 0 + + -{{ mot_max_angle/2 }} + {{ mot_max_angle/2 }} + + + 1.0 + 0 + 0 + + 1 + + + + + {%- set motor_mass = 0.025 -%} + {{ dist_x }} {{ dist_y }} {{ dist_z }} {{ 0 }} {{ 0 }} 0 + + 0 0 0 0 0 0 + 0.005 + + 9.75e-07 + 0 + 0 + 0.000166704 + 0 + 0.000167604 + + + + + + + {{ mot_prop_r * 2 / 0.25711 }} {{ mot_prop_r * 2 / 0.25711 }} {{ mot_prop_h / 0.00959 }} + model://cloudship2/meshes/prop_{{ direction }}.dae + + + + + + + + + + + {{ mot_prop_h }} + {{ mot_prop_r }} + + + + + + + rotor_{{ motor_num }} + motor_{{ motor_num }} + + 0 0 1 + + -1e+16 + 1e+16 + + + 0 + 0 + + + + +{# Only to visualize motor spin #} + + + rotor_{{ motor_num }}_joint + rotor_{{ motor_num }} + {{ direction }} + {{ reversible }} + {{ mot_tau }} + {{ mot_tau }} + {{ mot_max_vel }} + {{ mot_coeff }} + {{ 0 }} + 0.000806428 + 0.000001 + /gazebo/command/motor_speed + {{ motor_num }} + /motor_speed/{{ motor_num }} + {{sim_rotor_slow}} + +{%- endmacro -%} + +{# thruster #} +{%- macro thruster(motor_num, direction, parent, mot_prop_r, mot_tau, mot_max_vel, mot_coeff, reversible, dist_x, dist_y, dist_z, roll) -%} + + {%- set motor_mass = 0.0001 -%} + {{ dist_x }} {{ dist_y }} {{ dist_z }} {{ roll }} {{ 0 }} 0 + {{ inertial(motor_mass, motor_mass*mot_prop_h**2, motor_mass*(1/12)*((2*mot_prop_r)**2 + mot_prop_h**2), motor_mass*(1/12)*((2*mot_prop_r)**2 + mot_prop_h**2))|indent(6) }} + + false + false + + + + + + {{ mot_prop_r * 2 / 0.25711 }} {{ mot_prop_r * 2 / 0.25711 }} {{ mot_prop_h / 0.00959 }} + model://cloudship2/meshes/prop_{{ direction }}.dae + + + + + + + + + + + {{ mot_prop_h }} + {{ mot_prop_r }} + + + + + + + rotor_{{ motor_num }} + {{ parent }} + + 0 0 1 + + -1e+16 + 1e+16 + + + 0 + 0 + + + + +{# Only to visualize motor spin #} + + + rotor_{{ motor_num }}_joint + rotor_{{ motor_num }} + {{ direction }} + {{ reversible }} + {{ mot_tau }} + {{ mot_tau }} + {{ mot_max_vel }} + {{ mot_coeff }} + {{ 0 }} + 0.000806428 + 0.000001 + /gazebo/command/motor_speed + {{ motor_num }} + /motor_speed/{{ motor_num }} + {{sim_rotor_slow}} + +{%- endmacro -%} + +{# ---------------------------------------------------------------- #} +{# SDF description #} +{# ---------------------------------------------------------------- #} + + + + + + false + false + + {# Airship #} + + 0 0 0 0 0 0 + {{ inertial_offset(mass, ixx, iyy, izz, hull_cg_x, hull_cg_y, hull_cg_z)|indent(6) }} + + true + false + + + {# Hull #} + + {{ 0 }} {{ origin_y }} {{ origin_z }} -0.523599 0 0 + + + 0.001 0.001 0.001 + model://cloudship2/meshes/cloudship2.stl + + + + + + + + {# Hull Collsion: Made as box around hull #} + + {{ 0 }} {{ 0 }} {{ 0 }} 0 0 0 + + + {{hull_length}} {{hull_max_diameter}} {{hull_max_diameter}} + + + + + + + 0 0 -1.2 0 0 0 + + 0 0 0 0 0 0 + 0.0001 + + 1e-05 + 0 + 0 + 1e-05 + 0 + 1e-05 + + + + + + 0.01 + + + + + + + + + /imu_link + hull + + 1 0 0 + + 0 + 0 + 0 + 0 + + + 0 + 0 + + 1 + + + + {# Airspeed Sensor #} + + model://airspeed + 0 0 0 0 0 0 + airspeed + + + airspeed::link + hull + + + {# Airflow Sensor #} + + model://airflowsensor + 5 0 -3 0 0 + airflowsensor + + + airflowsensor::link + hull + + + {# First GPS #} + + model://gps + 0.1 0 -1.2 0 0 0 + gps + + + gps::link + hull + + + {# Lidar #} + + model://sf10a + 0 0 {{ -hull_max_diameter/2 - 0.01}} 0 0 0 + sf10a + + + sf10a::link + hull + + + {# Control surfaces #} + {%- set cs_angle = np.radians(angle_fin - angle_cs_diff_fin) -%} + {{ control_surface("rudder_top", 0, r_fin_cop_z, 0, 0, 0, 0, 1, 1, 0, fin_span, fin_thickness, fin_cs_chord, 1) }} + {{ control_surface("elevator_sb", -r_fin_cop_z*np.cos(np.pi/6), -r_fin_cop_z*np.sin(np.pi/6), np.pi/3*2, 0, 0, 1*np.cos(np.pi/6), -np.sin(np.pi/6), -np.sin(np.pi/6), np.cos(np.pi/6), fin_thickness, fin_span, fin_cs_chord, 1) }} + {{ control_surface("elevator_port", r_fin_cop_z*np.cos(np.pi/6), -r_fin_cop_z*np.sin(np.pi/6), 2*np.pi/3*2, 0, 0, 1*np.cos(np.pi/6), np.sin(np.pi/6), np.sin(np.pi/6), np.cos(np.pi/6), fin_thickness, fin_span, fin_cs_chord, 1) }} + + {# Starboard Main Thruster #} + {{ tilt_thruster(0, "cw", "hull", 2*tail_mot_prop_r, mot_tau, mot_max_vel, mot_coeff, false, 3.07, 2.5, -0.5, -np.pi/2) }} + {# Port Main Thruster #} + {{ tilt_thruster(1, "ccw", "hull", 2*tail_mot_prop_r, mot_tau, mot_max_vel, mot_coeff, false, 3.07, -2.5, -0.5, -np.pi/2) }} + + {# Yaw tail thruster #} + {{ thruster(7, "cw", "hull", tail_mot_prop_r, tail_mot_tau, tail_mot_max_vel, tail_mot_coeff, true, hull_cv - hull_length - tail_mot_prop_r, 0, 0, np.pi/2) }} + {# Vertical tail thruster #} + {{ thruster(8, "cw", "hull", tail_mot_prop_r, tail_mot_tau, tail_mot_max_vel, tail_mot_coeff, true, hull_cv - hull_length - tail_mot_prop_r - 0.5, 0, 0, 0) }} + + {# ---------------------------------------------------------------- #} + {# Plugins #} + {# ---------------------------------------------------------------- #} + {# Airship Dynamics #} + + + hull + {{ hull_volume }} + {{ fineness_ratio }} + {{ hull_length }} + {{ air_density }} + {{ m11 }} + {{ m22 }} + {{ m26 }} + {{ m33 }} + {{ m35 }} + {{ m44 }} + {{ m53 }} + {{ m55 }} + {{ m62 }} + {{ m66 }} + {{log_airship_dynamics}} + {{ hull_cv }} + {{ dist_potential_flow }} + {{ force_hull_inviscid_flow_coeff }} + {{ force_hull_viscous_flow_coeff }} + {{ moment_hull_inviscid_flow_coeff }} + {{ moment_hull_viscous_flow_coeff }} + {{ axial_drag_coeff }} + world_wind + + {# Fins #} + {{ fin_plugin("fin1", 0, r_fin_cop_z, 1, 0, 0) }} + {{ fin_plugin("fin2", -r_fin_cop_z*np.cos(np.pi/6), -r_fin_cop_z*np.sin(np.pi/6), -np.sin(np.pi/6), np.cos(np.pi/6), np.pi/3*2) }} + {{ fin_plugin("fin3", r_fin_cop_z*np.cos(np.pi/6), -r_fin_cop_z*np.sin(np.pi/6), np.sin(np.pi/6), np.cos(np.pi/6), 2*np.pi/3*2) }} + + {# IMU #} + + + /imu_link + /imu + 0.0003394 + 3.8785e-05 + 1000.0 + 0.0087 + 0.004 + 0.006 + 300.0 + 0.196 + + + {# Magnetometer #} + + + 100 + 0.0004 + 6.4e-06 + 600 + /mag + + + {# Barometer #} + + + 50 + /baro + 0 + + + {# MAVLink #} + + + /imu + /mag + /baro + /sf10a/link/lidar + INADDR_ANY + 4560 + 14560 + false + /dev/ttyACM0 + 921600 + INADDR_ANY + 14550 + + false + false + true + true + /gazebo/command/motor_speed + false + 1 + true + false + + + + 0 + 0 + {{ mot_max_vel }} + 0 + 0 + velocity + rotor_0_joint + + + 1 + 0 + {{ mot_max_vel }} + 0 + 0 + velocity + rotor_1_joint + + + 2 + 0 + {{ mot_max_angle/2 }} + 0 + 0 + position + motor_0_joint + +

10

+ 0 + 0 + 1.0 + -1.0 + {{ mot_max_angle/2 }} + {{ -mot_max_angle/2 }} + + + + 3 + 0 + {{ mot_max_angle/2 }} + 0 + 0 + position + motor_1_joint + +

10

+ 0 + 0 + 1.0 + -1.0 + {{ mot_max_angle/2 }} + {{ -mot_max_angle/2 }} + + + + 4 + 0 + {{ fin_cs_max_angle }} + 0 + 0 + position_kinematic + elevator_port_joint + + + 5 + 0 + {{ fin_cs_max_angle }} + 0 + 0 + position_kinematic + elevator_sb_joint + + + 6 + 0 + {{ fin_cs_max_angle }} + 0 + 0 + position_kinematic + rudder_top_joint + + + 7 + 0 + {{ tail_mot_max_vel }} + 0 + 0 + velocity + rotor_7_joint + + + 8 + 0 + {{ -1.0*tail_mot_max_vel }} + 0 + 0 + velocity + rotor_8_joint + + + + + + + + + + + + diff --git a/msgs/Airspeed.proto b/msgs/Airspeed.proto index 0b7c8f9f94..ae3c98dba5 100644 --- a/msgs/Airspeed.proto +++ b/msgs/Airspeed.proto @@ -5,4 +5,6 @@ message Airspeed { required int64 time_usec = 1; required double diff_pressure = 2; + optional double speed = 3; + optional double direction = 4; } diff --git a/msgs/WindSensor.proto b/msgs/WindSensor.proto deleted file mode 100644 index 6d8ca9dccd..0000000000 --- a/msgs/WindSensor.proto +++ /dev/null @@ -1,9 +0,0 @@ -syntax = "proto2"; -package sensor_msgs.msgs; - -message WindSensor -{ - required int64 time_usec = 1; - required double wind_speed = 2; - required double wind_direction = 3; -} diff --git a/src/gazebo_windsensor_plugin.cpp b/src/gazebo_airflowsensor_plugin.cpp similarity index 67% rename from src/gazebo_windsensor_plugin.cpp rename to src/gazebo_airflowsensor_plugin.cpp index 3e8fab2bc6..31c1a50e34 100644 --- a/src/gazebo_windsensor_plugin.cpp +++ b/src/gazebo_airflowsensor_plugin.cpp @@ -33,36 +33,36 @@ /** * @brief WindSensor Plugin * - * This plugin publishes Windsensor + * This plugin publishes airflow sensor measurements * * @author Henry Kotze */ -#include +#include #include namespace gazebo { -GZ_REGISTER_SENSOR_PLUGIN(WindSensorPlugin) +GZ_REGISTER_SENSOR_PLUGIN(AirflowSensorPlugin) -WindSensorPlugin::WindSensorPlugin() : SensorPlugin(), - wind_direction_(0.0), - wind_speed_(0.0f), +AirflowSensorPlugin::AirflowSensorPlugin() : SensorPlugin(), + airflow_direction_(0.0), + airflow_speed_(0.0f), gauss_dir_(0.0, 4*(M_PI/180.0)), gauss_speed_(0.0, 0.3) { } -WindSensorPlugin::~WindSensorPlugin() +AirflowSensorPlugin::~AirflowSensorPlugin() { if (updateConnection_) updateConnection_->~Connection(); } -void WindSensorPlugin::Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf) +void AirflowSensorPlugin::Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf) { // Get then name of the parent sensor this->parentSensor_ = std::dynamic_pointer_cast(_parent); if (!parentSensor_) - gzthrow("WindSensorPlugin requires a Wind Sensor as its parent"); + gzthrow("AirflowSensorPlugin requires a airflow Sensor as its parent"); // Get the root model name const std::string scopedName = _parent->ParentName(); @@ -79,14 +79,14 @@ void WindSensorPlugin::Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf) // store the model name model_name_ = names_splitted[0]; - // get windsensor topic name + // get airflow sensor topic name if(_sdf->HasElement("topic")) { - windsensor_topic_ = _sdf->GetElement("topic")->Get(); + airflowsensor_topic_ = _sdf->GetElement("topic")->Get(); } else { // if not set by parameter, get the topic name from the model name - windsensor_topic_ = parentSensorModelName; - gzwarn << "[gazebo_windsensor_plugin]: " + names_splitted.front() + "::" + names_splitted.rbegin()[1] + - " using windsensor topic \"" << parentSensorModelName << "\"\n"; + airflowsensor_topic_ = parentSensorModelName; + gzwarn << "[gazebo_airflowsensor_plugin]: " + names_splitted.front() + "::" + names_splitted.rbegin()[1] + + " using airflowsensor topic \"" << parentSensorModelName << "\"\n"; } @@ -96,7 +96,7 @@ void WindSensorPlugin::Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf) if (_sdf->HasElement("robotNamespace")) { namespace_ = _sdf->GetElement("robotNamespace")->Get(); } else { - gzerr << "[gazebo_windsensor_plugin] Please specify a robotNamespace.\n"; + gzerr << "[gazebo_airflowsensor_plugin] Please specify a robotNamespace.\n"; } this->node_handle_ = transport::NodePtr(new transport::Node()); @@ -104,15 +104,15 @@ void WindSensorPlugin::Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf) this->parentSensor_->SetUpdateRate(10.0); this->parentSensor_->SetActive(false); - updateSensorConnection_ = parentSensor_->ConnectUpdated(boost::bind(&WindSensorPlugin::OnSensorUpdate, this)); + updateSensorConnection_ = parentSensor_->ConnectUpdated(boost::bind(&AirflowSensorPlugin::OnSensorUpdate, this)); this->parentSensor_->SetActive(true); // Listen to the update event. This event is broadcast every // simulation iteration. - updateConnection_ = event::Events::ConnectWorldUpdateBegin(boost::bind(&WindSensorPlugin::OnUpdate, this, _1)); + updateConnection_ = event::Events::ConnectWorldUpdateBegin(boost::bind(&AirflowSensorPlugin::OnUpdate, this, _1)); - windsensor_pub_ = node_handle_->Advertise("~/" + model_name_ + "/link/" + windsensor_topic_, 10); - wind_sub_ = node_handle_->Subscribe("~/world_wind", &WindSensorPlugin::WindVelocityCallback, this); + airflow_sensor_pub_ = node_handle_->Advertise("~/" + model_name_ + "/link/" + airflowsensor_topic_, 10); + wind_sub_ = node_handle_->Subscribe("~/world_wind", &AirflowSensorPlugin::WindVelocityCallback, this); // Set initial wind as zero wind_.X() = 0; @@ -121,7 +121,7 @@ void WindSensorPlugin::Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf) } -void WindSensorPlugin::OnUpdate(const common::UpdateInfo&){ +void AirflowSensorPlugin::OnUpdate(const common::UpdateInfo&){ #if GAZEBO_MAJOR_VERSION >= 9 model_ = world_->ModelByName(model_name_); @@ -132,7 +132,7 @@ void WindSensorPlugin::OnUpdate(const common::UpdateInfo&){ #endif link_ = boost::dynamic_pointer_cast(parentEntity); if (link_ == NULL) - gzthrow("[gazebo_airspeed_plugin] Couldn't find specified link \"" << link_name_ << "\"."); + gzthrow("[gazebo_airflowsensor_plugin] Couldn't find specified link \"" << link_name_ << "\"."); #if GAZEBO_MAJOR_VERSION >= 9 common::Time current_time = world_->SimTime(); @@ -148,46 +148,41 @@ void WindSensorPlugin::OnUpdate(const common::UpdateInfo&){ ignition::math::Quaterniond C_W_I = T_W_I.Rot(); #if GAZEBO_MAJOR_VERSION >= 9 + body_wind_ = C_W_I.RotateVectorReverse(wind_); vel_a_ = link_->RelativeLinearVel() - C_W_I.RotateVectorReverse(wind_); - ang_a_ = link_->RelativeLinearVel() - C_W_I.RotateVectorReverse(wind_); + body_vel_ = link_->RelativeLinearVel(); #else + body_wind_ = C_W_I.RotateVectorReverse(wind_); vel_a_ = ignitionFromGazeboMath(link_->GetRelativeLinearVel()) - C_W_I.RotateVectorReverse(wind_); - ang_a_ = ignitionFromGazeboMath(link_->GetRelativeAngularVel()); + body_vel_ = ignitionFromGazeboMath(link_->GetRelativeLinearVel()); #endif last_time_ = current_time; - wind_direction_ = atan2f(vel_a_.Y(),vel_a_.X()) * (180.0/M_PI); + airflow_direction_ = atan2f(vel_a_.Y(),vel_a_.X()) * (180.0/M_PI); // resolution of 1 degree - wind_direction_ = round(wind_direction_) * (M_PI/180.0) + gauss_dir_(generator_); - // wind sensor cannot measure wind in the z-direction + airflow_direction_ = round(airflow_direction_) * (M_PI/180.0) + gauss_dir_(generator_); + // airflow sensor cannot measure wind in the z-direction vel_a_.Z() = 0; - wind_speed_ = vel_a_.Length()*10.0; + airflow_speed_ = vel_a_.Length()*10.0; // resolution of 0.1m/s - wind_speed_ = round(wind_speed_)/10.0 + gauss_speed_(generator_); + airflow_speed_ = round(airflow_speed_)/10.0 + gauss_speed_(generator_); } -void WindSensorPlugin::OnSensorUpdate() { +void AirflowSensorPlugin::OnSensorUpdate() { - - sensor_msgs::msgs::WindSensor windsensor_msg; - windsensor_msg.set_time_usec(last_time_.Double() * 1e6); - windsensor_msg.set_wind_speed(wind_speed_); - windsensor_msg.set_wind_direction(wind_direction_); - windsensor_pub_->Publish(windsensor_msg); + sensor_msgs::msgs::Airspeed airflow_sensor_msg; + airflow_sensor_msg.set_time_usec(last_time_.Double() * 1e6); + airflow_sensor_msg.set_diff_pressure(ignition::math::NAN_D); + airflow_sensor_msg.set_direction(airflow_direction_); + airflow_sensor_msg.set_speed(airflow_speed_); + airflow_sensor_pub_->Publish(airflow_sensor_msg); } -void WindSensorPlugin::WindVelocityCallback(WindPtr& wind) { +void AirflowSensorPlugin::WindVelocityCallback(WindPtr& wind) { // Get world wind velocity. ignition::math::Vector3d wind_world = ignition::math::Vector3d(wind->velocity().x(), wind->velocity().y(), wind->velocity().z()); - // Rotate to body frame - #if GAZEBO_MAJOR_VERSION >= 9 - wind_ = link_->WorldPose().Rot().Inverse().RotateVector(wind_world); - #else - wind_ = ignitionFromGazeboMath(link_->GetWorldPose()).Rot().Inverse().RotateVector(wind_world); - #endif - } } // namespace gazebo diff --git a/src/gazebo_mavlink_interface.cpp b/src/gazebo_mavlink_interface.cpp index add32f8720..7885b2f3ab 100644 --- a/src/gazebo_mavlink_interface.cpp +++ b/src/gazebo_mavlink_interface.cpp @@ -443,9 +443,9 @@ void GazeboMavlinkInterface::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf CreateSensorSubscription(&GazeboMavlinkInterface::SonarCallback, this, joints, nested_model, kDefaultSonarModelNaming); CreateSensorSubscription(&GazeboMavlinkInterface::GpsCallback, this, joints, nested_model, kDefaultGPSModelNaming); CreateSensorSubscription(&GazeboMavlinkInterface::AirspeedCallback, this, joints, nested_model, kDefaultAirspeedModelJointNaming); - CreateSensorSubscription(&GazeboMavlinkInterface::WindSensorCallback, this, joints, nested_model, kDefaultWindSensorModelJointNaming); CreateSensorSubscription(&GazeboMavlinkInterface::ImuCallback, this, joints, nested_model, kDefaultImuModelJointNaming); CreateSensorSubscription(&GazeboMavlinkInterface::MagnetometerCallback, this, joints, nested_model, kDefaultMagModelJointNaming); + CreateSensorSubscription(&GazeboMavlinkInterface::AirspeedCallback, this, joints, nested_model, kDefaultAirflowSensorModelJointNaming); // Publish gazebo's motor_speed message motor_velocity_reference_pub_ = node_handle_->Advertise("~/" + model_->GetName() + motor_velocity_reference_pub_topic_, 1); @@ -1090,16 +1090,11 @@ void GazeboMavlinkInterface::MagnetometerCallback(MagnetometerPtr& mag_msg, cons void GazeboMavlinkInterface::AirspeedCallback(AirspeedPtr& airspeed_msg, const int& id) { SensorData::Airspeed airspeed_data; airspeed_data.diff_pressure = airspeed_msg->diff_pressure(); + airspeed_data.speed = airspeed_msg->speed(); + airspeed_data.direction = airspeed_msg->direction(); mavlink_interface_->UpdateAirspeed(airspeed_data, id); } -void GazeboMavlinkInterface::WindSensorCallback(WindSensorPtr& windsensor_msg, const int& id) { - SensorData::WindSensor windsensor_data; - windsensor_data.wind_speed = windsensor_msg->wind_speed(); - windsensor_data.wind_direction = windsensor_msg->wind_direction(); - mavlink_interface_->UpdateWindSensor(windsensor_data, id); -} - void GazeboMavlinkInterface::BarometerCallback(BarometerPtr& baro_msg) { SensorData::Barometer baro_data; baro_data.temperature = baro_msg->temperature(); diff --git a/src/mavlink_interface.cpp b/src/mavlink_interface.cpp index 26d2ad3ef0..f78ec9816e 100644 --- a/src/mavlink_interface.cpp +++ b/src/mavlink_interface.cpp @@ -204,9 +204,12 @@ void MavlinkInterface::Load() void MavlinkInterface::SendSensorMessages(uint64_t time_usec) { for (auto& data : hil_data_) { - if (data.baro_updated | data.diff_press_updated | data.mag_updated | data.imu_updated | data.windsensor_updated) { + if (data.baro_updated | data.diff_press_updated | data.mag_updated | data.imu_updated) { SendSensorMessages(time_usec, data); } + if(data.airflow_updated){ + SendAirflowSensorMessages(time_usec, data); + } } } @@ -225,6 +228,24 @@ void MavlinkInterface::SendHeartbeat() { } } +void MavlinkInterface::SendAirflowSensorMessages(uint64_t time_usec, HILData &hil_data) { + + + mavlink_hil_airflow_sensor_t airflow_sensor_msg; + // airflow_sensor_msg.id = data.id; + airflow_sensor_msg.time_usec = time_usec; + airflow_sensor_msg.speed = hil_data.airflow_speed; + airflow_sensor_msg.direction = hil_data.airflow_direction; + + hil_data.airflow_updated = false; + + if (!hil_mode_ || (hil_mode_ && !hil_state_level_)) { + mavlink_message_t msg; + mavlink_msg_hil_airflow_sensor_encode_chan(1, 200, MAVLINK_COMM_0, &msg, &airflow_sensor_msg); + send_mavlink_message(&msg); + } +} + void MavlinkInterface::SendSensorMessages(uint64_t time_usec, HILData &hil_data) { const std::lock_guard lock(sensor_msg_mutex_); @@ -274,15 +295,6 @@ void MavlinkInterface::SendSensorMessages(uint64_t time_usec, HILData &hil_data) data->diff_press_updated = false; } - // send only windsensor data - - if (data->windsensor_updated) { - sensor_msg.wind_direction = data->wind_direction; - sensor_msg.wind_speed = data->wind_speed; - sensor_msg.fields_updated = sensor_msg.fields_updated | (uint16_t)SensorSource::WIND_SENSOR; - - data->windsensor_updated = false; - } if (!hil_mode_ || (hil_mode_ && !hil_state_level_)) { mavlink_message_t msg; @@ -336,23 +348,17 @@ 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::UpdateWindSensor(const SensorData::WindSensor &data, int id) { - const std::lock_guard lock(sensor_msg_mutex_); - for (auto& instance : hil_data_) { - if (instance.id == id) { - instance.wind_direction = data.wind_direction; - instance.wind_speed = data.wind_speed; - instance.windsensor_updated = true; - return; + if(!isnan(data.diff_pressure)){ + instance.diff_pressure = data.diff_pressure; + instance.diff_press_updated = true; + return; + } else { + instance.airflow_speed = data.speed; + instance.airflow_direction = data.direction; + instance.airflow_updated = true; + return; + } } } //Register new HIL instance if we have never seen the id From 1f59a4e0a624f9ed837778d3e6a68d2ecf88854b Mon Sep 17 00:00:00 2001 From: Henry Kotze Date: Wed, 21 Sep 2022 15:35:36 +0200 Subject: [PATCH 8/9] Fix missed cherry-pick conflict --- CMakeLists.txt | 14 -------------- 1 file changed, 14 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 5b4b1bf09a..ae252c7db7 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -330,15 +330,6 @@ set(sensor_msgs msgs/OpticalFlow.proto msgs/MagneticField.proto msgs/Pressure.proto -<<<<<<< HEAD - msgs/WindSensor.proto -======= - # Cloudline Additions - msgs/AirshipDynamicForces.proto - ) -set(PX4_msgs - msgs/HrtAbsolute.proto ->>>>>>> 3d7464a... Windsensor naming changed to airflow sensor ) PROTOBUF_GENERATE_CPP(MAV_PROTO_SRCS MAV_PROTO_HDRS ${mav_msgs}) @@ -412,12 +403,7 @@ set(plugins gazebo_usv_dynamics_plugin gazebo_parachute_plugin gazebo_drop_plugin -<<<<<<< HEAD - gazebo_windsensor_plugin -======= - # Cloudline Additions gazebo_airflowsensor_plugin ->>>>>>> 3d7464a... Windsensor naming changed to airflow sensor gazebo_airship_dynamics_plugin ) From 93d3362ee5e507f433750dd4946fd54f16508502 Mon Sep 17 00:00:00 2001 From: Henry Kotze Date: Wed, 21 Sep 2022 17:00:21 +0200 Subject: [PATCH 9/9] Removed Old windsensor function --- include/mavlink_interface.h | 1 - 1 file changed, 1 deletion(-) diff --git a/include/mavlink_interface.h b/include/mavlink_interface.h index cb18553bd2..b798097115 100644 --- a/include/mavlink_interface.h +++ b/include/mavlink_interface.h @@ -170,7 +170,6 @@ class MavlinkInterface { 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 UpdateWindSensor(const SensorData::WindSensor &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();