public/Tangent_Planner
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Update gtest.cpp

Browse Source
This commit is contained in:
majid
2024-04-05 08:37:43 +00:00
parent e9f436ddd7
commit ecbae9092b
1 changed files with 1 additions and 548 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/gtest.cpp
+1 -548
View File
@@ -143,556 +143,9 @@ TEST(TangentPlanner, average){
}
}
TEST(TangentPlanner, normalizeAngle) {
Planner::TangentPlanner rftp = Planner::TangentPlanner();
rftp.angle_error_t_ = (-3 * M_PI - M_PI_2) * 0.99;
double angle = -3 * M_PI - M_PI_2;
angle = rftp.normalizeAngle(angle);
std::cerr << angle;
// EXPECT_NEAR(angle, 0.1, 0.01);
// angle = M_PI_2;
// angle = rftp.normalizeAngle(angle);
// EXPECT_NEAR(angle, M_PI_2, 0.00001);
// angle = M_PI * 3;
// angle = rftp.normalizeAngle(angle);
// EXPECT_NEAR(angle, 0.1, 0.01);
// angle = M_PI_2; // angle = pi/2; fn should return pi/2
// rftp.normalizeAngle(angle);
// EXPECT_NEAR(angle, 1.5708, 0.00001);
// angle = M_PI;
// rftp.normalizeAngle(angle);
// EXPECT_NEAR(angle, 3.14159, 0.00001);
// angle = M_PI + M_PI_2;
// rftp.normalizeAngle(angle);
// EXPECT_NEAR(angle, 4.71239, 0.00001);
// angle = M_PI * 2 + 1;
// std::cerr << angle << std::endl;
// rftp.normalizeAngle(angle);
// std::cerr << angle << std::endl;
// EXPECT_NEAR(angle, 0, 0.00001);
// angle = M_PI + M_PI_2; // angle = 3pi/2; fn should return -pi/2
// rftp.normalizeAngle(angle);
// EXPECT_NEAR(angle, -m_pi/2, 0.001);
//
// angle = 2*M_PI + M_PI_2; //angle = 2pi+(pi/2); fn should return pi/2
// rftp.normalizeAngle(angle);
// EXPECT_NEAR(angle, m_pi/2, 0.001);
//
// angle = -M_PI - M_PI_2; //angle = -3pi/2; fn should return pi/2
// rftp.normalizeAngle(angle);
// EXPECT_NEAR(angle, m_pi/2, 0.001);
//
// angle = -2*M_PI - M_PI_2; // angle = -2pi-(pi/2); fn should return -pi/2
// rftp.normalizeAngle(angle);
// EXPECT_NEAR(angle, -m_pi/2, 0.001);
//
// angle = M_PI+0.1;
// // note angle > M_PI this time returns true
// rftp.normalizeAngle(angle);
// EXPECT_NEAR(angle, m_pi+0.1-2*m_pi, 0.001);
//
// angle = -M_PI;
// rftp.normalizeAngle(angle);
// EXPECT_NEAR(angle, m_pi, 0.001);
// std::cerr << rftp.normalizeAngle(0.022850);
//}
//TEST(TangentPlanner, normalizeAngle2){
// Planner::TangentPlanner rftp = Planner::TangentPlanner();
// float angle = -6.389263;
// rftp.angle_error_t_ = -0.2;
// rftp.normalizeAngle(angle);
// std::cerr << angle;
//}
TEST(TangentPlanner, setGoal) {
Planner::TangentPlanner rftp = Planner::TangentPlanner();
EXPECT_FALSE(rftp.setGoal());
std::shared_ptr<geometry_msgs::Pose2D> point = std::make_shared<geometry_msgs::Pose2D>();
point->x = 0;
point->y = 0;
point->theta = 0;
rftp.global_plan_.push_back(point);
EXPECT_TRUE(rftp.setGoal());
rftp.valid_dis_to_goal_ = 0.1;
EXPECT_FALSE(rftp.setGoal());
}
//
//TEST(TangentPlanner, isInGoalDirection) {
// Planner::TangentPlanner rftp = Planner::TangentPlanner();
// rftp.yaw_goal_tolerance_ = 2;
// rftp.goal_angle_ = 1.5;
// EXPECT_EQ(rftp.isInGoalDirection(), true);
// rftp.goal_angle_ = -1.5;
// EXPECT_EQ(rftp.isInGoalDirection(), true);
// rftp.goal_angle_ = 2;
// EXPECT_EQ(rftp.isInGoalDirection(), false);
// rftp.goal_angle_ = 3;
// EXPECT_EQ(rftp.isInGoalDirection(), false);
// rftp.goal_angle_ = -3;
// EXPECT_EQ(rftp.isInGoalDirection(), false);
//}
//
TEST(TangentPlanner, foundLocalMinima) {
// it seems that true anf false should be exchange
Planner::TangentPlanner rftp = Planner::TangentPlanner();
rftp.prev_heuristic_distance_ = 3;
rftp.heuristic_distance_ = 2;
rftp.xy_goal_tolerance_ = 1;
EXPECT_FALSE(rftp.foundLocalMinima());
rftp.xy_goal_tolerance_ = 0.5;
EXPECT_FALSE(rftp.foundLocalMinima());
rftp.xy_goal_tolerance_ = 0.1;
EXPECT_TRUE(rftp.foundLocalMinima());
rftp.prev_heuristic_distance_ = 2;
rftp.heuristic_distance_ = 3;
rftp.xy_goal_tolerance_ = 1;
EXPECT_FALSE(rftp.foundLocalMinima());
rftp.xy_goal_tolerance_ = 0.1;
EXPECT_FALSE(rftp.foundLocalMinima());
}
TEST(TangentPlanner, isAtGoal) {
Planner::TangentPlanner rftp = Planner::TangentPlanner();
rftp.xy_goal_tolerance_ = 2;
rftp.goal_distance_ = 1;
EXPECT_TRUE(rftp.isAtGoal());
rftp.goal_distance_ = 2;
EXPECT_FALSE(rftp.isAtGoal());
rftp.goal_distance_ = 3;
EXPECT_FALSE(rftp.isAtGoal());
}
TEST(TangentPlanner, isAtGoalOrientation) {
Planner::TangentPlanner rftp = Planner::TangentPlanner();
rftp.yaw_goal_tolerance_ = 2;
rftp.goal_orientation_ = 1;
EXPECT_EQ(rftp.isAtGoalOrientation(), true);
rftp.goal_orientation_ = 2;
EXPECT_EQ(rftp.isAtGoalOrientation(), false);
rftp.goal_orientation_ = -2;
EXPECT_EQ(rftp.isAtGoalOrientation(), false);
rftp.goal_orientation_ = 0.3;
EXPECT_EQ(rftp.isAtGoalOrientation(), true);
rftp.goal_orientation_ = -0.3;
EXPECT_EQ(rftp.isAtGoalOrientation(), true);
}
TEST(TangentPlanner, resetVariables) {
Planner::TangentPlanner rftp = Planner::TangentPlanner();
rftp.dfollowed_ = 3;
rftp.resetVariables();
EXPECT_EQ(rftp.dfollowed_, 0);
}
//TEST(TangentPlanner, foundObstacle){
// Planner::TangentPlanner rftp = Planner::TangentPlanner();
// rftp.range_msg_->range = 0.5;
// rftp.range_msg_->min_range = 0.001;
// rftp.range_msg_->max_range = 2.0;
// rftp.goal_distance_ = 1.5;
// EXPECT_EQ(rftp.foundObstacle(), true);
// rftp.range_msg_->range = 0.0005;
// EXPECT_EQ(rftp.foundObstacle(), false);
// rftp.range_msg_->range = 2.1;
// EXPECT_EQ(rftp.foundObstacle(), false);
// rftp.range_msg_->range = rftp.range_msg_->min_range;
// EXPECT_EQ(rftp.foundObstacle(), false);
// rftp.range_msg_->range = rftp.range_msg_->max_range;
// EXPECT_EQ(rftp.foundObstacle(), false);
//
// rftp.range_msg_->range = 1.8;
// EXPECT_EQ(rftp.foundObstacle(), false);
// rftp.range_msg_->range = 1.2;
// EXPECT_EQ(rftp.foundObstacle(), true);
// rftp.goal_distance_ = rftp.range_msg_->max_range;
// EXPECT_EQ(rftp.foundObstacle(), true);
//}
//TEST(TangentPlanner, followBoundaries){
// Planner::TangentPlanner rftp = Planner::TangentPlanner();
// std::shared_ptr<geometry_msgs::Pose2D> gp = std::make_shared<geometry_msgs::Pose2D>();
// gp->x = 4.0;
// gp->y = 5.0;
// gp->theta = M_PI_2;
// rftp.global_plan_.push_back(gp);
// rftp.goal_distance_ = 10.0;
// rftp.goal_angle_ = M_PI_2;
// rftp.dfollowed_ = std::numeric_limits<float>::max();
// rftp.state_ = Planner::TangentPlanner::BugState::FollowBoundary;
// rftp.range_msg_->range = 1.0;
// rftp.current_pose_->x = 2.0;
// rftp.current_pose_->y = 3.0;
// rftp.current_pose_->theta = M_PI_2;
// rftp.followBoundaries();
// EXPECT_EQ(rftp.goal_angle_, 0.0);
// EXPECT_EQ(rftp.dreached_, 10.0);
// EXPECT_NEAR(rftp.dfollowed_, sqrt(5), 0.001);
// EXPECT_EQ(rftp.state_, Planner::TangentPlanner::BugState::FollowBoundary);
//}
//
//TEST(TangentPlanner, selectDiscontinuityPoint){
// Planner::TangentPlanner rftp = Planner::TangentPlanner();
// rftp.range_msg_->field_of_view = M_PI;
// rftp.range_msg_->range = 1.0;
// rftp.range_msg_->max_range = 2.0;
// rftp.range_msg_->min_range = 0.001;
// rftp.goal_distance_ = 10;
// rftp.goal_angle_ = 0.0;
// rftp.selectDiscontinuityPoint();
// EXPECT_NEAR(rftp.heuristic_distance_, sqrt(101)+1.0, 0.001);
// EXPECT_NEAR(rftp.goal_angle_, -M_PI_2, 0.001);
//}
//
//TEST(TangentPlanner, updateVelocity){
// Planner::TangentPlanner rftp = Planner::TangentPlanner();
// rftp.angular_velocity_ = 5;
// rftp.max_rot_vel_ = 3;
// rftp.max_trans_vel_ = 10;
// rftp.kp_ = 2;
// rftp.updateVelocity();
// EXPECT_EQ(rftp.cmd_ptr_->angular.z, 3);
// rftp.angular_velocity_ = 3;
// rftp.max_rot_vel_ = 5;
// rftp.updateVelocity();
// EXPECT_EQ(rftp.cmd_ptr_->angular.z, 3);
// EXPECT_EQ(rftp.cmd_ptr_->linear.x, 8);
//
//}
//
//TEST(TangentPlanner, updateControllerStateStopAtGoal){
// Planner::TangentPlanner rftp = Planner::TangentPlanner();
// rftp.state_ = Planner::TangentPlanner::BugState::StopAtGoal;
// rftp.kp_ = 1;
// rftp.angular_velocity_ = 1;
// rftp.goal_reached_ = false;
// rftp.prev_heuristic_distance_ = -1;
// rftp.updateController();
// EXPECT_EQ(rftp.kp_, 0);
// EXPECT_EQ(rftp.angular_velocity_, 0);
// EXPECT_EQ(rftp.goal_reached_, true);
// EXPECT_EQ(rftp.prev_heuristic_distance_, 0);
//}
//
//TEST(TangentPlanner, updateControllerStateTurnToGoal){
// Planner::TangentPlanner rftp = Planner::TangentPlanner();
// rftp.state_ = Planner::TangentPlanner::BugState::TurnToGoal;
// rftp.kp_ = 1;
// rftp.goal_angle_ = 4;
// rftp.angular_velocity_ = 1;
// rftp.updateController();
// EXPECT_EQ(rftp.kp_, 0);
// rftp.yaw_goal_tolerance_ = 1;
// EXPECT_NEAR(rftp.goal_angle_, -2.28319, 0.001);
// rftp.goal_angle_ = M_PI_2;
// rftp.updateController();
// EXPECT_NEAR(rftp.angular_velocity_, 0.5*M_PI_2, 0.0001);
// rftp.goal_angle_ = 1;
// rftp.yaw_goal_tolerance_ = 5;
// rftp.updateController();
// EXPECT_EQ(rftp.goal_angle_, 0);
// EXPECT_EQ(rftp.angular_velocity_, 0);
//}
//
//TEST(TangentPlanner, updateControllerStateOrientToGoal){
// Planner::TangentPlanner rftp = Planner::TangentPlanner();
// rftp.state_ = Planner::TangentPlanner::BugState::OrientToGoal;
// rftp.kp_ = 1;
// rftp.goal_orientation_ = 2;
// rftp.updateController();
// EXPECT_EQ(rftp.kp_, 0);
// EXPECT_EQ(rftp.angular_velocity_, 1);
//}
//
//TEST(TangentPlanner, updateControllerStateMotionToGoal){
// Planner::TangentPlanner rftp = Planner::TangentPlanner();
// rftp.state_ = Planner::TangentPlanner::BugState::MotionToGoal;
// rftp.kp_ = 1.5;
// rftp.kp_i_ = 0.8;
// rftp.range_msg_->range = 0.0001;
// rftp.goal_angle_ = 1.5;
// rftp.updateController();
// EXPECT_EQ(rftp.kp_, 1);
// EXPECT_EQ(rftp.angular_velocity_, 1.5);
// rftp.kp_ = 0.1;
// rftp.updateController();
// EXPECT_NEAR(rftp.kp_, 0.9, 0.1);
//}
//
//TEST(TangentPlanner, updateControllerStateMotionToGoal2){
// Planner::TangentPlanner rftp = Planner::TangentPlanner();
// rftp.state_ = Planner::TangentPlanner::BugState::MotionToGoal;
// rftp.kp_ = 1.5;
// rftp.kp_i_ = 0.8;
// rftp.goal_distance_ = 10;
// rftp.goal_angle_ = 0.0;
// rftp.range_msg_->range = 1.0;
// rftp.range_msg_->min_range =0.001;
// rftp.range_msg_->max_range =2.0;
// rftp.range_msg_->field_of_view = M_PI;
// rftp.updateController();
// EXPECT_NEAR(rftp.heuristic_distance_, sqrt(101)+1.0, 0.1);
// EXPECT_NEAR(rftp.goal_angle_, -M_PI_2, 0.0001);
// EXPECT_NEAR(rftp.angular_velocity_, -M_PI_2, 0.0001);
//}
//
//TEST(TangentPlanner, updateControllerStateFollowBoundary){
// Planner::TangentPlanner rftp = Planner::TangentPlanner();
// rftp.state_ = Planner::TangentPlanner::BugState::FollowBoundary;
// std::shared_ptr<geometry_msgs::Pose2D> gp = std::make_shared<geometry_msgs::Pose2D>();
// rftp.kp_ = 1.5;
// rftp.kp_i_ = 0.8;
// gp->x = 4.0;
// gp->y = 5.0;
// gp->theta = M_PI_2;
// rftp.global_plan_.push_back(*gp);
// rftp.goal_distance_ = 10;
// rftp.goal_angle_ = M_PI_2;
// rftp.range_msg_->range = 1.0;
// rftp.current_pose_->x = 2.0;
// rftp.current_pose_->y = 3.0;
// rftp.current_pose_->theta = M_PI_2;
// rftp.updateController();
// EXPECT_EQ(rftp.kp_, 1);
// EXPECT_EQ(rftp.angular_velocity_, 0.0);
// rftp.kp_ = 0.1;
// rftp.updateController();
// EXPECT_NEAR(rftp.kp_, 0.9, 0.1);
//}
//
//TEST(TangentPlanner, updateControllerStateCloseToGoal){
// Planner::TangentPlanner rftp = Planner::TangentPlanner();
// rftp.state_ = Planner::TangentPlanner::BugState::CloseToGoal;
// rftp.kp_ = 0.3;
// rftp.kp_i_ = 0.1;
// rftp.goal_angle_ = 1.5;
// rftp.updateController();
// EXPECT_EQ(rftp.kp_, 0.25);
// EXPECT_EQ(rftp.angular_velocity_, 1.5);
// rftp.kp_ = 0.3;
// rftp.kp_i_ = 0.01;
// rftp.updateController();
// EXPECT_NEAR(rftp.kp_, 0.29, 0.01);
// EXPECT_EQ(rftp.angular_velocity_, 1.5);
//}
//
//
//TEST(TangentPlanner, updateStateToFollowBoundary) {
// Planner::TangentPlanner rftp = Planner::TangentPlanner();
// rftp.state_ = Planner::TangentPlanner::BugState::FollowBoundary;
// rftp.updateState();
// EXPECT_EQ(rftp.state_, Planner::TangentPlanner::BugState::FollowBoundary);
//}
//
//TEST(TangentPlanner, updateStateToTurnToGoal) {
// Planner::TangentPlanner rftp = Planner::TangentPlanner();
// rftp.state_ = Planner::TangentPlanner::BugState::OrientToGoal;
// rftp.goal_angle_ = 2;
// rftp.yaw_goal_tolerance_ = 1.5;
// rftp.goal_distance_ = 2;
// rftp.xy_goal_tolerance_ = 1.5;
// rftp.is_new_goal_ = true;
// rftp.updateState();
// EXPECT_EQ(rftp.state_, Planner::TangentPlanner::BugState::TurnToGoal);
//}
//
//TEST(TangentPlanner, updateStateSetIsNewGoalFalse) {
// Planner::TangentPlanner rftp = Planner::TangentPlanner();
// rftp.goal_angle_ = 1.5;
// rftp.yaw_goal_tolerance_ = 2;
// rftp.updateState();
// EXPECT_EQ(rftp.is_new_goal_, false);
//}
//TEST(TangentPlanner, updateStateToOrientToGoal) {
// Planner::TangentPlanner rftp = Planner::TangentPlanner();
// rftp.is_new_goal_ = false;
// rftp.goal_angle_ = 2;
// rftp.yaw_goal_tolerance_ = 1.5;
// rftp.goal_distance_ = 1.5;
// rftp.xy_goal_tolerance_ = 2;
// rftp.goal_orientation_ = 2;
// rftp.state_ = Planner::TangentPlanner::BugState::TurnToGoal;
// rftp.updateState();
// EXPECT_EQ(rftp.state_, Planner::TangentPlanner::BugState::OrientToGoal);
//}
//
//TEST(TangentPlanner, updateStateToCloseToGoal) {
// Planner::TangentPlanner rftp = Planner::TangentPlanner();
// rftp.is_new_goal_ = false;
// //rftp.goal_angle_ = 1.5;
// rftp.yaw_goal_tolerance_ = 2;
// rftp.kp_ = 7;
// rftp.max_trans_vel_ = 1;
// rftp.goal_distance_ = 4;
// rftp.state_ = Planner::TangentPlanner::BugState::TurnToGoal;
// rftp.updateState();
// //EXPECT_EQ(rftp.state_, Planner::TangentPlanner::BugState::CloseToGoal);
//}
//
//TEST(TangentPlanner, updateStateToCloseToGoal2) {
// Planner::TangentPlanner rftp = Planner::TangentPlanner();
// rftp.is_new_goal_ = false;
// //rftp.goal_angle_ = 2;
// rftp.yaw_goal_tolerance_ = 1.5;
// rftp.kp_ = 7;
// rftp.max_trans_vel_ = 1;
// rftp.range_msg_->range = 2.4;
// rftp.range_msg_->min_range = 0.001;
// rftp.range_msg_->max_range = 2.0;
// //rftp.goal_angle_ = 3.0;
// rftp.yaw_goal_tolerance_ = 2;
// rftp.goal_distance_ = 4;
// rftp.state_ = Planner::TangentPlanner::BugState::TurnToGoal;
// rftp.updateState();
// //EXPECT_EQ(rftp.state_, Planner::TangentPlanner::BugState::CloseToGoal);
//}
//
////printf("is in goal orientation: %d and is close to goal: %d and is at goal: %d and is in goal direction: %d",
////rftp.isInGoalDirection(),
//// rftp.isCloseToGoal(),
//// rftp.isAtGoal(),
//// rftp.isInGoalDirection());
//
//TEST(TangentPlanner, updateStateToStopAtGoal) {
// Planner::TangentPlanner rftp = Planner::TangentPlanner();
// rftp.xy_goal_tolerance_ = 2;
// rftp.goal_distance_ = 1;
// rftp.goal_orientation_ = -1;
// rftp.yaw_goal_tolerance_ = 2;
// rftp.is_new_goal_ = false;
// rftp.state_ = Planner::TangentPlanner::BugState::TurnToGoal;
// rftp.updateState();
// EXPECT_EQ(rftp.state_, Planner::TangentPlanner::BugState::StopAtGoal);
//}
//
//TEST(TangentPlanner, updateStateToMotionToGoal) {
// Planner::TangentPlanner rftp = Planner::TangentPlanner();
// rftp.xy_goal_tolerance_ = 8;
// rftp.goal_distance_ = 10;
// rftp.kp_ = 1;
// rftp.max_trans_vel_ = 1;
// rftp.goal_orientation_ = -3;
// rftp.yaw_goal_tolerance_ = 4;
// rftp.is_new_goal_ = false;
// rftp.state_ = Planner::TangentPlanner::BugState::TurnToGoal;
// rftp.updateState();
// EXPECT_EQ(rftp.state_, Planner::TangentPlanner::BugState::MotionToGoal);
//}
//
//TEST(TangentPlanner, updateStateToMotionToGoal2) {
// Planner::TangentPlanner rftp = Planner::TangentPlanner();
// rftp.xy_goal_tolerance_ = 8;
// rftp.goal_distance_ = 10;
// rftp.kp_ = 1;
// rftp.max_trans_vel_ = 1;
// rftp.goal_orientation_ = -3;
// rftp.is_new_goal_ = false;
// rftp.range_msg_->range = 0.5;
// rftp.range_msg_->min_range = 0.001;
// rftp.range_msg_->max_range = 2.0;
// //rftp.goal_angle_ = 3.0;
// rftp.yaw_goal_tolerance_ = 2;
// rftp.prev_heuristic_distance_ = 26.0;
// rftp.heuristic_distance_ = 9.0;
// rftp.state_ = Planner::TangentPlanner::BugState::TurnToGoal;
// rftp.updateState();
// EXPECT_EQ(rftp.state_, Planner::TangentPlanner::BugState::FollowBoundary);
//}
//
//TEST(TangentPlanner, updateGoal){
// Planner::TangentPlanner rftp = Planner::TangentPlanner();
// rftp.current_pose_->x = 1.0;
// rftp.current_pose_->y = 5.0;
// rftp.current_pose_->theta = M_PI;
// std::shared_ptr<geometry_msgs::Pose2D> gp = std::make_shared<geometry_msgs::Pose2D>();
// gp->x = 4.0;
// gp->y = 9.0;
// gp->theta = M_PI_2;
// float gp_atan2 = atan2(gp->y - rftp.current_pose_->y, gp->x - rftp.current_pose_->x) ;
// rftp.global_plan_.push_back(gp);
//
// //rftp.updateGoal();
// std::cout<<"step 000"<<std::endl;
// EXPECT_EQ(rftp.goal_distance_, 5.0);
// //EXPECT_NEAR(rftp.goal_angle_, gp_atan2 - rftp.current_pose_->theta, 0.0001);
// EXPECT_NEAR(rftp.goal_orientation_, -M_PI_2, 0.0001);
//}
//
//TEST(TangentPlanner, computeVelocityCommands){
// Planner::TangentPlanner rftp = Planner::TangentPlanner();
// rftp.state_ = Planner::TangentPlanner::BugState::MotionToGoal;
// rftp.initialized_ = false;
// EXPECT_EQ(rftp.computeVelocityCommands(), false);
// rftp.initialized_ = true;
// // goal position:
// std::shared_ptr<geometry_msgs::Pose2D> gp = std::make_shared<geometry_msgs::Pose2D>();
// gp->x = 4.0;
// gp->y = 8.0;
// gp->theta = M_PI_2;
// rftp.global_plan_.push_back(gp);
// // current position:
// rftp.current_pose_->x = 1.0;
// rftp.current_pose_->y = 5.0;
// rftp.current_pose_->theta = M_PI;
// rftp.range_msg_->range = 0.7;
// rftp.range_msg_->max_range = 2.0;
// rftp.range_msg_->min_range = 0.001;
// EXPECT_EQ(rftp.computeVelocityCommands(), true);
// EXPECT_EQ(rftp.state_, Planner::TangentPlanner::BugState::TurnToGoal);
// EXPECT_NEAR(rftp.cmd_ptr_->angular.z, 0.5*(M_PI_2/2-M_PI), 0.001);
//}
//
//TEST(TangentPlanner, whileLoop){
// Planner::TangentPlanner rftp = Planner::TangentPlanner();
// rftp.state_ = Planner::TangentPlanner::BugState::MotionToGoal;
// rftp.initialized_ = true;
// // number of goal points:
// int goalNumbers = 5;
// rftp.plan_size_ = goalNumbers;
// // goal positions:
// std::shared_ptr<geometry_msgs::Pose2D> gp = std::make_shared<geometry_msgs::Pose2D>();
// gp->x = 4.0;
// gp->y = 9.0;
// gp->theta = M_PI_2;
// for (int i=0; i<goalNumbers; i++){
// rftp.global_plan_.push_back(*gp);
// gp->x += 2.0;
// gp->y += 1.0;
// gp->theta = M_PI_2;
// }
// // while loop for multiple goals:
// uint counter = 0;
// rftp.range_msg_->max_range = 2.0;
// rftp.range_msg_->min_range = 0.001;
// for (int j=0; j<goalNumbers; j++){
// while (!rftp.isGoalReached() && counter!=100){
// counter++;
// // current position and range sensor:
// rftp.current_pose_->x = rand()%10;
// rftp.current_pose_->y = rand()%10;
// rftp.current_pose_->theta = rand()%3;
// rftp.range_msg_->range = rand()%2;
//
// rftp.computeVelocityCommands();
// EXPECT_EQ(rftp.computeVelocityCommands(), true);
// }
// std::cout<<"counter is:"<<counter<<std::endl;
// rftp.resetVariables();
// rftp.plan_size_--;
// counter = 0;
// rftp.goal_reached_ = false;
// }
//}
int main(int argc, char** argv) {
::testing::InitGoogleTest(&argc, argv);
ros::init(argc, argv, "tangent_planner_test");
ros::NodeHandle nh;
return RUN_ALL_TESTS();
}
}