rename tol_dis to tol_pos, add debug mode for traj_solver, fix warnings

include/mpl_planner/common/env_base.h

@@ -23,12 +23,12 @@ class env_base {
     virtual bool is_goal(const Waypoint<Dim>& state) const {
       if(state.t >= t_max_)
         return true;
-      bool goaled = (state.pos - goal_node_.pos).template lpNorm<Eigen::Infinity>() <= tol_dis_;
-      if(goaled && goal_node_.use_vel && tol_vel_ > 0)
+      bool goaled = (state.pos - goal_node_.pos).template lpNorm<Eigen::Infinity>() <= tol_pos_;
+      if(goaled && tol_vel_ >= 0)
         goaled = (state.vel - goal_node_.vel).template lpNorm<Eigen::Infinity>() <= tol_vel_;
-      if(goaled && goal_node_.use_acc && tol_acc_ > 0)
+      if(goaled && tol_acc_ >= 0)
         goaled = (state.acc - goal_node_.acc).template lpNorm<Eigen::Infinity>() <= tol_acc_;
-      if(goaled && goal_node_.use_yaw && tol_yaw_ > 0)
+      if(goaled && tol_yaw_ >= 0)
         goaled = std::abs(state.yaw - goal_node_.yaw) <= tol_yaw_;
       return goaled;
     }
@@ -269,8 +269,8 @@ class env_base {
     }
 
     ///Set distance tolerance for goal region
-    void set_tol_dis(decimal_t dis) {
-      tol_dis_ = dis;
+    void set_tol_pos(decimal_t pos) {
+      tol_pos_ = pos;
     }
 
     ///Set velocity tolerance for goal region
@@ -349,7 +349,7 @@ class env_base {
       printf("+              j_max: %.2f               +\n", j_max_);
       printf("+            yaw_max: %.2f               +\n", yaw_max_);
       printf("+              U num: %zu                +\n", U_.size());
-      printf("+            tol_dis: %.2f               +\n", tol_dis_);
+      printf("+            tol_pos: %.2f               +\n", tol_pos_);
       printf("+            tol_vel: %.2f               +\n", tol_vel_);
       printf("+            tol_acc: %.2f               +\n", tol_acc_);
       printf("+            tol_yaw: %.2f               +\n", tol_yaw_);
@@ -403,13 +403,13 @@ class env_base {
     /// weight of yaw
     decimal_t wyaw_{1};
     ///tolerance of position for goal region, 0.5 is the default
-    decimal_t tol_dis_{0.5};
+    decimal_t tol_pos_{0.5};
     ///tolerance of velocity for goal region, 0 means no tolerance
-    decimal_t tol_vel_{0.0};
+    decimal_t tol_vel_{-1};
     ///tolerance of acceleration for goal region, 0 means no tolerance
-    decimal_t tol_acc_{0.0};
+    decimal_t tol_acc_{-1};
     ///tolerance of yaw for goal region, 0 means no tolerance
-    decimal_t tol_yaw_{0.0};
+    decimal_t tol_yaw_{-1};
     ///max velocity
     decimal_t v_max_{-1};
     ///max acceleration

include/mpl_planner/common/planner_base.h

@@ -99,6 +99,46 @@ template <int Dim, typename Coord> class PlannerBase {
     }
     return ps;
   }
+  /// Get points at certain state
+  vec_Vecf<Dim> getStates(const Coord& state) const {
+    vec_Vecf<Dim> ps;
+    vec_Vecf<Dim> vels;
+    vec_Vecf<Dim> accs;
+    for (const auto &it : ss_ptr_->hm_) {
+      if (it.second) {
+        auto coord = it.second->coord;
+        bool add = true;
+        if(state.use_vel && (state.vel - coord.vel).norm() > 1e-3)
+          add = false;
+        if(state.use_acc && (state.acc - coord.acc).norm() > 1e-3)
+          add = false;
+        if(state.use_jrk && (state.jrk - coord.jrk).norm() > 1e-3)
+          add = false;
+        if(add) {
+          //std::cout << "add pos: " << coord.pos.transpose() <<
+          //  " vel: " << coord.vel.transpose() <<
+          //  " acc: " << coord.acc.transpose() << std::endl;
+          ps.push_back(coord.pos);
+          bool new_vel = true;
+          for(const auto& it: vels) {
+            if((it - coord.vel).norm() < 1e-3) {
+              new_vel = false;
+              break;
+            }
+          }
+
+          if(new_vel)
+            vels.push_back(coord.vel);
+        }
+      }
+    }
+
+    for(const auto& it: vels)
+      std::cout << "vel: " << it.transpose() << std::endl;
+    std::cout << "=========================" << std::endl;
+    return ps;
+  }
+
   /// Get expanded points, for A* it should be the same as the close set
   vec_Vecf<Dim> getExpandedNodes() const {
     return ENV_->expanded_nodes_;
@@ -208,12 +248,12 @@ template <int Dim, typename Coord> class PlannerBase {
       printf("[PlannerBase] set prior trajectory\n");
   }
   /// Set tolerance in geometric and dynamic spaces
-  void setTol(decimal_t tol_dis, decimal_t tol_vel = 0, decimal_t tol_acc = 0) {
-    ENV_->set_tol_dis(tol_dis);
+  void setTol(decimal_t tol_pos, decimal_t tol_vel = -1, decimal_t tol_acc = -1) {
+    ENV_->set_tol_pos(tol_pos);
     ENV_->set_tol_vel(tol_vel);
     ENV_->set_tol_acc(tol_acc);
     if (planner_verbose_) {
-      printf("[PlannerBase] set tol_dis: %f\n", tol_dis);
+      printf("[PlannerBase] set tol_pos: %f\n", tol_pos);
       printf("[PlannerBase] set tol_vel: %f\n", tol_vel);
       printf("[PlannerBase] set tol_acc: %f\n", tol_acc);
     }

include/mpl_planner/env/env_map.h

@@ -21,13 +21,18 @@ template <int Dim> class env_map : public env_base<Dim> {
 
   /// Check if state hit the goal region, use L-1 norm
   bool is_goal(const Waypoint<Dim> &state) const {
-    bool goaled = (state.pos - this->goal_node_.pos).template lpNorm<Eigen::Infinity>() <= this->tol_dis_;
-    if (goaled && this->goal_node_.use_vel && this->tol_vel_ > 0)
-      goaled = (state.vel - this->goal_node_.vel).template lpNorm<Eigen::Infinity>() <= this->tol_vel_;
-    if (goaled && this->goal_node_.use_acc && this->tol_acc_ > 0)
-      goaled = (state.acc - this->goal_node_.acc).template lpNorm<Eigen::Infinity>() <= this->tol_acc_;
-    if(goaled && this->goal_node_.use_yaw && this->tol_yaw_ > 0)
-      goaled = std::abs(state.yaw- this->goal_node_.yaw) <= this->tol_yaw_;
+    bool goaled =
+        (state.pos - this->goal_node_.pos).template lpNorm<Eigen::Infinity>() <=
+        this->tol_pos_;
+
+    if (goaled && this->tol_vel_ >= 0)
+      goaled = (state.vel - this->goal_node_.vel)
+                   .template lpNorm<Eigen::Infinity>() <= this->tol_vel_;
+    if (goaled && this->tol_acc_ >= 0)
+      goaled = (state.acc - this->goal_node_.acc)
+                   .template lpNorm<Eigen::Infinity>() <= this->tol_acc_;
+    if (goaled && this->tol_yaw_ >= 0)
+      goaled = std::abs(state.yaw - this->goal_node_.yaw) <= this->tol_yaw_;
     if (goaled) {
       auto pns = map_util_->rayTrace(state.pos, this->goal_node_.pos);
       for (const auto &it : pns) {
@@ -275,7 +280,7 @@ template <int Dim> class env_map : public env_base<Dim> {
       printf("+              j_max: %.2f               +\n", this->j_max_);
       printf("+            yaw_max: %.2f               +\n", this->yaw_max_);
       printf("+              U num: %zu                +\n", this->U_.size());
-      printf("+            tol_dis: %.2f               +\n", this->tol_dis_);
+      printf("+            tol_pos: %.2f               +\n", this->tol_pos_);
       printf("+            tol_vel: %.2f               +\n", this->tol_vel_);
       printf("+            tol_acc: %.2f               +\n", this->tol_acc_);
       printf("+            tol_yaw: %.2f               +\n", this->tol_yaw_);

include/mpl_traj_solver/traj_solver.h

@@ -16,14 +16,14 @@ template <int Dim> class TrajSolver {
    * @param yaw_control define the control flag for yaw start and end
    */
   TrajSolver(Control::Control control,
-             Control::Control yaw_control = Control::VEL)
+             Control::Control yaw_control = Control::VEL, bool debug = false)
       : control_(control), yaw_control_(yaw_control) {
     if(control == Control::VEL || control == Control::VELxYAW)
-      poly_solver_.reset(new PolySolver<Dim>(0, 1));
+      poly_solver_.reset(new PolySolver<Dim>(0, 1, debug));
     else if(control == Control::ACC || control == Control::ACCxYAW)
-      poly_solver_.reset(new PolySolver<Dim>(1, 2));
+      poly_solver_.reset(new PolySolver<Dim>(1, 2, debug));
     else if(control == Control::JRK || control == Control::JRKxYAW)
-      poly_solver_.reset(new PolySolver<Dim>(2, 3));
+      poly_solver_.reset(new PolySolver<Dim>(2, 3, debug));
     //Due to dimension issue, only workd up to thrid order
     //if(control == Control::SNP || control == Control::SNPxYAW)
     //poly_solver_.reset(new PolySolver<Dim>(3, 4));
@@ -114,6 +114,8 @@ template <int Dim> class TrajSolver {
   /// Get the Waypoint array used to solve trajectory
   vec_E<Waypoint<Dim>> getWaypoints() const { return waypoints_; }
 
+  /// Get the time allocation
+  std::vector<decimal_t> getDts() const { return dts_; }
 
 private:
   /// Internal time allocation from path and vel using L-inf

src/mpl_planner/map_planner.cpp

@@ -256,7 +256,7 @@ vec_E<Vecf<Dim>> MapPlanner<Dim>::calculateGradient(const Veci<Dim>& coord1, con
   int rn = std::ceil(potential_radius_(0)/map_util_->getRes());
 
   vec_E<Vecf<Dim>> g(dim(0) * dim(1), Vecf<Dim>::Zero());
-  Veci<Dim> n;
+  Veci<Dim> n = Veci<Dim>::Zero();
   for(n(0) = coord1(0)-rn; n(0) < coord2(0)+rn; n(0)++) {
     for(n(1) = coord1(1)-rn; n(1) < coord2(1)+rn; n(1)++) {
       if(map_util_->isOutside(n))

src/mpl_traj_solver/poly_solver.cpp

@@ -6,6 +6,10 @@ PolySolver<Dim>::PolySolver(unsigned int smooth_derivative_order,
     : N_(2 * (smooth_derivative_order + 1)), R_(minimize_derivative),
       debug_(debug) {
   ptraj_ = std::make_shared<PolyTraj<Dim>>();
+  if(debug_) {
+    std::cout << "smooth_derivative_order: " << smooth_derivative_order << std::endl;
+    std::cout << "minimize_derivative: " << minimize_derivative << std::endl;
+  }
 }
 
 template <int Dim>