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 c8353f46..977542d5 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 @@ -19,12 +19,22 @@ class TrackersManagerTester 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; + 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); bool srv_response; std::string srv_msg; - bool srv_succeed; + 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_; @@ -35,6 +45,9 @@ 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); }; TrackersManagerTester::TrackersManagerTester() : nh_("") @@ -49,9 +62,54 @@ TrackersManagerTester::TrackersManagerTester() : nh_("") transition_client_ = nh_.serviceClient("trackers_manager/transition"); } -void TrackersManagerTester::position_cmd_callback(const kr_mav_msgs::PositionCommand::ConstPtr &msg) {} +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 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; -void TrackersManagerTester::tracker_status_callback(const kr_tracker_msgs::TrackerStatus::ConstPtr &msg) {} + odom_pub_.publish(msg); +} + +void TrackersManagerTester::position_cmd_callback(const kr_mav_msgs::PositionCommand::ConstPtr &msg) +{ + std::lock_guard lock(mutex); + if(cmd) + { + cmd.reset(); + cmd = boost::make_shared(*msg); + } + else + { + cmd = boost::make_shared(*msg); + } + position_cmd_received = true; +} + +void TrackersManagerTester::tracker_status_callback(const kr_tracker_msgs::TrackerStatus::ConstPtr &msg) +{ + std::lock_guard lock(mutex); + if(status) + { + status.reset(); + status = boost::make_shared(*msg); + } + else + { + status = boost::make_shared(*msg); + } + tracker_status_received = true; +} /* * @brief Function to Initialize the tester and to see if the action clients are connected to the servers. @@ -96,8 +154,68 @@ void TrackersManagerTester::send_transition_request(std::string tracker_name) } } +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 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; +} + +void TrackersManagerTester::done_callback(const actionlib::SimpleClientGoalState &state, + const kr_tracker_msgs::LineTrackerResultConstPtr &result) +{ + std::lock_guard lock(mutex); + if(action_result) + { + action_result.reset(); + action_result = boost::make_shared(*result); + } + else + { + action_result = boost::make_shared(*result); + } + result_received = true; +} + +void TrackersManagerTester::feedback_callback(const kr_tracker_msgs::LineTrackerFeedbackConstPtr &feedback) +{ + std::lock_guard lock(mutex); + if(action_feedback) + { + action_feedback.reset(); + action_feedback = boost::make_shared(*feedback); + } + else + { + action_feedback = boost::make_shared(*feedback); + } + feedback_received = true; +} + /* - * @brief: Struct to store reference data for Test2 + * @brief Struct to store reference data for Test2 */ struct Test2Data { @@ -107,4 +225,100 @@ struct Test2Data "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; +}; + +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; +}; + #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 146735ce..4c9f7fe7 100644 --- a/trackers/kr_trackers_manager/test/kr_trackers_manager_test.cpp +++ b/trackers/kr_trackers_manager/test/kr_trackers_manager_test.cpp @@ -21,7 +21,7 @@ TEST(TrackersManagerTest, Test1) TEST(TrackersManagerTest, Test2) { TrackersManagerTester tester; - Test2Data ref; + Test2Data data; std::string tracker_names[3] = {"LineTrackerDistance", "kr_trackers/LineTrackerDistance", "kr_trackers/LineTrackerMinJerk"}; for(int i = 0; i < 3; i++) @@ -31,8 +31,158 @@ TEST(TrackersManagerTest, Test2) { std::lock_guard 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]); + EXPECT_EQ(tester.srv_response, data.srv_response_success[i]); + EXPECT_EQ(tester.srv_msg, data.srv_response_msg[i]); + } + } +} + +/* + * @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 + */ +TEST(TrackersManagerTest, Test3) +{ + 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 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 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); + } + } +} + +TEST(TrackersManagerTest, Test4) +{ + 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++) + { + 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.0).sleep(); + } + + { + std::lock_guard 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); + } } } }