diff --git a/CMakeLists.txt b/CMakeLists.txt
index 4e6e47a..949e67c 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -13,7 +13,6 @@ find_package(catkin REQUIRED COMPONENTS
   message_generation
   cmake_modules
   moveit_ros_planning_interface
-  move_base_msgs
   geometry_msgs
   std_msgs
   tf2
@@ -33,6 +32,7 @@ find_package(class_loader REQUIRED)
 
 add_message_files(FILES
   CustomFeedback.msg
+  NavigationFeedback.msg
 )
 
 add_service_files(FILES
@@ -48,8 +48,9 @@ add_service_files(FILES
   RobotGetTarget.srv
   RobotGetNamedTargets.srv
   RobotGetConfig.srv
-  RobotNavigationGoal.srv
+  NavigationGoal.srv
   RobotGripperControlPosition.srv
+  CancelNavigationGoal.srv
 )
 
 generate_messages(DEPENDENCIES
@@ -85,6 +86,7 @@ add_executable(temoto_robot_manager
   src/robot_features.cpp
   src/robot_common_procedures.cpp
   src/custom_plugin_helper.cpp
+  src/navigation_plugin_helper.cpp
 )
 add_dependencies(temoto_robot_manager ${catkin_EXPORTED_TARGETS} ${${PROJECT_NAME}_EXPORTED_TARGETS})
 target_link_libraries(temoto_robot_manager 
diff --git a/include/temoto_robot_manager/custom_datastructures.h b/include/temoto_robot_manager/custom_datastructures.h
index 607fbc2..86346db 100644
--- a/include/temoto_robot_manager/custom_datastructures.h
+++ b/include/temoto_robot_manager/custom_datastructures.h
@@ -1,54 +1,22 @@
 #ifndef TEMOTO_ROBOT_MANAGER__CUSTOM_DATASTRUCTURES_H
 #define TEMOTO_ROBOT_MANAGER__CUSTOM_DATASTRUCTURES_H
 
-#include <string>
-#include <vector>
+#include "temoto_robot_manager/rm_datastructures.h"
 
 namespace temoto_robot_manager
 {
 
 struct RmCustomRequest
 {
-  struct Header
-  {
-    std::string frame_id;
-    uint64_t timestamp;
-    uint64_t sequence_id;
-  };
-
-  struct Position
-  {
-    double x;
-    double y;
-    double z;
-  };
-
-  struct Orientation
-  {
-    double x;
-    double y;
-    double z;
-    double w;
-  };
-
-  struct Pose
-  {
-    Position position;
-    Orientation orientation;
-  }; 
-
-  struct PoseStamped
-  {
-    Header header;
-    Pose pose;
-  };
-
+  Header header;
+  Position position;  
+  Orientation orientation;
+  Pose pose;
+  PoseStamped poseStamped;
   std::string data_str;
   std::vector<std::string> data_str_array;
-
   double data_num;
   std::vector<double> data_num_array;
-
   PoseStamped data_pose;
   std::vector<PoseStamped> data_pose_array;
 };
diff --git a/include/temoto_robot_manager/navigation_datastructures.h b/include/temoto_robot_manager/navigation_datastructures.h
new file mode 100644
index 0000000..47c07ea
--- /dev/null
+++ b/include/temoto_robot_manager/navigation_datastructures.h
@@ -0,0 +1,23 @@
+#ifndef TEMOTO_ROBOT_MANAGER__NAVIGATION_DATASTRUCTURES_H
+#define TEMOTO_ROBOT_MANAGER__NAVIGATION_DATASTRUCTURES_H
+
+#include "temoto_robot_manager/rm_datastructures.h"
+
+namespace temoto_robot_manager
+{
+
+struct RmNavigationGoal
+{
+  PoseStamped goal_pose;
+};
+
+struct RmNavigationFeedback
+{
+  uint8_t status;
+  double progress;
+  PoseStamped base_position;
+};
+
+} // temoto_robot_manager
+
+#endif
\ No newline at end of file
diff --git a/include/temoto_robot_manager/navigation_plugin_base.h b/include/temoto_robot_manager/navigation_plugin_base.h
new file mode 100644
index 0000000..64914ae
--- /dev/null
+++ b/include/temoto_robot_manager/navigation_plugin_base.h
@@ -0,0 +1,23 @@
+#ifndef TEMOTO_ROBOT_MANAGER__NAVIGATION_PLUGIN_BASE_H
+#define TEMOTO_ROBOT_MANAGER__NAVIGATION_PLUGIN_BASE_H
+
+#include "temoto_robot_manager/navigation_datastructures.h"
+#include <optional>
+
+namespace temoto_robot_manager
+{
+
+class NavigationPluginBase
+{
+public:
+  virtual bool initialize(const std::string& robot_ns) = 0;
+  virtual bool sendGoal(RmNavigationGoal goal) = 0;
+  virtual std::optional<RmNavigationFeedback> getFeedback() = 0;
+  virtual bool cancelGoal() = 0;
+  virtual bool deinitialize() = 0;
+  virtual ~NavigationPluginBase(){};
+};
+
+} // temoto_robot_manager
+
+#endif
\ No newline at end of file
diff --git a/include/temoto_robot_manager/navigation_plugin_helper.h b/include/temoto_robot_manager/navigation_plugin_helper.h
new file mode 100644
index 0000000..6211669
--- /dev/null
+++ b/include/temoto_robot_manager/navigation_plugin_helper.h
@@ -0,0 +1,69 @@
+#ifndef TEMOTO_ROBOT_MANAGER__NAVIGATION_PLUGIN_HELPER_H
+#define TEMOTO_ROBOT_MANAGER__NAVIGATION_PLUGIN_HELPER_H
+
+#include "class_loader/class_loader.hpp"
+#include "temoto_robot_manager/navigation_plugin_base.h"
+#include <memory>
+#include <thread>
+
+namespace temoto_robot_manager
+{
+
+struct RmNavigationFeedbackWrap : RmNavigationFeedback
+{
+  std::string robot_name;
+  std::string request_id;
+};
+
+struct RmNavigationRequestWrap : RmNavigationGoal
+{
+  std::string robot_name;
+  std::string request_id;
+};
+
+class NavigationPluginHelper;
+
+typedef std::shared_ptr<NavigationPluginHelper> NavigationPluginHelperPtr;
+typedef std::function<void(const RmNavigationFeedbackWrap&)> NavigationFeatureUpdateCb;
+
+class NavigationPluginHelper
+{
+public:
+  enum class State
+  {
+    NOT_LOADED,
+    UNINITIALIZED,
+    INITIALIZED,
+    PROCESSING,
+    FINISHED,
+    STOPPING,
+    ERROR
+  };
+
+  NavigationPluginHelper(const std::string& plugin_path, const std::string& robot_ns, NavigationFeatureUpdateCb update_cb);
+  ~NavigationPluginHelper();
+  void initialize();
+  void sendGoal(const RmNavigationRequestWrap& request);
+  void sendUpdate() const;
+  void cancelGoal();
+  void deinitialize();
+  State getState() const;
+
+private:
+  void setState(State state);
+
+  std::shared_ptr<NavigationPluginBase> plugin;
+  std::shared_ptr<class_loader::ClassLoader> class_loader;
+  std::thread exec_thread_;
+  std::string plugin_path_;
+  std::string robot_ns_;
+  bool is_thread_running_ = false;
+
+  State state_;
+  mutable std::mutex mutex_state_;
+
+  std::optional<RmNavigationRequestWrap> current_request_;
+  NavigationFeatureUpdateCb update_cb_;
+};
+} // temoto_robot_manager namespace
+#endif
\ No newline at end of file
diff --git a/include/temoto_robot_manager/rm_datastructures.h b/include/temoto_robot_manager/rm_datastructures.h
new file mode 100644
index 0000000..0a6a212
--- /dev/null
+++ b/include/temoto_robot_manager/rm_datastructures.h
@@ -0,0 +1,46 @@
+#ifndef TEMOTO_ROBOT_MANAGER__RM_DATASTRUCTURES_H
+#define TEMOTO_ROBOT_MANAGER__RM_DATASTRUCTURES_H
+
+#include <string>
+#include <vector>
+
+namespace temoto_robot_manager
+{
+
+struct Header
+{
+  std::string frame_id;
+  uint64_t timestamp;
+  uint64_t sequence_id;
+};
+
+struct Position
+{
+  double x;
+  double y;
+  double z;
+};
+
+struct Orientation
+{
+  double x;
+  double y;
+  double z;
+  double w;
+};
+
+struct Pose
+{
+  Position position;
+  Orientation orientation;
+}; 
+
+struct PoseStamped
+{
+  Header header;
+  Pose pose;
+};
+
+} // temoto_robot_manager
+
+#endif
\ No newline at end of file
diff --git a/include/temoto_robot_manager/robot.h b/include/temoto_robot_manager/robot.h
index d360f68..da3e797 100644
--- a/include/temoto_robot_manager/robot.h
+++ b/include/temoto_robot_manager/robot.h
@@ -24,9 +24,9 @@
 #include "temoto_robot_manager/robot_common_procedures.h"
 #include "temoto_robot_manager/GripperControl.h"
 #include "temoto_robot_manager/custom_plugin_helper.h"
+#include "temoto_robot_manager/navigation_plugin_helper.h"
 #include <moveit/move_group_interface/move_group_interface.h>
 #include <moveit/planning_interface/planning_interface.h>
-#include <move_base_msgs/MoveBaseAction.h>
 #include <geometry_msgs/PoseWithCovarianceStamped.h>
 #include <string>
 #include <map>
@@ -43,7 +43,8 @@ class Robot
   Robot(RobotConfigPtr config_
   , const std::string& resource_id
   , temoto_resource_registrar::ResourceRegistrarRos1& resource_registrar
-  , CustomFeatureUpdateCb custom_feature_update_cb);
+  , CustomFeatureUpdateCb custom_feature_update_cb
+  , NavigationFeatureUpdateCb navigation_feature_update_cb);
 
   virtual ~Robot();
   void load();
@@ -60,7 +61,8 @@ class Robot
   std::vector<double> getCurrentJointValues(const std::string& planning_group_name);
   std::vector<std::string> getNamedTargetPoses(const std::string& planning_group_name);
   
-  void goalNavigation(const geometry_msgs::PoseStamped& target_pose);
+  void goalNavigation(const RmNavigationRequestWrap& request);
+  void cancelNavigationGoal();
   void controlGripper(const std::string& robot_name, const float position);
 
   void invokeCustomFeature(const std::string& custom_feature_name, const RmCustomRequestWrap& request);
@@ -135,10 +137,15 @@ class Robot
   std::map<std::string, std::unique_ptr<moveit::planning_interface::MoveGroupInterface>> planning_groups_;
 
   // Navigation related
-  typedef actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction> MoveBaseClient;
   ros::Subscriber localized_pose_sub_;
   geometry_msgs::PoseWithCovarianceStamped current_pose_navigation_;
-
+  
+  NavigationPluginHelperPtr navigation_feature_plugin_;
+  mutable std::mutex navigation_feature_plugin_mutex_;
+  NavigationFeatureUpdateCb navigation_feature_update_cb_;
+  std::thread navigation_feature_feedback_thread_;
+  bool navigation_feature_feedback_thread_running_;
+  
   // Custom related
   std::map<std::string, CustomPluginHelperPtr> custom_feature_plugins_;
   mutable std::mutex custom_feature_plugins_mutex_;
diff --git a/include/temoto_robot_manager/robot_features.h b/include/temoto_robot_manager/robot_features.h
index c766f6f..b56d3e4 100644
--- a/include/temoto_robot_manager/robot_features.h
+++ b/include/temoto_robot_manager/robot_features.h
@@ -240,12 +240,18 @@ class FeatureNavigation : public FeatureWithDriver
     return pose_topic_;
   }
 
+  const std::string& getControllerInterface() const
+  {
+    return controller_interface_;
+  }
+
 private:
   std::string global_planner_;
   std::string local_planner_;
   std::string odom_topic_;
   std::string cmd_vel_topic_;
   std::string pose_topic_;
+  std::string controller_interface_;
 };
 
 class FeatureGripper : public FeatureWithDriver
diff --git a/include/temoto_robot_manager/robot_manager.h b/include/temoto_robot_manager/robot_manager.h
index b4a2c9b..328b6a6 100644
--- a/include/temoto_robot_manager/robot_manager.h
+++ b/include/temoto_robot_manager/robot_manager.h
@@ -23,6 +23,7 @@
 #include "temoto_core/ConfigSync.h"
 #include "temoto_process_manager/process_manager_services.hpp"
 #include "temoto_robot_manager/custom_datastructures.h"
+// #include "temoto_robot_manager/navigation_datastructures.h"
 #include "temoto_robot_manager/robot_manager_services.h"
 #include "temoto_robot_manager/robot.h"
 #include "temoto_robot_manager/robot_config.h"
@@ -89,7 +90,7 @@ class RobotManager : public temoto_core::BaseSubsystem
 
   bool getManipulationNamedTargetsCb(RobotGetNamedTargets::Request& req, RobotGetNamedTargets::Response& res);
 
-  bool goalNavigationCb(RobotNavigationGoal::Request& req, RobotNavigationGoal::Response& res);
+  bool goalNavigationCb(NavigationGoal::Request& req, NavigationGoal::Response& res);
 
   bool gripperControlPositionCb(RobotGripperControlPosition::Request& req, RobotGripperControlPosition::Response& res);
 
@@ -109,6 +110,10 @@ class RobotManager : public temoto_core::BaseSubsystem
 
   void customFeatureUpdateCb(const RmCustomFeedbackWrap& feedback);
 
+  void navigationFeatureUpdateCb(const RmNavigationFeedbackWrap& feedback);
+
+  bool cancelNavigationGoalCb(CancelNavigationGoal::Request& req, CancelNavigationGoal::Response& res);
+
   RobotConfigs parseRobotConfigs(const YAML::Node& config); 
 
   RobotConfigPtr findRobot(const std::string& robot_name, const RobotConfigs& robot_infos);
@@ -142,6 +147,7 @@ class RobotManager : public temoto_core::BaseSubsystem
   ros::ServiceServer server_navigation_goal_;
   ros::ServiceServer server_gripper_control_position_;
   ros::ServiceServer server_get_robot_config_;
+  ros::ServiceServer server_cancel_navigation_goal_;
 
   ros::ServiceClient client_plan_;
   ros::ServiceClient client_exec_;
@@ -152,6 +158,13 @@ class RobotManager : public temoto_core::BaseSubsystem
   ros::ServiceClient client_set_mode_;
   ros::ServiceClient client_navigation_goal_;
   ros::ServiceClient client_gripper_control_position_;
+  ros::ServiceClient client_cancel_navigation_goal_;
+
+  std::map<std::string, NavigationGoal> ongoing_navigation_requests_;
+  std::mutex mutex_ongoing_navigation_requests_;
+  
+  ros::Publisher pub_navigation_feature_feedback_;
+  std::mutex mutex_pub_navigation_feature_feedback_;
 
   /*
    * CUSTOM FEATURE 
diff --git a/include/temoto_robot_manager/robot_manager_interface.h b/include/temoto_robot_manager/robot_manager_interface.h
index 58a7a5c..30d3d54 100644
--- a/include/temoto_robot_manager/robot_manager_interface.h
+++ b/include/temoto_robot_manager/robot_manager_interface.h
@@ -61,11 +61,14 @@ class RobotManagerInterface
       client_get_manipulation_named_targets_ =
         nh_.serviceClient<RobotGetNamedTargets>(srv_name::SERVER_GET_MANIPULATION_NAMED_TARGETS);
       client_navigation_goal_ =
-        nh_.serviceClient<RobotNavigationGoal>(srv_name::SERVER_NAVIGATION_GOAL);
+        nh_.serviceClient<NavigationGoal>(srv_name::SERVER_NAVIGATION_GOAL);
       client_gripper_control_position_ =
         nh_.serviceClient<RobotGripperControlPosition>(srv_name::SERVER_GRIPPER_CONTROL_POSITION);
       client_get_robot_config_ =
         nh_.serviceClient<RobotGetConfig>(srv_name::SERVER_GET_CONFIG);
+      
+      client_cancel_navigation_goal_ =
+        nh_.serviceClient<CancelNavigationGoal>(srv_name::SERVER_CANCEL_NAVIGATION_GOAL);
 
       client_custom_request_ = 
         nh_.serviceClient<CustomRequest>(channels::custom::REQUEST);
@@ -73,6 +76,7 @@ class RobotManagerInterface
         nh_.serviceClient<CustomRequestPreempt>(channels::custom::PREEMPT);
       custom_feedback_ = nh_.subscribe(channels::custom::FEEDBACK, 1, &RobotManagerInterface::customFeedback, this);
 
+      navigation_feedback_ = nh_.subscribe(srv_name::NAVIGATION_FEEDBACK, 1, &RobotManagerInterface::navigationFeedbackCb, this);
       initialized_ = true;
     }
     else
@@ -155,7 +159,7 @@ class RobotManagerInterface
     }
 
     return preempt_srv_msg.response.accepted;
-  } 
+  }
 
   YAML::Node getRobotConfig(const std::string& robot_name)
   try
@@ -359,10 +363,11 @@ class RobotManagerInterface
     return msg.response.named_target_poses;
   }
 
+  // TODO: Erase?
   void navigationGoal(const std::string& robot_name
   , const geometry_msgs::PoseStamped& pose)
   {
-    temoto_robot_manager::RobotNavigationGoal msg;
+    temoto_robot_manager::NavigationGoal msg;
     msg.request.target_pose = pose;
     
     if (pose.header.frame_id.empty())
@@ -376,13 +381,93 @@ class RobotManagerInterface
     {
       throw TEMOTO_ERRSTACK("Unable to reach robot_manager");
     }
-
+    
     if (!msg.response.success)
     {
       throw TEMOTO_ERRSTACK("Unsuccessful attempt to invoke 'navigationGoal'");
     }
   }
 
+  bool navigationGoal(NavigationGoal& goal)
+  {
+    if (goal.request.target_pose.header.frame_id.empty())
+    {
+      throw TEMOTO_ERRSTACK("Reference frame is not defined");
+    }
+
+    if (!client_navigation_goal_.call(goal))
+    {
+      throw TEMOTO_ERRSTACK("Unable to reach robot_manager");
+    }
+
+    if (!goal.response.success)
+    {
+      TEMOTO_INFO_("Goal response not success");
+    }
+    else
+    {
+      goal.request.request_id = goal.response.request_id;
+      std::lock_guard<std::mutex> lock(navigation_queries_mutex_);
+      ongoing_navigation_queries_.insert({goal.request.request_id , NavigationQuery(goal)});
+    }
+
+    // wait to finish execution
+    while (getNavigationFeedback(goal.request.request_id)->status != NavigationFeedback::FINISHED
+          && getNavigationFeedback(goal.request.request_id)->status != NavigationFeedback::CANCELLED && ros::ok())
+    {
+      ros::Duration(1).sleep();
+    }
+
+    if (getNavigationFeedback(goal.request.request_id)->status == NavigationFeedback::CANCELLED)
+    {
+      goal.response.success = false;
+    }
+
+    auto ongoing_query_it = ongoing_navigation_queries_.find(goal.request.request_id);
+    if (ongoing_query_it != ongoing_navigation_queries_.end())
+    {
+      std::lock_guard<std::mutex> lock(navigation_queries_mutex_);
+      ongoing_navigation_queries_.erase(ongoing_query_it);
+    }
+
+    return goal.response.success;
+  }
+
+  std::optional<NavigationFeedback> getNavigationFeedback(const std::string& request_id)
+  {
+    std::lock_guard<std::mutex> lock(navigation_queries_mutex_);
+    auto ongoing_query_it = ongoing_navigation_queries_.find(request_id);
+    if (ongoing_query_it == ongoing_navigation_queries_.end())
+    {
+      TEMOTO_INFO_STREAM_("There's no ongoing query with " << request_id << " request id");
+      return {};
+    }
+    return ongoing_query_it->second.feedback;
+  }
+
+  bool cancelNavigationGoal(const std::string& request_id)
+  {
+    std::lock_guard<std::mutex> lock(navigation_queries_mutex_);
+    auto ongoing_query_it = ongoing_navigation_queries_.find(request_id);
+
+    if (ongoing_query_it == ongoing_navigation_queries_.end())
+    {
+      throw TEMOTO_ERRSTACK("Could not find the request in the list of ongoing requests");
+    }
+
+    CancelNavigationGoal cancel_goal;
+    cancel_goal.request.robot_name = ongoing_query_it->second.request.request.robot_name;
+    cancel_goal.request.priority = ongoing_query_it->second.request.request.priority;
+    cancel_goal.request.request_id = ongoing_query_it->first;
+
+    if (!client_cancel_navigation_goal_.call(cancel_goal))
+    {
+      throw TEMOTO_ERRSTACK("Unable to reach the CancelNavigationGoal server");
+    }
+
+    return cancel_goal.response.result;
+  }
+
   void controlGripperPosition(const std::string& robot_name, const float& position)
   {
     temoto_robot_manager::RobotGripperControlPosition msg;    
@@ -452,6 +537,7 @@ class RobotManagerInterface
     client_set_manipulation_target_.shutdown();
     client_get_manipulation_target_.shutdown();
     client_navigation_goal_.shutdown();
+    client_cancel_navigation_goal_.shutdown();
     client_gripper_control_position_.shutdown();
 
     TEMOTO_DEBUG_("RobotManagerInterface destroyed.");
@@ -466,6 +552,13 @@ class RobotManagerInterface
     CustomFeedback feedback;
   };
 
+  struct NavigationQuery
+  {
+    NavigationQuery(const NavigationGoal& nr) : request{nr}{}
+    NavigationGoal request;
+    NavigationFeedback feedback;
+  };
+
   void customFeedback(const CustomFeedback& msg)
   {
     std::lock_guard<std::mutex> lock(custom_queries_mutex_);
@@ -475,7 +568,16 @@ class RobotManagerInterface
     {
       ongoing_query_it->second.feedback = msg;
     }
+  }
 
+  void navigationFeedbackCb(const NavigationFeedback& msg)
+  {
+    std::lock_guard<std::mutex> lock(navigation_queries_mutex_);
+    auto ongoing_nav_query_it = ongoing_navigation_queries_.find(msg.request_id);
+    if (ongoing_nav_query_it != ongoing_navigation_queries_.end())
+    {
+      ongoing_nav_query_it->second.feedback = msg;
+    }
     return;
   }
 
@@ -493,6 +595,7 @@ class RobotManagerInterface
   ros::ServiceClient client_get_manipulation_target_;
   ros::ServiceClient client_get_manipulation_named_targets_;
   ros::ServiceClient client_navigation_goal_;
+  ros::ServiceClient client_cancel_navigation_goal_;
   ros::ServiceClient client_gripper_control_position_;
   ros::ServiceClient client_get_robot_config_;
 
@@ -502,6 +605,10 @@ class RobotManagerInterface
   std::mutex custom_queries_mutex_;
   std::map<std::string, CustomQuery> ongoing_custom_queries_;
 
+  ros::Subscriber navigation_feedback_;
+  std::mutex navigation_queries_mutex_;
+  std::map<std::string, NavigationQuery> ongoing_navigation_queries_;
+
   std::unique_ptr<temoto_resource_registrar::ResourceRegistrarRos1> resource_registrar_;
 };
 
diff --git a/include/temoto_robot_manager/robot_manager_services.h b/include/temoto_robot_manager/robot_manager_services.h
index daeca23..6f8d2f3 100644
--- a/include/temoto_robot_manager/robot_manager_services.h
+++ b/include/temoto_robot_manager/robot_manager_services.h
@@ -25,12 +25,14 @@
 #include "temoto_robot_manager/RobotGetVizInfo.h"
 #include "temoto_robot_manager/RobotGetTarget.h"
 #include "temoto_robot_manager/RobotGetNamedTargets.h"
-#include "temoto_robot_manager/RobotNavigationGoal.h"
+#include "temoto_robot_manager/NavigationGoal.h"
 #include "temoto_robot_manager/RobotGripperControlPosition.h"
 #include "temoto_robot_manager/RobotGetConfig.h"
 #include "temoto_robot_manager/CustomRequest.h"
 #include "temoto_robot_manager/CustomRequestPreempt.h"
 #include "temoto_robot_manager/CustomFeedback.h"
+#include "temoto_robot_manager/NavigationFeedback.h"
+#include "temoto_robot_manager/CancelNavigationGoal.h"
 
 #include <string>
 
@@ -52,8 +54,14 @@ const std::string SERVER_GET_MANIPULATION_NAMED_TARGETS = MANAGER + "/" + "get_m
 const std::string SERVER_NAVIGATION_GOAL = MANAGER + "/" + "navigation_goal";
 const std::string SERVER_SET_MODE = MANAGER + "/" + "set_mode";
 const std::string SERVER_GRIPPER_CONTROL_POSITION = MANAGER + "/" + "gripper_control_position";
+const std::string SERVER_CANCEL_NAVIGATION_GOAL = MANAGER + "/" + "cancel_navigation_goal";
+
+const std::string NAVIGATION_FEEDBACK = MANAGER + "/" + "navigation_feedback";
+
 }
 
+
+
 namespace channels
 {
 namespace custom
diff --git a/msg/NavigationFeedback.msg b/msg/NavigationFeedback.msg
new file mode 100644
index 0000000..6b6ae56
--- /dev/null
+++ b/msg/NavigationFeedback.msg
@@ -0,0 +1,12 @@
+Header header
+string robot_name
+string request_id
+
+uint8 status
+uint8 IDLE=2
+uint8 RUNNING=3
+uint8 FINISHED=4
+uint8 CANCELLED=5
+
+float64 progress
+geometry_msgs/PoseStamped base_position
\ No newline at end of file
diff --git a/package.xml b/package.xml
index aa2fd78..b1c9941 100644
--- a/package.xml
+++ b/package.xml
@@ -15,7 +15,6 @@
   <depend>tf2_geometry_msgs</depend>
   <depend>tf2</depend>
   <depend>moveit_ros_planning_interface</depend>
-  <depend>move_base_msgs</depend>
   <depend>yaml-cpp</depend>
   <depend>temoto_core</depend>
   <depend>temoto_resource_registrar</depend>
diff --git a/src/navigation_plugin_helper.cpp b/src/navigation_plugin_helper.cpp
new file mode 100644
index 0000000..5c81a76
--- /dev/null
+++ b/src/navigation_plugin_helper.cpp
@@ -0,0 +1,217 @@
+#include "temoto_robot_manager/navigation_plugin_helper.h"
+#include "temoto_resource_registrar/temoto_error.h"
+#include <chrono>
+
+#include <iostream>
+
+namespace temoto_robot_manager
+{
+
+NavigationPluginHelper::NavigationPluginHelper(const std::string& plugin_path, const std::string& robot_ns, NavigationFeatureUpdateCb update_cb)
+: plugin_path_(plugin_path)
+, state_(State::NOT_LOADED)
+, update_cb_(update_cb)
+, current_request_{}
+, robot_ns_(robot_ns)
+{
+try
+{
+  class_loader = std::make_shared<class_loader::ClassLoader>(plugin_path_, false);
+
+  if (class_loader->getAvailableClasses<NavigationPluginBase>().empty())
+  {
+    setState(State::ERROR);
+    throw TEMOTO_ERRSTACK("Library contains no plugins, check if the path is correct: '" + plugin_path_ + "'");
+  }
+
+  std::string plugin_name = class_loader->getAvailableClasses<NavigationPluginBase>().front();
+  plugin = class_loader->createSharedInstance<NavigationPluginBase>(plugin_name);
+  if (!class_loader->isLibraryLoaded())
+  {
+    setState(State::ERROR);
+    throw TEMOTO_ERRSTACK("Unable to load plugin '" + plugin_path_ + "'");
+  }
+
+  setState(State::UNINITIALIZED);  
+}
+catch(class_loader::ClassLoaderException & e)
+{
+  throw TEMOTO_ERRSTACK(e.what());
+}
+}
+
+NavigationPluginHelper::~NavigationPluginHelper()
+{
+  if (getState() == State::PROCESSING)
+  {
+    plugin->cancelGoal();
+
+    while (!exec_thread_.joinable())
+    {
+      std::this_thread::sleep_for(std::chrono::milliseconds(100));
+    }
+
+    exec_thread_.join();
+    plugin->deinitialize();
+  }
+
+  else if (getState() == State::STOPPING)
+  {
+    while (!exec_thread_.joinable())
+    {
+      std::this_thread::sleep_for(std::chrono::milliseconds(100));
+    }
+
+    exec_thread_.join();
+    plugin->deinitialize();
+  }
+
+  else if(getState() == State::INITIALIZED || getState() == State::FINISHED || getState() == State::ERROR)
+  {
+    if (exec_thread_.joinable())
+    {
+      exec_thread_.join();
+    }
+
+    plugin->deinitialize();
+  }
+  
+  plugin.reset();
+}
+
+void NavigationPluginHelper::initialize()
+try
+{
+  if (getState() != State::UNINITIALIZED && getState() != State::FINISHED)
+  {
+    setState(State::ERROR);
+    throw TEMOTO_ERRSTACK("Cannot initalize the plugin. It has to be in 'UNINITIALIZED' state for that");
+  }
+  if (!plugin->initialize(robot_ns_))
+  {
+    setState(State::ERROR);
+    plugin.reset();
+    throw TEMOTO_ERRSTACK("Unable to initialize the plugin");
+  }
+  setState(State::INITIALIZED);
+}
+catch(class_loader::ClassLoaderException & e)
+{
+  throw TEMOTO_ERRSTACK(e.what());
+}
+
+void NavigationPluginHelper::sendGoal(const RmNavigationRequestWrap& request)
+{
+  initialize();
+
+  if (getState() != State::INITIALIZED)
+  {
+    setState(State::ERROR);
+    throw TEMOTO_ERRSTACK("Cannot send navigation goal. The plugin has to be in 'INITIALIZED' state for that");
+  }
+
+  if (exec_thread_.joinable())
+  {
+    exec_thread_.join();
+    is_thread_running_ = false;
+  }
+  if(!is_thread_running_)
+  {
+    current_request_ = request;
+    exec_thread_ = std::thread([this, request]
+    {
+      if (this->plugin->sendGoal(request))
+      {
+        setState(State::FINISHED);
+      }
+      else
+      {
+        setState(State::ERROR);
+      }
+      sendUpdate();
+    });
+    is_thread_running_ = true;
+  }
+
+  setState(State::PROCESSING);
+  sendUpdate();
+}
+
+void NavigationPluginHelper::cancelGoal()
+{
+
+  if (getState() == State::FINISHED)
+  {
+    std::cout << "\033[1;32m [Plugin helper] There is no goal to cancel.\033[0m\n" <<std::endl;
+    sendUpdate();
+    return;
+  }
+
+  if (getState() != State::PROCESSING)
+  {
+    setState(State::ERROR);
+    throw TEMOTO_ERRSTACK("Cannot pre-empt the plugin. It has to be in 'PROCESSING' state for that");
+  }
+  
+  if (!plugin->cancelGoal())
+  {
+    setState(State::ERROR);
+    throw TEMOTO_ERRSTACK("Unable to cancel goal");
+  }
+
+  setState(State::STOPPING);
+  sendUpdate();
+  
+}
+
+void NavigationPluginHelper::deinitialize()
+try
+{
+  if (getState() != State::INITIALIZED)
+  {
+    setState(State::ERROR);
+    throw TEMOTO_ERRSTACK("Cannot deinitialize the plugin. It has to be in 'INITIALIZED' state for that");
+  }
+
+  if (!plugin->deinitialize())
+  {
+    setState(State::ERROR);
+    throw TEMOTO_ERRSTACK("Unable to deinitialize the plugin");
+  }
+
+  setState(State::UNINITIALIZED);
+}
+catch(...)
+{
+  throw TEMOTO_ERRSTACK("Unable to deinitialize the plugin");
+}
+
+NavigationPluginHelper::State NavigationPluginHelper::getState() const
+{
+  std::lock_guard<std::mutex> l(mutex_state_);
+  return state_;
+}
+
+void NavigationPluginHelper::setState(State state)
+{
+  std::lock_guard<std::mutex> l(mutex_state_);
+  state_ = state;
+}
+
+void NavigationPluginHelper::sendUpdate() const
+{
+  auto fb = plugin->getFeedback();
+  if (fb.has_value())
+  {
+    RmNavigationFeedbackWrap fbw;
+    
+    fbw.robot_name = current_request_->robot_name;
+    fbw.request_id = current_request_->request_id;
+    fbw.status = uint8_t(state_);
+    fbw.progress = fb->progress;
+    fbw.base_position = fb->base_position;
+    update_cb_(fbw);
+  }
+}
+
+}
\ No newline at end of file
diff --git a/src/robot.cpp b/src/robot.cpp
index d7dbfd9..e87f7e9 100644
--- a/src/robot.cpp
+++ b/src/robot.cpp
@@ -17,6 +17,7 @@
 #include "ros/package.h"
 #include "temoto_robot_manager/robot.h"
 #include "temoto_robot_manager/custom_plugin_base.h"
+#include "temoto_robot_manager/navigation_plugin_base.h"
 #include "temoto_resource_registrar/temoto_error.h"
 
 namespace temoto_robot_manager
@@ -24,16 +25,19 @@ namespace temoto_robot_manager
 Robot::Robot(RobotConfigPtr config
 , const std::string& resource_id
 , temoto_resource_registrar::ResourceRegistrarRos1& resource_registrar
-, CustomFeatureUpdateCb custom_feature_update_cb)
+, CustomFeatureUpdateCb custom_feature_update_cb
+, NavigationFeatureUpdateCb navigation_feature_update_cb)
 : config_(config)
 , robot_resource_id_(resource_id)
 , resource_registrar_(resource_registrar)
 , custom_feature_update_cb_(custom_feature_update_cb)
+, navigation_feature_update_cb_(navigation_feature_update_cb)
 , is_plan_valid_(false)
 , robot_operational_(true)
 , state_in_error_(false)
 , robot_loaded_(false)
 , custom_feature_feedback_thread_running_(false)
+, navigation_feature_feedback_thread_running_(false)
 {}
 
 Robot::~Robot()
@@ -72,9 +76,18 @@ Robot::~Robot()
     config_->getFeatureManipulation().setDriverLoaded(false);
   }
 
+  // Stop Thread
+  navigation_feature_feedback_thread_running_ = false;
+  while (!navigation_feature_feedback_thread_.joinable())
+  {
+    std::this_thread::sleep_for(std::chrono::milliseconds(5));
+  }
+  navigation_feature_feedback_thread_.join();
+
   if (config_->getFeatureNavigation().isLoaded())
   {
-    TEMOTO_DEBUG_("Unloading Navigation Feature.");
+    TEMOTO_DEBUG_("Unloading Navigation Feature.");    
+    navigation_feature_plugin_.reset();
     config_->getFeatureNavigation().setLoaded(false);
   }
 
@@ -388,12 +401,11 @@ void Robot::loadNavigationController()
   {
     return; // Return if already loaded.
   }
-
   try
   {
     FeatureNavigation& ftr = config_->getFeatureNavigation();
+    
     rosExecute(ftr.getPackageName(), ftr.getExecutable(), ftr.getArgs());
-
     // wait for command velocity to be published
     std::string cmd_vel_topic = "/" + config_->getAbsRobotNamespace() + "/" + ftr.getCmdVelTopic();
     waitForTopic(cmd_vel_topic);
@@ -406,6 +418,32 @@ void Robot::loadNavigationController()
       , &Robot::robotPoseCallback
       , this);
     }
+    
+    
+    TEMOTO_INFO_("Load Navigation contoller lib");
+    try
+    {
+      const std::string& plugin_path = ftr.getControllerInterface();
+      std::string act_rob_ns = "/" + config_->getAbsRobotNamespace();
+
+      NavigationPluginHelperPtr plugin_helper = std::make_shared<NavigationPluginHelper>(plugin_path, act_rob_ns, navigation_feature_update_cb_);
+
+      std::lock_guard<std::mutex> l(navigation_feature_plugin_mutex_);
+      navigation_feature_plugin_ = plugin_helper;
+    }
+    catch(resource_registrar::TemotoErrorStack& error_stack)
+    {
+      throw FWD_TEMOTO_ERRSTACK(error_stack);
+    }
+    catch(std::exception& e)
+    {
+      throw TEMOTO_ERRSTACK(e.what());
+    }
+    catch(...)
+    {
+      throw TEMOTO_ERRSTACK("Could not load navigation feature");
+    }
+    
 
     ros::Duration(5).sleep();
     ftr.setLoaded(true);
@@ -430,6 +468,8 @@ void Robot::loadNavigationDriver()
     FeatureNavigation& ftr = config_->getFeatureNavigation();
     rosExecute(ftr.getDriverPackageName(), ftr.getDriverExecutable(), ftr.getDriverArgs());
     std::string odom_topic = "/" + config_->getAbsRobotNamespace() + "/" + ftr.getOdomTopic();
+    TEMOTO_INFO_(odom_topic);
+
     waitForTopic(odom_topic);
     ftr.setDriverLoaded(true);
     TEMOTO_DEBUG_("Feature 'Navigation Driver' loaded.");        
@@ -953,43 +993,48 @@ std::vector<std::string> Robot::getNamedTargetPoses(const std::string& planning_
   return group_it->second->getNamedTargets();
 }
 
-void Robot::goalNavigation(const geometry_msgs::PoseStamped& target_pose)
+void Robot::goalNavigation(const RmNavigationRequestWrap& request)
 {
   if (!isRobotOperational())
   {
     throw TEMOTO_ERRSTACK("Could not navigate the robot because robot is not operational");
   }
-
   FeatureNavigation& ftr = config_->getFeatureNavigation();
-  std::string act_rob_ns = "/" + config_->getAbsRobotNamespace() + "/move_base";
-  MoveBaseClient ac(act_rob_ns, true);
-  
-  if (!ac.waitForServer(ros::Duration(5.0)))
+  navigation_feature_plugin_->sendGoal(request);
+
+  /*
+  * Start the navigation feature feedback thread
+  */
+  if (!navigation_feature_feedback_thread_running_)
   {
-    TEMOTO_ERRSTACK("The move_base action server did not come up");
-  }
+    navigation_feature_feedback_thread_running_ = true;
+    navigation_feature_feedback_thread_ = std::thread(
+    [&]
+    {
+      TEMOTO_DEBUG_("Navigation feature feedback thread running");
 
-  move_base_msgs::MoveBaseGoal goal;
-  goal.target_pose = target_pose;
-  goal.target_pose.header.stamp = ros::Time::now();
-  ac.sendGoal(goal);
+      while (navigation_feature_feedback_thread_running_)
+      {
+        std::lock_guard<std::mutex> l(navigation_feature_plugin_mutex_);
 
-  // Wait until either the goal is finished or robot has encountered a system issue
-  while((ac.getState() == actionlib::SimpleClientGoalState::PENDING || ac.getState() == actionlib::SimpleClientGoalState::ACTIVE)
-     && isRobotOperational())
-  {
-    ros::Duration(1).sleep();
-  }
+        navigation_feature_plugin_->sendUpdate();
+        std::this_thread::sleep_for(std::chrono::milliseconds(200));
+      }
 
-  if (!isRobotOperational())
-  {
-    ac.cancelGoal();
-    throw TEMOTO_ERRSTACK("Could not finish the navigation goal because the robot is not operational");
-  }
-  else if(ac.getState() != actionlib::SimpleClientGoalState::SUCCEEDED)
-  {
-    throw TEMOTO_ERRSTACK("The base failed to move");
-  }
+      TEMOTO_DEBUG_("Navigation feature feedback thread finished");
+    });
+  }  
+}
+
+void Robot::cancelNavigationGoal()
+try
+{
+  navigation_feature_plugin_->cancelGoal();
+}
+catch(resource_registrar::TemotoErrorStack& e)
+{
+  std::string message = "Unable to cancel navigation goal of robot '" + config_->getName() + "'.";
+  throw FWD_TEMOTO_ERRSTACK_WMSG(e, message);
 }
 
 void Robot::controlGripper(const std::string& robot_name,const float position)
diff --git a/src/robot_common_procedures.cpp b/src/robot_common_procedures.cpp
index 6f0d488..569480b 100644
--- a/src/robot_common_procedures.cpp
+++ b/src/robot_common_procedures.cpp
@@ -23,7 +23,7 @@ CommonProcedure::CommonProcedure(const std::string& name, const YAML::Node& comm
 {
   this->executable_ = common_conf["executable"].as<std::string>();
   this->executable_type_ = common_conf["executable_type"].as<std::string>();
-  // setFromConfig(common_conf["executable_type"], this->executable_type_);
+  
   if (common_conf["args"])
   {
     this->args_ = common_conf["args"].as<std::string>();
diff --git a/src/robot_features.cpp b/src/robot_features.cpp
index 33c8c8f..78a0bce 100644
--- a/src/robot_features.cpp
+++ b/src/robot_features.cpp
@@ -106,9 +106,10 @@ FeatureNavigation::FeatureNavigation(const YAML::Node& nav_conf)
    * Get the controller configuration. Not required
    */
   if (nav_conf["controller"].IsDefined())
-  {
+  {    
     this->feature_enabled_ = setFromConfig(nav_conf["controller"]["package_name"], this->package_name_)
-                          && setFromConfig(nav_conf["controller"]["executable"], this->executable_);
+                          && setFromConfig(nav_conf["controller"]["executable"], this->executable_)
+                          && setFromConfig(nav_conf["controller"]["controller_interface"], this->controller_interface_);
     // Optional parameters                     
     if (this->feature_enabled_)
     {
@@ -116,14 +117,14 @@ FeatureNavigation::FeatureNavigation(const YAML::Node& nav_conf)
       setFromConfig(nav_conf["controller"]["global_planner"], this->global_planner_);
       setFromConfig(nav_conf["controller"]["local_planner"], this->local_planner_);
       setFromConfig(nav_conf["controller"]["pose_topic"], this->pose_topic_);
-    }
+    }    
   }
 
   /*
    * Get the driver configuration.
    */
   if (nav_conf["driver"].IsDefined())
-  {
+  {    
     this->driver_enabled_ = setFromConfig(nav_conf["driver"]["package_name"], this->driver_package_name_)
                         && setFromConfig(nav_conf["driver"]["executable"], this->driver_executable_);
     // Optional parameters
diff --git a/src/robot_manager.cpp b/src/robot_manager.cpp
index 73ba048..7c2a4c2 100644
--- a/src/robot_manager.cpp
+++ b/src/robot_manager.cpp
@@ -114,6 +114,10 @@ RobotManager::RobotManager(const std::string& config_base_path, bool restore_fro
     srv_name::SERVER_GET_CONFIG,
     &RobotManager::getRobotConfigCb,
     this);
+  server_cancel_navigation_goal_ = nh_.advertiseService(
+    srv_name::SERVER_CANCEL_NAVIGATION_GOAL, 
+    &RobotManager::cancelNavigationGoalCb,
+    this);
 
   /*
    * Set up the Custom Feature channel
@@ -127,6 +131,7 @@ RobotManager::RobotManager(const std::string& config_base_path, bool restore_fro
     &RobotManager::customFeaturePreemptCb,
     this);
   pub_custom_feature_feedback_ = nh_.advertise<CustomFeedback>(channels::custom::FEEDBACK, 10);
+  pub_navigation_feature_feedback_ = nh_.advertise<NavigationFeedback>(srv_name::NAVIGATION_FEEDBACK, 10);
 
   TEMOTO_INFO_("Robot manager is ready.\n");
 }
@@ -162,7 +167,7 @@ try
   RobotPtr loaded_robot = findLoadedRobot(req.robot_name);
 
   /*
-   * Pre-empt the lower priority request
+   *Pre-empt the lower priority request
    */
   if (custom_request_it != ongoing_custom_requests_.end())
   {
@@ -183,7 +188,6 @@ try
   req_rm.data_num = req.data_num;
   req_rm.data_num_array = req.data_num_array;
   
-  // req_rm.data_pose = RmCustomRequest::PoseStamped{};                    // TODO
   req_rm.data_pose.header.frame_id = req.data_pose.header.frame_id;
   req_rm.data_pose.pose.position.x = req.data_pose.pose.position.x;
   req_rm.data_pose.pose.position.y = req.data_pose.pose.position.y;
@@ -193,7 +197,7 @@ try
   req_rm.data_pose.pose.orientation.z = req.data_pose.pose.orientation.z;
   req_rm.data_pose.pose.orientation.w = req.data_pose.pose.orientation.w;
   
-  req_rm.data_pose_array = std::vector<RmCustomRequest::PoseStamped>{}; // TODO
+  req_rm.data_pose_array = std::vector<PoseStamped>{}; // TODO
 
   // TODO: Add pose and pose array
   loaded_robot->invokeCustomFeature(req.custom_feature_name, req_rm);
@@ -295,6 +299,31 @@ void RobotManager::customFeatureUpdateCb(const RmCustomFeedbackWrap& feedback)
   pub_custom_feature_feedback_.publish(msg);
 }
 
+void RobotManager::navigationFeatureUpdateCb(const RmNavigationFeedbackWrap& feedback)
+{
+  NavigationFeedback msg;
+
+  msg.header.stamp = ros::Time::now();
+  msg.robot_name = feedback.robot_name;
+  msg.request_id = feedback.request_id;
+  msg.status = feedback.status;
+  msg.progress = feedback.progress;
+
+  // Convert from RM/PoseStamped to geomety_msgs/PoseStamped
+  msg.base_position.header.frame_id = feedback.base_position.header.frame_id;
+  msg.base_position.header.stamp = ros::Time::now();
+  msg.base_position.pose.position.x = feedback.base_position.pose.position.x;
+  msg.base_position.pose.position.y = feedback.base_position.pose.position.y;
+  msg.base_position.pose.position.z = feedback.base_position.pose.position.z;
+  msg.base_position.pose.orientation.x = feedback.base_position.pose.orientation.x;
+  msg.base_position.pose.orientation.y = feedback.base_position.pose.orientation.y;
+  msg.base_position.pose.orientation.z = feedback.base_position.pose.orientation.z;
+  msg.base_position.pose.orientation.w = feedback.base_position.pose.orientation.w;  
+
+  std::lock_guard<std::mutex> l(mutex_pub_navigation_feature_feedback_);
+  pub_navigation_feature_feedback_.publish(msg);
+}
+
 void RobotManager::findRobotDescriptionFiles(boost::filesystem::path current_dir)
 {
   if (std::string(current_dir.c_str()).empty())
@@ -348,7 +377,8 @@ void RobotManager::loadCb(RobotLoad::Request& req, RobotLoad::Response& res)
   try
   {
     auto loaded_robot = std::make_shared<Robot>(config, res.temoto_metadata.request_id, resource_registrar_
-    , std::bind(&RobotManager::customFeatureUpdateCb, this, std::placeholders::_1));
+    , std::bind(&RobotManager::customFeatureUpdateCb, this, std::placeholders::_1)
+    , std::bind(&RobotManager::navigationFeatureUpdateCb, this, std::placeholders::_1));
 
     loaded_robot->load();
     loaded_robots_.push_back(loaded_robot);
@@ -386,7 +416,8 @@ void RobotManager::loadCb(RobotLoad::Request& req, RobotLoad::Response& res)
 
     TEMOTO_INFO_("Call to remote RobotManager was sucessful.");
     auto loaded_robot = std::make_shared<Robot>(config, res.temoto_metadata.request_id, resource_registrar_
-    , std::bind(&RobotManager::customFeatureUpdateCb, this, std::placeholders::_1));
+    , std::bind(&RobotManager::customFeatureUpdateCb, this, std::placeholders::_1)
+    , std::bind(&RobotManager::navigationFeatureUpdateCb, this, std::placeholders::_1));
     loaded_robots_.push_back(loaded_robot);
 
     return;
@@ -750,15 +781,81 @@ catch(resource_registrar::TemotoErrorStack& e)
   return true;
 }
 
-bool RobotManager::goalNavigationCb(RobotNavigationGoal::Request& req, RobotNavigationGoal::Response& res)
+bool RobotManager::goalNavigationCb(NavigationGoal::Request& req, NavigationGoal::Response& res)
 try
 {
+  TEMOTO_INFO_("Received a goal Navigation request");
+  TEMOTO_DEBUG_STREAM_("Request:\n" << req);
+  std::lock_guard<std::mutex> l(mutex_ongoing_navigation_requests_);
+
+  auto navigation_request_it = std::find_if(
+    ongoing_navigation_requests_.begin()
+  , ongoing_navigation_requests_.end()
+  , [&](const auto& ongoing_req)
+  {
+    return req == ongoing_req.second.request;
+  });
+
+  /*
+   * Check if that request is already processed by a client with higher priority
+   */
+  if (navigation_request_it != ongoing_navigation_requests_.end() &&
+      navigation_request_it->second.request.priority > req.priority)
+  {
+    TEMOTO_WARN_STREAM_("Request declined as it is already in process by client with higher priority" << std::endl);
+    return true;
+  }
   RobotPtr loaded_robot = findLoadedRobot(req.robot_name);
+
+  /*
+   * Pre-empt the lower priority request
+   */
+  if (navigation_request_it != ongoing_navigation_requests_.end())
+  {
+    TEMOTO_INFO_("This request is already in process under a lower priority, preempting.");
+    loaded_robot->cancelNavigationGoal();
+    ongoing_navigation_requests_.erase(navigation_request_it);
+  }
+
+  req.request_id = generateId();
+  res.request_id = req.request_id;
+
+  RmNavigationRequestWrap req_rm;
+  req_rm.robot_name = req.robot_name;
+  req_rm.request_id = res.request_id;
+  req_rm.goal_pose.header.frame_id = req.target_pose.header.frame_id;
+  req_rm.goal_pose.pose.position.x = req.target_pose.pose.position.x;
+  req_rm.goal_pose.pose.position.y = req.target_pose.pose.position.y;
+  req_rm.goal_pose.pose.position.z = req.target_pose.pose.position.z;
+  req_rm.goal_pose.pose.orientation.x = req.target_pose.pose.orientation.x;
+  req_rm.goal_pose.pose.orientation.y = req.target_pose.pose.orientation.y;
+  req_rm.goal_pose.pose.orientation.z = req.target_pose.pose.orientation.z;
+  req_rm.goal_pose.pose.orientation.w = req.target_pose.pose.orientation.w;
+
   if (loaded_robot->isLocal())
   {
     TEMOTO_DEBUG_STREAM_("Navigating '" << req.robot_name << " to pose: " << req.target_pose << " ...");
-    loaded_robot->goalNavigation(req.target_pose);  // The robot would move with respect to the coordinate frame defined in the header
-    res.success = true;   
+    loaded_robot->goalNavigation(req_rm);
+    
+    NavigationGoal goal;
+    goal.request = req;
+    goal.response = res;
+    
+    auto it = std::find_if(
+      ongoing_navigation_requests_.begin()
+    , ongoing_navigation_requests_.end()
+    , [&](const auto& ongoing_req)
+    {
+      return req.robot_name == ongoing_req.second.request.robot_name;
+    });
+    if (it != ongoing_navigation_requests_.end())
+    {
+      it->second = goal;
+    }
+    else
+    {
+      ongoing_navigation_requests_.insert(std::make_pair(req.request_id, goal));
+    }
   }
   else
   {
@@ -766,8 +863,8 @@ try
       + srv_name::SERVER_NAVIGATION_GOAL;
     TEMOTO_DEBUG_STREAM_("Forwarding the request to remote robot manager at '" << topic << "'.");
 
-    ros::ServiceClient client_navigation_goal_ = nh_.serviceClient<RobotNavigationGoal>(topic);
-    RobotNavigationGoal fwd_goal_srvc;
+    ros::ServiceClient client_navigation_goal_ = nh_.serviceClient<NavigationGoal>(topic);
+    NavigationGoal fwd_goal_srvc;
     fwd_goal_srvc.request = req;
     fwd_goal_srvc.response = res;
     if (client_navigation_goal_.call(fwd_goal_srvc))
@@ -780,7 +877,7 @@ try
     }
   }
   res.success = true;
-  return true;  
+  return true;
 }
 catch(resource_registrar::TemotoErrorStack& e)
 {
@@ -788,6 +885,91 @@ catch(resource_registrar::TemotoErrorStack& e)
   return true;
 }
 
+bool RobotManager::cancelNavigationGoalCb(CancelNavigationGoal::Request& req, CancelNavigationGoal::Response& res)
+try
+{
+  TEMOTO_INFO_("Cancel Navigation goal request");
+  TEMOTO_DEBUG_STREAM_("Request:\n" << req);
+
+  std::lock_guard<std::mutex> l(mutex_ongoing_navigation_requests_);
+
+  auto navigation_request_it = std::find_if(
+    ongoing_navigation_requests_.begin()
+  , ongoing_navigation_requests_.end()
+  , [&](const auto& ongoing_req)
+  {
+    return req.robot_name == ongoing_req.second.request.robot_name;
+  });
+
+  if (navigation_request_it == ongoing_navigation_requests_.end())
+  {
+    TEMOTO_WARN_STREAM_("There is no ongoing navigation. Request id " << req.request_id << " is invalid" << std::endl);
+    return true;
+  }
+
+  /*
+   * DECLINE: If priority is low
+   */
+  if (req.request_id.empty() && (req.priority <= navigation_request_it->second.request.priority))
+  {
+    res.message = "Cancel goal request declined: Priority lower than required";
+    TEMOTO_WARN_STREAM_(res.message << std::endl);
+    return true;
+  }
+
+  /*
+   * DECLINE: If ID was provided but mismatches
+   */
+  if (!req.request_id.empty() && (req.request_id != navigation_request_it->second.response.request_id))
+  {
+    res.message = "Cancel Goal request declined: Request ID mismatch";
+    TEMOTO_WARN_STREAM_(res.message << std::endl);
+    res.result = false;
+    return true;
+  }
+
+  /*
+   * ACCEPT: If ID matches or the priority is higher
+   */
+  RobotPtr loaded_robot = findLoadedRobot(req.robot_name);
+  if (loaded_robot->isLocal())
+  {
+    loaded_robot->cancelNavigationGoal();
+    if (navigation_request_it != ongoing_navigation_requests_.end())
+    {
+      ongoing_navigation_requests_.erase(navigation_request_it);
+    }
+  }
+  else
+  {
+    std::string topic = "/" + loaded_robot->getConfig()->getTemotoNamespace() + "/"
+      + srv_name::SERVER_CANCEL_NAVIGATION_GOAL;
+    TEMOTO_DEBUG_STREAM_("Forwarding the request to remote robot manager at '" << topic << "'.");
+
+    ros::ServiceClient client_cancel_navigation_goal_ = nh_.serviceClient<CancelNavigationGoal>(topic);
+    CancelNavigationGoal fwd_cancel_goal_srvc;
+    fwd_cancel_goal_srvc.request = req;
+    fwd_cancel_goal_srvc.response = res;
+    if (client_cancel_navigation_goal_.call(fwd_cancel_goal_srvc))
+    {
+      res = fwd_cancel_goal_srvc.response;
+    }
+    else
+    {
+      throw TEMOTO_ERRSTACK("Call to remote RobotManager service failed.");
+    }
+  }
+  res.result = true;
+  return true;  
+}
+catch(resource_registrar::TemotoErrorStack& e)
+{
+  res.message = std::string("Cancel Navigation Goal request declined: \n") + e.what();
+  TEMOTO_WARN_STREAM_(res.message << std::endl);
+  res.result = false;
+  return true;
+}
+
 void RobotManager::resourceStatusCb(RobotLoad srv_msg, temoto_resource_registrar::Status status_msg)
 {
   TEMOTO_DEBUG_("status info was received");
@@ -993,7 +1175,8 @@ void RobotManager::restoreState()
       continue;
     }
     auto robot = std::make_shared<Robot>(robot_config, query.response.temoto_metadata.request_id, resource_registrar_
-    , std::bind(&RobotManager::customFeatureUpdateCb, this, std::placeholders::_1));
+    , std::bind(&RobotManager::customFeatureUpdateCb, this, std::placeholders::_1)
+    , std::bind(&RobotManager::navigationFeatureUpdateCb, this, std::placeholders::_1));
     robot->recover(query.response.temoto_metadata.request_id);
     loaded_robots_.push_back(robot);
   }
diff --git a/srv/CancelNavigationGoal.srv b/srv/CancelNavigationGoal.srv
new file mode 100644
index 0000000..d058980
--- /dev/null
+++ b/srv/CancelNavigationGoal.srv
@@ -0,0 +1,8 @@
+string robot_name
+uint8 priority
+string request_id
+
+---
+
+bool result
+string message
\ No newline at end of file
diff --git a/srv/NavigationGoal.srv b/srv/NavigationGoal.srv
new file mode 100644
index 0000000..2b93672
--- /dev/null
+++ b/srv/NavigationGoal.srv
@@ -0,0 +1,10 @@
+string robot_name
+geometry_msgs/PoseStamped target_pose
+string client_id
+string request_id
+uint8 priority
+
+---
+
+string request_id
+bool success
\ No newline at end of file
diff --git a/srv/RobotNavigationGoal.srv b/srv/RobotNavigationGoal.srv
deleted file mode 100644
index 5f77352..0000000
--- a/srv/RobotNavigationGoal.srv
+++ /dev/null
@@ -1,6 +0,0 @@
-string robot_name
-geometry_msgs/PoseStamped target_pose
-
----
-
-bool success