Skip to content

Commit

Permalink
updated comments
Browse files Browse the repository at this point in the history
  • Loading branch information
@pranavpshah
pranavpshah committed Oct 15, 2024
1 parent 04d6dbe commit 2d00e3a
Show file tree
Hide file tree
Showing 2 changed files with 132 additions and 43 deletions.
123 changes: 96 additions & 27 deletions trackers/kr_trackers_manager/include/kr_trackers_manager/tester_utils.hpp
View file
Original file line number Diff line number Diff line change
@@ -1,5 +1,4 @@
#ifndef TESTER_UTILS_HPP
#define TESTER_UTILS_HPP
#pragma once

#include <actionlib/client/simple_action_client.h>
#include <kr_mav_msgs/PositionCommand.h>
Expand All @@ -11,6 +10,9 @@

#include <vector>

/*
* @brief Class of helper functions and variables to test kr_trackers_manager
*/
class TrackersManagerTester
{
public:
Expand All @@ -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;
Expand All @@ -45,11 +48,15 @@ class TrackersManagerTester
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);
Expand All @@ -62,6 +69,44 @@ TrackersManagerTester::TrackersManagerTester() : nh_("")
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)
{
Expand All @@ -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<std::mutex> lock(mutex);
Expand All @@ -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<std::mutex> lock(mutex);
Expand All @@ -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<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);
Expand All @@ -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)
{
Expand Down Expand Up @@ -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)
{
Expand All @@ -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<std::mutex> lock(mutex);
Expand All @@ -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<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
*/
Expand Down Expand Up @@ -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};
Expand Down Expand Up @@ -320,5 +391,3 @@ struct Test4Data
float result_length = 0.34;
uint8_t result_status = 3;
};

#endif // TESTER_UTILS_HPP
52 changes: 36 additions & 16 deletions trackers/kr_trackers_manager/test/kr_trackers_manager_test.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand All @@ -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;
Expand Down Expand Up @@ -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;
Expand All @@ -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);
Expand All @@ -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<std::mutex> lock(tester.mutex);

if(data.cmd_pos_x[i] != -100.0)
{
ASSERT_TRUE(tester.position_cmd_received);
Expand Down Expand Up @@ -184,6 +203,7 @@ TEST(TrackersManagerTest, Test4)
EXPECT_NEAR(tester.action_result->duration, data.result_duration, 1e-4);
}
}
tester.reset_flags();
}
}

Expand Down

0 comments on commit 2d00e3a

Please sign in to comment.