Controller Bug Fixes (#212)

* Bug Fixes
Fix seg faults related to short paths sampling only every five vertices
Use min speed parameter from config file

* Warnings reductions
Change `int` to `size_t` in many for loops
Use some const references in functions

main/src/vtr_path_planning/include/vtr_path_planning/cbit/cbit_path_planner.hpp

@@ -42,6 +42,9 @@
 // Note long term, this class should probably be inherited by the base path planner
 
 // enum for path traversal direction:
+
+using namespace vtr;
+
 enum PathDirection 
 {
     PATH_DIRECTION_REVERSE = -1,
@@ -52,8 +55,8 @@ class CBITPlanner {
     public:
         CBITConfig conf;
         std::shared_ptr<CBITPath> global_path;
-        std::shared_ptr<Node> p_start;
-        std::shared_ptr<Node> p_goal;
+        Node::Ptr p_start;
+        Node::Ptr p_goal;
         std::vector<double> path_x;
         std::vector<double> path_y;
         double sample_box_height;
@@ -64,10 +67,11 @@ class CBITPlanner {
 
         // Repair mode variables
         bool repair_mode = false; // Flag for whether or not we should resume the planner in repair mode to update the tree following a state update
-        std::shared_ptr<Node> repair_vertex;
+        Node::Ptr repair_vertex;
         double repair_g_T_old;
         double repair_g_T_weighted_old;
-        std::shared_ptr<Node> p_goal_backup;
+
+        Node::Ptr p_goal_backup;
 
 
         // For storing the most up-to-date euclidean robot pose
@@ -106,7 +110,7 @@ class CBITPlanner {
         void InitializePlanningSpace();
         void Planning(vtr::path_planning::BasePathPlanner::RobotState& robot_state, std::shared_ptr<CBITCostmap> costmap_ptr, std::shared_ptr<CBITCorridor> corridor_ptr, PathDirection path_direction);
         void ResetPlanner();
-        std::shared_ptr<Node> UpdateStateSID(int SID, vtr::tactic::EdgeTransform T_p_r);
+        std::shared_ptr<Node> UpdateStateSID(size_t SID, vtr::tactic::EdgeTransform T_p_r);
         std::vector<std::shared_ptr<Node>> SampleBox(int m);
         std::vector<std::shared_ptr<Node>> SampleFreeSpace(int m);
         double BestVertexQueueValue();
@@ -127,4 +131,4 @@ class CBITPlanner {
         bool costmap_col_tight(Node node);
         bool discrete_collision(std::vector<std::vector<double>> obs, double discretization, Node start, Node end);
         std::shared_ptr<Node> col_check_path_v2(double max_lookahead_p);
-};
+};

main/src/vtr_path_planning/include/vtr_path_planning/cbit/generate_pq.hpp

@@ -48,8 +48,8 @@ class CBITPath {
         double delta_p_calc(Pose start_pose, Pose end_pose, double alpha); // Function for computing delta p intervals in p,q space
     // Internal function declarations
     private:
-        Eigen::Spline<double, 2> spline_path_xy(std::vector<Pose> input_path); // Processes the input discrete path into a cubic spline
-        Eigen::Spline<double, 2> spline_path_xz_yz(std::vector<Pose> input_path); // Processes the input discrete path into a cubic spline
+        Eigen::Spline<double, 2> spline_path_xy(const std::vector<Pose> &input_path); // Processes the input discrete path into a cubic spline
+        Eigen::Spline<double, 2> spline_path_xz_yz(const std::vector<Pose> &input_path); // Processes the input discrete path into a cubic spline
 
         double radius_of_curvature(double dist, Eigen::Spline<double, 2> spline); // Calculates the radius of curvature using the spline at a given distance along the spline
 

main/src/vtr_path_planning/include/vtr_path_planning/cbit/utils.hpp

@@ -59,17 +59,20 @@ class Pose {
 
 class Node {
     public:
+        using Ptr = std::shared_ptr<Node>;
+
         double p;
         double q;
         double z; // experimental
         double g_T = NAN;
         double g_T_weighted = NAN;
-        std::shared_ptr<Node> parent; // Shared pointer method (safer) // NOTE: Shared pointers initialize to null
-        std::shared_ptr<Node> child; // Shared pointer method (safer)
+        Node::Ptr parent; // Shared pointer method (safer) // NOTE: Shared pointers initialize to null
+        Node::Ptr child; // Shared pointer method (safer)
         bool worm_hole = false;
         Node(double p_in, double q_in) // Node constructor
         : p{p_in}
         , q{q_in}
+        , z{0}
         {
         }
 

main/src/vtr_path_planning/include/vtr_path_planning/mpc/custom_loss_functions.hpp

@@ -53,7 +53,7 @@ class L2WeightedLossFunc : public BaseLossFunc {
    * \brief Weight for iteratively reweighted least-squares (influence function
    * div. by error)
    */
-  double weight(double whitened_error_norm) const override { return weight_val; }
+  double weight(double) const override { return weight_val; }
 
 };
 

main/src/vtr_path_planning/include/vtr_path_planning/mpc/mpc_path_planner.hpp

@@ -87,7 +87,7 @@ struct MPCResult SolveMPC(const MPCConfig& config);
 struct PoseResultTracking GenerateTrackingReference(std::shared_ptr<std::vector<Pose>> cbit_path_ptr,  std::tuple<double, double, double, double, double, double> robot_pose, int K, double DT, double VF);
 
 // Helper function for generating reference measurements poses from a discrete path to use for tracking the path at a desired forward velocity
-struct PoseResultHomotopy GenerateHomotopyReference(std::shared_ptr<CBITPath> global_path_ptr, std::shared_ptr<CBITCorridor> corridor_ptr, std::tuple<double, double, double, double, double, double> robot_pose, int K, double DT, double VF, int current_sid, std::vector<double> p_interp_vec);
+struct PoseResultHomotopy GenerateHomotopyReference(std::shared_ptr<CBITPath> global_path_ptr, std::shared_ptr<CBITCorridor> corridor_ptr, std::tuple<double, double, double, double, double, double> robot_pose, const std::vector<double> &p_interp_vec);
 
 // Helper function for post-processing and saturating the velocity command
 Eigen::Matrix<double, 2, 1> SaturateVel(Eigen::Matrix<double, 2, 1> applied_vel, double v_lim, double w_lim);

main/src/vtr_path_planning/include/vtr_path_planning/mpc/speed_scheduler.hpp

@@ -24,4 +24,4 @@
 #include "vtr_path_planning/cbit/generate_pq.hpp"
 
 
-double ScheduleSpeed(const std::vector<double>& disc_path_curvature_xy, const std::vector<double>& disc_path_curvature_xz_yz, double VF, int curr_sid, double planar_curv_weight, double profile_curv_weight, double eop_weight, double horizon_step_size, double min_vel);
+double ScheduleSpeed(const std::vector<double>& disc_path_curvature_xy, const std::vector<double>& disc_path_curvature_xz_yz, double VF, unsigned curr_sid, double planar_curv_weight, double profile_curv_weight, double eop_weight, double horizon_step_size, double min_vel);

main/src/vtr_path_planning/src/cbit/cbit.cpp

@@ -21,7 +21,7 @@
 #include "vtr_path_planning/mpc/mpc_path_planner.hpp"
 
 #include <tf2/convert.h>
-#include <tf2_eigen/tf2_eigen.h>
+#include <tf2_eigen/tf2_eigen.hpp>
 
 namespace vtr {
 namespace path_planning {
@@ -363,7 +363,7 @@ auto CBIT::computeCommand(RobotState& robot_state) -> Command {
 
     // Storing sequences of costmaps for temporal filtering purposes
     // For the first x iterations, fill the obstacle vector
-    if (costmap_ptr->obs_map_vect.size() < config_->costmap_history)
+    if (costmap_ptr->obs_map_vect.size() < (unsigned) config_->costmap_history)
     {
       costmap_ptr->obs_map_vect.push_back(obs_map);
       costmap_ptr->T_c_w_vect.push_back(costmap_ptr->T_c_w);
@@ -497,19 +497,19 @@ auto CBIT::computeCommand(RobotState& robot_state) -> Command {
     std::vector<double> q_interp_vec = ref_tracking_result.q_interp_vec;
 
     // Synchronized Tracking/Teach Reference Poses For Homotopy Guided MPC:
-    auto ref_homotopy_result = GenerateHomotopyReference(global_path_ptr, corridor_ptr, robot_pose, K,  DT, VF, curr_sid, p_interp_vec);
+    auto ref_homotopy_result = GenerateHomotopyReference(global_path_ptr, corridor_ptr, robot_pose, p_interp_vec);
     auto homotopy_poses = ref_homotopy_result.poses;
     bool point_stabilization = ref_homotopy_result.point_stabilization;
     std::vector<double> barrier_q_left = ref_homotopy_result.barrier_q_left;
     std::vector<double> barrier_q_right = ref_homotopy_result.barrier_q_right;
+
     std::vector<lgmath::se3::Transformation> tracking_pose_vec;
-    for (int i = 0; i<tracking_poses.size(); i++)
-    {
+    for (size_t i = 0; i < tracking_poses.size(); i++) {
       tracking_pose_vec.push_back(tracking_poses[i].inverse());
     }
+
     std::vector<lgmath::se3::Transformation> homotopy_pose_vec;
-    for (int i = 0; i<homotopy_poses.size(); i++)
-    {
+    for (size_t i = 0; i < homotopy_poses.size(); i++) {
       homotopy_pose_vec.push_back(homotopy_poses[i].inverse());
     }