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 index 977542d5..38a3dc68 100644 --- a/trackers/kr_trackers_manager/include/kr_trackers_manager/tester_utils.hpp +++ b/trackers/kr_trackers_manager/include/kr_trackers_manager/tester_utils.hpp @@ -1,5 +1,4 @@ -#ifndef TESTER_UTILS_HPP -#define TESTER_UTILS_HPP +#pragma once #include #include @@ -11,6 +10,9 @@ #include +/* + * @brief Class of helper functions and variables to test kr_trackers_manager + */ class TrackersManagerTester { public: @@ -23,6 +25,7 @@ class TrackersManagerTester 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; @@ -45,11 +48,15 @@ class TrackersManagerTester typedef actionlib::SimpleActionClient ClientType; std::shared_ptr distance_client_; std::shared_ptr 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("trackers_manager/odom", 5, true); @@ -62,6 +69,44 @@ TrackersManagerTester::TrackersManagerTester() : nh_("") transition_client_ = nh_.serviceClient("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 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) { @@ -81,6 +126,11 @@ void TrackersManagerTester::publish_odom_msg(int secs, int nsecs, float pos_x, f 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 lock(mutex); @@ -96,6 +146,11 @@ void TrackersManagerTester::position_cmd_callback(const kr_mav_msgs::PositionCom 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 lock(mutex); @@ -112,30 +167,10 @@ void TrackersManagerTester::tracker_status_callback(const kr_tracker_msgs::Track } /* - * @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. + * @brief Function to send a tracker transition request and read the response of the server + * + * @param[in] tracker_name (std::string) */ -bool TrackersManagerTester::initial_checks() -{ - std::lock_guard 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 lock(mutex); @@ -154,6 +189,18 @@ void TrackersManagerTester::send_transition_request(std::string tracker_name) } } +/* + * @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) { @@ -183,6 +230,9 @@ void TrackersManagerTester::send_action_goal(std::string tracker_name, float x, 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) { @@ -199,6 +249,9 @@ void TrackersManagerTester::done_callback(const actionlib::SimpleClientGoalState 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 lock(mutex); @@ -214,6 +267,19 @@ void TrackersManagerTester::feedback_callback(const kr_tracker_msgs::LineTracker feedback_received = true; } +/* + * @brief Function to reset the flags set by callbacks + */ +void TrackersManagerTester::reset_flags() +{ + std::lock_guard 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 */ @@ -274,6 +340,11 @@ struct Test3Data 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}; @@ -320,5 +391,3 @@ struct Test4Data float result_length = 0.34; uint8_t result_status = 3; }; - -#endif // TESTER_UTILS_HPP diff --git a/trackers/kr_trackers_manager/test/kr_trackers_manager_test.cpp b/trackers/kr_trackers_manager/test/kr_trackers_manager_test.cpp index 4c9f7fe7..e73f8c1a 100644 --- a/trackers/kr_trackers_manager/test/kr_trackers_manager_test.cpp +++ b/trackers/kr_trackers_manager/test/kr_trackers_manager_test.cpp @@ -9,16 +9,17 @@ /* * @brief Test1: test if the tester connects properly with the trackers_manager. */ -TEST(TrackersManagerTest, Test1) +TEST(TrackersManagerTest, InitializationChecks) { TrackersManagerTester tester; ASSERT_TRUE(tester.initial_checks()); } /* - * @brief Test2: test if any tracker is found or activated using the transition service + * @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, Test2) +TEST(TrackersManagerTest, TrackerTransitionCheck) { TrackersManagerTester tester; Test2Data data; @@ -34,14 +35,20 @@ TEST(TrackersManagerTest, Test2) 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 + * 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, Test3) +TEST(TrackersManagerTest, GoalCompletionCheck) { TrackersManagerTester tester; Test3Data data; @@ -92,21 +99,31 @@ TEST(TrackersManagerTest, Test3) 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); + 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(); } } -TEST(TrackersManagerTest, Test4) +/* + * @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; @@ -117,6 +134,7 @@ TEST(TrackersManagerTest, Test4) 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); @@ -129,11 +147,12 @@ TEST(TrackersManagerTest, Test4) 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(); + ros::Duration(1.2).sleep(); } { std::lock_guard lock(tester.mutex); + if(data.cmd_pos_x[i] != -100.0) { ASSERT_TRUE(tester.position_cmd_received); @@ -184,6 +203,7 @@ TEST(TrackersManagerTest, Test4) EXPECT_NEAR(tester.action_result->duration, data.result_duration, 1e-4); } } + tester.reset_flags(); } }