Path planner dev

config/honeycomb_grizzly_default.yaml

@@ -14,7 +14,7 @@
       - tactic.module
       - tactic.module.live_mem_manager
       - tactic.module.graph_mem_manager
-        # path planner
+        # path planner        
         # "path_planning",
         # "path_planning.teb",
         # "path_planning.cbit",
@@ -44,8 +44,15 @@
       # "lidar.terrain_assessment",
     robot_frame: base_link
     env_info_topic: env_info
+    # lidar related
     lidar_frame: velodyne
     lidar_topic: /velodyne_points
+
+    ############ map projection configuration ############
+    graph_projection:
+      origin_lat: 43.78220 # PETAWAWA: 45.8983, # UTIAS: 43.78220
+      origin_lng: -79.4661 # PETAWA: -77.2829, # UTIAS: -79.4661
+      origin_theta: 0.0
     graph_map:
       origin_lat: 43.7822
       origin_lng: -79.4661
@@ -58,7 +65,8 @@
       localization_skippable: false
       task_queue_num_threads: 1
       task_queue_size: -1
-      route_completion_translation_threshold: 2.0
+
+      route_completion_translation_threshold: 0.5 
       chain:
         min_cusp_distance: 1.5
         angle_weight: 7.0
@@ -101,23 +109,30 @@
       filtering:
         type: lidar.preprocessing_v2
         num_threads: 8
+
         crop_range: 20.0
-        frame_voxel_size: 0.1
-        vertical_angle_res: 0.0132645
-        polar_r_scale: 2.0
-        r_scale: 4.0
-        h_scale: 1.54
-        num_sample1: 10000
-        min_norm_score1: 0.95
-        num_sample2: 10000
-        min_norm_score2: 0.2
-        min_normal_estimate_dist: 1.0
-        max_normal_estimate_angle: 0.44
-        cluster_num_sample: 10000
+
+        frame_voxel_size: 0.1 # grid subsampling voxel size
+
+        vertical_angle_res: 0.0132645 # vertical angle resolution in radius, equal to 0.76 degree documented in the manual
+        polar_r_scale: 2.0 # polar_r_scale x vertical_angle_res = nearest neighbor search radius for normal computation
+        r_scale: 4.0 # scale down point range by this value after taking log, whatever works
+        h_scale: 1.54 # scale down yaw(phi) by this value so that vertical resolution ~= horizontal resolution, horizontal resolution when 5Hz spin frequence is ~1.17 degree, so 1.17 / 0.76 = 1.54
+
+        num_sample1: 10000 # max number of sample after filtering based on planarity
+        min_norm_score1: 0.95 # min planarity score
+
+        num_sample2: 10000 # max number of sample after filtering based on planarity
+        min_norm_score2: 0.2 # 0.2 is when the incident angle 5/12 * pi
+        min_normal_estimate_dist: 1.0 # minimum distance to estimate normal in meters
+        max_normal_estimate_angle: 0.44 # must <1/2, this value will be timed by M_PI
+
+        cluster_num_sample: 10000 # maxnumber of sample after removing isolated points
         visualize: true
     odometry:
       icp:
         type: lidar.odometry_icp
+        # continuous time estimation
         use_trajectory_estimation: false
         traj_num_extra_states: 0
         traj_lock_prev_pose: false
@@ -144,7 +159,7 @@
         visualize: true
       mapping:
         type: lidar.odometry_map_maintenance_v2
-        map_voxel_size: 0.2
+        map_voxel_size: 0.1
         crop_range_front: 40.0
         back_over_front_ratio: 0.5
         point_life_time: 20.0
@@ -156,22 +171,22 @@
       intra_exp_merging:
         type: lidar.intra_exp_merging_v2
         depth: 6.0
-        map_voxel_size: 0.2
+        map_voxel_size: 0.1
         crop_range_front: 40.0
         back_over_front_ratio: 0.5
         visualize: true
       dynamic_detection:
         type: lidar.dynamic_detection
         depth: 12.0
-        horizontal_resolution: 0.041
-        vertical_resolution: 0.026
-        max_num_observations: 2000
+        horizontal_resolution: 0.041 # 0.02042
+        vertical_resolution: 0.026 # 0.01326
+        max_num_observations: 10000
         min_num_observations: 4
         dynamic_threshold: 0.3
         visualize: true
       inter_exp_merging:
         type: lidar.inter_exp_merging_v2
-        map_voxel_size: 0.2
+        map_voxel_size: 0.1
         max_num_experiences: 128
         distance_threshold: 0.6
         planar_threshold: 0.2
@@ -222,8 +237,12 @@
         support_radius: 0.25
         support_variance: 0.05
         support_threshold: 2.5
-        influence_distance: 0.55
-        minimum_distance: 0.35
+        influence_distance: 0.50 # was 0.5 Jordy # Note that the total distancr=e where grid cells have values > 0 is min dist + influence dist, not influence dist!
+        minimum_distance: 0.9 # was 0.3 Jordy 
+
+        # cost map
+        costmap_history_size: 15 # Keep between 3 and 20, used for temporal filtering
+
         resolution: 0.25
         size_x: 16.0
         size_y: 8.0
@@ -292,8 +311,8 @@
         run_async: true
         visualize: true
     path_planning:
-      type: cbit
-      control_period: 33
+      type: cbit.lidar # teb.lidar for old teb version, cbit for obstacle free path tracker, cbit.lidar for obstacle avoidance version
+      control_period: 50 # ms
       teb:
         visualize: true
         extrapolate: false
@@ -313,53 +332,52 @@
       cbit:
         obs_padding: 0.0
         curv_to_euclid_discretization: 20
-        sliding_window_width: 10.0
+        sliding_window_width: 12.0
         sliding_window_freespace_padding: 0.5
         corridor_resolution: 0.05
-        state_update_freq: 0.5
+        state_update_freq: 1.0
         update_state: true
         rand_seed: 1
-        initial_samples: 250
-        batch_samples: 100
+
+        # Planner Tuning Params
+        initial_samples: 300
+        batch_samples: 150
         pre_seed_resolution: 0.5
         alpha: 0.5
         q_max: 2.5
         frame_interval: 50
         iter_max: 10000000
-        eta: 1.1
-        rad_m_exhange: 1.0
+        eta: 1.0
+        rad_m_exhange: 1.00
         initial_exp_rad: 0.75
         extrapolation: false
         incremental_plotting: false
         plotting: true
       costmap:
         costmap_filter_value: 0.01
-        costmap_history: 15
+        costmap_history: 20 # Note I dont think im actually using this one anymore, am using the one in the change detection v3 now
+
       mpc:
-        horizon_steps: 40
-        horizon_step_size: 0.1
-        forward_vel: 1.0
+        # Controller Params
+        horizon_steps: 20
+        horizon_step_size: 0.3
+        forward_vel: 0.75
         max_lin_vel: 1.25
         max_ang_vel: 0.75
-        vehicle_model: unicycle
-        pose_error_cov:
-          - 0.5
-          - 0.5
-          - 0.5
-          - 10.0
-          - 10.0
-          - 10.0
-        vel_error_cov:
-          - 0.1
-          - 1.0
-        acc_error_cov:
-          - 0.1
-          - 0.75
-        kin_error_cov:
-          - 0.001
-          - 0.001
-          - 0.001
-          - 0.001
-          - 0.001
-          - 0.001
+        vehicle_model: "unicycle"
+
+        # Cost Function Weights
+        pose_error_cov: [0.5, 0.5, 0.5, 10000.0, 10000.0, 10000.0] # Used to have yaw components set to 1000 but it seems to cause some instability, others set to 1.0
+        vel_error_cov: [1.0, 10.0] # was [0.1 2.0] # No longer using this cost term
+        acc_error_cov: [1.0, 1.0]
+        kin_error_cov: [0.001, 0.001, 0.001, 0.001, 0.001, 0.001]
+
+        #backup
+        #pose_error_cov: [0.5, 0.5, 0.5, 10.0, 10.0, 10.0] # Used to have yaw components set to 1000 but it seems to cause some instability, others set to 1.0
+        #vel_error_cov: [0.1, 1.0] # was [0.1 2.0]
+        #acc_error_cov: [0.1, 0.75]
+        #kin_error_cov: [0.001, 0.001, 0.001, 0.001, 0.001, 0.001]
+
+        # Misc
+
         command_history_length: 100

main/src/vtr_gui/vtr_gui/vtr-gui/src/components/graph/GraphMap.js

@@ -214,7 +214,8 @@ class GraphMap extends React.Component {
       <>
         {/* Leaflet map container with initial center set to UTIAS (only for initialization) */}
         <MapContainer
-          center={[43.782, -79.466]}
+
+          center={[43.78220, -79.4661]} /* Jordy Modification For PETAWAWA center={[45.8983, -77.2829]} => TODO We should make this set dynamically from the yaml config*/
           zoom={18}
           zoomControl={false}
           whenCreated={this.mapCreatedCallback.bind(this)}

main/src/vtr_lidar/CMakeLists.txt

@@ -71,6 +71,34 @@ ament_target_dependencies(${PROJECT_NAME}_path_planning
   lgmath steam
 )
 
+# START OF JORDY EXPERIMENTAL MATPLOTLIB CHANGES
+# find python libraries
+find_package(Python3 COMPONENTS Interpreter Development NumPy REQUIRED)
+find_package(PythonLibs 3.0 REQUIRED)
+include_directories(${PYTHON3_INCLUDE_DIRS} ${NumPy_INCLUDE_DIRS})
+
+# populate matplotlib repository
+include(FetchContent)
+FetchContent_Declare(
+    matplotlib
+    GIT_REPOSITORY https://github.com/lava/matplotlib-cpp.git
+    GIT_TAG        f23347fca25219d1c42cbb91608b5556814bf572
+)
+FetchContent_GetProperties(matplotlib)
+if(NOT matplotlib_POPULATED)
+    FetchContent_Populate(matplotlib)
+endif()
+include_directories(SYSTEM ${matplotlib_SOURCE_DIR})
+
+target_link_libraries(${PROJECT_NAME}_path_planning ${PYTHON_LIBRARIES})
+
+
+find_package(PythonLibs REQUIRED)
+include_directories(${PYTHON_INCLUDE_DIRS})
+target_link_libraries(${PROJECT_NAME}_path_planning ${PYTHON_LIBRARIES})
+# END OF JORDY EXPERIMENTAL MATPLOTLIBCPP Changes FOR PLOTTING
+
+
 # additional tools for experiments
 file(GLOB_RECURSE TOOLS_SRC
   src/mesh2pcd/*.cpp

main/src/vtr_lidar/include/vtr_lidar/data_types/costmap.hpp

@@ -132,6 +132,10 @@ class DenseCostMap : public BaseCostMap {
   /** \brief update from a sparse cost map */
   void update(const std::unordered_map<costmap::PixKey, float>& values);
 
+  // Modification by Jordy, DEBUG
+  /** \brief update from a standard unordered_map */
+  void update(const std::unordered_map<std::pair<float,float>, float> values);
+
   XY2ValueMap filter(const float& threshold) const override;
 
   /** \brief Returns content of this class as a OccupancyGrid message. */

main/src/vtr_lidar/include/vtr_lidar/modules/planning/change_detection_module_v3.hpp

@@ -58,6 +58,7 @@ class ChangeDetectionModuleV3 : public tactic::BaseModule {
     float support_threshold = 0.0;
 
     // cost map
+    int costmap_history_size = 10;
     float resolution = 1.0;
     float size_x = 20.0;
     float size_y = 20.0;
@@ -88,9 +89,14 @@ class ChangeDetectionModuleV3 : public tactic::BaseModule {
   bool publisher_initialized_ = false;
   rclcpp::Publisher<PointCloudMsg>::SharedPtr scan_pub_;
   rclcpp::Publisher<OccupancyGridMsg>::SharedPtr costmap_pub_;
+  rclcpp::Publisher<OccupancyGridMsg>::SharedPtr filtered_costmap_pub_ ;
   rclcpp::Publisher<PointCloudMsg>::SharedPtr costpcd_pub_;
 
   VTR_REGISTER_MODULE_DEC_TYPE(ChangeDetectionModuleV3);
+
+
+  // Modificatons for Temporal costmap filter
+  std::vector<std::unordered_map<std::pair<float, float>, float>> costmap_history;
 };
 
 }  // namespace lidar

main/src/vtr_lidar/include/vtr_lidar/path_planning/mpc.hpp

@@ -43,4 +43,7 @@ struct mpc_result SolveMPC(Eigen::Matrix<double, 2, 1> previous_vel, lgmath::se3
 struct meas_result GenerateReferenceMeas(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 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);
+Eigen::Matrix<double, 2, 1> SaturateVel(Eigen::Matrix<double, 2, 1> applied_vel, double v_lim, double w_lim);
+
+// Helper function in Generate Reference Meas which interpolates a Transformation measurement gen the cbit_path and the desired measurements p value along the path
+lgmath::se3::Transformation InterpolateMeas(double p_meas, std::vector<double> cbit_p, std::vector<Pose> cbit_path);

main/src/vtr_lidar/src/data_types/costmap.cpp

@@ -102,6 +102,40 @@ void DenseCostMap::update(
   }
 }
 
+// Modification by Jordy for updating dense maps from unordered maps with pairs of points ()
+void DenseCostMap::update(
+    const std::unordered_map<std::pair<float,float>, float> values) {    
+
+  for (const auto val : values) {
+    auto shifted_k = costmap::PixKey(val.first.first / dl_, val.first.second / dl_) - origin_;
+
+    //const auto shifted_k_x = val.first.first;// - origin_.x;
+    //const auto shifted_k_y = val.first.second;// - origin_.y;
+
+
+    // Handling an error where after transformations the shifted key could fall outside the costmap area resulting in an eigen indexing error
+    if (shifted_k.x >= size_x_ / dl_)
+    {
+      shifted_k.x = size_x_ / dl_;
+    }
+    if (shifted_k.x <= -1)
+    {
+      shifted_k.x = 0;
+    }
+    if (shifted_k.y >= size_y_ / dl_)
+    {
+      shifted_k.y = size_y_ / dl_;
+    }
+    if (shifted_k.y <= -1)
+    {
+      shifted_k.y = 0;
+    }
+
+    values_(shifted_k.x, shifted_k.y) = val.second;
+
+  }
+}
+
 auto DenseCostMap::filter(const float& threshold) const -> XY2ValueMap {
   XY2ValueMap filtered;
   filtered.reserve(values_.size());

main/src/vtr_lidar/src/modules/odometry/odometry_icp_module.cpp

@@ -498,10 +498,10 @@ void OdometryICPModule::run_(QueryCache &qdata0, OutputCache &,
     }
     *qdata.T_r_m_odo = T_r_m_eval->value();
     *qdata.timestamp_odo = query_stamp;
-#if 1
-    CLOG(WARNING, "lidar.odometry_icp") << "T_m_r is: " << qdata.T_r_m_odo->inverse().vec().transpose();
-    CLOG(WARNING, "lidar.odometry_icp") << "w_m_r_in_r is: " << qdata.w_m_r_in_r_odo->transpose();
-#endif
+//#if 1
+//    CLOG(WARNING, "lidar.odometry_icp") << "T_m_r is: " << qdata.T_r_m_odo->inverse().vec().transpose();
+//    CLOG(WARNING, "lidar.odometry_icp") << "w_m_r_in_r is: " << qdata.w_m_r_in_r_odo->transpose();
+//#endif
     //
     /// \todo double check validity when no vertex has been created
     *qdata.T_r_v_odo = T_r_m_icp * sliding_map_odo.T_vertex_this().inverse();