WIP: Unit tests scripts and setup for kr_trackers_manager

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()

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

trackers/kr_trackers_manager/config/trackers.yaml

@@ -0,0 +1,4 @@
+trackers:
+  - kr_trackers/LineTrackerMinJerk
+  - kr_trackers/LineTrackerDistance
+

trackers/kr_trackers_manager/include/kr_trackers_manager/tester_utils.hpp

@@ -0,0 +1,110 @@
+#ifndef TESTER_UTILS_HPP
+#define TESTER_UTILS_HPP
+
+#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>
+
+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 done_callback(const actionlib::SimpleClientGoalState &state, const
+  // kr_tracker_msgs::LineTrackerActionResultConstPtr &result);
+  std::mutex mutex;
+  bool srv_response;
+  std::string srv_msg;
+  bool srv_succeed;
+
+ 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_;
+};
+
+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");
+}
+
+void TrackersManagerTester::position_cmd_callback(const kr_mav_msgs::PositionCommand::ConstPtr &msg) {}
+
+void TrackersManagerTester::tracker_status_callback(const kr_tracker_msgs::TrackerStatus::ConstPtr &msg) {}
+
+/*
+ *	@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;
+}
+
+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: 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"};
+};
+
+#endif  // TESTER_UTILS_HPP

trackers/kr_trackers_manager/package.xml

@@ -6,19 +6,24 @@
   <maintainer email="[email protected]">Kartik Mohta</maintainer>
   <maintainer email="[email protected]">Michael Watterson</maintainer>
   <maintainer email="[email protected]">Justin Thomas</maintainer>
+  <maintainer email="[email protected]">Pranav Shah</maintainer>
 
   <license>BSD</license>
 
   <author>Kartik Mohta</author>
 
   <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>

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>

trackers/kr_trackers_manager/test/kr_trackers_manager_test.cpp

@@ -0,0 +1,59 @@
+#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, Test1)
+{
+  TrackersManagerTester tester;
+  ASSERT_TRUE(tester.initial_checks());
+}
+
+/*
+ * @brief Test2: test if any tracker is found or activated using the transition service
+ */
+TEST(TrackersManagerTest, Test2)
+{
+  TrackersManagerTester tester;
+  Test2Data ref;
+  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, ref.srv_response_success[i]);
+      EXPECT_EQ(tester.srv_msg, ref.srv_response_msg[i]);
+    }
+  }
+}
+
+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;
+}

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>