Add HITL support for Fixed-wing, VTOL and Rover

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_

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,8 +147,10 @@ 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();
@@ -158,15 +164,18 @@ namespace mavlink_interface
       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];
+      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 +185,14 @@ 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};
+      int mc_motor_vel_scaling_{1000};
+      int fw_motor_vel_scaling_{3500};
 
       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};

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_;

src/gazebo_mavlink_interface.cpp

@@ -30,6 +30,7 @@
 #include <gz/sim/components/Magnetometer.hh>
 #include <gz/sim/components/Imu.hh>
 #include <gz/sim/components/Pose.hh>
+#include <gz/transport/Discovery.hh>
 
 GZ_ADD_PLUGIN(
     mavlink_interface::GazeboMavlinkInterface,
@@ -40,8 +41,8 @@ GZ_ADD_PLUGIN(
 using namespace mavlink_interface;
 
 GazeboMavlinkInterface::GazeboMavlinkInterface() :
-    input_scaling_ {},
-    input_index_ {}
+    motor_input_index_ {},
+    servo_input_index_ {}
     {
       mavlink_interface_ = std::make_shared<MavlinkInterface>();
     }
@@ -78,17 +79,14 @@ void GazeboMavlinkInterface::Configure(const gz::sim::Entity &_entity,
   gazebo::getSdfParam<std::string>(_sdf, "irlockSubTopic", irlock_sub_topic_, irlock_sub_topic_);
   gazebo::getSdfParam<std::string>(_sdf, "imuSubTopic", imu_sub_topic_, imu_sub_topic_);
   gazebo::getSdfParam<std::string>(_sdf, "magSubTopic", mag_sub_topic_, mag_sub_topic_);
+  gazebo::getSdfParam<std::string>(_sdf, "cmdVelSubTopic", cmd_vel_sub_topic_, cmd_vel_sub_topic_);
   gazebo::getSdfParam<std::string>(_sdf, "baroSubTopic", baro_sub_topic_, baro_sub_topic_);
+  gazebo::getSdfParam<int>(_sdf, "mc_motor_vel_scaling", mc_motor_vel_scaling_, mc_motor_vel_scaling_);
+  gazebo::getSdfParam<int>(_sdf, "fw_motor_vel_scaling", fw_motor_vel_scaling_, fw_motor_vel_scaling_);
 
-  // set input_reference_ from inputs.control
-  input_reference_.resize(n_out_max);
-
-  ///TODO: Parse input reference
-  input_scaling_.resize(n_out_max);
-  input_scaling_(0) = 1000;
-  input_scaling_(1) = 1000;
-  input_scaling_(2) = 1000;
-  input_scaling_(3) = 1000;
+  // set motor and servo input_reference_ from inputs.control
+  motor_input_reference_.resize(n_out_max);
+  servo_input_reference_.resize(n_out_max);
 
   bool use_tcp = false;
   if (_sdf->HasElement("use_tcp"))
@@ -103,7 +101,7 @@ void GazeboMavlinkInterface::Configure(const gz::sim::Entity &_entity,
     tcp_client_mode = _sdf->Get<bool>("tcp_client_mode");
     mavlink_interface_->SetUseTcpClientMode(tcp_client_mode);
   }
-  gzmsg << "Connecting to PX4 SITL using " << (use_tcp ? (tcp_client_mode ? "TCP (client mode)" : "TCP (server mode)") : "UDP") << std::endl;
+  gzmsg << "Connecting to PX4 HITL using " << (use_tcp ? (tcp_client_mode ? "TCP (client mode)" : "TCP (server mode)") : "UDP") << std::endl;
 
   if (_sdf->HasElement("enable_lockstep"))
   {
@@ -134,9 +132,21 @@ void GazeboMavlinkInterface::Configure(const gz::sim::Entity &_entity,
 
   auto world_name = "/" + gz::sim::scopedName(gz::sim::worldEntity(_ecm), _ecm);
 
-  auto vehicle_scope_prefix = world_name + gz::sim::topicFromScopedName(
+  auto model_name = gz::sim::topicFromScopedName(
     _ecm.EntityByComponents(gz::sim::components::Name(model_name_)), _ecm, false);
 
+  auto vehicle_scope_prefix = world_name + model_name;
+
+  // Publish to servo control
+  auto servo_control_topic = model_name + "/servo_";
+  for (int i = 0; i < servo_input_reference_.size(); i++) {
+    servo_control_pub_[i] = node.Advertise<gz::msgs::Double>(servo_control_topic + std::to_string(i));
+  }
+
+  // Publish to cmd vel (for rover control)
+  auto cmd_vel_topic = model_name + cmd_vel_sub_topic_;
+  cmd_vel_pub_ = node.Advertise<gz::msgs::Twist>(cmd_vel_topic);
+
   // Subscribe to messages of sensors.
   auto imu_topic = vehicle_scope_prefix + imu_sub_topic_;
   node.Subscribe(imu_topic, &GazeboMavlinkInterface::ImuCallback, this);
@@ -241,7 +251,12 @@ void GazeboMavlinkInterface::PreUpdate(const gz::sim::UpdateInfo &_info,
   handle_control(dt);
 
   if (received_first_actuator_) {
-    PublishRotorVelocities(_ecm, input_reference_);
+    if (input_is_cmd_vel_) {
+      PublishCmdVelocities(cmd_vel_thrust_, cmd_vel_torque_);
+    } else {
+      PublishMotorVelocities(_ecm, motor_input_reference_);
+      PublishServoVelocities(servo_input_reference_);
+    }
   }
 }
 
@@ -417,30 +432,77 @@ void GazeboMavlinkInterface::handle_actuator_controls(const gz::sim::UpdateInfo
 
   last_actuator_time_ = _info.simTime;
 
-  for (unsigned i = 0; i < n_out_max; i++) {
-    input_index_[i] = i;
-  }
-  // Read Input References
-  input_reference_.resize(n_out_max);
-
   Eigen::VectorXd actuator_controls = mavlink_interface_->GetActuatorControls();
   if (actuator_controls.size() < n_out_max) return; //TODO: Handle this properly
 
-  for (int i = 0; i < input_reference_.size(); i++) {
+  // Read Cmd vel input for rover
+  if (actuator_controls[n_out_max - 1] != 0.0 || actuator_controls[n_out_max - 2] != 0.0) {
+    cmd_vel_thrust_ = armed ? actuator_controls[n_out_max - 1] : 0.0;
+    cmd_vel_torque_ = armed ? actuator_controls[n_out_max - 2] : 0.0;
+    input_is_cmd_vel_ = true;
+    received_first_actuator_ = mavlink_interface_->GetReceivedFirstActuator();
+    return;
+  } else {
+    input_is_cmd_vel_ = false;
+  }
+
+  // Read Input References for servos
+  if (servo_input_reference_.size() == n_out_max) {
+    unsigned n_servos = 0;
+    for (unsigned i = 0; i < n_out_max; i++) {
+      if (!mavlink_interface_->IsInputMotorAtIndex(i)) {
+        servo_input_index_[n_servos++] = i;
+      }
+    }
+    servo_input_reference_.resize(n_servos);
+  }
+
+  for (int i = 0; i < servo_input_reference_.size(); i++) {
+    if (armed) {
+      servo_input_reference_[i] = actuator_controls[servo_input_index_[i]];
+    } else {
+      servo_input_reference_[i] = 0;
+    }
+  }
+
+  // Read Input References for motors
+  if (motor_input_reference_.size() == n_out_max) {
+    unsigned n_motors = 0;
+    for (unsigned i = 0; i < n_out_max; i++) {
+      if (mavlink_interface_->IsInputMotorAtIndex(i)) {
+        motor_input_index_[n_motors++] = i;
+      }
+    }
+    motor_input_reference_.resize(n_motors);
+  }
+
+  for (int i = 0; i < motor_input_reference_.size(); i++) {
     if (armed) {
-      input_reference_[i] = actuator_controls[input_index_[i]] * input_scaling_(i);
+      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<double>(fw_motor_vel_scaling_);
+        } else {
+          scaling = static_cast<double>(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<double>(mc_motor_vel_scaling_);
+      }
     } else {
-      input_reference_[i] = 0;
-      // std::cout << input_reference_ << ", ";
+      motor_input_reference_[i] = 0;
     }
   }
+
   received_first_actuator_ = mavlink_interface_->GetReceivedFirstActuator();
 }
 
 void GazeboMavlinkInterface::handle_control(double _dt)
 {
   // set joint positions
-  // for (int i = 0; i < input_reference_.size(); i++) {
+  // for (int i = 0; i < motor_input_reference_.size(); i++) {
   // }
 }
 
@@ -454,17 +516,17 @@ void GazeboMavlinkInterface::onSigInt() {
 }
 
 // The following snippet was copied from https://github.com/gzrobotics/ign-gazebo/blob/ign-gazebo4/src/systems/multicopter_control/MulticopterVelocityControl.cc
-void GazeboMavlinkInterface::PublishRotorVelocities(
+void GazeboMavlinkInterface::PublishMotorVelocities(
     gz::sim::EntityComponentManager &_ecm,
     const Eigen::VectorXd &_vels)
 {
-  if (_vels.size() != rotor_velocity_message_.velocity_size())
+  if (_vels.size() != motor_velocity_message_.velocity_size())
   {
-    rotor_velocity_message_.mutable_velocity()->Resize(_vels.size(), 0);
+    motor_velocity_message_.mutable_velocity()->Resize(_vels.size(), 0);
   }
   for (int i = 0; i < _vels.size(); ++i)
   {
-    rotor_velocity_message_.set_velocity(i, _vels(i));
+    motor_velocity_message_.set_velocity(i, _vels(i));
   }
   // Publish the message by setting the Actuators component on the model entity.
   // This assumes that the MulticopterMotorModel system is attached to this
@@ -479,15 +541,35 @@ void GazeboMavlinkInterface::PublishRotorVelocities(
       return std::equal(_a.velocity().begin(), _a.velocity().end(),
                         _b.velocity().begin());
     };
-    auto state = actuatorMsgComp->SetData(this->rotor_velocity_message_, compFunc)
+    auto state = actuatorMsgComp->SetData(this->motor_velocity_message_, compFunc)
                      ? gz::sim::ComponentState::PeriodicChange
                      : gz::sim::ComponentState::NoChange;
     _ecm.SetChanged(model_.Entity(), gz::sim::components::Actuators::typeId, state);
   }
   else
   {
     _ecm.CreateComponent(model_.Entity(),
-                         gz::sim::components::Actuators(this->rotor_velocity_message_));
+                         gz::sim::components::Actuators(this->motor_velocity_message_));
+  }
+}
+
+void GazeboMavlinkInterface::PublishServoVelocities(const Eigen::VectorXd &_vels)
+{
+  for (int i = 0; i < _vels.size(); i++) {
+    gz::msgs::Double servo_input;
+    servo_input.set_data(_vels(i));
+    servo_control_pub_[i].Publish(servo_input);
+  }
+}
+
+void GazeboMavlinkInterface::PublishCmdVelocities(const float _thrust, const float _torque)
+{
+  gz::msgs::Twist cmd_vel_message;
+  cmd_vel_message.mutable_linear()->set_x(_thrust);
+  cmd_vel_message.mutable_angular()->set_z(_torque);
+
+  if (cmd_vel_pub_.Valid()) {
+    cmd_vel_pub_.Publish(cmd_vel_message);
   }
 }
 

src/mavlink_interface.cpp

@@ -97,13 +97,13 @@ void MavlinkInterface::Load()
       fds_[LISTEN_FD].events = POLLIN; // only listens for new connections on tcp
     }
   } else {
-    // When connecting to SITL, we specify the port where the mavlink traffic originates from.
+    // When connecting to HITL, we specify the port where the mavlink traffic originates from.
     remote_simulator_addr_.sin_addr.s_addr = mavlink_addr_;
     remote_simulator_addr_.sin_port = htons(mavlink_udp_remote_port_);
     local_simulator_addr_.sin_addr.s_addr = htonl(INADDR_ANY);
     local_simulator_addr_.sin_port = htons(mavlink_udp_local_port_);
 
-    std::cout << "Creating UDP socket for SITL input on local port : " << mavlink_udp_local_port_ << " and remote port " << mavlink_udp_remote_port_ << std::endl;
+    std::cout << "Creating UDP socket for HITL input on local port : " << mavlink_udp_local_port_ << " and remote port " << mavlink_udp_remote_port_ << std::endl;
 
     if ((simulator_socket_fd_ = socket(AF_INET, SOCK_DGRAM, 0)) < 0) {
       std::cerr << "Creating UDP socket failed: " << strerror(errno) << ", aborting" << std::endl;
@@ -474,15 +474,12 @@ void MavlinkInterface::handle_actuator_controls(mavlink_message_t *msg)
   mavlink_hil_actuator_controls_t controls;
   mavlink_msg_hil_actuator_controls_decode(msg, &controls);
 
-  armed_ = (controls.mode & MAV_MODE_FLAG_SAFETY_ARMED);
+  armed_ = (controls.mode & MAV_MODE_FLAG_SAFETY_ARMED || controls.mode & MAV_MODE_FLAG_TEST_ENABLED);
 
-  for (unsigned i = 0; i < n_out_max; i++) {
-    input_index_[i] = i;
-  }
-
-  // set rotor speeds, controller targets
+  // set rotor and servo speeds, controller targets
   input_reference_.resize(n_out_max);
   for (int i = 0; i < input_reference_.size(); i++) {
+    input_is_motor_[i] = controls.flags & (1 << i);
     input_reference_[i] = controls.controls[i];
   }
   received_actuator_ = true;
@@ -553,6 +550,11 @@ Eigen::VectorXd MavlinkInterface::GetActuatorControls() {
   return input_reference_;
 }
 
+bool MavlinkInterface::IsInputMotorAtIndex(int index) {
+  const std::lock_guard<std::mutex> lock(actuator_mutex_);
+  return input_is_motor_[index];
+}
+
 bool MavlinkInterface::GetArmedState() {
   const std::lock_guard<std::mutex> lock(actuator_mutex_);
   return armed_;