fixed an issue with reversing

karlkurzer · May 20, 2016 · 5886e9b · 5886e9b
1 parent b9e8816
commit 5886e9b
Show file tree

Hide file tree

Showing 7 changed files with 13 additions and 13 deletions.
diff --git a/CMakeLists.txt b/CMakeLists.txt
@@ -48,7 +48,7 @@ set(HEADERS
    ${CMAKE_CURRENT_SOURCE_DIR}/include/gradient.h
    ${CMAKE_CURRENT_SOURCE_DIR}/include/dubins.h
 )
-add_library(HYAS ${SOURCES} ${HEADERS})
+#add_library(HYAS ${SOURCES} ${HEADERS})
 
 ## Declare a catkin package
 catkin_package()
@@ -69,4 +69,4 @@ target_link_libraries(tf_broadcaster ${catkin_LIBRARIES})
 add_executable(hybrid_astar src/main.cpp ${SOURCES} ${HEADERS})
 target_link_libraries(hybrid_astar ${catkin_LIBRARIES})
 target_link_libraries(hybrid_astar ${OMPL_LIBRARIES})
-target_link_libraries(hybrid_astar ${HYAS})
+#target_link_libraries(hybrid_astar ${HYAS})
diff --git a/include/collisiondetection.h b/include/collisiondetection.h
@@ -15,10 +15,7 @@ namespace HybridAStar {
 class CollisionDetection {
  public:
   /// Constructor
-  CollisionDetection() {
-    this->grid = nullptr;
-    Lookup::collisionLookup(collisionLookup);
-  }
+  CollisionDetection();
 
   /*!
      \brief evaluates whether the configuration is safe

diff --git a/include/constants.h b/include/constants.h
@@ -37,13 +37,13 @@ static const bool visualization = 0 * manual;
 static const bool visualization2D = 0 * manual;
 
 // flag -- to turn on reversin of the vehicle
-static const bool reverse = false;
+static const bool reverse = true;
 
 // flag -- to turn on the dubins shot
 static const bool dubinsShot = true;
 
 // flag -- to turn on the dubins heuristic
-static const bool dubins = true;
+static const bool dubins = false;
 
 // flag -- to turn on the dubins lookup
 static const bool dubinsLookup = false * dubins;
@@ -95,7 +95,7 @@ static const float tieBreaker = 0.01;
 static const float factor2D = sqrt(5) / sqrt(2) + 1;
 
 // [#] -- penalty for turning
-static const float penaltyTurning = 1.1;
+static const float penaltyTurning = 1.05;
 
 // [m] -- dubins shot step size
 static const float dubinsStepSize = 1;

diff --git a/src/algorithm.cpp b/src/algorithm.cpp
@@ -146,8 +146,7 @@ Node3D* Algorithm::hybridAStar(Node3D& start,
       // _________
       // GOAL TEST
       if (*nPred == goal) {
-        //DEBUG
-        // std::cout << "max diff " << max << std::endl;
+        // DEBUG
         return nPred;
       }
 

diff --git a/src/collisiondetection.cpp b/src/collisiondetection.cpp
@@ -2,3 +2,7 @@
 
 using namespace HybridAStar;
 
+CollisionDetection::CollisionDetection() {
+  this->grid = nullptr;
+  Lookup::collisionLookup(collisionLookup);
+}
diff --git a/src/node3d.cpp b/src/node3d.cpp
@@ -65,7 +65,7 @@ Node3D* Node3D::createSuccessor(const int i) {
   else {
     xSucc = x - dx[i - 3] * cos(t) - dy[i - 3] * sin(t);
     ySucc = y - dx[i - 3] * sin(t) + dy[i - 3] * cos(t);
-    tSucc = helper::normalizeHeadingRad(t + dt[i - 3] + M_PI);
+    tSucc = helper::normalizeHeadingRad(t - dt[i - 3]);
   }
 
   return new Node3D(xSucc, ySucc, tSucc, g, 0, this, i);

diff --git a/src/planner.cpp b/src/planner.cpp
@@ -174,7 +174,7 @@ void Planner::plan() {
     // CLEAR THE VISUALIZATION
     visualization.clear();
     // FIND THE PATH
-    Node3D* nSolution = Algorithm::hybridAStar(nStart, nGoal, nodes3D, nodes2D, grid, collisionLookup, dubinsLookup, visualization,configurationSpace);
+    Node3D* nSolution = Algorithm::hybridAStar(nStart, nGoal, nodes3D, nodes2D, grid, collisionLookup, dubinsLookup, visualization, configurationSpace);
     // CLEAR THE PATH
     path.clear();
     // TRACE THE PATH