diff --git a/trackers/kr_trackers_manager/CMakeLists.txt b/trackers/kr_trackers_manager/CMakeLists.txt
index 6a522c6e..e5564cb7 100644
--- a/trackers/kr_trackers_manager/CMakeLists.txt
+++ b/trackers/kr_trackers_manager/CMakeLists.txt
@@ -45,4 +45,26 @@ install(
   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})
 
 install(DIRECTORY include/${PROJECT_NAME}/ DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION})
+install(DIRECTORY config/ DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/config)
+install(DIRECTORY test/ DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/test)
 install(FILES nodelet_plugin.xml DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
+
+if(CATKIN_ENABLE_TESTING)
+  find_package(catkin REQUIRED COMPONENTS
+    roscpp
+    rostest
+    kr_tracker_msgs
+    kr_mav_msgs
+    nav_msgs
+    actionlib
+  )
+
+  include_directories(include ${catkin_INCLUDE_DIRS})
+  include_directories(${GTEST_INCLUDE_DIRS})
+
+  add_executable(kr_trackers_manager_test test/kr_trackers_manager_test.cpp)
+  target_link_libraries(kr_trackers_manager_test ${catkin_LIBRARIES} ${GTEST_LIBRARIES})
+
+  add_rostest(test/kr_trackers_manager_nodelet.test)
+
+endif()
diff --git a/trackers/kr_trackers_manager/config/tracker_params.yaml b/trackers/kr_trackers_manager/config/tracker_params.yaml
new file mode 100644
index 00000000..b14d3e33
--- /dev/null
+++ b/trackers/kr_trackers_manager/config/tracker_params.yaml
@@ -0,0 +1,12 @@
+# This should contain tracker parameters
+
+line_tracker_distance:
+  default_v_des: 2.0
+  default_a_des: 1.0
+  epsilon: 0.1
+
+line_tracker_min_jerk:
+  default_v_des: 2.0
+  default_a_des: 1.0
+  default_yaw_v_des: 0.8
+  default_yaw_a_des: 0.2
diff --git a/trackers/kr_trackers_manager/config/trackers.yaml b/trackers/kr_trackers_manager/config/trackers.yaml
new file mode 100644
index 00000000..96195e6d
--- /dev/null
+++ b/trackers/kr_trackers_manager/config/trackers.yaml
@@ -0,0 +1,4 @@
+trackers:
+  - kr_trackers/LineTrackerMinJerk
+  - kr_trackers/LineTrackerDistance
+
diff --git a/trackers/kr_trackers_manager/include/kr_trackers_manager/tester_utils.hpp b/trackers/kr_trackers_manager/include/kr_trackers_manager/tester_utils.hpp
new file mode 100644
index 00000000..38a3dc68
--- /dev/null
+++ b/trackers/kr_trackers_manager/include/kr_trackers_manager/tester_utils.hpp
@@ -0,0 +1,393 @@
+#pragma once
+
+#include <actionlib/client/simple_action_client.h>
+#include <kr_mav_msgs/PositionCommand.h>
+#include <kr_tracker_msgs/LineTrackerAction.h>
+#include <kr_tracker_msgs/TrackerStatus.h>
+#include <kr_tracker_msgs/Transition.h>
+#include <nav_msgs/Odometry.h>
+#include <ros/ros.h>
+
+#include <vector>
+
+/*
+ * @brief Class of helper functions and variables to test kr_trackers_manager
+ */
+class TrackersManagerTester
+{
+ public:
+  TrackersManagerTester();
+  bool initial_checks();
+  void position_cmd_callback(const kr_mav_msgs::PositionCommand::ConstPtr &msg);
+  void tracker_status_callback(const kr_tracker_msgs::TrackerStatus::ConstPtr &msg);
+  void send_transition_request(std::string tracker_name);
+  void send_action_goal(std::string tracker_name, float x, float y, float z, float yaw, float v_des, float a_des,
+                        uint8_t relative);
+  void publish_odom_msg(int secs, int nsecs, float pos_x, float pos_y, float pos_z, float orient_x, float orient_y,
+                        float orient_z, float orient_w);
+  void reset_flags();
+  bool srv_response;
+  std::string srv_msg;
+  bool srv_succeed = false;
+  bool result_received = false;
+  bool feedback_received = false;
+  kr_tracker_msgs::LineTrackerResultPtr action_result;
+  kr_tracker_msgs::LineTrackerFeedbackPtr action_feedback;
+  bool position_cmd_received = false;
+  kr_mav_msgs::PositionCommandPtr cmd;
+  bool tracker_status_received = false;
+  kr_tracker_msgs::TrackerStatusPtr status;
+  std::mutex mutex;
+
+ private:
+  ros::NodeHandle nh_;
+  ros::Publisher odom_pub_;
+  ros::Subscriber position_cmd_sub_, tracker_status_sub_;
+  ros::ServiceClient transition_client_;
+
+  typedef actionlib::SimpleActionClient<kr_tracker_msgs::LineTrackerAction> ClientType;
+  std::shared_ptr<ClientType> distance_client_;
+  std::shared_ptr<ClientType> min_jerk_client_;
+
+  void done_callback(const actionlib::SimpleClientGoalState &state,
+                     const kr_tracker_msgs::LineTrackerResultConstPtr &result);
+  void feedback_callback(const kr_tracker_msgs::LineTrackerFeedbackConstPtr &feedback);
+};
+
+/*
+ * @brief Constructor to initialize publishers, subscribers and service clients.
+ */
+TrackersManagerTester::TrackersManagerTester() : nh_("")
+{
+  odom_pub_ = nh_.advertise<nav_msgs::Odometry>("trackers_manager/odom", 5, true);
+  position_cmd_sub_ = nh_.subscribe<kr_mav_msgs::PositionCommand>("trackers_manager/cmd", 5,
+                                                                  &TrackersManagerTester::position_cmd_callback, this);
+  tracker_status_sub_ = nh_.subscribe<kr_tracker_msgs::TrackerStatus>(
+      "trackers_manager/status", 5, &TrackersManagerTester::tracker_status_callback, this);
+  distance_client_ = std::make_shared<ClientType>("trackers_manager/line_tracker_distance/LineTracker", true);
+  min_jerk_client_ = std::make_shared<ClientType>("trackers_manager/line_tracker_min_jerk/LineTracker", true);
+  transition_client_ = nh_.serviceClient<kr_tracker_msgs::Transition>("trackers_manager/transition");
+}
+
+/*
+ *	@brief Function to Initialize the tester and to see if the action clients are connected to the servers.
+ *				 Also checks if the subscribers are connected to the publishers.
+ */
+bool TrackersManagerTester::initial_checks()
+{
+  std::lock_guard<std::mutex> lock(mutex);
+  bool flag1, flag2, flag3, temp1, temp2;
+  distance_client_->waitForServer();
+  min_jerk_client_->waitForServer();
+  temp1 = distance_client_->isServerConnected();
+  ros::Duration(0.5).sleep();
+  temp2 = min_jerk_client_->isServerConnected();
+  flag1 = temp1 && temp2;
+
+  temp1 = position_cmd_sub_.getNumPublishers() > 0;
+  ros::Duration(0.5).sleep();
+  temp2 = tracker_status_sub_.getNumPublishers() > 0;
+  flag2 = temp1 && temp2;
+
+  flag3 = transition_client_.exists();
+
+  return flag1 && flag2 && flag3;
+}
+
+/*
+ * @brief Function to publish a single odom message defined by the function arguements.
+ *
+ * @param[in] secs (int) Time in seconds for header
+ * @param[in] nsecs (int) Time in nanoseconds for header
+ * @param[in] pos_x (float) X position
+ * @param[in] pos_y (float) Y position
+ * @param[in] pos_z (float) Z Position
+ * @param[in] orient_x (float) Orientation quaternion X
+ * @param[in] orient_y (float) Orientation quaternion Y
+ * @param[in] orient_z (float) Orientation quaternion Z
+ * @param[in] orient_w (float) Orientation quaternion W
+ */
+void TrackersManagerTester::publish_odom_msg(int secs, int nsecs, float pos_x, float pos_y, float pos_z, float orient_x,
+                                             float orient_y, float orient_z, float orient_w)
+{
+  std::lock_guard<std::mutex> lock(mutex);
+
+  nav_msgs::Odometry msg;
+  msg.header.stamp.sec = secs;
+  msg.header.stamp.nsec = nsecs;
+  msg.pose.pose.position.x = pos_x;
+  msg.pose.pose.position.y = pos_y;
+  msg.pose.pose.position.z = pos_z;
+  msg.pose.pose.orientation.x = orient_x;
+  msg.pose.pose.orientation.y = orient_y;
+  msg.pose.pose.orientation.z = orient_z;
+  msg.pose.pose.orientation.w = orient_w;
+
+  odom_pub_.publish(msg);
+}
+
+/*
+ * @brief Callback function for Position Commands
+ *
+ * @param[in] msg (kr_mav_msgs::PositionCommand::ConstPtr)
+ */
+void TrackersManagerTester::position_cmd_callback(const kr_mav_msgs::PositionCommand::ConstPtr &msg)
+{
+  std::lock_guard<std::mutex> lock(mutex);
+  if(cmd)
+  {
+    cmd.reset();
+    cmd = boost::make_shared<kr_mav_msgs::PositionCommand>(*msg);
+  }
+  else
+  {
+    cmd = boost::make_shared<kr_mav_msgs::PositionCommand>(*msg);
+  }
+  position_cmd_received = true;
+}
+
+/*
+ * @brief Callback function for Tracker Status
+ *
+ * @param[in] msg (kr_tracker_msgs::TrackerStatus::ConstPtr)
+ */
+void TrackersManagerTester::tracker_status_callback(const kr_tracker_msgs::TrackerStatus::ConstPtr &msg)
+{
+  std::lock_guard<std::mutex> lock(mutex);
+  if(status)
+  {
+    status.reset();
+    status = boost::make_shared<kr_tracker_msgs::TrackerStatus>(*msg);
+  }
+  else
+  {
+    status = boost::make_shared<kr_tracker_msgs::TrackerStatus>(*msg);
+  }
+  tracker_status_received = true;
+}
+
+/*
+ * @brief Function to send a tracker transition request and read the response of the server
+ *
+ * @param[in] tracker_name (std::string)
+ */
+void TrackersManagerTester::send_transition_request(std::string tracker_name)
+{
+  std::lock_guard<std::mutex> lock(mutex);
+  kr_tracker_msgs::Transition msg;
+  msg.request.tracker = tracker_name;
+
+  if(transition_client_.call(msg))
+  {
+    srv_succeed = true;
+    srv_response = msg.response.success;
+    srv_msg = msg.response.message;
+  }
+  else
+  {
+    srv_succeed = false;
+  }
+}
+
+/*
+ * @brief Function to send a goal to the action server
+ *
+ * @param[in] tracker_name (std::string)
+ * @param[in] x (float)
+ * @param[in] y (float)
+ * @param[in] z (float)
+ * @param[in] yaw (float)
+ * @param[in] v_des (float)
+ * @param[in] a_des (float)
+ * @param[in] relative (uint8_t)
+ */
+void TrackersManagerTester::send_action_goal(std::string tracker_name, float x, float y, float z, float yaw,
+                                             float v_des, float a_des, uint8_t relative)
+{
+  std::lock_guard<std::mutex> lock(mutex);
+  kr_tracker_msgs::LineTrackerGoal goal;
+  goal.x = x;
+  goal.y = y;
+  goal.z = z;
+  goal.yaw = yaw;
+  goal.v_des = v_des;
+  goal.a_des = a_des;
+  goal.relative = relative;
+
+  if(tracker_name == std::string("kr_trackers/LineTrackerDistance"))
+  {
+    distance_client_->sendGoal(goal, boost::bind(&TrackersManagerTester::done_callback, this, _1, _2),
+                               ClientType::SimpleActiveCallback(),
+                               boost::bind(&TrackersManagerTester::feedback_callback, this, _1));
+  }
+  else if(tracker_name == std::string("kr_trackers/LineTrackerMinJerk"))
+  {
+    min_jerk_client_->sendGoal(goal, boost::bind(&TrackersManagerTester::done_callback, this, _1, _2),
+                               ClientType::SimpleActiveCallback(),
+                               boost::bind(&TrackersManagerTester::feedback_callback, this, _1));
+  }
+  result_received = false;
+  feedback_received = false;
+}
+
+/*
+ * @brief Callback function for when the action goal is completed
+ */
+void TrackersManagerTester::done_callback(const actionlib::SimpleClientGoalState &state,
+                                          const kr_tracker_msgs::LineTrackerResultConstPtr &result)
+{
+  std::lock_guard<std::mutex> lock(mutex);
+  if(action_result)
+  {
+    action_result.reset();
+    action_result = boost::make_shared<kr_tracker_msgs::LineTrackerResult>(*result);
+  }
+  else
+  {
+    action_result = boost::make_shared<kr_tracker_msgs::LineTrackerResult>(*result);
+  }
+  result_received = true;
+}
+
+/*
+ * @brief Callback function to record the feedback from the action server
+ */
+void TrackersManagerTester::feedback_callback(const kr_tracker_msgs::LineTrackerFeedbackConstPtr &feedback)
+{
+  std::lock_guard<std::mutex> lock(mutex);
+  if(action_feedback)
+  {
+    action_feedback.reset();
+    action_feedback = boost::make_shared<kr_tracker_msgs::LineTrackerFeedback>(*feedback);
+  }
+  else
+  {
+    action_feedback = boost::make_shared<kr_tracker_msgs::LineTrackerFeedback>(*feedback);
+  }
+  feedback_received = true;
+}
+
+/*
+ * @brief Function to reset the flags set by callbacks
+ */
+void TrackersManagerTester::reset_flags()
+{
+  std::lock_guard<std::mutex> lock(mutex);
+  srv_succeed = false;
+  position_cmd_received = false;
+  tracker_status_received = false;
+  result_received = false;
+  feedback_received = false;
+}
+
+/*
+ * @brief Struct to store reference data for Test2
+ */
+struct Test2Data
+{
+  bool srv_response_success[3] = {false, false, false};
+  std::string srv_response_msg[3] = {"Cannot find tracker LineTrackerDistance, cannot transition",
+                                     "Failed to activate tracker kr_trackers/LineTrackerDistance, cannot transition",
+                                     "Failed to activate tracker kr_trackers/LineTrackerMinJerk, cannot transition"};
+};
+
+/*
+ * @brief Struct to store data for Test3.
+ *        Send a goal, activate tracker and send odom messages to reach goal.
+ *        A value of -100 indicates no need to check that value
+ */
+struct Test3Data
+{
+  int odom_secs[7] = {0, 0, 0, 0, 0, 0, 0};
+  int odom_nsecs[7] = {100000000, 200000000, 300000000, 400000000, 500000000, 600000000, 700000000};
+  float odom_pos_x[7] = {0.0, 0.1, 0.3, 0.5, 0.7, 0.9, 1.0};
+  float odom_pos_y[7] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
+  float odom_pos_z[7] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
+  float odom_orient_x[7] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
+  float odom_orient_y[7] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
+  float odom_orient_z[7] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
+  float odom_orient_w[7] = {1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0};
+
+  float feedback[7] = {1.0, 0.89999, 0.69999, 0.5, 0.3, 0.1, -100.0};
+
+  float cmd_pos_x[7] = {0.005, 0.1497213, 0.38245967, 0.595, 0.77245968, 0.939721345, 1.0};
+  float cmd_pos_y[7] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
+  float cmd_pos_z[7] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
+  float cmd_vel_x[7] = {0.0, 0.44721359, 0.77459669, 1.0, 0.77459669, 0.447213649, 0.0};
+  float cmd_vel_y[7] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
+  float cmd_vel_z[7] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
+  float cmd_accel_x[7] = {1.0, 1.0, 1.0, -1.0, -1.0, -1.0, 0.0};
+  float cmd_accel_y[7] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
+  float cmd_accel_z[7] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
+  float cmd_jerk_x[7] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
+  float cmd_jerk_y[7] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
+  float cmd_jerk_z[7] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
+  double cmd_yaw[7] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
+  double cmd_yawdot[7] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
+
+  std::string status_tracker[7] = {"kr_trackers/LineTrackerDistance", "kr_trackers/LineTrackerDistance",
+                                   "kr_trackers/LineTrackerDistance", "kr_trackers/LineTrackerDistance",
+                                   "kr_trackers/LineTrackerDistance", "kr_trackers/LineTrackerDistance",
+                                   "kr_trackers/LineTrackerDistance"};
+  uint8_t status_status[7] = {0, 0, 0, 0, 0, 0, 1};
+
+  float result_x = 1.0;
+  float result_y = 0.0;
+  float result_z = 0.0;
+  float result_yaw = 0.0;
+  float result_duration = 0.7;
+  float result_length = 1.0;
+  uint8_t result_status = 3;
+};
+
+/*
+ * @brief Struct to store data for Test4.
+ *        Send a goal, send some odom messages, send another goal, send some odom messages to complete new goal.
+ *        A value of -100 indicates no need to check that value
+ */
+struct Test4Data
+{
+  int odom_secs[7] = {-100, 1, 1, 1, -100, 1, 1};
+  int odom_nsecs[7] = {-100, 000000000, 100000000, 400000000, -100, 600000000, 700000000};
+  float odom_pos_x[7] = {-100.0, 1.0, 1.2, 1.6, -100.0, 1.8, 1.94};
+  float odom_pos_y[7] = {-100.0, 0.0, 0.0, 0.0, -100.0, 0.0, 0.0};
+  float odom_pos_z[7] = {-100.0, 0.0, 0.0, 0.0, -100.0, 0.0, 0.0};
+  float odom_orient_x[7] = {-100.0, 0.0, 0.0, 0.0, -100.0, 0.0, 0.0};
+  float odom_orient_y[7] = {-100.0, 0.0, 0.0, 0.0, -100.0, 0.0, 0.0};
+  float odom_orient_z[7] = {-100.0, 0.0, 0.0, 0.0, -100.0, 0.0, 0.0};
+  float odom_orient_w[7] = {-100.0, 1.0, 1.0, 1.0, -100.0, 1.0, 1.0};
+
+  float feedback[7] = {-100.0, 2.0, 1.79999, 1.39999, -100.0, 0.2, -100.0};
+
+  float cmd_pos_x[7] = {-100.0, 1.0449999, 1.268245577, 1.9736335277, -100.0, 1.946491, 2.0};
+  float cmd_pos_y[7] = {-100.0, 0.0, 0.0, 0.0, -100.0, 0.0, 0.0};
+  float cmd_pos_z[7] = {-100.0, 0.0, 0.0, 0.0, -100.0, 0.0, 0.0};
+  float cmd_vel_x[7] = {-100.0, 0.0, 0.632455587, 1.09544515, -100.0, 0.6324554, 0.0};
+  float cmd_vel_y[7] = {-100.0, 0.0, 0.0, 0.0, -100.0, 0.0, 0.0};
+  float cmd_vel_z[7] = {-100.0, 0.0, 0.0, 0.0, -100.0, 0.0, 0.0};
+  float cmd_accel_x[7] = {-100.0, 1.0, 1.0, 1.0, -100.0, 1.0, 0.0};
+  float cmd_accel_y[7] = {-100.0, 0.0, 0.0, 0.0, -100.0, 0.0, 0.0};
+  float cmd_accel_z[7] = {-100.0, 0.0, 0.0, 0.0, -100.0, 0.0, 0.0};
+  float cmd_jerk_x[7] = {-100.0, 0.0, 0.0, 0.0, -100.0, 0.0, 0.0};
+  float cmd_jerk_y[7] = {-100.0, 0.0, 0.0, 0.0, -100.0, 0.0, 0.0};
+  float cmd_jerk_z[7] = {-100.0, 0.0, 0.0, 0.0, -100.0, 0.0, 0.0};
+  double cmd_yaw[7] = {-100.0, 0.0, 0.0, 0.0, -100.0, 0.0, 0.0};
+  double cmd_yawdot[7] = {-100.0, 0.0, 0.0, 0.0, -100.0, 0.0, 0.0};
+
+  std::string status_tracker[7] = {"NoCheck",
+                                   "kr_trackers/LineTrackerDistance",
+                                   "kr_trackers/LineTrackerDistance",
+                                   "kr_trackers/LineTrackerDistance",
+                                   "NoCheck",
+                                   "kr_trackers/LineTrackerDistance",
+                                   "kr_trackers/LineTrackerDistance"};
+  uint8_t status_status[7] = {0, 0, 0, 0, 0, 0, 1};
+
+  float result_x = 2.0;
+  float result_y = 0.0;
+  float result_z = 0.0;
+  float result_yaw = 0.0;
+  float result_duration = 0.3;
+  float result_length = 0.34;
+  uint8_t result_status = 3;
+};
diff --git a/trackers/kr_trackers_manager/package.xml b/trackers/kr_trackers_manager/package.xml
index aa5fc728..1060fee7 100644
--- a/trackers/kr_trackers_manager/package.xml
+++ b/trackers/kr_trackers_manager/package.xml
@@ -13,12 +13,16 @@
 
   <buildtool_depend>catkin</buildtool_depend>
   <depend>roscpp</depend>
+  <depend>actionlib</depend>
   <depend>nodelet</depend>
   <depend>pluginlib</depend>
   <depend>kr_mav_msgs</depend>
   <depend>nav_msgs</depend>
   <depend>kr_tracker_msgs</depend>
 
+  <test_depend>rostest</test_depend>
+  <test_depend>gtest</test_depend>
+
   <export>
     <nodelet plugin="${prefix}/nodelet_plugin.xml"/>
   </export>
diff --git a/trackers/kr_trackers_manager/test/kr_trackers_manager_nodelet.test b/trackers/kr_trackers_manager/test/kr_trackers_manager_nodelet.test
new file mode 100644
index 00000000..d2b76855
--- /dev/null
+++ b/trackers/kr_trackers_manager/test/kr_trackers_manager_nodelet.test
@@ -0,0 +1,9 @@
+<launch>
+  <node pkg="nodelet" type="nodelet" name="kr_trackers_nodelet_manager" args="manager"/>
+  <node pkg="nodelet" type="nodelet" name="trackers_manager" args="load kr_trackers_manager/TrackersManager kr_trackers_nodelet_manager" output="screen">
+  <rosparam file="$(find kr_trackers_manager)/config/trackers.yaml"/>
+	<rosparam file="$(find kr_trackers_manager)/config/tracker_params.yaml"/> 
+  </node>
+
+  <test test-name="kr_trackers_manager_test" pkg="kr_trackers_manager" type="kr_trackers_manager_test" time-limit="60.0"/>
+</launch>
diff --git a/trackers/kr_trackers_manager/test/kr_trackers_manager_test.cpp b/trackers/kr_trackers_manager/test/kr_trackers_manager_test.cpp
new file mode 100644
index 00000000..e73f8c1a
--- /dev/null
+++ b/trackers/kr_trackers_manager/test/kr_trackers_manager_test.cpp
@@ -0,0 +1,229 @@
+#include <gtest/gtest.h>
+#include <ros/ros.h>
+
+#include <mutex>
+#include <thread>
+
+#include "kr_trackers_manager/tester_utils.hpp"
+
+/*
+ * @brief Test1: test if the tester connects properly with the trackers_manager.
+ */
+TEST(TrackersManagerTest, InitializationChecks)
+{
+  TrackersManagerTester tester;
+  ASSERT_TRUE(tester.initial_checks());
+}
+
+/*
+ * @brief Test2: test if any tracker is found or activated using the transition service.
+          No tracker should be activated without any prior odom or goal messages.
+ */
+TEST(TrackersManagerTest, TrackerTransitionCheck)
+{
+  TrackersManagerTester tester;
+  Test2Data data;
+  std::string tracker_names[3] = {"LineTrackerDistance", "kr_trackers/LineTrackerDistance",
+                                  "kr_trackers/LineTrackerMinJerk"};
+  for(int i = 0; i < 3; i++)
+  {
+    tester.send_transition_request(tracker_names[i]);
+    ros::Duration(0.5).sleep();
+    {
+      std::lock_guard<std::mutex> lock(tester.mutex);
+      ASSERT_TRUE(tester.srv_succeed);
+      EXPECT_EQ(tester.srv_response, data.srv_response_success[i]);
+      EXPECT_EQ(tester.srv_msg, data.srv_response_msg[i]);
+    }
+    tester.reset_flags();
+  }
+}
+
+/*
+ * @brief Test3: Send a goal, activate tracker and send odom messages to reach goal.
+ *        A value of -100 indicates no need to check that value/index.
+ *        This test is solely to tests the action client and server communication.
+ *        1. Send a goal
+ *        2. Send a odom message
+ *        3. Send a tracker transition request
+ *        4. Send multiple odom messages until goal is reached.
+ */
+TEST(TrackersManagerTest, GoalCompletionCheck)
+{
+  TrackersManagerTester tester;
+  Test3Data data;
+  std::string tracker_name = "kr_trackers/LineTrackerDistance";
+  ros::Duration(1.0).sleep();
+  tester.send_action_goal(tracker_name, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0);
+  ros::Duration(0.5).sleep();
+  tester.publish_odom_msg(0, 0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0);
+  ros::Duration(0.5).sleep();
+  tester.send_transition_request(tracker_name);
+  ros::Duration(0.5).sleep();
+  {
+    std::lock_guard<std::mutex> lock(tester.mutex);
+    ASSERT_TRUE(tester.srv_succeed);
+    EXPECT_TRUE(tester.srv_response);
+  }
+  int num_samples = 7;
+  for(int i = 0; i < num_samples; i++)
+  {
+    tester.publish_odom_msg(data.odom_secs[i], data.odom_nsecs[i], data.odom_pos_x[i], data.odom_pos_y[i],
+                            data.odom_pos_z[i], data.odom_orient_x[i], data.odom_orient_y[i], data.odom_orient_z[i],
+                            data.odom_orient_w[i]);
+    ros::Duration(1.0).sleep();
+    {
+      std::lock_guard<std::mutex> lock(tester.mutex);
+
+      ASSERT_TRUE(tester.position_cmd_received);
+      EXPECT_NEAR(tester.cmd->position.x, data.cmd_pos_x[i], 1e-4);
+      EXPECT_NEAR(tester.cmd->position.y, data.cmd_pos_y[i], 1e-4);
+      EXPECT_NEAR(tester.cmd->position.z, data.cmd_pos_z[i], 1e-4);
+      EXPECT_NEAR(tester.cmd->velocity.x, data.cmd_vel_x[i], 1e-4);
+      EXPECT_NEAR(tester.cmd->velocity.y, data.cmd_vel_y[i], 1e-4);
+      EXPECT_NEAR(tester.cmd->velocity.z, data.cmd_vel_z[i], 1e-4);
+      EXPECT_NEAR(tester.cmd->acceleration.x, data.cmd_accel_x[i], 1e-4);
+      EXPECT_NEAR(tester.cmd->acceleration.y, data.cmd_accel_y[i], 1e-4);
+      EXPECT_NEAR(tester.cmd->acceleration.z, data.cmd_accel_z[i], 1e-4);
+      EXPECT_NEAR(tester.cmd->jerk.x, data.cmd_jerk_x[i], 1e-4);
+      EXPECT_NEAR(tester.cmd->jerk.y, data.cmd_jerk_y[i], 1e-4);
+      EXPECT_NEAR(tester.cmd->jerk.z, data.cmd_jerk_z[i], 1e-4);
+      EXPECT_NEAR(tester.cmd->yaw, data.cmd_yaw[i], 1e-4);
+      EXPECT_NEAR(tester.cmd->yaw_dot, data.cmd_yawdot[i], 1e-4);
+
+      if(data.feedback[i] != -100.0)
+      {
+        ASSERT_TRUE(tester.feedback_received);
+        EXPECT_NEAR(tester.action_feedback->distance_from_goal, data.feedback[i], 1e-4);
+      }
+
+      EXPECT_EQ(tester.status->tracker, data.status_tracker[i]);
+      EXPECT_EQ(tester.status->status, data.status_status[i]);
+
+      if(i == num_samples - 1)
+      {
+        EXPECT_EQ(tester.action_result->x, data.result_x);
+        EXPECT_EQ(tester.action_result->y, data.result_y);
+        EXPECT_EQ(tester.action_result->z, data.result_z);
+        EXPECT_EQ(tester.action_result->yaw, data.result_yaw);
+        EXPECT_EQ(tester.action_result->length, data.result_length);
+        EXPECT_NEAR(tester.action_result->duration, data.result_duration, 1e-4);
+      }
+    }
+    tester.reset_flags();
+  }
+}
+
+/*
+ * @brief Test4: Send a goal during an active goal and see if the first goal is cancelled new goal accepted.
+ *        A value of -100 indicates no need to check that value/index.
+ *        This test is solely to tests the action client and server communication.
+ *        1. Send a goal
+ *        2. Send some odom messages
+ *        3. Send another foal
+ *        4. Send odom messages until new goal is reached.
+ */
+TEST(TrackersManagerTest, GoalPreEmptionCheck)
+{
+  TrackersManagerTester tester;
+  Test4Data data;
+  std::string tracker_name = "kr_trackers/LineTrackerDistance";
+  ros::Duration(1.0).sleep();
+  tester.send_action_goal(tracker_name, 3.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0);
+  ros::Duration(0.5).sleep();
+  int num_samples = 7;
+  for(int i = 0; i < num_samples; i++)
+  {
+    // sending a new goal while a goal is active
+    if(i == 4)
+    {
+      tester.send_action_goal(tracker_name, 2.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0);
+      ros::Duration(0.5).sleep();
+      continue;
+    }
+
+    if(data.odom_secs[i] != -100)
+    {
+      tester.publish_odom_msg(data.odom_secs[i], data.odom_nsecs[i], data.odom_pos_x[i], data.odom_pos_y[i],
+                              data.odom_pos_z[i], data.odom_orient_x[i], data.odom_orient_y[i], data.odom_orient_z[i],
+                              data.odom_orient_w[i]);
+      ros::Duration(1.2).sleep();
+    }
+
+    {
+      std::lock_guard<std::mutex> lock(tester.mutex);
+
+      if(data.cmd_pos_x[i] != -100.0)
+      {
+        ASSERT_TRUE(tester.position_cmd_received);
+        EXPECT_NEAR(tester.cmd->position.x, data.cmd_pos_x[i], 1e-4);
+        EXPECT_NEAR(tester.cmd->position.y, data.cmd_pos_y[i], 1e-4);
+        EXPECT_NEAR(tester.cmd->position.z, data.cmd_pos_z[i], 1e-4);
+        EXPECT_NEAR(tester.cmd->velocity.x, data.cmd_vel_x[i], 1e-4);
+        EXPECT_NEAR(tester.cmd->velocity.y, data.cmd_vel_y[i], 1e-4);
+        EXPECT_NEAR(tester.cmd->velocity.z, data.cmd_vel_z[i], 1e-4);
+        EXPECT_NEAR(tester.cmd->acceleration.x, data.cmd_accel_x[i], 1e-4);
+        EXPECT_NEAR(tester.cmd->acceleration.y, data.cmd_accel_y[i], 1e-4);
+        EXPECT_NEAR(tester.cmd->acceleration.z, data.cmd_accel_z[i], 1e-4);
+        EXPECT_NEAR(tester.cmd->jerk.x, data.cmd_jerk_x[i], 1e-4);
+        EXPECT_NEAR(tester.cmd->jerk.y, data.cmd_jerk_y[i], 1e-4);
+        EXPECT_NEAR(tester.cmd->jerk.z, data.cmd_jerk_z[i], 1e-4);
+        EXPECT_NEAR(tester.cmd->yaw, data.cmd_yaw[i], 1e-4);
+        EXPECT_NEAR(tester.cmd->yaw_dot, data.cmd_yawdot[i], 1e-4);
+      }
+
+      if(data.feedback[i] != -100.0)
+      {
+        ASSERT_TRUE(tester.feedback_received);
+        EXPECT_NEAR(tester.action_feedback->distance_from_goal, data.feedback[i], 1e-4);
+      }
+
+      if(data.status_tracker[i] != std::string("NoCheck"))
+      {
+        EXPECT_EQ(tester.status->tracker, data.status_tracker[i]);
+        EXPECT_EQ(tester.status->status, data.status_status[i]);
+      }
+
+      if(i == 4)
+      {
+        EXPECT_EQ(tester.action_result->x, 0.0);
+        EXPECT_EQ(tester.action_result->y, 0.0);
+        EXPECT_EQ(tester.action_result->z, 0.0);
+        EXPECT_EQ(tester.action_result->yaw, 0.0);
+        EXPECT_EQ(tester.action_result->length, 0.0);
+        EXPECT_NEAR(tester.action_result->duration, 0.0, 1e-4);
+      }
+      else if(i == num_samples - 1)
+      {
+        EXPECT_EQ(tester.action_result->x, data.result_x);
+        EXPECT_EQ(tester.action_result->y, data.result_y);
+        EXPECT_EQ(tester.action_result->z, data.result_z);
+        EXPECT_EQ(tester.action_result->yaw, data.result_yaw);
+        EXPECT_NEAR(tester.action_result->length, data.result_length, 1e-4);
+        EXPECT_NEAR(tester.action_result->duration, data.result_duration, 1e-4);
+      }
+    }
+    tester.reset_flags();
+  }
+}
+
+int main(int argc, char **argv)
+{
+  ros::init(argc, argv, "kr_trackers_manager_tester");
+  testing::InitGoogleTest(&argc, argv);
+
+  std::thread t(
+      []
+      {
+        while(ros::ok())
+          ros::spin();
+      });
+
+  auto res = RUN_ALL_TESTS();
+
+  ros::shutdown();
+
+  t.join();
+
+  return res;
+}
diff --git a/trackers/kr_trackers_manager/test/sample.launch b/trackers/kr_trackers_manager/test/sample.launch
new file mode 100644
index 00000000..2417d3ff
--- /dev/null
+++ b/trackers/kr_trackers_manager/test/sample.launch
@@ -0,0 +1,7 @@
+<launch>
+  <node pkg="nodelet" type="nodelet" name="kr_trackers_nodelet_manager" args="manager"/>
+  <node pkg="nodelet" type="nodelet" name="trackers_manager" args="load kr_trackers_manager/TrackersManager kr_trackers_nodelet_manager" output="screen">
+  <rosparam file="$(find kr_trackers_manager)/config/trackers.yaml"/>
+	<rosparam file="$(find kr_trackers_manager)/config/tracker_params.yaml"/> 
+  </node>
+</launch>