Merge pull request #9 from tiiuae/DP-9681_HITL-vtol-fw-rover-support

Add HITL support for Fixed-wing, VTOL and Rover
tiiuae · Sep 26, 2024 · 06b5c16 · 06b5c16
2 parents f2da227 + 7c4bdeb
commit 06b5c16
Show file tree

Hide file tree

Showing 5 changed files with 183 additions and 63 deletions.
diff --git a/include/common.h b/include/common.h
@@ -1,5 +1,5 @@
-#ifndef SITL_GAZEBO_COMMON_H_
-#define SITL_GAZEBO_COMMON_H_
+#ifndef HITL_GAZEBO_COMMON_H_
+#define HITL_GAZEBO_COMMON_H_
 /*
  * Copyright 2015 Fadri Furrer, ASL, ETH Zurich, Switzerland
  * Copyright 2015 Michael Burri, ASL, ETH Zurich, Switzerland
@@ -218,4 +218,4 @@ inline std::pair<double, double> reproject(gz::math::Vector3d& pos,
   return std::make_pair (lat_rad, lon_rad);
 }
 
-#endif  // SITL_GAZEBO_COMMON_H_
+#endif  // HITL_GAZEBO_COMMON_H_
diff --git a/include/gazebo_mavlink_interface.h b/include/gazebo_mavlink_interface.h
@@ -95,6 +95,7 @@ static const std::string kDefaultGPSTopic = "/gps";
 static const std::string kDefaultVisionTopic = "/vision_odom";
 static const std::string kDefaultMagTopic = "/magnetometer";
 static const std::string kDefaultBarometerTopic = "/air_pressure";
+static const std::string kDefaultCmdVelTopic = "/cmd_vel";
 
 namespace mavlink_interface
 {
@@ -119,7 +120,10 @@ namespace mavlink_interface
       gz::common::ConnectionPtr sigIntConnection_;
       std::shared_ptr<MavlinkInterface> mavlink_interface_;
       bool received_first_actuator_{false};
-      Eigen::VectorXd input_reference_;
+      Eigen::VectorXd motor_input_reference_;
+      Eigen::VectorXd servo_input_reference_;
+      float cmd_vel_thrust_{0.0};
+      float cmd_vel_torque_{0.0};
 
       gz::sim::Entity entity_{gz::sim::kNullEntity};
       gz::sim::Model model_{gz::sim::kNullEntity};
@@ -143,30 +147,36 @@ namespace mavlink_interface
       void MagnetometerCallback(const gz::msgs::Magnetometer &_msg);
       void GpsCallback(const gz::msgs::NavSat &_msg);
       void SendSensorMessages(const gz::sim::UpdateInfo &_info);
-      void PublishRotorVelocities(gz::sim::EntityComponentManager &_ecm,
+      void PublishMotorVelocities(gz::sim::EntityComponentManager &_ecm,
           const Eigen::VectorXd &_vels);
+      void PublishServoVelocities(const Eigen::VectorXd &_vels);
+      void PublishCmdVelocities(const float _thrust, const float _torque);
       void handle_actuator_controls(const gz::sim::UpdateInfo &_info);
-      void handle_control(double _dt);
       void onSigInt();
       bool IsRunning();
       bool resolveHostName();
       void ResolveWorker();
       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;
 
       double input_offset_[n_out_max];
-      Eigen::VectorXd input_scaling_;
       std::string joint_control_type_[n_out_max];
       std::string gztopic_[n_out_max];
       double zero_position_disarmed_[n_out_max];
       double zero_position_armed_[n_out_max];
-      int input_index_[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};
 
-      /// \brief gz communication node.
+      /// \brief gz communication node and publishers.
       gz::transport::Node node;
+      gz::transport::Node::Publisher servo_control_pub_[n_out_max];
+      gz::transport::Node::Publisher cmd_vel_pub_;
 
       std::string pose_sub_topic_{kDefaultPoseTopic};
       std::string imu_sub_topic_{kDefaultImuTopic};
@@ -176,11 +186,12 @@ namespace mavlink_interface
       std::string vision_sub_topic_{kDefaultVisionTopic};
       std::string mag_sub_topic_{kDefaultMagTopic};
       std::string baro_sub_topic_{kDefaultBarometerTopic};
+      std::string cmd_vel_sub_topic_{kDefaultCmdVelTopic};
 
       std::mutex last_imu_message_mutex_ {};
 
       gz::msgs::IMU last_imu_message_;
-      gz::msgs::Actuators rotor_velocity_message_;
+      gz::msgs::Actuators motor_velocity_message_;
 
       std::chrono::steady_clock::duration last_imu_time_{0};
       std::chrono::steady_clock::duration lastControllerUpdateTime{0};

diff --git a/include/mavlink_interface.h b/include/mavlink_interface.h
@@ -148,6 +148,7 @@ class MavlinkInterface {
     void UpdateIMU(const SensorData::Imu &data);
     void UpdateMag(const SensorData::Magnetometer &data);
     Eigen::VectorXd GetActuatorControls();
+    bool IsInputMotorAtIndex(int index);
     bool GetArmedState();
     void onSigInt();
     uint16_t FinalizeOutgoingMessage(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id,
@@ -186,7 +187,7 @@ class MavlinkInterface {
 
     static const unsigned n_out_max = 16;
 
-    int input_index_[n_out_max];
+    bool input_is_motor_[n_out_max];
 
     struct sockaddr_in local_simulator_addr_;
     socklen_t local_simulator_addr_len_;