diff --git a/config/honeycomb_grizzly_default.yaml b/config/honeycomb_grizzly_default.yaml index c4fe2a185..ed3b83e38 100644 --- a/config/honeycomb_grizzly_default.yaml +++ b/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 diff --git a/main/src/vtr_gui/vtr_gui/vtr-gui/src/components/graph/GraphMap.js b/main/src/vtr_gui/vtr_gui/vtr-gui/src/components/graph/GraphMap.js index c4657a51d..fd4c1754e 100644 --- a/main/src/vtr_gui/vtr_gui/vtr-gui/src/components/graph/GraphMap.js +++ b/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) */} TODO We should make this set dynamically from the yaml config*/ zoom={18} zoomControl={false} whenCreated={this.mapCreatedCallback.bind(this)} diff --git a/main/src/vtr_lidar/CMakeLists.txt b/main/src/vtr_lidar/CMakeLists.txt index 806239eeb..befb57ad1 100644 --- a/main/src/vtr_lidar/CMakeLists.txt +++ b/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 diff --git a/main/src/vtr_lidar/include/vtr_lidar/data_types/costmap.hpp b/main/src/vtr_lidar/include/vtr_lidar/data_types/costmap.hpp index c934db4bf..ce7d2cd48 100644 --- a/main/src/vtr_lidar/include/vtr_lidar/data_types/costmap.hpp +++ b/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& values); + // Modification by Jordy, DEBUG + /** \brief update from a standard unordered_map */ + void update(const std::unordered_map, float> values); + XY2ValueMap filter(const float& threshold) const override; /** \brief Returns content of this class as a OccupancyGrid message. */ diff --git a/main/src/vtr_lidar/include/vtr_lidar/modules/planning/change_detection_module_v3.hpp b/main/src/vtr_lidar/include/vtr_lidar/modules/planning/change_detection_module_v3.hpp index 5a37835b3..9bf19e8fd 100644 --- a/main/src/vtr_lidar/include/vtr_lidar/modules/planning/change_detection_module_v3.hpp +++ b/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::SharedPtr scan_pub_; rclcpp::Publisher::SharedPtr costmap_pub_; + rclcpp::Publisher::SharedPtr filtered_costmap_pub_ ; rclcpp::Publisher::SharedPtr costpcd_pub_; VTR_REGISTER_MODULE_DEC_TYPE(ChangeDetectionModuleV3); + + + // Modificatons for Temporal costmap filter + std::vector, float>> costmap_history; }; } // namespace lidar diff --git a/main/src/vtr_lidar/include/vtr_lidar/path_planning/mpc.hpp b/main/src/vtr_lidar/include/vtr_lidar/path_planning/mpc.hpp index 4ec60ce95..b6faa57cf 100644 --- a/main/src/vtr_lidar/include/vtr_lidar/path_planning/mpc.hpp +++ b/main/src/vtr_lidar/include/vtr_lidar/path_planning/mpc.hpp @@ -43,4 +43,7 @@ struct mpc_result SolveMPC(Eigen::Matrix previous_vel, lgmath::se3 struct meas_result GenerateReferenceMeas(std::shared_ptr> cbit_path_ptr, std::tuple robot_pose, int K, double DT, double VF); // Helper function for post-processing and saturating the velocity command -Eigen::Matrix SaturateVel(Eigen::Matrix applied_vel, double v_lim, double w_lim); \ No newline at end of file +Eigen::Matrix SaturateVel(Eigen::Matrix 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 cbit_p, std::vector cbit_path); \ No newline at end of file diff --git a/main/src/vtr_lidar/src/data_types/costmap.cpp b/main/src/vtr_lidar/src/data_types/costmap.cpp index 2060a4046..d0cdc2c00 100644 --- a/main/src/vtr_lidar/src/data_types/costmap.cpp +++ b/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, 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()); diff --git a/main/src/vtr_lidar/src/modules/odometry/odometry_icp_module.cpp b/main/src/vtr_lidar/src/modules/odometry/odometry_icp_module.cpp index d0cbc5977..44a18131b 100644 --- a/main/src/vtr_lidar/src/modules/odometry/odometry_icp_module.cpp +++ b/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(); diff --git a/main/src/vtr_lidar/src/modules/planning/change_detection_module_v3.cpp b/main/src/vtr_lidar/src/modules/planning/change_detection_module_v3.cpp index 4e317e96a..c5c2f3cf8 100644 --- a/main/src/vtr_lidar/src/modules/planning/change_detection_module_v3.cpp +++ b/main/src/vtr_lidar/src/modules/planning/change_detection_module_v3.cpp @@ -112,6 +112,7 @@ auto ChangeDetectionModuleV3::Config::fromROS( config->support_variance = node->declare_parameter(param_prefix + ".support_variance", config->support_variance); config->support_threshold = node->declare_parameter(param_prefix + ".support_threshold", config->support_threshold); // cost map + config->costmap_history_size = node->declare_parameter(param_prefix + ".costmap_history_size", config->costmap_history_size); config->resolution = node->declare_parameter(param_prefix + ".resolution", config->resolution); config->size_x = node->declare_parameter(param_prefix + ".size_x", config->size_x); config->size_y = node->declare_parameter(param_prefix + ".size_y", config->size_y); @@ -134,6 +135,7 @@ void ChangeDetectionModuleV3::run_(QueryCache &qdata0, OutputCache &output0, // clang-format off scan_pub_ = qdata.node->create_publisher("change_detection_scan", 5); costmap_pub_ = qdata.node->create_publisher("change_detection_costmap", 5); + filtered_costmap_pub_ = qdata.node->create_publisher("filtered_change_detection_costmap", 5); costpcd_pub_ = qdata.node->create_publisher("change_detection_costpcd", 5); // clang-format on publisher_initialized_ = true; @@ -306,6 +308,221 @@ void ChangeDetectionModuleV3::run_(QueryCache &qdata0, OutputCache &output0, costmap->vertex_id() = vid_loc; costmap->vertex_sid() = sid_loc; + + + + + // Jordy Modifications for temporal costmap filtering (UNDER DEVELOPMENT) + + + + // declaration of the final costmap which we are outputting + auto dense_costmap = std::make_shared(config_->resolution, config_->size_x, config_->size_y); + dense_costmap->T_vertex_this() = tactic::EdgeTransform(true); + dense_costmap->vertex_id() = vid_loc; + dense_costmap->vertex_sid() = sid_loc; + + // Create a sparse costmap and store it in a sliding window history + const auto sparse_costmap = std::make_shared(config_->resolution, config_->size_x, config_->size_y); + sparse_costmap->update(detect_change_op); + // add transform to the localization vertex + sparse_costmap->T_vertex_this() = tactic::EdgeTransform(true); + sparse_costmap->vertex_id() = vid_loc; + sparse_costmap->vertex_sid() = sid_loc; + + // Get the localization chain transform (lets us transform from costmap frame to world frame): + auto& chain = *output.chain; + auto T_w_c = chain.pose(sid_loc); + auto T_c_w = T_w_c.inverse(); + //CLOG(WARNING, "obstacle_detection.cbit") << "T_w_c: " << T_w_c; // debug + + // Initialize an unordered map to store the sparse world obstacle representations + std::unordered_map, float> sparse_world_map; + + // Filter non-obstacles + vtr::lidar::BaseCostMap::XY2ValueMap sparse_obs_map = sparse_costmap->filter(0.01); + + // Iterate through the key value pairs, convert to a world frame unordered_map + std::vector> keys; + keys.reserve(sparse_obs_map.size()); + std::vector vals; + vals.reserve(sparse_obs_map.size()); + for(auto kv : sparse_obs_map) + { + float key_x = kv.first.first; + float key_y = kv.first.second; + // Convert the centre of each obstacle key value pair into the world frame + Eigen::Matrix grid_pt({key_x+(config_->resolution/2), key_y+(config_->resolution/2), 0.0, 1}); + auto collision_pt = T_w_c * grid_pt; + + //CLOG(DEBUG, "obstacle_detection.cbit") << "Displaying Transform: " << (sparse_costmap->T_vertex_this().inverse()); + + float world_key_x = floor(collision_pt[0] / config_->resolution) * config_->resolution; + float world_key_y = floor(collision_pt[1] / config_->resolution) * config_->resolution; + + // Experimental debug! Im thinking that perhaps here these keys could be outside the legal area + + + + float world_value = kv.second; + std::pair world_keys(world_key_x, world_key_y); + sparse_world_map.insert(std::make_pair(world_keys,world_value)); + + //CLOG(DEBUG, "obstacle_detection.cbit") << "Displaying Key X: " << key_x; + //CLOG(DEBUG, "obstacle_detection.cbit") << "Displaying Key Y: " << key_y; + //CLOG(DEBUG, "obstacle_detection.cbit") << "Displaying World Key X: " << world_key_x; + //CLOG(DEBUG, "obstacle_detection.cbit") << "Displaying World Key Y: " << world_key_y; + + + keys.push_back(kv.first); + vals.push_back(kv.second); + } + //CLOG(ERROR, "obstacle_detection.cbit") << "The size of each map is" << sparse_obs_map.size(); + //CLOG(ERROR, "obstacle_detection.cbit") << "Displaying all Keys: " << keys; + //CLOG(ERROR, "obstacle_detection.cbit") << "Displaying all Values: " << vals; + + //CLOG(WARNING, "obstacle_detection.cbit") << "T_c_w " << T_c_w; + //CLOG(WARNING, "obstacle_detection.cbit") << "T_m_s: " << T_m_s; + //CLOG(WARNING, "obstacle_detection.cbit") << "T_s_r_inv: " << T_s_r.inverse(); + + + + // if the costmap_history is smaller then some minimum value, just tack it on the end + // TODO need to get rid of these magic numbers + if (costmap_history.size() < config_->costmap_history_size) + { + costmap_history.push_back(sparse_world_map); + } + // After that point, we then do a sliding window using shift operations, moving out the oldest map and appending the newest one + else + { + for (int i = 0; i < (config_->costmap_history_size-1); i++) + { + costmap_history[i] = costmap_history[i + 1]; + } + costmap_history[(config_->costmap_history_size-1)] = sparse_world_map; + + + //CLOG(WARNING, "obstacle_detection.cbit") << "costmap_history size " <, float> merged_world_map = costmap_history[(config_->costmap_history_size-1)]; + //CLOG(WARNING, "obstacle_detection.cbit") << "merged world map size " <, float> filtered_world_map; + for (int i = 0; i < (costmap_history.size()-1); i++) + { + merged_world_map.merge(costmap_history[i]); + //CLOG(WARNING, "obstacle_detection.cbit") << "merged world map size " <> world_keys; + //world_keys.reserve(merged_world_map.size()); + //std::vector world_vals; + //world_vals.reserve(merged_world_map.size()); + for(auto kv : merged_world_map) + { + //world_keys.push_back(kv.first); + //world_vals.push_back(kv.second); + + int key_vote_counter = 0; + for (int i = 0; i < costmap_history.size(); i++) + { + if ((costmap_history[i]).find(kv.first) != (costmap_history[i]).end()) + { + key_vote_counter = key_vote_counter + 1; + } + } + + // Store the filtered (both temporal and transient) points in a sparse filtered unordered_map + if (key_vote_counter >= 3) + { + filtered_world_map.insert(std::make_pair(kv.first,kv.second)); + } + } + + // Convert the filtered world map back to the current costmap frame (note, might actually just store the sparse world map, its more useful for me anyways) + // Probably still want to convert back so we can republish it though + // Iterate through the key value pairs, convert to a world frame unordered_map + std::unordered_map, float> filtered_loc_map; + std::vector> filtered_keys; + filtered_keys.reserve(filtered_world_map.size()); + std::vector filtered_vals; + filtered_vals.reserve(filtered_world_map.size()); + + for(auto kv : filtered_world_map) + { + float key_x = kv.first.first; + float key_y = kv.first.second; + // Convert the centre of each obstacle key value pair into the world frame + Eigen::Matrix grid_pt({key_x+(config_->resolution/2), key_y+(config_->resolution/2), 0.0, 1}); + auto collision_pt = T_c_w * grid_pt; + + //CLOG(DEBUG, "obstacle_detection.cbit") << "Displaying Transform: " << (sparse_costmap->T_vertex_this().inverse()); + + float loc_key_x = floor(collision_pt[0] / config_->resolution) * config_->resolution; + float loc_key_y = floor(collision_pt[1] / config_->resolution) * config_->resolution; + + + // debug to check loc_key is in a valid range: + + + float loc_value = kv.second; + std::pair loc_keys(loc_key_x, loc_key_y); + filtered_loc_map.insert(std::make_pair(loc_keys,loc_value)); + } + + // Build the dense map, publish and save the results so the planner can access it + //const auto dense_costmap = std::make_shared(config_->resolution, config_->size_x, config_->size_y); // need to delcare this earlier + + dense_costmap->update(filtered_loc_map); + + + // add transform to the localization vertex + //dense_costmap->T_vertex_this() = tactic::EdgeTransform(true); + //dense_costmap->vertex_id() = vid_loc; + //dense_costmap->vertex_sid() = sid_loc; + + // debug, temporarily using my filtered costmap as the main costmap + //auto costmap_msg = dense_costmap->toCostMapMsg(); + //costmap_msg.header.frame_id = "loc vertex frame"; + // costmap_msg.header.stamp = rclcpp::Time(*qdata.stamp); + //costmap_pub_->publish(costmap_msg); + + //CLOG(WARNING, "obstacle_detection.cbit") << "Successfully update the dense costmap"; + + + // publish the filtered occupancy grid + auto filtered_costmap_msg = dense_costmap->toCostMapMsg(); + filtered_costmap_msg.header.frame_id = "loc vertex frame"; + // costmap_msg.header.stamp = rclcpp::Time(*qdata.stamp); + filtered_costmap_pub_->publish(filtered_costmap_msg); + + // Debug check that the filtered maps look okay + vtr::lidar::BaseCostMap::XY2ValueMap dense_map = dense_costmap->filter(0.01); + std::vector> keys2; + keys2.reserve(dense_map.size()); + std::vector vals2; + vals2.reserve(dense_map.size()); + for(auto kv : dense_map) { + keys2.push_back(kv.first); + vals2.push_back(kv.second); + } + //CLOG(ERROR, "obstacle_detection.cbit") << "The size of the dense map is" << dense_map.size(); + //CLOG(ERROR, "obstacle_detection.cbit") << "Displaying all Keys: " << keys2; + //CLOG(ERROR, "obstacle_detection.cbit") << "Displaying all Values: " << vals2; + } + + + + + + // End of Jordy's temporal filter changes + + + + + /// publish the transformed pointcloud if (config_->visualize) { // publish the aligned points @@ -331,10 +548,21 @@ void ChangeDetectionModuleV3::run_(QueryCache &qdata0, OutputCache &output0, /// output auto change_detection_costmap_ref = output.change_detection_costmap.locked(); auto &change_detection_costmap = change_detection_costmap_ref.get(); - change_detection_costmap = costmap; + //change_detection_costmap = costmap; + + //Jordy debug, using experimental new costmaps instead + if (costmap_history.size() < 10.0) + { + change_detection_costmap = costmap; + } + else + { + change_detection_costmap = dense_costmap; + } CLOG(INFO, "lidar.change_detection") << "Change detection for lidar scan at stamp: " << stamp << " - DONE"; + } } // namespace lidar diff --git a/main/src/vtr_lidar/src/path_planning/cbit.cpp b/main/src/vtr_lidar/src/path_planning/cbit.cpp index c35aa9e64..424abf0ae 100644 --- a/main/src/vtr_lidar/src/path_planning/cbit.cpp +++ b/main/src/vtr_lidar/src/path_planning/cbit.cpp @@ -178,6 +178,7 @@ auto LidarCBIT::computeCommand(RobotState& robot_state0) -> Command { // Store the transform T_c_w (from costmap to world) costmap_ptr->T_c_w = T_start_vertex.inverse(); // note that T_start_vertex is T_w_c if we want to bring keypoints to the world frame // Store the grid resoltuion + CLOG(DEBUG, "obstacle_detection.cbit") << "The costmap to world transform is: " << T_start_vertex.inverse(); costmap_ptr->grid_resolution = change_detection_costmap->dl(); // Experimental: Storing sequences of costmaps for temporal filtering purposes @@ -190,11 +191,12 @@ auto LidarCBIT::computeCommand(RobotState& robot_state0) -> Command { // After that point, we then do a sliding window using shift operations, moving out the oldest map and appending the newest one else { - for (int i = 0; i < (config_->costmap_history-1); i++) - { - costmap_ptr->obs_map_vect[i] = costmap_ptr->obs_map_vect[i + 1]; - costmap_ptr->T_c_w_vect[i] = costmap_ptr->T_c_w_vect[i + 1]; - } + // Legacy code for when I was filtering inside cbit (made a quick and dirty work around temporarily) + //for (int i = 0; i < (config_->costmap_history-1); i++) + //{ + // costmap_ptr->obs_map_vect[i] = costmap_ptr->obs_map_vect[i + 1]; + // costmap_ptr->T_c_w_vect[i] = costmap_ptr->T_c_w_vect[i + 1]; + //} costmap_ptr->obs_map_vect[config_->costmap_history-1] = obs_map; costmap_ptr->T_c_w_vect[config_->costmap_history-1] = costmap_ptr->T_c_w ; } @@ -260,13 +262,13 @@ auto LidarCBIT::computeCommand(RobotState& robot_state0) -> Command { kin_noise_vect = config_->kin_error_cov; - + // Extrapolating robot pose into the future by using the history of applied mpc velocity commands const auto curr_time = now(); // always in nanoseconds const auto dt = static_cast(curr_time - stamp) * 1e-9; - CLOG(INFO, "mpc_debug.cbit") << "History of the Robot Velocities" << vel_history; + CLOG(INFO, "mpc_debug.cbit") << "History of the Robot Velocities:" << vel_history; // Check the time past since the last state update was received // Go back through the vel_history to approximately dt seconds in the past @@ -275,18 +277,18 @@ auto LidarCBIT::computeCommand(RobotState& robot_state0) -> Command { auto T_p_r2 = T_p_r; for (int i=std::floor(dt / control_period); i > 0; i--) { - //CLOG(DEBUG, "mpc_debug.cbit") << "The iteration Index i is: " << i; + CLOG(DEBUG, "mpc_debug.cbit") << "The iteration Index i is: " << i; w_p_r_in_r(0) = -1* vel_history[vel_history.size()-(i+1)][0]; w_p_r_in_r(1) = 0.0; w_p_r_in_r(2) = 0.0; w_p_r_in_r(3) = 0.0; w_p_r_in_r(4) = 0.0; w_p_r_in_r(5) = -1* vel_history[vel_history.size()-(i+1)][1]; - //CLOG(DEBUG, "mpc_debug.cbit") << "Robot velocity Used for Extrapolation: " << -w_p_r_in_r.transpose() << std::endl; + CLOG(DEBUG, "mpc_debug.cbit") << "Robot velocity Used for Extrapolation: " << -w_p_r_in_r.transpose() << std::endl; Eigen::Matrix xi_p_r_in_r(control_period * w_p_r_in_r); T_p_r2 = T_p_r2 * tactic::EdgeTransform(xi_p_r_in_r).inverse(); - //CLOG(DEBUG, "mpc_debug.cbit") << "Make sure the lie algebra is changing right:" << T_p_r2; + CLOG(DEBUG, "mpc_debug.cbit") << "Make sure the lie algebra is changing right:" << T_p_r2; } // Apply the final partial period velocity @@ -303,7 +305,7 @@ auto LidarCBIT::computeCommand(RobotState& robot_state0) -> Command { const auto T_p_r_extp2 = T_p_r2; CLOG(DEBUG, "mpc_debug.cbit") << "New extrapolated pose:" << T_p_r_extp2; - + // Uncomment if we use the extrapolated robot pose for control (constant velocity model from odometry) //lgmath::se3::Transformation T0 = lgmath::se3::Transformation(T_w_p * T_p_r_extp); @@ -313,6 +315,8 @@ auto LidarCBIT::computeCommand(RobotState& robot_state0) -> Command { // no extrapolation (comment this out if we are not using extrapolation) //lgmath::se3::Transformation T0 = lgmath::se3::Transformation(T_w_p * T_p_r); + // TODO: Set whether to use mpc extrapolation as a config param (though there is almost never a good reason not to use it) + //Convert to x,y,z,roll, pitch, yaw std::tuple robot_pose = T2xyzrpy(T0); CLOG(DEBUG, "mpc_debug.cbit") << "The Current Robot Pose (from planning) is - x: " << std::get<0>(robot_pose) << " y: " << std::get<1>(robot_pose) << " yaw: " << std::get<5>(robot_pose); @@ -331,13 +335,22 @@ auto LidarCBIT::computeCommand(RobotState& robot_state0) -> Command { bool point_stabilization = meas_result.point_stabilization; - // Create and solve the STEAM optimization problem - CLOG(INFO, "mpc.cbit") << "Attempting to solve the MPC problem"; - auto mpc_result = SolveMPC(applied_vel, T0, measurements, K, DT, VF, pose_noise_vect, vel_noise_vect, accel_noise_vect, kin_noise_vect, point_stabilization); - applied_vel = mpc_result.applied_vel; // note dont re-declare applied vel here - auto mpc_poses = mpc_result.mpc_poses; - CLOG(INFO, "mpc.cbit") << "Successfully solved MPC problem"; + std::vector mpc_poses; + try + { + CLOG(INFO, "mpc.cbit") << "Attempting to solve the MPC problem"; + auto mpc_result = SolveMPC(applied_vel, T0, measurements, K, DT, VF, pose_noise_vect, vel_noise_vect, accel_noise_vect, kin_noise_vect, point_stabilization); + applied_vel = mpc_result.applied_vel; // note dont re-declare applied vel here + mpc_poses = mpc_result.mpc_poses; + CLOG(INFO, "mpc.cbit") << "Successfully solved MPC problem"; + } + catch(...) + { + CLOG(ERROR, "mpc.cbit") << "STEAM Optimization Failed; Commanding to Stop the Vehicle"; + applied_vel(0) = 0.0; + applied_vel(1) = 0.0; + } CLOG(INFO, "mpc.cbit") << "The linear velocity is: " << applied_vel(0) << " The angular vel is: " << applied_vel(1); @@ -351,7 +364,7 @@ auto LidarCBIT::computeCommand(RobotState& robot_state0) -> Command { vel_history.push_back(saturated_vel); // Store the current robot state in the robot state path so it can be visualized - robot_poses.push_back(T0); + robot_poses.push_back(T_w_p * T_p_r); // Send the robot poses and mpc prediction to rviz visualize(stamp, T_w_p, T_p_r, T_p_r_extp, T_p_r_extp2, mpc_poses, robot_poses); @@ -368,13 +381,11 @@ auto LidarCBIT::computeCommand(RobotState& robot_state0) -> Command { << command.angular.z << "]"; return command; - - } // If localized and an initial solution is found. - + } // Otherwise stop the robot else { - CLOG(INFO, "mpc.cbit") << "There is not a valid plan yet, returning 0.0 velocity command"; + CLOG(INFO, "mpc.cbit") << "There is not a valid plan yet, returning zero velocity commands"; applied_vel << 0.0, 0.0; vel_history.erase(vel_history.begin()); @@ -383,5 +394,7 @@ auto LidarCBIT::computeCommand(RobotState& robot_state0) -> Command { } } + + } // namespace lidar } // namespace vtr \ No newline at end of file diff --git a/main/src/vtr_lidar/src/path_planning/mpc.cpp b/main/src/vtr_lidar/src/path_planning/mpc.cpp index 15edacfe3..18484f270 100644 --- a/main/src/vtr_lidar/src/path_planning/mpc.cpp +++ b/main/src/vtr_lidar/src/path_planning/mpc.cpp @@ -45,6 +45,15 @@ struct mpc_result SolveMPC(Eigen::Matrix previous_vel, lgmath::se3 0, 0, 0, -1; + // Lateral constraint projection matrices (Experimental) + Eigen::Matrix I_4; + I_4 << 0, + 0, + 0, + 1; + Eigen::Matrix I_2_tran; + I_2_tran << 0, 1, 0, 0; + // Setup shared loss functions and noise models for all cost terms const auto sharedLossFunc = steam::L2LossFunc::MakeShared(); @@ -61,6 +70,12 @@ struct mpc_result SolveMPC(Eigen::Matrix previous_vel, lgmath::se3 const auto sharedKinNoiseModel = steam::StaticNoiseModel<6>::MakeShared(kin_noise_vect); + // Experimental lat constraint (hardcoded for now) + Eigen::Matrix lat_noise_vect; + lat_noise_vect << 1.0; + const auto sharedLatNoiseModel = + steam::StaticNoiseModel<1>::MakeShared(lat_noise_vect); + // Generate STEAM States for the velocity vector and SE3 state transforms @@ -74,12 +89,13 @@ struct mpc_result SolveMPC(Eigen::Matrix previous_vel, lgmath::se3 // Pushback the initial states (current robot state) pose_states.push_back(T0_inv); // Change this to T0 when implementing on robot, T0_1 for debug //vel_states.push_back(std::vector {0.0, 0.0}); //I think a single line way t odo this is something like Eigen::Matrix::Zero() - vel_states.push_back(v0); + vel_states.push_back(v0); // Set the remaining states for (int i=0; i previous_vel, lgmath::se3 for (int i=0; i previous_vel, lgmath::se3 //const auto vel_cost_term = steam::WeightedLeastSqCostTerm<2>::MakeShared(vel_state_vars[i], sharedVelNoiseModel, sharedLossFunc); //opt_problem.addCostTerm(vel_cost_term); - // Experimental velocity set-point constraint (instead of non zero velocity penalty) - // Only add this cost term if we are not in point stabilization mode (end of path) - if (point_stabilization == false) + + + + // Kinematic constraints (softened but penalized heavily) + if (i < (K-1)) { - const auto vel_cost_term = steam::WeightedLeastSqCostTerm<2>::MakeShared(steam::vspace::VSpaceErrorEvaluator<2>::MakeShared(vel_state_vars[i],v_ref), sharedVelNoiseModel, sharedLossFunc); - opt_problem.addCostTerm(vel_cost_term); - } + const auto lhs = steam::se3::ComposeInverseEvaluator::MakeShared(pose_state_vars[i+1], pose_state_vars[i]); + const auto vel_proj = steam::vspace::MatrixMultEvaluator<6,2>::MakeShared(vel_state_vars[i], P_tran); // TODO, I guess this version of steam doesnt have this one, will need to do it myself + const auto scaled_vel_proj = steam::vspace::ScalarMultEvaluator<6>::MakeShared(vel_proj, DT); + const auto rhs = steam::se3::ExpMapEvaluator::MakeShared(scaled_vel_proj); + const auto kin_error_func = steam::se3::LogMapEvaluator::MakeShared(steam::se3::ComposeInverseEvaluator::MakeShared(lhs, rhs)); + const auto kin_cost_term = steam::WeightedLeastSqCostTerm<6>::MakeShared(kin_error_func, sharedKinNoiseModel, sharedLossFunc); + opt_problem.addCostTerm(kin_cost_term); + + + // Experimental velocity set-point constraint (instead of non zero velocity penalty) + // Only add this cost term if we are not in point stabilization mode (end of path) + //if (point_stabilization == false) + //{ + // const auto vel_cost_term = steam::WeightedLeastSqCostTerm<2>::MakeShared(steam::vspace::VSpaceErrorEvaluator<2>::MakeShared(vel_state_vars[i],v_ref), sharedVelNoiseModel, sharedLossFunc); + // opt_problem.addCostTerm(vel_cost_term); + //} // Experimental acceleration limits + if (i == 0) { // On the first iteration, we need to use an error with the previously applied control command state @@ -139,27 +177,29 @@ struct mpc_result SolveMPC(Eigen::Matrix previous_vel, lgmath::se3 const auto accel_diff = steam::vspace::AdditionEvaluator<2>::MakeShared(vel_state_vars[i], steam::vspace::NegationEvaluator<2>::MakeShared(vel_state_vars[i-1])); const auto accel_cost_term = steam::WeightedLeastSqCostTerm<2>::MakeShared(accel_diff, sharedAccelNoiseModel, sharedLossFunc); opt_problem.addCostTerm(accel_cost_term); - } + } + + } - // Kinematic constraints (softened but penalized heavily) - if (i < (K-1)) - { - const auto lhs = steam::se3::ComposeInverseEvaluator::MakeShared(pose_state_vars[i+1], pose_state_vars[i]); - const auto vel_proj = steam::vspace::MatrixMultEvaluator<6,2>::MakeShared(vel_state_vars[i], P_tran); // TODO, I guess this version of steam doesnt have this one, will need to do it myself - const auto scaled_vel_proj = steam::vspace::ScalarMultEvaluator<6>::MakeShared(vel_proj, DT); - const auto rhs = steam::se3::ExpMapEvaluator::MakeShared(scaled_vel_proj); - const auto kin_error_func = steam::se3::LogMapEvaluator::MakeShared(steam::se3::ComposeInverseEvaluator::MakeShared(lhs, rhs)); - const auto kin_cost_term = steam::WeightedLeastSqCostTerm<6>::MakeShared(kin_error_func, sharedKinNoiseModel, sharedLossFunc); - opt_problem.addCostTerm(kin_cost_term); - } + // Experimental Hard Lateral Constraints + /* + CLOG(DEBUG, "debug") << "Attempting to add a lateral cost term"; + const auto inv_state = steam::se3::InverseEvaluator::MakeShared(pose_state_vars[i]); + const auto pose_err = steam::vspace::MatrixMultEvaluator<4,4>::MakeShared(inv_state, measurements[i]); // I think this line is the problem, we cannot really convert the se3 evaluators to vspace evaluators + const auto left_mul = steam::vspace::MatrixMultEvaluator<1,4>::MakeShared(pose_err, I_2_tran); + const auto right_mul = steam::vspace::MatrixMultEvaluator<4,1>::MakeShared(pose_err, I_4); + const auto lat_cost_term = steam::WeightedLeastSqCostTerm<6>::MakeShared(right_mul, sharedLatNoiseModel, sharedLossFunc); + opt_problem.addCostTerm(lat_cost_term); + CLOG(DEBUG, "debug") << "Successfully added Lat cost term"; + */ } // Solve the optimization problem with GuassNewton solver using SolverType = steam::GaussNewtonSolver; // Initialize parameters (enable verbose mode) SolverType::Params params; - params.verbose = false; // Makes the output display for debug + params.verbose = false; // Makes the output display for debug when true SolverType solver(opt_problem, params); solver.optimize(); @@ -243,152 +283,139 @@ struct meas_result GenerateReferenceMeas(std::shared_ptr> cbit // Save a copy of the current path solution to work on auto cbit_path = *cbit_path_ptr; + // Experimental Changes to improve controller instability (completed but not rigourously field tested yet) - // Take in the current euclidean path solution from the cbit planner in the world frame, the current robot state, and determine - // which measurements we wish to track to follow the path at the desired target velocity - CLOG(DEBUG, "mpc_debug.cbit") << "Reference Path Points:"; - std::vector measurements; - double starting_dist = INFINITY; + // PSEUDO CODE: + // 1. Find the closest point on the cbit path to the current state of the robot + // 2. Using K, DT, VF, we generate a vector of "p" values that we want to create Euclidean measurements for (we know these up front) + // 3. After we have our starting closest point on the path, assign that point a p value of 0. Compute the p values for each subsequent point in the lookahead window + // 4. using the desired p values, and the known p values, interpolate a, x,y,z,yaw, value each measurement + // 5. Create the proper measurement transform for each measurement and get it ready for the using with the optimization problem + + // Limiting the size of the cbit path based on the sliding window and then assigning p values + double lookahead_dist = 0.0; + double p_dist = 0.0; + + double min_dist = INFINITY; double new_dist; double dx; double dy; - double delta_dist = 0; - double index_counter = 0; - - // Find closest point on the path to the current state (new version, experimental) - double min_dist = INFINITY; - CLOG(DEBUG, "mpc_debug.cbit") << "Total CBIT Path Size: " << cbit_path.size(); - for (int i = 0; i < 200; i++) // magic number for now, corresponds to about a 5m lookahead (need to remove dependency on this) - { - // handle end of path case: - if (i >= cbit_path.size()) - { - break; - } - CLOG(DEBUG, "mpc_debug.cbit") << "x: " << (cbit_path)[i].x << "y: " << (cbit_path)[i].y << "z: " << (cbit_path)[i].z; + double p_correction = 0.0; + + std::vector cbit_p; + cbit_p.reserve(cbit_path.size()); + cbit_p.push_back(0.0); + for (int i = 0; i < (cbit_path.size()-2); i++) // the last value of vector is size()-1, so second to last will be size-2 + { + // calculate the p value for the point + p_dist = sqrt((((cbit_path)[i].x - (cbit_path)[i+1].x) * ((cbit_path)[i].x - (cbit_path)[i+1].x)) + (((cbit_path)[i].y - (cbit_path)[i+1].y) * ((cbit_path)[i].y - (cbit_path)[i+1].y))); + lookahead_dist = lookahead_dist + p_dist; + cbit_p.push_back(lookahead_dist); + + // Keep track of the closest point to the robot state dx = (cbit_path)[i].x - std::get<0>(robot_pose); dy = (cbit_path)[i].y - std::get<1>(robot_pose); new_dist = sqrt((dx * dx) + (dy * dy)); - CLOG(DEBUG, "mpc_debug.cbit") << "Dist to Pt: " << new_dist; if (new_dist < min_dist) { CLOG(DEBUG, "mpc_debug.cbit") << "Minimum Distance: " << new_dist; - index_counter = i; // I wonder if we should add some buffer, as this point could still be behind the current robot + p_correction = lookahead_dist; min_dist = new_dist; } - } - /* - // Find closest point on the path to the current state (Old version, mostly worked, but I think if the cbit path was not smooth there was a chance it would exit early before finding closest point) - while (delta_dist >= 0) - { - CLOG(DEBUG, "mpc_debug.cbit") << "x: " << (cbit_path)[index_counter].x << "y: " << (cbit_path)[index_counter].y << "z: " << (cbit_path)[index_counter].z; - dx = (cbit_path)[index_counter].x - std::get<0>(robot_pose); - dy = (cbit_path)[index_counter].y - std::get<1>(robot_pose); - new_dist = sqrt((dx * dx) + (dy * dy)); - delta_dist = starting_dist - new_dist; - CLOG(DEBUG, "mpc_debug.cbit") << "Dist to Pt: " << new_dist; - CLOG(DEBUG, "mpc_debug.cbit") << "Delta Dist: " << delta_dist; - if (delta_dist >= 0) - { - starting_dist = new_dist; - index_counter = index_counter + 1; - } - else + + // Stop once we get about 12m ahead of the robot (magic number for now, but this is just a conservative estimate of any reasonable lookahead window and mpc horizon) + if (lookahead_dist > 12.0) { - CLOG(DEBUG, "mpc_debug.cbit") << "Delta Dist Negative, Return i = " << index_counter-1; - index_counter = index_counter - 1; + break; } } - */ + //CLOG(DEBUG, "debug") << "cbit_p is: " << cbit_p; - // Keep iterating through the rest of the path, storing points in the path as measurements if they maintain an approximate - // forward path velocity of VF (//TODO, may need to also interpolate in some instances if we want to be very particular) - for (int i = index_counter; i < (cbit_path).size(); i++) + // Determine the p_values we need for our measurement horizon, corrected for the p value of the closest point on the path to the current robot state + std::vector p_meas_vec; + std::vector measurements; + p_meas_vec.reserve(K); + for (int i = 0; i < K; i++) { - // The first iteration we need to add the closest point to the initial position as a measurement - // Subesequent iterations we want to select points on the path to track carefully based on the desired velocity we want to meet. - - // Reset the measurement distance - double delta_dist = 0.0; - if (index_counter != i) - { - // scan the path into the future until we proceed approximately VF*DT meters forward longitudinally long the path - // The resulting indice of the path will be the one we use for the next measurement - while ((delta_dist <= (VF*DT)) && (i < (cbit_path).size())) // Need to add this and condition to handle end of path case - { - - double prev_x = (cbit_path)[i-1].x; - double prev_y = (cbit_path)[i-1].y; - double next_x = (cbit_path)[i].x; - double next_y = (cbit_path)[i].y; - delta_dist = delta_dist + sqrt(((next_x-prev_x) * (next_x-prev_x)) + ((next_y-prev_y) * (next_y-prev_y))); - i = i + 1; - } - - // With the above setup, pretty sure the resulting i will be 1 too far when we break the loop, so we need to decrement it once at the end - i = i-1; - } + p_meas_vec.push_back((i * DT * VF) + p_correction); + } + //CLOG(DEBUG, "debug") << "p_meas_vec is: " << p_meas_vec; + // todo: Iterate through the p_measurements and interpolate euclidean measurements from the cbit_path and the corresponding cbit_p values + // Note this could be combined in the previous loop too + bool point_stabilization = false; + for (int i = 0; i < p_meas_vec.size(); i++) + { + // handle end of path case: + // if the p meas we would have needed exceeds the final measurement pose, set it equal to our last p value in the path + // This will cause the intepolation to return the final cbit_path pose for all measurements past this point + //CLOG(INFO, "debug") << "The specific p_meas_vec[i] is: " << p_meas_vec[i]; + //CLOG(INFO, "debug") << "The size of cbit_p is:" << cbit_p.size(); + //CLOG(INFO, "debug") << "The final cbit_p value is:" << cbit_p[cbit_p.size()-1]; + if (p_meas_vec[i] > cbit_p[cbit_p.size()-1]) + { + p_meas_vec[i] = cbit_p[cbit_p.size()-1]; + point_stabilization = true; // Enable point stabilization configs + CLOG(INFO, "mpc.cbit") << "Approaching End of Path, Converting MPC to Point Stabilization Problem"; + } + lgmath::se3::Transformation meas = InterpolateMeas(p_meas_vec[i], cbit_p, cbit_path); - // Derive a yaw direction for each point based on the vector formed between the current point and the previous point on the path - double yaw = std::atan2(((cbit_path)[i].y - (cbit_path)[i-1].y), ((cbit_path)[i].x - (cbit_path)[i-1].x)); - //CLOG(DEBUG, "mpc_debug.cbit") << "The yaw of the path pt is: " << yaw; + // add to measurement vector + measurements.push_back(meas); + } - // Generate a reference Transformation matrix (ignores roll/pitch) - Eigen::Matrix4d T_ref; - T_ref << std::cos(yaw),-1*std::sin(yaw),0,(cbit_path)[i].x, - std::sin(yaw), std::cos(yaw),0,(cbit_path)[i].y, - 0, 0, 1,(cbit_path)[i].z, - 0, 0, 0, 1; - T_ref = T_ref.inverse().eval(); + return {measurements, point_stabilization}; - measurements.push_back(lgmath::se3::Transformation(T_ref)); - // Early break condition when the number of measurements we need is satisfied based on the horizon - if (measurements.size() == K) - { - break; - } - // TODO handle end of path case => will want to repeat the final measurements and turn problem into a point stabilization MPC. + //End of Experimental Changes - } +} - // If we reach the end of the path without generating enough measurements (approaching end of path), populate the measurements - // with the final point in the path (point stabilization problem) - if (measurements.size() < K) +// function takes in a the cbit path solution with a vector defining hte p axis of the path, and then a desired p_meas +// Then tries to output a euclidean pose interpolated for the desired p_meas. +lgmath::se3::Transformation InterpolateMeas(double p_val, std::vector cbit_p, std::vector cbit_path) +{ + // Find the lower bound of the p values + for (int i = 0; i < cbit_p.size(); i++) + { + if (cbit_p[i] < p_val) { - CLOG(INFO, "mpc.cbit") << "Approaching End of Path, Converting MPC to Point Stabilization Problem"; - - // debug logging messages - /* - CLOG(ERROR, "mpc.cbit") << "Displaying all measurements so far:"; - CLOG(ERROR, "mpc.cbit") << "Size of cbit path is: " << cbit_path.size(); - CLOG(ERROR, "mpc.cbit") << "Size of measurements is: " << measurements.size(); - for (int k = 0; k < measurements.size(); k++) - { - CLOG(ERROR, "mpc.cbit") << "Measurement k: " << k << " is: " << measurements[k]; - } - - CLOG(ERROR, "mpc.cbit") << "The last valid transform was: " << measurements[measurements.size()-1]; - */ - for (int j = measurements.size(); j < K; j++) - { - measurements.push_back(measurements[j-1]); - //CLOG(ERROR, "mpc.cbit") << "Appending this transform to the measurements: " << measurements[j-1]; - } - - return {measurements, true}; + continue; } - else { - return {measurements, false}; - } + double p_lower = cbit_p[i-1]; + double p_upper = cbit_p[i]; + Pose pose_lower = cbit_path[i-1]; + Pose pose_upper = cbit_path[i]; + + double x_int = pose_lower.x + ((p_val - p_lower) / (p_upper - p_lower)) * (pose_upper .x - pose_lower.x); + double y_int = pose_lower.y + ((p_val - p_lower) / (p_upper - p_lower)) * (pose_upper .y - pose_lower.y); + double z_int = pose_lower.z + ((p_val - p_lower) / (p_upper - p_lower)) * (pose_upper .z - pose_lower.z); + + // For yaw we need to be abit careful about sign and angle wrap around + // Derive the yaw by creating the vector connecting the pose_upp and pose_lower pts + + double yaw_int = std::atan2((pose_upper.y - pose_lower.y), (pose_upper.x - pose_lower.x)); -} + // Build the transformation matrix + Eigen::Matrix4d T_ref; + T_ref << std::cos(yaw_int),-1*std::sin(yaw_int),0, x_int, + std::sin(yaw_int), std::cos(yaw_int),0, y_int, + 0, 0, 1, z_int, + 0, 0, 0, 1; + T_ref = T_ref.inverse().eval(); + + lgmath::se3::Transformation meas = lgmath::se3::Transformation(T_ref); + + CLOG(DEBUG, "mpc_debug.cbit") << "The measurement Euclidean state is - x: " << x_int << " y: " << y_int << " z: " << z_int << " yaw: " << yaw_int; + return meas; + } + } +} // Simple function for checking that the current output velocity command is saturated between our mechanical velocity limits @@ -410,32 +437,49 @@ Eigen::Matrix SaturateVel(Eigen::Matrix applied_vel, } */ - if (applied_vel(0) >= v_lim) + if (abs(applied_vel(0)) >= v_lim) { - command_lin_x = v_lim; - } - else if (applied_vel(0) <= 0.0) - { - command_lin_x = 0.0; + if (applied_vel(0) > 0.0) + { + command_lin_x = v_lim; + } + else if (applied_vel(0) < 0.0) + { + command_lin_x = -1.0* v_lim; + } } + // Removed for bi-directional control purposes + //else if (applied_vel(0) <= 0.0) + //{ + // command_lin_x = 0.0; + //} else { command_lin_x = applied_vel(0) ; } - if (applied_vel(1) >= w_lim) + if (abs(applied_vel(1)) >= w_lim) { - command_ang_z = w_lim; - } - else if (applied_vel(1) <= -1*w_lim) - { - command_ang_z = -1*w_lim; + if (applied_vel(1) > 0.0) + { + command_ang_z = w_lim; + } + else if (applied_vel(1) < 0.0) + { + command_ang_z = -1.0 * w_lim; + } } + //else if (applied_vel(1) <= -1*w_lim) + //{ + // command_ang_z = -1*w_lim; + //} else { command_ang_z = applied_vel(1) ; } + // Changes for Bi-directional path traversal, we no longer want to saturate commands less than 0.0 + saturated_vel << command_lin_x, command_ang_z; return saturated_vel; } diff --git a/main/src/vtr_navigation/CMakeLists.txt b/main/src/vtr_navigation/CMakeLists.txt index 60821d024..adf45363d 100644 --- a/main/src/vtr_navigation/CMakeLists.txt +++ b/main/src/vtr_navigation/CMakeLists.txt @@ -103,6 +103,34 @@ target_include_directories(${PROJECT_NAME} $ $) + +# 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} ${PYTHON_LIBRARIES}) + + +find_package(PythonLibs REQUIRED) +include_directories(${PYTHON_INCLUDE_DIRS}) +target_link_libraries(${PROJECT_NAME} ${PYTHON_LIBRARIES}) +# END OF JORDY EXPERIMENTAL MATPLOTLIBCPP Changes FOR PLOTTING + install( DIRECTORY include/ DESTINATION include diff --git a/main/src/vtr_path_planning/CMakeLists.txt b/main/src/vtr_path_planning/CMakeLists.txt index a3bad1a48..b89bdd948 100644 --- a/main/src/vtr_path_planning/CMakeLists.txt +++ b/main/src/vtr_path_planning/CMakeLists.txt @@ -51,6 +51,45 @@ ament_target_dependencies(${PROJECT_NAME}_cbit lgmath steam ) +# Hacky way of declaring the helper scripts +file(GLOB_RECURSE SRC src/cbit/generate_pq.cpp) + +file(GLOB_RECURSE SRC src/cbit/utils.cpp) + +file(GLOB_RECURSE SRC src/cbit/cbit_path_planner.cpp) + +# START OF JORDY EXPERIMENTAL MATPLOTLIB CHANGES (note also had to make similar changes in vtr_lidar and vtr_navigation cmakelist.txt also for some dumb reason) +# Declare the plotting tools +file(GLOB_RECURSE SRC src/cbit/cbit_plotting.cpp) + +# 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}_cbit ${PYTHON_LIBRARIES}) + + +find_package(PythonLibs REQUIRED) +include_directories(${PYTHON_INCLUDE_DIRS}) +target_link_libraries(${PROJECT_NAME}_cbit ${PYTHON_LIBRARIES}) +# EXPERIMENTAL, JORDY ADDITION FOR MATPLOTLIBCPP PLOTTING +#target_link_libraries($vtr_path_planning ${PYTHON_LIBRARIES} Python3::NumPy) +# END OF EXPERIMENTAL MATPLOTLIBCPP + ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) ament_export_dependencies( rclcpp tf2 tf2_ros tf2_eigen geometry_msgs diff --git a/main/src/vtr_path_planning/include/vtr_path_planning/cbit/cbit.hpp b/main/src/vtr_path_planning/include/vtr_path_planning/cbit/cbit.hpp index cce062778..d3fc61586 100644 --- a/main/src/vtr_path_planning/include/vtr_path_planning/cbit/cbit.hpp +++ b/main/src/vtr_path_planning/include/vtr_path_planning/cbit/cbit.hpp @@ -161,11 +161,16 @@ class CBIT : public BasePathPlanner { rclcpp::Publisher::SharedPtr mpc_path_pub_; rclcpp::Publisher::SharedPtr robot_path_pub_; rclcpp::Publisher::SharedPtr path_pub_; + rclcpp::Publisher::SharedPtr corridor_pub_l_; + rclcpp::Publisher::SharedPtr corridor_pub_r_; // Pointers to the output path std::vector cbit_path; std::shared_ptr> cbit_path_ptr; + // Pointers to the corridor + std::shared_ptr corridor_ptr; + tactic::Timestamp prev_stamp; // Store the previously applied velocity and a sliding window history of MPC results diff --git a/main/src/vtr_path_planning/include/vtr_path_planning/cbit/cbit_path_planner.hpp b/main/src/vtr_path_planning/include/vtr_path_planning/cbit/cbit_path_planner.hpp index 7ef469661..ce34f6674 100644 --- a/main/src/vtr_path_planning/include/vtr_path_planning/cbit/cbit_path_planner.hpp +++ b/main/src/vtr_path_planning/include/vtr_path_planning/cbit/cbit_path_planner.hpp @@ -34,6 +34,7 @@ #include "vtr_path_planning/cbit/generate_pq.hpp" #include "vtr_path_planning/cbit/cbit_config.hpp" #include "vtr_path_planning/cbit/cbit_costmap.hpp" +#include "vtr_path_planning/cbit/cbit_plotting.hpp" #include "vtr_tactic/tactic.hpp" #pragma once @@ -77,7 +78,7 @@ class CBITPlanner { // Costmap pointer std::shared_ptr cbit_costmap_ptr; - CBITPlanner(CBITConfig conf_in, std::shared_ptr path_in, vtr::path_planning::BasePathPlanner::RobotState& robot_state, std::shared_ptr> path_ptr, std::shared_ptr costmap_ptr); + CBITPlanner(CBITConfig conf_in, std::shared_ptr path_in, vtr::path_planning::BasePathPlanner::RobotState& robot_state, std::shared_ptr> path_ptr, std::shared_ptr costmap_ptr, std::shared_ptr corridor_ptr); protected: struct ChainInfo { @@ -93,9 +94,9 @@ class CBITPlanner { private: void InitializePlanningSpace(); - void Planning(vtr::path_planning::BasePathPlanner::RobotState& robot_state, std::shared_ptr costmap_ptr); + void Planning(vtr::path_planning::BasePathPlanner::RobotState& robot_state, std::shared_ptr costmap_ptr, std::shared_ptr corridor_ptr); void ResetPlanner(); - void HardReset(vtr::path_planning::BasePathPlanner::RobotState& robot_state, std::shared_ptr costmap_ptr); + void HardReset(vtr::path_planning::BasePathPlanner::RobotState& robot_state, std::shared_ptr costmap_ptr, std::shared_ptr corridor_ptr); std::shared_ptr UpdateState(); std::vector> SampleBox(int m); std::vector> SampleFreeSpace(int m); @@ -108,19 +109,33 @@ class CBITPlanner { std::vector ExtractEuclidPath(); void Prune(double c_best, double c_best_weighted); bool edge_in_tree(Node v, Node x); + bool edge_in_tree_v2(std::shared_ptr v, std::shared_ptr x); bool node_in_tree(Node x); + bool node_in_tree_v2(std::shared_ptr x); double cost_col(std::vector> obs, Node start, Node end); double weighted_cost_col(std::vector> obs, Node start, Node end); Node curve_to_euclid(Node node); Pose lin_interpolate(int p_ind, double p_val); bool is_inside_obs(std::vector> obs, Node node); bool costmap_col(Node node); + bool costmap_col_tight(Node node); bool discrete_collision(std::vector> obs, double discretization, Node start, Node end); bool col_check_path(); + std::shared_ptr col_check_path_v2(double max_lookahead_p); void restore_tree(double g_T_update, double g_T_weighted_update); // Add class for Tree // Add dictionary (or some other structure) for the cost to come lookup using NodeID as key // Constructor: Needs to initialize all my objects im using + + + // Temporary functions for corridor updates, long term want to move these to a different file + void update_corridor(std::shared_ptr corridor, std::vector homotopy_p, std::vector homotopy_q, Node robot_state); + struct collision_result + { + bool bool_result; + Node col_node; + }; + struct collision_result discrete_collision_v2(double discretization, Node start, Node end); }; \ No newline at end of file diff --git a/main/src/vtr_path_planning/include/vtr_path_planning/cbit/cbit_plotting.hpp b/main/src/vtr_path_planning/include/vtr_path_planning/cbit/cbit_plotting.hpp new file mode 100644 index 000000000..83974a017 --- /dev/null +++ b/main/src/vtr_path_planning/include/vtr_path_planning/cbit/cbit_plotting.hpp @@ -0,0 +1,32 @@ +// Copyright 2021, Autonomous Space Robotics Lab (ASRL) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/** + * \file cbit_plotting.hpp + * \author Jordy Sehn, Autonomous Space Robotics Lab (ASRL) + */ + +// Contains Matplotlibcpp debug plotting tools for the cbit planner +// This library is not meant to be included in the main vt&r branch, debug only + +#include +#include "matplotlibcpp.h" // experimental plotting +#include "vtr_path_planning/cbit/utils.hpp" + +#pragma once + +void plot_tree(Tree current_tree, Node robot_pq, std::vector path_p, std::vector path_q, std::vector> samples); +void plot_robot(Node robot_pq); + +void initialize_plot(); \ No newline at end of file diff --git a/main/src/vtr_path_planning/include/vtr_path_planning/cbit/generate_pq.hpp b/main/src/vtr_path_planning/include/vtr_path_planning/cbit/generate_pq.hpp index 6f3f18943..e6a8cc1b9 100644 --- a/main/src/vtr_path_planning/include/vtr_path_planning/cbit/generate_pq.hpp +++ b/main/src/vtr_path_planning/include/vtr_path_planning/cbit/generate_pq.hpp @@ -48,4 +48,25 @@ class CBITPath { // Actually I think ill just do this in the constructor for now //std::vector ProcessPath(std::vector disc_path); // Function for assigning p distance values for each euclidean point in pre-processing +}; + +// Class for storing the dynamic corridor information +class CBITCorridor { + public: + std::vector p_bins; + std::vector q_left; + std::vector q_right; + std::vector x_left; + std::vector x_right; + std::vector y_left; + std::vector y_right; + double q_max; + double sliding_window_width; + double curv_to_euclid_discretization; + + CBITCorridor(CBITConfig config, std::shared_ptr global_path_ptr); // Constructor, Feed this the taught path and config + CBITCorridor() = default; + + //void update_corridor(homotopy_p, homotopy_q, robot_state); + }; \ No newline at end of file diff --git a/main/src/vtr_path_planning/include/vtr_path_planning/cbit/utils.hpp b/main/src/vtr_path_planning/include/vtr_path_planning/cbit/utils.hpp index b21b1c09f..9c62b1c42 100644 --- a/main/src/vtr_path_planning/include/vtr_path_planning/cbit/utils.hpp +++ b/main/src/vtr_path_planning/include/vtr_path_planning/cbit/utils.hpp @@ -90,6 +90,7 @@ class Tree { std::vector> V_Old; std::vector> V_Repair_Backup; std::vector, std::shared_ptr>> E; + std::vector, std::shared_ptr>> E_Old; std::vector> QV; // using shared pointers std::vector, std::shared_ptr>> QE; Tree() = default; diff --git a/main/src/vtr_path_planning/include/vtr_path_planning/mpc/mpc_path_planner.hpp b/main/src/vtr_path_planning/include/vtr_path_planning/mpc/mpc_path_planner.hpp index 4ec60ce95..b6faa57cf 100644 --- a/main/src/vtr_path_planning/include/vtr_path_planning/mpc/mpc_path_planner.hpp +++ b/main/src/vtr_path_planning/include/vtr_path_planning/mpc/mpc_path_planner.hpp @@ -43,4 +43,7 @@ struct mpc_result SolveMPC(Eigen::Matrix previous_vel, lgmath::se3 struct meas_result GenerateReferenceMeas(std::shared_ptr> cbit_path_ptr, std::tuple robot_pose, int K, double DT, double VF); // Helper function for post-processing and saturating the velocity command -Eigen::Matrix SaturateVel(Eigen::Matrix applied_vel, double v_lim, double w_lim); \ No newline at end of file +Eigen::Matrix SaturateVel(Eigen::Matrix 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 cbit_p, std::vector cbit_path); \ No newline at end of file diff --git a/main/src/vtr_path_planning/src/cbit/cbit.cpp b/main/src/vtr_path_planning/src/cbit/cbit.cpp index 9f2785801..f8868c1ee 100644 --- a/main/src/vtr_path_planning/src/cbit/cbit.cpp +++ b/main/src/vtr_path_planning/src/cbit/cbit.cpp @@ -118,6 +118,8 @@ CBIT::CBIT(const Config::ConstPtr& config, mpc_path_pub_ = node->create_publisher("mpc_prediction", 10); robot_path_pub_ = node->create_publisher("robot_path", 10); path_pub_ = node->create_publisher("planning_path", 10); + corridor_pub_l_ = node->create_publisher("corridor_path_left", 10); + corridor_pub_r_ = node->create_publisher("corridor_path_right", 10); // Updating cbit_configs // Environment @@ -230,16 +232,20 @@ void CBIT::initializeRoute(RobotState& robot_state) { CLOG(INFO, "path_planning.cbit") << "Trying to create global path"; // Create the path class object (Path preprocessing) CBITPath global_path(cbit_config, euclid_path_vec); - // Make a pointer to this path std::shared_ptr global_path_ptr = std::make_shared(global_path); + CLOG(INFO, "path_planning.cbit") << "Teach Path has been pre-processed. Attempting to initialize the dynamic corridor"; - CLOG(INFO, "path_planning.cbit") << "Teach Path has been pre-processed. Attempting to instantiate the planner"; + // Initialize the dynamic corridor + CBITCorridor corridor(cbit_config, global_path_ptr); + // Make a pointer to the corridor + corridor_ptr = std::make_shared(corridor); + CLOG(INFO, "path_planning.cbit") << "Corridor generated successfully. Attempting to instantiate the planner"; - // Instantiate the planner - CBITPlanner cbit(cbit_config, global_path_ptr, robot_state, cbit_path_ptr, costmap_ptr); + // Instantiate the planner + CBITPlanner cbit(cbit_config, global_path_ptr, robot_state, cbit_path_ptr, costmap_ptr, corridor_ptr); CLOG(INFO, "path_planning.cbit") << "Planner successfully created and resolved"; } @@ -376,11 +382,21 @@ auto CBIT::computeCommand(RobotState& robot_state) -> Command { // Create and solve the STEAM optimization problem - CLOG(INFO, "mpc.cbit") << "Attempting to solve the MPC problem"; - auto mpc_result = SolveMPC(applied_vel, T0, measurements, K, DT, VF, pose_noise_vect, vel_noise_vect, accel_noise_vect, kin_noise_vect, point_stabilization); - applied_vel = mpc_result.applied_vel; // note dont re-declare applied vel here - auto mpc_poses = mpc_result.mpc_poses; - CLOG(INFO, "mpc.cbit") << "Successfully solved MPC problem"; + std::vector mpc_poses; + try + { + CLOG(INFO, "mpc.cbit") << "Attempting to solve the MPC problem"; + auto mpc_result = SolveMPC(applied_vel, T0, measurements, K, DT, VF, pose_noise_vect, vel_noise_vect, accel_noise_vect, kin_noise_vect, point_stabilization); + applied_vel = mpc_result.applied_vel; // note dont re-declare applied vel here + mpc_poses = mpc_result.mpc_poses; + CLOG(INFO, "mpc.cbit") << "Successfully solved MPC problem"; + } + catch(...) + { + CLOG(ERROR, "mpc.cbit") << "STEAM Optimization Failed; Commanding to Stop the Vehicle"; + applied_vel(0) = 0.0; + applied_vel(1) = 0.0; + } CLOG(INFO, "mpc.cbit") << "The linear velocity is: " << applied_vel(0) << " The angular vel is: " << applied_vel(1); @@ -394,7 +410,7 @@ auto CBIT::computeCommand(RobotState& robot_state) -> Command { vel_history.push_back(saturated_vel); // Store the current robot state in the robot state path so it can be visualized - robot_poses.push_back(T0); + robot_poses.push_back(T_w_p * T_p_r); // Send the robot poses and mpc prediction to rviz visualize(stamp, T_w_p, T_p_r, T_p_r_extp, T_p_r_extp2, mpc_poses, robot_poses); @@ -516,7 +532,6 @@ void CBIT::visualize(const tactic::Timestamp& stamp, const tactic::EdgeTransform // Attempting to publish the actual path which we are receiving from the shared pointer in the cbitplanner // The path is stored as a vector of se3 Pose objects from cbit/utils, need to iterate through and construct proper ros2 nav_msgs PoseStamped - /// Publish the intermediate goals in the planning frame { nav_msgs::msg::Path path; path.header.frame_id = "world"; @@ -542,6 +557,52 @@ void CBIT::visualize(const tactic::Timestamp& stamp, const tactic::EdgeTransform path_pub_->publish(path); } + + + + // Attempting to Publish the left and right dynamic corridor for the current path homotopy class + { + nav_msgs::msg::Path corridor_left; + corridor_left.header.frame_id = "world"; + corridor_left.header.stamp = rclcpp::Time(stamp); + auto& poses_l = corridor_left.poses; + + nav_msgs::msg::Path corridor_right; + corridor_right.header.frame_id = "world"; + corridor_right.header.stamp = rclcpp::Time(stamp); + auto& poses_r = corridor_right.poses; + + // iterate through the corridor paths + geometry_msgs::msg::Pose test_pose_l; + geometry_msgs::msg::Pose test_pose_r; + for (unsigned i = 0; i < corridor_ptr->x_left.size(); i++) + { + //lhs + auto& pose_l = poses_l.emplace_back(); + test_pose_l.position.x = corridor_ptr->x_left[i]; + test_pose_l.position.y = corridor_ptr->y_left[i]; + test_pose_l.position.z = 0.0; // setting this 0.0 for now for flat world assumption, but long term we might want to add a z component + test_pose_l.orientation.x = 0.0; + test_pose_l.orientation.y = 0.0; + test_pose_l.orientation.z = 0.0; + test_pose_l.orientation.w = 1.0; + pose_l.pose = test_pose_l; + + // rhs + auto& pose_r = poses_r.emplace_back(); + test_pose_r.position.x = corridor_ptr->x_right[i]; + test_pose_r.position.y = corridor_ptr->y_right[i]; + test_pose_r.position.z = 0.0; // setting this 0.0 for now for flat world assumption, but long term we might want to add a z component + test_pose_r.orientation.x = 0.0; + test_pose_r.orientation.y = 0.0; + test_pose_r.orientation.z = 0.0; + test_pose_r.orientation.w = 1.0; + pose_r.pose = test_pose_r; + } + + corridor_pub_l_->publish(corridor_left); + corridor_pub_r_->publish(corridor_right); + } return; } diff --git a/main/src/vtr_path_planning/src/cbit/cbit_path_planner.cpp b/main/src/vtr_path_planning/src/cbit/cbit_path_planner.cpp index 5387282d5..36b6d1bdd 100644 --- a/main/src/vtr_path_planning/src/cbit/cbit_path_planner.cpp +++ b/main/src/vtr_path_planning/src/cbit/cbit_path_planner.cpp @@ -28,7 +28,7 @@ namespace { inline std::tuple T2xyzrpy( const vtr::tactic::EdgeTransform& T) { const auto Tm = T.matrix(); - return std::make_tuple(Tm(0, 3), Tm(1, 3), Tm(2,3), std::atan2(Tm(2, 1), Tm(2, 2)), std::atan2(-1*Tm(2, 0), sqrt(pow(Tm(2, 1),2) + pow(Tm(2, 2),2))), std::atan2(Tm(1, 0), Tm(0, 0))); + return std::make_tuple(Tm(0, 3), Tm(1, 3), Tm(2,3), std::atan2(Tm(2, 1), Tm(2, 2)), std::atan2(-1.0*Tm(2, 0), sqrt(pow(Tm(2, 1),2) + pow(Tm(2, 2),2))), std::atan2(Tm(1, 0), Tm(0, 0))); } } @@ -44,7 +44,7 @@ auto CBITPlanner::getChainInfo(vtr::path_planning::BasePathPlanner::RobotState& } // Class Constructor: -CBITPlanner::CBITPlanner(CBITConfig conf_in, std::shared_ptr path_in, vtr::path_planning::BasePathPlanner::RobotState& robot_state, std::shared_ptr> path_ptr, std::shared_ptr costmap_ptr) +CBITPlanner::CBITPlanner(CBITConfig conf_in, std::shared_ptr path_in, vtr::path_planning::BasePathPlanner::RobotState& robot_state, std::shared_ptr> path_ptr, std::shared_ptr costmap_ptr, std::shared_ptr corridor_ptr) { // Setting random seed @@ -73,9 +73,12 @@ CBITPlanner::CBITPlanner(CBITConfig conf_in, std::shared_ptr path_in, p_start = std::make_shared (global_path->p.back(), 0.0); dynamic_window_width = conf.sliding_window_width; + + // DEBUG PLOTTING + //initialize_plot(); InitializePlanningSpace(); - Planning(robot_state, costmap_ptr); + Planning(robot_state, costmap_ptr, corridor_ptr); // DEBUG CODE, ROBOT STATE UPDATE QUERY EXAMPLE /* @@ -129,17 +132,17 @@ void CBITPlanner::InitializePlanningSpace() // Initialize obstacles // Permanent simulated obstacles can be populated here like so, but in the latest version obstacles are updated from the costmap //obs_rectangle = {{2.0, -2.0, 1, 3}, {3.0, 4.0, 3.0, 1.0}, {3.0, 8.0, 2.0, 2.0}}; // Obstacle format is {{x,y,w,h}} where x,y is the lower left corner coordinate - //obs_rectangle = {{2.0, -2.0, 1, 3}, {3.0, 4.0, 3.0, 1.0}}; + //obs_rectangle = {{4.0, -1.0, 2, 2}}; // for plotting debugger tests obs_rectangle = {}; // Initialize sliding window dimensions for plotting and radius expansion calc; sample_box_height = conf.q_max * 2.0; - sample_box_width = conf.sliding_window_width; + sample_box_width = conf.sliding_window_width + 2 * conf.sliding_window_freespace_padding; } // Reset fuction which goes through the entire reset procedure (including calling ResetPlanner and restarting the planner itself) -void CBITPlanner::HardReset(vtr::path_planning::BasePathPlanner::RobotState& robot_state, std::shared_ptr costmap_ptr) +void CBITPlanner::HardReset(vtr::path_planning::BasePathPlanner::RobotState& robot_state, std::shared_ptr costmap_ptr, std::shared_ptr corridor_ptr) { // I think we may also want to add a small time delay just so we arent repeatedly spamming an optimal solution CLOG(ERROR, "path_planning.cbit_planner") << "Plan could not be improved, Initiating Reset"; @@ -202,7 +205,7 @@ void CBITPlanner::HardReset(vtr::path_planning::BasePathPlanner::RobotState& rob CLOG(INFO, "path_planning.cbit_planner") << "The p,q coordinate of the robots goal is now: p: " << p_goal->p << " q: " << p_goal->q; CLOG(INFO, "path_planning.cbit_planner") << "The p,q coordinate of the robots start is now: p: " << p_start->p << " q: " << p_start->q << " g_T: " << p_start->g_T; - Planning(robot_state, costmap_ptr); + Planning(robot_state, costmap_ptr, corridor_ptr); } // If we ever exit the planner due to a fault, we will do a hard reset, everything but the current robot_state (p_goal) and the inputs will be reinitialized @@ -234,8 +237,14 @@ void CBITPlanner::ResetPlanner() // Main planning function -void CBITPlanner::Planning(vtr::path_planning::BasePathPlanner::RobotState& robot_state, std::shared_ptr costmap_ptr) +void CBITPlanner::Planning(vtr::path_planning::BasePathPlanner::RobotState& robot_state, std::shared_ptr costmap_ptr, std::shared_ptr corridor_ptr) { + // find some better places to initialize these + double prev_path_cost = INFINITY; // experimental + int compute_time = 0; // exprimental + int vertex_rej_prob = 100; // experimental + int dyn_batch_samples = conf.batch_samples; + // Grab the amount of time in ms between robot state updates int control_period_ms = (1.0 / conf.state_update_freq) * 1000.0; auto state_update_time = std::chrono::high_resolution_clock::now() + std::chrono::milliseconds(control_period_ms); @@ -269,12 +278,12 @@ void CBITPlanner::Planning(vtr::path_planning::BasePathPlanner::RobotState& robo // Check whether a robot state update should be applied // We only update the state if A: we have first found a valid initial solution, and B: if the current time has elapsed the control period - if (conf.update_state == true && repair_mode == false) + if (conf.update_state == true) { if ((p_goal->parent != nullptr) && (std::chrono::high_resolution_clock::now() >= state_update_time)) { // Update timers - CLOG(INFO, "path_planning.cbit_planner") << "Attempting to Update Robot State"; + //CLOG(INFO, "path_planning.cbit_planner") << "Attempting to Update Robot State"; state_update_time = std::chrono::high_resolution_clock::now() + std::chrono::milliseconds(control_period_ms); // get the euclidean robot state in the world frame from vt&r @@ -282,6 +291,7 @@ void CBITPlanner::Planning(vtr::path_planning::BasePathPlanner::RobotState& robo if (chain.isLocalized() == 0) { // If we ever become unlocalized, I think we just need to break, then set a flag to exit the outter loop + // This also triggers after we reach end of path and effectively shuts down the planner bool localization_flag = false; CLOG(ERROR, "path_planning.cbit_planner") << "Localization was Lost, Exiting Inner Planning Loop"; break; @@ -292,7 +302,8 @@ void CBITPlanner::Planning(vtr::path_planning::BasePathPlanner::RobotState& robo const auto chain_info = getChainInfo(robot_state); auto [stamp, w_p_r_in_r, T_p_r, T_w_p, curr_sid] = chain_info; - // Experimental Pose extrapolation (seems to work well) + // Experimental Pose extrapolation (I find often this doesnt work well with te direct path tracking method) + // It can be desireable to delay pruning the path behind where we think the robot is if (conf.extrapolation == true) { @@ -310,16 +321,18 @@ void CBITPlanner::Planning(vtr::path_planning::BasePathPlanner::RobotState& robo }(); robot_pose = T2xyzrpy(T_w_p * T_p_r_extp); - CLOG(INFO, "path_planning.cbit_planner") << "Extrapolated Robot Pose: x: " << std::get<0>(robot_pose) << " y: " - << std::get<1>(robot_pose) << " z: " << std::get<2>(robot_pose) << " roll: " << std::get<3>(robot_pose) << " pitch: " - << std::get<4>(robot_pose) << " yaw: " << std::get<5>(robot_pose); + //CLOG(INFO, "path_planning.cbit_planner") << "Extrapolated Robot Pose: x: " << std::get<0>(robot_pose) << " y: " + //<< std::get<1>(robot_pose) << " z: " << std::get<2>(robot_pose) << " roll: " << std::get<3>(robot_pose) << " pitch: " + //<< std::get<4>(robot_pose) << " yaw: " << std::get<5>(robot_pose); } else { robot_pose= T2xyzrpy(T_w_p * T_p_r); - CLOG(INFO, "path_planning.cbit_planner") << "Robot Pose: x: " << std::get<0>(robot_pose) << " y: " - << std::get<1>(robot_pose) << " z: " << std::get<2>(robot_pose) << " roll: " << std::get<3>(robot_pose) << " pitch: " - << std::get<4>(robot_pose) << " yaw: " << std::get<5>(robot_pose); + + // experimental, finding the direction the robot is facing (planing forward or in reverse) + //debug + //auto test_pose = T2xyzrpy(T_p_r); + //CLOG(ERROR, "path_planning.cbit_planner") << "Robot Pose in planing frame yaw: " << std::get<5>(test_pose); } @@ -335,7 +348,12 @@ void CBITPlanner::Planning(vtr::path_planning::BasePathPlanner::RobotState& robo // Perform a state update to convert the actual robot position to its corresponding pq space: p_goal = UpdateState(); + //TODO: It could be useful to convert this p_goal back to euclid and compare with se3_robot_pose to verify the conversion worked properly (error should be very low) + // If its not, we probably need to handle it or return an error + + // EXPERIMENTAL TODO: If robot pose nears the goal (p_start), exit the planner with the current solution + // Note, not sure if this is super easy, because I think if the planner exits the vt&r code will immediately relaunch it //CLOG(ERROR, "path_planning") << "Trying the distance to goal is: " << abs(p_goal->p - p_start->p); //if (abs(p_goal->p - p_start->p) <= 2.0) //{ @@ -344,11 +362,7 @@ void CBITPlanner::Planning(vtr::path_planning::BasePathPlanner::RobotState& robo //} // If we arent in repair mode, update the backup goal - - if (repair_mode == false) - { - p_goal_backup = p_goal; - } + // There is a case where the robot is actually starting behind the world frame of the teach path (or the localization thinks it is at first anyways) @@ -363,25 +377,69 @@ void CBITPlanner::Planning(vtr::path_planning::BasePathPlanner::RobotState& robo // Debug, check to see what p value we are spitting out - CLOG(INFO, "path_planning.cbit_planner") << "The p,q coordinate of the robots updated state goal is now: p: " << p_goal->p << " q: " << p_goal->q; + //CLOG(INFO, "path_planning.cbit_planner") << "The p,q coordinate of the robots updated state goal is now: p: " << p_goal->p << " q: " << p_goal->q; // Add the new goal (robot state) to the samples so it can be found again + // EXPERIMENTAL - want to try flushing all the samples just before pushing the goal, then resampling locally in a radius around the new goal. + // It probably makes sense that the sample density in the radius ring is based on the rgg radius as well + //samples.clear(); samples.push_back(p_goal); + //double r_s; + //double theta_s; + //double x_s; + //double y_s; + //for (int i = 0; i < 100; i++) + //{ + // r_s = 2.0 * sqrt(static_cast (rand()) / static_cast (RAND_MAX)) ; // todo replace magic number 2.0 in this one (not the next) + // theta_s = (static_cast (rand()) / static_cast (RAND_MAX)) * 2.0 * 3.1415; + // x_s = r_s * cos(theta_s) + p_goal->p; + // y_s = r_s * sin(theta_s) + p_goal->q; + // Node new_sample(x_s, y_s); + // samples.push_back(std::make_shared (new_sample)); + // CLOG(DEBUG, "path_planning.cbit_planner") << "Sample x: " << x_s << " Sample y: " << y_s; + //} // Find vertices in the tree which are close to the new state, then populate the vertex queue with only these values. tree.QV.clear(); tree.QE.clear(); + + // EXPERIMENTAL, only take the closest point in the tree and use this to connect to the new robot state + /* + //int nearby_sample_counter = 0; + double closest_sample_dist = INFINITY; + int closest_sample_ind; + double sample_dist; for (int i = 0; i < tree.V.size(); i++) { - if (calc_dist(*(tree.V[i]), *p_goal) <= 1.0 ) // TODO: replace magic number with a param, represents radius to search for state update rewires + sample_dist = calc_dist(*(tree.V[i]), *p_goal); + if ((sample_dist <= closest_sample_dist) && ((tree.V[i])->p > (p_goal->p + (conf.initial_exp_rad/2)))) // TODO: replace magic number with a param, represents radius to search for state update rewires { - tree.QV.push_back(tree.V[i]); + closest_sample_dist = sample_dist; // used when only taking closest point in the tree to consider + closest_sample_ind = i; // used when only taking closest point in the tree to consider + //tree.QV.push_back(tree.V[i]); // comment out when only taking closest point in the tree to consider + } + } + tree.QV.push_back(tree.V[closest_sample_ind]); // used when only taking closest point in the tree to consider + */ + + // Alternative: Take all points in a radius of 2.0m around the new robot state (only if they are a ahead though) + double sample_dist; + for (int i = 0; i < tree.V.size(); i++) + { + sample_dist = calc_dist(*(tree.V[i]), *p_goal); + if ((sample_dist <= 2.0) && ((tree.V[i])->p > (p_goal->p + (conf.initial_exp_rad/2)))) // TODO: replace magic number with a param, represents radius to search for state update rewires + { + tree.QV.push_back(tree.V[i]); // comment out when only taking closest point in the tree to consider } } - CLOG(INFO, "path_planning.cbit_planner") << "Robot State Updated Successfully"; - CLOG(INFO, "path_planning.cbit_planner") << "QV size: " << tree.QV.size(); + + + + CLOG(DEBUG, "path_planning.cbit_planner") << "Robot State Updated Successfully, p: " << p_goal->p << " q: " << p_goal->q; + //CLOG(DEBUG, "path_planning.cbit_planner") << "Nearest Vertex, p: " << tree.V[closest_sample_ind]->p << " q: " << tree.V[closest_sample_ind]->q; + CLOG(DEBUG, "path_planning.cbit_planner") << "QV size: " << tree.QV.size(); // When the planner resumes, this will cause it to immediately try to rewire locally to the new robot state in a short amount of time @@ -407,82 +465,169 @@ void CBITPlanner::Planning(vtr::path_planning::BasePathPlanner::RobotState& robo if (p_goal->parent != nullptr) { - if (repair_mode==false) - { - //std::cout << "Iteration: " << k << std::endl; - //std::cout << "Path Cost: " << p_goal->g_T_weighted << std::endl; - - // Benchmark current compute time - auto stop_time = std::chrono::high_resolution_clock::now(); - auto duration = std::chrono::duration_cast(stop_time - start_time); - //std::cout << "Batch Compute Time (ms): " << duration.count() << std::endl; - + //std::cout << "Iteration: " << k << std::endl; + //std::cout << "Path Cost: " << p_goal->g_T_weighted << std::endl; - // Debug message for online use (wont use std::out long term) - CLOG(INFO, "path_planning.cbit_planner") << "New Batch - Iteration: " << k << " Path Cost: " << p_goal->g_T_weighted << " Batch Compute Time (ms): " << duration.count(); - CLOG(INFO, "path_planning.cbit_planner") << "Tree Size: " << tree.V.size() << " Vertex Queue Size: " << tree.QV.size() << " Sample Size: " <(stop_time - start_time); + //std::cout << "Batch Compute Time (ms): " << duration.count() << std::endl; - // Extract the solution - //std::cout << "Made it just before extractpath" << std::endl; - std::tuple, std::vector> curv_path = ExtractPath(robot_state, costmap_ptr); - path_x = std::get<0>(curv_path); // p coordinates of the current path (I should probably rename, this is misleading its not x and y) - path_y = std::get<1>(curv_path); // q coordinates of the current path (I should probably rename, this is misleading its not x and y) + compute_time = static_cast (duration.count()); + // Debug message for online use (wont use std::out long term) + CLOG(INFO, "path_planning.cbit_planner") << "New Batch - Iteration: " << k << " Path Cost: " << p_goal->g_T_weighted << " Batch Compute Time (ms): " << duration.count(); + CLOG(INFO, "path_planning.cbit_planner") << "Tree Vertex Size: " << tree.V.size() << " Tree Edge Size: " << tree.E.size() << " Sample Size: " << samples.size(); - // Store the Euclidean solution in the shared pointer memory (vector of Pose classes) so it can be accessed in the CBIT class - //std::cout << "Made it just before extracting euclid path" << std::endl; - std::vector euclid_path = ExtractEuclidPath(); - *cbit_path_ptr = euclid_path; - - // Reset the start time - start_time = std::chrono::high_resolution_clock::now(); - //std::cout << "Made it to end of new batch process code" << std::endl; + // If the solution does not improve, backup the tree to restrict unnecessary growth + if (abs(p_goal->g_T_weighted - prev_path_cost) < 1e-6) + { + CLOG(DEBUG, "path_planning.cbit_planner") << "There Was No Path Improvement, Restoring Tree to Previous Batch To Restrict Growth."; + tree.V = tree.V_Old; + tree.E = tree.E_Old; } + prev_path_cost = p_goal->g_T_weighted; - - // Check if we are in repair mode, if we are and we reach this point, it means we will have just successfully rewired - // Now we need to try to recover the old tree and update all cost to come values - if (repair_mode==true) + // EXPERIMENTAL + // if the batch compute time starts to creep up, it means the tree is getting abit to bloated + // Its likely that more samples will not really help us much while going around obstacles, so we should reduce the batch size + if (compute_time >= 200) { - double g_T_update = p_goal->g_T - repair_g_T_old; - double g_T_weighted_update = p_goal->g_T_weighted - repair_g_T_weighted_old; - - p_goal = p_goal_backup; - //CLOG(ERROR, "path_planning.cbit_planner") << "The p_goal is now set to p: " << p_goal->p << " q: " << p_goal->q; - try // experimental debug + if (vertex_rej_prob > 60) // never want to exceed some minimums { - restore_tree(g_T_update, g_T_weighted_update); + dyn_batch_samples = dyn_batch_samples - 10; + // Also increase the probability of rejecting vertex queue vertices to improve performance + vertex_rej_prob = vertex_rej_prob - 10; + CLOG(DEBUG, "path_planning.cbit_planner") << "Compute Falling Behind, Reducing Batch Size To " << dyn_batch_samples << " and Decreasing Vertex Acceptance Probability to " << vertex_rej_prob << "%"; } - catch(...) + } + // Conversely if batch compute time is fast, we can handle more samples (up to a max) + else + { + if (vertex_rej_prob < 100) { - CLOG(ERROR, "path_planning.cbit_planner") << "Failed to restore the tree, initiating hard reset."; + vertex_rej_prob = vertex_rej_prob + 2; + dyn_batch_samples = dyn_batch_samples + 2; + CLOG(DEBUG, "path_planning.cbit_planner") << "Compute Catching Up, Increasing Batch Size To " << dyn_batch_samples << " and Decreasing Vertex Acceptance Probability to " << vertex_rej_prob << "%"; } - + } + m = dyn_batch_samples; + + // Extract the solution + //std::cout << "Made it just before extractpath" << std::endl; + std::tuple, std::vector> curv_path = ExtractPath(robot_state, costmap_ptr); + path_x = std::get<0>(curv_path); // p coordinates of the current path (I should probably rename, this is misleading its not x and y) + path_y = std::get<1>(curv_path); // q coordinates of the current path (I should probably rename, this is misleading its not x and y) + + + // Store the Euclidean solution in the shared pointer memory (vector of Pose classes) so it can be accessed in the CBIT class + //std::cout << "Made it just before extracting euclid path" << std::endl; + std::vector euclid_path = ExtractEuclidPath(); + *cbit_path_ptr = euclid_path; + + // Reset the start time + start_time = std::chrono::high_resolution_clock::now(); + //std::cout << "Made it to end of new batch process code" << std::endl; + + //DEBUG PLOTTING + //auto plot_start_time = std::chrono::high_resolution_clock::now(); + //plot_tree(tree, *p_goal, path_x, path_y, samples); + //auto plot_stop_time = std::chrono::high_resolution_clock::now(); + //auto duration_plot = std::chrono::duration_cast(plot_stop_time - plot_start_time); + //CLOG(ERROR, "path_planning.cbit_planner") << "Plot Time: " << duration_plot.count() << "ms"; + - // Reset the goal (NOTE IN PYTHON I DO THIS AFTER REPAIR, BUT I THINK WITH MY QUICK AND DIRTY REPAIR I NEED TO SET IT BEFORE) - //p_goal = p_goal_backup; - repair_mode = false; + + // First grab the latest obstacles (could potentially take this opportunity to make a more compact approximate obs representation) + + // Collision Check the batch solution: TODO:, will need to make wormhole modifcations long term + std::shared_ptr col_free_vertex = col_check_path_v2((p_goal->p + conf.sliding_window_width + conf.sliding_window_freespace_padding)); // outputs NULL if no collision + if (col_free_vertex != nullptr) + { + // If there is a collision, prune the tree of all vertices to the left of the this vertex + CLOG(WARNING, "path_planning.cbit_planner") << "Collision Detected:"; + CLOG(WARNING, "path_planning.cbit_planner") << "Collision Free Vertex is - p: " << col_free_vertex->p << " q: " << col_free_vertex->q; - tree.QV.clear(); - tree.QE.clear(); - samples.clear(); + + // Vertex Prune (maintain only vertices to the right of the collision free vertex) + std::vector> pruned_vertex_tree; + pruned_vertex_tree.reserve(tree.V.size()); + for (int i =0; ip >= col_free_vertex->p) + { + pruned_vertex_tree.push_back(tree.V[i]); + } + } + tree.V = pruned_vertex_tree; - CLOG(INFO, "path_planning.cbit_planner") << "REPAIR MODE COMPLETED SUCCESSFULLY"; - CLOG(INFO, "path_planning.cbit_planner") << "The p_goal is now set to p: " << p_goal->p << " q: " << p_goal->q; - continue; - } + // Edge Prune (maintain only edges to the right of the collision free vertex) + std::vector, std::shared_ptr>> pruned_edge_tree; + pruned_edge_tree.reserve(tree.E.size()); + for (int i = 0; i (tree.E[i])->p >= col_free_vertex->p) + { + pruned_edge_tree.push_back(tree.E[i]); + } + } + + tree.E = pruned_edge_tree; + + + // Experimental, just trying to see whether not restoring at all yields better repair performance time + //tree.V.clear(); + //tree.V_Repair_Backup.clear(); + //tree.V_Old.clear() + //tree.E.clear(); + //tree.V.push_back(std::shared_ptr (p_start)); + // End of experimental + + // Reset the goal, and add it to the samples + p_goal->parent = nullptr; + p_goal->g_T = INFINITY; + p_goal->g_T_weighted = INFINITY; + samples.clear(); // first clear samples so they dont accumulate excessively + samples.push_back(p_goal); + + // This will cause samplefreespace to run this iteration, but if we made it here m will be set to conf.batch_samples (as its reset every iteration) + m = conf.initial_samples; + //k=0; + + // Re-initialize sliding window dimensions for plotting and radius expansion calc; + sample_box_height = conf.q_max * 2.0; + sample_box_width = conf.sliding_window_width + 2 * conf.sliding_window_freespace_padding; + + // Disable pre-seeds from being regenerated + repair_mode = true; + + // Sample free-space wont generate pre-seeds, so we should generate our own in the portion of the tree that was dropped (col_free_vertex.p to p_goal.p) + int pre_seeds = abs(col_free_vertex->p - p_goal->p) / 0.25; // Note needed to change p_goal to p_zero. When the sliding window padding is large, pre-seeds wont get generated all the way to the goal + //int pre_seeds = abs(p_start->p - p_goal->p) / 0.25; // EXPERIMENTAL< DONT FORGET TO REMOVE THIS AND UNCOMMENT ABOVE IF NOT USING THE FULL RESET METHOD + double p_step = 0.25; + double p_val = p_goal->p; + for (int i = 0; i < (pre_seeds-1); i++) + { + Node node((p_val+p_step), 0); + samples.push_back(std::make_shared (node)); + p_val = p_val + p_step; + } - // After restoring the tree, (or finding a solution for a batch) we need to collision check the path to see if we need to go into repair mode + } + else + { + // Reset the free space flag in sample freespace + //CLOG(WARNING, "path_planning.cbit_planner") << "No Collision:"; + repair_mode = false; - repair_mode = col_check_path(); - // DEBUG, dont leave this! bipasses tree restore and repair mode - //if (repair_mode) - //{ - // HardReset(robot_state, costmap_ptr); - //} + // EXPERIMENTAL: Updating the dynamic corridor (now that we know our path is collision free): + //update_corridor(corridor_ptr, path_x, path_y, *p_goal); + + } + } //std::cout << "Made it just before prune" << std::endl; Prune(p_goal->g_T, p_goal->g_T_weighted); @@ -507,28 +652,67 @@ void CBITPlanner::Planning(vtr::path_planning::BasePathPlanner::RobotState& robo } - + // Backup the old tree: + tree.V_Old.clear(); + tree.E_Old.clear(); for (int i = 0; i < tree.V.size(); i++) { - tree.V_Old.push_back(std::shared_ptr (tree.V[i])); + tree.V_Old.push_back((tree.V[i])); + } + + for (int i = 0; i < tree.E.size(); i++) + { + tree.E_Old.push_back(tree.E[i]); } - // Initialize the Vertex Queue; - //tree.QV = tree.V; // C++ is dumb so this doesnt work with shared pointer vectors, need to do the following instead + // Initialize the Vertex Queue with all nearby vertices in the tree; + //tree.QV = tree.V; // C++ is dumb so this doesnt work with shared pointer vectors, need to do the following instead + //bool reject_vertex = false; + std::random_device rd; // Only used once to initialise (seed) engine + std::mt19937 rng(rd()); // Random-number engine used (Mersenne-Twister in this case) + std::uniform_int_distribution uni(1,100); // Guaranteed unbiased + //CLOG(ERROR, "path_planning.cbit_planner") << "Vertex Lookahead: "<< p_goal->p + dynamic_window_width + conf.sliding_window_freespace_padding+5; for (int i = 0; i < tree.V.size(); i++) { - tree.QV.push_back(std::shared_ptr (tree.V[i])); + // TODO: It may also be worth considering only adding a random subset of the vertex tree to the queue in times when batches are taking a long time + //if (compute_time >= 200) + //{ + auto random_integer = uni(rng); + random_integer = static_cast (random_integer); + //reject_vertex = static_cast (random_integer); + //CLOG(DEBUG, "path_planning.cbit_planner") << "Compute Falling Behind, Vertices, reject vertex: " << reject_vertex; + //} + //else + //{ + // reject_vertex = false; + //} + + // if we have no 1st batch solution (we are in the first iteration or have just reset), add the whole tree to the queue + if (k == 0) + { + tree.QV.push_back(tree.V[i]); + } + // Otherwise, only add the portions of the tree within the sliding window to avoid processing preseeded vertices which are already optimal + //else if (((tree.V[i]->p) <= p_goal->p + dynamic_window_width + (conf.sliding_window_freespace_padding*2)) && ((vertex_rej_prob / random_integer) >= 1.0)) + else if (((vertex_rej_prob / random_integer) >= 1.0)) // for some reason using the lookahead queue doesnt work reliably for collisions, not sure why, need to investigate + { + tree.QV.push_back(tree.V[i]); + } + } + CLOG(DEBUG, "path_planning.cbit_planner") << "QV size after batch: " << tree.QV.size(); // TODO: Dynamic Expansion radius selection: - /* - if (p_goal->g_T != INFINITY) - { - conf.initial_exp_rad = exp_radius((tree.V.size() + samples.size()), sample_box_height, sample_box_width, conf.eta); - } - */ + // Its debatable whether this works well or not, but going to try it + + //if (p_goal->g_T != INFINITY) + //{ + // conf.initial_exp_rad = exp_radius((tree.V.size() + samples.size()), sample_box_height, sample_box_width, conf.eta); + // CLOG(DEBUG, "path_planning.cbit_planner") << "New expansion radius is: " << conf.initial_exp_rad; + //} + } @@ -551,7 +735,9 @@ void CBITPlanner::Planning(vtr::path_planning::BasePathPlanner::RobotState& robo // TODO: Handle edge case where a perfect path results in empty queues (see python code) if (vm == NULL) { - break; + // If we just continue here, we enter a state where after every single iteration we will complete a batch, but I think this is fine + // It will just stay this way until we move off the path and have a non perfect path solution + continue; } // Remove the edge from the queue (I think best to do this inside BestInEdgeQueue function) @@ -571,7 +757,7 @@ void CBITPlanner::Planning(vtr::path_planning::BasePathPlanner::RobotState& robo if (vm->g_T_weighted + weighted_cost < xm->g_T_weighted) { // Check if xm is in the tree, if it is, we need to do some rewiring of any other edge which has xm as an endpoint - if (node_in_tree(*xm) == true) + if (node_in_tree_v2(xm) == true) { // TODO: Needs some additional wormhole logic in here which I havent done yet @@ -579,7 +765,7 @@ void CBITPlanner::Planning(vtr::path_planning::BasePathPlanner::RobotState& robo int upper_edge_index = tree.E.size(); for (int i = 0; i < upper_edge_index; i++) { - if (std::get<1>(tree.E[i])->p == xm->p && std::get<1>(tree.E[i])->q == xm->q) + if (std::get<1>(tree.E[i]) == xm) // changed to equating pointer addresses { // Once we process the vertex and have it ready to return, remove it from the edge queue (This is a fast way of doing so) @@ -601,7 +787,7 @@ void CBITPlanner::Planning(vtr::path_planning::BasePathPlanner::RobotState& robo // Remove it from the samples, add it to the vertex tree and vertex queue: for (int i = 0; i < samples.size(); i++) { - if (samples[i]->p == xm->p && samples[i]->q == xm->q) + if (samples[i] == xm) { auto it = samples.begin() + i; *it = std::move(samples.back()); @@ -617,23 +803,21 @@ void CBITPlanner::Planning(vtr::path_planning::BasePathPlanner::RobotState& robo // Set cost to comes xm->g_T_weighted = vm->g_T_weighted + weighted_cost; xm->g_T = vm->g_T + actual_cost; - + xm->parent = vm; // TODO: wormhole handling // Generate edge, create parent chain tree.E.push_back(std::tuple, std::shared_ptr> {vm, xm}); - - - xm->parent = vm; + // Filter edge queue as now any other edge with worse cost heuristic can be ignored int upper_queue_index = tree.QE.size(); for (int i = 0; i < upper_queue_index; i++) { Node v = *std::get<0>(tree.QE[i]); - Node x = *std::get<1>(tree.QE[i]); + std::shared_ptr x = std::get<1>(tree.QE[i]); - if (x.p == xm->p && x.q == xm->q && (v.g_T_weighted + calc_weighted_dist(v, *xm, conf.alpha) >= xm->g_T_weighted)) + if ((x == xm) && (v.g_T_weighted + calc_weighted_dist(v, *xm, conf.alpha) >= xm->g_T_weighted)) { auto it = tree.QE.begin() + i; *it = std::move(tree.QE.back()); @@ -661,13 +845,28 @@ void CBITPlanner::Planning(vtr::path_planning::BasePathPlanner::RobotState& robo } // End of main planning for loop + // Exiting cleanly: + // If we make it here and are no longer localized, we've reached end of path and should clear the bitstar path from memory + if (localization_flag == false) + { + CLOG(ERROR, "path_planning.cbit_planner") << "Reached End of Plan, Exiting cleanly"; + (*cbit_path_ptr).clear(); + + } + // FAULT AND EARLY MAIN LOOP EXIT HANDLING // If we make it here and are still localized, then try to reset the planner - if (localization_flag == true) - { - HardReset(robot_state, costmap_ptr); + //if (localization_flag == true) + //{ + //HardReset(robot_state, costmap_ptr); + + + + + + // The below code was all wrapped into HardReset, still experimental however. /* @@ -725,12 +924,16 @@ void CBITPlanner::Planning(vtr::path_planning::BasePathPlanner::RobotState& robo Planning(robot_state, costmap_ptr); */ - } - else - { + + + + + //} + //else + //{ // But if we arent localized, then there is really nothing else we can do but return an error and end the planning session - CLOG(ERROR, "path_planning.cbit_planner") << "Localization Failed, Terminating the Planning Instance"; - } + // CLOG(ERROR, "path_planning.cbit_planner") << "Localization Failed, Terminating the Planning Instance"; + //} } // End of main planning function @@ -750,7 +953,7 @@ std::vector> CBITPlanner::SampleBox(int m) double c_min = p_start->p - p_goal->p; double padding = (c_best - c_min) / 2.0; // Padding to apply to maintain the heuristic ellipse additional width for probabilstic completeness double lookahead = dynamic_window_width; - double box_tolerance = 0.1; // Need to add a little extra height to the box when using ROC regions + double box_tolerance = 0.1; // Need to add a little extra height to the box when using ROC regions TODO make this config param // Calculate dimensions double q_max = *std::max_element(path_y.begin(), path_y.end()) + box_tolerance; // apparently this function can be abit slow, so be aware @@ -758,7 +961,13 @@ std::vector> CBITPlanner::SampleBox(int m) q_max = std::max(fabs(q_max), fabs(q_min)); - double p_max = p_goal->p + lookahead - padding; + // maintain a minimum sample box height, this prevents sample density from becoming too ridiculous when following the path + if (q_max < 0.5) // TODO make this config param + { + q_max = 0.5; + } + + double p_max = p_goal->p + lookahead + padding; double p_zero = p_goal->p - padding; //std::cout << "p_max: " << p_max << std::endl; @@ -839,7 +1048,7 @@ std::vector> CBITPlanner::SampleFreeSpace(int m) // In the python version I do this line thing which is more robust, but for now im going to do this quick and dirty double p_step = 0.25; double p_val = p_zero; // needed to modify the starting i to be p_zero - for (int i = 0; i < (pre_seeds); i++) + for (int i = 0; i < (pre_seeds-1); i++) { Node node((p_val+p_step), 0); @@ -909,34 +1118,27 @@ void CBITPlanner::Prune(double c_best, double c_best_weighted) } // We also check the tree and add samples for unconnected vertices back to the sample set - // paper says this is to maintain uniform sample density, but practically I dont see how any tree vertex would have infinite cost to come? + //We also do a prune of the vertex tree by heuristic //&& (samples[i]->g_T != INFINITY)) - for (int i = 0; i < tree.V.size(); i++) + std::vector> vertex_pruned; + int vertices_size = tree.V.size(); + for (int i = 0; i < vertices_size; i++) { if (tree.V[i]->g_T_weighted == INFINITY) { samples_pruned.push_back(std::shared_ptr (tree.V[i])); } - } - // After we do both the above loops update the sample vector - samples = samples_pruned; - - - - // Similar prune of the Vertex Tree - // TODO: Wormhole accomodations - - std::vector> vertex_pruned; - for (int i = 0; i g_T_weighted < INFINITY)) { vertex_pruned.push_back(std::shared_ptr (tree.V[i])); } } + // After we do both the above loops update the sample vector + samples = samples_pruned; tree.V = vertex_pruned; - + + // TODO: Wormhole accomodations // Similar Prune of the Edges // TODO:Wormhole accomodations @@ -1027,7 +1229,10 @@ std::shared_ptr CBITPlanner::UpdateState() int q_sign = sgn((B.p - A.p) * (new_state->y - A.q) - ((B.q - A.q) * (new_state->x - A.p))); q_min = q_min * q_sign; - //std::cout << "q_min is: " << q_min << std::endl; + //std::cout << "q_min is: " << q_min << std::endl; // debug; + + // Note I think we also need to take into account the direction the robot is facing on the path for reverse planning too + // Once we have the closest point on the path, it may not actually be the correct p-value because of singularities in the euclid to curv conversion // We need to use this points p-value as a starting point, then search p, qmin space in either direction discretely and find the point with @@ -1180,17 +1385,21 @@ void CBITPlanner::ExpandVertex(std::shared_ptr v) } } - // DEBUG + // I found the below code, while does follow the original algorithm, tends not to hurt compute performance + // yet allows the pre-seeded samples to much much better in subsequent iterations + // Removing this lets the tree use old verticies to find better paths more often (but subequently means you check more) + /* // First check to see that v is not in the old tree already, if it is return early for (int i = 0; i < tree.V_Old.size(); i++) { - if ((v->p == tree.V_Old[i]->p) && (v->q == tree.V_Old[i]->q)) + if (v == tree.V_Old[i]) { //CLOG(WARNING, "path_planning.cbit_planner") << "Returning early, expansion vertex is in V_old"; // For debug return; } } - // END OF DEBUG + */ + // find nearby vertices and filter by heuristic potential for (int i = 0; i < tree.V.size(); i++) @@ -1200,6 +1409,15 @@ void CBITPlanner::ExpandVertex(std::shared_ptr v) //auto test3 = (v->g_T_weighted + calc_weighted_dist(*v, *tree.V[i], conf.alpha)); if ((calc_dist(*tree.V[i], *v) <= conf.initial_exp_rad) && ((g_estimated_admissible(*v, *p_start) + calc_weighted_dist(*v, *tree.V[i], conf.alpha) + h_estimated_admissible(*tree.V[i], *p_goal)) < p_goal->g_T_weighted) && ((v->g_T_weighted + calc_weighted_dist(*v, *tree.V[i], conf.alpha)) < tree.V[i]->g_T_weighted)) { + if (edge_in_tree_v2(v, tree.V[i]) == false) + { + // If all conditions satisfied, add the edge to the queue + //tree.V[i]->g_T = INFINITY; // huh actually looking at this again, im not sure if I should be doing this (yeah I think was a mistake) + //tree.V[i]->g_T_weighted = INFINITY; // huh actually looking at this again, im not sure if I should be doing this (yeah I think its a mistake) + tree.QE.push_back(std::tuple, std::shared_ptr> {(v), (tree.V[i])}); + + } + /* // Also check whether the edge is in the tree or not if (edge_in_tree(*v, *tree.V[i]) == false) { @@ -1208,9 +1426,8 @@ void CBITPlanner::ExpandVertex(std::shared_ptr v) //tree.V[i]->g_T_weighted = INFINITY; //I think these two lines were a mistake, should not be here tree.QE.push_back(std::tuple, std::shared_ptr> {(v), (tree.V[i])}); - } - - } + */ + } } } @@ -1220,11 +1437,11 @@ std::tuple, std::shared_ptr> CBITPlanner::BestInEdge if (tree.QE.size() == 0) // need to handle a case where the return path is 100% optimal in which case things get stuck and need ot be flagged to break { //std::cout << "Edge Queue is Empty! Optimal Solution Found" << std::endl; - CLOG(WARNING, "path_planning.cbit_planner") << "Edge Queue is Empty, Solution Could Not be Improved This Batch"; - CLOG(WARNING, "path_planning.cbit_planner") << "Tree Size: " << tree.V.size() << " Vertex Queue Size: " << tree.QV.size() << " Sample Size: " <p << " q: " << tree.V[0]->q << " g_T_weighted: " << tree.V[0]->g_T_weighted; + CLOG(DEBUG, "path_planning.cbit_planner") << "Edge Queue is Empty, Solution Could Not be Improved This Batch"; + //CLOG(WARNING, "path_planning.cbit_planner") << "Tree Size: " << tree.V.size() << " Vertex Queue Size: " << tree.QV.size() << " Sample Size: " <p << " q: " << tree.V[0]->q << " g_T_weighted: " << tree.V[0]->g_T_weighted; - CLOG(WARNING, "path_planning.cbit_planner") << "Repair mode is: " << repair_mode; + //CLOG(WARNING, "path_planning.cbit_planner") << "Repair mode is: " << repair_mode; //CLOG(WARNING, "path_planning.cbit_planner") << "final sample is p: " << samples[samples.size()-1]->p; // note this can cause indexing errors in some cases @@ -1283,7 +1500,11 @@ std::tuple, std::vector> CBITPlanner::ExtractPath(vt { CLOG(DEBUG, "path_planning.cbit_planner") << "Something Went Wrong - Infinite Loop Detected, Initiating Hard Reset:"; //std::this_thread::sleep_for(std::chrono::milliseconds(2000)); //temp, remove this - HardReset(robot_state, costmap_ptr); + //HardReset(robot_state, costmap_ptr); + + + + /* CLOG(ERROR, "path_planning.cbit_planner") << "Node p:" << node.p << " q: " << node.q; CLOG(ERROR, "path_planning.cbit_planner") << "Node Parent p:" << (*node.parent).p << " q: " << (*node.parent).q; @@ -1493,7 +1714,6 @@ bool CBITPlanner::col_check_path() tree.E = pruned_edge_tree; - // TODO: potentially check if plotting is enabled, and if so, clear the current plot and basically reset and replot all the current samples, vertices, edges // (Might be a good idea to just make a function for this, it would probably come in handy) @@ -1525,6 +1745,52 @@ bool CBITPlanner::col_check_path() } +std::shared_ptr CBITPlanner::col_check_path_v2(double max_lookahead_p) +{ + + // Generate path to collision check + std::vector> curv_path; + std::shared_ptr node_ptr = p_goal; + curv_path.push_back(node_ptr); + + while ((node_ptr->parent != nullptr)) + { + node_ptr = node_ptr->parent; + curv_path.push_back(node_ptr); + } + + // find the first vertex which results in a collision (curv_path is generated from left to right, so we should search in reverse) + std::shared_ptr col_free_vertex = nullptr; + + // TODO, actually we probably only want to consider edges in the lookahead (not way far away down the path) but I might deal with this later + for (int i = curv_path.size()-1; i>=0; i--) // I decided to have it -3 instead of -1, this prevents us from collision checking the end of path, but I think I want that for now and it lets me in + { + Node vertex = *curv_path[i]; + // If the vertex in the path is outside of our sliding window then skip it + // This prevents us from having to rewire the path for distance obstacles outside of our window (especially in cases where our path crosses itself) + if (vertex.p > max_lookahead_p) + { + continue; + } + Node euclid_pt = curve_to_euclid(vertex); + //if (is_inside_obs(obs_rectangle, euclid_pt)) // Legacy collision checking + if (costmap_col_tight(euclid_pt)) + { + //col_free_vertex = curv_path[i+1]; // take the vertex just before the collision vertex + if (i+4 < curv_path.size()-1) + { + col_free_vertex = curv_path[i+4]; // im actually finding that this vertex may be a little too close to the obstacles. Better to take one slightly further ahead if we can + } + else + { + col_free_vertex = curv_path[i+1]; + } + } + + } + return col_free_vertex; +} + void CBITPlanner::restore_tree(double g_T_update, double g_T_weighted_update) @@ -1567,7 +1833,6 @@ void CBITPlanner::restore_tree(double g_T_update, double g_T_weighted_update) // Function which takes in a beginning vertex v, and an end vertex x, and checks whether its in the tree or not already -// C++ sucks so this is actually very annoying to do bool CBITPlanner::edge_in_tree(Node v, Node x) { for (int i = 0; i < tree.E.size(); i++ ) @@ -1585,12 +1850,27 @@ bool CBITPlanner::edge_in_tree(Node v, Node x) */ } return false; - +} +// Function which takes in a beginning vertex v, and an end vertex x, and checks whether its in the tree or not already +// This version uses address matching (safer) +bool CBITPlanner::edge_in_tree_v2(std::shared_ptr v, std::shared_ptr x) +{ + int edges_size = tree.E.size(); + for (int i = 0; i < edges_size; i++ ) + { + // Alternative way to do this using node addresses + if ((std::get<0>(tree.E[i]) == v) && (std::get<1>(tree.E[i]) == x)) + { + return true; + } + } + return false; } + // Function for checking whether a node lives in the Vertex tree bool CBITPlanner::node_in_tree(Node x) { @@ -1606,6 +1886,22 @@ bool CBITPlanner::node_in_tree(Node x) } +// Function for checking whether a node lives in the Vertex tree, updated to use address matching (safer) +bool CBITPlanner::node_in_tree_v2(std::shared_ptr x) +{ + int vertices_size = tree.V.size(); + for (int i = 0; i < vertices_size; i++ ) + { + + if (x == tree.V[i]) + { + return true; + } + } + return false; + +} + // Edge cost calculation with collision check double CBITPlanner::cost_col(std::vector> obs, Node start, Node end) @@ -1687,51 +1983,86 @@ bool CBITPlanner::is_inside_obs(std::vector> obs, Node node) { for (int i = 0; i < obs.size(); i++) { - double x = obs[i][0]; - double y = obs[i][1]; - double w = obs[i][2]; - double h = obs[i][3]; - //bool test1 = (0 <= (node.p - x) <= w); - //bool test2 = ((0 <= (node.p -x)) && ((node.p -x) <= w) && (0 <= (node.p -x)) && ((node.p -x) <= h)); - //bool test3 = ((0 <= (node.p -x)) && ((node.p -x) <= w)); - //bool test4 = ((0 <= (node.p -x)) && ((node.p -x) <= h)); - if ((0 <= (node.p - x)) && ((node.p - x) <= w) && (0 <= (node.q - y)) && ((node.q - y) <= h)) - { - return true; - } + + + double x = obs[i][0]; + double y = obs[i][1]; + double w = obs[i][2]; + double h = obs[i][3]; + //bool test1 = (0 <= (node.p - x) <= w); + //bool test2 = ((0 <= (node.p -x)) && ((node.p -x) <= w) && (0 <= (node.p -x)) && ((node.p -x) <= h)); + //bool test3 = ((0 <= (node.p -x)) && ((node.p -x) <= w)); + //bool test4 = ((0 <= (node.p -x)) && ((node.p -x) <= h)); + if ((0 <= (node.p - x)) && ((node.p - x) <= w) && (0 <= (node.q - y)) && ((node.q - y) <= h)) + { + return true; + } } return false; } -// DEBUG: Experimental costmap collision checking -bool CBITPlanner::costmap_col(Node node) + +// This collision check is only used at the end of each batch and determines whether the path should be rewired using the bare minimum obstacle distance +// Under normal operation we plan paths around a slightly more conservative buffer around each obstacle (equal to influence dist + min dist) +bool CBITPlanner::costmap_col_tight(Node node) { + //CLOG(DEBUG, "path_planning.cbit_planner") << "Original Node: x: " << node.p << " y: " << node.q << " z: " << node.z; + Eigen::Matrix test_pt({node.p, node.q, node.z, 1}); - // DEBUG: Make a spoofed obstacle to add to the costmap to make sure collision detection works - //std::pair fake_key(4.0, 0.0); - //float fake_value = 1.0; - //cbit_costmap_ptr->obs_map.insert(std::make_pair(fake_key,fake_value)); + // I am no longer temporally filtering here, instead this takes place in the costmap itself in the change detection module + // This means the costmap and transform size should never be more than 1 + for (int i = 0; i < cbit_costmap_ptr->T_c_w_vect.size(); i++) + { - //CLOG(DEBUG, "path_planning.cbit_planner") << "Original Node: x: " << node.p << " y: " << node.q << " z: " << node.z; - Eigen::Matrix test_pt({node.p, node.q, node.z, 1}); + auto collision_pt = cbit_costmap_ptr->T_c_w_vect[i] * test_pt; + // Round the collision point x and y values down to the nearest grid resolution so that it can be found in the obstacle unordered_map + float x_key = floor(collision_pt[0] / cbit_costmap_ptr->grid_resolution) * cbit_costmap_ptr->grid_resolution; + float y_key = floor(collision_pt[1] / cbit_costmap_ptr->grid_resolution) * cbit_costmap_ptr->grid_resolution; + //CLOG(DEBUG, "path_planning.cbit_planner") << "X_key: " << x_key; + //CLOG(DEBUG, "path_planning.cbit_planner") << "Y_key: " << y_key; + + float grid_value; + + // Check to see if the point is in the obstacle map + // We need to use a try/catch in this metod as if the key value pair doesnt exist (it usually wont) we catch an out of range error + try + { + // Block of code to try + grid_value = cbit_costmap_ptr->obs_map_vect[i].at(std::pair (x_key, y_key)); + //CLOG(ERROR, "path_planning.cbit_planner") << "Key Value: " << grid_value; + } + catch (std::out_of_range) + { + grid_value = 0.0; + } + + if (grid_value >= 0.89) // By switching this from > 0.0 to 0.9, we effectively only collision check the path out to the "minimum_distance" obs config param + { + return true; + } + } + + // If we make it here can return false for no collision + return false; +} + + + +// More conservative costmap checking out to a distance of "influence_distance" + "minimum_distance" away +bool CBITPlanner::costmap_col(Node node) +{ + //CLOG(DEBUG, "path_planning.cbit_planner") << "Original Node: x: " << node.p << " y: " << node.q << " z: " << node.z; + Eigen::Matrix test_pt({node.p, node.q, node.z, 1}); - // Experimental, temporal filter (iterate through a sliding window of collision maps) - // For the moment what it does is collision check a history of 5 maps, and if any of those maps result in a collision, return collision - // Should make fluttery obstacles stay in place for about a second now, but the downside is false positives will also hang around, so may need to deal with this still - // Maybe by taking a vote? idk lets see how fast this is first - - //bool collision_result = false; - int vote_counter = 0; for (int i = 0; i < cbit_costmap_ptr->T_c_w_vect.size(); i++) { //auto collision_pt = cbit_costmap_ptr->T_c_w * test_pt; auto collision_pt = cbit_costmap_ptr->T_c_w_vect[i] * test_pt; - //CLOG(DEBUG, "path_planning.cbit_planner") << "Displaying the point in the costmap frame we are trying to collision check: " << collision_pt; //CLOG(DEBUG, "path_planning.cbit_planner") << "X: " << collision_pt[0]; //CLOG(DEBUG, "path_planning.cbit_planner") << "Y: " << collision_pt[1]; @@ -1748,37 +2079,31 @@ bool CBITPlanner::costmap_col(Node node) float grid_value; // Check to see if the point is in the obstacle map - // Note may just make more sense to bring in the returns to this try/catch - try { - // Block of code to try - //grid_value = cbit_costmap_ptr->obs_map.at(std::pair (x_key, y_key)); - grid_value = cbit_costmap_ptr->obs_map_vect[i].at(std::pair (x_key, y_key)); - - //CLOG(ERROR, "path_planning.cbit_planner") << "Key Value: " << grid_value; + // If it isnt in the map we will catch an out of range error + try + { + grid_value = cbit_costmap_ptr->obs_map_vect[i].at(std::pair (x_key, y_key)); + //CLOG(DEBUG, "debug") << " The Grid value for this cell is: " << grid_value; } - catch (std::out_of_range) { - // Block of code to handle errors + + catch (std::out_of_range) + { + grid_value = 0.0; - //CLOG(ERROR, "path_planning.cbit_planner") << "NO COLLISION!!!"; } if (grid_value > 0.0) - { - vote_counter = vote_counter + 1; - //return true; - } - - if (vote_counter >= 3)// Magic number for now - { - return true; - } + { + return true; + } } + // If we make it here, return false for no collision return false; } - +// note with new collision checker from global costmap I dont think we use this obs anymore? bool CBITPlanner::discrete_collision(std::vector> obs, double discretization, Node start, Node end) { // We dynamically determine the discretization based on the length of the edge @@ -1824,4 +2149,220 @@ bool CBITPlanner::discrete_collision(std::vector> obs, doubl +// Corridor Update functions +// DEBUG: Long term when I restructure things I want these to be inside the generate_pq.cpp file +// The only thing keeping me from doing this right away is that all the curvilinear conversions and collision checkers are here +// But long term I should move all those over to utils or even a different header and script + +struct CBITPlanner::collision_result CBITPlanner::discrete_collision_v2(double discretization, Node start, Node end) +{ + // We dynamically determine the discretization based on the length of the edge + discretization = round(calc_dist(start, end) * discretization); + + // Generate discretized test nodes + std::vector p_test; + std::vector q_test; + + double p_step = fabs(end.p - start.p) / discretization; + double q_step = fabs(end.q - start.q) / discretization; + + p_test.push_back(start.p); + q_test.push_back(start.q); + + for (int i = 0; i < discretization-1; i++) + { + p_test.push_back(p_test[i] + p_step*sgn(end.p-start.p) ); + q_test.push_back(q_test[i] + q_step*sgn(end.q-start.q) ); + } + p_test.push_back(end.p); + q_test.push_back(end.q); + + + + // Loop through the test curvilinear points, convert to euclid, collision check obstacles + Node curv_pt; + for (int i = 0; i < p_test.size(); i++) + { + curv_pt = Node(p_test[i], q_test[i]); + + // Convert to euclid TODO: + //Node euclid_pt = curv_pt; // DEBUG DO NOT LEAVE THIS HERE, NEED TO REPLACE WITH COLLISION CHECK FUNCTION + Node euclid_pt = curve_to_euclid(curv_pt); + //if (is_inside_obs(obs, euclid_pt)) + if (costmap_col(euclid_pt)) + { + return {true, curv_pt}; + } + } + + return {false, curv_pt}; +} + +// dont love how this function is coded, could be done much more efficiently with some more effort I think +void CBITPlanner::update_corridor(std::shared_ptr corridor, std::vector homotopy_p, std::vector homotopy_q, Node robot_state) +{ + // Reset q_left and q_right (I did this on the python side, but I dont know why exactly, in theory I should be able to just + // update it incrementally in a sliding window? I must have had a reason for it but ill leave it out for now). + // Ah the reason i did this was because I was not handling the no collision case (in which we reset q_left to q_right for that bin to the max) + // Resetting at the beginning prevents needing to do this, but I think I prefer the way im doing it here. + + // Take a subset of the p_bins based on the current robot state and our dynamic window + std::vector p_bins_subset; + + // Note there is probably a much better way to do this which is faster exploiting that the vector is sorted, but for now just doing this for convenience + for (int i = 0; i < corridor->p_bins.size(); i++) + { + double p_bin = corridor->p_bins[i]; + if ((p_bin >= robot_state.p) && (p_bin <= robot_state.p + corridor->sliding_window_width)) + { + p_bins_subset.push_back(p_bin); + } + // Exit early if the p values become larger then the window + if (p_bin > robot_state.p + corridor->sliding_window_width) + { + break; + } + } + + + // Iterate through each of the subset of p_bins + double p_upper; + double p_lower; + double q_upper; + double q_lower; + for (int i = 0; i < p_bins_subset.size(); i++) + { + int ind_counter = 0; + + // iterate through the current path solution (in p,q space) + for (int j = 0; j < homotopy_p.size(); j++) + { + // If the point on the path is just larger then the p_bin_subset, take that point and the previous one, interpolate a q_bin value at the place + if (homotopy_p[j] >= p_bins_subset[i]) + { + p_upper = homotopy_p[ind_counter]; + p_lower = homotopy_p[ind_counter - 1]; + q_upper = homotopy_q[ind_counter]; + q_lower = homotopy_q[ind_counter - 1]; + break; + } + ind_counter = ind_counter + 1; + } + + double q_bin = q_lower + ((p_bins_subset[i] - p_lower) / (p_upper - p_lower)) * (q_upper - q_lower); + //CLOG(DEBUG, "path_planning.corridor_debug") << "q_bin is: " << q_bin; + Node start = Node(p_bins_subset[i], q_bin); // starting point for collision check + Node end_left = Node(p_bins_subset[i], corridor->q_max + 0.01); // end point for 1st collision check + a small buffer + Node end_right = Node(p_bins_subset[i], (-1.0 * corridor->q_max - 0.01)); // end point for 2nd collision check + a small buffer + //CLOG(DEBUG, "path_planning.corridor_debug") << "start node is p: " << start.p << " q: " << start.q; + //CLOG(DEBUG, "path_planning.corridor_debug") << "end_left node is p: " << end_left.p << " q: " << end_left.q; + + // Note long term we need to handle special case when the start point is in a wormhole region, but for now should be fine + //Node euclid_start = CBITPlanner::curve_to_euclid(start); + + + // debug, testing to see how often the bit* point is in a collision by the time we get here (in theory this should never happen but I think it is) + auto test_check = discrete_collision_v2(corridor->curv_to_euclid_discretization, start, start); + if (test_check.bool_result == true) + { + CLOG(WARNING, "path_planning.corridor_debug") << "Something has gone wrong, cbit path has a collision, ignoring this p_bin update"; + break; + } + + // collision check left and right using a special version of discrete_collision check + // In this version we output both a boolean and the 1st point that comes into collision if there is one + auto collision_check_result1 = discrete_collision_v2(corridor->curv_to_euclid_discretization, start, end_left); + auto collision_check_result2 = discrete_collision_v2(corridor->curv_to_euclid_discretization, start, end_right); + + // if there is a collision, set q_left at the location of the current p_bin being processed to the value of q_left/q_right + if (collision_check_result1.bool_result == true) + { + //CLOG(DEBUG, "path_planning.corridor_debug") << "start node is p: " << start.p << " q: " << start.q; + //CLOG(DEBUG, "path_planning.corridor_debug") << "end_left node is p: " << end_left.p << " q: " << end_left.q; + double q_left = collision_check_result1.col_node.q; + auto it = find(corridor->p_bins.begin(), corridor->p_bins.end(), p_bins_subset[i]); + if (it != corridor->p_bins.end()) + { + int index = it - corridor->p_bins.begin(); + corridor->q_left[index] = q_left; + CLOG(DEBUG, "path_planning.corridor_debug") << "Q_left is: " << q_left; + } + } + // else set it back to the maximums + else + { + double q_left = corridor->q_max; + auto it = find(corridor->p_bins.begin(), corridor->p_bins.end(), p_bins_subset[i]); + if (it != corridor->p_bins.end()) + { + int index = it - corridor->p_bins.begin(); + corridor->q_left[index] = q_left; + } + } + + // Repeat for the other side + + if (collision_check_result2.bool_result == true) + { + //CLOG(DEBUG, "path_planning.corridor_debug") << "start node is p: " << start.p << " q: " << start.q; + //CLOG(DEBUG, "path_planning.corridor_debug") << "end_right node is p: " << end_right.p << " q: " << end_right.q; + double q_right = collision_check_result2.col_node.q; + auto it = find(corridor->p_bins.begin(), corridor->p_bins.end(), p_bins_subset[i]); + if (it != corridor->p_bins.end()) + { + int index = it - corridor->p_bins.begin(); + corridor->q_right[index] = q_right; + CLOG(DEBUG, "path_planning.corridor_debug") << "Q_right is: " << q_right; + } + } + else + { + double q_right = -1.0 * corridor->q_max; + auto it = find(corridor->p_bins.begin(), corridor->p_bins.end(), p_bins_subset[i]); + if (it != corridor->p_bins.end()) + { + int index = it - corridor->p_bins.begin(); + corridor->q_right[index] = q_right; + } + } + + } + + + // Updating the full euclidean corridor vectors by iterating through all bins. + + // TODO: Make whether we do this a configurable parameter, I realised that I dont actually need this from the control perspective at all + // Its purely for visualization purposes and it will waste some compute (not much though so im not too concerned for everyday use) + + // Note for convenience I can do this in a separate loop, but really we could also be doing this incrementally the same way we update + // q_left/q_right. This requires a proper initialization of the euclid corridor in generate_pq.cpp, which is certainly possible + // But not currently because the curve_to_euclid is not in the utils package. When I change that we can do this. Until then, brute force it is. + + // Benchmarking the compute time for this operation since its likely much less efficient then it could be with the incremental approach + // Need to first clear out the old corridor, otherwise it just keeps stacking + corridor->x_left.clear(); + corridor->y_left.clear(); + corridor->x_right.clear(); + corridor->y_right.clear(); + //auto corridor_start_time = std::chrono::high_resolution_clock::now(); + for (int i = 0; i < corridor->p_bins.size(); i++) + { + Node euclid_left = curve_to_euclid(Node(corridor->p_bins[i],corridor->q_left[i])); + Node euclid_right = curve_to_euclid(Node(corridor->p_bins[i],corridor->q_right[i])); + + //CLOG(ERROR, "path_planning.corridor_debug") << "Euclid Left is x: " << euclid_left.p << " y: " << euclid_right.p; + corridor->x_left.push_back(euclid_left.p); + corridor->y_left.push_back(euclid_left.q); + corridor->x_right.push_back(euclid_right.p); + corridor->y_right.push_back(euclid_right.q); + } + + //auto corridor_stop_time = std::chrono::high_resolution_clock::now(); + //auto duration_corridor = std::chrono::duration_cast(corridor_stop_time - corridor_start_time); + //CLOG(ERROR, "path_planning.corridor_debug") << "Corridor Update Time: " << duration_corridor.count() << "ms"; + +} + + + diff --git a/main/src/vtr_path_planning/src/cbit/cbit_plotting.cpp b/main/src/vtr_path_planning/src/cbit/cbit_plotting.cpp new file mode 100644 index 000000000..413489b3f --- /dev/null +++ b/main/src/vtr_path_planning/src/cbit/cbit_plotting.cpp @@ -0,0 +1,117 @@ +#include "vtr_path_planning/cbit/cbit_plotting.hpp" + + +void initialize_plot() +{ + // Testing plots + matplotlibcpp::figure_size(700, 1400); // Larger more useful size for debugging + //matplotlibcpp::figure_size(350, 700); // For mini more practical version for everyday use + matplotlibcpp::ylim(-2, 7); + matplotlibcpp::xlim(-3, 3); + matplotlibcpp::title("Curvilinear Batch Informed Trees (C++)"); + matplotlibcpp::xlabel("q [m]"); + matplotlibcpp::ylabel("p [m]"); + + // TODO: Configure Title, Legend + + //matplotlibcpp::axis("equal"); // Didnt like the axis equal behaviour + matplotlibcpp::draw(); + matplotlibcpp::pause(0.001); + return; +} + +// Function for plotting all the edges in the current tree (Called at the conclusion of each batch) +// Note this will also get triggered following state updates and repairs as well +void plot_tree(Tree tree, Node robot_pq, std::vector path_p, std::vector path_q, std::vector> samples) +{ + // Clear the figure + matplotlibcpp::clf(); + + // Configure plotting keyword settings + std::unordered_map keywords_robot; + keywords_robot["color"] = "lime"; + + + // Set sliding window based on current robot state + matplotlibcpp::ylim((robot_pq.p - 1), (robot_pq.p + 11)); + matplotlibcpp::xlim(-3,3); // TODO: replace with param + + + // Iterate through the current tree, plot each edge + for (int i = 0; i < tree.E.size(); i++) + { + std::shared_ptr vm = std::get<0>(tree.E[i]); + std::shared_ptr xm = std::get<1>(tree.E[i]); + std::vector plot_p_edge = {vm->p,xm->p}; + std::vector plot_q_edge = {-1*vm->q,-1*xm->q}; + matplotlibcpp::plot(plot_q_edge, plot_p_edge, "c"); + } + // Need one stand in point to populate the legend + std::vector dummy_edge = {0}; + matplotlibcpp::named_plot("Edges", dummy_edge, dummy_edge, "c"); + + + // Plot the current path solution + // Invert the q values + for (int j= 0; j < path_q.size(); j++) + { + path_q[j] = path_q[j] * -1; + } + matplotlibcpp::named_plot("BIT* Path", path_q, path_p, "b"); + + + // Plot a hashed centre line for the taught path + std::vector taught_p = {0,(*tree.V[0]).p}; + std::vector taught_q = {0,0}; + matplotlibcpp::named_plot("Taught Path", taught_q, taught_p, "r--"); + + + // Plot samples (optional, but seems like compute is okay so why not) + // I find its actually abit distracting having the samples and it doesnt really help very much so commenting out for now + /* + for (int k= 0; k < samples.size(); k++) + { + Node sample = *(samples[k]); + std::vector plot_p_sample= {sample.p,sample.p}; + std::vector plot_q_sample = {-1*sample.q,-1*sample.q}; + matplotlibcpp::plot(plot_q_sample, plot_p_sample, ".k"); + } + */ + + // Plot the current robot state + matplotlibcpp::named_plot("Robot State", {-1*robot_pq.q}, std::vector {robot_pq.p}, ".g"); + matplotlibcpp::scatter(std::vector {-1*robot_pq.q}, std::vector {robot_pq.p}, 200.0, keywords_robot); // try to change colour, need to implement + + + // Format Axis/title + matplotlibcpp::title("Batch Informed Trees"); + matplotlibcpp::xlabel("q [m]"); + matplotlibcpp::ylabel("p [m]"); + matplotlibcpp::legend(); + + + // Display and show while continuing script (adds 1ms delay to draw) + //matplotlibcpp::draw(); + matplotlibcpp::pause(0.001); + return; +} + +// TODO: Experiment with a Euclidean version that I can display everything in. See whether I can discretize and convert all edges +// of the tree to euclidean space. + +// TODO: Test other plotting functions which update more regularly in places that are more relevant (like robot state updates, published path) +// I think you can do this, but the problem is clearing the figure, if the plots are asychronous this will be an issue. + +void plot_robot(Node robot_pq) +{ + // Configure plotting keyword settings + std::unordered_map keywords_robot; + keywords_robot["color"] = "lime"; + + // Plot the current robot state + matplotlibcpp::scatter(std::vector {-1*robot_pq.q}, std::vector {robot_pq.p}, 100.0, keywords_robot); // try to change colour, need to implement + + matplotlibcpp::draw(); + matplotlibcpp::pause(0.001); + return; +} \ No newline at end of file diff --git a/main/src/vtr_path_planning/src/cbit/generate_pq.cpp b/main/src/vtr_path_planning/src/cbit/generate_pq.cpp index ce74dc106..bc866773e 100644 --- a/main/src/vtr_path_planning/src/cbit/generate_pq.cpp +++ b/main/src/vtr_path_planning/src/cbit/generate_pq.cpp @@ -43,7 +43,7 @@ CBITPath::CBITPath(CBITConfig config, std::vector initial_path) p.push_back(p[i-1] + delta_p_calc(disc_path[i-1], disc_path[i], alpha)); } - CLOG(INFO, "path_planning.teb") << "Successfully Built a Path in generate_pq.cpp and Displayed log"; + CLOG(INFO, "path_planning.cbit") << "Successfully Built a Path in generate_pq.cpp and Displayed log"; } @@ -76,4 +76,47 @@ void CBITPath::spline_curvature() // TODO auto test = 1; +} + + +// Corridor Path constructor: +CBITCorridor::CBITCorridor(CBITConfig config, std::shared_ptr global_path_ptr) +{ + q_max = config.q_max; + sliding_window_width = config.sliding_window_width + config.sliding_window_freespace_padding; + curv_to_euclid_discretization = config.curv_to_euclid_discretization; + double length_p = global_path_ptr->p.back(); + int num_bins = ceil(length_p / config.corridor_resolution); + // Initialize vector lengths + p_bins.reserve(num_bins); + q_left.reserve(num_bins); + q_right.reserve(num_bins); + x_left.reserve(num_bins); + x_right.reserve(num_bins); + y_left.reserve(num_bins); + y_right.reserve(num_bins); + + // Initialize bins + p_bins = linspace(0, length_p, num_bins); + for (int i = 0; i < num_bins; i++) + { + q_left.push_back(config.q_max); + q_right.push_back(-1.0 * config.q_max); + } + + // I think its wise if here we also initialize the euclidean corridor points as well. This is annoying though because out curv to euclid + // is not in utils (yet). TODO I really should move all the collision checking things external and independant from the planner for this reason + + // For now I may instead brute force the euclid generate at the end of each corridor update, I feel like even a 50000 bin loop (2.5km) wouldnt take + // more then a few ms (I believe early experiments showed you could do like 4million loop iterations every second or so in c++) + + // debug prints to make sure this happened correctly + CLOG(DEBUG, "path_planning.corridor_debug") << "Length of P is: " << length_p; + CLOG(DEBUG, "path_planning.corridor_debug") << "Number of Bins is: " << num_bins; + CLOG(DEBUG, "path_planning.corridor_debug") << "P_bins are: " << p_bins; + CLOG(DEBUG, "path_planning.corridor_debug") << "Q_left is: " << q_left; + CLOG(DEBUG, "path_planning.corridor_debug") << "Q_right is: " << q_right; + CLOG(DEBUG, "path_planning.corridor_debug") << "Size of p_bins: " << p_bins.size(); + CLOG(DEBUG, "path_planning.corridor_debug") << "Size of q_left,q_right: " << q_left.size(); + } \ No newline at end of file diff --git a/main/src/vtr_path_planning/src/cbit/utils.cpp b/main/src/vtr_path_planning/src/cbit/utils.cpp index c39e45946..fea689a3a 100644 --- a/main/src/vtr_path_planning/src/cbit/utils.cpp +++ b/main/src/vtr_path_planning/src/cbit/utils.cpp @@ -216,14 +216,15 @@ double exp_radius(double q, double sample_box_height, double sample_box_width, d */ double d = 2; - double lambda_x = sample_box_height + sample_box_width; + double lambda_x = sample_box_height * sample_box_width; double zeta = M_PI; double radius = 2.0 * eta * (pow((1.0 + (1.0/d)),(1.0/d))) * (pow((lambda_x/zeta),0.5)) * (pow((log(q) / q),(1.0/d))); - std::cout << "Expansion Radius: " << radius << std::endl; + //std::cout << "Expansion Radius: " << radius << std::endl; return radius; } // Moving this to cbit.cpp so I dont need to pass the whole giant path vector every time I call +// though actually I think long term I may just want to pass the pointer and still do it in utils, I want to clean up what is in cbitplanner /* // Function for converting a p,q coordinate value into a euclidean coordinate using the pre-processed path to follow Node curve_to_euclid(Node node, std::vector<) diff --git a/main/src/vtr_path_planning/src/mpc/mpc_path_planner.cpp b/main/src/vtr_path_planning/src/mpc/mpc_path_planner.cpp index db9b5cc25..7945fa2dd 100644 --- a/main/src/vtr_path_planning/src/mpc/mpc_path_planner.cpp +++ b/main/src/vtr_path_planning/src/mpc/mpc_path_planner.cpp @@ -13,7 +13,7 @@ // limitations under the License. /** - * \file mpc_path_planner.cpp + * \file mpc.cpp * \author Jordy Sehn, Autonomous Space Robotics Lab (ASRL) */ #include "vtr_path_planning/mpc/mpc_path_planner.hpp" @@ -45,6 +45,15 @@ struct mpc_result SolveMPC(Eigen::Matrix previous_vel, lgmath::se3 0, 0, 0, -1; + // Lateral constraint projection matrices (Experimental) + Eigen::Matrix I_4; + I_4 << 0, + 0, + 0, + 1; + Eigen::Matrix I_2_tran; + I_2_tran << 0, 1, 0, 0; + // Setup shared loss functions and noise models for all cost terms const auto sharedLossFunc = steam::L2LossFunc::MakeShared(); @@ -61,6 +70,12 @@ struct mpc_result SolveMPC(Eigen::Matrix previous_vel, lgmath::se3 const auto sharedKinNoiseModel = steam::StaticNoiseModel<6>::MakeShared(kin_noise_vect); + // Experimental lat constraint (hardcoded for now) + Eigen::Matrix lat_noise_vect; + lat_noise_vect << 1.0; + const auto sharedLatNoiseModel = + steam::StaticNoiseModel<1>::MakeShared(lat_noise_vect); + // Generate STEAM States for the velocity vector and SE3 state transforms @@ -74,12 +89,13 @@ struct mpc_result SolveMPC(Eigen::Matrix previous_vel, lgmath::se3 // Pushback the initial states (current robot state) pose_states.push_back(T0_inv); // Change this to T0 when implementing on robot, T0_1 for debug //vel_states.push_back(std::vector {0.0, 0.0}); //I think a single line way t odo this is something like Eigen::Matrix::Zero() - vel_states.push_back(v0); + vel_states.push_back(v0); // Set the remaining states for (int i=0; i previous_vel, lgmath::se3 for (int i=0; i previous_vel, lgmath::se3 //const auto vel_cost_term = steam::WeightedLeastSqCostTerm<2>::MakeShared(vel_state_vars[i], sharedVelNoiseModel, sharedLossFunc); //opt_problem.addCostTerm(vel_cost_term); + + + // Kinematic constraints (softened but penalized heavily) + if (i < (K-1)) + { + const auto lhs = steam::se3::ComposeInverseEvaluator::MakeShared(pose_state_vars[i+1], pose_state_vars[i]); + const auto vel_proj = steam::vspace::MatrixMultEvaluator<6,2>::MakeShared(vel_state_vars[i], P_tran); // TODO, I guess this version of steam doesnt have this one, will need to do it myself + const auto scaled_vel_proj = steam::vspace::ScalarMultEvaluator<6>::MakeShared(vel_proj, DT); + const auto rhs = steam::se3::ExpMapEvaluator::MakeShared(scaled_vel_proj); + const auto kin_error_func = steam::se3::LogMapEvaluator::MakeShared(steam::se3::ComposeInverseEvaluator::MakeShared(lhs, rhs)); + const auto kin_cost_term = steam::WeightedLeastSqCostTerm<6>::MakeShared(kin_error_func, sharedKinNoiseModel, sharedLossFunc); + opt_problem.addCostTerm(kin_cost_term); + + // Experimental velocity set-point constraint (instead of non zero velocity penalty) // Only add this cost term if we are not in point stabilization mode (end of path) - if (point_stabilization == false) - { - const auto vel_cost_term = steam::WeightedLeastSqCostTerm<2>::MakeShared(steam::vspace::VSpaceErrorEvaluator<2>::MakeShared(vel_state_vars[i],v_ref), sharedVelNoiseModel, sharedLossFunc); - opt_problem.addCostTerm(vel_cost_term); - } + //if (point_stabilization == false) + //{ + // const auto vel_cost_term = steam::WeightedLeastSqCostTerm<2>::MakeShared(steam::vspace::VSpaceErrorEvaluator<2>::MakeShared(vel_state_vars[i],v_ref), sharedVelNoiseModel, sharedLossFunc); + // opt_problem.addCostTerm(vel_cost_term); + //} + // Experimental acceleration limits + if (i == 0) { // On the first iteration, we need to use an error with the previously applied control command state @@ -138,27 +176,30 @@ struct mpc_result SolveMPC(Eigen::Matrix previous_vel, lgmath::se3 const auto accel_diff = steam::vspace::AdditionEvaluator<2>::MakeShared(vel_state_vars[i], steam::vspace::NegationEvaluator<2>::MakeShared(vel_state_vars[i-1])); const auto accel_cost_term = steam::WeightedLeastSqCostTerm<2>::MakeShared(accel_diff, sharedAccelNoiseModel, sharedLossFunc); opt_problem.addCostTerm(accel_cost_term); - } + } + + } - // Kinematic constraints (softened but penalized heavily) - if (i < (K-1)) - { - const auto lhs = steam::se3::ComposeInverseEvaluator::MakeShared(pose_state_vars[i+1], pose_state_vars[i]); - const auto vel_proj = steam::vspace::MatrixMultEvaluator<6,2>::MakeShared(vel_state_vars[i], P_tran); // TODO, I guess this version of steam doesnt have this one, will need to do it myself - const auto scaled_vel_proj = steam::vspace::ScalarMultEvaluator<6>::MakeShared(vel_proj, DT); - const auto rhs = steam::se3::ExpMapEvaluator::MakeShared(scaled_vel_proj); - const auto kin_error_func = steam::se3::LogMapEvaluator::MakeShared(steam::se3::ComposeInverseEvaluator::MakeShared(lhs, rhs)); - const auto kin_cost_term = steam::WeightedLeastSqCostTerm<6>::MakeShared(kin_error_func, sharedKinNoiseModel, sharedLossFunc); - opt_problem.addCostTerm(kin_cost_term); - } + + // Experimental Hard Lateral Constraints + /* + CLOG(DEBUG, "debug") << "Attempting to add a lateral cost term"; + const auto inv_state = steam::se3::InverseEvaluator::MakeShared(pose_state_vars[i]); + const auto pose_err = steam::vspace::MatrixMultEvaluator<4,4>::MakeShared(inv_state, measurements[i]); // I think this line is the problem, we cannot really convert the se3 evaluators to vspace evaluators + const auto left_mul = steam::vspace::MatrixMultEvaluator<1,4>::MakeShared(pose_err, I_2_tran); + const auto right_mul = steam::vspace::MatrixMultEvaluator<4,1>::MakeShared(pose_err, I_4); + const auto lat_cost_term = steam::WeightedLeastSqCostTerm<6>::MakeShared(right_mul, sharedLatNoiseModel, sharedLossFunc); + opt_problem.addCostTerm(lat_cost_term); + CLOG(DEBUG, "debug") << "Successfully added Lat cost term"; + */ } // Solve the optimization problem with GuassNewton solver using SolverType = steam::GaussNewtonSolver; // Initialize parameters (enable verbose mode) SolverType::Params params; - params.verbose = false; // Makes the output display for debug + params.verbose = false; // Makes the output display for debug when true SolverType solver(opt_problem, params); solver.optimize(); @@ -242,152 +283,139 @@ struct meas_result GenerateReferenceMeas(std::shared_ptr> cbit // Save a copy of the current path solution to work on auto cbit_path = *cbit_path_ptr; + // Experimental Changes to improve controller instability (completed but not rigourously field tested yet) - // Take in the current euclidean path solution from the cbit planner in the world frame, the current robot state, and determine - // which measurements we wish to track to follow the path at the desired target velocity - CLOG(DEBUG, "mpc_debug.cbit") << "Reference Path Points:"; - std::vector measurements; - double starting_dist = INFINITY; + // PSEUDO CODE: + // 1. Find the closest point on the cbit path to the current state of the robot + // 2. Using K, DT, VF, we generate a vector of "p" values that we want to create Euclidean measurements for (we know these up front) + // 3. After we have our starting closest point on the path, assign that point a p value of 0. Compute the p values for each subsequent point in the lookahead window + // 4. using the desired p values, and the known p values, interpolate a, x,y,z,yaw, value each measurement + // 5. Create the proper measurement transform for each measurement and get it ready for the using with the optimization problem + + // Limiting the size of the cbit path based on the sliding window and then assigning p values + double lookahead_dist = 0.0; + double p_dist = 0.0; + + double min_dist = INFINITY; double new_dist; double dx; double dy; - double delta_dist = 0; - double index_counter = 0; - - // Find closest point on the path to the current state (new version, experimental) - double min_dist = INFINITY; - CLOG(DEBUG, "mpc_debug.cbit") << "Total CBIT Path Size: " << cbit_path.size(); - for (int i = 0; i < 200; i++) // magic number for now, corresponds to about a 5m lookahead (need to remove dependency on this) - { - // handle end of path case: - if (i >= cbit_path.size()) - { - break; - } - CLOG(DEBUG, "mpc_debug.cbit") << "x: " << (cbit_path)[i].x << "y: " << (cbit_path)[i].y << "z: " << (cbit_path)[i].z; + double p_correction = 0.0; + + std::vector cbit_p; + cbit_p.reserve(cbit_path.size()); + cbit_p.push_back(0.0); + for (int i = 0; i < (cbit_path.size()-2); i++) // the last value of vector is size()-1, so second to last will be size-2 + { + // calculate the p value for the point + p_dist = sqrt((((cbit_path)[i].x - (cbit_path)[i+1].x) * ((cbit_path)[i].x - (cbit_path)[i+1].x)) + (((cbit_path)[i].y - (cbit_path)[i+1].y) * ((cbit_path)[i].y - (cbit_path)[i+1].y))); + lookahead_dist = lookahead_dist + p_dist; + cbit_p.push_back(lookahead_dist); + + // Keep track of the closest point to the robot state dx = (cbit_path)[i].x - std::get<0>(robot_pose); dy = (cbit_path)[i].y - std::get<1>(robot_pose); new_dist = sqrt((dx * dx) + (dy * dy)); - CLOG(DEBUG, "mpc_debug.cbit") << "Dist to Pt: " << new_dist; if (new_dist < min_dist) { CLOG(DEBUG, "mpc_debug.cbit") << "Minimum Distance: " << new_dist; - index_counter = i; // I wonder if we should add some buffer, as this point could still be behind the current robot + p_correction = lookahead_dist; min_dist = new_dist; } - } - /* - // Find closest point on the path to the current state (Old version, mostly worked, but I think if the cbit path was not smooth there was a chance it would exit early before finding closest point) - while (delta_dist >= 0) - { - CLOG(DEBUG, "mpc_debug.cbit") << "x: " << (cbit_path)[index_counter].x << "y: " << (cbit_path)[index_counter].y << "z: " << (cbit_path)[index_counter].z; - dx = (cbit_path)[index_counter].x - std::get<0>(robot_pose); - dy = (cbit_path)[index_counter].y - std::get<1>(robot_pose); - new_dist = sqrt((dx * dx) + (dy * dy)); - delta_dist = starting_dist - new_dist; - CLOG(DEBUG, "mpc_debug.cbit") << "Dist to Pt: " << new_dist; - CLOG(DEBUG, "mpc_debug.cbit") << "Delta Dist: " << delta_dist; - if (delta_dist >= 0) - { - starting_dist = new_dist; - index_counter = index_counter + 1; - } - else + + // Stop once we get about 12m ahead of the robot (magic number for now, but this is just a conservative estimate of any reasonable lookahead window and mpc horizon) + if (lookahead_dist > 12.0) { - CLOG(DEBUG, "mpc_debug.cbit") << "Delta Dist Negative, Return i = " << index_counter-1; - index_counter = index_counter - 1; + break; } } - */ + //CLOG(DEBUG, "debug") << "cbit_p is: " << cbit_p; - // Keep iterating through the rest of the path, storing points in the path as measurements if they maintain an approximate - // forward path velocity of VF (//TODO, may need to also interpolate in some instances if we want to be very particular) - for (int i = index_counter; i < (cbit_path).size(); i++) + // Determine the p_values we need for our measurement horizon, corrected for the p value of the closest point on the path to the current robot state + std::vector p_meas_vec; + std::vector measurements; + p_meas_vec.reserve(K); + for (int i = 0; i < K; i++) { - // The first iteration we need to add the closest point to the initial position as a measurement - // Subesequent iterations we want to select points on the path to track carefully based on the desired velocity we want to meet. - - // Reset the measurement distance - double delta_dist = 0.0; - if (index_counter != i) - { - // scan the path into the future until we proceed approximately VF*DT meters forward longitudinally long the path - // The resulting indice of the path will be the one we use for the next measurement - while ((delta_dist <= (VF*DT)) && (i < (cbit_path).size())) // Need to add this and condition to handle end of path case - { - - double prev_x = (cbit_path)[i-1].x; - double prev_y = (cbit_path)[i-1].y; - double next_x = (cbit_path)[i].x; - double next_y = (cbit_path)[i].y; - delta_dist = delta_dist + sqrt(((next_x-prev_x) * (next_x-prev_x)) + ((next_y-prev_y) * (next_y-prev_y))); - i = i + 1; - } - // With the above setup, pretty sure the resulting i will be 1 too far when we break the loop, so we need to decrement it once at the end - i = i-1; - } + p_meas_vec.push_back((i * DT * VF) + p_correction); + } + //CLOG(DEBUG, "debug") << "p_meas_vec is: " << p_meas_vec; + // todo: Iterate through the p_measurements and interpolate euclidean measurements from the cbit_path and the corresponding cbit_p values + // Note this could be combined in the previous loop too + bool point_stabilization = false; + for (int i = 0; i < p_meas_vec.size(); i++) + { + // handle end of path case: + // if the p meas we would have needed exceeds the final measurement pose, set it equal to our last p value in the path + // This will cause the intepolation to return the final cbit_path pose for all measurements past this point + //CLOG(INFO, "debug") << "The specific p_meas_vec[i] is: " << p_meas_vec[i]; + //CLOG(INFO, "debug") << "The size of cbit_p is:" << cbit_p.size(); + //CLOG(INFO, "debug") << "The final cbit_p value is:" << cbit_p[cbit_p.size()-1]; + if (p_meas_vec[i] > cbit_p[cbit_p.size()-1]) + { + p_meas_vec[i] = cbit_p[cbit_p.size()-1]; + point_stabilization = true; // Enable point stabilization configs + CLOG(INFO, "mpc.cbit") << "Approaching End of Path, Converting MPC to Point Stabilization Problem"; + } + lgmath::se3::Transformation meas = InterpolateMeas(p_meas_vec[i], cbit_p, cbit_path); + // add to measurement vector + measurements.push_back(meas); + } - // Derive a yaw direction for each point based on the vector formed between the current point and the previous point on the path - double yaw = std::atan2(((cbit_path)[i].y - (cbit_path)[i-1].y), ((cbit_path)[i].x - (cbit_path)[i-1].x)); - //CLOG(DEBUG, "mpc_debug.cbit") << "The yaw of the path pt is: " << yaw; + return {measurements, point_stabilization}; - // Generate a reference Transformation matrix (ignores roll/pitch) - Eigen::Matrix4d T_ref; - T_ref << std::cos(yaw),-1*std::sin(yaw),0,(cbit_path)[i].x, - std::sin(yaw), std::cos(yaw),0,(cbit_path)[i].y, - 0, 0, 1,(cbit_path)[i].z, - 0, 0, 0, 1; - T_ref = T_ref.inverse().eval(); - measurements.push_back(lgmath::se3::Transformation(T_ref)); + //End of Experimental Changes - // Early break condition when the number of measurements we need is satisfied based on the horizon - if (measurements.size() == K) - { - break; - } - // TODO handle end of path case => will want to repeat the final measurements and turn problem into a point stabilization MPC. +} - } - - // If we reach the end of the path without generating enough measurements (approaching end of path), populate the measurements - // with the final point in the path (point stabilization problem) - if (measurements.size() < K) +// function takes in a the cbit path solution with a vector defining hte p axis of the path, and then a desired p_meas +// Then tries to output a euclidean pose interpolated for the desired p_meas. +lgmath::se3::Transformation InterpolateMeas(double p_val, std::vector cbit_p, std::vector cbit_path) +{ + // Find the lower bound of the p values + for (int i = 0; i < cbit_p.size(); i++) + { + if (cbit_p[i] < p_val) { - CLOG(WARNING, "mpc.cbit") << "Approaching End of Path, Converting MPC to Point Stabilization Problem"; - - // debug logging messages - /* - CLOG(ERROR, "mpc.cbit") << "Displaying all measurements so far:"; - CLOG(ERROR, "mpc.cbit") << "Size of cbit path is: " << cbit_path.size(); - CLOG(ERROR, "mpc.cbit") << "Size of measurements is: " << measurements.size(); - for (int k = 0; k < measurements.size(); k++) - { - CLOG(ERROR, "mpc.cbit") << "Measurement k: " << k << " is: " << measurements[k]; - } - - CLOG(ERROR, "mpc.cbit") << "The last valid transform was: " << measurements[measurements.size()-1]; - */ - for (int j = measurements.size(); j < K; j++) - { - measurements.push_back(measurements[j-1]); - //CLOG(ERROR, "mpc.cbit") << "Appending this transform to the measurements: " << measurements[j-1]; - } - - return {measurements, true}; + continue; } - else { - return {measurements, false}; - } + double p_lower = cbit_p[i-1]; + double p_upper = cbit_p[i]; + Pose pose_lower = cbit_path[i-1]; + Pose pose_upper = cbit_path[i]; + + double x_int = pose_lower.x + ((p_val - p_lower) / (p_upper - p_lower)) * (pose_upper .x - pose_lower.x); + double y_int = pose_lower.y + ((p_val - p_lower) / (p_upper - p_lower)) * (pose_upper .y - pose_lower.y); + double z_int = pose_lower.z + ((p_val - p_lower) / (p_upper - p_lower)) * (pose_upper .z - pose_lower.z); + + // For yaw we need to be abit careful about sign and angle wrap around + // Derive the yaw by creating the vector connecting the pose_upp and pose_lower pts + + double yaw_int = std::atan2((pose_upper.y - pose_lower.y), (pose_upper.x - pose_lower.x)); -} + // Build the transformation matrix + Eigen::Matrix4d T_ref; + T_ref << std::cos(yaw_int),-1*std::sin(yaw_int),0, x_int, + std::sin(yaw_int), std::cos(yaw_int),0, y_int, + 0, 0, 1, z_int, + 0, 0, 0, 1; + T_ref = T_ref.inverse().eval(); + + lgmath::se3::Transformation meas = lgmath::se3::Transformation(T_ref); + + CLOG(DEBUG, "mpc_debug.cbit") << "The measurement Euclidean state is - x: " << x_int << " y: " << y_int << " z: " << z_int << " yaw: " << yaw_int; + return meas; + } + } +} // Simple function for checking that the current output velocity command is saturated between our mechanical velocity limits @@ -409,32 +437,49 @@ Eigen::Matrix SaturateVel(Eigen::Matrix applied_vel, } */ - if (applied_vel(0) >= v_lim) + if (abs(applied_vel(0)) >= v_lim) { - command_lin_x = v_lim; - } - else if (applied_vel(0) <= 0.0) - { - command_lin_x = 0.0; + if (applied_vel(0) > 0.0) + { + command_lin_x = v_lim; + } + else if (applied_vel(0) < 0.0) + { + command_lin_x = -1.0* v_lim; + } } + // Removed for bi-directional control purposes + //else if (applied_vel(0) <= 0.0) + //{ + // command_lin_x = 0.0; + //} else { command_lin_x = applied_vel(0) ; } - if (applied_vel(1) >= w_lim) - { - command_ang_z = w_lim; - } - else if (applied_vel(1) <= -1*w_lim) + if (abs(applied_vel(1)) >= w_lim) { - command_ang_z = -1*w_lim; + if (applied_vel(1) > 0.0) + { + command_ang_z = w_lim; + } + else if (applied_vel(1) < 0.0) + { + command_ang_z = -1.0 * w_lim; + } } + //else if (applied_vel(1) <= -1*w_lim) + //{ + // command_ang_z = -1*w_lim; + //} else { command_ang_z = applied_vel(1) ; } + // Changes for Bi-directional path traversal, we no longer want to saturate commands less than 0.0 + saturated_vel << command_lin_x, command_ang_z; return saturated_vel; } @@ -467,6 +512,7 @@ Eigen::Matrix SaturateVel(Eigen::Matrix applied_vel, + \ No newline at end of file diff --git a/main/src/vtr_radar/src/modules/odometry/odometry_icp_module.cpp b/main/src/vtr_radar/src/modules/odometry/odometry_icp_module.cpp index 3b03476e6..d43dac769 100644 --- a/main/src/vtr_radar/src/modules/odometry/odometry_icp_module.cpp +++ b/main/src/vtr_radar/src/modules/odometry/odometry_icp_module.cpp @@ -549,10 +549,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, "radar.odometry_icp") << "T_m_r is: " << qdata.T_r_m_odo->inverse().vec().transpose(); - CLOG(WARNING, "radar.odometry_icp") << "w_m_r_in_r is: " << qdata.w_m_r_in_r_odo->transpose(); -#endif +//#if 1 +// CLOG(WARNING, "radar.odometry_icp") << "T_m_r is: " << qdata.T_r_m_odo->inverse().vec().transpose(); +// CLOG(WARNING, "radar.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(); diff --git a/main/src/vtr_tactic/src/tactic.cpp b/main/src/vtr_tactic/src/tactic.cpp index de41dd65a..5b0f1b66c 100644 --- a/main/src/vtr_tactic/src/tactic.cpp +++ b/main/src/vtr_tactic/src/tactic.cpp @@ -163,10 +163,23 @@ bool Tactic::passedSeqId(const uint64_t& sid) const { bool Tactic::routeCompleted() const { auto lock = chain_->guard(); - if (chain_->trunkSequenceId() < (chain_->sequence().size() - 1)) return false; const auto translation = chain_->T_leaf_trunk().r_ab_inb().norm(); + //const auto test = (chain_->pose(chain_->size()-1)).r_ab_inb().norm(); + //CLOG(ERROR, "tactic") << "My test pose error is: " << test; + //if (test < config_->route_completion_translation_threshold) // experimental jordy modification + //{ + // CLOG(ERROR, "tactic") << "Route Completetion Threshold is: " << translation; + // CLOG(ERROR, "tactic") << "My test pose error is: " << test; + // return true; + //} + if (chain_->trunkSequenceId() < (chain_->sequence().size() - 1)) + { + return false; + } if (translation > config_->route_completion_translation_threshold) + { return false; + } return true; } diff --git a/rviz/honeycomb.rviz b/rviz/honeycomb.rviz index c75d458bd..89519667a 100644 --- a/rviz/honeycomb.rviz +++ b/rviz/honeycomb.rviz @@ -11,16 +11,16 @@ Panels: - /localization path1 - /curr map loc1/Topic1 - /reference path1 + - /reference path1/Topic1 - /planned path1/Offset1 - /planned path (poses)1/Topic1 - /global path1 - /global path1/Offset1 - /planning change detection scan1 - /planning curr map loc1 - - /Path1 - - /Path2 + - /Corridor Path Left1/Topic1 Splitter Ratio: 0.4507042169570923 - Tree Height: 898 + Tree Height: 865 - Class: rviz_common/Selection Name: Selection - Class: rviz_common/Tool Properties @@ -89,7 +89,7 @@ Visualization Manager: robot planning (extrapolated): Value: false robot planning (extrapolated) mpc: - Value: true + Value: false world: Value: false world (offset): @@ -365,7 +365,7 @@ Visualization Manager: Head Length: 0.20000000298023224 Length: 1 Line Style: Billboards - Line Width: 0.05000000074505806 + Line Width: 0.07999999821186066 Name: reference path Offset: X: 0 @@ -992,27 +992,6 @@ Visualization Manager: Value: /vtr/safe_corridor_costmap_updates Use Timestamp: false Value: false - - Alpha: 0.699999988079071 - Class: rviz_default_plugins/Map - Color Scheme: costmap - Draw Behind: false - Enabled: true - Name: obstacle cost map - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /vtr/change_detection_costmap - Update Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /vtr/change_detection_costmap_updates - Use Timestamp: false - Value: true - Alpha: 1 Autocompute Intensity Bounds: true Autocompute Value Bounds: @@ -1205,6 +1184,104 @@ Visualization Manager: Reliability Policy: Reliable Value: /vtr/robot_path Value: true + - Alpha: 0.699999988079071 + Class: rviz_default_plugins/Map + Color Scheme: costmap + Draw Behind: false + Enabled: false + Name: obstacle cost map + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /vtr/change_detection_costmap + Update Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /vtr/change_detection_costmap_updates + Use Timestamp: false + Value: false + - Alpha: 0.699999988079071 + Class: rviz_default_plugins/Map + Color Scheme: raw + Draw Behind: false + Enabled: true + Name: filtered_obstacle_map + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /vtr/filtered_change_detection_costmap + Update Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /vtr/filtered_change_detection_costmap_updates + Use Timestamp: false + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz_default_plugins/Path + Color: 170; 0; 255 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Billboards + Line Width: 0.07999999821186066 + Name: Corridor Path Left + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /vtr/corridor_path_left + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz_default_plugins/Path + Color: 170; 0; 255 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Billboards + Line Width: 0.07999999821186066 + Name: Corridor Path Right + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /vtr/corridor_path_right + Value: true Enabled: true Global Options: Background Color: 181; 181; 181 @@ -1251,25 +1328,25 @@ Visualization Manager: Views: Current: Class: rviz_default_plugins/Orbit - Distance: 13.163816452026367 + Distance: 19.4349422454834 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false Focal Point: - X: -0.07232196629047394 - Y: 0.004224673379212618 - Z: 0.5842546820640564 + X: 0.722629964351654 + Y: 0.6375535130500793 + Z: -0.5986025333404541 Focal Shape Fixed Size: false Focal Shape Size: 0.05000000074505806 Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: 1.5697963237762451 + Pitch: 1.3647969961166382 Target Frame: robot Value: Orbit (rviz_default_plugins) - Yaw: 3.145707845687866 + Yaw: 4.070685863494873 Saved: - Class: rviz_default_plugins/Orbit Distance: 111.39698028564453 @@ -1474,10 +1551,10 @@ Visualization Manager: Window Geometry: Displays: collapsed: false - Height: 1043 + Height: 1016 Hide Left Dock: false Hide Right Dock: true - QMainWindow State: 000000ff00000000fd000000040000000000000281000003bdfc0200000007fc0000003d000001a20000000000fffffffaffffffff0100000002fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730000000000ffffffff0000008f00fffffffb0000001200530065006c0065006300740069006f006e0000000000000002530000006c00fffffffb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b000003bd000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c0000026100000001000001c00000030ffc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d0000030f000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000005000000006efc0100000002fb0000000800540069006d00650000000000000005000000024400fffffffb0000000800540069006d00650100000000000004500000000000000000000004f9000003bd00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd0000000400000000000002810000039efc0200000007fc0000003d000001a20000000000fffffffaffffffff0100000002fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730000000000ffffffff0000009d00fffffffb0000001200530065006c0065006300740069006f006e0000000000000002530000007400fffffffb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d0000039e000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c0000026100000001000001c0000003bdfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003b000003bd000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000005000000006efc0100000002fb0000000800540069006d0065000000000000000500000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000004b10000039e00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Time: @@ -1486,6 +1563,6 @@ Window Geometry: collapsed: false Views: collapsed: true - Width: 1920 - X: 1920 - Y: 0 + Width: 1848 + X: 1992 + Y: 27