Planning: fix test for open space

modules/planning/conf/planner_open_space_config.pb.txt

@@ -11,9 +11,9 @@ warm_start_config : {
   next_node_num : 10
   step_size : 0.5
   back_penalty : 0.0
-  gear_switch_penalty : 100.0
+  gear_switch_penalty : 10.0
   steer_penalty : 100.0
-  steer_change_penalty : 100.0
+  steer_change_penalty : 10.0
 }
 
 distance_approach_config : {

modules/planning/open_space/distance_approach_ipopt_interface.cc

@@ -15,7 +15,7 @@
  *****************************************************************************/
 
 /*
- * @file distance_approach_problem_interface.cc
+ * @file
  */
 
 #include "modules/planning/open_space/distance_approach_ipopt_interface.h"

modules/planning/open_space/distance_approach_ipopt_interface.h

@@ -15,7 +15,7 @@
  *****************************************************************************/
 
 /*
- * @file distance_approach_ipopt_interface.h
+ * @file
  */
 
 #ifndef MODULES_PLANNING_OPEN_SPACE_DISTANCE_APPROACH_IPOPT_INTERFACE_H_

modules/planning/open_space/distance_approach_ipopt_interface_test.cc

@@ -15,7 +15,7 @@
  *****************************************************************************/
 
 /**
- * @file distance_approach_ipopt_interface_test.h
+ * @file
  **/
 #include "modules/planning/open_space/distance_approach_ipopt_interface.h"
 #include "IpTNLP.hpp"

modules/planning/open_space/distance_approach_problem.cc

@@ -15,7 +15,7 @@
  *****************************************************************************/
 
 /*
- * @file distance_approach_problem.cc
+ * @file
  */
 
 #include "modules/planning/open_space/distance_approach_problem.h"

modules/planning/open_space/distance_approach_problem.h

@@ -15,7 +15,7 @@
  *****************************************************************************/
 
 /*
- * @file distance_approach_problem.h
+ * @file
  */
 
 #ifndef MODULES_PLANNING_OPEN_SPACE_DISTANCE_APPROACH_PROBLEM_H_

modules/planning/open_space/distance_approach_problem_test.cc

@@ -15,7 +15,7 @@
  *****************************************************************************/
 
 /**
- * @file distance_approach_problem_test.cc
+ * @file
  **/
 #include "modules/planning/open_space/distance_approach_problem.h"
 

modules/planning/open_space/hybrid_a_star.cc

@@ -15,7 +15,7 @@
  *****************************************************************************/
 
 /*
- * @file hybrid_a_star.cc
+ * @file
  */
 
 #include "modules/planning/open_space/hybrid_a_star.h"

modules/planning/open_space/hybrid_a_star.h

@@ -15,7 +15,7 @@
  *****************************************************************************/
 
 /*
- * @file hybrid_a_star.h
+ * @file
  */
 
 /*

modules/planning/open_space/hybrid_a_star_test.cc

@@ -15,7 +15,7 @@
  *****************************************************************************/
 
 /*
- * @file hybrid_a_star_test.cc
+ * @file
  */
 
 #include <memory>
@@ -43,18 +43,18 @@ class HybridATest : public ::testing::Test {
 };
 
 TEST_F(HybridATest, test1) {
-  double sx = 0.0;
+  double sx = -15.0;
   double sy = 0.0;
   double sphi = 0.0;
-  double ex = 100.0;
-  double ey = 100.0;
-  double ephi = M_PI;
+  double ex = 15.0;
+  double ey = 0.0;
+  double ephi = 0.0;
   ThreadSafeIndexedObstacles obstacles_list;
   Result result;
   std::unique_ptr<Obstacle> obstacle_class =
       std::unique_ptr<Obstacle>(new Obstacle());
-  Vec2d obstacle_center(50.0, 50.0);
-  Box2d obstacle_box(obstacle_center, 0.0, 20.0, 20.0);
+  Vec2d obstacle_center(0.0, 0.0);
+  Box2d obstacle_box(obstacle_center, 0.0, 5.0, 5.0);
   std::unique_ptr<Obstacle> obstacle =
       obstacle_class->CreateStaticVirtualObstacles("a box in center",
                                                    obstacle_box);

modules/planning/open_space/hybrid_a_star_wrapper.cc

@@ -15,7 +15,7 @@
  *****************************************************************************/
 
 /*
- * @file hybrid_a_star_wrapper.cc
+ * @file
  */
 #include "modules/planning/open_space/hybrid_a_star.h"
 

modules/planning/open_space/node3d.cc

@@ -15,7 +15,7 @@
  *****************************************************************************/
 
 /*
- * @file node3d.cc
+ * @file
  */
 
 #include "modules/planning/open_space/node3d.h"

modules/planning/open_space/node3d.h

@@ -15,7 +15,7 @@
  *****************************************************************************/
 
 /*
- * @file node3d.h
+ * @file
  */
 
 #ifndef MODULES_PLANNING_OPEN_SPACE_NODE3D_H_

modules/planning/open_space/node3d_test.cc

@@ -15,7 +15,7 @@
  *****************************************************************************/
 
 /*
- * @file node3d_test.cc
+ * @file
  */
 #include "modules/planning/open_space/node3d.h"
 #include "gtest/gtest.h"

modules/planning/open_space/reeds_shepp_path.cc

@@ -15,7 +15,7 @@
  *****************************************************************************/
 
 /*
- * @file reeds_shepp_path.cc
+ * @file
  */
 
 #include "modules/planning/open_space/reeds_shepp_path.h"
@@ -869,19 +869,8 @@ bool ReedShepp::SetRSP(int size, double lengths[], char types[],
   double sum = 0.0;
   for (int i = 0; i < size; i++) {
     sum += std::abs(lengths[i]);
-    if (std::abs(lengths[i]) >= 100) {
-      AINFO << "std::abs(lengths[i])" << std::abs(lengths[i]);
-      AINFO << "length too large";
-      return false;
-    }
   }
   path.total_length = sum;
-
-  if (path.total_length >= 100) {
-    AINFO << "total length too large";
-    return false;
-  }
-
   if (path.total_length <= 0.0) {
     AINFO << "total length smaller than 0";
     return false;
@@ -942,8 +931,9 @@ bool ReedShepp::GenerateLocalConfigurations(
       index++;
       Interpolation(index, l, m, ox, oy, ophi, &px, &py, &pphi, &pgear);
     }
-    while (px.back() == 0.0 && py.back() == 0.0 && pphi.back() == 0.0 &&
-           pgear.back() == true) {
+    double epsilon = 1e-15;
+    while (std::fabs(px.back()) < epsilon && std::fabs(py.back()) < epsilon &&
+           std::fabs(pphi.back()) < epsilon && pgear.back()) {
       px.pop_back();
       py.pop_back();
       pphi.pop_back();

modules/planning/open_space/reeds_shepp_path.h

@@ -15,7 +15,7 @@
  *****************************************************************************/
 
 /*
- * @file reeds_shepp_path.h
+ * @file
  */
 
 #ifndef MODULES_PLANNING_OPEN_SPACE_REEDS_SHEPP_PATH_H_

modules/planning/open_space/reeds_shepp_path_test.cc

@@ -15,7 +15,7 @@
  *****************************************************************************/
 
 /*
- * @file reeds_shepp_path_test.cc
+ * @file
  */
 
 #include <memory>

modules/planning/open_space/warm_start_ipopt_interface.cc

@@ -15,7 +15,7 @@
  *****************************************************************************/
 
 /*
- * @file warm_start_ipopt_interface.cc
+ * @file
  */
 
 #include "modules/planning/open_space/warm_start_ipopt_interface.h"

modules/planning/open_space/warm_start_ipopt_interface.h

@@ -15,7 +15,7 @@
  *****************************************************************************/
 
 /*
- * @file warm_start_ipopt_interface.h
+ * @file
  */
 
 #ifndef MODULES_PLANNING_OPEN_SPACE_WARM_START_IPOPT_INTERFACE_H_

modules/planning/open_space/warm_start_problem.cc

@@ -15,7 +15,7 @@
  *****************************************************************************/
 
 /*
- * @file warm_start_problem.cc
+ * @file
  */
 
 #include "modules/planning/open_space/warm_start_problem.h"

modules/planning/open_space/warm_start_problem.h

@@ -15,7 +15,7 @@
  *****************************************************************************/
 
 /*
- * @file warm_start_problem.h
+ * @file
  */
 
 #ifndef MODULES_PLANNING_OPEN_SPACE_WARM_START_PROBLEM_H_

modules/planning/planner/open_space/open_space_planner.cc

@@ -15,7 +15,7 @@
  *****************************************************************************/
 
 /**
- * @file open_space_planner.cc
+ * @file
  **/
 
 #include "modules/planning/planner/open_space/open_space_planner.h"

modules/planning/planner/open_space/open_space_planner.h

@@ -15,7 +15,7 @@
  *****************************************************************************/
 
 /**
- * @file open_space_planner.h
+ * @file
  **/
 
 #ifndef MODULES_PLANNING_PLANNER_OPENSPACE_OPENSPACE_PLANNER_H_

modules/planning/planner/open_space/open_space_planner_test.cc

@@ -15,7 +15,7 @@
  *****************************************************************************/
 
 /**
- * @file open_space_planner_test.cc
+ * @file
  **/
 
 #include "modules/planning/planner/open_space/open_space_planner.h"