start testing lpastar for poly_map_planner

mpl_external_planner/include/mpl_external_planner/poly_map_planner/env_poly_map.h

@@ -15,9 +15,6 @@ namespace MPL {
  */
 template <int Dim>
 class env_poly_map : public env_base<Dim> {
-protected:
-  std::shared_ptr<PolyMapUtil<Dim>> map_util_;
-
 public:
   /// Simple constructor
   env_poly_map() {}
@@ -51,6 +48,8 @@ class env_poly_map : public env_base<Dim> {
 		succ_cost.clear();
 		action_idx.clear();
 
+    this->expanded_nodes_.push_back(curr.pos);
+
 		for (size_t i = 0; i < this->U_.size(); i++) {
 			Primitive<Dim> pr(curr, this->U_[i], this->dt_);
       Waypoint<Dim> tn = pr.evaluate(this->dt_);
@@ -70,6 +69,10 @@ class env_poly_map : public env_base<Dim> {
 			action_idx.push_back(i);
 		}
 	}
+protected:
+  std::shared_ptr<PolyMapUtil<Dim>> map_util_;
+
+
 };
 }
 

mpl_external_planner/include/mpl_external_planner/poly_map_planner/poly_map_planner.h

@@ -33,18 +33,15 @@ class PolyMapPlanner : public PlannerBase<Dim, Waypoint<Dim>> {
  }
 
  void setStaticObstacles(const vec_E<PolyhedronObstacle<Dim>>& polys) {
-   for(const auto& poly: polys)
-     map_util_->addStaticObstacle(poly);
+   map_util_->setStaticObstacle(polys);
  }
 
  void setLinearObstacles(const vec_E<PolyhedronLinearObstacle<Dim>>& polys) {
-   for(const auto& poly: polys)
-     map_util_->addLinearObstacle(poly);
+   map_util_->setLinearObstacle(polys);
  }
 
  void setNonlinearObstacles(const vec_E<PolyhedronNonlinearObstacle<Dim>>& polys) {
-   for(const auto& poly: polys)
-     map_util_->addNonlinearObstacle(poly);
+   map_util_->setNonlinearObstacle(polys);
  }
 
  vec_E<Polyhedron<Dim>> getPolyhedrons(decimal_t time) const {
@@ -55,8 +52,53 @@ class PolyMapPlanner : public PlannerBase<Dim, Waypoint<Dim>> {
    return map_util_->getBoundingBox();
  }
 
+ void updateNodes() {
+   blocked_prs_.clear();
+   cleared_prs_.clear();
+
+   if(!this->ss_ptr_)
+     return;
+
+   std::vector<std::pair<Waypoint<Dim>, int>> blocked_nodes;
+   std::vector<std::pair<Waypoint<Dim>, int>> cleared_nodes;
+   for (const auto &it : this->ss_ptr_->hm_) {
+     const auto &succNode_ptr = it.second;
+
+     for(size_t i = 0; i < succNode_ptr->pred_coord.size(); i++) {
+       Primitive<Dim> pr;
+       this->ENV_->forward_action(succNode_ptr->pred_coord[i],
+                                  succNode_ptr->pred_action_id[i], pr);
+
+       if(!map_util_->isFree(pr, succNode_ptr->pred_coord[i].t)) {
+         if(!std::isinf(succNode_ptr->pred_action_cost[i])) {
+           blocked_nodes.push_back(std::make_pair(it.first, i));
+           blocked_prs_.push_back(pr);
+         }
+       }
+       else {
+         if(std::isinf(succNode_ptr->pred_action_cost[i])) {
+           cleared_nodes.push_back(std::make_pair(it.first, i));
+           cleared_prs_.push_back(pr);
+         }
+       }
+     }
+   }
+
+   this->ss_ptr_->increaseCost(blocked_nodes);
+   this->ss_ptr_->decreaseCost(cleared_nodes, this->ENV_);
+ }
+
+ vec_E<Primitive<Dim>> getBlockedPrimitives() {
+   return blocked_prs_;
+ }
+
+ vec_E<Primitive<Dim>> getClearedPrimitives() {
+   return cleared_prs_;
+ }
 protected:
  std::shared_ptr<PolyMapUtil<Dim>> map_util_;
+ vec_E<Primitive<Dim>> blocked_prs_;
+ vec_E<Primitive<Dim>> cleared_prs_;
 };
 
 typedef PolyMapPlanner<2> PolyMapPlanner2D;

mpl_external_planner/include/mpl_external_planner/poly_map_planner/poly_map_util.h

@@ -19,21 +19,20 @@ class PolyMapUtil {
     PolyMapUtil() {}
 
     ///Set static polyhedron obstacles
-    void addStaticObstacle(const PolyhedronObstacle<Dim>& o) {
-      static_obs_.push_back(o);
+    void setStaticObstacle(const vec_E<PolyhedronObstacle<Dim>>& obs) {
+      static_obs_ = obs;
     }
 
     ///Set linear polyhedron obstacles
-    void addLinearObstacle(const PolyhedronLinearObstacle<Dim>& o) {
-      linear_obs_.push_back(o);
+    void setLinearObstacle(const vec_E<PolyhedronLinearObstacle<Dim>>& obs) {
+      linear_obs_ = obs;
     }
 
     ///Set non-linear polyhedron obstacles
-    void addNonlinearObstacle(const PolyhedronNonlinearObstacle<Dim>& o) {
-      nonlinear_obs_.push_back(o);
+    void setNonlinearObstacle(const vec_E<PolyhedronNonlinearObstacle<Dim>>& obs) {
+      nonlinear_obs_ = obs;
     }
 
-
     ///Set bounding box for 2D
     template<int U = Dim>
       typename std::enable_if<U == 2>::type
@@ -60,23 +59,21 @@ class PolyMapUtil {
         bbox_ = Vs;
       }
 
-    /// Check if a point is inside bounding box and outside static obstacles
+    /// Check if a point is inside bounding box
     bool isValid(const Vecf<Dim>& pt) const {
-      if (!bbox_.inside(pt))
-        return false;
-      for(const auto& poly: static_obs_) {
-        if(poly.inside(pt))
-          return false;
-      }
-
-      return true;
+      return bbox_.inside(pt);
     }
 
     /// Check if a point is valid and not colliding moving obstacles
     /*
     bool isFree(const Vecf<Dim>& pt, decimal_t t) const {
       if(!isValid(pt))
         return false;
+      for(const auto& poly: static_obs_) {
+        if(poly.inside(pt))
+          return false;
+      }
+
       for(const auto& poly: linear_obs_) {
         if(poly.inside(pt, t))
           return false;

mpl_test_node/CMakeLists.txt

@@ -15,6 +15,9 @@ catkin_simple()
 cs_add_executable(map_planner_node src/map_planner_node.cpp)
 target_link_libraries(map_planner_node ${MOTION_PRIMITIVE_LIBRARY_LIBRARIES})
 
+cs_add_executable(map_replanner_node src/map_replanner_node.cpp)
+target_link_libraries(map_replanner_node ${MOTION_PRIMITIVE_LIBRARY_LIBRARIES})
+
 cs_add_executable(distance_map_planner_node src/distance_map_planner_node.cpp)
 target_link_libraries(distance_map_planner_node ${MOTION_PRIMITIVE_LIBRARY_LIBRARIES})
 
@@ -24,8 +27,8 @@ target_link_libraries(ellipsoid_planner_node ${MOTION_PRIMITIVE_LIBRARY_LIBRARIE
 cs_add_executable(poly_map_planner_node src/poly_map_planner_node.cpp)
 target_link_libraries(poly_map_planner_node ${MOTION_PRIMITIVE_LIBRARY_LIBRARIES})
 
-cs_add_executable(map_replanner_node src/map_replanner_node.cpp)
-target_link_libraries(map_replanner_node ${MOTION_PRIMITIVE_LIBRARY_LIBRARIES})
+cs_add_executable(poly_map_replanner_node src/poly_map_replanner_node.cpp)
+target_link_libraries(poly_map_replanner_node ${MOTION_PRIMITIVE_LIBRARY_LIBRARIES})
 
 cs_add_executable(traj_solver_node src/traj_solver_node.cpp)
 target_link_libraries(traj_solver_node ${MOTION_PRIMITIVE_LIBRARY_LIBRARIES})

mpl_test_node/launch/poly_map_replanner_node/replan.sh

@@ -0,0 +1 @@
+rostopic pub -1 /replan std_msgs/Bool "{data: true}"

mpl_test_node/launch/poly_map_replanner_node/rviz.launch

@@ -0,0 +1,9 @@
+<launch>
+  <arg name="test" default="poly_map_replanner_node"/>
+
+  <node pkg="rviz"
+    type="rviz"
+    name="rviz"
+    output="screen"
+    args="-d $(find mpl_test_node)/launch/$(arg test)/test.rviz"/>
+</launch>

mpl_test_node/launch/poly_map_replanner_node/test.launch

@@ -0,0 +1,30 @@
+<launch>
+  <arg name="debug" default="false"/>
+  <!--<arg name="debug_valgrind" default="false"/>-->
+
+  <arg name="prefix" value="" unless="$(arg debug)"/>
+  <arg name="prefix" value="gdb -ex run --args" if="$(arg debug)"/>
+
+  <node pkg="mpl_test_node"
+    type="poly_map_replanner_node"
+    name="test_primitive"
+    launch-prefix="$(arg prefix)"
+    clear_params="true"
+    output="screen">
+    <remap from="~replan" to="/replan"/>
+    <!-- Set start and goal -->
+    <param name="goal_x" value="12"/>
+    <param name="goal_y" value="0"/>
+    <param name="start_vx" value="0.0"/>
+    <param name="start_vy" value="0.0"/>
+    <param name="start_x" value="8.1"/>
+    <param name="start_y" value="0.0"/>
+    <!-- Set dynamic constraints -->
+    <param name="v_max" value="2.0"/>
+    <param name="a_max" value="1.0"/>
+    <param name="u" value="1.0"/>
+    <param name="dt" value="1.0"/>
+    <!-- Set obstacle config -->
+  </node>
+
+</launch>