Add support for cmd_vel control for rover

tiiuae · Aug 28, 2024 · 7ae6aea · 7ae6aea
1 parent a22d53e
commit 7ae6aea
Show file tree

Hide file tree

Showing 4 changed files with 88 additions and 31 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
 {
@@ -121,6 +122,8 @@ namespace mavlink_interface
       bool received_first_actuator_{false};
       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};
@@ -147,6 +150,7 @@ namespace mavlink_interface
       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();
@@ -166,10 +170,12 @@ namespace mavlink_interface
       double zero_position_armed_[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};
@@ -179,6 +185,9 @@ 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_ {};
 

diff --git a/src/gazebo_mavlink_interface.cpp b/src/gazebo_mavlink_interface.cpp
@@ -79,7 +79,10 @@ 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 motor and servo input_reference_ from inputs.control
   motor_input_reference_.resize(n_out_max);
@@ -98,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"))
   {
@@ -140,6 +143,10 @@ void GazeboMavlinkInterface::Configure(const gz::sim::Entity &_entity,
     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);
@@ -244,8 +251,12 @@ void GazeboMavlinkInterface::PreUpdate(const gz::sim::UpdateInfo &_info,
   handle_control(dt);
 
   if (received_first_actuator_) {
-    PublishMotorVelocities(_ecm, motor_input_reference_);
-    PublishServoVelocities(servo_input_reference_);
+    if (input_is_cmd_vel_) {
+      PublishCmdVelocities(cmd_vel_thrust_, cmd_vel_torque_);
+    } else {
+      PublishMotorVelocities(_ecm, motor_input_reference_);
+      PublishServoVelocities(servo_input_reference_);
+    }
   }
 }
 
@@ -424,37 +435,64 @@ void GazeboMavlinkInterface::handle_actuator_controls(const gz::sim::UpdateInfo
   Eigen::VectorXd actuator_controls = mavlink_interface_->GetActuatorControls();
   if (actuator_controls.size() < n_out_max) return; //TODO: Handle this properly
 
-  // Read Input References for motors
-  unsigned n_motors = 0;
-  for (unsigned i = 0; i < n_out_max; i++) {
-    if (mavlink_interface_->IsInputMotorAtIndex(i)) {
-      motor_input_index_[n_motors++] = 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);
   }
-  motor_input_reference_.resize(n_motors);
-  for (int i = 0; i < motor_input_reference_.size(); i++) {
+
+  for (int i = 0; i < servo_input_reference_.size(); i++) {
     if (armed) {
-      motor_input_reference_[i] = actuator_controls[motor_input_index_[i]] * 1000;
+      servo_input_reference_[i] = actuator_controls[servo_input_index_[i]];
     } else {
-      motor_input_reference_[i] = 0;
-      // std::cout << motor_input_reference_ << ", ";
+      servo_input_reference_[i] = 0;
     }
   }
 
-  // Read Input References for servos
-  unsigned n_servos = 0;
-  for (unsigned i = 0; i < n_out_max; i++) {
-    if (!mavlink_interface_->IsInputMotorAtIndex(i)) {
-      servo_input_index_[n_servos++] = i;
+  // 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);
   }
-  servo_input_reference_.resize(n_servos);
-  for (int i = 0; i < servo_input_reference_.size(); i++) {
+
+  for (int i = 0; i < motor_input_reference_.size(); i++) {
     if (armed) {
-      servo_input_reference_[i] = actuator_controls[servo_input_index_[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 {
-      servo_input_reference_[i] = 0;
-      // std::cout << servo_input_reference_ << ", ";
+      motor_input_reference_[i] = 0;
     }
   }
 
@@ -515,7 +553,6 @@ void GazeboMavlinkInterface::PublishMotorVelocities(
   }
 }
 
-// Publish to servo control topic (gz.msgs.Double)
 void GazeboMavlinkInterface::PublishServoVelocities(const Eigen::VectorXd &_vels)
 {
   for (int i = 0; i < _vels.size(); i++) {
@@ -525,6 +562,17 @@ void GazeboMavlinkInterface::PublishServoVelocities(const Eigen::VectorXd &_vels
   }
 }
 
+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);
+  }
+}
+
 bool GazeboMavlinkInterface::resolveHostName()
 {
   if (!mavlink_hostname_str_.empty()) {

diff --git a/src/mavlink_interface.cpp b/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,7 +474,7 @@ 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);
 
   // set rotor and servo speeds, controller targets
   input_reference_.resize(n_out_max);