cov_v doesn't work for lpastar somehow

sikang · Aug 5, 2018 · c6d19a6 · c6d19a6
1 parent 00c2c4a
commit c6d19a6
Show file tree

Hide file tree

Showing 12 changed files with 178 additions and 79 deletions.
diff --git a/motion_primitive_library b/motion_primitive_library
diff --git a/mpl_external_planner/include/mpl_external_planner/poly_map_planner/poly_map_planner.h b/mpl_external_planner/include/mpl_external_planner/poly_map_planner/poly_map_planner.h
@@ -84,8 +84,8 @@ class PolyMapPlanner : public PlannerBase<Dim, Waypoint<Dim>> {
      }
    }
 
-   this->ss_ptr_->increaseCost(blocked_nodes);
    this->ss_ptr_->decreaseCost(cleared_nodes, this->ENV_);
+   this->ss_ptr_->increaseCost(blocked_nodes);
  }
 
  vec_E<Primitive<Dim>> getBlockedPrimitives() {
@@ -95,6 +95,7 @@ class PolyMapPlanner : public PlannerBase<Dim, Waypoint<Dim>> {
  vec_E<Primitive<Dim>> getClearedPrimitives() {
    return cleared_prs_;
  }
+
 protected:
  std::shared_ptr<PolyMapUtil<Dim>> map_util_;
  vec_E<Primitive<Dim>> blocked_prs_;

diff --git a/mpl_external_planner/include/mpl_external_planner/poly_map_planner/poly_map_util.h b/mpl_external_planner/include/mpl_external_planner/poly_map_planner/poly_map_util.h
@@ -86,7 +86,7 @@ class PolyMapUtil {
     }
     */
 
-		///Check if a primitive is inside the SFC from \f$t: 0 \rightarrow dt\f$
+		///Check if a primitive is in free space, \f$t\f$ is the start time of primitive
     bool isFree(const Primitive<Dim> &pr, decimal_t t) const {
       for(const auto& poly: static_obs_) {
         if(collide(pr, poly))

diff --git a/mpl_external_planner/include/mpl_external_planner/poly_map_planner/simple_obstacle.h b/mpl_external_planner/include/mpl_external_planner/poly_map_planner/simple_obstacle.h
@@ -33,6 +33,14 @@ class PolyhedronObstacle {
   virtual void update(decimal_t t) {
   }
 
+  vec_E<Polyhedron<Dim>> predict(decimal_t dt, size_t N, decimal_t t = 0) const {
+    vec_E<Polyhedron<Dim>> polys;
+    for(size_t i = 0; i < N; i++) {
+      polys.push_back(poly(i * dt + t));
+    }
+    return polys;
+  }
+
  protected:
   /// Obstacle initial contour
   Polyhedron<Dim> poly_;

diff --git a/mpl_test_node/launch/poly_map_replanner_node/test.launch b/mpl_test_node/launch/poly_map_replanner_node/test.launch
@@ -23,7 +23,7 @@
     <param name="start_x" value="4"/>
     <param name="start_y" value="0.0"/>
     <!-- Set dynamic constraints -->
-    <param name="v_max" value="2.0"/>
+    <param name="v_max" value="1.0"/>
     <param name="a_max" value="1.0"/>
     <param name="u" value="1.0"/>
     <param name="dt" value="1.0"/>

diff --git a/mpl_test_node/launch/poly_map_replanner_node/test.rviz b/mpl_test_node/launch/poly_map_replanner_node/test.rviz
@@ -7,6 +7,8 @@ Panels:
         - /Global Options1
         - /Status1
         - /lpastar1
+        - /PolyhedronArray2
+        - /PrimitiveArray1
       Splitter Ratio: 0.5
     Tree Height: 747
   - Class: rviz/Selection
@@ -27,7 +29,7 @@ Panels:
     Experimental: false
     Name: Time
     SyncMode: 0
-    SyncSource: ""
+    SyncSource: sg
 Visualization Manager:
   Class: ""
   Displays:
@@ -59,7 +61,7 @@ Visualization Manager:
     - Alpha: 1
       Autocompute Intensity Bounds: true
       Autocompute Value Bounds:
-        Max Value: 0
+        Max Value: 1
         Min Value: 0
         Value: true
       Axis: Z
@@ -216,6 +218,43 @@ Visualization Manager:
           YawVis: false
       Enabled: true
       Name: lpastar
+    - Alpha: 0.200000003
+      BoundColor: 255; 0; 0
+      Class: decomp_rviz_plugins/PolyhedronArray
+      Enabled: true
+      MeshColor: 0; 170; 255
+      Name: PolyhedronArray
+      Scale: 0.0500000007
+      State: Bound
+      Topic: /test_primitive/predicts
+      Unreliable: false
+      Value: true
+      VsColor: 0; 255; 0
+      VsScale: 1
+    - AccColor: 10; 200; 55
+      AccScale: 0.0199999996
+      AccVis: false
+      Class: planning_rviz_plugins/PrimitiveArray
+      Enabled: true
+      JrkColor: 200; 20; 55
+      JrkScale: 0.0199999996
+      JrkVis: false
+      Name: PrimitiveArray
+      Num: 2
+      NumYaw: 2
+      PosColor: 204; 51; 204
+      PosScale: 0.0299999993
+      Topic: /test_primitive/graph
+      Unreliable: false
+      Value: true
+      VelColor: 85; 85; 255
+      VelScale: 0.0199999996
+      VelVis: false
+      YawColor: 100; 20; 55
+      YawScale: 0.0500000007
+      YawTriangleAngle: 0.699999988
+      YawTriangleScale: 0.5
+      YawVis: false
   Enabled: true
   Global Options:
     Background Color: 255; 255; 255
@@ -250,7 +289,7 @@ Visualization Manager:
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.00999999978
-      Scale: 89.3197174
+      Scale: 41.4804382
       Target Frame: <Fixed Frame>
       Value: TopDownOrtho (rviz)
       X: 9.86995983
@@ -273,4 +312,4 @@ Window Geometry:
     collapsed: false
   Width: 1920
   X: 0
-  Y: 48
+  Y: 24
diff --git a/mpl_test_node/src/obstacle_config.hpp b/mpl_test_node/src/obstacle_config.hpp
@@ -72,6 +72,7 @@ struct ObstacleCourse {
 
     return Trajectory<Dim>(segs);
   }
+
 };