diff --git a/include/common.h b/include/common.h
index eb84b26..e63e8ce 100644
--- 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
index 0db49ce..8c3964d 100644
--- 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
 {
@@ -147,6 +148,7 @@ namespace mavlink_interface
       void PublishMotorVelocities(gz::sim::EntityComponentManager &_ecm,
           const Eigen::VectorXd &_vels);
       void PublishServoVelocities(const Eigen::VectorXd &_vels);
+      void PublishCmdVelocities(const Eigen::VectorXd &_vels);
       void handle_actuator_controls(const gz::sim::UpdateInfo &_info);
       void handle_control(double _dt);
       void onSigInt();
@@ -166,10 +168,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 +183,7 @@ 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_ {};
 
diff --git a/src/gazebo_mavlink_interface.cpp b/src/gazebo_mavlink_interface.cpp
index 41dedd7..3caf8fa 100644
--- a/src/gazebo_mavlink_interface.cpp
+++ b/src/gazebo_mavlink_interface.cpp
@@ -80,6 +80,7 @@ void GazeboMavlinkInterface::Configure(const gz::sim::Entity &_entity,
   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, "baroSubTopic", baro_sub_topic_, baro_sub_topic_);
+  gazebo::getSdfParam<std::string>(_sdf, "cmdVelSubTopic", cmd_vel_sub_topic_, cmd_vel_sub_topic_);
 
   // set motor and servo input_reference_ from inputs.control
   motor_input_reference_.resize(n_out_max);
@@ -98,7 +99,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 +141,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 +249,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(motor_input_reference_);
+    } else {
+      PublishMotorVelocities(_ecm, motor_input_reference_);
+      PublishServoVelocities(servo_input_reference_);
+    }
   }
 }
 
@@ -424,14 +433,28 @@ 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 Cmd vel input for rover
+  if ((actuator_controls[n_out_max - 1] - 0.0) > 1e-6 || (actuator_controls[n_out_max - 2] - 0.0) > 1e-6) {
+    motor_input_reference_[n_out_max - 1] = actuator_controls[n_out_max - 1];
+    motor_input_reference_[n_out_max - 2] = actuator_controls[n_out_max - 2];
+    input_is_cmd_vel_ = true;
+    received_first_actuator_ = mavlink_interface_->GetReceivedFirstActuator();
+    return;
+  } else {
+    input_is_cmd_vel_ = false;
+  }
+
   // 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;
+  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);
   }
-  motor_input_reference_.resize(n_motors);
+
   for (int i = 0; i < motor_input_reference_.size(); i++) {
     if (armed) {
       motor_input_reference_[i] = actuator_controls[motor_input_index_[i]] * 1000;
@@ -442,13 +465,16 @@ void GazeboMavlinkInterface::handle_actuator_controls(const gz::sim::UpdateInfo
   }
 
   // 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;
+  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);
   }
-  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]];
@@ -515,7 +541,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 +550,17 @@ void GazeboMavlinkInterface::PublishServoVelocities(const Eigen::VectorXd &_vels
   }
 }
 
+void GazeboMavlinkInterface::PublishCmdVelocities(const Eigen::VectorXd &_vels)
+{
+  gz::msgs::Twist cmd_vel_message;
+  cmd_vel_message.mutable_linear()->set_x(_vels[n_out_max - 1]);
+  cmd_vel_message.mutable_angular()->set_z(_vels[n_out_max - 2]);
+
+  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
index 2f1d23e..d76b947 100644
--- 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);