From 4d876ecee1b224a73ac286c61f83c3aba29735e4 Mon Sep 17 00:00:00 2001
From: Deepak singh <deepaks.ec.16@nsit.net.in>
Date: Sun, 8 Aug 2021 23:49:29 +0530
Subject: [PATCH 1/2] Updated kinodynamic_astar.cpp to check if point in map or
 not

---
 fast_planner/path_searching/src/kinodynamic_astar.cpp | 8 ++++++++
 1 file changed, 8 insertions(+)

diff --git a/fast_planner/path_searching/src/kinodynamic_astar.cpp b/fast_planner/path_searching/src/kinodynamic_astar.cpp
index 139f4698d..2b0b76039 100644
--- a/fast_planner/path_searching/src/kinodynamic_astar.cpp
+++ b/fast_planner/path_searching/src/kinodynamic_astar.cpp
@@ -178,6 +178,14 @@ int KinodynamicAstar::search(Eigen::Vector3d start_pt, Eigen::Vector3d start_v,
 
         Eigen::Vector3d pro_pos = pro_state.head(3);
 
+        // check if in map
+        if (pro_state(0) <= origin_(0) || pro_state(0) >= map_size_3d_(0) ||
+            pro_state(1) <= origin_(1) || pro_state(1) >= map_size_3d_(1) ||
+            pro_state(2) <= origin_(2) || pro_state(2) >= map_size_3d_(2)) {
+              std::cout<<"outside map"<<std::endl;
+          continue;
+        }
+
         // Check if in close set
         Eigen::Vector3i pro_id = posToIndex(pro_pos);
         int pro_t_id = timeToIndex(pro_t);

From 3634d3d1e5d4cb614f2d27b3f6335c89db694ebf Mon Sep 17 00:00:00 2001
From: Deepak singh <deepaks.ec.16@nsit.net.in>
Date: Sun, 8 Aug 2021 23:51:23 +0530
Subject: [PATCH 2/2] Updated kinodynamic_astar.cpp

---
 fast_planner/path_searching/src/kinodynamic_astar.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/fast_planner/path_searching/src/kinodynamic_astar.cpp b/fast_planner/path_searching/src/kinodynamic_astar.cpp
index 2b0b76039..32b2d6140 100644
--- a/fast_planner/path_searching/src/kinodynamic_astar.cpp
+++ b/fast_planner/path_searching/src/kinodynamic_astar.cpp
@@ -182,7 +182,7 @@ int KinodynamicAstar::search(Eigen::Vector3d start_pt, Eigen::Vector3d start_v,
         if (pro_state(0) <= origin_(0) || pro_state(0) >= map_size_3d_(0) ||
             pro_state(1) <= origin_(1) || pro_state(1) >= map_size_3d_(1) ||
             pro_state(2) <= origin_(2) || pro_state(2) >= map_size_3d_(2)) {
-              std::cout<<"outside map"<<std::endl;
+              
           continue;
         }