diff --git a/nav2_controller/include/nav2_controller/nav2_controller.hpp b/nav2_controller/include/nav2_controller/nav2_controller.hpp
index f59b65b999..3b6479de97 100644
--- a/nav2_controller/include/nav2_controller/nav2_controller.hpp
+++ b/nav2_controller/include/nav2_controller/nav2_controller.hpp
@@ -68,7 +68,7 @@ class ControllerServer : public nav2_util::LifecycleNode
    * @throw pluginlib::PluginlibException When failed to initialize controller
    * plugin
    */
-  nav2_util::CallbackReturn on_configure(const rclcpp_lifecycle::State & state) override;
+  virtual nav2_util::CallbackReturn on_configure(const rclcpp_lifecycle::State & state);
   /**
    * @brief Activates member variables
    *
@@ -77,7 +77,7 @@ class ControllerServer : public nav2_util::LifecycleNode
    * @param state LifeCycle Node's state
    * @return Success or Failure
    */
-  nav2_util::CallbackReturn on_activate(const rclcpp_lifecycle::State & state) override;
+  virtual nav2_util::CallbackReturn on_activate(const rclcpp_lifecycle::State & state);
   /**
    * @brief Deactivates member variables
    *
@@ -86,7 +86,7 @@ class ControllerServer : public nav2_util::LifecycleNode
    * @param state LifeCycle Node's state
    * @return Success or Failure
    */
-  nav2_util::CallbackReturn on_deactivate(const rclcpp_lifecycle::State & state) override;
+  virtual nav2_util::CallbackReturn on_deactivate(const rclcpp_lifecycle::State & state);
   /**
    * @brief Calls clean up states and resets member variables.
    *
@@ -95,13 +95,13 @@ class ControllerServer : public nav2_util::LifecycleNode
    * @param state LifeCycle Node's state
    * @return Success or Failure
    */
-  nav2_util::CallbackReturn on_cleanup(const rclcpp_lifecycle::State & state) override;
+  virtual nav2_util::CallbackReturn on_cleanup(const rclcpp_lifecycle::State & state);
   /**
    * @brief Called when in Shutdown state
    * @param state LifeCycle Node's state
    * @return Success or Failure
    */
-  nav2_util::CallbackReturn on_shutdown(const rclcpp_lifecycle::State & state) override;
+  virtual nav2_util::CallbackReturn on_shutdown(const rclcpp_lifecycle::State & state);
 
   using Action = nav2_msgs::action::FollowPath;
   using ActionServer = nav2_util::SimpleActionServer<Action>;
@@ -133,11 +133,11 @@ class ControllerServer : public nav2_util::LifecycleNode
    * @brief Assigns path to controller
    * @param path Path received from action server
    */
-  void setPlannerPath(const nav_msgs::msg::Path & path);
+  virtual void setPlannerPath(const nav_msgs::msg::Path & path);
   /**
    * @brief Calculates velocity and publishes to "cmd_vel" topic
    */
-  void computeAndPublishVelocity();
+  virtual void computeAndPublishVelocity();
   /**
    * @brief Calls setPlannerPath method with an updated path received from
    * action server
@@ -156,13 +156,13 @@ class ControllerServer : public nav2_util::LifecycleNode
    * @brief Checks if goal is reached
    * @return true or false
    */
-  bool isGoalReached();
+  virtual bool isGoalReached();
   /**
    * @brief Obtain current pose of the robot
    * @param pose To store current pose of the robot
    * @return true if able to obtain current pose of the robot, else false
    */
-  bool getRobotPose(geometry_msgs::msg::PoseStamped & pose);
+  virtual bool getRobotPose(geometry_msgs::msg::PoseStamped & pose);
 
   /**
    * @brief get the thresholded velocity
@@ -195,7 +195,7 @@ class ControllerServer : public nav2_util::LifecycleNode
 
   // Publishers and subscribers
   std::unique_ptr<nav_2d_utils::OdomSubscriber> odom_sub_;
-  rclcpp_lifecycle::LifecyclePublisher<geometry_msgs::msg::Twist>::SharedPtr vel_publisher_;
+  rclcpp_lifecycle::LifecyclePublisher<geometry_msgs::msg::TwistStamped>::SharedPtr vel_publisher_;
 
   // Progress Checker Plugin
   pluginlib::ClassLoader<nav2_core::ProgressChecker> progress_checker_loader_;
diff --git a/nav2_controller/src/nav2_controller.cpp b/nav2_controller/src/nav2_controller.cpp
index dd938c4dbc..26738b8e39 100644
--- a/nav2_controller/src/nav2_controller.cpp
+++ b/nav2_controller/src/nav2_controller.cpp
@@ -160,7 +160,7 @@ ControllerServer::on_configure(const rclcpp_lifecycle::State & state)
     "Controller Server has %s controllers available.", controller_ids_concat_.c_str());
 
   odom_sub_ = std::make_unique<nav_2d_utils::OdomSubscriber>(node);
-  vel_publisher_ = create_publisher<geometry_msgs::msg::Twist>("cmd_vel", 1);
+  vel_publisher_ = create_publisher<geometry_msgs::msg::TwistStamped>("cmd_vel", 1);
 
   // Create the action server that we implement with our followPath method
   action_server_ = std::make_unique<ActionServer>(
@@ -394,12 +394,9 @@ void ControllerServer::updateGlobalPath()
 
 void ControllerServer::publishVelocity(const geometry_msgs::msg::TwistStamped & velocity)
 {
-  auto cmd_vel = std::make_unique<geometry_msgs::msg::Twist>(velocity.twist);
-  if (
-    vel_publisher_->is_activated() &&
-    this->count_subscribers(vel_publisher_->get_topic_name()) > 0)
-  {
-    vel_publisher_->publish(std::move(cmd_vel));
+  // Updated(Sergi): We need TwistStamped instead of Twist because Copter HAL uses frames for TF
+  if (vel_publisher_->is_activated() && vel_publisher_->get_subscription_count() > 0) {
+    vel_publisher_->publish(velocity);
   }
 }