From 4e34b92edb14dd083cc3012fa9537c81af88ed9a Mon Sep 17 00:00:00 2001 From: haitomatic Date: Wed, 25 Sep 2024 08:24:31 +0000 Subject: [PATCH] Fix motor vel scalings --- include/gazebo_mavlink_interface.h | 4 +- src/gazebo_mavlink_interface.cpp | 81 +++++++++++++++++++++--------- 2 files changed, 59 insertions(+), 26 deletions(-) diff --git a/include/gazebo_mavlink_interface.h b/include/gazebo_mavlink_interface.h index 978ac4a..eddcb2e 100644 --- a/include/gazebo_mavlink_interface.h +++ b/include/gazebo_mavlink_interface.h @@ -160,6 +160,7 @@ namespace mavlink_interface float AddSimpleNoise(float value, float mean, float stddev); void RotateQuaternion(gz::math::Quaterniond &q_FRD_to_NED, const gz::math::Quaterniond q_FLU_to_ENU); + void ParseMulticopterMotorModelPlugins(const std::string &sdfFilePath); static const unsigned n_out_max = 16; @@ -169,6 +170,7 @@ namespace mavlink_interface double zero_position_disarmed_[n_out_max]; double zero_position_armed_[n_out_max]; int motor_input_index_[n_out_max]; + double motor_vel_scalings_[n_out_max] {1.0}; int servo_input_index_[n_out_max]; bool input_is_cmd_vel_{false}; @@ -186,8 +188,6 @@ namespace mavlink_interface std::string mag_sub_topic_{kDefaultMagTopic}; std::string baro_sub_topic_{kDefaultBarometerTopic}; std::string cmd_vel_sub_topic_{kDefaultCmdVelTopic}; - int mc_motor_vel_scaling_{1000}; - int fw_motor_vel_scaling_{3500}; std::mutex last_imu_message_mutex_ {}; diff --git a/src/gazebo_mavlink_interface.cpp b/src/gazebo_mavlink_interface.cpp index 55b1358..87da8c0 100644 --- a/src/gazebo_mavlink_interface.cpp +++ b/src/gazebo_mavlink_interface.cpp @@ -41,11 +41,11 @@ GZ_ADD_PLUGIN( using namespace mavlink_interface; GazeboMavlinkInterface::GazeboMavlinkInterface() : - motor_input_index_ {}, - servo_input_index_ {} - { - mavlink_interface_ = std::make_shared(); - } + motor_input_index_ {}, + servo_input_index_ {} +{ + mavlink_interface_ = std::make_shared(); +} GazeboMavlinkInterface::~GazeboMavlinkInterface() { mavlink_interface_->close(); @@ -71,7 +71,6 @@ void GazeboMavlinkInterface::Configure(const gz::sim::Entity &_entity, protocol_version_ = _sdf->Get("protocol_version"); } - gazebo::getSdfParam(_sdf, "poseSubTopic", pose_sub_topic_, pose_sub_topic_); gazebo::getSdfParam(_sdf, "gpsSubTopic", gps_sub_topic_, gps_sub_topic_); gazebo::getSdfParam(_sdf, "visionSubTopic", vision_sub_topic_, vision_sub_topic_); @@ -81,13 +80,14 @@ void GazeboMavlinkInterface::Configure(const gz::sim::Entity &_entity, gazebo::getSdfParam(_sdf, "magSubTopic", mag_sub_topic_, mag_sub_topic_); gazebo::getSdfParam(_sdf, "cmdVelSubTopic", cmd_vel_sub_topic_, cmd_vel_sub_topic_); gazebo::getSdfParam(_sdf, "baroSubTopic", baro_sub_topic_, baro_sub_topic_); - gazebo::getSdfParam(_sdf, "mc_motor_vel_scaling", mc_motor_vel_scaling_, mc_motor_vel_scaling_); - gazebo::getSdfParam(_sdf, "fw_motor_vel_scaling", fw_motor_vel_scaling_, fw_motor_vel_scaling_); - // set motor and servo input_reference_ from inputs.control + // Set motor and servo input_reference_ from inputs.control motor_input_reference_.resize(n_out_max); servo_input_reference_.resize(n_out_max); + // Parse the MulticopterMotorModel plugins to get the motor velocity scalings + ParseMulticopterMotorModelPlugins(model_.SourceFilePath(_ecm)); + bool use_tcp = false; if (_sdf->HasElement("use_tcp")) { @@ -127,7 +127,7 @@ void GazeboMavlinkInterface::Configure(const gz::sim::Entity &_entity, gzmsg << "Speed factor set to: " << speed_factor_ << std::endl; } - // // Listen to Ctrl+C / SIGINT. + // Listen to Ctrl+C / SIGINT. sigIntConnection_ = _em.Connect(std::bind(&GazeboMavlinkInterface::onSigInt, this)); auto world_name = "/" + gz::sim::scopedName(gz::sim::worldEntity(_ecm), _ecm); @@ -478,19 +478,7 @@ void GazeboMavlinkInterface::handle_actuator_controls(const gz::sim::UpdateInfo for (int i = 0; i < motor_input_reference_.size(); i++) { if (armed) { - if (i == motor_input_reference_.size() - 1) { - // Set pusher motor velocity scaling if airframe is either fw or vtol. This assumes the pusher motor is the last motor! - double scaling; - if (servo_input_reference_.size() > 0 && servo_input_reference_.size() != n_out_max) { - scaling = static_cast(fw_motor_vel_scaling_); - } else { - scaling = static_cast(mc_motor_vel_scaling_); - } - motor_input_reference_[i] = actuator_controls[motor_input_index_[i]] * scaling; - } - else { - motor_input_reference_[i] = actuator_controls[motor_input_index_[i]] * static_cast(mc_motor_vel_scaling_); - } + motor_input_reference_[i] = actuator_controls[motor_input_index_[i]] * motor_vel_scalings_[i]; } else { motor_input_reference_[i] = 0; } @@ -627,4 +615,49 @@ void GazeboMavlinkInterface::RotateQuaternion(gz::math::Quaterniond &q_FRD_to_NE // final rotation composition q_FRD_to_NED = q_ENU_to_NED * q_FLU_to_ENU * q_FLU_to_FRD.Inverse(); -} \ No newline at end of file +} + +void GazeboMavlinkInterface::ParseMulticopterMotorModelPlugins(const std::string &sdfFilePath) +{ + // Load the SDF file + sdf::Root root; + sdf::Errors errors = root.Load(sdfFilePath); + if (!errors.empty()) + { + for (const auto &error : errors) + { + gzerr << "[gazebo_mavlink_interface] Error: " << error.Message() << std::endl; + } + return; + } + + // Load the model + const sdf::Model *model = root.Model(); + if (!model) + { + gzerr << "[gazebo_mavlink_interface] No models found in SDF file." << std::endl; + return; + } + + // Iterate through all plugins in the model + for (const sdf::Plugin plugin : model->Plugins()) + { + // Check if the plugin is a MulticopterMotorModel + if (plugin.Name() == "gz::sim::systems::MulticopterMotorModel") { + if (plugin.Element()->HasElement("motorNumber")) + { + int motorNumber = plugin.Element()->Get("motorNumber"); + if (motorNumber >= n_out_max) + { + gzerr << "[gazebo_mavlink_interface] Motor number " << motorNumber + << " exceeds maximum number of motors " << n_out_max << std::endl; + continue; + } + if (plugin.Element()->HasElement("motorNumber")) + { + motor_vel_scalings_[motorNumber] = plugin.Element()->Get("maxRotVelocity"); + } + } + } + } +}