diff --git a/src/gtest.cpp b/src/gtest.cpp
index d840ccb..9493102 100755
--- a/src/gtest.cpp
+++ b/src/gtest.cpp
@@ -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();
-}
\ No newline at end of file
+}