From ac47b1255dc8079625b9e6e100a4ca7551d6814f Mon Sep 17 00:00:00 2001 From: JaeyoungLim Date: Mon, 6 May 2024 16:18:56 +0200 Subject: [PATCH] Add Gazebo Coldgas thruster plugin --- CMakeLists.txt | 1 + include/gazebo_coldgas_thruster_plugin.h | 100 +++++++++++++++++++++ src/gazebo_coldgas_thruster_plugin.cpp | 110 +++++++++++++++++++++++ 3 files changed, 211 insertions(+) create mode 100644 include/gazebo_coldgas_thruster_plugin.h create mode 100644 src/gazebo_coldgas_thruster_plugin.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 502ab760dd..8a38558aef 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -380,6 +380,7 @@ add_library(gazebo_parachute_plugin SHARED src/gazebo_parachute_plugin.cpp) add_library(gazebo_pose_sniffer_plugin SHARED src/gazebo_pose_sniffer_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) +add_library(gazebo_coldgas_thruster_plugin SHARED src/gazebo_coldgas_thruster_plugin.cpp) set(plugins gazebo_airspeed_plugin diff --git a/include/gazebo_coldgas_thruster_plugin.h b/include/gazebo_coldgas_thruster_plugin.h new file mode 100644 index 0000000000..11e0d1ca61 --- /dev/null +++ b/include/gazebo_coldgas_thruster_plugin.h @@ -0,0 +1,100 @@ +/**************************************************************************** + * + * Copyright (c) 2024 Jaeyoung Lim, Autonomous Systems Lab, ETH Zurich. + * 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 of the copyright holder 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. + * + ****************************************************************************/ + + +#include + +#include +#include +#include +#include +#include +#include +#include "CommandMotorSpeed.pb.h" +#include "gazebo/transport/transport.hh" +#include "gazebo/msgs/msgs.hh" +#include "MotorSpeed.pb.h" +#include "Float.pb.h" + +#include "common.h" + + +namespace gazebo { +// Default values +static const std::string kDefaultNamespace = ""; +static const std::string kDefaultCommandSubTopic = "/gazebo/command/motor_speed"; + +typedef const boost::shared_ptr CommandMotorSpeedPtr; + +static constexpr double kDefaultThrust = 1.4; + +class GazeboColdGasThrusterPlugin : public ModelPlugin { + public: + GazeboColdGasThrusterPlugin() + : ModelPlugin() { + } + + virtual ~GazeboColdGasThrusterPlugin(); + + virtual void InitializeParams(); + protected: + virtual void UpdateForcesAndMoments(const double &duty_cycle, const double &period); + virtual void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf); + virtual void OnUpdate(const common::UpdateInfo & /*_info*/); + + private: + std::string command_sub_topic_{kDefaultCommandSubTopic}; + std::string joint_name_; + std::string link_name_; + std::string namespace_; + + int motor_number_{0}; + + double max_thrust_{kDefaultThrust}; + double ref_duty_cycle_{0.0}; + double sampling_time_{0.0}; + double cycle_start_time_{0.0}; + double pwm_frequency_{10.0}; + + transport::NodePtr node_handle_; + transport::PublisherPtr motor_velocity_pub_; + transport::SubscriberPtr command_sub_; + + physics::ModelPtr model_; + physics::LinkPtr link_; + /// \brief Pointer to the update event connection. + event::ConnectionPtr updateConnection_; + void VelocityCallback(CommandMotorSpeedPtr &rot_velocities); +}; +} diff --git a/src/gazebo_coldgas_thruster_plugin.cpp b/src/gazebo_coldgas_thruster_plugin.cpp new file mode 100644 index 0000000000..800bbfbd69 --- /dev/null +++ b/src/gazebo_coldgas_thruster_plugin.cpp @@ -0,0 +1,110 @@ +/**************************************************************************** + * + * Copyright (c) 2024 Jaeyoung Lim, Autonomous Systems Lab, ETH Zurich. + * 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 of the copyright holder 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. + * + ****************************************************************************/ + + +#include "gazebo_coldgas_thruster_plugin.h" +#include + +namespace gazebo { + +GazeboColdGasThrusterPlugin::~GazeboColdGasThrusterPlugin() { + updateConnection_->~Connection(); +} + +void GazeboColdGasThrusterPlugin::InitializeParams() {} + +void GazeboColdGasThrusterPlugin::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf) { + model_ = _model; + + namespace_.clear(); + + if (_sdf->HasElement("robotNamespace")) + namespace_ = _sdf->GetElement("robotNamespace")->Get(); + else + gzerr << "[gazebo_motor_model] Please specify a robotNamespace.\n"; + node_handle_ = transport::NodePtr(new transport::Node()); + node_handle_->Init(namespace_); + + if (_sdf->HasElement("linkName")) + link_name_ = _sdf->GetElement("linkName")->Get(); + else + gzerr << "[gazebo_motor_model] Please specify a linkName of the rotor.\n"; + link_ = model_->GetLink(link_name_); + if (link_ == NULL) + gzthrow("[gazebo_motor_model] Couldn't find specified link \"" << link_name_ << "\"."); + + + if (_sdf->HasElement("motorNumber")) + motor_number_ = _sdf->GetElement("motorNumber")->Get(); + else + gzerr << "[gazebo_motor_model] Please specify a motorNumber.\n"; + getSdfParam(_sdf, "commandSubTopic", command_sub_topic_, command_sub_topic_); + getSdfParam(_sdf, "pwmFrequency", pwm_frequency_, 10.0); + getSdfParam(_sdf, "maxThrust", max_thrust_, 1.4); + + // Listen to the update event. This event is broadcast every + // simulation iteration. + updateConnection_ = event::Events::ConnectWorldUpdateBegin(boost::bind(&GazeboColdGasThrusterPlugin::OnUpdate, this, _1)); + + command_sub_ = node_handle_->Subscribe("~/" + model_->GetName() + command_sub_topic_, &GazeboColdGasThrusterPlugin::VelocityCallback, this); +} + +// This gets called by the world update start event. +void GazeboColdGasThrusterPlugin::OnUpdate(const common::UpdateInfo& _info) { + ///TODO: Compute duty cycle time + if (sampling_time_ >= 1.0/pwm_frequency_) { + cycle_start_time_ = _info.simTime.Double(); + } + sampling_time_ = _info.simTime.Double() - cycle_start_time_; + double period = sampling_time_ *pwm_frequency_; + ///TODO: Get duty cycle from control input + UpdateForcesAndMoments(ref_duty_cycle_, period); +} + +void GazeboColdGasThrusterPlugin::VelocityCallback(CommandMotorSpeedPtr &rot_velocities) { + if(rot_velocities->motor_speed_size() < motor_number_) { + std::cout << "You tried to access index " << motor_number_ + << " of the MotorSpeed message array which is of size " << rot_velocities->motor_speed_size() << "." << std::endl; + } else ref_duty_cycle_ = std::min(static_cast(rot_velocities->motor_speed(motor_number_)), 1.0); +} + +void GazeboColdGasThrusterPlugin::UpdateForcesAndMoments(const double &duty_cycle, const double &period) { + // Thrust is only generated uring the duty cycle + double force = duty_cycle > period ? max_thrust_ : 0.0; + // std::cout << "duty cycle: " << duty_cycle << " period: " << period << " thrust: " << force << std::endl; + link_->AddRelativeForce(ignition::math::Vector3d(0, 0, force)); +} + +GZ_REGISTER_MODEL_PLUGIN(GazeboColdGasThrusterPlugin); +}