Spacecrafts and Thrusters Plugin

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

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 <stdio.h>
+
+#include <boost/bind.hpp>
+#include <Eigen/Eigen>
+#include <gazebo/gazebo.hh>
+#include <gazebo/physics/physics.hh>
+#include <gazebo/common/common.hh>
+#include <gazebo/common/Plugin.hh>
+#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/actuator_outputs";
+
+typedef const boost::shared_ptr<const mav_msgs::msgs::CommandMotorSpeed> 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 &ref_duty_cycle_, const double &sampling_time_);
+  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);
+};
+}

include/gazebo_mavlink_interface.h

@@ -106,6 +106,7 @@ static const std::string kDefaultNamespace = "";
 // This just proxies the motor commands from command/motor_speed to the single motors via internal
 // ConsPtr passing, such that the original commands don't have to go n_motors-times over the wire.
 static const std::string kDefaultMotorVelocityReferencePubTopic = "/gazebo/command/motor_speed";
+static const std::string kDefaultActuatorOutputsReferencePubTopic = "/gazebo/command/actuator_outputs";
 
 static const std::string kDefaultImuTopic = "/imu";
 static const std::string kDefaultOpticalFlowTopic = "/px4flow/link/opticalFlow";
@@ -145,6 +146,7 @@ class GazeboMavlinkInterface : public ModelPlugin {
   void OnUpdate(const common::UpdateInfo&  /*_info*/);
 
 private:
+  bool pub_actuator_outputs_{false};
   bool received_first_actuator_{false};
   Eigen::VectorXd input_reference_;
 
@@ -154,11 +156,13 @@ class GazeboMavlinkInterface : public ModelPlugin {
 
   std::string namespace_{kDefaultNamespace};
   std::string motor_velocity_reference_pub_topic_{kDefaultMotorVelocityReferencePubTopic};
+  std::string actuator_outputs_pub_topic_{kDefaultMotorVelocityReferencePubTopic};
   std::string mavlink_control_sub_topic_;
   std::string link_name_;
 
   transport::NodePtr node_handle_;
   transport::PublisherPtr motor_velocity_reference_pub_;
+  transport::PublisherPtr actuator_outputs_reference_pub_;
   transport::SubscriberPtr mav_control_sub_;
 
   physics::ModelPtr model_{};