Support custom maps

zm0612 · Jun 16, 2024 · cf0cc99 · cf0cc99
1 parent d6cd611
commit cf0cc99
Show file tree

Hide file tree

Showing 5 changed files with 19 additions and 18 deletions.
diff --git a/include/hybrid_a_star/trajectory_optimizer.h b/include/hybrid_a_star/trajectory_optimizer.h
@@ -35,7 +35,7 @@
 /*!
  * Trajectory optimization function
  *
- * Note: According to hyrbid a star's paper, I have not reproduced very good results.
+ * Note: According to hybrid a star's paper, I have not reproduced very good results.
  *       However, I have obtained a good trajectory in the front-end trajectory search stage,
  *       so even if I don't use back-end optimization,
  *       the trajectory can basically meet the usage requirements.

diff --git a/maps/20220630.pgm b/maps/20220630.pgm
diff --git a/maps/20220630.yaml b/maps/20220630.yaml
@@ -0,0 +1,8 @@
+image: 20220630.pgm
+resolution: 0.100000
+origin: [ -44.800000, -50.300000, 0.0 ]
+negate: 0
+occupied_thresh: 0.65
+free_thresh: 0.196
+
+# This file comes from @StanleyYake, thanks to him!
diff --git a/src/hybrid_a_star.cpp b/src/hybrid_a_star.cpp
@@ -270,8 +270,8 @@ bool HybridAStar::HasObstacle(const Vec2i &grid_index) const {
 }
 
 void HybridAStar::SetObstacle(unsigned int x, unsigned int y) {
-    if (x < 0u || x > static_cast<unsigned int>(MAP_GRID_SIZE_X_)
-        || y < 0u || y > static_cast<unsigned int>(MAP_GRID_SIZE_Y_)) {
+    if (x > static_cast<unsigned int>(MAP_GRID_SIZE_X_)
+        || y > static_cast<unsigned int>(MAP_GRID_SIZE_Y_)) {
         return;
     }
 
@@ -423,8 +423,8 @@ void HybridAStar::DynamicModel(const double &step_size, const double &phi,
 
 double HybridAStar::Mod2Pi(const double &x) {
     double v = fmod(x, 2 * M_PI);
-  
-      if (v < -M_PI) {
+
+    if (v < -M_PI) {
         v += 2.0 * M_PI;
     } else if (v > M_PI) {
         v -= 2.0 * M_PI;

diff --git a/src/hybrid_a_star_flow.cpp b/src/hybrid_a_star_flow.cpp
@@ -33,7 +33,7 @@
 #include <tf/transform_datatypes.h>
 #include <tf/transform_broadcaster.h>
 
-double Mod2Pi(const double &x) {
+__attribute__((unused)) double Mod2Pi(const double &x) {
     double v = fmod(x, 2 * M_PI);
 
     if (v < -M_PI) {
@@ -82,26 +82,20 @@ void HybridAStarFlow::Run() {
         current_costmap_ptr_ = costmap_deque_.front();
         costmap_deque_.pop_front();
 
-        const double map_resolution = 0.2;
+        const double map_resolution = static_cast<float>(current_costmap_ptr_->info.resolution);
         kinodynamic_astar_searcher_ptr_->Init(
                 current_costmap_ptr_->info.origin.position.x,
                 1.0 * current_costmap_ptr_->info.width * current_costmap_ptr_->info.resolution,
                 current_costmap_ptr_->info.origin.position.y,
                 1.0 * current_costmap_ptr_->info.height * current_costmap_ptr_->info.resolution,
-                current_costmap_ptr_->info.resolution,
-                map_resolution
+                1.0, map_resolution
         );
 
-        unsigned int map_w = std::floor(current_costmap_ptr_->info.width / map_resolution);
-        unsigned int map_h = std::floor(current_costmap_ptr_->info.height / map_resolution);
+        unsigned int map_w = std::floor(current_costmap_ptr_->info.width);
+        unsigned int map_h = std::floor(current_costmap_ptr_->info.height);
         for (unsigned int w = 0; w < map_w; ++w) {
             for (unsigned int h = 0; h < map_h; ++h) {
-                auto x = static_cast<unsigned int> ((w + 0.5) * map_resolution
-                                                    / current_costmap_ptr_->info.resolution);
-                auto y = static_cast<unsigned int> ((h + 0.5) * map_resolution
-                                                    / current_costmap_ptr_->info.resolution);
-
-                if (current_costmap_ptr_->data[y * current_costmap_ptr_->info.width + x]) {
+                if (current_costmap_ptr_->data[h * current_costmap_ptr_->info.width + w]) {
                     kinodynamic_astar_searcher_ptr_->SetObstacle(w, h);
                 }
             }
@@ -170,7 +164,6 @@ void HybridAStarFlow::Run() {
             }
         }
 
-
         // debug
 //        std::cout << "visited nodes: " << kinodynamic_astar_searcher_ptr_->GetVisitedNodesNumber() << std::endl;
         kinodynamic_astar_searcher_ptr_->Reset();