From 84ec8231b577083255fda06daa5201a7679fff0e Mon Sep 17 00:00:00 2001 From: Alec Krawciw Date: Fri, 20 Jan 2023 13:44:24 -0500 Subject: [PATCH 01/77] Initial support for HDL-64. Added estimation of beam times to velodyne preprocessing --- config/honeycomb_grizzly_default.yaml | 20 ++++++----- launch/offline_honeycomb_grizzly.launch.yaml | 2 +- .../velodyne_conversion_module_v2.hpp | 2 ++ .../velodyne_conversion_module.cpp | 6 +++- .../velodyne_conversion_module_v2.cpp | 35 +++++++++++++++---- 5 files changed, 47 insertions(+), 18 deletions(-) diff --git a/config/honeycomb_grizzly_default.yaml b/config/honeycomb_grizzly_default.yaml index 88152b9e0..d67271812 100644 --- a/config/honeycomb_grizzly_default.yaml +++ b/config/honeycomb_grizzly_default.yaml @@ -7,6 +7,7 @@ - navigation - navigation.graph_map_server - navigation.command + - navigation.sensor_input # tactic - tactic - tactic.pipeline @@ -26,9 +27,9 @@ # pose graph - pose_graph # pipeline specific - # "lidar.pipeline", - # "lidar.honeycomb_converter", - # "lidar.preprocessing", + - lidar.pipeline + - lidar.velodyne_converter_v2 + - lidar.preprocessing # "lidar.odometry_icp", # "lidar.odometry_map_maintenance", # "lidar.vertex_test", @@ -43,8 +44,8 @@ # "lidar.terrain_assessment", robot_frame: base_link env_info_topic: env_info - lidar_frame: honeycomb - lidar_topic: /points + lidar_frame: velodyne + lidar_topic: /velodyne_points graph_map: origin_lat: 43.7822 origin_lng: -79.4661 @@ -87,15 +88,16 @@ localization: - recall - icp - - safe_corridor - - change_detection_sync + # - safe_corridor + # - change_detection_sync - memory submap_translation_threshold: 1.5 submap_rotation_threshold: 30.0 preprocessing: conversion: - type: lidar.honeycomb_converter_v2 + type: lidar.velodyne_converter_v2 visualize: true + estimate_time: true filtering: type: lidar.preprocessing num_threads: 8 @@ -116,7 +118,7 @@ odometry: icp: type: lidar.odometry_icp - use_trajectory_estimation: true + use_trajectory_estimation: false traj_num_extra_states: 0 traj_lock_prev_pose: false traj_lock_prev_vel: false diff --git a/launch/offline_honeycomb_grizzly.launch.yaml b/launch/offline_honeycomb_grizzly.launch.yaml index 5cde9165f..db2278c77 100644 --- a/launch/offline_honeycomb_grizzly.launch.yaml +++ b/launch/offline_honeycomb_grizzly.launch.yaml @@ -19,7 +19,7 @@ windows: ros2 launch vtr_navigation vtr.launch.py base_params:=honeycomb_grizzly_default.yaml data_dir:=${VTRTEMP}/lidar - start_new_graph:=true + start_new_graph:=false use_sim_time:=true # - htop # monitor hardware usage diff --git a/main/src/vtr_lidar/include/vtr_lidar/modules/preprocessing/conversions/velodyne_conversion_module_v2.hpp b/main/src/vtr_lidar/include/vtr_lidar/modules/preprocessing/conversions/velodyne_conversion_module_v2.hpp index a03dd84a3..a508b7007 100644 --- a/main/src/vtr_lidar/include/vtr_lidar/modules/preprocessing/conversions/velodyne_conversion_module_v2.hpp +++ b/main/src/vtr_lidar/include/vtr_lidar/modules/preprocessing/conversions/velodyne_conversion_module_v2.hpp @@ -41,6 +41,8 @@ class VelodyneConversionModuleV2 : public tactic::BaseModule { PTR_TYPEDEFS(Config); bool visualize = false; + bool estimate_time = false; + double angular_vel = 600*M_PI/30; static ConstPtr fromROS(const rclcpp::Node::SharedPtr &node, const std::string ¶m_prefix); diff --git a/main/src/vtr_lidar/src/modules/preprocessing/conversions/velodyne_conversion_module.cpp b/main/src/vtr_lidar/src/modules/preprocessing/conversions/velodyne_conversion_module.cpp index 2c8fdeebe..6a40ca032 100644 --- a/main/src/vtr_lidar/src/modules/preprocessing/conversions/velodyne_conversion_module.cpp +++ b/main/src/vtr_lidar/src/modules/preprocessing/conversions/velodyne_conversion_module.cpp @@ -58,7 +58,7 @@ auto VelodyneConversionModule::Config::fromROS( void VelodyneConversionModule::run_(QueryCache &qdata0, OutputCache &, const Graph::Ptr &, - const TaskExecutor::Ptr &) { + const TaskExecutor::Ptr &) { auto &qdata = dynamic_cast(qdata0); // Create a node for visualization if necessary @@ -76,11 +76,15 @@ void VelodyneConversionModule::run_(QueryCache &qdata0, OutputCache &, auto point_cloud = std::make_shared>(points.rows(), 1); + + CLOG(INFO, "lidar.velodyne_converter") << "Made PCL"; + for (size_t idx = 0; idx < (size_t)points.rows(); idx++) { // cartesian coordinates point_cloud->at(idx).x = points(idx, 0); point_cloud->at(idx).y = points(idx, 1); point_cloud->at(idx).z = points(idx, 2); + CLOG(DEBUG, "lidar.velodyne_converter") << points(idx, 0); // pointwise timestamp point_cloud->at(idx).timestamp = diff --git a/main/src/vtr_lidar/src/modules/preprocessing/conversions/velodyne_conversion_module_v2.cpp b/main/src/vtr_lidar/src/modules/preprocessing/conversions/velodyne_conversion_module_v2.cpp index 7923aa039..8797317f7 100644 --- a/main/src/vtr_lidar/src/modules/preprocessing/conversions/velodyne_conversion_module_v2.cpp +++ b/main/src/vtr_lidar/src/modules/preprocessing/conversions/velodyne_conversion_module_v2.cpp @@ -44,6 +44,14 @@ void velodyneCart2Pol(pcl::PointCloud &point_cloud) { } } +void estimateTime(pcl::PointCloud &point_cloud, int64_t time_offset, double angular_vel) { + for (size_t i = 0; i < point_cloud.size(); i++) { + auto &p = point_cloud[i]; + + p.timestamp = time_offset + static_cast(p.phi / angular_vel * 1e9); + } +} + } // namespace auto VelodyneConversionModuleV2::Config::fromROS( @@ -52,6 +60,9 @@ auto VelodyneConversionModuleV2::Config::fromROS( auto config = std::make_shared(); // clang-format off config->visualize = node->declare_parameter(param_prefix + ".visualize", config->visualize); + config->estimate_time = node->declare_parameter(param_prefix + ".estimate_time", config->estimate_time); + config->angular_vel = node->declare_parameter(param_prefix + ".angular_vel", config->angular_vel); + // clang-format on return config; } @@ -78,23 +89,33 @@ void VelodyneConversionModuleV2::run_(QueryCache &qdata0, OutputCache &, // iterators // clang-format off sensor_msgs::PointCloud2ConstIterator iter_x(*msg, "x"), iter_y(*msg, "y"), iter_z(*msg, "z"); - sensor_msgs::PointCloud2ConstIterator iter_time(*msg, "t"); - // clang-format on - for (size_t idx = 0; iter_x != iter_x.end(); - ++idx, ++iter_x, ++iter_y, ++iter_z, ++iter_time) { + ++idx, ++iter_x, ++iter_y, ++iter_z) { // cartesian coordinates point_cloud->at(idx).x = *iter_x; point_cloud->at(idx).y = *iter_y; point_cloud->at(idx).z = *iter_z; - - // pointwise timestamp - point_cloud->at(idx).timestamp = static_cast(*iter_time * 1e9); } + + // clang-format on + + // Velodyne has no polar coordinates, so compute them manually. velodyneCart2Pol(*point_cloud); + if (config_->estimate_time){ + CLOG(INFO, "lidar.velodyne_converter_v2") << "Timings wil be estimated from yaw angle"; + estimateTime(*point_cloud, *qdata.stamp, config_->angular_vel); + } else { + sensor_msgs::PointCloud2ConstIterator iter_time(*msg, "t"); + // pointwise timestamp + for (size_t idx = 0; iter_time != iter_time.end(); + ++idx, ++iter_time) { + point_cloud->at(idx).timestamp = static_cast(*iter_time * 1e9); + } + } + // Output qdata.raw_point_cloud = point_cloud; From 5110cbd21b4c0d3124e67b8a9fd065b73a1a358d Mon Sep 17 00:00:00 2001 From: Alec Krawciw Date: Fri, 20 Jan 2023 15:28:24 -0500 Subject: [PATCH 02/77] Improved messages parsing metadat yaml --- main/src/vtr_storage/src/storage/metadata_io.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/main/src/vtr_storage/src/storage/metadata_io.cpp b/main/src/vtr_storage/src/storage/metadata_io.cpp index 4323c9870..44d459fde 100644 --- a/main/src/vtr_storage/src/storage/metadata_io.cpp +++ b/main/src/vtr_storage/src/storage/metadata_io.cpp @@ -285,7 +285,7 @@ BagMetadata MetadataIo::read_metadata(const std::string & uri) } return metadata; } catch (const YAML::Exception & ex) { - throw std::runtime_error(std::string("Exception on parsing info file: ") + ex.what()); + throw std::runtime_error(std::string("Exception while parsing info file: " + get_metadata_file_name(uri) + " ") + ex.what()); } } From 2250ffec8db8c274e77a5068f515e4c4dbf5d631 Mon Sep 17 00:00:00 2001 From: Alec Krawciw Date: Mon, 23 Jan 2023 09:13:43 -0500 Subject: [PATCH 03/77] Run VTR through V HDL64 --- Dockerfile | 3 +- config/honeycomb_grizzly_default.yaml | 10 +- launch/offline_honeycomb_grizzly.launch.yaml | 2 +- .../vtr_storage/src/storage/metadata_io.cpp | 3 + rviz/lidar.rviz | 100 +++++++++++++----- 5 files changed, 85 insertions(+), 33 deletions(-) diff --git a/Dockerfile b/Dockerfile index c5c92fb9d..80f19093b 100644 --- a/Dockerfile +++ b/Dockerfile @@ -27,7 +27,8 @@ USER ${USERID}:${GROUPID} ENV VTRROOT=${HOMEDIR}/ASRL/vtr3 ENV VTRSRC=${VTRROOT}/src \ VTRDATA=${VTRROOT}/data \ - VTRTEMP=${VTRROOT}/temp + VTRTEMP=${VTRROOT}/temp \ + GRIZZLY=${VTRROOT}/grizzly ## Switch to root to install dependencies USER 0:0 diff --git a/config/honeycomb_grizzly_default.yaml b/config/honeycomb_grizzly_default.yaml index d67271812..c4fe2a185 100644 --- a/config/honeycomb_grizzly_default.yaml +++ b/config/honeycomb_grizzly_default.yaml @@ -89,8 +89,8 @@ - recall - icp # - safe_corridor - # - change_detection_sync - - memory + - change_detection_sync + #- memory submap_translation_threshold: 1.5 submap_rotation_threshold: 30.0 preprocessing: @@ -99,10 +99,10 @@ visualize: true estimate_time: true filtering: - type: lidar.preprocessing + type: lidar.preprocessing_v2 num_threads: 8 - crop_range: 40.0 - frame_voxel_size: 0.2 + crop_range: 20.0 + frame_voxel_size: 0.1 vertical_angle_res: 0.0132645 polar_r_scale: 2.0 r_scale: 4.0 diff --git a/launch/offline_honeycomb_grizzly.launch.yaml b/launch/offline_honeycomb_grizzly.launch.yaml index db2278c77..9e868c677 100644 --- a/launch/offline_honeycomb_grizzly.launch.yaml +++ b/launch/offline_honeycomb_grizzly.launch.yaml @@ -20,7 +20,7 @@ windows: base_params:=honeycomb_grizzly_default.yaml data_dir:=${VTRTEMP}/lidar start_new_graph:=false - use_sim_time:=true + use_sim_time:=false # - htop # monitor hardware usage diff --git a/main/src/vtr_storage/src/storage/metadata_io.cpp b/main/src/vtr_storage/src/storage/metadata_io.cpp index 44d459fde..6c04fbad6 100644 --- a/main/src/vtr_storage/src/storage/metadata_io.cpp +++ b/main/src/vtr_storage/src/storage/metadata_io.cpp @@ -186,6 +186,9 @@ struct convert> { Node node; node["nanoseconds_since_epoch"] = start_time.time_since_epoch().count(); + if (start_time.time_since_epoch().count() < 0) { + node["nanoseconds_since_epoch"] = 0; + } return node; } diff --git a/rviz/lidar.rviz b/rviz/lidar.rviz index 7e3f2c98c..7b066042d 100644 --- a/rviz/lidar.rviz +++ b/rviz/lidar.rviz @@ -6,14 +6,18 @@ Panels: Expanded: - /Global Options1 - /TF1 + - /TF1/Status1 + - /raw scan1 + - /filtered scan1 - /odometry path1/Shape1 - /curr map loc1/Topic1 - /planned path1/Offset1 - /planned path (poses)1/Topic1 - /global path1/Offset1 - /planning fake obs. scan1 + - /PointCloud21 Splitter Ratio: 0.4507042169570923 - Tree Height: 441 + Tree Height: 871 - Class: rviz_common/Selection Name: Selection - Class: rviz_common/Tool Properties @@ -23,14 +27,15 @@ Panels: Name: Tool Properties Splitter Ratio: 0.5886790156364441 - Class: rviz_common/Views - Expanded: ~ + Expanded: + - /Current View1 Name: Views Splitter Ratio: 0.5 - Class: rviz_common/Time Experimental: false Name: Time SyncMode: 0 - SyncSource: "" + SyncSource: raw scan Visualization Manager: Class: "" Displays: @@ -66,8 +71,12 @@ Visualization Manager: All Enabled: false lidar: Value: false + odo vertex frame: + Value: true robot: Value: true + velodyne: + Value: true world: Value: true world (offset): @@ -79,6 +88,11 @@ Visualization Manager: Show Names: false Tree: world: + odo vertex frame: + robot: + lidar: + velodyne: + {} world (offset): {} Update Interval: 0 @@ -90,7 +104,7 @@ Visualization Manager: Min Value: -10 Value: true Axis: Z - Channel Name: flex21 + Channel Name: flex24 Class: rviz_default_plugins/PointCloud2 Color: 255; 255; 255 Color Transformer: Intensity @@ -98,9 +112,9 @@ Visualization Manager: Enabled: false Invert Rainbow: false Max Color: 0; 255; 0 - Max Intensity: 0.09950866550207138 + Max Intensity: 0 Min Color: 255; 0; 0 - Min Intensity: -0.09992675483226776 + Min Intensity: 0 Name: raw scan Position Transformer: XYZ Selectable: true @@ -129,7 +143,7 @@ Visualization Manager: Color: 0; 255; 0 Color Transformer: FlatColor Decay Time: 0 - Enabled: false + Enabled: true Invert Rainbow: false Max Color: 255; 255; 255 Max Intensity: 999999 @@ -150,7 +164,7 @@ Visualization Manager: Value: /vtr/filtered_point_cloud Use Fixed Frame: true Use rainbow: true - Value: false + Value: true - Angle Tolerance: 0.10000000149011612 Class: rviz_default_plugins/Odometry Covariance: @@ -1057,7 +1071,7 @@ Visualization Manager: Color: 255; 255; 0 Color Transformer: Intensity Decay Time: 0 - Enabled: true + Enabled: false Invert Rainbow: false Max Color: 255; 255; 255 Max Intensity: 2 @@ -1078,7 +1092,7 @@ Visualization Manager: Value: /vtr/fake_obstacle_scan Use Fixed Frame: true Use rainbow: true - Value: true + Value: false - Alpha: 0.20000000298023224 Autocompute Intensity Bounds: false Autocompute Value Bounds: @@ -1091,7 +1105,7 @@ Visualization Manager: Color: 0; 0; 0 Color Transformer: FlatColor Decay Time: 0 - Enabled: true + Enabled: false Invert Rainbow: false Max Color: 255; 255; 255 Max Intensity: 3 @@ -1112,6 +1126,40 @@ Visualization Manager: Value: /vtr/submap_loc Use Fixed Frame: true Use rainbow: true + Value: false + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 255 + Min Color: 0; 0; 0 + Min Intensity: 1 + Name: PointCloud2 + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.02500000037252903 + Style: Flat Squares + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /velodyne_points + Use Fixed Frame: true + Use rainbow: true Value: true Enabled: true Global Options: @@ -1160,25 +1208,25 @@ Visualization Manager: Views: Current: Class: rviz_default_plugins/Orbit - Distance: 47.42359161376953 + Distance: 10.982351303100586 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false Focal Point: - X: 48.304954528808594 - Y: 2.685429811477661 - Z: 20.119665145874023 + X: 3.0496435165405273 + Y: -0.9416981339454651 + Z: 2.5215511322021484 Focal Shape Fixed Size: false Focal Shape Size: 0.05000000074505806 Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: 0.7647982835769653 + Pitch: 0.3797965943813324 Target Frame: robot Value: Orbit (rviz_default_plugins) - Yaw: 2.295583963394165 + Yaw: 5.934669494628906 Saved: - Class: rviz_default_plugins/Orbit Distance: 111.39698028564453 @@ -1382,11 +1430,11 @@ Visualization Manager: Yaw: 2.295583963394165 Window Geometry: Displays: - collapsed: true - Height: 586 - Hide Left Dock: true - Hide Right Dock: true - QMainWindow State: 000000ff00000000fd0000000400000000000002810000037afc0200000007fc0000003d000001a20000000000fffffffaffffffff0100000002fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730000000000ffffffff0000009d00fffffffb0000001200530065006c0065006300740069006f006e0000000000000002530000007400fffffffb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073000000003d0000037a000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c0000026100000001000001c00000030ffc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d0000030f000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000005000000006efc0100000002fb0000000800540069006d0065000000000000000500000002eb00fffffffb0000000800540069006d006501000000000000045000000000000000000000031e000001f000000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + collapsed: false + Height: 1016 + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd000000040000000000000281000003a2fc0200000007fc0000003d000001a20000000000fffffffaffffffff0100000002fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730000000000ffffffff0000008c00fffffffb0000001200530065006c0065006300740069006f006e0000000000000002530000006c00fffffffb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b000003a2000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c0000026100000001000001c0000003a2fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003b000003a2000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000005000000006efc0100000002fb0000000800540069006d00650000000000000005000000025300fffffffb0000000800540069006d00650100000000000004500000000000000000000002eb000003a200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Time: @@ -1394,7 +1442,7 @@ Window Geometry: Tool Properties: collapsed: false Views: - collapsed: true - Width: 798 - X: 3093 - Y: 36 + collapsed: false + Width: 1848 + X: 72 + Y: 1107 From 42577c5cac5357bed89639aba2c29c28ea1904ec Mon Sep 17 00:00:00 2001 From: Alec Krawciw Date: Fri, 27 Jan 2023 17:38:57 -0500 Subject: [PATCH 04/77] Added a module to show the difference between the map point cloud and the live scan at different voxelization thresholds --- Dockerfile | 1 + config/honeycomb_grizzly_default.yaml | 30 +-- launch/offline_honeycomb_grizzly.launch.yaml | 2 +- launch/online_honeycomb_grizzly.launch.yaml | 2 +- .../include/vtr_lidar/modules/modules.hpp | 1 + .../planning/change_detection_module_v3.hpp | 1 + .../modules/planning/diff_generator.hpp | 81 ++++++++ .../vtr_lidar/utils/nanoflann_utils.hpp | 39 ++++ .../planning/change_detection_module_v3.cpp | 23 ++- .../src/modules/planning/diff_generator.cpp | 195 ++++++++++++++++++ .../preprocessing/preprocessing_module.cpp | 2 + rviz/lidar.rviz | 109 +++++++--- 12 files changed, 439 insertions(+), 47 deletions(-) create mode 100644 main/src/vtr_lidar/include/vtr_lidar/modules/planning/diff_generator.hpp create mode 100644 main/src/vtr_lidar/src/modules/planning/diff_generator.cpp diff --git a/Dockerfile b/Dockerfile index 80f19093b..993e87395 100644 --- a/Dockerfile +++ b/Dockerfile @@ -67,6 +67,7 @@ RUN apt update && apt install -q -y \ ros-humble-xacro \ ros-humble-vision-opencv \ ros-humble-perception-pcl ros-humble-pcl-ros +RUN apt install ros-humble-tf2-tools ## Install misc dependencies RUN apt update && apt install -q -y \ diff --git a/config/honeycomb_grizzly_default.yaml b/config/honeycomb_grizzly_default.yaml index ed3b83e38..3060331fc 100644 --- a/config/honeycomb_grizzly_default.yaml +++ b/config/honeycomb_grizzly_default.yaml @@ -6,7 +6,7 @@ - lidar.terrain_assessment - navigation - navigation.graph_map_server - - navigation.command + #- navigation.command - navigation.sensor_input # tactic - tactic @@ -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", @@ -65,8 +65,8 @@ localization_skippable: false task_queue_num_threads: 1 task_queue_size: -1 - - route_completion_translation_threshold: 0.5 + + route_completion_translation_threshold: 0.5 chain: min_cusp_distance: 1.5 angle_weight: 7.0 @@ -107,12 +107,12 @@ visualize: true estimate_time: true filtering: - type: lidar.preprocessing_v2 + type: lidar.preprocessing num_threads: 8 crop_range: 20.0 - frame_voxel_size: 0.1 # grid subsampling voxel size + frame_voxel_size: 0.2 # 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 @@ -159,7 +159,7 @@ visualize: true mapping: type: lidar.odometry_map_maintenance_v2 - map_voxel_size: 0.1 + map_voxel_size: 0.2 crop_range_front: 40.0 back_over_front_ratio: 0.5 point_life_time: 20.0 @@ -171,7 +171,7 @@ intra_exp_merging: type: lidar.intra_exp_merging_v2 depth: 6.0 - map_voxel_size: 0.1 + map_voxel_size: 0.2 crop_range_front: 40.0 back_over_front_ratio: 0.5 visualize: true @@ -186,7 +186,7 @@ visualize: true inter_exp_merging: type: lidar.inter_exp_merging_v2 - map_voxel_size: 0.1 + map_voxel_size: 0.2 max_num_experiences: 128 distance_threshold: 0.6 planar_threshold: 0.2 @@ -238,7 +238,7 @@ support_variance: 0.05 support_threshold: 2.5 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 + minimum_distance: 0.9 # was 0.3 Jordy # cost map costmap_history_size: 15 # Keep between 3 and 20, used for temporal filtering @@ -338,7 +338,7 @@ state_update_freq: 1.0 update_state: true rand_seed: 1 - + # Planner Tuning Params initial_samples: 300 batch_samples: 150 @@ -356,7 +356,7 @@ costmap: costmap_filter_value: 0.01 costmap_history: 20 # Note I dont think im actually using this one anymore, am using the one in the change detection v3 now - + mpc: # Controller Params horizon_steps: 20 @@ -365,19 +365,19 @@ max_lin_vel: 1.25 max_ang_vel: 0.75 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/launch/offline_honeycomb_grizzly.launch.yaml b/launch/offline_honeycomb_grizzly.launch.yaml index 9e868c677..db2278c77 100644 --- a/launch/offline_honeycomb_grizzly.launch.yaml +++ b/launch/offline_honeycomb_grizzly.launch.yaml @@ -20,7 +20,7 @@ windows: base_params:=honeycomb_grizzly_default.yaml data_dir:=${VTRTEMP}/lidar start_new_graph:=false - use_sim_time:=false + use_sim_time:=true # - htop # monitor hardware usage diff --git a/launch/online_honeycomb_grizzly.launch.yaml b/launch/online_honeycomb_grizzly.launch.yaml index b035b24ef..498759a2e 100644 --- a/launch/online_honeycomb_grizzly.launch.yaml +++ b/launch/online_honeycomb_grizzly.launch.yaml @@ -37,7 +37,7 @@ windows: - window_name: rviz2 layout: main-horizontal shell_command_before: - - source /opt/ros/galactic/setup.bash + - source /opt/ros/humble/setup.bash panes: - ros2 run rviz2 rviz2 -d ${VTRSRC}/rviz/lidar.rviz # - ros2 run rqt_reconfigure rqt_reconfigure diff --git a/main/src/vtr_lidar/include/vtr_lidar/modules/modules.hpp b/main/src/vtr_lidar/include/vtr_lidar/modules/modules.hpp index b5a9db8c7..c20970f9d 100644 --- a/main/src/vtr_lidar/include/vtr_lidar/modules/modules.hpp +++ b/main/src/vtr_lidar/include/vtr_lidar/modules/modules.hpp @@ -47,5 +47,6 @@ #include "vtr_lidar/modules/planning/obstacle_detection_module.hpp" #include "vtr_lidar/modules/planning/safe_corridor_module.hpp" #include "vtr_lidar/modules/planning/terrain_assessment_module.hpp" +#include "vtr_lidar/modules/planning/diff_generator.hpp" #include "vtr_lidar/modules/planning/fake_obstacle_module.hpp" 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 9bf19e8fd..5a070769e 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 @@ -91,6 +91,7 @@ class ChangeDetectionModuleV3 : public tactic::BaseModule { rclcpp::Publisher::SharedPtr costmap_pub_; rclcpp::Publisher::SharedPtr filtered_costmap_pub_ ; rclcpp::Publisher::SharedPtr costpcd_pub_; + rclcpp::Publisher::SharedPtr diffpcd_pub_; VTR_REGISTER_MODULE_DEC_TYPE(ChangeDetectionModuleV3); diff --git a/main/src/vtr_lidar/include/vtr_lidar/modules/planning/diff_generator.hpp b/main/src/vtr_lidar/include/vtr_lidar/modules/planning/diff_generator.hpp new file mode 100644 index 000000000..ea6ec55f9 --- /dev/null +++ b/main/src/vtr_lidar/include/vtr_lidar/modules/planning/diff_generator.hpp @@ -0,0 +1,81 @@ +// 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 diff_generator.hpp + * \author Alec Krawciw, Autonomous Space Robotics Lab (ASRL) + */ +#pragma once + +#include "tf2/convert.h" +#include "tf2_eigen/tf2_eigen.hpp" +#include "tf2_ros/transform_broadcaster.h" + +#include "vtr_lidar/cache.hpp" +#include "vtr_tactic/modules/base_module.hpp" +#include "vtr_tactic/task_queue.hpp" + +namespace vtr { +namespace lidar { + +class DifferenceDetector : public tactic::BaseModule { + public: + PTR_TYPEDEFS(DifferenceDetector); + using PointCloudMsg = sensor_msgs::msg::PointCloud2; + + static constexpr auto static_name = "lidar.diff_generator"; + + /** \brief Collection of config parameters */ + struct Config : public BaseModule::Config { + PTR_TYPEDEFS(Config); + + // change detection + float detection_range = 10.0; //m + float minimum_distance = 0.0; //m + + float neighbour_threshold = 0.05; //m + float voxel_size = 0.2; //m + + float angle_weight = 10.0/2/M_PI; + + // + bool visualize = false; + + static ConstPtr fromROS(const rclcpp::Node::SharedPtr &node, + const std::string ¶m_prefix); + }; + + DifferenceDetector( + const Config::ConstPtr &config, + const std::shared_ptr &module_factory = nullptr, + const std::string &name = static_name) + : tactic::BaseModule{module_factory, name}, config_(config) {} + + private: + void run_(tactic::QueryCache &qdata, tactic::OutputCache &output, + const tactic::Graph::Ptr &graph, + const tactic::TaskExecutor::Ptr &executor) override; + + Config::ConstPtr config_; + + /** \brief for visualization only */ + bool publisher_initialized_ = false; + rclcpp::Publisher::SharedPtr diffpcd_pub_; + + VTR_REGISTER_MODULE_DEC_TYPE(DifferenceDetector); + +}; + +} // namespace lidar +} // namespace vtr diff --git a/main/src/vtr_lidar/include/vtr_lidar/utils/nanoflann_utils.hpp b/main/src/vtr_lidar/include/vtr_lidar/utils/nanoflann_utils.hpp index d50f80dea..d65dd85f9 100644 --- a/main/src/vtr_lidar/include/vtr_lidar/utils/nanoflann_utils.hpp +++ b/main/src/vtr_lidar/include/vtr_lidar/utils/nanoflann_utils.hpp @@ -56,6 +56,41 @@ struct NanoFLANNAdapter { } }; +template +struct NanoFLANNSphericalAdapter { + NanoFLANNSphericalAdapter(const pcl::PointCloud& points, const float angle_weight) : points_(points), angle_weights(angle_weight){ } + + const pcl::PointCloud& points_; + const float angle_weights; + + // Must return the number of data points + inline size_t kdtree_get_point_count() const { return points_.size(); } + + // Returns the dim'th component of the idx'th point in the class: + // Since this is inlined and the "dim" argument is typically an immediate + // value, the "if/else's" are actually solved at compile time. + inline float kdtree_get_pt(const size_t idx, const size_t dim) const { + if (dim == 0) + return points_[idx].rho; + else if (dim == 1) + return points_[idx].phi*angle_weights; + else + return points_[idx].theta*angle_weights; + } + + // Optional bounding-box computation: return false to default to a standard + // bbox computation loop. + // Return true if the BBOX was already computed by the class and returned in + // "bb" so it can be avoided to redo it again. Look at bb.size() to find out + // the expected dimensionality (e.g. 2 or 3 for point clouds) + template + bool kdtree_get_bbox(BBOX& /* bb */) const { + return false; + } +}; + + +//Store all neighbours within a given radius template class NanoFLANNRadiusResultSet { public: @@ -110,6 +145,10 @@ using KDTree = nanoflann::KDTreeSingleIndexAdaptor< nanoflann::L2_Simple_Adaptor>, NanoFLANNAdapter>; template +using SphericalKDTree = nanoflann::KDTreeSingleIndexAdaptor< + nanoflann::L2_Simple_Adaptor>, + NanoFLANNSphericalAdapter>; +template using DynamicKDTree = nanoflann::KDTreeSingleIndexDynamicAdaptor< nanoflann::L2_Simple_Adaptor>, NanoFLANNAdapter>; 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 c5c2f3cf8..0424d6fb0 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 @@ -21,6 +21,8 @@ #include "pcl/features/normal_3d.h" #include "vtr_lidar/data_types/costmap.hpp" +#include "vtr_lidar/filters/voxel_downsample.hpp" + #include "vtr_lidar/utils/nanoflann_utils.hpp" namespace vtr { @@ -137,6 +139,7 @@ void ChangeDetectionModuleV3::run_(QueryCache &qdata0, OutputCache &output0, 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); + diffpcd_pub_ = qdata.node->create_publisher("change_detection_diff", 5); // clang-format on publisher_initialized_ = true; } @@ -147,7 +150,7 @@ void ChangeDetectionModuleV3::run_(QueryCache &qdata0, OutputCache &output0, const auto &vid_loc = *qdata.vid_loc; const auto &sid_loc = *qdata.sid_loc; const auto &T_r_v_loc = *qdata.T_r_v_loc; - const auto &points = *qdata.undistorted_point_cloud; + const auto &points = *qdata.raw_point_cloud; const auto &submap_loc = *qdata.submap_loc; const auto &map_point_cloud = submap_loc.point_cloud(); const auto &T_v_m_loc = *qdata.T_v_m_loc; @@ -163,6 +166,8 @@ void ChangeDetectionModuleV3::run_(QueryCache &qdata0, OutputCache &output0, query_indices.emplace_back(i); } pcl::PointCloud query_points(points, query_indices); + + voxelDownsample(query_points, 0.2); // Eigen matrix of original data (only shallow copy of ref clouds) const auto query_mat = query_points.getMatrixXfMap(4, PointWithInfo::size(), PointWithInfo::cartesian_offset()); @@ -236,8 +241,10 @@ void ChangeDetectionModuleV3::run_(QueryCache &qdata0, OutputCache &output0, // clang-format on }(); // - if (nn_dists[i] < 0.0 || (cost > config_->negprob_threshold)) + if (nn_dists[i] < 0.0 || (cost > config_->negprob_threshold)){ aligned_points[i].flex23 = 1.0f; + aligned_points[i].flex21 = cost; + } } // add support region @@ -389,6 +396,8 @@ void ChangeDetectionModuleV3::run_(QueryCache &qdata0, OutputCache &output0, // 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 + // TODO A linked list would make more sense here + if (costmap_history.size() < config_->costmap_history_size) { costmap_history.push_back(sparse_world_map); @@ -396,7 +405,7 @@ void ChangeDetectionModuleV3::run_(QueryCache &qdata0, OutputCache &output0, // 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++) + for (unsigned int i = 0; i < (config_->costmap_history_size-1); i++) { costmap_history[i] = costmap_history[i + 1]; } @@ -408,7 +417,7 @@ void ChangeDetectionModuleV3::run_(QueryCache &qdata0, OutputCache &output0, std::unordered_map, 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++) + for (unsigned 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 " <publish(pointcloud_msg); + + PointCloudMsg filter_msg; + pcl::toROSMsg(filtered_points, filter_msg); + filter_msg.header.frame_id = "loc vertex frame"; + // pointcloud_msg.header.stamp = rclcpp::Time(*qdata.stamp); + diffpcd_pub_->publish(filter_msg); } /// output diff --git a/main/src/vtr_lidar/src/modules/planning/diff_generator.cpp b/main/src/vtr_lidar/src/modules/planning/diff_generator.cpp new file mode 100644 index 000000000..09b5a9b64 --- /dev/null +++ b/main/src/vtr_lidar/src/modules/planning/diff_generator.cpp @@ -0,0 +1,195 @@ +// 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 diff_generator.cpp + * \author Alec Krawciw, Autonomous Space Robotics Lab (ASRL) + */ +#include "vtr_lidar/modules/planning/diff_generator.hpp" + +#include "pcl/features/normal_3d.h" +#include "vtr_lidar/filters/voxel_downsample.hpp" + +#include "vtr_lidar/utils/nanoflann_utils.hpp" + +namespace vtr { +namespace lidar { + +void velodyneCart2Pol(pcl::PointCloud& point_cloud) { + for (size_t i = 0; i < point_cloud.size(); i++) { + auto &p = point_cloud[i]; + auto &pm1 = i > 0 ? point_cloud[i - 1] : point_cloud[i]; + + p.rho = sqrt(p.x * p.x + p.y * p.y + p.z * p.z); + p.theta = atan2(sqrt(p.x * p.x + p.y * p.y), p.z); + p.phi = atan2(p.y, p.x) + M_PI / 2; + + if (i > 0 && (p.phi - pm1.phi) > 1.5 * M_PI) + p.phi -= 2 * M_PI; + else if (i > 0 && (p.phi - pm1.phi) < -1.5 * M_PI) + p.phi += 2 * M_PI; + } +} + + + +using namespace tactic; + +auto DifferenceDetector::Config::fromROS( + const rclcpp::Node::SharedPtr &node, const std::string ¶m_prefix) + -> ConstPtr { + auto config = std::make_shared(); + // clang-format off + // change detection + config->detection_range = node->declare_parameter(param_prefix + ".detection_range", config->detection_range); + config->minimum_distance = node->declare_parameter(param_prefix + ".minimum_distance", config->minimum_distance); + config->neighbour_threshold = node->declare_parameter(param_prefix + ".neighbour_threshold", config->neighbour_threshold); + + //filtering + config->voxel_size = node->declare_parameter(param_prefix + ".voxel_size", config->voxel_size); + config->angle_weight = node->declare_parameter(param_prefix + ".angle_weight", config->angle_weight); + + // general + config->visualize = node->declare_parameter(param_prefix + ".visualize", config->visualize); + // clang-format on + return config; +} + +void DifferenceDetector::run_(QueryCache &qdata0, OutputCache &output0, + const Graph::Ptr & /* graph */, + const TaskExecutor::Ptr & /* executor */) { + auto &qdata = dynamic_cast(qdata0); + auto &output = dynamic_cast(output0); + + /// visualization setup + if (config_->visualize && !publisher_initialized_) { + // clang-format off + diffpcd_pub_ = qdata.node->create_publisher("change_detection_diff", 5); + // clang-format on + publisher_initialized_ = true; + } + + // inputs + const auto &stamp = *qdata.stamp; + const auto &T_s_r = *qdata.T_s_r; + const auto &vid_loc = *qdata.vid_loc; + const auto &sid_loc = *qdata.sid_loc; + const auto &T_r_v_loc = *qdata.T_r_v_loc; + const auto &points = *qdata.raw_point_cloud; + const auto &submap_loc = *qdata.submap_loc; + const auto &map_point_cloud = submap_loc.point_cloud(); + const auto &T_v_m_loc = *qdata.T_v_m_loc; + + // clang-format off + CLOG(INFO, static_name) << "Diff detection for lidar scan at stamp: " << stamp; + + // filter out points that are too far away or too close + std::vector query_indices; + for (size_t i = 0; i < points.size(); ++i) { + const auto &pt = points.at(i); + if (pt.getVector3fMap().norm() < config_->detection_range && pt.getVector3fMap().norm() > config_->minimum_distance) + query_indices.emplace_back(i); + } + pcl::PointCloud query_points(points, query_indices); + + voxelDownsample(query_points, config_->voxel_size); + + // Eigen matrix of original data (only shallow copy of ref clouds) + const auto query_mat = query_points.getMatrixXfMap(4, PointWithInfo::size(), PointWithInfo::cartesian_offset()); + const auto query_norms_mat = query_points.getMatrixXfMap(4, PointWithInfo::size(), PointWithInfo::normal_offset()); + + + const auto map_mat = map_point_cloud.getMatrixXfMap(4, PointWithInfo::size(), PointWithInfo::cartesian_offset()); + const auto map_norms_mat = map_point_cloud.getMatrixXfMap(4, PointWithInfo::size(), PointWithInfo::normal_offset()); + + // retrieve the pre-processed scan and convert it to the local map frame + pcl::PointCloud aligned_points(query_points); + auto aligned_mat = aligned_points.getMatrixXfMap(4, PointWithInfo::size(), PointWithInfo::cartesian_offset()); + auto aligned_norms_mat = aligned_points.getMatrixXfMap(4, PointWithInfo::size(), PointWithInfo::normal_offset()); + + const auto T_s_m = (T_s_r * T_r_v_loc * T_v_m_loc).matrix(); + const auto T_m_s = (T_s_r * T_r_v_loc * T_v_m_loc).inverse().matrix(); + aligned_mat = T_m_s.cast() * query_mat; + aligned_norms_mat = T_m_s.cast() * query_norms_mat; + + pcl::PointCloud aligned_map(map_point_cloud); + auto aligned_map_mat = aligned_map.getMatrixXfMap(4, PointWithInfo::size(), PointWithInfo::cartesian_offset()); + auto aligned_map_norms_mat = aligned_map.getMatrixXfMap(4, PointWithInfo::size(), PointWithInfo::normal_offset()); + + //Put map into lidar frame + aligned_map_mat = T_s_m.cast() * map_mat; + aligned_map_norms_mat = T_s_m.cast() * map_norms_mat; + + + velodyneCart2Pol(aligned_map); + + // create kd-tree of the map + //Could this be done ahead of time and stored? + NanoFLANNAdapter adapter(aligned_map);//, config_->angle_weight); + KDTreeSearchParams search_params; + KDTreeParams tree_params(10 /* max leaf */); + auto kdtree = std::make_unique>(3, adapter, tree_params); + kdtree->buildIndex(); + + std::vector nn_inds(query_points.size()); + std::vector nn_dists(query_points.size(), -1.0f); + // compute nearest neighbors and point to point distances + for (size_t i = 0; i < query_points.size(); i++) { + KDTreeResultSet result_set(1); + result_set.init(&nn_inds[i], &nn_dists[i]); + kdtree->findNeighbors(result_set, query_points[i].data, search_params); + } + + std::vector diff_indices; + for (size_t i = 0; i < query_points.size(); i++) { + aligned_points[i].flex23 = std::sqrt(nn_dists[i]); + if (aligned_points[i].flex23 > config_->neighbour_threshold){ + diff_indices.push_back(i); + } + } + + // add support region + + // retrieve the pre-processed scan and convert it to the vertex frame + pcl::PointCloud aligned_points2(aligned_points); + // clang-format off + auto aligned_mat2 = aligned_points2.getMatrixXfMap(4, PointWithInfo::size(), PointWithInfo::cartesian_offset()); + auto aligned_norms_mat2 = aligned_points2.getMatrixXfMap(4, PointWithInfo::size(), PointWithInfo::normal_offset()); + // clang-format on + + // transform to vertex frame + aligned_mat2 = T_v_m_loc.matrix().cast() * aligned_mat; + aligned_norms_mat2 = T_v_m_loc.matrix().cast() * aligned_norms_mat; + + pcl::PointCloud filtered_points(aligned_points2, diff_indices); + + + + /// publish the transformed pointcloud + if (config_->visualize) { + PointCloudMsg filter_msg; + pcl::toROSMsg(filtered_points, filter_msg); + filter_msg.header.frame_id = "loc vertex frame"; + // pointcloud_msg.header.stamp = rclcpp::Time(*qdata.stamp); + diffpcd_pub_->publish(filter_msg); + } + + + CLOG(INFO, static_name) + << "Diff detection for lidar scan at stamp: " << stamp << " - DONE"; + +} + +} // namespace lidar +} // namespace vtr \ No newline at end of file diff --git a/main/src/vtr_lidar/src/modules/preprocessing/preprocessing_module.cpp b/main/src/vtr_lidar/src/modules/preprocessing/preprocessing_module.cpp index 676981d87..68bd6125f 100644 --- a/main/src/vtr_lidar/src/modules/preprocessing/preprocessing_module.cpp +++ b/main/src/vtr_lidar/src/modules/preprocessing/preprocessing_module.cpp @@ -154,6 +154,7 @@ void PreprocessingModule::run_(QueryCache &qdata0, OutputCache &, /// Filtering based on normal scores (planarity + linearity) + #if true // Remove points with a low normal score auto sorted_norm_scores = norm_scores; std::sort(sorted_norm_scores.begin(), sorted_norm_scores.end()); @@ -171,6 +172,7 @@ void PreprocessingModule::run_(QueryCache &qdata0, OutputCache &, *filtered_point_cloud = pcl::PointCloud(*filtered_point_cloud, indices); } + #endif CLOG(DEBUG, "lidar.preprocessing") << "planarity sampled point size: " << filtered_point_cloud->size(); diff --git a/rviz/lidar.rviz b/rviz/lidar.rviz index 7b066042d..efc3623a5 100644 --- a/rviz/lidar.rviz +++ b/rviz/lidar.rviz @@ -6,15 +6,17 @@ Panels: Expanded: - /Global Options1 - /TF1 - - /TF1/Status1 + - /TF1/Frames1 + - /TF1/Tree1 - /raw scan1 - /filtered scan1 - /odometry path1/Shape1 - /curr map loc1/Topic1 - /planned path1/Offset1 - /planned path (poses)1/Topic1 + - /change detection scan1 + - /chang detection cost pcd1 - /global path1/Offset1 - - /planning fake obs. scan1 - /PointCloud21 Splitter Ratio: 0.4507042169570923 Tree Height: 871 @@ -35,7 +37,7 @@ Panels: Experimental: false Name: Time SyncMode: 0 - SyncSource: raw scan + SyncSource: "" Visualization Manager: Class: "" Displays: @@ -69,12 +71,38 @@ Visualization Manager: Frame Timeout: 99999 Frames: All Enabled: false + base_link: + Value: true + chassis_link: + Value: true + front_axle_link: + Value: true + front_left_wheel_link: + Value: true + front_right_wheel_link: + Value: true + imu_link: + Value: true lidar: Value: false - odo vertex frame: + navsat_link: + Value: true + odom: + Value: true + rear_antenna_link: + Value: true + rear_left_wheel_link: + Value: true + rear_right_wheel_link: + Value: true + rear_sensor_base_link: + Value: true + rear_sensor_plate_link: Value: true robot: Value: true + sensor_anchor_link: + Value: true velodyne: Value: true world: @@ -85,14 +113,9 @@ Visualization Manager: Name: TF Show Arrows: false Show Axes: true - Show Names: false + Show Names: true Tree: world: - odo vertex frame: - robot: - lidar: - velodyne: - {} world (offset): {} Update Interval: 0 @@ -109,7 +132,7 @@ Visualization Manager: Color: 255; 255; 255 Color Transformer: Intensity Decay Time: 0 - Enabled: false + Enabled: true Invert Rainbow: false Max Color: 0; 255; 0 Max Intensity: 0 @@ -130,7 +153,7 @@ Visualization Manager: Value: /vtr/raw_point_cloud Use Fixed Frame: true Use rainbow: true - Value: false + Value: true - Alpha: 1 Autocompute Intensity Bounds: true Autocompute Value Bounds: @@ -143,7 +166,7 @@ Visualization Manager: Color: 0; 255; 0 Color Transformer: FlatColor Decay Time: 0 - Enabled: true + Enabled: false Invert Rainbow: false Max Color: 255; 255; 255 Max Intensity: 999999 @@ -164,7 +187,7 @@ Visualization Manager: Value: /vtr/filtered_point_cloud Use Fixed Frame: true Use rainbow: true - Value: true + Value: false - Angle Tolerance: 0.10000000149011612 Class: rviz_default_plugins/Odometry Covariance: @@ -452,7 +475,7 @@ Visualization Manager: Min Value: -10 Value: true Axis: Z - Channel Name: flex24 + Channel Name: flex23 Class: rviz_default_plugins/PointCloud2 Color: 255; 255; 0 Color Transformer: Intensity @@ -460,7 +483,7 @@ Visualization Manager: Enabled: false Invert Rainbow: false Max Color: 255; 255; 255 - Max Intensity: 2 + Max Intensity: 1 Min Color: 0; 0; 0 Min Intensity: 0 Name: change detection scan @@ -513,6 +536,40 @@ Visualization Manager: Use Fixed Frame: true Use rainbow: false Value: false + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 0.8999999761581421 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: chang detection cost pcd + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.30000001192092896 + Style: Flat Squares + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /vtr/change_detection_diff + Use Fixed Frame: true + Use rainbow: true + Value: true - Alpha: 1 Autocompute Intensity Bounds: true Autocompute Value Bounds: @@ -953,7 +1010,7 @@ Visualization Manager: Class: rviz_default_plugins/Map Color Scheme: map Draw Behind: false - Enabled: true + Enabled: false Name: safe corridor cost map Topic: Depth: 5 @@ -969,7 +1026,7 @@ Visualization Manager: Reliability Policy: Reliable Value: /vtr/safe_corridor_costmap_updates Use Timestamp: false - Value: true + Value: false - Alpha: 0.699999988079071 Class: rviz_default_plugins/Map Color Scheme: costmap @@ -1163,7 +1220,7 @@ Visualization Manager: Value: true Enabled: true Global Options: - Background Color: 255; 255; 255 + Background Color: 248; 206; 255 Fixed Frame: world Frame Rate: 30 Name: root @@ -1208,25 +1265,25 @@ Visualization Manager: Views: Current: Class: rviz_default_plugins/Orbit - Distance: 10.982351303100586 + Distance: 26.967164993286133 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false Focal Point: - X: 3.0496435165405273 - Y: -0.9416981339454651 - Z: 2.5215511322021484 + X: 0 + Y: 0 + Z: 0 Focal Shape Fixed Size: false Focal Shape Size: 0.05000000074505806 Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: 0.3797965943813324 + Pitch: 0.7153981328010559 Target Frame: robot Value: Orbit (rviz_default_plugins) - Yaw: 5.934669494628906 + Yaw: 0.4403983950614929 Saved: - Class: rviz_default_plugins/Orbit Distance: 111.39698028564453 @@ -1445,4 +1502,4 @@ Window Geometry: collapsed: false Width: 1848 X: 72 - Y: 1107 + Y: 27 From 3fc43ac29fc21fdc7b610bf206d68cb8811e390b Mon Sep 17 00:00:00 2001 From: Alec Krawciw Date: Sun, 29 Jan 2023 14:20:52 -0500 Subject: [PATCH 05/77] Add compile option to store complete point clouds --- main/src/vtr_common/vtr_include.cmake | 1 + main/src/vtr_lidar/src/pipeline.cpp | 2 +- 2 files changed, 2 insertions(+), 1 deletion(-) diff --git a/main/src/vtr_common/vtr_include.cmake b/main/src/vtr_common/vtr_include.cmake index d6477ea10..726c3ab8c 100644 --- a/main/src/vtr_common/vtr_include.cmake +++ b/main/src/vtr_common/vtr_include.cmake @@ -31,3 +31,4 @@ endif() ## Enable certain pipelines add_definitions(-DVTR_ENABLE_LIDAR) add_definitions(-DVTR_ENABLE_RADAR) +add_definitions(-DSAVE_FULL_LIDAR) diff --git a/main/src/vtr_lidar/src/pipeline.cpp b/main/src/vtr_lidar/src/pipeline.cpp index c4d5327ec..274aee368 100644 --- a/main/src/vtr_lidar/src/pipeline.cpp +++ b/main/src/vtr_lidar/src/pipeline.cpp @@ -153,7 +153,7 @@ void LidarPipeline::onVertexCreation_(const QueryCache::Ptr &qdata0, "filtered_point_cloud", "vtr_lidar_msgs/msg/PointScan", scan_odo_msg); } // raw point cloud -#if false +#if defined(VTR_ENABLE_LIDAR) && defined(SAVE_FULL_LIDAR) { auto raw_scan_odo = std::make_shared>(); raw_scan_odo->point_cloud() = *qdata->undistorted_raw_point_cloud; From 105de6f762599e9e5ad187d189ee6fc5363b0532 Mon Sep 17 00:00:00 2001 From: Alec Krawciw Date: Mon, 30 Jan 2023 13:16:23 -0500 Subject: [PATCH 06/77] Add intensity and raw point cloud messages to the pose graph --- .../conversions/velodyne_conversion_module_v2.cpp | 4 +++- main/src/vtr_lidar/src/pipeline.cpp | 2 +- 2 files changed, 4 insertions(+), 2 deletions(-) diff --git a/main/src/vtr_lidar/src/modules/preprocessing/conversions/velodyne_conversion_module_v2.cpp b/main/src/vtr_lidar/src/modules/preprocessing/conversions/velodyne_conversion_module_v2.cpp index 8797317f7..a7c8cc1cb 100644 --- a/main/src/vtr_lidar/src/modules/preprocessing/conversions/velodyne_conversion_module_v2.cpp +++ b/main/src/vtr_lidar/src/modules/preprocessing/conversions/velodyne_conversion_module_v2.cpp @@ -89,12 +89,14 @@ void VelodyneConversionModuleV2::run_(QueryCache &qdata0, OutputCache &, // iterators // clang-format off sensor_msgs::PointCloud2ConstIterator iter_x(*msg, "x"), iter_y(*msg, "y"), iter_z(*msg, "z"); + sensor_msgs::PointCloud2ConstIterator iter_intensity(*msg, "intensity"); for (size_t idx = 0; iter_x != iter_x.end(); - ++idx, ++iter_x, ++iter_y, ++iter_z) { + ++idx, ++iter_x, ++iter_y, ++iter_z, ++iter_intensity) { // cartesian coordinates point_cloud->at(idx).x = *iter_x; point_cloud->at(idx).y = *iter_y; point_cloud->at(idx).z = *iter_z; + point_cloud->at(idx).flex14 = *iter_intensity; } diff --git a/main/src/vtr_lidar/src/pipeline.cpp b/main/src/vtr_lidar/src/pipeline.cpp index 274aee368..3e5e87022 100644 --- a/main/src/vtr_lidar/src/pipeline.cpp +++ b/main/src/vtr_lidar/src/pipeline.cpp @@ -156,7 +156,7 @@ void LidarPipeline::onVertexCreation_(const QueryCache::Ptr &qdata0, #if defined(VTR_ENABLE_LIDAR) && defined(SAVE_FULL_LIDAR) { auto raw_scan_odo = std::make_shared>(); - raw_scan_odo->point_cloud() = *qdata->undistorted_raw_point_cloud; + raw_scan_odo->point_cloud() = *qdata->raw_point_cloud; raw_scan_odo->T_vertex_this() = qdata->T_s_r->inverse(); raw_scan_odo->vertex_id() = *qdata->vid_odo; // From 683f8b478f3879fa3f07a18644510cb2ecdc3826 Mon Sep 17 00:00:00 2001 From: Alec Krawciw Date: Fri, 3 Feb 2023 14:41:03 -0500 Subject: [PATCH 07/77] Debugging file access race conditions --- main/src/vtr_common/vtr_include.cmake | 1 + .../src/modules/odometry/vertex_test_module.cpp | 9 +++++++++ main/src/vtr_lidar/src/pipeline.cpp | 5 +++++ 3 files changed, 15 insertions(+) diff --git a/main/src/vtr_common/vtr_include.cmake b/main/src/vtr_common/vtr_include.cmake index 726c3ab8c..b54b97a66 100644 --- a/main/src/vtr_common/vtr_include.cmake +++ b/main/src/vtr_common/vtr_include.cmake @@ -32,3 +32,4 @@ endif() add_definitions(-DVTR_ENABLE_LIDAR) add_definitions(-DVTR_ENABLE_RADAR) add_definitions(-DSAVE_FULL_LIDAR) +#add_definitions(-DSAVE_ALL_REPEATS) diff --git a/main/src/vtr_lidar/src/modules/odometry/vertex_test_module.cpp b/main/src/vtr_lidar/src/modules/odometry/vertex_test_module.cpp index 757a7a3b2..5a68655ac 100644 --- a/main/src/vtr_lidar/src/modules/odometry/vertex_test_module.cpp +++ b/main/src/vtr_lidar/src/modules/odometry/vertex_test_module.cpp @@ -45,6 +45,7 @@ void VertexTestModule::run_(QueryCache &qdata0, OutputCache &, const auto &first_frame = *qdata.first_frame; const auto &T_r_v = *qdata.T_r_v_odo; const auto &success = *qdata.odo_success; + const auto &pipeline_mode = *qdata.pipeline_mode; // output auto &result = *qdata.vertex_test_result; @@ -54,6 +55,14 @@ void VertexTestModule::run_(QueryCache &qdata0, OutputCache &, // check if we successfully register this frame if (!success) return; + #ifdef SAVE_ALL_REPEATS + if (pipeline_mode == PipelineMode::RepeatMetricLoc || pipeline_mode == PipelineMode::RepeatFollow){ + result = VertexTestResult::CREATE_VERTEX; + return; + } + + #endif + auto se3vec = T_r_v.vec(); auto translation_distance = se3vec.head<3>().norm(); auto rotation_distance = se3vec.tail<3>().norm() * 57.29577; // 180/pi diff --git a/main/src/vtr_lidar/src/pipeline.cpp b/main/src/vtr_lidar/src/pipeline.cpp index 3e5e87022..16f542235 100644 --- a/main/src/vtr_lidar/src/pipeline.cpp +++ b/main/src/vtr_lidar/src/pipeline.cpp @@ -138,6 +138,8 @@ void LidarPipeline::onVertexCreation_(const QueryCache::Ptr &qdata0, /// update current map vertex id and transform sliding_map_odo_->T_vertex_this() = *T_r_m_odo_; sliding_map_odo_->vertex_id() = *qdata->vid_odo; + CLOG(DEBUG, "lidar.pipeline") << "Saving data to vertex" << vertex; + /// store the live frame point cloud // motion compensated point cloud @@ -151,6 +153,8 @@ void LidarPipeline::onVertexCreation_(const QueryCache::Ptr &qdata0, auto scan_odo_msg = std::make_shared(scan_odo, *qdata->stamp); vertex->insert>( "filtered_point_cloud", "vtr_lidar_msgs/msg/PointScan", scan_odo_msg); + CLOG(DEBUG, "lidar.pipeline") << "Saved filtered pointcloud to vertex" << vertex; + } // raw point cloud #if defined(VTR_ENABLE_LIDAR) && defined(SAVE_FULL_LIDAR) @@ -166,6 +170,7 @@ void LidarPipeline::onVertexCreation_(const QueryCache::Ptr &qdata0, vertex->insert>( "raw_point_cloud", "vtr_lidar_msgs/msg/PointScan", raw_scan_odo_msg); } + CLOG(DEBUG, "lidar.pipeline") << "Saved raw pointcloud to vertex" << vertex; #endif /// save the sliding map as vertex submap if we have traveled far enough From f6190ee479bdbcc9ee1eccf12725193876f9d8c0 Mon Sep 17 00:00:00 2001 From: asrl Date: Mon, 13 Mar 2023 17:53:06 -0400 Subject: [PATCH 08/77] check lidar fields for t and time before estimating --- .../velodyne_conversion_module_v2.cpp | 27 +++++++++++++------ 1 file changed, 19 insertions(+), 8 deletions(-) diff --git a/main/src/vtr_lidar/src/modules/preprocessing/conversions/velodyne_conversion_module_v2.cpp b/main/src/vtr_lidar/src/modules/preprocessing/conversions/velodyne_conversion_module_v2.cpp index a7c8cc1cb..88509fa68 100644 --- a/main/src/vtr_lidar/src/modules/preprocessing/conversions/velodyne_conversion_module_v2.cpp +++ b/main/src/vtr_lidar/src/modules/preprocessing/conversions/velodyne_conversion_module_v2.cpp @@ -106,16 +106,27 @@ void VelodyneConversionModuleV2::run_(QueryCache &qdata0, OutputCache &, // Velodyne has no polar coordinates, so compute them manually. velodyneCart2Pol(*point_cloud); - if (config_->estimate_time){ + try { + try{ + sensor_msgs::PointCloud2ConstIterator iter_time(*msg, "t"); + // pointwise timestamp + for (size_t idx = 0; iter_time != iter_time.end(); + ++idx, ++iter_time) { + point_cloud->at(idx).timestamp = static_cast(*iter_time * 1e9); + } + } + catch (...){ + sensor_msgs::PointCloud2ConstIterator iter_time(*msg, "time"); + // pointwise timestamp + for (size_t idx = 0; iter_time != iter_time.end(); + ++idx, ++iter_time) { + point_cloud->at(idx).timestamp = static_cast(*iter_time * 1e9); + } + } + } + catch(...){ CLOG(INFO, "lidar.velodyne_converter_v2") << "Timings wil be estimated from yaw angle"; estimateTime(*point_cloud, *qdata.stamp, config_->angular_vel); - } else { - sensor_msgs::PointCloud2ConstIterator iter_time(*msg, "t"); - // pointwise timestamp - for (size_t idx = 0; iter_time != iter_time.end(); - ++idx, ++iter_time) { - point_cloud->at(idx).timestamp = static_cast(*iter_time * 1e9); - } } // Output From 06c237376c2e60111c5e8ae655fac7c1e6200976 Mon Sep 17 00:00:00 2001 From: Alec Krawciw Date: Mon, 13 Mar 2023 17:53:18 -0400 Subject: [PATCH 09/77] Improvements to the config structure --- config/hdl64_grizzly_default.yaml | 387 +++++++++++++++++++++ launch/online_velodyne_grizzly.launch.yaml | 43 +++ 2 files changed, 430 insertions(+) create mode 100644 config/hdl64_grizzly_default.yaml create mode 100644 launch/online_velodyne_grizzly.launch.yaml diff --git a/config/hdl64_grizzly_default.yaml b/config/hdl64_grizzly_default.yaml new file mode 100644 index 000000000..e95f91e4b --- /dev/null +++ b/config/hdl64_grizzly_default.yaml @@ -0,0 +1,387 @@ +/**: + ros__parameters: + log_to_file: true + log_debug: true + log_enabled: + - lidar.terrain_assessment + - navigation + - navigation.graph_map_server + #- navigation.command + - navigation.sensor_input + # tactic + - tactic + - tactic.pipeline + - tactic.module + - tactic.module.live_mem_manager + - tactic.module.graph_mem_manager + # path planner + # "path_planning", + # "path_planning.teb", + # "path_planning.cbit", + # "path_planning.cbit_planner", + # "mpc.cbit", + # "mpc_debug.cbit", + # mission planner + - mission.server + - mission.state_machine + # pose graph + - pose_graph + # pipeline specific + #- lidar.pipeline + #- lidar.velodyne_converter_v2 + - lidar.preprocessing + # "lidar.odometry_icp", + # "lidar.odometry_map_maintenance", + # "lidar.vertex_test", + # "lidar.localization_map_recall", + # "lidar.localization_icp", + # "lidar.intra_exp_merging", + # "lidar.dynamic_detection", + # "lidar.inter_exp_merging", + # "lidar.change_detection", + # "lidar.ground_extraction", + # "lidar.obstacle_detection", + # "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 + origin_theta: 1.3 + scale: 1.0 + tactic: + enable_parallelization: true + preprocessing_skippable: false + odometry_mapping_skippable: false + localization_skippable: false + task_queue_num_threads: 1 + task_queue_size: -1 + + route_completion_translation_threshold: 0.5 + chain: + min_cusp_distance: 1.5 + angle_weight: 7.0 + search_depth: 5 + search_back_depth: 10 + distance_warning: 5.0 + save_odometry_result: true + save_localization_result: true + visualize: true + rviz_loc_path_offset: + - 0.0 + - 0.0 + - 0.0 + pipeline: + type: lidar + preprocessing: + - conversion + - filtering + odometry: + - icp + - mapping + - vertex_test + - intra_exp_merging + #- dynamic_detection + #- inter_exp_merging + - memory + localization: + - recall + - icp + # - safe_corridor + #- change_detection_sync + - memory + submap_translation_threshold: 1.5 + submap_rotation_threshold: 30.0 + preprocessing: + conversion: + type: lidar.velodyne_converter_v2 + visualize: true + estimate_time: true + downsample_ratio: 2 + + filtering: + type: lidar.preprocessing + num_threads: 8 + + crop_range: 20.0 + + frame_voxel_size: 0.2 # grid subsampling voxel size + + #velodyne lidar has 1/3 degree upper block and 1/2 degree lower block + vertical_angle_res: 0.00872664626 # vertical angle resolution in radians (0.5deg), + 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 + #horizontal resolution 0.1728degree 0.1728/(0.5) + h_scale: 0.175 # 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 + traj_lock_prev_vel: false + traj_qc_diag: + - 1.0 + - 0.1 + - 0.1 + - 0.1 + - 0.1 + - 1.0 + num_threads: 8 + first_num_steps: 2 + initial_max_iter: 10 + initial_max_pairing_dist: 1.5 + initial_max_planar_dist: 1.0 + refined_max_iter: 50 + refined_max_pairing_dist: 1.0 + refined_max_planar_dist: 0.3 + averaging_num_steps: 2 + verbose: false + max_iterations: 1 + min_matched_ratio: 0.5 + visualize: true + mapping: + type: lidar.odometry_map_maintenance_v2 + map_voxel_size: 0.2 + crop_range_front: 40.0 + back_over_front_ratio: 0.5 + point_life_time: 20.0 + visualize: true + vertex_test: + type: lidar.vertex_test + max_translation: 0.5 + max_rotation: 10.0 + intra_exp_merging: + type: lidar.intra_exp_merging_v2 + depth: 6.0 + map_voxel_size: 0.2 + 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 # 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 + max_num_experiences: 128 + distance_threshold: 0.6 + planar_threshold: 0.2 + normal_threshold: 0.8 + dynamic_obs_threshold: 1 + visualize: true + memory: + type: live_mem_manager + window_size: 5 + localization: + recall: + type: lidar.localization_map_recall + map_version: pointmap + visualize: true + icp: + type: lidar.localization_icp + use_pose_prior: true + num_threads: 8 + first_num_steps: 2 + initial_max_iter: 10 + initial_max_pairing_dist: 1.5 + initial_max_planar_dist: 1.0 + refined_max_iter: 50 + refined_max_pairing_dist: 1.0 + refined_max_planar_dist: 0.3 + averaging_num_steps: 2 + verbose: false + max_iterations: 1 + min_matched_ratio: 0.3 + safe_corridor: + type: lidar.safe_corridor + lookahead_distance: 5.0 + corridor_width: 3.5 + influence_distance: 1.0 + resolution: 0.25 + size_x: 16.0 + size_y: 8.0 + visualize: true + change_detection_sync: + type: lidar.change_detection_v3 + detection_range: 8.0 + search_radius: 0.25 + negprob_threshold: 0.1 + use_prior: true + alpha0: 3.0 + beta0: 0.03 + use_support_filtering: true + support_radius: 0.25 + support_variance: 0.05 + support_threshold: 2.5 + 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 + visualize: true + change_detection: + type: lidar.change_detection_v2 + detection_range: 8 + search_radius: 0.25 + negprob_threshold: 0.1 + use_prior: true + alpha0: 3.0 + beta0: 0.03 + use_support_filtering: true + support_radius: 0.25 + support_variance: 0.05 + support_threshold: 2.5 + resolution: 0.5 + size_x: 16.0 + size_y: 8.0 + run_online: false + run_async: true + visualize: false + save_module_result: false + memory: + type: graph_mem_manager + vertex_life_span: 5 + window_size: 3 + obstacle_detection: + type: lidar.obstacle_detection + z_min: 0.5 + z_max: 2.0 + resolution: 0.6 + size_x: 40.0 + size_y: 20.0 + run_async: true + visualize: true + ground_extraction: + type: lidar.ground_extraction + z_offset: 0.2 + alpha: 0.035 + tolerance: 0.25 + Tm: 0.3 + Tm_small: 0.1 + Tb: 0.5 + Trmse: 0.1 + Tdprev: 1.0 + rmin: 2.0 + num_bins_small: 30.0 + bin_size_small: 0.5 + num_bins_large: 10.0 + bin_size_large: 1.0 + resolution: 0.6 + size_x: 40.0 + size_y: 20.0 + run_async: true + visualize: true + terrain_assessment: + type: lidar.terrain_assessment + lookahead_distance: 15.0 + corridor_width: 1.0 + search_radius: 1.0 + resolution: 0.5 + size_x: 40.0 + size_y: 20.0 + run_online: false + run_async: true + visualize: true + path_planning: + 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 + extrapolation_timeout: 2.0 + lookahead_distance: 8.5 + robot_model: circular + robot_radius: 0.5 + map_frame: planning frame + enable_homotopy_class_planning: true + free_goal_vel: true + max_vel_x: 0.6 + max_vel_y: 0.0 + max_vel_theta: 0.3 + min_turning_radius: 3 + weight_viapoint: 0.5 + weight_costmap: 100.0 + cbit: + obs_padding: 0.0 + curv_to_euclid_discretization: 20 + sliding_window_width: 12.0 + sliding_window_freespace_padding: 0.5 + corridor_resolution: 0.05 + state_update_freq: 1.0 + update_state: true + rand_seed: 1 + + # 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.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: 20 # Note I dont think im actually using this one anymore, am using the one in the change detection v3 now + + mpc: + # 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" + + # 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/launch/online_velodyne_grizzly.launch.yaml b/launch/online_velodyne_grizzly.launch.yaml new file mode 100644 index 000000000..b13eaa10a --- /dev/null +++ b/launch/online_velodyne_grizzly.launch.yaml @@ -0,0 +1,43 @@ +## Online Lidar VTR3 +session_name: vtr_online_velodyne_grizzly + +environment: + CYCLONEDDS_URI: ${VTRSRC}/config/cyclonedds.default.xml # custom dds configs + # ROS_DOMAIN_ID: "1" # set this to a unique number when multiple ROS2 dependent system running on the same network + +start_directory: ${VTRTEMP} + +suppress_history: false + +windows: + - window_name: vtr_navigation_system + layout: main-horizontal + shell_command_before: + - source ${VTRSRC}/main/install/setup.bash + panes: + - > + ros2 launch vtr_navigation vtr.launch.py + base_params:=hdl64_grizzly_default.yaml + data_dir:=${VTRTEMP}/lidar/$(date '+%F')/$(date '+%F') + start_new_graph:=false + use_sim_time:=false + + - htop # monitor hardware usage + + - window_name: vtr_gui + layout: main-horizontal + shell_command_before: + - source ${VTRSRC}/main/install/setup.bash + panes: + - ros2 run vtr_gui socket_client --ros-args -r __ns:=/vtr + - ros2 run vtr_gui socket_server --ros-args -r __ns:=/vtr + - ros2 run vtr_gui web_server --ros-args -r __ns:=/vtr + # - firefox --new-window "localhost:5200" # the webpage has to wait for everything above + + - window_name: rviz2 + layout: main-horizontal + shell_command_before: + - source /opt/ros/humble/setup.bash + panes: + - ros2 run rviz2 rviz2 -d ${VTRSRC}/rviz/lidar.rviz + # - ros2 run rqt_reconfigure rqt_reconfigure From 1b4e453802679415bfd6bd137e4ca23201f80d08 Mon Sep 17 00:00:00 2001 From: Alec Krawciw Date: Tue, 14 Mar 2023 10:40:09 -0400 Subject: [PATCH 10/77] Added downsampling option for the velodyne lidar. --- config/honeycomb_grizzly_default.yaml | 10 +- main/src/vtr_common/vtr_include.cmake | 2 +- .../velodyne_conversion_module_v2.hpp | 1 + .../odometry_map_maintenance_module_v2.cpp | 20 ++- .../velodyne_conversion_module_v2.cpp | 23 +++- .../vtr_pose_graph/test/path/test_path.cpp | 123 ++++++++++++++++++ 6 files changed, 166 insertions(+), 13 deletions(-) diff --git a/config/honeycomb_grizzly_default.yaml b/config/honeycomb_grizzly_default.yaml index 3060331fc..c9ae184e5 100644 --- a/config/honeycomb_grizzly_default.yaml +++ b/config/honeycomb_grizzly_default.yaml @@ -27,8 +27,8 @@ # pose graph - pose_graph # pipeline specific - - lidar.pipeline - - lidar.velodyne_converter_v2 + #- lidar.pipeline + #- lidar.velodyne_converter_v2 - lidar.preprocessing # "lidar.odometry_icp", # "lidar.odometry_map_maintenance", @@ -90,15 +90,15 @@ - mapping - vertex_test - intra_exp_merging - - dynamic_detection + #- dynamic_detection - inter_exp_merging - memory localization: - recall - icp # - safe_corridor - - change_detection_sync - #- memory + #- change_detection_sync + - memory submap_translation_threshold: 1.5 submap_rotation_threshold: 30.0 preprocessing: diff --git a/main/src/vtr_common/vtr_include.cmake b/main/src/vtr_common/vtr_include.cmake index b54b97a66..f45eb4c00 100644 --- a/main/src/vtr_common/vtr_include.cmake +++ b/main/src/vtr_common/vtr_include.cmake @@ -30,6 +30,6 @@ endif() ## Enable certain pipelines add_definitions(-DVTR_ENABLE_LIDAR) -add_definitions(-DVTR_ENABLE_RADAR) +#add_definitions(-DVTR_ENABLE_RADAR) add_definitions(-DSAVE_FULL_LIDAR) #add_definitions(-DSAVE_ALL_REPEATS) diff --git a/main/src/vtr_lidar/include/vtr_lidar/modules/preprocessing/conversions/velodyne_conversion_module_v2.hpp b/main/src/vtr_lidar/include/vtr_lidar/modules/preprocessing/conversions/velodyne_conversion_module_v2.hpp index a508b7007..a63b0c4c6 100644 --- a/main/src/vtr_lidar/include/vtr_lidar/modules/preprocessing/conversions/velodyne_conversion_module_v2.hpp +++ b/main/src/vtr_lidar/include/vtr_lidar/modules/preprocessing/conversions/velodyne_conversion_module_v2.hpp @@ -43,6 +43,7 @@ class VelodyneConversionModuleV2 : public tactic::BaseModule { bool visualize = false; bool estimate_time = false; double angular_vel = 600*M_PI/30; + int horizontal_downsample = 1; static ConstPtr fromROS(const rclcpp::Node::SharedPtr &node, const std::string ¶m_prefix); diff --git a/main/src/vtr_lidar/src/modules/odometry/odometry_map_maintenance_module_v2.cpp b/main/src/vtr_lidar/src/modules/odometry/odometry_map_maintenance_module_v2.cpp index e7a7fabc2..191a68579 100644 --- a/main/src/vtr_lidar/src/modules/odometry/odometry_map_maintenance_module_v2.cpp +++ b/main/src/vtr_lidar/src/modules/odometry/odometry_map_maintenance_module_v2.cpp @@ -55,10 +55,24 @@ void OdometryMapMaintenanceModuleV2::run_(QueryCache &qdata0, OutputCache &, publisher_initialized_ = true; } + // construct output (construct the map if not exist) if (!qdata.sliding_map_odo) qdata.sliding_map_odo.emplace(config_->map_voxel_size); + // Do not update the map if registration failed. + if (!(*qdata.odo_success)) { + CLOG(WARNING, "lidar.odometry_map_maintenance") + << "Point cloud registration failed - not updating the map."; + return; + } + + // if (*qdata.pipeline_mode == tactic::PipelineMode::RepeatFollow){ + // CLOG(WARNING, "lidar.odometry_map_maintenance") + // << "Skipped updating map, repeating"; + // return; + // } + // Get input and output data // input const auto &T_s_r = *qdata.T_s_r; @@ -67,12 +81,6 @@ void OdometryMapMaintenanceModuleV2::run_(QueryCache &qdata0, OutputCache &, // the following has to be copied because we need to change them auto points = *qdata.undistorted_point_cloud; - // Do not update the map if registration failed. - if (!(*qdata.odo_success)) { - CLOG(WARNING, "lidar.odometry_map_maintenance") - << "Point cloud registration failed - not updating the map."; - return; - } // Transform points into the map frame auto T_m_s = (T_s_r * T_r_m_odo).inverse().matrix().cast(); diff --git a/main/src/vtr_lidar/src/modules/preprocessing/conversions/velodyne_conversion_module_v2.cpp b/main/src/vtr_lidar/src/modules/preprocessing/conversions/velodyne_conversion_module_v2.cpp index a7c8cc1cb..5a8048cd2 100644 --- a/main/src/vtr_lidar/src/modules/preprocessing/conversions/velodyne_conversion_module_v2.cpp +++ b/main/src/vtr_lidar/src/modules/preprocessing/conversions/velodyne_conversion_module_v2.cpp @@ -62,6 +62,7 @@ auto VelodyneConversionModuleV2::Config::fromROS( config->visualize = node->declare_parameter(param_prefix + ".visualize", config->visualize); config->estimate_time = node->declare_parameter(param_prefix + ".estimate_time", config->estimate_time); config->angular_vel = node->declare_parameter(param_prefix + ".angular_vel", config->angular_vel); + config->horizontal_downsample = node->declare_parameter(param_prefix + ".downsample_ratio", config->horizontal_downsample); // clang-format on return config; @@ -118,8 +119,28 @@ void VelodyneConversionModuleV2::run_(QueryCache &qdata0, OutputCache &, } } + + auto filtered_point_cloud = + std::make_shared>(*point_cloud); + CLOG(DEBUG, "lidar.velodyne_converter_v2") << "Reducing the point cloud density by " << config_->horizontal_downsample; + if (config_->horizontal_downsample > 1) { + + /// Range cropping + + std::vector indices; + indices.reserve(filtered_point_cloud->size()); + for (size_t i = 0; i < filtered_point_cloud->size(); ++i) { + if (i % config_->horizontal_downsample == 0) + indices.emplace_back(i); + } + *filtered_point_cloud = + pcl::PointCloud(*filtered_point_cloud, indices); + + } + + // Output - qdata.raw_point_cloud = point_cloud; + qdata.raw_point_cloud = filtered_point_cloud; // Visualize if (config_->visualize) { diff --git a/main/src/vtr_pose_graph/test/path/test_path.cpp b/main/src/vtr_pose_graph/test/path/test_path.cpp index 52dfdfaaf..e8f8aaeb3 100644 --- a/main/src/vtr_pose_graph/test/path/test_path.cpp +++ b/main/src/vtr_pose_graph/test/path/test_path.cpp @@ -70,6 +70,62 @@ class PathTest : public Test { BasicGraph::Ptr graph_ = std::make_shared(); }; + +class MergedPathTest : public Test { + public: + MergedPathTest() {} + ~MergedPathTest() override {} + + void SetUp() override { + /* Create the following graph + * R0: 0 --- 1 --- 2 --- 3 -- 0 (only vertices) + + */ + + // clang-format off + // Add a graph with 2 runs and 5 vertices per run. + for (int major_idx = 0; major_idx < 1; ++major_idx) { + // Create the robochunk directories + graph_->addRun(); + graph_->addVertex(); + for (int minor_idx = 0; minor_idx < 4 - 1; ++minor_idx) { + graph_->addVertex(); + graph_->addEdge(VertexId(major_idx, minor_idx), VertexId(major_idx, minor_idx + 1), EdgeType::Temporal, false, EdgeTransform(true)); + } + } + // Add merged loop closure edge across runs. + graph_->addEdge(VertexId(0, 3), VertexId(0, 0), EdgeType::Temporal, false, EdgeTransform(true)); + + + // set the edge's transform to something special; + { + Eigen::Matrix4d transform = Eigen::Matrix4d::Identity(); + transform(0, 3) = 1; + graph_->at(EdgeId(VertexId(0, 0),VertexId(0, 1)))->setTransform(EdgeTransform(transform).inverse()); + } + { + Eigen::Matrix4d transform = Eigen::Matrix4d::Identity(); + transform(1, 3) = 1; + graph_->at(EdgeId(VertexId(0, 1),VertexId(0, 2)))->setTransform(EdgeTransform(transform).inverse()); + } + { + Eigen::Matrix4d transform = Eigen::Matrix4d::Identity(); + transform(0, 3) = -1; + graph_->at(EdgeId(VertexId(0, 2),VertexId(0, 3)))->setTransform(EdgeTransform(transform).inverse()); + } + { + Eigen::Matrix4d transform = Eigen::Matrix4d::Identity(); + transform(1, 3) = -0.5; + graph_->at(EdgeId(VertexId(0, 3),VertexId(0, 0)))->setTransform(EdgeTransform(transform).inverse()); + } + } + + void TearDown() override {} + + BasicGraph::Ptr graph_ = std::make_shared(); +}; + +/* TEST_F(PathTest, PathTest) { using BasicPath = Path; using Sequence = BasicPath::Sequence; @@ -143,10 +199,77 @@ TEST_F(PathTest, PathTest) { EXPECT_TRUE(path->pose(1).vec() == path->pose(3).vec()); EXPECT_TRUE(path->pose(2).vec() == end_vec); } +}*/ + +TEST_F(MergedPathTest, MergedPathTest) { + using BasicPath = Path; + using Sequence = BasicPath::Sequence; + auto path = std::make_shared(graph_); + CLOG(ERROR, "test")<< "Hello?"; + + // SECTION("A valid path") + { + // run 0 ids + Sequence seq = {0ul, 1ul, 2ul, 3ul}; + // copy the sequence into the path + path->setSequence(seq); + CLOG(INFO, "test") << "About to verify the sequence: " << path->sequence(); + EXPECT_TRUE(path->verifySequence()); + } + + // SECTION("A valid offset path") + { + // run 0 ids + Sequence seq = {1ul, 2ul, 3ul, 0ul}; + // copy the sequence into the path + path->setSequence(seq); + CLOG(INFO, "test") << "About to verify the sequence: " << path->sequence(); + EXPECT_TRUE(path->verifySequence()); + } + + // SECTION("A valid offset path") + { + // run 0 ids + Sequence seq = {1ul, 2ul, 3ul, 0ul}; + // copy the sequence into the path + path->setSequence(seq); + CLOG(INFO, "test") << "About to verify the sequence: " << path->sequence(); + // The pose at the far end of the path + Eigen::Matrix end_vec; + end_vec.setZero(); + end_vec(0) = -1.; + end_vec(1) = 0.5; + + CLOG(INFO, "test") << "About to evaluate the pose: " << path->pose(3); + + EXPECT_TRUE(path->pose(3).vec() == end_vec); + } + + { + // run 0 ids + Sequence seq = {1ul, 2ul, 3ul, 0ul, 1ul}; + // copy the sequence into the path + path->setSequence(seq); + CLOG(INFO, "test") << "About to verify the sequence: " << path->sequence(); + // The pose at the far end of the path + Eigen::Matrix end_vec; + end_vec.setZero(); + end_vec(0) = 0; + end_vec(1) = 0.5; + + CLOG(INFO, "test") << "About to evaluate the pose: " << path->pose(3); + + EXPECT_TRUE(path->pose(4).vec() == end_vec); + } + + } + int main(int argc, char** argv) { configureLogging("", true); + CLOG(ERROR, "test")<< "Hello?"; + testing::InitGoogleTest(&argc, argv); return RUN_ALL_TESTS(); } From 7c76aa2024d859cf927fa944af965ab2939aef10 Mon Sep 17 00:00:00 2001 From: HuguesTHOMAS Date: Fri, 17 Mar 2023 15:10:28 -0400 Subject: [PATCH 11/77] remove obsolete broadcast parameter and change socket address --- main/src/vtr_gui/vtr_gui/socket_server.py | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/main/src/vtr_gui/vtr_gui/socket_server.py b/main/src/vtr_gui/vtr_gui/socket_server.py index f550908a6..9aa95abaf 100755 --- a/main/src/vtr_gui/vtr_gui/socket_server.py +++ b/main/src/vtr_gui/vtr_gui/socket_server.py @@ -21,7 +21,7 @@ from vtr_navigation.vtr_ui_builder import build_remote # socket io server address and port -SOCKET_ADDRESS = 'localhost' +SOCKET_ADDRESS = '0.0.0.0' SOCKET_PORT = 5201 logger = logging.getLogger('SocketServer') @@ -128,42 +128,42 @@ def handle_change_env_info(data): def handle_server_state(json): logger.info('Broadcasting server state') server_state = json['server_state'] - socketio.emit(u"mission/server_state", server_state, broadcast=True) + socketio.emit(u"mission/server_state", server_state) @socketio.on('notification/graph_state') def handle_graph_state(json): logger.info('Broadcasting graph state') graph_state = json['graph_state'] - socketio.emit(u"graph/state", graph_state, broadcast=True) + socketio.emit(u"graph/state", graph_state) @socketio.on('notification/graph_update') def handle_graph_update(json): logger.info('Broadcasting graph update') graph_update = json['graph_update'] - socketio.emit(u"graph/update", graph_update, broadcast=True) + socketio.emit(u"graph/update", graph_update) @socketio.on('notification/robot_state') def handle_robot_state(json): logger.info('Broadcasting robot state') robot_state = json['robot_state'] - socketio.emit(u"robot/state", robot_state, broadcast=True) + socketio.emit(u"robot/state", robot_state) @socketio.on('notification/following_route') def handle_following_route(json): logger.info('Broadcasting following route') following_route = json['following_route'] - socketio.emit(u"following_route", following_route, broadcast=True) + socketio.emit(u"following_route", following_route) @socketio.on('notification/task_queue_update') def handle_task_queue_update(json): logger.info('Broadcasting task queue update') task_queue_update = json['task_queue_update'] - socketio.emit(u"task_queue/update", task_queue_update, broadcast=True) + socketio.emit(u"task_queue/update", task_queue_update) def main(): From 696ef421a0516d5c623710552bd1036b2a1f2401 Mon Sep 17 00:00:00 2001 From: Alec Krawciw Date: Mon, 10 Apr 2023 20:03:22 +0000 Subject: [PATCH 12/77] Velodyne needs some cleanup --- Dockerfile | 5 +- config/hdl64_grizzly_default.yaml | 12 +- config/honeycomb_grizzly_default.yaml | 146 ++++++++---------- launch/online_velodyne_grizzly.launch.yaml | 6 +- main/src/deps/steam | 2 +- main/src/vtr_common/vtr_include.cmake | 2 +- main/src/vtr_gui/vtr_gui/socket_server.py | 2 +- .../src/components/goal/GoalManager.js | 70 ++++++--- .../vtr-gui/src/components/graph/GraphMap.js | 12 +- .../src/components/task_queue/TaskQueue.js | 4 +- main/src/vtr_gui/vtr_gui/vtr-gui/src/index.js | 11 ++ .../src/vtr_lidar/include/vtr_lidar/cache.hpp | 1 + .../preprocessing/preprocessing_module.hpp | 2 + .../localization/localization_icp_module.cpp | 8 +- .../modules/odometry/odometry_icp_module.cpp | 7 + .../velodyne_conversion_module_v2.cpp | 49 +++--- .../preprocessing/preprocessing_module.cpp | 42 +++-- main/src/vtr_lidar/src/pipeline.cpp | 15 ++ .../mission_server/mission_server.hpp | 7 +- .../src/mission_server/mission_server.cpp | 3 + main/src/vtr_navigation/src/navigator.cpp | 8 +- .../vtr_navigation/src/ros_mission_server.cpp | 3 + .../vtr_navigation_msgs/msg/ServerState.msg | 1 + 23 files changed, 256 insertions(+), 162 deletions(-) diff --git a/Dockerfile b/Dockerfile index c7510994a..5fd64ec10 100644 --- a/Dockerfile +++ b/Dockerfile @@ -66,7 +66,9 @@ RUN curl -sSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.key -o RUN apt update && apt install -q -y \ ros-humble-xacro \ ros-humble-vision-opencv \ - ros-humble-perception-pcl ros-humble-pcl-ros + ros-humble-perception-pcl ros-humble-pcl-ros \ + ros-humble-rmw-cyclonedds-cpp + RUN apt install ros-humble-tf2-tools ## Install misc dependencies @@ -97,6 +99,7 @@ RUN pip3 install \ websocket-client RUN apt install htop +RUN apt install ros-humble-velodyne -q -y # Install vim RUN apt update && apt install -q -y vim diff --git a/config/hdl64_grizzly_default.yaml b/config/hdl64_grizzly_default.yaml index e95f91e4b..48c119b13 100644 --- a/config/hdl64_grizzly_default.yaml +++ b/config/hdl64_grizzly_default.yaml @@ -9,11 +9,11 @@ #- navigation.command - navigation.sensor_input # tactic - - tactic + #- tactic - tactic.pipeline - - tactic.module - - tactic.module.live_mem_manager - - tactic.module.graph_mem_manager + #- tactic.module + #- tactic.module.live_mem_manager + #- tactic.module.graph_mem_manager # path planner # "path_planning", # "path_planning.teb", @@ -22,8 +22,8 @@ # "mpc.cbit", # "mpc_debug.cbit", # mission planner - - mission.server - - mission.state_machine + #- mission.server + #- mission.state_machine # pose graph - pose_graph # pipeline specific diff --git a/config/honeycomb_grizzly_default.yaml b/config/honeycomb_grizzly_default.yaml index c9ae184e5..88152b9e0 100644 --- a/config/honeycomb_grizzly_default.yaml +++ b/config/honeycomb_grizzly_default.yaml @@ -6,8 +6,7 @@ - lidar.terrain_assessment - navigation - navigation.graph_map_server - #- navigation.command - - navigation.sensor_input + - navigation.command # tactic - tactic - tactic.pipeline @@ -27,9 +26,9 @@ # pose graph - pose_graph # pipeline specific - #- lidar.pipeline - #- lidar.velodyne_converter_v2 - - lidar.preprocessing + # "lidar.pipeline", + # "lidar.honeycomb_converter", + # "lidar.preprocessing", # "lidar.odometry_icp", # "lidar.odometry_map_maintenance", # "lidar.vertex_test", @@ -44,15 +43,8 @@ # "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 + lidar_frame: honeycomb + lidar_topic: /points graph_map: origin_lat: 43.7822 origin_lng: -79.4661 @@ -65,8 +57,7 @@ localization_skippable: false task_queue_num_threads: 1 task_queue_size: -1 - - route_completion_translation_threshold: 0.5 + route_completion_translation_threshold: 2.0 chain: min_cusp_distance: 1.5 angle_weight: 7.0 @@ -90,50 +81,42 @@ - mapping - vertex_test - intra_exp_merging - #- dynamic_detection + - dynamic_detection - inter_exp_merging - memory localization: - recall - icp - # - safe_corridor - #- change_detection_sync + - safe_corridor + - change_detection_sync - memory submap_translation_threshold: 1.5 submap_rotation_threshold: 30.0 preprocessing: conversion: - type: lidar.velodyne_converter_v2 + type: lidar.honeycomb_converter_v2 visualize: true - estimate_time: true filtering: type: lidar.preprocessing num_threads: 8 - - crop_range: 20.0 - - frame_voxel_size: 0.2 # 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 + crop_range: 40.0 + frame_voxel_size: 0.2 + 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 visualize: true odometry: icp: type: lidar.odometry_icp - # continuous time estimation - use_trajectory_estimation: false + use_trajectory_estimation: true traj_num_extra_states: 0 traj_lock_prev_pose: false traj_lock_prev_vel: false @@ -178,9 +161,9 @@ dynamic_detection: type: lidar.dynamic_detection depth: 12.0 - horizontal_resolution: 0.041 # 0.02042 - vertical_resolution: 0.026 # 0.01326 - max_num_observations: 10000 + horizontal_resolution: 0.041 + vertical_resolution: 0.026 + max_num_observations: 2000 min_num_observations: 4 dynamic_threshold: 0.3 visualize: true @@ -237,12 +220,8 @@ support_radius: 0.25 support_variance: 0.05 support_threshold: 2.5 - 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 - + influence_distance: 0.55 + minimum_distance: 0.35 resolution: 0.25 size_x: 16.0 size_y: 8.0 @@ -311,8 +290,8 @@ run_async: true visualize: true path_planning: - 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 + type: cbit + control_period: 33 teb: visualize: true extrapolate: false @@ -332,52 +311,53 @@ cbit: obs_padding: 0.0 curv_to_euclid_discretization: 20 - sliding_window_width: 12.0 + sliding_window_width: 10.0 sliding_window_freespace_padding: 0.5 corridor_resolution: 0.05 - state_update_freq: 1.0 + state_update_freq: 0.5 update_state: true rand_seed: 1 - - # Planner Tuning Params - initial_samples: 300 - batch_samples: 150 + initial_samples: 250 + batch_samples: 100 pre_seed_resolution: 0.5 alpha: 0.5 q_max: 2.5 frame_interval: 50 iter_max: 10000000 - eta: 1.0 - rad_m_exhange: 1.00 + eta: 1.1 + rad_m_exhange: 1.0 initial_exp_rad: 0.75 extrapolation: false incremental_plotting: false plotting: true costmap: costmap_filter_value: 0.01 - costmap_history: 20 # Note I dont think im actually using this one anymore, am using the one in the change detection v3 now - + costmap_history: 15 mpc: - # Controller Params - horizon_steps: 20 - horizon_step_size: 0.3 - forward_vel: 0.75 + horizon_steps: 40 + horizon_step_size: 0.1 + forward_vel: 1.0 max_lin_vel: 1.25 max_ang_vel: 0.75 - 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 - + 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 command_history_length: 100 diff --git a/launch/online_velodyne_grizzly.launch.yaml b/launch/online_velodyne_grizzly.launch.yaml index b13eaa10a..8488560b4 100644 --- a/launch/online_velodyne_grizzly.launch.yaml +++ b/launch/online_velodyne_grizzly.launch.yaml @@ -2,7 +2,9 @@ session_name: vtr_online_velodyne_grizzly environment: - CYCLONEDDS_URI: ${VTRSRC}/config/cyclonedds.default.xml # custom dds configs + RMW_IMPLEMENTATION: rmw_fastrtps_cpp + RMW_FASTRTPS_PUBLICATION_MODE: ASYNCHRONOUS + #CYCLONEDDS_URI: ${VTRSRC}/config/cyclonedds.default.xml # custom dds configs # ROS_DOMAIN_ID: "1" # set this to a unique number when multiple ROS2 dependent system running on the same network start_directory: ${VTRTEMP} @@ -22,8 +24,6 @@ windows: start_new_graph:=false use_sim_time:=false - - htop # monitor hardware usage - - window_name: vtr_gui layout: main-horizontal shell_command_before: diff --git a/main/src/deps/steam b/main/src/deps/steam index 62a2188bc..53060505f 160000 --- a/main/src/deps/steam +++ b/main/src/deps/steam @@ -1 +1 @@ -Subproject commit 62a2188bceb81e056e5156c1b48bd6f20b109f3e +Subproject commit 53060505f4ac84315ee28829f664895d1da56ff9 diff --git a/main/src/vtr_common/vtr_include.cmake b/main/src/vtr_common/vtr_include.cmake index f45eb4c00..52e367205 100644 --- a/main/src/vtr_common/vtr_include.cmake +++ b/main/src/vtr_common/vtr_include.cmake @@ -31,5 +31,5 @@ endif() ## Enable certain pipelines add_definitions(-DVTR_ENABLE_LIDAR) #add_definitions(-DVTR_ENABLE_RADAR) -add_definitions(-DSAVE_FULL_LIDAR) +#add_definitions(-DSAVE_FULL_LIDAR) #add_definitions(-DSAVE_ALL_REPEATS) diff --git a/main/src/vtr_gui/vtr_gui/socket_server.py b/main/src/vtr_gui/vtr_gui/socket_server.py index 9aa95abaf..4a3270b6e 100755 --- a/main/src/vtr_gui/vtr_gui/socket_server.py +++ b/main/src/vtr_gui/vtr_gui/socket_server.py @@ -32,7 +32,7 @@ logger.addHandler(hd) app = flask.Flask(__name__) -app.config['DEBUG'] = False +app.config['DEBUG'] = True app.secret_key = 'asecretekey' app.logger.setLevel(logging.ERROR) diff --git a/main/src/vtr_gui/vtr_gui/vtr-gui/src/components/goal/GoalManager.js b/main/src/vtr_gui/vtr_gui/vtr-gui/src/components/goal/GoalManager.js index 07e27f0f0..737331afa 100644 --- a/main/src/vtr_gui/vtr_gui/vtr-gui/src/components/goal/GoalManager.js +++ b/main/src/vtr_gui/vtr_gui/vtr-gui/src/components/goal/GoalManager.js @@ -53,6 +53,50 @@ class GoalManager extends React.Component { mergeIds, } = this.props; const { goal_panel_open } = this.state; + + let stateBox; + + switch (serverState) { + case "PAUSED": + case "PENDING_PAUSE": + stateBox = (); + break; + case "CRASHED": + stateBox =(); + break; + case "RUNNING": + default: + stateBox =(); + break; + } + + return ( <> {/* Start, Pause and Clear buttons */} @@ -69,31 +113,7 @@ class GoalManager extends React.Component { justifyContent: "center", }} > - {serverState === "PAUSED" || serverState === "PENDING_PAUSE" ? ( - - ) : ( - <> - - - )} + {stateBox} {/* current environment info */} {currentTool === null && ( 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 fd4c1754e..a9b24ecf8 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 @@ -23,6 +23,8 @@ import "leaflet-rotatedmarker"; // enable marker rotation import { MapContainer, TileLayer, ZoomControl } from "react-leaflet"; import { kdTree } from "kd-tree-javascript"; +import {fetchWithTimeout} from "../../index" + import ToolsMenu from "../tools/ToolsMenu"; import GoalManager from "../goal/GoalManager"; import TaskQueue from "../task_queue/TaskQueue"; @@ -224,7 +226,7 @@ class GraphMap extends React.Component { @@ -317,7 +319,7 @@ class GraphMap extends React.Component { fetchGraphState(center = false) { console.info("Fetching the current pose graph state (full)."); - fetch("/vtr/graph") + fetchWithTimeout("/vtr/graph") .then((response) => { if (response.status !== 200) throw new Error("Failed to fetch pose graph state: " + response.status); response.json().then((data) => { @@ -393,7 +395,7 @@ class GraphMap extends React.Component { fetchRobotState() { console.info("Fetching the current robot state (full)."); - fetch("/vtr/robot") + fetchWithTimeout("/vtr/robot") .then((response) => { if (response.status !== 200) throw new Error("Failed to fetch robot state: " + response.status); response.json().then((data) => { @@ -449,7 +451,7 @@ class GraphMap extends React.Component { fetchFollowingRoute() { console.info("Fetching the current following route."); - fetch("/vtr/following_route") + fetchWithTimeout("/vtr/following_route") .then((response) => { if (response.status !== 200) throw new Error("Failed to fetch following route: " + response.status); response.json().then((data) => { @@ -496,7 +498,7 @@ class GraphMap extends React.Component { fetchServerState() { console.info("Fetching the current server state (full)."); - fetch("/vtr/server") + fetchWithTimeout("/vtr/server") .then((response) => { if (response.status !== 200) throw new Error("Failed to fetch server state: " + response.status); response.json().then((data) => { diff --git a/main/src/vtr_gui/vtr_gui/vtr-gui/src/components/task_queue/TaskQueue.js b/main/src/vtr_gui/vtr_gui/vtr-gui/src/components/task_queue/TaskQueue.js index 8d7366a4a..c85c53833 100644 --- a/main/src/vtr_gui/vtr_gui/vtr-gui/src/components/task_queue/TaskQueue.js +++ b/main/src/vtr_gui/vtr_gui/vtr-gui/src/components/task_queue/TaskQueue.js @@ -22,6 +22,8 @@ import { Table, TableBody, TableContainer, TableCell, TableHead, TableRow } from import ArrowBackIosIcon from "@mui/icons-material/ArrowBackIos"; import ArrowForwardIosIcon from "@mui/icons-material/ArrowForwardIos"; import StorageIcon from "@mui/icons-material/Storage"; +import {fetchWithTimeout} from "../../index" + // const TASK_PANEL_WIDTH = 300; @@ -143,7 +145,7 @@ class TaskQueue extends React.Component { fetchTaskQueueState() { console.info("Fetching the current task queue state (full)."); - fetch("/vtr/task_queue") + fetchWithTimeout("/vtr/task_queue") .then((response) => { if (response.status !== 200) throw new Error("Failed to fetch task queue state: " + response.status); response.json().then((data) => { diff --git a/main/src/vtr_gui/vtr_gui/vtr-gui/src/index.js b/main/src/vtr_gui/vtr_gui/vtr-gui/src/index.js index 62c4aee1e..026b8792f 100644 --- a/main/src/vtr_gui/vtr_gui/vtr-gui/src/index.js +++ b/main/src/vtr_gui/vtr_gui/vtr-gui/src/index.js @@ -10,6 +10,17 @@ import GraphMap from "./components/graph/GraphMap"; /// socket io constants and instance const SOCKET = io(window.location.hostname + ":5201"); +export async function fetchWithTimeout(resource, options = {}) { + const { timeoutMS = 5000 } = options; + + const response = await fetch(resource, { + ...options, + signal: AbortSignal.timeout(timeoutMS), + }); + + return response; +} + class VTRGUI extends React.Component { constructor(props) { super(props); diff --git a/main/src/vtr_lidar/include/vtr_lidar/cache.hpp b/main/src/vtr_lidar/include/vtr_lidar/cache.hpp index ab7c427d0..580ea36a4 100644 --- a/main/src/vtr_lidar/include/vtr_lidar/cache.hpp +++ b/main/src/vtr_lidar/include/vtr_lidar/cache.hpp @@ -40,6 +40,7 @@ struct LidarQueryCache : virtual public tactic::QueryCache { // preprocessing tactic::Cache> raw_point_cloud; tactic::Cache> preprocessed_point_cloud; + tactic::Cache> nn_point_cloud; // odometry & mapping tactic::Cache> undistorted_point_cloud; diff --git a/main/src/vtr_lidar/include/vtr_lidar/modules/preprocessing/preprocessing_module.hpp b/main/src/vtr_lidar/include/vtr_lidar/modules/preprocessing/preprocessing_module.hpp index f881b29ef..89516c320 100644 --- a/main/src/vtr_lidar/include/vtr_lidar/modules/preprocessing/preprocessing_module.hpp +++ b/main/src/vtr_lidar/include/vtr_lidar/modules/preprocessing/preprocessing_module.hpp @@ -47,6 +47,8 @@ class PreprocessingModule : public tactic::BaseModule { float r_scale = 4.0; float h_scale = 0.5; float frame_voxel_size = 0.1; + float nn_voxel_size = 0.05; + bool filter_by_normal_score = true; int num_sample1 = 100000; float min_norm_score1 = 0.0; int num_sample2 = 100000; diff --git a/main/src/vtr_lidar/src/modules/localization/localization_icp_module.cpp b/main/src/vtr_lidar/src/modules/localization/localization_icp_module.cpp index 212913c24..16ac00f66 100644 --- a/main/src/vtr_lidar/src/modules/localization/localization_icp_module.cpp +++ b/main/src/vtr_lidar/src/modules/localization/localization_icp_module.cpp @@ -59,6 +59,12 @@ void LocalizationICPModule::run_(QueryCache &qdata0, OutputCache &, const TaskExecutor::Ptr &) { auto &qdata = dynamic_cast(qdata0); + //Check that + if(!*qdata.odo_success) { + CLOG(WARNING, "lidar.localization_icp") << "Odometry failed, skip localization"; + return; + } + // Inputs // const auto &query_stamp = *qdata.stamp; const auto &query_points = *qdata.undistorted_point_cloud; @@ -86,7 +92,7 @@ void LocalizationICPModule::run_(QueryCache &qdata0, OutputCache &, /// use odometry as a prior WeightedLeastSqCostTerm<6>::Ptr prior_cost_term = nullptr; - if (config_->use_pose_prior) { + if (config_->use_pose_prior && *qdata.odo_success) { auto loss_func = L2LossFunc::MakeShared(); auto noise_model = StaticNoiseModel<6>::MakeShared(T_r_v.cov()); auto T_r_v_meas = SE3StateVar::MakeShared(T_r_v); T_r_v_meas->locked() = true; 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 44a18131b..097d263c8 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 @@ -178,6 +178,13 @@ void OdometryICPModule::run_(QueryCache &qdata0, OutputCache &, // Time prev_time(static_cast(timestamp_odo)); Time query_time(static_cast(query_stamp)); + + if (prev_time == query_time) { + CLOG(WARNING, "lidar.odometry") << "Skipping point cloud with duplicate stamp"; + *qdata.odo_success = false; + return; + } + const Eigen::Matrix xi_m_r_in_r_odo((query_time - prev_time).seconds() * w_m_r_in_r_odo); const auto T_r_m_odo_extp = tactic::EdgeTransform(xi_m_r_in_r_odo) * T_r_m_odo; const auto T_r_m_var = SE3StateVar::MakeShared(T_r_m_odo_extp); diff --git a/main/src/vtr_lidar/src/modules/preprocessing/conversions/velodyne_conversion_module_v2.cpp b/main/src/vtr_lidar/src/modules/preprocessing/conversions/velodyne_conversion_module_v2.cpp index 5b5b15748..925b8b3c8 100644 --- a/main/src/vtr_lidar/src/modules/preprocessing/conversions/velodyne_conversion_module_v2.cpp +++ b/main/src/vtr_lidar/src/modules/preprocessing/conversions/velodyne_conversion_module_v2.cpp @@ -107,33 +107,44 @@ void VelodyneConversionModuleV2::run_(QueryCache &qdata0, OutputCache &, // Velodyne has no polar coordinates, so compute them manually. velodyneCart2Pol(*point_cloud); - try { - try{ - sensor_msgs::PointCloud2ConstIterator iter_time(*msg, "t"); - // pointwise timestamp - for (size_t idx = 0; iter_time != iter_time.end(); - ++idx, ++iter_time) { - point_cloud->at(idx).timestamp = static_cast(*iter_time * 1e9); + if (config_->estimate_time){ + CLOG(INFO, "lidar.velodyne_converter_v2") << "Timings wil be estimated from yaw angle"; + estimateTime(*point_cloud, *qdata.stamp, config_->angular_vel); + } else { + try { + try{ + sensor_msgs::PointCloud2ConstIterator iter_time(*msg, "t"); + // pointwise timestamp + for (size_t idx = 0; iter_time != iter_time.end(); + ++idx, ++iter_time) { + point_cloud->at(idx).timestamp = static_cast(*iter_time * 1e9); + } + CLOG(INFO, "lidar.velodyne_converter_v2") << "Timings from t"; } - } - catch (...){ - sensor_msgs::PointCloud2ConstIterator iter_time(*msg, "time"); - // pointwise timestamp - for (size_t idx = 0; iter_time != iter_time.end(); - ++idx, ++iter_time) { - point_cloud->at(idx).timestamp = static_cast(*iter_time * 1e9); + catch (...){ + sensor_msgs::PointCloud2ConstIterator iter_time(*msg, "time"); + // pointwise timestamp + for (size_t idx = 0; iter_time != iter_time.end(); + ++idx, ++iter_time) { + point_cloud->at(idx).timestamp = static_cast(*iter_time * 1e9); + } + CLOG(INFO, "lidar.velodyne_converter_v2") << "Timings from time"; + } } + catch(...){ + CLOG(INFO, "lidar.velodyne_converter_v2") << "Timings wil be estimated from yaw angle"; + estimateTime(*point_cloud, *qdata.stamp, config_->angular_vel); + } } - catch(...){ - CLOG(INFO, "lidar.velodyne_converter_v2") << "Timings wil be estimated from yaw angle"; - estimateTime(*point_cloud, *qdata.stamp, config_->angular_vel); - } + + auto filtered_point_cloud = std::make_shared>(*point_cloud); - CLOG(DEBUG, "lidar.velodyne_converter_v2") << "Reducing the point cloud density by " << config_->horizontal_downsample; + CLOG(DEBUG, "lidar.velodyne_converter_v2") << "Reducing the point cloud density by " << config_->horizontal_downsample + << "original size was " << point_cloud->size(); if (config_->horizontal_downsample > 1) { /// Range cropping diff --git a/main/src/vtr_lidar/src/modules/preprocessing/preprocessing_module.cpp b/main/src/vtr_lidar/src/modules/preprocessing/preprocessing_module.cpp index 68bd6125f..87e6840b9 100644 --- a/main/src/vtr_lidar/src/modules/preprocessing/preprocessing_module.cpp +++ b/main/src/vtr_lidar/src/modules/preprocessing/preprocessing_module.cpp @@ -76,7 +76,10 @@ auto PreprocessingModule::Config::fromROS(const rclcpp::Node::SharedPtr &node, config->r_scale = node->declare_parameter(param_prefix + ".r_scale", config->r_scale); config->h_scale = node->declare_parameter(param_prefix + ".h_scale", config->h_scale); config->frame_voxel_size = node->declare_parameter(param_prefix + ".frame_voxel_size", config->frame_voxel_size); + config->nn_voxel_size = node->declare_parameter(param_prefix + ".nn_voxel_size", config->nn_voxel_size); + + config->filter_by_normal_score = node->declare_parameter(param_prefix + ".filter_normal", config->filter_by_normal_score); config->num_sample1 = node->declare_parameter(param_prefix + ".num_sample1", config->num_sample1); config->min_norm_score1 = node->declare_parameter(param_prefix + ".min_norm_score1", config->min_norm_score1); @@ -118,6 +121,8 @@ void PreprocessingModule::run_(QueryCache &qdata0, OutputCache &, auto filtered_point_cloud = std::make_shared>(*point_cloud); + auto nn_downsampled_cloud = + std::make_shared>(*point_cloud); /// Range cropping { @@ -138,6 +143,7 @@ void PreprocessingModule::run_(QueryCache &qdata0, OutputCache &, // Get subsampling of the frame in carthesian coordinates voxelDownsample(*filtered_point_cloud, config_->frame_voxel_size); + voxelDownsample(*nn_downsampled_cloud, config_->nn_voxel_size); CLOG(DEBUG, "lidar.preprocessing") << "grid subsampled point cloud size: " << filtered_point_cloud->size(); @@ -154,25 +160,38 @@ void PreprocessingModule::run_(QueryCache &qdata0, OutputCache &, /// Filtering based on normal scores (planarity + linearity) - #if true - // Remove points with a low normal score - auto sorted_norm_scores = norm_scores; - std::sort(sorted_norm_scores.begin(), sorted_norm_scores.end()); - float min_score = sorted_norm_scores[std::max( - 0, (int)sorted_norm_scores.size() - config_->num_sample1)]; - min_score = std::max(config_->min_norm_score1, min_score); - if (min_score >= 0) { + if (config_->filter_by_normal_score){ + // Remove points with a low normal score + auto sorted_norm_scores = norm_scores; + std::sort(sorted_norm_scores.begin(), sorted_norm_scores.end()); + float min_score = sorted_norm_scores[std::max( + 0, (int)sorted_norm_scores.size() - config_->num_sample1)]; + min_score = std::max(config_->min_norm_score1, min_score); + if (min_score >= 0) { + std::vector indices; + indices.reserve(filtered_point_cloud->size()); + int i = 0; + for (const auto &point : *filtered_point_cloud) { + if (point.normal_score >= min_score) indices.emplace_back(i); + i++; + } + *filtered_point_cloud = + pcl::PointCloud(*filtered_point_cloud, indices); + } + } else { std::vector indices; indices.reserve(filtered_point_cloud->size()); int i = 0; for (const auto &point : *filtered_point_cloud) { - if (point.normal_score >= min_score) indices.emplace_back(i); + if (i < config_->num_sample1) indices.emplace_back(i); i++; } *filtered_point_cloud = - pcl::PointCloud(*filtered_point_cloud, indices); + pcl::PointCloud(*filtered_point_cloud, indices); + } - #endif + + CLOG(DEBUG, "lidar.preprocessing") << "planarity sampled point size: " << filtered_point_cloud->size(); @@ -243,6 +262,7 @@ void PreprocessingModule::run_(QueryCache &qdata0, OutputCache &, /// Output qdata.preprocessed_point_cloud = filtered_point_cloud; + qdata.nn_point_cloud = nn_downsampled_cloud; } } // namespace lidar diff --git a/main/src/vtr_lidar/src/pipeline.cpp b/main/src/vtr_lidar/src/pipeline.cpp index 16f542235..7d735b2e3 100644 --- a/main/src/vtr_lidar/src/pipeline.cpp +++ b/main/src/vtr_lidar/src/pipeline.cpp @@ -173,6 +173,21 @@ void LidarPipeline::onVertexCreation_(const QueryCache::Ptr &qdata0, CLOG(DEBUG, "lidar.pipeline") << "Saved raw pointcloud to vertex" << vertex; #endif + // raw point cloud + { + auto nn_scan = std::make_shared>(); + nn_scan->point_cloud() = *qdata->nn_point_cloud; + nn_scan->T_vertex_this() = qdata->T_s_r->inverse(); + nn_scan->vertex_id() = *qdata->vid_odo; + // + using PointScanLM = storage::LockableMessage>; + auto nn_scan_odo_msg = + std::make_shared(nn_scan, *qdata->stamp); + vertex->insert>( + "nn_point_cloud", "vtr_lidar_msgs/msg/PointScan", nn_scan_odo_msg); + } + CLOG(DEBUG, "lidar.pipeline") << "Saved nn pointcloud to vertex" << vertex; + /// save the sliding map as vertex submap if we have traveled far enough const bool create_submap = [&] { // diff --git a/main/src/vtr_mission_planning/include/vtr_mission_planning/mission_server/mission_server.hpp b/main/src/vtr_mission_planning/include/vtr_mission_planning/mission_server/mission_server.hpp index 037104381..7ab12e826 100644 --- a/main/src/vtr_mission_planning/include/vtr_mission_planning/mission_server/mission_server.hpp +++ b/main/src/vtr_mission_planning/include/vtr_mission_planning/mission_server/mission_server.hpp @@ -36,7 +36,8 @@ enum class ServerState : int8_t { Empty = -1, // No goals exist Processing = 0, // We are working on 1 or more goals PendingPause, // We are working on a goal, but will pause when done - Paused // Execution is paused, and will resume with more goals + Paused, // Execution is paused, and will resume with more goals + Crashed // Main process has died notify GUI }; std::ostream& operator<<(std::ostream& os, const ServerState& server_state); @@ -199,6 +200,10 @@ void MissionServer::stop() { cv_thread_finish_.wait(lock, [&] { return thread_count_ == 0; }); if (goal_starting_thread_.joinable()) goal_starting_thread_.join(); if (goal_finishing_thread_.joinable()) goal_finishing_thread_.join(); + + current_server_state_ = mission_planning::ServerState::Crashed; + CLOG(DEBUG, "mission.server") << "Setting state to crashed for closing"; + // serverStateChanged(); lock.unlock(); diff --git a/main/src/vtr_mission_planning/src/mission_server/mission_server.cpp b/main/src/vtr_mission_planning/src/mission_server/mission_server.cpp index f7cde5928..92d669c32 100644 --- a/main/src/vtr_mission_planning/src/mission_server/mission_server.cpp +++ b/main/src/vtr_mission_planning/src/mission_server/mission_server.cpp @@ -53,6 +53,9 @@ std::ostream& operator<<(std::ostream& os, const ServerState& server_state) { case ServerState::Paused: os << "Paused"; return os; + case ServerState::Crashed: + os << "Crashed"; + return os; }; return os; } diff --git a/main/src/vtr_navigation/src/navigator.cpp b/main/src/vtr_navigation/src/navigator.cpp index a88abe4f6..2d1fa569c 100644 --- a/main/src/vtr_navigation/src/navigator.cpp +++ b/main/src/vtr_navigation/src/navigator.cpp @@ -221,7 +221,11 @@ void Navigator::envInfoCallback(const tactic::EnvInfo::SharedPtr msg) { void Navigator::lidarCallback( const sensor_msgs::msg::PointCloud2::SharedPtr msg) { LockGuard lock(mutex_); - CLOG(DEBUG, "navigation") << "Received a lidar pointcloud."; + + // set the timestamp + Timestamp timestamp = msg->header.stamp.sec * 1e9 + msg->header.stamp.nanosec; + + CLOG(DEBUG, "navigation") << "Received a lidar pointcloud with stamp " << timestamp; if (pointcloud_in_queue_) { CLOG(WARNING, "navigation") @@ -236,8 +240,6 @@ void Navigator::lidarCallback( // some modules require node for visualization query_data->node = node_; - // set the timestamp - Timestamp timestamp = msg->header.stamp.sec * 1e9 + msg->header.stamp.nanosec; query_data->stamp.emplace(timestamp); // add the current environment info diff --git a/main/src/vtr_navigation/src/ros_mission_server.cpp b/main/src/vtr_navigation/src/ros_mission_server.cpp index 86bbf883e..375d1b7f8 100644 --- a/main/src/vtr_navigation/src/ros_mission_server.cpp +++ b/main/src/vtr_navigation/src/ros_mission_server.cpp @@ -131,6 +131,9 @@ void ROSMissionServer::serverStateChanged() { case mission_planning::ServerState::Processing: server_state_msg_.server_state = ServerStateMsg::PROCESSING; break; + case mission_planning::ServerState::Crashed: + server_state_msg_.server_state = ServerStateMsg::CRASHED; + } // if (server_state_pub_) server_state_pub_->publish(server_state_msg_); diff --git a/main/src/vtr_navigation_msgs/msg/ServerState.msg b/main/src/vtr_navigation_msgs/msg/ServerState.msg index 739fe850a..0b9580625 100644 --- a/main/src/vtr_navigation_msgs/msg/ServerState.msg +++ b/main/src/vtr_navigation_msgs/msg/ServerState.msg @@ -2,6 +2,7 @@ uint8 EMPTY = 0 uint8 PROCESSING = 1 uint8 PENDING_PAUSE = 2 uint8 PAUSED = 3 +uint8 CRASHED = 4 uint8 server_state uint8[16] current_goal_id From e51b4367f404584188112feacf3814370d6a654d Mon Sep 17 00:00:00 2001 From: Alec Krawciw Date: Mon, 1 May 2023 09:25:46 -0400 Subject: [PATCH 13/77] Minor modifications for saving. --- .../modules/planning/change_detection_module_v2.cpp | 11 +++++++++-- .../conversions/honeycomb_conversion_module_v2.cpp | 2 +- 2 files changed, 10 insertions(+), 3 deletions(-) diff --git a/main/src/vtr_lidar/src/modules/planning/change_detection_module_v2.cpp b/main/src/vtr_lidar/src/modules/planning/change_detection_module_v2.cpp index de30d0964..f1d65c395 100644 --- a/main/src/vtr_lidar/src/modules/planning/change_detection_module_v2.cpp +++ b/main/src/vtr_lidar/src/modules/planning/change_detection_module_v2.cpp @@ -409,10 +409,17 @@ void ChangeDetectionModuleV2::runAsync_( CLOG(INFO, "lidar.change_detection") << "Saving change detection result"; const auto ros_msg = std::make_shared(); pcl::toROSMsg(module_result, *ros_msg); - using PoincCloudMsgLM = storage::LockableMessage; - auto msg = std::make_shared(ros_msg, *qdata.stamp); + using PointCloudMsgLM = storage::LockableMessage; + auto msg = std::make_shared(ros_msg, *qdata.stamp); graph->write("change_detection_v2_result", "sensor_msgs/msg/PointCloud2", msg); + + const auto scan_msg = std::make_shared(); + pcl::toROSMsg(aligned_points, *scan_msg); + + auto scan_odo_msg = std::make_shared(scan_msg, *qdata.stamp); + graph->write( + "changed_pointcloud", "sensor_msgs/msg/PointCloud2", scan_odo_msg); } /// publish the transformed pointcloud diff --git a/main/src/vtr_lidar/src/modules/preprocessing/conversions/honeycomb_conversion_module_v2.cpp b/main/src/vtr_lidar/src/modules/preprocessing/conversions/honeycomb_conversion_module_v2.cpp index 2520c4b2b..56b6afef2 100644 --- a/main/src/vtr_lidar/src/modules/preprocessing/conversions/honeycomb_conversion_module_v2.cpp +++ b/main/src/vtr_lidar/src/modules/preprocessing/conversions/honeycomb_conversion_module_v2.cpp @@ -65,7 +65,7 @@ void HoneycombConversionModuleV2::run_(QueryCache &qdata0, OutputCache &, /// following data: /// beam side 0: from -180 to 180 in 0.2 seconds /// beam side 1: from 0 to 180 then -180 to 0 in 0.2 seconds - /// From the Hondycomb documentation, the message time stamp is given at the + /// From the Honeycomb documentation, the message time stamp is given at the /// center of spin, where beam side 0 is 0 degree and beam side 1 is +-180 /// degree. From 4d867969ad12b8ef6c4e0c840d914d52222d38c3 Mon Sep 17 00:00:00 2001 From: Alec Krawciw Date: Tue, 2 May 2023 23:35:58 -0400 Subject: [PATCH 14/77] Initial Torch compilation --- Dockerfile | 13 +- config/honeycomb_grizzly_learned.yaml | 367 ++++++++++++++++++ main/src/vtr_lidar/CMakeLists.txt | 5 +- .../include/vtr_lidar/modules/modules.hpp | 1 + .../modules/odometry/sample_module.hpp | 63 +++ main/src/vtr_lidar/package.xml | 1 + .../src/modules/odometry/sample_module.cpp | 47 +++ main/src/vtr_navigation/CMakeLists.txt | 9 +- main/src/vtr_navigation/package.xml | 1 + main/src/vtr_radar_lidar/CMakeLists.txt | 3 +- main/src/vtr_radar_lidar/package.xml | 1 + main/src/vtr_torch/CMakeLists.txt | 76 ++++ .../vtr_torch/modules/torch_module.hpp | 100 +++++ .../src/vtr_torch/include/vtr_torch/types.hpp | 5 + main/src/vtr_torch/package.xml | 31 ++ .../vtr_torch/src/modules/torch_module.cpp | 55 +++ 16 files changed, 769 insertions(+), 9 deletions(-) create mode 100644 config/honeycomb_grizzly_learned.yaml create mode 100644 main/src/vtr_lidar/include/vtr_lidar/modules/odometry/sample_module.hpp create mode 100644 main/src/vtr_lidar/src/modules/odometry/sample_module.cpp create mode 100644 main/src/vtr_torch/CMakeLists.txt create mode 100644 main/src/vtr_torch/include/vtr_torch/modules/torch_module.hpp create mode 100644 main/src/vtr_torch/include/vtr_torch/types.hpp create mode 100644 main/src/vtr_torch/package.xml create mode 100644 main/src/vtr_torch/src/modules/torch_module.cpp diff --git a/Dockerfile b/Dockerfile index ac0c68a5c..da7d7647b 100644 --- a/Dockerfile +++ b/Dockerfile @@ -1,4 +1,4 @@ -FROM ubuntu:22.04 +FROM nvidia/cuda:11.7.1-cudnn8-devel-ubuntu22.04 CMD ["/bin/bash"] @@ -27,7 +27,8 @@ USER ${USERID}:${GROUPID} ENV VTRROOT=${HOMEDIR}/ASRL/vtr3 ENV VTRSRC=${VTRROOT}/src \ VTRDATA=${VTRROOT}/data \ - VTRTEMP=${VTRROOT}/temp + VTRTEMP=${VTRROOT}/temp \ + VTRMODELS=${VTRROOT}/models ## Switch to root to install dependencies USER 0:0 @@ -50,6 +51,7 @@ RUN mkdir -p ${HOMEDIR}/proj && cd ${HOMEDIR}/proj \ && cmake .. && cmake --build . -j${NUMPROC} --target install ENV LD_LIBRARY_PATH=/usr/local/lib${LD_LIBRARY_PATH:+:${LD_LIBRARY_PATH}} + ## Install ROS2 # UTF-8 RUN apt install -q -y locales \ @@ -99,5 +101,12 @@ RUN apt install htop # Install vim RUN apt update && apt install -q -y vim +##Install LibTorch +RUN curl https://download.pytorch.org/libtorch/cu117/libtorch-cxx11-abi-shared-with-deps-2.0.0%2Bcu117.zip --output libtorch.zip +RUN unzip libtorch.zip -d /opt/torch +ENV TORCH_LIB=/opt/torch/libtorch +ENV LD_LIBRARY_PATH=$TORCH_LIB/lib:${LD_LIBRARY_PATH:+:${LD_LIBRARY_PATH}} +ENV CMAKE_PREFIX_PATH=$TORCH_LIB:$CMAKE_PREFIX_PATH + ## Switch to specified user USER ${USERID}:${GROUPID} diff --git a/config/honeycomb_grizzly_learned.yaml b/config/honeycomb_grizzly_learned.yaml new file mode 100644 index 000000000..1619dde30 --- /dev/null +++ b/config/honeycomb_grizzly_learned.yaml @@ -0,0 +1,367 @@ +/**: + ros__parameters: + log_to_file: true + log_debug: true + log_enabled: + - lidar.terrain_assessment + - navigation + - navigation.graph_map_server + - navigation.command + # tactic + - tactic + - tactic.pipeline + - tactic.module + - tactic.module.live_mem_manager + - tactic.module.graph_mem_manager + # path planner + # "path_planning", + # "path_planning.teb", + # "path_planning.cbit", + # "path_planning.cbit_planner", + # "mpc.cbit", + # "mpc_debug.cbit", + # mission planner + - mission.server + - mission.state_machine + # pose graph + - pose_graph + # pipeline specific + # "lidar.pipeline", + # "lidar.honeycomb_converter", + # "lidar.preprocessing", + # "lidar.odometry_icp", + # "lidar.odometry_map_maintenance", + # "lidar.vertex_test", + # "lidar.localization_map_recall", + # "lidar.localization_icp", + # "lidar.intra_exp_merging", + # "lidar.dynamic_detection", + # "lidar.inter_exp_merging", + # "lidar.change_detection", + # "lidar.ground_extraction", + # "lidar.obstacle_detection", + # "lidar.terrain_assessment", + robot_frame: base_link + env_info_topic: env_info + lidar_frame: honeycomb + lidar_topic: /points + graph_map: + origin_lat: 43.7822 + origin_lng: -79.4661 + origin_theta: 1.3 + scale: 1.0 + tactic: + enable_parallelization: true + preprocessing_skippable: false + odometry_mapping_skippable: false + localization_skippable: false + task_queue_num_threads: 1 + task_queue_size: -1 + route_completion_translation_threshold: 2.0 + chain: + min_cusp_distance: 1.5 + angle_weight: 7.0 + search_depth: 5 + search_back_depth: 10 + distance_warning: 5.0 + save_odometry_result: true + save_localization_result: true + visualize: true + rviz_loc_path_offset: + - 0.0 + - 0.0 + - 0.0 + pipeline: + type: lidar + preprocessing: + - conversion + - filtering + odometry: + - icp + - mapping + - vertex_test + - intra_exp_merging + - dynamic_detection + - inter_exp_merging + - memory + - network + localization: + - recall + - icp + - safe_corridor + - change_detection_sync + - memory + submap_translation_threshold: 1.5 + submap_rotation_threshold: 30.0 + preprocessing: + conversion: + type: lidar.honeycomb_converter_v2 + visualize: true + filtering: + type: lidar.preprocessing + num_threads: 8 + crop_range: 40.0 + frame_voxel_size: 0.2 + 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 + visualize: true + odometry: + network: + type: torch.sample + filepath: /home/alec/ASRL/vtr3/models/sample_model.pt + icp: + type: lidar.odometry_icp + use_trajectory_estimation: true + traj_num_extra_states: 0 + traj_lock_prev_pose: false + traj_lock_prev_vel: false + traj_qc_diag: + - 1.0 + - 0.1 + - 0.1 + - 0.1 + - 0.1 + - 1.0 + num_threads: 8 + first_num_steps: 2 + initial_max_iter: 4 + initial_max_pairing_dist: 1.5 + initial_max_planar_dist: 1.0 + refined_max_iter: 50 + refined_max_pairing_dist: 1.0 + refined_max_planar_dist: 0.3 + averaging_num_steps: 2 + verbose: false + max_iterations: 1 + min_matched_ratio: 0.5 + visualize: true + mapping: + type: lidar.odometry_map_maintenance_v2 + map_voxel_size: 0.2 + crop_range_front: 40.0 + back_over_front_ratio: 0.5 + point_life_time: 20.0 + visualize: true + vertex_test: + type: lidar.vertex_test + max_translation: 0.3 + max_rotation: 10.0 + intra_exp_merging: + type: lidar.intra_exp_merging_v2 + depth: 6.0 + map_voxel_size: 0.2 + 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 + min_num_observations: 4 + dynamic_threshold: 0.3 + visualize: true + inter_exp_merging: + type: lidar.inter_exp_merging_v2 + map_voxel_size: 0.2 + max_num_experiences: 128 + distance_threshold: 0.6 + planar_threshold: 0.2 + normal_threshold: 0.8 + dynamic_obs_threshold: 1 + visualize: true + memory: + type: live_mem_manager + window_size: 5 + localization: + recall: + type: lidar.localization_map_recall + map_version: pointmap + visualize: true + icp: + type: lidar.localization_icp + use_pose_prior: true + num_threads: 8 + first_num_steps: 2 + initial_max_iter: 4 + initial_max_pairing_dist: 1.5 + initial_max_planar_dist: 1.0 + refined_max_iter: 50 + refined_max_pairing_dist: 1.0 + refined_max_planar_dist: 0.3 + averaging_num_steps: 2 + verbose: false + max_iterations: 1 + min_matched_ratio: 0.3 + safe_corridor: + type: lidar.safe_corridor + lookahead_distance: 5.0 + corridor_width: 3.5 + influence_distance: 1.0 + resolution: 0.25 + size_x: 16.0 + size_y: 8.0 + visualize: true + change_detection_sync: + type: lidar.change_detection_v3 + detection_range: 8.0 + search_radius: 0.25 + negprob_threshold: 0.1 + use_prior: true + alpha0: 3.0 + beta0: 0.03 + use_support_filtering: true + support_radius: 0.25 + support_variance: 0.05 + support_threshold: 2.5 + influence_distance: 0.55 + minimum_distance: 0.35 + resolution: 0.25 + size_x: 16.0 + size_y: 8.0 + visualize: true + change_detection: + type: lidar.change_detection_v2 + detection_range: 8 + search_radius: 0.25 + negprob_threshold: 0.1 + use_prior: true + alpha0: 3.0 + beta0: 0.03 + use_support_filtering: true + support_radius: 0.25 + support_variance: 0.05 + support_threshold: 2.5 + resolution: 0.5 + size_x: 16.0 + size_y: 8.0 + run_online: false + run_async: true + visualize: false + save_module_result: false + memory: + type: graph_mem_manager + vertex_life_span: 5 + window_size: 3 + obstacle_detection: + type: lidar.obstacle_detection + z_min: 0.5 + z_max: 2.0 + resolution: 0.6 + size_x: 40.0 + size_y: 20.0 + run_async: true + visualize: true + ground_extraction: + type: lidar.ground_extraction + z_offset: 0.2 + alpha: 0.035 + tolerance: 0.25 + Tm: 0.3 + Tm_small: 0.1 + Tb: 0.5 + Trmse: 0.1 + Tdprev: 1.0 + rmin: 2.0 + num_bins_small: 30.0 + bin_size_small: 0.5 + num_bins_large: 10.0 + bin_size_large: 1.0 + resolution: 0.6 + size_x: 40.0 + size_y: 20.0 + run_async: true + visualize: true + terrain_assessment: + type: lidar.terrain_assessment + lookahead_distance: 15.0 + corridor_width: 1.0 + search_radius: 1.0 + resolution: 0.5 + size_x: 40.0 + size_y: 20.0 + run_online: false + run_async: true + visualize: true + path_planning: + type: cbit + control_period: 33 + teb: + visualize: true + extrapolate: false + extrapolation_timeout: 2.0 + lookahead_distance: 8.5 + robot_model: circular + robot_radius: 0.5 + map_frame: planning frame + enable_homotopy_class_planning: true + free_goal_vel: true + max_vel_x: 0.6 + max_vel_y: 0.0 + max_vel_theta: 0.3 + min_turning_radius: 3 + weight_viapoint: 0.5 + weight_costmap: 100.0 + cbit: + obs_padding: 0.0 + curv_to_euclid_discretization: 20 + sliding_window_width: 10.0 + sliding_window_freespace_padding: 0.5 + corridor_resolution: 0.05 + state_update_freq: 0.5 + update_state: true + rand_seed: 1 + initial_samples: 250 + batch_samples: 100 + 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 + initial_exp_rad: 0.75 + extrapolation: false + incremental_plotting: false + plotting: true + costmap: + costmap_filter_value: 0.01 + costmap_history: 15 + mpc: + horizon_steps: 40 + horizon_step_size: 0.1 + forward_vel: 1.0 + 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 + command_history_length: 100 diff --git a/main/src/vtr_lidar/CMakeLists.txt b/main/src/vtr_lidar/CMakeLists.txt index 806239eeb..d8243e281 100644 --- a/main/src/vtr_lidar/CMakeLists.txt +++ b/main/src/vtr_lidar/CMakeLists.txt @@ -27,6 +27,7 @@ find_package(vtr_lidar_msgs REQUIRED) find_package(vtr_logging REQUIRED) find_package(vtr_path_planning REQUIRED) find_package(vtr_tactic REQUIRED) +find_package(vtr_torch REQUIRED) ## C++ Libraries include_directories(PUBLIC @@ -56,7 +57,7 @@ target_link_libraries(${PROJECT_NAME}_pipeline ${PROJECT_NAME}_components) ament_target_dependencies(${PROJECT_NAME}_pipeline Eigen3 pcl_conversions pcl_ros nav_msgs visualization_msgs - lgmath steam + lgmath steam vtr_torch vtr_logging vtr_tactic vtr_lidar_msgs ) @@ -86,7 +87,7 @@ ament_export_dependencies( Eigen3 pcl_conversions pcl_ros nav_msgs visualization_msgs lgmath steam - vtr_logging vtr_tactic vtr_path_planning vtr_lidar_msgs + vtr_logging vtr_tactic vtr_path_planning vtr_lidar_msgs vtr_torch ) install( diff --git a/main/src/vtr_lidar/include/vtr_lidar/modules/modules.hpp b/main/src/vtr_lidar/include/vtr_lidar/modules/modules.hpp index b5a9db8c7..05fcbee6d 100644 --- a/main/src/vtr_lidar/include/vtr_lidar/modules/modules.hpp +++ b/main/src/vtr_lidar/include/vtr_lidar/modules/modules.hpp @@ -30,6 +30,7 @@ #include "vtr_lidar/modules/odometry/odometry_map_maintenance_module.hpp" #include "vtr_lidar/modules/odometry/odometry_map_maintenance_module_v2.hpp" #include "vtr_lidar/modules/odometry/vertex_test_module.hpp" +#include "vtr_lidar/modules/odometry/sample_module.hpp" #include "vtr_lidar/modules/localization/localization_icp_module.hpp" #include "vtr_lidar/modules/localization/localization_map_recall_module.hpp" diff --git a/main/src/vtr_lidar/include/vtr_lidar/modules/odometry/sample_module.hpp b/main/src/vtr_lidar/include/vtr_lidar/modules/odometry/sample_module.hpp new file mode 100644 index 000000000..264a41d3a --- /dev/null +++ b/main/src/vtr_lidar/include/vtr_lidar/modules/odometry/sample_module.hpp @@ -0,0 +1,63 @@ +// Copyright 2023, 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 sample_module.hpp + * \author Alec Krawciw, Autonomous Space Robotics Lab (ASRL) + */ +#pragma once + +#include "vtr_tactic/modules/base_module.hpp" +#include "vtr_tactic/task_queue.hpp" +#include +#include "vtr_lidar/cache.hpp" +#include + +namespace vtr { +namespace lidar { + +/** \brief Load and store Torch Models */ +class TestNNModule : public nn::TorchModule { + public: + PTR_TYPEDEFS(TestNNModule); + /** \brief Static module identifier. */ + static constexpr auto static_name = "torch.sample"; + + /** \brief Config parameters. */ + struct Config : public nn::TorchModule::Config { + PTR_TYPEDEFS(Config); + + static ConstPtr fromROS(const rclcpp::Node::SharedPtr &node, + const std::string ¶m_prefix); + }; + + TestNNModule( + const Config::ConstPtr &config, + const std::shared_ptr &module_factory = nullptr, + const std::string &name = static_name) + : nn::TorchModule{config, module_factory, name}, config_(config) {} + + private: + void run_(tactic::QueryCache &qdata, tactic::OutputCache &output, + const tactic::Graph::Ptr &graph, + const tactic::TaskExecutor::Ptr &executor) override; + + Config::ConstPtr config_; + + VTR_REGISTER_MODULE_DEC_TYPE(TestNNModule); + +}; + +} // namespace nn +} // namespace vtr \ No newline at end of file diff --git a/main/src/vtr_lidar/package.xml b/main/src/vtr_lidar/package.xml index bb8b0dcc2..0bbaaa546 100644 --- a/main/src/vtr_lidar/package.xml +++ b/main/src/vtr_lidar/package.xml @@ -31,6 +31,7 @@ vtr_logging vtr_path_planning vtr_tactic + vtr_torch ament_cmake_gmock ament_lint_auto diff --git a/main/src/vtr_lidar/src/modules/odometry/sample_module.cpp b/main/src/vtr_lidar/src/modules/odometry/sample_module.cpp new file mode 100644 index 000000000..3749b0128 --- /dev/null +++ b/main/src/vtr_lidar/src/modules/odometry/sample_module.cpp @@ -0,0 +1,47 @@ +// Copyright 2023, 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 torch_module.cpp + * \author Alec Krawciw, Autonomous Space Robotics Lab (ASRL) + */ +#include + + +namespace vtr { +namespace lidar { + +using namespace tactic; + +auto TestNNModule::Config::fromROS(const rclcpp::Node::SharedPtr &node, + const std::string ¶m_prefix) + -> ConstPtr { + auto config = std::make_shared(); + // clang-format off + // clang-format on + return config; +} + + +void TestNNModule::run_(QueryCache &qdata0, OutputCache &, + const Graph::Ptr &, const TaskExecutor::Ptr &) { + auto &qdata = dynamic_cast(qdata0); + + std::vector inputs {2, 5}; + evaluateModel(inputs); + +} + +} // namespace nn +} // namespace vtr \ No newline at end of file diff --git a/main/src/vtr_navigation/CMakeLists.txt b/main/src/vtr_navigation/CMakeLists.txt index 60821d024..4872595c6 100644 --- a/main/src/vtr_navigation/CMakeLists.txt +++ b/main/src/vtr_navigation/CMakeLists.txt @@ -21,6 +21,7 @@ find_package(vtr_pose_graph REQUIRED) find_package(vtr_tactic REQUIRED) find_package(vtr_mission_planning REQUIRED) find_package(vtr_navigation_msgs REQUIRED) +find_package(vtr_torch REQUIRED) ## TODO make these two optional (depending on which pipeline to use) find_package(vtr_lidar REQUIRED) @@ -67,8 +68,8 @@ add_library(${PROJECT_NAME}_navigator ${SRC}) ament_target_dependencies(${PROJECT_NAME}_navigator rclcpp tf2 tf2_ros sensor_msgs # for visualization only - vtr_common vtr_logging vtr_pose_graph vtr_tactic vtr_mission_planning - vtr_lidar + vtr_common vtr_logging vtr_pose_graph vtr_tactic vtr_mission_planning vtr_torch + vtr_lidar # vtr_vision ) target_link_libraries(${PROJECT_NAME}_navigator @@ -85,9 +86,9 @@ target_include_directories(${PROJECT_NAME}_navigator ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) ament_export_dependencies( rclcpp tf2 tf2_ros - vtr_common vtr_logging vtr_pose_graph vtr_tactic vtr_mission_planning + vtr_common vtr_logging vtr_pose_graph vtr_tactic vtr_mission_planning vtr_torch vtr_navigation_msgs - vtr_lidar + vtr_lidar # vtr_vision PROJ ) diff --git a/main/src/vtr_navigation/package.xml b/main/src/vtr_navigation/package.xml index 567db1523..2bff739ab 100644 --- a/main/src/vtr_navigation/package.xml +++ b/main/src/vtr_navigation/package.xml @@ -25,6 +25,7 @@ vtr_navigation_msgs vtr_lidar + vtr_torch ament_cmake_gtest diff --git a/main/src/vtr_radar_lidar/CMakeLists.txt b/main/src/vtr_radar_lidar/CMakeLists.txt index e0a4ca618..509bb6ecb 100644 --- a/main/src/vtr_radar_lidar/CMakeLists.txt +++ b/main/src/vtr_radar_lidar/CMakeLists.txt @@ -29,6 +29,7 @@ find_package(vtr_logging REQUIRED) find_package(vtr_tactic REQUIRED) find_package(vtr_radar REQUIRED) find_package(vtr_lidar REQUIRED) +find_package(vtr_torch REQUIRED) ## C++ Libraries include_directories(PUBLIC @@ -46,7 +47,7 @@ ament_target_dependencies(${PROJECT_NAME}_pipeline Eigen3 OpenCV cv_bridge pcl_conversions pcl_ros lgmath steam - vtr_common vtr_logging vtr_tactic vtr_radar vtr_lidar + vtr_common vtr_logging vtr_tactic vtr_radar vtr_lidar vtr_torch nav_msgs visualization_msgs ) diff --git a/main/src/vtr_radar_lidar/package.xml b/main/src/vtr_radar_lidar/package.xml index d72e84c00..ce1b0ae99 100644 --- a/main/src/vtr_radar_lidar/package.xml +++ b/main/src/vtr_radar_lidar/package.xml @@ -31,6 +31,7 @@ vtr_tactic vtr_radar vtr_lidar + vtr_torch ament_cmake_gmock ament_lint_auto diff --git a/main/src/vtr_torch/CMakeLists.txt b/main/src/vtr_torch/CMakeLists.txt new file mode 100644 index 000000000..77feceb2b --- /dev/null +++ b/main/src/vtr_torch/CMakeLists.txt @@ -0,0 +1,76 @@ +cmake_minimum_required(VERSION 3.8) +project(vtr_torch) + + +## Common setup for vtr packages +include("${CMAKE_CURRENT_LIST_DIR}/../vtr_common/vtr_include.cmake") + +find_package(ament_cmake REQUIRED) +find_package(eigen3_cmake_module REQUIRED) + +find_package(Eigen3 REQUIRED) + +find_package(rclcpp REQUIRED) + +find_package(vtr_common REQUIRED) +find_package(vtr_tactic REQUIRED) +find_package(Torch REQUIRED) +set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${TORCH_CXX_FLAGS}") + +# find dependencies +find_package(ament_cmake REQUIRED) + + + +include_directories(PUBLIC + $ + $ + ${TORCH_INCLUDE_DIRS} +) + +file(GLOB_RECURSE MODULE_SRC + src/modules/torch_module.cpp +) +add_library(${PROJECT_NAME}_modules ${MODULE_SRC}) +target_link_libraries(${PROJECT_NAME}_modules ${TORCH_LIBRARIES}) + +ament_target_dependencies(${PROJECT_NAME}_modules + Eigen3 Torch + vtr_common vtr_logging vtr_tactic +) + + +ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) +ament_export_dependencies( + Eigen3 Torch + vtr_common vtr_logging vtr_tactic +) + +install( + DIRECTORY include/ + DESTINATION include +) + +install( + TARGETS + ${PROJECT_NAME}_modules + EXPORT export_${PROJECT_NAME} + LIBRARY DESTINATION lib + ARCHIVE DESTINATION lib + RUNTIME DESTINATION lib/${PROJECT_NAME} + INCLUDES DESTINATION include +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # comment the line when a copyright and license is added to all source files + set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # comment the line when this package is in a git repo and when + # a copyright and license is added to all source files + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/main/src/vtr_torch/include/vtr_torch/modules/torch_module.hpp b/main/src/vtr_torch/include/vtr_torch/modules/torch_module.hpp new file mode 100644 index 000000000..790dad5ac --- /dev/null +++ b/main/src/vtr_torch/include/vtr_torch/modules/torch_module.hpp @@ -0,0 +1,100 @@ +// Copyright 2023, 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 torch_module.hpp + * \author Alec Krawciw, Autonomous Space Robotics Lab (ASRL) + */ +#pragma once + +//#define C10_UTIL_LOGGING_IS_NOT_GOOGLE_GLOG_H_ +#include "vtr_tactic/modules/base_module.hpp" +#include "vtr_tactic/task_queue.hpp" +#include +#include +#include "vtr_torch/types.hpp" +#include + +namespace vtr { +namespace nn { + +/** \brief Load and store Torch Models */ +class TorchModule : public tactic::BaseModule { + public: + PTR_TYPEDEFS(TorchModule); + /** \brief Static module identifier. */ + static constexpr auto static_name = "torch"; + + /** \brief Config parameters. */ + struct Config : public tactic::BaseModule::Config { + PTR_TYPEDEFS(Config); + + std::string model_filepath = ""; + + static ConstPtr fromROS(const rclcpp::Node::SharedPtr &node, + const std::string ¶m_prefix); + + virtual ~Config() = default; // for polymorphism + + }; + + TorchModule( + const Config::ConstPtr &config, + const std::shared_ptr &module_factory = nullptr, + const std::string &name = static_name) + : tactic::BaseModule{module_factory, name}, config_(config) { + + //Load the model + //Should the system crash out if the model is loaded incorrectly? + try { + // Deserialize the ScriptModule from a file using torch::jit::load(). + network = torch::jit::load(config_->model_filepath); + } catch (const c10::Error& e) { + CLOG(ERROR, "torch") << "error loading the model\n"; + } + + //Copy the model weights into the graph folder for saving? + + } + + + virtual ~TorchModule(); + + private: + virtual void run_(tactic::QueryCache &qdata, tactic::OutputCache &output, + const tactic::Graph::Ptr &graph, + const tactic::TaskExecutor::Ptr &executor) = 0; + + Config::ConstPtr config_; + + protected: + Module network; + + template + std::vector evaluateModel(std::vector inputs){ + torch::NoGradGuard no_grad; + std::vector jit_inputs; + jit_inputs.reserve(inputs.size()); + + for (auto& val : inputs) + jit_inputs.push_back(val); + + auto output = network.forward(jit_inputs).toTensor(); + std::cout << output; + } + +}; + +} // namespace nn +} // namespace vtr \ No newline at end of file diff --git a/main/src/vtr_torch/include/vtr_torch/types.hpp b/main/src/vtr_torch/include/vtr_torch/types.hpp new file mode 100644 index 000000000..3b331e965 --- /dev/null +++ b/main/src/vtr_torch/include/vtr_torch/types.hpp @@ -0,0 +1,5 @@ + +#include + +using ModulePtr = std::shared_ptr; +using Module = torch::jit::Module; diff --git a/main/src/vtr_torch/package.xml b/main/src/vtr_torch/package.xml new file mode 100644 index 000000000..4814e2dde --- /dev/null +++ b/main/src/vtr_torch/package.xml @@ -0,0 +1,31 @@ + + + + vtr_torch + 0.0.0 + Base modules and helpers for creating modules that using TorchScript + Alec Krawciw + Apache License 2.0 + + ament_cmake + + + rclcpp + tf2 + tf2_ros + tf2_eigen + sensor_msgs + nav_msgs + + vtr_logging + vtr_pose_graph + vtr_tactic + TorchScript + + ament_cmake_gtest + ament_lint_auto + + + ament_cmake + + diff --git a/main/src/vtr_torch/src/modules/torch_module.cpp b/main/src/vtr_torch/src/modules/torch_module.cpp new file mode 100644 index 000000000..a8f9a341f --- /dev/null +++ b/main/src/vtr_torch/src/modules/torch_module.cpp @@ -0,0 +1,55 @@ +// Copyright 2023, 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 torch_module.cpp + * \author Alec Krawciw, Autonomous Space Robotics Lab (ASRL) + */ +#include "vtr_torch/modules/torch_module.hpp" + + +namespace vtr { +namespace nn { + +using namespace tactic; + +auto TorchModule::Config::fromROS(const rclcpp::Node::SharedPtr &node, + const std::string ¶m_prefix) + -> ConstPtr { + auto config = std::make_shared(); + // clang-format off + + config->model_filepath = node->declare_parameter(param_prefix + ".filepath", config->model_filepath); + // clang-format on + return config; +} + +TorchModule::~TorchModule() {} + +// template +// std::vector TorchModule::evaluateModel(std::vector inputs){ +// torch::NoGradGuard no_grad; +// std::vector jit_inputs; +// jit_inputs.reserve(inputs.size()); + +// for (auto& val : inputs) +// jit_inputs.push_back(val); + +// auto output = network.forward(jit_inputs).toTensor(); +// std::cout << output; +// } + + +} // namespace nn +} // namespace vtr \ No newline at end of file From 9401cae3eadaad67f8fa9cc28d7421b02adfe735 Mon Sep 17 00:00:00 2001 From: Alec Krawciw Date: Tue, 2 May 2023 23:53:34 -0400 Subject: [PATCH 15/77] Loads and crashes torch model! --- launch/offline_honeycomb_grizzly.launch.yaml | 2 +- main/src/vtr_lidar/src/modules/odometry/sample_module.cpp | 4 +++- main/src/vtr_torch/include/vtr_torch/modules/torch_module.hpp | 2 +- 3 files changed, 5 insertions(+), 3 deletions(-) diff --git a/launch/offline_honeycomb_grizzly.launch.yaml b/launch/offline_honeycomb_grizzly.launch.yaml index 5cde9165f..1afc196de 100644 --- a/launch/offline_honeycomb_grizzly.launch.yaml +++ b/launch/offline_honeycomb_grizzly.launch.yaml @@ -17,7 +17,7 @@ windows: panes: - > ros2 launch vtr_navigation vtr.launch.py - base_params:=honeycomb_grizzly_default.yaml + base_params:=honeycomb_grizzly_learned.yaml data_dir:=${VTRTEMP}/lidar start_new_graph:=true use_sim_time:=true diff --git a/main/src/vtr_lidar/src/modules/odometry/sample_module.cpp b/main/src/vtr_lidar/src/modules/odometry/sample_module.cpp index 3749b0128..d070ba665 100644 --- a/main/src/vtr_lidar/src/modules/odometry/sample_module.cpp +++ b/main/src/vtr_lidar/src/modules/odometry/sample_module.cpp @@ -27,7 +27,9 @@ using namespace tactic; auto TestNNModule::Config::fromROS(const rclcpp::Node::SharedPtr &node, const std::string ¶m_prefix) -> ConstPtr { - auto config = std::make_shared(); + auto config = std::make_shared(); + auto base_config = std::static_pointer_cast(config); + *base_config = *nn::TorchModule::Config::fromROS(node, param_prefix); // clang-format off // clang-format on return config; diff --git a/main/src/vtr_torch/include/vtr_torch/modules/torch_module.hpp b/main/src/vtr_torch/include/vtr_torch/modules/torch_module.hpp index 790dad5ac..5db8cbaf5 100644 --- a/main/src/vtr_torch/include/vtr_torch/modules/torch_module.hpp +++ b/main/src/vtr_torch/include/vtr_torch/modules/torch_module.hpp @@ -61,7 +61,7 @@ class TorchModule : public tactic::BaseModule { // Deserialize the ScriptModule from a file using torch::jit::load(). network = torch::jit::load(config_->model_filepath); } catch (const c10::Error& e) { - CLOG(ERROR, "torch") << "error loading the model\n"; + CLOG(ERROR, "torch") << "error loading the model\n" << "Tried to load " << config_->model_filepath; } //Copy the model weights into the graph folder for saving? From e8d821fe10f2f2829a1945dd3699dd8c95fc19aa Mon Sep 17 00:00:00 2001 From: Alec Krawciw Date: Wed, 3 May 2023 10:05:21 -0400 Subject: [PATCH 16/77] Running models on GPU --- Dockerfile | 2 ++ config/honeycomb_grizzly_learned.yaml | 5 ++-- launch/offline_honeycomb_grizzly.launch.yaml | 1 + .../src/modules/odometry/sample_module.cpp | 4 +-- .../vtr_torch/modules/torch_module.hpp | 26 ++++++++++--------- .../vtr_torch/modules/torch_module.inl | 24 +++++++++++++++++ .../src/vtr_torch/include/vtr_torch/types.hpp | 2 ++ .../vtr_torch/src/modules/torch_module.cpp | 14 ++-------- 8 files changed, 50 insertions(+), 28 deletions(-) create mode 100644 main/src/vtr_torch/include/vtr_torch/modules/torch_module.inl diff --git a/Dockerfile b/Dockerfile index da7d7647b..add482e74 100644 --- a/Dockerfile +++ b/Dockerfile @@ -108,5 +108,7 @@ ENV TORCH_LIB=/opt/torch/libtorch ENV LD_LIBRARY_PATH=$TORCH_LIB/lib:${LD_LIBRARY_PATH:+:${LD_LIBRARY_PATH}} ENV CMAKE_PREFIX_PATH=$TORCH_LIB:$CMAKE_PREFIX_PATH +ENV NVIDIA_DRIVER_CAPABILITIES compute,utility,graphics + ## Switch to specified user USER ${USERID}:${GROUPID} diff --git a/config/honeycomb_grizzly_learned.yaml b/config/honeycomb_grizzly_learned.yaml index 1619dde30..7821f1c40 100644 --- a/config/honeycomb_grizzly_learned.yaml +++ b/config/honeycomb_grizzly_learned.yaml @@ -3,8 +3,7 @@ log_to_file: true log_debug: true log_enabled: - - lidar.terrain_assessment - - navigation + #- navigation - navigation.graph_map_server - navigation.command # tactic @@ -13,6 +12,7 @@ - tactic.module - tactic.module.live_mem_manager - tactic.module.graph_mem_manager + - torch # path planner # "path_planning", # "path_planning.teb", @@ -117,6 +117,7 @@ odometry: network: type: torch.sample + use_gpu: true filepath: /home/alec/ASRL/vtr3/models/sample_model.pt icp: type: lidar.odometry_icp diff --git a/launch/offline_honeycomb_grizzly.launch.yaml b/launch/offline_honeycomb_grizzly.launch.yaml index 1afc196de..4bd5b45fe 100644 --- a/launch/offline_honeycomb_grizzly.launch.yaml +++ b/launch/offline_honeycomb_grizzly.launch.yaml @@ -19,6 +19,7 @@ windows: ros2 launch vtr_navigation vtr.launch.py base_params:=honeycomb_grizzly_learned.yaml data_dir:=${VTRTEMP}/lidar + model_dir:=${VTRMODELS} start_new_graph:=true use_sim_time:=true diff --git a/main/src/vtr_lidar/src/modules/odometry/sample_module.cpp b/main/src/vtr_lidar/src/modules/odometry/sample_module.cpp index d070ba665..e9d4cc866 100644 --- a/main/src/vtr_lidar/src/modules/odometry/sample_module.cpp +++ b/main/src/vtr_lidar/src/modules/odometry/sample_module.cpp @@ -40,8 +40,8 @@ void TestNNModule::run_(QueryCache &qdata0, OutputCache &, const Graph::Ptr &, const TaskExecutor::Ptr &) { auto &qdata = dynamic_cast(qdata0); - std::vector inputs {2, 5}; - evaluateModel(inputs); + std::vector inputs {2, 5}; + evaluateModel(inputs, {1, 2}); } diff --git a/main/src/vtr_torch/include/vtr_torch/modules/torch_module.hpp b/main/src/vtr_torch/include/vtr_torch/modules/torch_module.hpp index 5db8cbaf5..f869442b9 100644 --- a/main/src/vtr_torch/include/vtr_torch/modules/torch_module.hpp +++ b/main/src/vtr_torch/include/vtr_torch/modules/torch_module.hpp @@ -41,6 +41,8 @@ class TorchModule : public tactic::BaseModule { PTR_TYPEDEFS(Config); std::string model_filepath = ""; + bool use_gpu = false; + bool abs_filepath = true; static ConstPtr fromROS(const rclcpp::Node::SharedPtr &node, const std::string ¶m_prefix); @@ -64,6 +66,12 @@ class TorchModule : public tactic::BaseModule { CLOG(ERROR, "torch") << "error loading the model\n" << "Tried to load " << config_->model_filepath; } + if (config_->use_gpu && torch::cuda::is_available()){ + device = torch::kCUDA; + network.to(device); + } + CLOG(INFO, "torch") << "Using device " << device << std::endl; + //Copy the model weights into the graph folder for saving? } @@ -77,24 +85,18 @@ class TorchModule : public tactic::BaseModule { const tactic::TaskExecutor::Ptr &executor) = 0; Config::ConstPtr config_; + torch::Device device = torch::kCPU; + protected: Module network; template - std::vector evaluateModel(std::vector inputs){ - torch::NoGradGuard no_grad; - std::vector jit_inputs; - jit_inputs.reserve(inputs.size()); - - for (auto& val : inputs) - jit_inputs.push_back(val); - - auto output = network.forward(jit_inputs).toTensor(); - std::cout << output; - } + void evaluateModel(std::vector inputs, const at::IntArrayRef shape); }; } // namespace nn -} // namespace vtr \ No newline at end of file +} // namespace vtr + +#include "torch_module.inl" \ No newline at end of file diff --git a/main/src/vtr_torch/include/vtr_torch/modules/torch_module.inl b/main/src/vtr_torch/include/vtr_torch/modules/torch_module.inl new file mode 100644 index 000000000..6d5358f1e --- /dev/null +++ b/main/src/vtr_torch/include/vtr_torch/modules/torch_module.inl @@ -0,0 +1,24 @@ +#include +#include +#include "vtr_torch/types.hpp" + +using namespace torch::indexing; + +namespace vtr { +namespace nn { + + template + void TorchModule::evaluateModel(std::vector inputs, const Shape shape){ + torch::NoGradGuard no_grad; + std::vector jit_inputs; + + auto t_input = torch::from_blob(inputs.data(), shape).to(device); + jit_inputs.push_back(t_input); + + + auto output = network.forward(jit_inputs); + CLOG(DEBUG, "torch") << output; + } + +} // namespace nn +} // namespace vtr \ No newline at end of file diff --git a/main/src/vtr_torch/include/vtr_torch/types.hpp b/main/src/vtr_torch/include/vtr_torch/types.hpp index 3b331e965..574ae4f48 100644 --- a/main/src/vtr_torch/include/vtr_torch/types.hpp +++ b/main/src/vtr_torch/include/vtr_torch/types.hpp @@ -3,3 +3,5 @@ using ModulePtr = std::shared_ptr; using Module = torch::jit::Module; + +using Shape = at::IntArrayRef; diff --git a/main/src/vtr_torch/src/modules/torch_module.cpp b/main/src/vtr_torch/src/modules/torch_module.cpp index a8f9a341f..484ced9a9 100644 --- a/main/src/vtr_torch/src/modules/torch_module.cpp +++ b/main/src/vtr_torch/src/modules/torch_module.cpp @@ -31,24 +31,14 @@ auto TorchModule::Config::fromROS(const rclcpp::Node::SharedPtr &node, // clang-format off config->model_filepath = node->declare_parameter(param_prefix + ".filepath", config->model_filepath); + config->use_gpu = node->declare_parameter(param_prefix + ".use_gpu", config->use_gpu); + config->abs_filepath = node->declare_parameter(param_prefix + ".abs_filepath", config->abs_filepath); // clang-format on return config; } TorchModule::~TorchModule() {} -// template -// std::vector TorchModule::evaluateModel(std::vector inputs){ -// torch::NoGradGuard no_grad; -// std::vector jit_inputs; -// jit_inputs.reserve(inputs.size()); - -// for (auto& val : inputs) -// jit_inputs.push_back(val); - -// auto output = network.forward(jit_inputs).toTensor(); -// std::cout << output; -// } } // namespace nn From 920e95ca3212aff15d59fd57c8e79a1a38d3e3fa Mon Sep 17 00:00:00 2001 From: Alec Krawciw Date: Wed, 3 May 2023 11:46:16 -0400 Subject: [PATCH 17/77] Added relative file paths --- config/honeycomb_grizzly_learned.yaml | 3 ++- main/src/vtr_navigation/launch/vtr.launch.py | 2 ++ .../include/vtr_torch/modules/torch_module.hpp | 3 ++- main/src/vtr_torch/src/modules/torch_module.cpp | 10 +++++++++- 4 files changed, 15 insertions(+), 3 deletions(-) diff --git a/config/honeycomb_grizzly_learned.yaml b/config/honeycomb_grizzly_learned.yaml index 7821f1c40..ba5bc1ccd 100644 --- a/config/honeycomb_grizzly_learned.yaml +++ b/config/honeycomb_grizzly_learned.yaml @@ -118,7 +118,8 @@ network: type: torch.sample use_gpu: true - filepath: /home/alec/ASRL/vtr3/models/sample_model.pt + abs_filepath: false + filepath: sample_model.pt icp: type: lidar.odometry_icp use_trajectory_estimation: true diff --git a/main/src/vtr_navigation/launch/vtr.launch.py b/main/src/vtr_navigation/launch/vtr.launch.py index 459b43bdf..19859ecfa 100644 --- a/main/src/vtr_navigation/launch/vtr.launch.py +++ b/main/src/vtr_navigation/launch/vtr.launch.py @@ -13,6 +13,7 @@ def generate_launch_description(): return LaunchDescription([ DeclareLaunchArgument('data_dir', description='data directory (store log files and result pose graph)'), + DeclareLaunchArgument('model_dir', description='model directory (folder for PyTorch .pt models)'), DeclareLaunchArgument('start_new_graph', default_value='false', description='whether to start a new pose graph'), DeclareLaunchArgument('use_sim_time', default_value='false', description='use simulated time for playback'), DeclareLaunchArgument('base_params', description='base parameter file (sensor, robot specific)'), @@ -26,6 +27,7 @@ def generate_launch_description(): parameters=[ { "data_dir": LaunchConfiguration("data_dir"), + "model_dir": LaunchConfiguration("model_dir"), "start_new_graph": LaunchConfiguration("start_new_graph"), "use_sim_time": LaunchConfiguration("use_sim_time"), }, diff --git a/main/src/vtr_torch/include/vtr_torch/modules/torch_module.hpp b/main/src/vtr_torch/include/vtr_torch/modules/torch_module.hpp index f869442b9..8592d42fc 100644 --- a/main/src/vtr_torch/include/vtr_torch/modules/torch_module.hpp +++ b/main/src/vtr_torch/include/vtr_torch/modules/torch_module.hpp @@ -20,6 +20,7 @@ //#define C10_UTIL_LOGGING_IS_NOT_GOOGLE_GLOG_H_ #include "vtr_tactic/modules/base_module.hpp" +#include "vtr_common/utils/filesystem.hpp" #include "vtr_tactic/task_queue.hpp" #include #include @@ -40,7 +41,7 @@ class TorchModule : public tactic::BaseModule { struct Config : public tactic::BaseModule::Config { PTR_TYPEDEFS(Config); - std::string model_filepath = ""; + std::string model_filepath = "default"; bool use_gpu = false; bool abs_filepath = true; diff --git a/main/src/vtr_torch/src/modules/torch_module.cpp b/main/src/vtr_torch/src/modules/torch_module.cpp index 484ced9a9..d3e98f1f1 100644 --- a/main/src/vtr_torch/src/modules/torch_module.cpp +++ b/main/src/vtr_torch/src/modules/torch_module.cpp @@ -30,9 +30,17 @@ auto TorchModule::Config::fromROS(const rclcpp::Node::SharedPtr &node, auto config = std::make_shared(); // clang-format off - config->model_filepath = node->declare_parameter(param_prefix + ".filepath", config->model_filepath); config->use_gpu = node->declare_parameter(param_prefix + ".use_gpu", config->use_gpu); config->abs_filepath = node->declare_parameter(param_prefix + ".abs_filepath", config->abs_filepath); + + auto model_dir = node->declare_parameter("model_dir", "defalut2"); + model_dir = common::utils::expand_user(common::utils::expand_env(model_dir)); + + if (config->abs_filepath){ + config->model_filepath = node->declare_parameter(param_prefix + ".filepath", config->model_filepath); + } else { + config->model_filepath = model_dir + "/" + node->declare_parameter(param_prefix + ".filepath", config->model_filepath); + } // clang-format on return config; } From 8a8fdb495471e6d8f8fde0ef90a7d22d6763970d Mon Sep 17 00:00:00 2001 From: Alec Krawciw Date: Wed, 3 May 2023 20:18:23 -0400 Subject: [PATCH 18/77] Change output type of data to tensor. --- .../vtr_torch/include/vtr_torch/modules/torch_module.hpp | 2 +- .../vtr_torch/include/vtr_torch/modules/torch_module.inl | 7 ++++--- main/src/vtr_torch/src/modules/torch_module.cpp | 2 -- 3 files changed, 5 insertions(+), 6 deletions(-) diff --git a/main/src/vtr_torch/include/vtr_torch/modules/torch_module.hpp b/main/src/vtr_torch/include/vtr_torch/modules/torch_module.hpp index 8592d42fc..0f88af978 100644 --- a/main/src/vtr_torch/include/vtr_torch/modules/torch_module.hpp +++ b/main/src/vtr_torch/include/vtr_torch/modules/torch_module.hpp @@ -93,7 +93,7 @@ class TorchModule : public tactic::BaseModule { Module network; template - void evaluateModel(std::vector inputs, const at::IntArrayRef shape); + torch::Tensor evaluateModel(std::vector inputs, const Shape shape); }; diff --git a/main/src/vtr_torch/include/vtr_torch/modules/torch_module.inl b/main/src/vtr_torch/include/vtr_torch/modules/torch_module.inl index 6d5358f1e..224c781ed 100644 --- a/main/src/vtr_torch/include/vtr_torch/modules/torch_module.inl +++ b/main/src/vtr_torch/include/vtr_torch/modules/torch_module.inl @@ -8,7 +8,7 @@ namespace vtr { namespace nn { template - void TorchModule::evaluateModel(std::vector inputs, const Shape shape){ + torch::Tensor TorchModule::evaluateModel(std::vector inputs, const Shape shape){ torch::NoGradGuard no_grad; std::vector jit_inputs; @@ -16,8 +16,9 @@ namespace nn { jit_inputs.push_back(t_input); - auto output = network.forward(jit_inputs); - CLOG(DEBUG, "torch") << output; + auto output = network(jit_inputs); + + return output.toTensor().cpu(); } } // namespace nn diff --git a/main/src/vtr_torch/src/modules/torch_module.cpp b/main/src/vtr_torch/src/modules/torch_module.cpp index d3e98f1f1..0a11bde0e 100644 --- a/main/src/vtr_torch/src/modules/torch_module.cpp +++ b/main/src/vtr_torch/src/modules/torch_module.cpp @@ -47,7 +47,5 @@ auto TorchModule::Config::fromROS(const rclcpp::Node::SharedPtr &node, TorchModule::~TorchModule() {} - - } // namespace nn } // namespace vtr \ No newline at end of file From 646aacec08c172890e8e4675cb8bbd235a538df8 Mon Sep 17 00:00:00 2001 From: Alec Krawciw Date: Wed, 10 May 2023 11:52:43 -0400 Subject: [PATCH 19/77] Cleanups for PR --- launch/online_velodyne_grizzly.launch.yaml | 2 +- main/src/vtr_common/vtr_include.cmake | 2 +- .../vtr_lidar/include/vtr_lidar/pipeline.hpp | 2 + .../vtr_lidar/utils/nanoflann_utils.hpp | 38 ------------------- .../planning/change_detection_module_v3.cpp | 6 +-- .../velodyne_conversion_module.cpp | 3 +- .../velodyne_conversion_module_v2.cpp | 7 ++-- main/src/vtr_lidar/src/pipeline.cpp | 7 +++- 8 files changed, 14 insertions(+), 53 deletions(-) diff --git a/launch/online_velodyne_grizzly.launch.yaml b/launch/online_velodyne_grizzly.launch.yaml index 8488560b4..91cab322e 100644 --- a/launch/online_velodyne_grizzly.launch.yaml +++ b/launch/online_velodyne_grizzly.launch.yaml @@ -37,7 +37,7 @@ windows: - window_name: rviz2 layout: main-horizontal shell_command_before: - - source /opt/ros/humble/setup.bash + - source ${VTRSRC}/main/install/setup.bash panes: - ros2 run rviz2 rviz2 -d ${VTRSRC}/rviz/lidar.rviz # - ros2 run rqt_reconfigure rqt_reconfigure diff --git a/main/src/vtr_common/vtr_include.cmake b/main/src/vtr_common/vtr_include.cmake index 52e367205..5a3155c1e 100644 --- a/main/src/vtr_common/vtr_include.cmake +++ b/main/src/vtr_common/vtr_include.cmake @@ -30,6 +30,6 @@ endif() ## Enable certain pipelines add_definitions(-DVTR_ENABLE_LIDAR) -#add_definitions(-DVTR_ENABLE_RADAR) +add_definitions(-DVTR_ENABLE_RADAR) #add_definitions(-DSAVE_FULL_LIDAR) #add_definitions(-DSAVE_ALL_REPEATS) diff --git a/main/src/vtr_lidar/include/vtr_lidar/pipeline.hpp b/main/src/vtr_lidar/include/vtr_lidar/pipeline.hpp index c0129e6e9..afe47232a 100644 --- a/main/src/vtr_lidar/include/vtr_lidar/pipeline.hpp +++ b/main/src/vtr_lidar/include/vtr_lidar/pipeline.hpp @@ -44,6 +44,8 @@ class LidarPipeline : public tactic::BasePipeline { double submap_translation_threshold = 0.0; // in meters double submap_rotation_threshold = 0.0; // in degrees + bool save_nn_point_cloud = false; + static ConstPtr fromROS(const rclcpp::Node::SharedPtr &node, const std::string ¶m_prefix); }; diff --git a/main/src/vtr_lidar/include/vtr_lidar/utils/nanoflann_utils.hpp b/main/src/vtr_lidar/include/vtr_lidar/utils/nanoflann_utils.hpp index d65dd85f9..6e3f769f9 100644 --- a/main/src/vtr_lidar/include/vtr_lidar/utils/nanoflann_utils.hpp +++ b/main/src/vtr_lidar/include/vtr_lidar/utils/nanoflann_utils.hpp @@ -56,40 +56,6 @@ struct NanoFLANNAdapter { } }; -template -struct NanoFLANNSphericalAdapter { - NanoFLANNSphericalAdapter(const pcl::PointCloud& points, const float angle_weight) : points_(points), angle_weights(angle_weight){ } - - const pcl::PointCloud& points_; - const float angle_weights; - - // Must return the number of data points - inline size_t kdtree_get_point_count() const { return points_.size(); } - - // Returns the dim'th component of the idx'th point in the class: - // Since this is inlined and the "dim" argument is typically an immediate - // value, the "if/else's" are actually solved at compile time. - inline float kdtree_get_pt(const size_t idx, const size_t dim) const { - if (dim == 0) - return points_[idx].rho; - else if (dim == 1) - return points_[idx].phi*angle_weights; - else - return points_[idx].theta*angle_weights; - } - - // Optional bounding-box computation: return false to default to a standard - // bbox computation loop. - // Return true if the BBOX was already computed by the class and returned in - // "bb" so it can be avoided to redo it again. Look at bb.size() to find out - // the expected dimensionality (e.g. 2 or 3 for point clouds) - template - bool kdtree_get_bbox(BBOX& /* bb */) const { - return false; - } -}; - - //Store all neighbours within a given radius template class NanoFLANNRadiusResultSet { @@ -145,10 +111,6 @@ using KDTree = nanoflann::KDTreeSingleIndexAdaptor< nanoflann::L2_Simple_Adaptor>, NanoFLANNAdapter>; template -using SphericalKDTree = nanoflann::KDTreeSingleIndexAdaptor< - nanoflann::L2_Simple_Adaptor>, - NanoFLANNSphericalAdapter>; -template using DynamicKDTree = nanoflann::KDTreeSingleIndexDynamicAdaptor< nanoflann::L2_Simple_Adaptor>, NanoFLANNAdapter>; 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 0424d6fb0..d557ec368 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 @@ -150,7 +150,7 @@ void ChangeDetectionModuleV3::run_(QueryCache &qdata0, OutputCache &output0, const auto &vid_loc = *qdata.vid_loc; const auto &sid_loc = *qdata.sid_loc; const auto &T_r_v_loc = *qdata.T_r_v_loc; - const auto &points = *qdata.raw_point_cloud; + const auto &points = *qdata.undistorted_point_cloud; const auto &submap_loc = *qdata.submap_loc; const auto &map_point_cloud = submap_loc.point_cloud(); const auto &T_v_m_loc = *qdata.T_v_m_loc; @@ -315,10 +315,6 @@ 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) diff --git a/main/src/vtr_lidar/src/modules/preprocessing/conversions/velodyne_conversion_module.cpp b/main/src/vtr_lidar/src/modules/preprocessing/conversions/velodyne_conversion_module.cpp index a0fe55aef..050c1865e 100644 --- a/main/src/vtr_lidar/src/modules/preprocessing/conversions/velodyne_conversion_module.cpp +++ b/main/src/vtr_lidar/src/modules/preprocessing/conversions/velodyne_conversion_module.cpp @@ -87,8 +87,7 @@ void VelodyneConversionModule::run_(QueryCache &qdata0, OutputCache &, point_cloud->at(idx).x = points(idx, 0); point_cloud->at(idx).y = points(idx, 1); point_cloud->at(idx).z = points(idx, 2); - CLOG(DEBUG, "lidar.velodyne_converter") << points(idx, 0); - + // pointwise timestamp point_cloud->at(idx).timestamp = static_cast(points(idx, 5) * 1e9);// + stamp; See comment above where stamp is loaded diff --git a/main/src/vtr_lidar/src/modules/preprocessing/conversions/velodyne_conversion_module_v2.cpp b/main/src/vtr_lidar/src/modules/preprocessing/conversions/velodyne_conversion_module_v2.cpp index 925b8b3c8..1595c47b0 100644 --- a/main/src/vtr_lidar/src/modules/preprocessing/conversions/velodyne_conversion_module_v2.cpp +++ b/main/src/vtr_lidar/src/modules/preprocessing/conversions/velodyne_conversion_module_v2.cpp @@ -140,15 +140,14 @@ void VelodyneConversionModuleV2::run_(QueryCache &qdata0, OutputCache &, + //If a lower horizontal resolution is acceptable, then set the horizontal downsample > 1. + //A value of 2 will leave 1/2 the points, in general 1/n points will be retained. auto filtered_point_cloud = std::make_shared>(*point_cloud); CLOG(DEBUG, "lidar.velodyne_converter_v2") << "Reducing the point cloud density by " << config_->horizontal_downsample << "original size was " << point_cloud->size(); - if (config_->horizontal_downsample > 1) { - - /// Range cropping - + if (config_->horizontal_downsample > 1) { std::vector indices; indices.reserve(filtered_point_cloud->size()); for (size_t i = 0; i < filtered_point_cloud->size(); ++i) { diff --git a/main/src/vtr_lidar/src/pipeline.cpp b/main/src/vtr_lidar/src/pipeline.cpp index 7d735b2e3..b55faf1dc 100644 --- a/main/src/vtr_lidar/src/pipeline.cpp +++ b/main/src/vtr_lidar/src/pipeline.cpp @@ -38,6 +38,8 @@ auto LidarPipeline::Config::fromROS(const rclcpp::Node::SharedPtr &node, // submap creation thresholds config->submap_translation_threshold = node->declare_parameter(param_prefix + ".submap_translation_threshold", config->submap_translation_threshold); config->submap_rotation_threshold = node->declare_parameter(param_prefix + ".submap_rotation_threshold", config->submap_rotation_threshold); + + config->save_nn_point_cloud = node->declare_parameter(param_prefix + ".save_nn_point_cloud", config->save_nn_point_cloud); // clang-format on return config; } @@ -174,7 +176,7 @@ void LidarPipeline::onVertexCreation_(const QueryCache::Ptr &qdata0, #endif // raw point cloud - { + if (config_->save_nn_point_cloud) { auto nn_scan = std::make_shared>(); nn_scan->point_cloud() = *qdata->nn_point_cloud; nn_scan->T_vertex_this() = qdata->T_s_r->inverse(); @@ -185,8 +187,9 @@ void LidarPipeline::onVertexCreation_(const QueryCache::Ptr &qdata0, std::make_shared(nn_scan, *qdata->stamp); vertex->insert>( "nn_point_cloud", "vtr_lidar_msgs/msg/PointScan", nn_scan_odo_msg); + + CLOG(DEBUG, "lidar.pipeline") << "Saved nn pointcloud to vertex" << vertex; } - CLOG(DEBUG, "lidar.pipeline") << "Saved nn pointcloud to vertex" << vertex; /// save the sliding map as vertex submap if we have traveled far enough const bool create_submap = [&] { From 31e48352d6dba32b66d861a0c78707f1a1caab48 Mon Sep 17 00:00:00 2001 From: Alec Krawciw Date: Wed, 10 May 2023 14:51:22 -0400 Subject: [PATCH 20/77] Enable debugging --- main/src/vtr_common/vtr_include.cmake | 3 +++ 1 file changed, 3 insertions(+) diff --git a/main/src/vtr_common/vtr_include.cmake b/main/src/vtr_common/vtr_include.cmake index 5a3155c1e..31bc6f24b 100644 --- a/main/src/vtr_common/vtr_include.cmake +++ b/main/src/vtr_common/vtr_include.cmake @@ -9,6 +9,9 @@ add_compile_options(-march=native -O3 -pthread -Wall -Wextra) # template instantiation help # add_compile_options(-frepo) +#Add debug symbols +add_compile_options(-g -Og) + # built time and memory report # add_compile_options(-ftime-report -fmem-report) From 210be8b8d256140c09e339ab36a3bb701784f039 Mon Sep 17 00:00:00 2001 From: HuguesTHOMAS Date: Wed, 10 May 2023 14:58:07 -0400 Subject: [PATCH 21/77] add myhal floorplan and open at location from config --- main/src/vtr_gui/vtr_gui/socket_client.py | 11 + .../vtr-gui/src/components/graph/GraphMap.js | 41 +- .../vtr_gui/vtr-gui/src/images/myhal-plan.svg | 4614 +++++++++++++++++ main/src/vtr_gui/vtr_gui/web_server.py | 5 + .../vtr_navigation/graph_map_server.hpp | 11 + .../vtr_navigation/src/graph_map_server.cpp | 15 +- .../vtr_navigation/vtr_navigation/vtr_ui.py | 10 + .../vtr_navigation/vtr_ui_builder.py | 5 +- main/src/vtr_pose_graph_msgs/CMakeLists.txt | 7 +- main/src/vtr_pose_graph_msgs/srv/MapInfo.srv | 2 + 10 files changed, 4713 insertions(+), 8 deletions(-) create mode 100644 main/src/vtr_gui/vtr_gui/vtr-gui/src/images/myhal-plan.svg create mode 100644 main/src/vtr_pose_graph_msgs/srv/MapInfo.srv diff --git a/main/src/vtr_gui/vtr_gui/socket_client.py b/main/src/vtr_gui/vtr_gui/socket_client.py index 8ab30acbe..402371bed 100755 --- a/main/src/vtr_gui/vtr_gui/socket_client.py +++ b/main/src/vtr_gui/vtr_gui/socket_client.py @@ -90,6 +90,13 @@ def robot_state_from_ros(ros_robot_state): 'target_localized': ros_robot_state.target_localized, } +def map_info_from_ros(ros_map_info): + return { + 'lat': ros_map_info.lat, + 'lng': ros_map_info.lng, + 'theta': ros_map_info.theta, + 'scale': ros_map_info.scale, + } def following_route_from_ros(ros_following_route): return {'ids': [id for id in ros_following_route.ids]} @@ -195,6 +202,10 @@ def get_graph_state(self): def get_robot_state(self): ros_robot_state = super().get_robot_state() return robot_state_from_ros(ros_robot_state) + + def get_map_info(self): + ros_map_info = super().get_map_info() + return map_info_from_ros(ros_map_info) def get_server_state(self): ros_server_state = super().get_server_state() 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 a9b24ecf8..b7864095d 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 @@ -20,7 +20,7 @@ import React from "react"; import "leaflet/dist/leaflet.css"; import L from "leaflet"; import "leaflet-rotatedmarker"; // enable marker rotation -import { MapContainer, TileLayer, ZoomControl } from "react-leaflet"; +import { MapContainer, TileLayer, ZoomControl, ImageOverlay } from "react-leaflet"; import { kdTree } from "kd-tree-javascript"; import {fetchWithTimeout} from "../../index" @@ -39,6 +39,7 @@ import SelectorStartSVG from "../../images/selector-start.svg"; import MoveGraphTranslationSvg from "../../images/move-graph-translation.svg"; import MoveGraphRotationSvg from "../../images/move-graph-rotation.svg"; import MoveGraphScaleSvg from "../../images/move-graph-scale.svg"; +import MyhalPlan from "../../images/myhal-plan.svg" /// pose graph constants const ROUTE_TYPE_COLOR = ["#f44336", "#ff9800", "#ffeb3b", "#4caf50", "#00bcd4", "#2196f3", "#9c27b0"]; @@ -129,8 +130,10 @@ class GraphMap extends React.Component { move_graph_change: { lng: 0, lat: 0, theta: 0, scale: 1 }, // move robot move_robot_vertex: { lng: 0, lat: 0, id: -1 }, + // map center + map_center: {lat: 43.78220, lng: -79.4661}, }; - + this.fetchMapCenter() /// leaflet map this.map = null; // leaflet map instance @@ -210,14 +213,18 @@ class GraphMap extends React.Component { merge_ids, move_graph_change, move_robot_vertex, + map_center } = this.state; - + const imageBounds = [ + [43.660511, -79.397019], // Bottom-left coordinates of the image + [43.661091, -79.395995], // Top-right coordinates of the image + ]; return ( <> {/* Leaflet map container with initial center set to UTIAS (only for initialization) */} TODO We should make this set dynamically from the yaml config*/ + center={[map_center.lat, map_center.lng]} + // center={[43.6605, -79.3964]} /* Jordy Modification For PETAWAWA center={[45.8983, -77.2829]} => TODO We should make this set dynamically from the yaml config*/ zoom={18} zoomControl={false} whenCreated={this.mapCreatedCallback.bind(this)} @@ -228,6 +235,8 @@ class GraphMap extends React.Component { url="/tile/{s}/{x}/{y}/{z}" // load from backend (potentially cached) maxZoom={24} /> + {/* Add the ImageOverlay component to the map */} + { + if (response.status !== 200) throw new Error("Failed to fetch map info: " + response.status); + response.json().then((data) => { + // console.info("Map center state was: ", this.state.map_center) + console.info("Received the map info: ", data); + const map_center_val = this.state.map_center + if (map_center_val != {lat: data.lat, lng: data.lng}){ + this.setState({map_center: {lat: data.lat, lng: data.lng}}); + if (this.map){ + this.map.setView([this.state.map_center.lat, this.state.map_center.lng]) + } + } + }); + }) + .catch((error) => { + console.error(error); + }); + } + /** @brief Leaflet map creationg callback */ mapCreatedCallback(map) { console.debug("Leaflet map created."); diff --git a/main/src/vtr_gui/vtr_gui/vtr-gui/src/images/myhal-plan.svg b/main/src/vtr_gui/vtr_gui/vtr-gui/src/images/myhal-plan.svg new file mode 100644 index 000000000..3f520a2fd --- /dev/null +++ b/main/src/vtr_gui/vtr_gui/vtr-gui/src/images/myhal-plan.svg @@ -0,0 +1,4614 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + Campus & + Facilities + Planning + University + of Toronto + Revisions + DWG. # + + + + + + + + + + + + + + + 20 + + + + Scale(m) + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + Scale(ft) + + + + + + + + + + + + 2 + 4 + 6 + 10 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 50 + + + + + + + + + + + + + + + 10 + 30 + 40 + + + + + + + 20 + 60 + + + Myhal + Centre for + Engineering + Innovation + and + Entrepreneurship + Bldg #87 + Level 5 + 19/03 + 7 of 11 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 527 + + + 529 + 554 + 556 + 566 + 545 + 535 + 549 + 537 + 533 + 546 + 570 + 580 + 513 + 512 + 523 + 512 + 559 + 506 + 571 + + + F.H.C. + F.H.C. + F.H.C. + + + 525 + 522 + + + + + F.H.C. + + + 540 + 590K + 540K + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 507 + 509 + 508 + 510K + 510S + STAIR D + DN + 510E + 511E + UP + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + DN + UP + 565K + 560S + STAIR E + UP + DN + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 557 + 552 + 547 + 539 + + + 522K + + + 512K + 564 + 562 + 590S + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/main/src/vtr_gui/vtr_gui/web_server.py b/main/src/vtr_gui/vtr_gui/web_server.py index f00162992..af6466866 100755 --- a/main/src/vtr_gui/vtr_gui/web_server.py +++ b/main/src/vtr_gui/vtr_gui/web_server.py @@ -15,6 +15,7 @@ # limitations under the License. import io +import json import os import os.path as osp import logging @@ -135,6 +136,10 @@ def get_task_queue_state(): task_queue_state = build_remote().get_task_queue_state() return flask.jsonify(task_queue_state) +@app.route('/vtr/map_info') +def get_map_info(): + map_info = build_remote().get_map_info() + return flask.jsonify(map_info) def main(): logger.info("Launching the web server.") diff --git a/main/src/vtr_navigation/include/vtr_navigation/graph_map_server.hpp b/main/src/vtr_navigation/include/vtr_navigation/graph_map_server.hpp index 5b1ebe7d0..8da8fb522 100644 --- a/main/src/vtr_navigation/include/vtr_navigation/graph_map_server.hpp +++ b/main/src/vtr_navigation/include/vtr_navigation/graph_map_server.hpp @@ -35,6 +35,8 @@ #include "vtr_navigation_msgs/srv/following_route.hpp" #include "vtr_navigation_msgs/srv/graph_state.hpp" #include "vtr_navigation_msgs/srv/robot_state.hpp" +#include "vtr_pose_graph_msgs/msg/map_info.hpp" +#include "vtr_pose_graph_msgs/srv/map_info.hpp" namespace vtr { namespace navigation { @@ -56,6 +58,9 @@ class GraphMapServer : public tactic::Graph::Callback, using FollowingRoute = vtr_navigation_msgs::msg::GraphRoute; using FollowingRouteSrv = vtr_navigation_msgs::srv::FollowingRoute; + using MapInfo = vtr_pose_graph_msgs::msg::MapInfo; + using MapInfoSrv = vtr_pose_graph_msgs::srv::MapInfo; + using MoveGraphMsg = vtr_navigation_msgs::msg::MoveGraph; using AnnotateRouteMsg = vtr_navigation_msgs::msg::AnnotateRoute; @@ -93,6 +98,9 @@ class GraphMapServer : public tactic::Graph::Callback, void followingRouteSrvCallback( const std::shared_ptr, std::shared_ptr response) const; + void mapInfoSrvCallback( + const std::shared_ptr, + std::shared_ptr response) const; void moveGraphCallback(const MoveGraphMsg::ConstSharedPtr msg); void annotateRouteCallback(const AnnotateRouteMsg::ConstSharedPtr msg); @@ -166,6 +174,9 @@ class GraphMapServer : public tactic::Graph::Callback, rclcpp::Publisher::SharedPtr robot_state_pub_; rclcpp::Service::SharedPtr robot_state_srv_; + /** \brief Service to request the initialized map info */ + rclcpp::Service::SharedPtr map_info_srv_; + /** \brief Publishes current route being followed */ rclcpp::Publisher::SharedPtr following_route_pub_; rclcpp::Service::SharedPtr following_route_srv_; diff --git a/main/src/vtr_navigation/src/graph_map_server.cpp b/main/src/vtr_navigation/src/graph_map_server.cpp index ef851aa7a..532008a6a 100644 --- a/main/src/vtr_navigation/src/graph_map_server.cpp +++ b/main/src/vtr_navigation/src/graph_map_server.cpp @@ -80,7 +80,7 @@ void GraphMapServer::start(const rclcpp::Node::SharedPtr& node, following_route_pub_ = node->create_publisher("following_route", 10); following_route_srv_ = node->create_service("following_route_srv", std::bind(&GraphMapServer::followingRouteSrvCallback, this, std::placeholders::_1, std::placeholders::_2), rmw_qos_profile_services_default, callback_group_); - // graph manipulation + // graph manipulation auto sub_opt = rclcpp::SubscriptionOptions(); sub_opt.callback_group = callback_group_; annotate_route_sub_ = node->create_subscription("annotate_route", rclcpp::QoS(10), std::bind(&GraphMapServer::annotateRouteCallback, this, std::placeholders::_1), sub_opt); @@ -100,6 +100,9 @@ void GraphMapServer::start(const rclcpp::Node::SharedPtr& node, map_info.set = true; graph->setMapInfo(map_info); } + //map info + map_info_srv_ = node->create_service("map_info_srv", std::bind(&GraphMapServer::mapInfoSrvCallback, this, std::placeholders::_1, std::placeholders::_2), rmw_qos_profile_services_default, callback_group_); + if (graph->numberOfVertices() == 0) return; auto graph_lock = graph->guard(); // lock graph then internal lock @@ -127,6 +130,16 @@ void GraphMapServer::robotStateSrvCallback( response->robot_state = robot_state_; } +void GraphMapServer::mapInfoSrvCallback( + const std::shared_ptr, + std::shared_ptr response) const { + CLOG(DEBUG, "navigation.graph_map_server") << "Received map info request"; + SharedLock lock(mutex_); + const auto graph = getGraph(); + auto map_info = graph->getMapInfo(); + response->map_info = map_info; +} + void GraphMapServer::followingRouteSrvCallback( const std::shared_ptr, std::shared_ptr response) const { diff --git a/main/src/vtr_navigation/vtr_navigation/vtr_ui.py b/main/src/vtr_navigation/vtr_navigation/vtr_ui.py index f97d17938..0966a68f9 100755 --- a/main/src/vtr_navigation/vtr_navigation/vtr_ui.py +++ b/main/src/vtr_navigation/vtr_navigation/vtr_ui.py @@ -26,6 +26,7 @@ from vtr_navigation_msgs.msg import MoveGraph, AnnotateRoute from vtr_navigation_msgs.msg import MissionCommand, ServerState from vtr_navigation_msgs.msg import TaskQueueUpdate +from vtr_pose_graph_msgs.srv import MapInfo as MapInfoSrv from vtr_tactic_msgs.msg import EnvInfo from vtr_navigation.ros_manager import ROSManager @@ -89,6 +90,11 @@ def setup_ros(self, *args, **kwargs): # env info self._change_env_info_pub = self.create_publisher(EnvInfo, 'env_info', 1) + # map center + self._map_info_cli = self.create_client(MapInfoSrv, "map_info_srv") + while not self._map_info_cli.wait_for_service(timeout_sec=1.0): + vtr_ui_logger.info("Waiting for map_info_srv service...") + @ROSManager.on_ros def get_graph_state(self): return self._graph_state_cli.call(GraphStateSrv.Request()).graph_state @@ -109,6 +115,10 @@ def get_robot_state(self): def robot_state_callback(self, robot_state): self.notify("robot_state", robot_state=robot_state) + @ROSManager.on_ros + def get_map_info(self): + return self._map_info_cli.call(MapInfoSrv.Request()).map_info + @ROSManager.on_ros def get_following_route(self): return self._following_route_cli.call(FollowingRouteSrv.Request()).following_route diff --git a/main/src/vtr_navigation/vtr_navigation/vtr_ui_builder.py b/main/src/vtr_navigation/vtr_navigation/vtr_ui_builder.py index 61705215a..174781f51 100644 --- a/main/src/vtr_navigation/vtr_navigation/vtr_ui_builder.py +++ b/main/src/vtr_navigation/vtr_navigation/vtr_ui_builder.py @@ -28,7 +28,7 @@ class VTRUIProxy(BaseProxy): _exposed_ = ('get_graph_state', 'get_robot_state', 'get_following_route', 'get_server_state', 'get_task_queue_state', 'set_pause', 'add_goal', 'cancel_goal', 'annotate_route', 'move_graph', 'move_robot', 'merge', - 'confirm_merge', 'continue_teach', 'change_env_info') + 'confirm_merge', 'continue_teach', 'change_env_info', 'get_map_info') def get_graph_state(self): return self._callmethod('get_graph_state') @@ -74,6 +74,9 @@ def move_graph(self, move_graph_change): def change_env_info(self, env_info): return self._callmethod('change_env_info', args=(env_info,)) + + def get_map_info(self): + return self._callmethod('get_map_info') def build_master(vtr_ui_class=VTRUI): diff --git a/main/src/vtr_pose_graph_msgs/CMakeLists.txt b/main/src/vtr_pose_graph_msgs/CMakeLists.txt index 2fd09097a..d7397b9c5 100644 --- a/main/src/vtr_pose_graph_msgs/CMakeLists.txt +++ b/main/src/vtr_pose_graph_msgs/CMakeLists.txt @@ -20,8 +20,13 @@ file( RELATIVE ${CMAKE_CURRENT_SOURCE_DIR} msg/*.msg ) +file( + GLOB_RECURSE SRV_SRC + RELATIVE ${CMAKE_CURRENT_SOURCE_DIR} + srv/*.srv +) rosidl_generate_interfaces(${PROJECT_NAME} - ${MSG_SRC} + ${MSG_SRC} ${SRV_SRC} DEPENDENCIES builtin_interfaces std_msgs geometry_msgs vtr_common_msgs ) diff --git a/main/src/vtr_pose_graph_msgs/srv/MapInfo.srv b/main/src/vtr_pose_graph_msgs/srv/MapInfo.srv new file mode 100644 index 000000000..dbc477eea --- /dev/null +++ b/main/src/vtr_pose_graph_msgs/srv/MapInfo.srv @@ -0,0 +1,2 @@ +--- +MapInfo map_info \ No newline at end of file From f5a4183100d4ece045fb4d3dfc68ebac7152e4ca Mon Sep 17 00:00:00 2001 From: marijapili Date: Wed, 10 May 2023 15:22:35 -0400 Subject: [PATCH 22/77] add Jackal config and launch files --- config/velodyne_jackal_default.yaml | 424 +++++++++++++++++++++ launch/offline_velodyne_jackal.launch.yaml | 43 +++ launch/online_velodyne_jackal.launch.yaml | 45 +++ 3 files changed, 512 insertions(+) create mode 100644 config/velodyne_jackal_default.yaml create mode 100644 launch/offline_velodyne_jackal.launch.yaml create mode 100644 launch/online_velodyne_jackal.launch.yaml diff --git a/config/velodyne_jackal_default.yaml b/config/velodyne_jackal_default.yaml new file mode 100644 index 000000000..5c8683ba8 --- /dev/null +++ b/config/velodyne_jackal_default.yaml @@ -0,0 +1,424 @@ +/**: + ros__parameters: + log_to_file: true + log_debug: true + log_enabled: + - lidar.terrain_assessment + - navigation + - navigation.graph_map_server + - navigation.command + # tactic + - tactic + - tactic.pipeline + - tactic.module + - tactic.module.live_mem_manager + - tactic.module.graph_mem_manager + # path planner + #"path_planning", + #"path_planning.cbit", + #"path_planning.cbit_planner", + #"mpc.cbit", + #"mpc_debug.cbit", + #"obstacle_detection.cbit", + # mission planner + - mission.server + - mission.state_machine + # pose graph + - pose_graph + # pipeline specific + # "lidar.pipeline", + # "lidar.honeycomb_converter", + # "lidar.preprocessing", + # "lidar.odometry_icp", + # "lidar.odometry_map_maintenance", + # "lidar.vertex_test", + # "lidar.localization_map_recall", + # "lidar.localization_icp", + # "lidar.intra_exp_merging", + # "lidar.dynamic_detection", + # "lidar.inter_exp_merging", + # "lidar.change_detection", + # "lidar.ground_extraction", + # "lidar.obstacle_detection", + # "lidar.terrain_assessment", + #"grizzly_controller_tests.cbit" + + + # control frame from the urdf + robot_frame: base_link + env_info_topic: env_info + lidar_frame: velodyne + lidar_topic: /velodyne_points + graph_projection: + origin_lat: 43.6605 #43.7822 + origin_lng: -79.3964 #-79.4661 + origin_theta: 1.3 + scale: 1.0 + tactic: + enable_parallelization: true + preprocessing_skippable: false + odometry_mapping_skippable: false + localization_skippable: false + task_queue_num_threads: 1 + task_queue_size: -1 + + route_completion_translation_threshold: 0.5 + + chain: + min_cusp_distance: 1.5 + angle_weight: 7.0 + search_depth: 5 + search_back_depth: 10 + distance_warning: 5.0 + save_odometry_result: true + save_localization_result: true + visualize: true + rviz_loc_path_offset: + - 0.0 + - 0.0 + - 0.0 + pipeline: + type: lidar + preprocessing: + - conversion + - filtering + odometry: + - icp + - mapping + - vertex_test + - intra_exp_merging + - dynamic_detection + - inter_exp_merging + - memory + localization: + - recall + - icp + - safe_corridor + - change_detection_sync + - memory + submap_translation_threshold: 1.5 + submap_rotation_threshold: 30.0 + preprocessing: + conversion: + type: lidar.velodyne_converter_v2 + visualize: true + filtering: + type: lidar.preprocessing_v2 + num_threads: 8 + crop_range: 30.0 + + frame_voxel_size: 0.1 # grid subsampling voxel size + + vertical_angle_res: 0.002181662 # 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: 0.16 # 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.85 # 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.3 # 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 + traj_lock_prev_vel: false + traj_qc_diag: + - 1.0 + - 0.1 + - 0.1 + - 0.1 + - 0.1 + - 1.0 + num_threads: 8 + first_num_steps: 2 + initial_max_iter: 4 + initial_max_pairing_dist: 1.5 + initial_max_planar_dist: 1.0 + refined_max_iter: 50 + refined_max_pairing_dist: 1.0 + refined_max_planar_dist: 0.3 + averaging_num_steps: 2 + verbose: false + max_iterations: 1 + min_matched_ratio: 0.5 + visualize: true + mapping: + type: lidar.odometry_map_maintenance_v2 + + map_voxel_size: 0.1 + + crop_range_front: 40.0 + back_over_front_ratio: 0.5 + point_life_time: 20.0 + visualize: true + vertex_test: + type: lidar.vertex_test + + max_translation: 0.2 # was 0.3 + max_rotation: 10.0 + intra_exp_merging: + type: lidar.intra_exp_merging_v2 + depth: 6.0 + + 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 # 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.1 + max_num_experiences: 128 + distance_threshold: 0.6 + planar_threshold: 0.2 + normal_threshold: 0.8 + dynamic_obs_threshold: 1 + visualize: true + memory: + type: live_mem_manager + window_size: 5 + localization: + recall: + type: lidar.localization_map_recall + map_version: pointmap + visualize: true + icp: + type: lidar.localization_icp + use_pose_prior: true + num_threads: 8 + first_num_steps: 2 + initial_max_iter: 4 + initial_max_pairing_dist: 1.5 + initial_max_planar_dist: 1.0 + refined_max_iter: 50 + refined_max_pairing_dist: 1.0 + refined_max_planar_dist: 0.3 + averaging_num_steps: 2 + verbose: false + max_iterations: 1 + min_matched_ratio: 0.3 + safe_corridor: + type: lidar.safe_corridor + lookahead_distance: 2.5 + corridor_width: 1.5 + influence_distance: 1.0 + resolution: 0.25 + size_x: 16.0 + size_y: 8.0 + visualize: true + change_detection_sync: + type: lidar.change_detection_v3 + detection_range: 8.0 + search_radius: 0.25 + + negprob_threshold: 0.25 # was 0.015 # -1.86 without prior, 0.015 with prior # Jordy: I found I needed to bump this up abit (approx 0.075+) to reduce false positives + use_prior: true + alpha0: 3.0 + beta0: 0.03 + use_support_filtering: true + support_radius: 0.25 + support_variance: 0.05 + support_threshold: 2.5 + + influence_distance: 0.5 # was 0.5 Jordy # Note that the total distance where grid cells have values > 0 is min dist + influence dist, not influence dist! + minimum_distance: 0.8 # 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 + visualize: true + change_detection: + type: lidar.change_detection_v2 + detection_range: 8 + search_radius: 0.25 + negprob_threshold: 0.1 + use_prior: true + alpha0: 3.0 + beta0: 0.03 + use_support_filtering: true + support_radius: 0.25 + support_variance: 0.05 + support_threshold: 2.5 + resolution: 0.5 + size_x: 16.0 + size_y: 8.0 + run_online: false + run_async: true + visualize: false + save_module_result: false + memory: + type: graph_mem_manager + vertex_life_span: 5 + window_size: 3 + obstacle_detection: + type: lidar.obstacle_detection + z_min: 0.5 + z_max: 2.0 + resolution: 0.6 + size_x: 40.0 + size_y: 20.0 + run_async: true + visualize: true + ground_extraction: + type: lidar.ground_extraction + z_offset: 0.2 + alpha: 0.035 + tolerance: 0.25 + Tm: 0.3 + Tm_small: 0.1 + Tb: 0.5 + Trmse: 0.1 + Tdprev: 1.0 + rmin: 2.0 + num_bins_small: 30.0 + bin_size_small: 0.5 + num_bins_large: 10.0 + bin_size_large: 1.0 + resolution: 0.6 + size_x: 40.0 + size_y: 20.0 + run_async: true + visualize: true + terrain_assessment: + type: lidar.terrain_assessment + lookahead_distance: 15.0 + corridor_width: 1.0 + search_radius: 1.0 + resolution: 0.5 + size_x: 40.0 + size_y: 20.0 + run_online: false + run_async: true + visualize: true + path_planning: + type: cbit # cbit for obstacle free, cbit.lidar for obstacle avoidance version + control_period: 100 # ms + teb: + visualize: true + extrapolate: false + extrapolation_timeout: 2.0 + lookahead_distance: 8.5 + robot_model: circular + robot_radius: 0.5 + map_frame: planning frame + enable_homotopy_class_planning: true + free_goal_vel: true + max_vel_x: 0.6 + max_vel_y: 0.0 + max_vel_theta: 0.3 + min_turning_radius: 3 + weight_viapoint: 0.5 + weight_costmap: 100.0 + cbit: + obs_padding: 0.0 + curv_to_euclid_discretization: 10 + sliding_window_width: 12.0 + sliding_window_freespace_padding: 0.5 + corridor_resolution: 0.2 + state_update_freq: 1.0 + update_state: true + rand_seed: 1 + + # Planner Tuning Params + initial_samples: 300 + batch_samples: 100 + pre_seed_resolution: 0.5 + alpha: 0.5 + q_max: 2.5 + frame_interval: 50 + iter_max: 10000000 + 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: 30 # Note I dont think im actually using this one anymore, am using the one in the change detection v3 now + + mpc: + # Controller Params + horizon_steps: 20 + horizon_step_size: 0.1 #was 0.3 + forward_vel: 1.0 + max_lin_vel: 1.25 + max_ang_vel: 1.5 + vehicle_model: "unicycle" + + # Cost Function Covariance Matrices + #pose_error_cov: [0.75, 0.75, 0.75, 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: [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] + #lat_error_cov: [100.0] + + # Cost Function Weights + #pose_error_weight: 1.0 + #acc_error_weight: 1.0 + #kin_error_weight: 1.0 + #lat_error_weight: 0.01 + + + # Cost Function Covariance Matrices + pose_error_cov: [30.0, 30.0, 300.0, 300.0, 300.0, 6.50] + vel_error_cov: [10.0, 100.0] # was [0.1 2.0] # No longer using this cost term + acc_error_cov: [10.0, 10.0] + kin_error_cov: [0.004, 0.004, 0.004, 0.004, 0.004, 0.004] + lat_error_cov: [20.0] + + # Cost Function Weights + pose_error_weight: 5.0 + vel_error_weight: 5.0 + acc_error_weight: 5.0 + kin_error_weight: 1000.0 + lat_error_weight: 0.01 + + + # Cost Function Covariance Matrices + #pose_error_cov: [2.0, 2.0, 2.0, 2.0, 2.0, 2.0] + #vel_error_cov: [10.0, 10.0] # was [0.1 2.0] # No longer using this cost term + #acc_error_cov: [2.0, 2.0] + #kin_error_cov: [0.05, 0.05, 0.05, 0.05, 0.05, 0.05] + #lat_error_cov: [10.0] + + # Cost Function Weights + #pose_error_weight: 5.0 + #vel_error_weight: 2.0 + #acc_error_weight: 1.0 + #kin_error_weight: 200.0 + #lat_error_weight: 0.5 + + #backup for tracking mpc + #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/launch/offline_velodyne_jackal.launch.yaml b/launch/offline_velodyne_jackal.launch.yaml new file mode 100644 index 000000000..9e53fa771 --- /dev/null +++ b/launch/offline_velodyne_jackal.launch.yaml @@ -0,0 +1,43 @@ +## Offline Lidar VTR3 (using datasets) +session_name: vtr_offline_velodyne_jackal + +environment: + CYCLONEDDS_URI: ${VTRSRC}/config/cyclonedds.default.xml # custom dds configs + # ROS_DOMAIN_ID: "1" # set this to a unique number when multiple ROS2 dependent system running on the same network + +start_directory: ${VTRTEMP} + +suppress_history: false + +windows: + - window_name: vtr_navigation_system + layout: main-vertical + shell_command_before: + - source ${VTRSRC}/main/install/setup.bash + panes: + - > + ros2 launch vtr_navigation vtr.launch.py + base_params:=velodyne_jackal_default.yaml + data_dir:=${VTRTEMP}/lidar + start_new_graph:=false + use_sim_time:=true + +# - htop # monitor hardware usage + + - window_name: vtr_gui + layout: main-horizontal + shell_command_before: + - source ${VTRSRC}/main/install/setup.bash + panes: + - ros2 run vtr_gui socket_client --ros-args -r __ns:=/vtr + - ros2 run vtr_gui socket_server --ros-args -r __ns:=/vtr + - ros2 run vtr_gui web_server --ros-args -r __ns:=/vtr + # - firefox --new-window "localhost:5200" # the webpage has to wait for everything above + + # - window_name: rviz2 + # layout: main-horizontal + # shell_command_before: + # - source /opt/ros/humble/setup.bash + # panes: + # - ros2 run rviz2 rviz2 -d ${VTRSRC}/rviz/lidar.rviz + # - ros2 run rqt_reconfigure rqt_reconfigure diff --git a/launch/online_velodyne_jackal.launch.yaml b/launch/online_velodyne_jackal.launch.yaml new file mode 100644 index 000000000..76e491a95 --- /dev/null +++ b/launch/online_velodyne_jackal.launch.yaml @@ -0,0 +1,45 @@ +## Offline Lidar VTR3 (using datasets) +session_name: vtr_online_velodyne_jackal + +environment: + CYCLONEDDS_URI: ${VTRSRC}/config/cyclonedds.default.xml # custom dds configs + # ROS_DOMAIN_ID: "1" # set this to a unique number when multiple ROS2 dependent system running on the same network + +start_directory: ${VTRTEMP} + +suppress_history: false + +windows: + - window_name: vtr_navigation_system + layout: main-vertical + shell_command_before: + - source /opt/ros/humble/setup.bash + && source ${VTRSRC}/main/install/setup.bash + panes: + - > + ros2 launch vtr_navigation vtr.launch.py + base_params:=velodyne_jackal_default.yaml + data_dir:=${VTRTEMP}/lidar + start_new_graph:=false + use_sim_time:=false + +# - htop # monitor hardware usage + + - window_name: vtr_gui + layout: main-horizontal + shell_command_before: + - source /opt/ros/humble/setup.bash + && source ${VTRSRC}/main/install/setup.bash + panes: + - ros2 run vtr_gui socket_client --ros-args -r __ns:=/vtr + - ros2 run vtr_gui socket_server --ros-args -r __ns:=/vtr + - ros2 run vtr_gui web_server --ros-args -r __ns:=/vtr -p config:=config_myhal.json + # - firefox --new-window "localhost:5200" # the webpage has to wait for everything above + + # - window_name: rviz2 + # layout: main-horizontal + # shell_command_before: + # - source /opt/ros/humble/setup.bash + # panes: + # - ros2 run rviz2 rviz2 -d ${VTRSRC}/rviz/lidar.rviz + # # - ros2 run rqt_reconfigure rqt_reconfigure From 077f49728e8f51203f1e27eb3f2cdcc61c6e63c5 Mon Sep 17 00:00:00 2001 From: Jordy Sehn Date: Thu, 11 May 2023 08:50:22 -0400 Subject: [PATCH 23/77] Update to Path Direction Checking Code To Handle Direction Transition Cases --- config/honeycomb_grizzly_default.yaml | 2 +- .../include/vtr_path_planning/cbit/cbit.hpp | 2 +- .../vtr_path_planning/cbit/cbit_config.hpp | 2 +- .../src/cbit/cbit_path_planner.cpp | 41 +++++++++++++++++-- 4 files changed, 41 insertions(+), 6 deletions(-) diff --git a/config/honeycomb_grizzly_default.yaml b/config/honeycomb_grizzly_default.yaml index 14be8fced..1e67ae1eb 100644 --- a/config/honeycomb_grizzly_default.yaml +++ b/config/honeycomb_grizzly_default.yaml @@ -318,7 +318,7 @@ sliding_window_width: 12.0 sliding_window_freespace_padding: 0.5 corridor_resolution: 0.2 - state_update_freq: 1.0 + state_update_freq: 2.0 update_state: true rand_seed: 1 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 197cfb528..fce82412d 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 @@ -75,7 +75,7 @@ class CBIT : public BasePathPlanner { int rand_seed = 1; // ROC - double roc_lookahead = 5.0; + double roc_lookahead = 1.0; int roc_discretization = 40; double roc_q_tolerance = 0.001; diff --git a/main/src/vtr_path_planning/include/vtr_path_planning/cbit/cbit_config.hpp b/main/src/vtr_path_planning/include/vtr_path_planning/cbit/cbit_config.hpp index 3538fc63e..438da4f0b 100644 --- a/main/src/vtr_path_planning/include/vtr_path_planning/cbit/cbit_config.hpp +++ b/main/src/vtr_path_planning/include/vtr_path_planning/cbit/cbit_config.hpp @@ -36,7 +36,7 @@ class CBITConfig { int rand_seed = 1; // ROC - double roc_lookahead = 5.0; + double roc_lookahead = 1.0; int roc_discretization = 40; double roc_q_tolerance = 0.001; 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 a5b70d2a6..1ab47e0d0 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 @@ -1270,7 +1270,7 @@ void CBITPlanner::Prune(double c_best, double c_best_weighted) std::shared_ptr CBITPlanner::UpdateState(PathDirection path_direction) { //std::cout << "The new state is x: " << new_state->x << " y: " << new_state->y << std::endl; - + //CLOG(ERROR, "path_planning.cbit_planner") << "Robot Pose: x: " << new_state->x << "Robot Pose: y: " << new_state->y <<"Robot Pose: yaw: " << new_state->yaw; //First calc the qmin distance to the euclidean path (borrow code from ROC generation) //To prevent us from having to search the entire euclidean path (and avoid crosses/loops) in the path, use the previous pose as a reference (will always be p=0) //and define a subset of points with a lookahead distance @@ -1306,6 +1306,40 @@ std::shared_ptr CBITPlanner::UpdateState(PathDirection path_direction) } } + // Experimental code to dynamically predict path direction + //CLOG(ERROR, "path_planning.cbit_planner") << "Closest Euclid Pt is - x: " << closest_pt.p << " y: " << closest_pt.q; + //CLOG(ERROR, "path_planning.cbit_planner") << "The Next Euclid Pt is - x: " << euclid_subset[closest_pt_ind+1].p << " y: " << euclid_subset[closest_pt_ind+1].q; + double test_dx = euclid_subset[closest_pt_ind+1].p - closest_pt.p; + double test_dy = euclid_subset[closest_pt_ind+1].q - closest_pt.q; + double test_yaw = atan2(test_dy,test_dx); + //CLOG(ERROR, "path_planning.cbit_planner") << "Predicted Yaw is: " << test_yaw; + // Ensure that yaw1 and yaw2 are in the range -pi to pi + double yaw1 = fmod(test_yaw + 2*M_PI, 2*M_PI); + if (yaw1 < 0) { + yaw1 += 2*M_PI; + } + double yaw2 = fmod(new_state->yaw + 2*M_PI, 2*M_PI); + if (yaw2 < 0) { + yaw2 += 2*M_PI; + } + + // Calculate the angle between the yaws and ensure it is in the range 0 to pi + double angle = fmod(fabs(yaw1 - yaw2), 2*M_PI); + if (angle > M_PI) { + angle = 2*M_PI - angle; + } + + // Check if the angle is greater than 90 degrees + double path_direction2; + if (angle > M_PI/2) { + CLOG(ERROR, "path_planning.cbit_planner") << "Path direction is -1.0 (Reverse)"; + path_direction2 = -1.0; + } else { + CLOG(ERROR, "path_planning.cbit_planner") << "Path direction is +1.0 (Forward)"; + path_direction2 = 1.0; + } + + // Once we have the closest point, we need to try to find the sign (above or below the path) // This is a little tricky, but I think what is reasonable is to create a plane with the closest point and its neighbours // Then depending on which side the new state point is of the plane, is the sign we use for q_min: @@ -1339,9 +1373,10 @@ std::shared_ptr CBITPlanner::UpdateState(PathDirection path_direction) //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 - q_min = q_min * q_sign; + q_min = q_min * q_sign * path_direction2; //std::cout << "q_min is: " << q_min << std::endl; // debug; + /* switch (path_direction) { case PATH_DIRECTION_REVERSE: q_min = -q_min; @@ -1353,7 +1388,7 @@ std::shared_ptr CBITPlanner::UpdateState(PathDirection path_direction) // Handle error case where path_direction is not a valid value break; } - + */ // 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 From 16d38b1347306e5a18c7c92040d52a74aaa4cc1a Mon Sep 17 00:00:00 2001 From: Jordy Sehn Date: Thu, 11 May 2023 12:46:47 -0400 Subject: [PATCH 24/77] Reference Pose Selection Bug Fix --- .../vtr_lidar/src/path_planning/mpc_path_planner2.cpp | 10 ++++++---- .../vtr_path_planning/src/cbit/cbit_path_planner.cpp | 4 ++-- .../vtr_path_planning/src/mpc/mpc_path_planner2.cpp | 6 +++--- 3 files changed, 11 insertions(+), 9 deletions(-) diff --git a/main/src/vtr_lidar/src/path_planning/mpc_path_planner2.cpp b/main/src/vtr_lidar/src/path_planning/mpc_path_planner2.cpp index d2abae328..e15bf2d3e 100644 --- a/main/src/vtr_lidar/src/path_planning/mpc_path_planner2.cpp +++ b/main/src/vtr_lidar/src/path_planning/mpc_path_planner2.cpp @@ -224,7 +224,8 @@ struct mpc_result SolveMPC2(Eigen::Matrix previous_vel, lgmath::se 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, accelLossFunc); opt_problem.addCostTerm(accel_cost_term); - } + } + } @@ -277,6 +278,7 @@ struct mpc_result SolveMPC2(Eigen::Matrix previous_vel, lgmath::se opt_problem.addCostTerm(lat_cost_term_right); const auto lat_cost_term_left = steam::WeightedLeastSqCostTerm<1>::MakeShared(lat_barrier_left, sharedLatNoiseModel, latLossFunc); opt_problem.addCostTerm(lat_cost_term_left); + } } @@ -288,11 +290,11 @@ struct mpc_result SolveMPC2(Eigen::Matrix previous_vel, lgmath::se // Initialize solver parameters SolverType::Params params; params.verbose = true; // Makes the output display for debug when true - params.relative_cost_change_threshold = 1e-6; + params.relative_cost_change_threshold = 1e-4; params.max_iterations = 100; - params.absolute_cost_change_threshold = 1e-6; + params.absolute_cost_change_threshold = 1e-4; params.backtrack_multiplier = 0.5; // Line Search Specific Params, will fail to build if using GaussNewtonSolver - params.max_backtrack_steps = 200; // Line Search Specific Params, will fail to build if using GaussNewtonSolver + params.max_backtrack_steps = 400; // Line Search Specific Params, will fail to build if using GaussNewtonSolver //params.ratio_threshold_shrink = 0.1; // Dogleg Specific Params, will fail to build if using GaussNewtonSolver /// Grow trust region if ratio of actual to predicted cost reduction above /// this (range: 0.0-1.0) 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 1ab47e0d0..f3177e44b 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 @@ -1332,10 +1332,10 @@ std::shared_ptr CBITPlanner::UpdateState(PathDirection path_direction) // Check if the angle is greater than 90 degrees double path_direction2; if (angle > M_PI/2) { - CLOG(ERROR, "path_planning.cbit_planner") << "Path direction is -1.0 (Reverse)"; + //CLOG(ERROR, "path_planning.cbit_planner") << "Path direction is -1.0 (Reverse)"; path_direction2 = -1.0; } else { - CLOG(ERROR, "path_planning.cbit_planner") << "Path direction is +1.0 (Forward)"; + //CLOG(ERROR, "path_planning.cbit_planner") << "Path direction is +1.0 (Forward)"; path_direction2 = 1.0; } diff --git a/main/src/vtr_path_planning/src/mpc/mpc_path_planner2.cpp b/main/src/vtr_path_planning/src/mpc/mpc_path_planner2.cpp index 60e0de308..5bc27a6af 100644 --- a/main/src/vtr_path_planning/src/mpc/mpc_path_planner2.cpp +++ b/main/src/vtr_path_planning/src/mpc/mpc_path_planner2.cpp @@ -275,10 +275,10 @@ struct mpc_result SolveMPC2(Eigen::Matrix previous_vel, lgmath::se // Generate least square cost terms and add them to the optimization problem const auto lat_cost_term_right = steam::WeightedLeastSqCostTerm<1>::MakeShared(lat_barrier_right, sharedLatNoiseModel, latLossFunc); - opt_problem.addCostTerm(lat_cost_term_right); + //opt_problem.addCostTerm(lat_cost_term_right); const auto lat_cost_term_left = steam::WeightedLeastSqCostTerm<1>::MakeShared(lat_barrier_left, sharedLatNoiseModel, latLossFunc); - opt_problem.addCostTerm(lat_cost_term_left); - + //opt_problem.addCostTerm(lat_cost_term_left); + CLOG(ERROR, "mpc_debug.cbit") << "WE ARE IN THE RIGHT THING"; } } From beec890be5d0e47edb115814aa2443e0c4dba0f0 Mon Sep 17 00:00:00 2001 From: Jordy Sehn Date: Thu, 11 May 2023 12:53:24 -0400 Subject: [PATCH 25/77] param update --- config/honeycomb_grizzly_default.yaml | 19 ++++++++++--------- 1 file changed, 10 insertions(+), 9 deletions(-) diff --git a/config/honeycomb_grizzly_default.yaml b/config/honeycomb_grizzly_default.yaml index 1e67ae1eb..26790b3a0 100644 --- a/config/honeycomb_grizzly_default.yaml +++ b/config/honeycomb_grizzly_default.yaml @@ -353,19 +353,20 @@ vehicle_model: "unicycle" # Cost Function Covariance Matrices - pose_error_cov: [30.0, 30.0, 300.0, 300.0, 300.0, 6.50] - vel_error_cov: [10.0, 100.0] # was [0.1 2.0] # No longer using this cost term - acc_error_cov: [10.0, 10.0] + # Cost Function Covariance Matrices + pose_error_cov: [30.0, 30.0, 30.0, 1000.0, 1000.0, 1000.0] + vel_error_cov: [100.0, 100.0] # was [0.1 2.0] # No longer using this cost term + acc_error_cov: [30.0, 30.0] kin_error_cov: [0.004, 0.004, 0.004, 0.004, 0.004, 0.004] - lat_error_cov: [20.0] + lat_error_cov: [10.0] # Cost Function Weights - pose_error_weight: 5.0 - vel_error_weight: 5.0 - acc_error_weight: 5.0 - kin_error_weight: 1000.0 + pose_error_weight: 1.0 + vel_error_weight: 1.0 + acc_error_weight: 1.0 + kin_error_weight: 1.0 lat_error_weight: 0.01 - + #backup for tracking mpc #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] From 6ad864c87db498ef8b76ab6491e23bf03cbfbdb4 Mon Sep 17 00:00:00 2001 From: Alec Krawciw Date: Fri, 12 May 2023 10:39:54 -0400 Subject: [PATCH 26/77] Add gdb to Dockerfile. Fix seg fault in hange detection v3 --- Dockerfile | 1 + .../modules/planning/change_detection_module_v3.cpp | 13 ++++++++----- main/src/vtr_navigation/launch/vtr.launch.py | 2 +- 3 files changed, 10 insertions(+), 6 deletions(-) diff --git a/Dockerfile b/Dockerfile index 0db47c108..c257969c4 100644 --- a/Dockerfile +++ b/Dockerfile @@ -108,6 +108,7 @@ RUN apt install ros-humble-velodyne -q -y # Install vim RUN apt update && apt install -q -y vim +RUN apt install -q -y libc6-dbg gdb valgrind ## Switch to specified user USER ${USERID}:${GROUPID} 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 d557ec368..15fb7b0f0 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 @@ -497,11 +497,14 @@ void ChangeDetectionModuleV3::run_(QueryCache &qdata0, OutputCache &output0, //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); + if (config_->visualize) { + // 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); diff --git a/main/src/vtr_navigation/launch/vtr.launch.py b/main/src/vtr_navigation/launch/vtr.launch.py index 459b43bdf..ccb901111 100644 --- a/main/src/vtr_navigation/launch/vtr.launch.py +++ b/main/src/vtr_navigation/launch/vtr.launch.py @@ -22,7 +22,7 @@ def generate_launch_description(): namespace='vtr', executable='vtr_navigation', output='screen', - # prefix=['xterm -e gdb --args'], + #prefix=['xterm -e gdb -ex run --args'], parameters=[ { "data_dir": LaunchConfiguration("data_dir"), From 2f0376952e75dddbe2fa94891314d4c731e2587e Mon Sep 17 00:00:00 2001 From: Alec Krawciw Date: Fri, 12 May 2023 15:21:44 -0400 Subject: [PATCH 27/77] Updated parameters to newest mpc --- config/hdl64_grizzly_default.yaml | 56 +++++++++---------- config/honeycomb_grizzly_default.yaml | 30 +++------- .../planning/change_detection_module_v3.cpp | 2 +- 3 files changed, 33 insertions(+), 55 deletions(-) diff --git a/config/hdl64_grizzly_default.yaml b/config/hdl64_grizzly_default.yaml index 48c119b13..1a465aa40 100644 --- a/config/hdl64_grizzly_default.yaml +++ b/config/hdl64_grizzly_default.yaml @@ -316,36 +316,20 @@ visualize: true path_planning: 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 - extrapolation_timeout: 2.0 - lookahead_distance: 8.5 - robot_model: circular - robot_radius: 0.5 - map_frame: planning frame - enable_homotopy_class_planning: true - free_goal_vel: true - max_vel_x: 0.6 - max_vel_y: 0.0 - max_vel_theta: 0.3 - min_turning_radius: 3 - weight_viapoint: 0.5 - weight_costmap: 100.0 + control_period: 100 # ms cbit: obs_padding: 0.0 - curv_to_euclid_discretization: 20 + curv_to_euclid_discretization: 10 sliding_window_width: 12.0 sliding_window_freespace_padding: 0.5 - corridor_resolution: 0.05 - state_update_freq: 1.0 + corridor_resolution: 0.2 + state_update_freq: 2.0 update_state: true rand_seed: 1 # Planner Tuning Params initial_samples: 300 - batch_samples: 150 + batch_samples: 100 pre_seed_resolution: 0.5 alpha: 0.5 q_max: 2.5 @@ -360,28 +344,38 @@ costmap: costmap_filter_value: 0.01 costmap_history: 20 # Note I dont think im actually using this one anymore, am using the one in the change detection v3 now - mpc: # Controller Params horizon_steps: 20 horizon_step_size: 0.3 - forward_vel: 0.75 - max_lin_vel: 1.25 - max_ang_vel: 0.75 + forward_vel: 1.1 + max_lin_vel: 1.5 + max_ang_vel: 1.5 + robot_linear_velocity_scale: 1.1 # Used to scale the output twist linear velocity messages by some constant factor to compensate internal low level control miscalibration + robot_angular_velocity_scale: 1.0 # Used to scale the output twist angular velocity messages by some constant factor to compensate internal low level control miscalibration + vehicle_model: "unicycle" + # Cost Function Covariance Matrices + # Cost Function Covariance Matrices + pose_error_cov: [30.0, 30.0, 30.0, 1000.0, 1000.0, 1000.0] + vel_error_cov: [100.0, 100.0] # was [0.1 2.0] # No longer using this cost term + acc_error_cov: [30.0, 30.0] + kin_error_cov: [0.004, 0.004, 0.004, 0.004, 0.004, 0.004] + lat_error_cov: [10.0] + # 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] + pose_error_weight: 1.0 + vel_error_weight: 1.0 + acc_error_weight: 1.0 + kin_error_weight: 1.0 + lat_error_weight: 0.01 - #backup + #backup for tracking mpc #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/config/honeycomb_grizzly_default.yaml b/config/honeycomb_grizzly_default.yaml index d79ad869b..4987f3be3 100644 --- a/config/honeycomb_grizzly_default.yaml +++ b/config/honeycomb_grizzly_default.yaml @@ -58,7 +58,7 @@ localization_skippable: false task_queue_num_threads: 1 task_queue_size: -1 - + route_completion_translation_threshold: 0.5 chain: @@ -241,7 +241,7 @@ support_threshold: 2.5 influence_distance: 0.5 # was 0.5 Jordy # Note that the total distance where grid cells have values > 0 is min dist + influence dist, not influence dist! - minimum_distance: 0.8 # was 0.3 Jordy + minimum_distance: 0.8 # was 0.3 Jordy # cost map costmap_history_size: 15 # Keep between 3 and 20, used for temporal filtering @@ -296,22 +296,6 @@ path_planning: type: cbit # cbit for obstacle free path tracker, cbit.lidar for obstacle avoidance version control_period: 100 # ms - teb: - visualize: true - extrapolate: false - extrapolation_timeout: 2.0 - lookahead_distance: 8.5 - robot_model: circular - robot_radius: 0.5 - map_frame: planning frame - enable_homotopy_class_planning: true - free_goal_vel: true - max_vel_x: 0.6 - max_vel_y: 0.0 - max_vel_theta: 0.3 - min_turning_radius: 3 - weight_viapoint: 0.5 - weight_costmap: 100.0 cbit: obs_padding: 0.0 curv_to_euclid_discretization: 10 @@ -321,7 +305,7 @@ state_update_freq: 2.0 update_state: true rand_seed: 1 - + # Planner Tuning Params initial_samples: 300 batch_samples: 100 @@ -339,7 +323,7 @@ costmap: costmap_filter_value: 0.01 costmap_history: 30 # Note I dont think im actually using this one anymore, am using the one in the change detection v3 now - + mpc: # Controller Params horizon_steps: 20 @@ -359,19 +343,19 @@ acc_error_cov: [30.0, 30.0] kin_error_cov: [0.004, 0.004, 0.004, 0.004, 0.004, 0.004] lat_error_cov: [10.0] - + # Cost Function Weights pose_error_weight: 1.0 vel_error_weight: 1.0 acc_error_weight: 1.0 kin_error_weight: 1.0 lat_error_weight: 0.01 - + #backup for tracking mpc #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_lidar/src/modules/planning/change_detection_module_v3.cpp b/main/src/vtr_lidar/src/modules/planning/change_detection_module_v3.cpp index 15fb7b0f0..1f1c528e6 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 @@ -478,7 +478,7 @@ void ChangeDetectionModuleV3::run_(QueryCache &qdata0, OutputCache &output0, } // 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 + //const auto dense_costmap = std::make_shared(config_->resolution, config_->size_x, config_->size_y); // need to declare this earlier dense_costmap->update(filtered_loc_map); From 37932a0413a8a91c2f2a65748da2d028e29423b2 Mon Sep 17 00:00:00 2001 From: Alec Krawciw Date: Sat, 13 May 2023 13:12:06 -0400 Subject: [PATCH 28/77] Print tensor values. --- config/honeycomb_grizzly_learned.yaml | 5 +++-- main/src/vtr_lidar/src/modules/odometry/sample_module.cpp | 3 ++- 2 files changed, 5 insertions(+), 3 deletions(-) diff --git a/config/honeycomb_grizzly_learned.yaml b/config/honeycomb_grizzly_learned.yaml index ba5bc1ccd..b42c39fbd 100644 --- a/config/honeycomb_grizzly_learned.yaml +++ b/config/honeycomb_grizzly_learned.yaml @@ -13,6 +13,7 @@ - tactic.module.live_mem_manager - tactic.module.graph_mem_manager - torch + - torch.sample # path planner # "path_planning", # "path_planning.teb", @@ -117,9 +118,9 @@ odometry: network: type: torch.sample - use_gpu: true + use_gpu: false abs_filepath: false - filepath: sample_model.pt + filepath: sample_model_script.pt icp: type: lidar.odometry_icp use_trajectory_estimation: true diff --git a/main/src/vtr_lidar/src/modules/odometry/sample_module.cpp b/main/src/vtr_lidar/src/modules/odometry/sample_module.cpp index e9d4cc866..2c282bdc9 100644 --- a/main/src/vtr_lidar/src/modules/odometry/sample_module.cpp +++ b/main/src/vtr_lidar/src/modules/odometry/sample_module.cpp @@ -41,7 +41,8 @@ void TestNNModule::run_(QueryCache &qdata0, OutputCache &, auto &qdata = dynamic_cast(qdata0); std::vector inputs {2, 5}; - evaluateModel(inputs, {1, 2}); + auto tensor = evaluateModel(inputs, {1, 2}); + CLOG(INFO, "torch.sample") << tensor; } From 8ab45861a15864b044e43834aa5ccc569f218170 Mon Sep 17 00:00:00 2001 From: Alec Krawciw Date: Mon, 15 May 2023 17:20:50 -0400 Subject: [PATCH 29/77] Surround optimization with try/catch --- .../src/modules/localization/localization_icp_module.cpp | 7 ++++++- .../src/modules/odometry/odometry_icp_module.cpp | 8 +++++++- 2 files changed, 13 insertions(+), 2 deletions(-) diff --git a/main/src/vtr_lidar/src/modules/localization/localization_icp_module.cpp b/main/src/vtr_lidar/src/modules/localization/localization_icp_module.cpp index 16ac00f66..c0520d272 100644 --- a/main/src/vtr_lidar/src/modules/localization/localization_icp_module.cpp +++ b/main/src/vtr_lidar/src/modules/localization/localization_icp_module.cpp @@ -236,7 +236,12 @@ void LocalizationICPModule::run_(QueryCache &qdata0, OutputCache &, params.verbose = config_->verbose; params.max_iterations = (unsigned int)config_->max_iterations; GaussNewtonSolver solver(problem, params); - solver.optimize(); + try{ + solver.optimize(); + } catch (std::runtime_error& e) { + CLOG(WARNING, "lidar.localization_icp") << "Steam failed.\n e.what(): " << e.what(); + break; + } Covariance covariance(solver); timer[3]->stop(); 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 e54512f0a..b64835044 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 @@ -351,7 +351,13 @@ void OdometryICPModule::run_(QueryCache &qdata0, OutputCache &, params.verbose = config_->verbose; params.max_iterations = (unsigned int)config_->max_iterations; GaussNewtonSolver solver(problem, params); - solver.optimize(); + + try{ + solver.optimize(); + } catch (std::runtime_error& e) { + CLOG(WARNING, "lidar.odometry_icp") << "Steam failed.\n e.what(): " << e.what(); + break; + } Covariance covariance(solver); timer[3]->stop(); From f1babf074ba94c9af905d5984766bf52f17c569c Mon Sep 17 00:00:00 2001 From: Alec Krawciw Date: Tue, 16 May 2023 09:56:02 -0400 Subject: [PATCH 30/77] Minor changes to velodyne pipeline. --- config/hdl64_grizzly_default.yaml | 27 ++++++------ launch/offline_velodyne_grizzly.launch.yaml | 41 +++++++++++++++++++ .../include/vtr_lidar/data_types/point.hpp | 1 + .../velodyne_conversion_module_v2.cpp | 8 ++-- 4 files changed, 60 insertions(+), 17 deletions(-) create mode 100644 launch/offline_velodyne_grizzly.launch.yaml diff --git a/config/hdl64_grizzly_default.yaml b/config/hdl64_grizzly_default.yaml index 1a465aa40..a69828093 100644 --- a/config/hdl64_grizzly_default.yaml +++ b/config/hdl64_grizzly_default.yaml @@ -28,11 +28,11 @@ - pose_graph # pipeline specific #- lidar.pipeline - #- lidar.velodyne_converter_v2 + - lidar.velodyne_converter_v2 - lidar.preprocessing - # "lidar.odometry_icp", + - lidar.odometry_icp # "lidar.odometry_map_maintenance", - # "lidar.vertex_test", + - lidar.vertex_test # "lidar.localization_map_recall", # "lidar.localization_icp", # "lidar.intra_exp_merging", @@ -90,8 +90,8 @@ - mapping - vertex_test - intra_exp_merging - #- dynamic_detection - #- inter_exp_merging + - dynamic_detection + - inter_exp_merging - memory localization: - recall @@ -112,7 +112,7 @@ type: lidar.preprocessing num_threads: 8 - crop_range: 20.0 + crop_range: 40.0 frame_voxel_size: 0.2 # grid subsampling voxel size @@ -150,7 +150,7 @@ - 1.0 num_threads: 8 first_num_steps: 2 - initial_max_iter: 10 + initial_max_iter: 8 initial_max_pairing_dist: 1.5 initial_max_planar_dist: 1.0 refined_max_iter: 50 @@ -159,7 +159,7 @@ averaging_num_steps: 2 verbose: false max_iterations: 1 - min_matched_ratio: 0.5 + min_matched_ratio: 0.85 visualize: true mapping: type: lidar.odometry_map_maintenance_v2 @@ -171,7 +171,7 @@ vertex_test: type: lidar.vertex_test max_translation: 0.5 - max_rotation: 10.0 + max_rotation: 5.0 intra_exp_merging: type: lidar.intra_exp_merging_v2 depth: 6.0 @@ -182,8 +182,8 @@ dynamic_detection: type: lidar.dynamic_detection depth: 12.0 - horizontal_resolution: 0.041 # 0.02042 - vertical_resolution: 0.026 # 0.01326 + horizontal_resolution: 0.1728 # 0.02042 + vertical_resolution: 0.00872664626 # 0.01326 max_num_observations: 10000 min_num_observations: 4 dynamic_threshold: 0.3 @@ -315,7 +315,7 @@ run_async: true visualize: true path_planning: - type: cbit.lidar # teb.lidar for old teb version, cbit for obstacle free path tracker, cbit.lidar for obstacle avoidance version + type: cbit # cbit for obstacle free path tracker, cbit.lidar for obstacle avoidance version control_period: 100 # ms cbit: obs_padding: 0.0 @@ -343,7 +343,8 @@ plotting: true costmap: costmap_filter_value: 0.01 - costmap_history: 20 # Note I dont think im actually using this one anymore, am using the one in the change detection v3 now + costmap_history: 30 # Note I dont think im actually using this one anymore, am using the one in the change detection v3 now + mpc: # Controller Params horizon_steps: 20 diff --git a/launch/offline_velodyne_grizzly.launch.yaml b/launch/offline_velodyne_grizzly.launch.yaml new file mode 100644 index 000000000..485c0d2ca --- /dev/null +++ b/launch/offline_velodyne_grizzly.launch.yaml @@ -0,0 +1,41 @@ +## Offline Lidar VTR3 +session_name: vtr_offline_velodyne_grizzly + +#environment: + #CYCLONEDDS_URI: ${VTRSRC}/config/cyclonedds.default.xml # custom dds configs + # ROS_DOMAIN_ID: "1" # set this to a unique number when multiple ROS2 dependent system running on the same network + +start_directory: ${VTRTEMP} + +suppress_history: false + +windows: + - window_name: vtr_navigation_system + layout: main-horizontal + shell_command_before: + - source ${VTRSRC}/main/install/setup.bash + panes: + - > + ros2 launch vtr_navigation vtr.launch.py + base_params:=hdl64_grizzly_default.yaml + data_dir:=${VTRTEMP}/lidar/ + start_new_graph:=true + use_sim_time:=true + + - window_name: vtr_gui + layout: main-horizontal + shell_command_before: + - source ${VTRSRC}/main/install/setup.bash + panes: + - ros2 run vtr_gui socket_client --ros-args -r __ns:=/vtr + - ros2 run vtr_gui socket_server --ros-args -r __ns:=/vtr + - ros2 run vtr_gui web_server --ros-args -r __ns:=/vtr + # - firefox --new-window "localhost:5200" # the webpage has to wait for everything above + + - window_name: rviz2 + layout: main-horizontal + shell_command_before: + - source ${VTRSRC}/main/install/setup.bash + panes: + - ros2 run rviz2 rviz2 -d ${VTRSRC}/rviz/lidar.rviz + # - ros2 run rqt_reconfigure rqt_reconfigure diff --git a/main/src/vtr_lidar/include/vtr_lidar/data_types/point.hpp b/main/src/vtr_lidar/include/vtr_lidar/data_types/point.hpp index 593a6a8b9..1437e0a71 100644 --- a/main/src/vtr_lidar/include/vtr_lidar/data_types/point.hpp +++ b/main/src/vtr_lidar/include/vtr_lidar/data_types/point.hpp @@ -42,6 +42,7 @@ namespace lidar { float rho; \ float theta; \ float phi; \ + float intensity; \ }; \ struct { \ float dynamic_obs; \ diff --git a/main/src/vtr_lidar/src/modules/preprocessing/conversions/velodyne_conversion_module_v2.cpp b/main/src/vtr_lidar/src/modules/preprocessing/conversions/velodyne_conversion_module_v2.cpp index 1595c47b0..65cf65a3a 100644 --- a/main/src/vtr_lidar/src/modules/preprocessing/conversions/velodyne_conversion_module_v2.cpp +++ b/main/src/vtr_lidar/src/modules/preprocessing/conversions/velodyne_conversion_module_v2.cpp @@ -35,7 +35,7 @@ void velodyneCart2Pol(pcl::PointCloud &point_cloud) { p.rho = sqrt(p.x * p.x + p.y * p.y + p.z * p.z); p.theta = atan2(sqrt(p.x * p.x + p.y * p.y), p.z); - p.phi = atan2(p.y, p.x) + M_PI / 2; + p.phi = atan2(p.y, p.x); // + M_PI / 2; if (i > 0 && (p.phi - pm1.phi) > 1.5 * M_PI) p.phi -= 2 * M_PI; @@ -48,7 +48,7 @@ void estimateTime(pcl::PointCloud &point_cloud, int64_t time_offs for (size_t i = 0; i < point_cloud.size(); i++) { auto &p = point_cloud[i]; - p.timestamp = time_offset + static_cast(p.phi / angular_vel * 1e9); + p.timestamp = time_offset + static_cast((p.phi) / angular_vel * 1e9); } } @@ -97,7 +97,7 @@ void VelodyneConversionModuleV2::run_(QueryCache &qdata0, OutputCache &, point_cloud->at(idx).x = *iter_x; point_cloud->at(idx).y = *iter_y; point_cloud->at(idx).z = *iter_z; - point_cloud->at(idx).flex14 = *iter_intensity; + point_cloud->at(idx).intensity = *iter_intensity; } @@ -151,7 +151,7 @@ void VelodyneConversionModuleV2::run_(QueryCache &qdata0, OutputCache &, std::vector indices; indices.reserve(filtered_point_cloud->size()); for (size_t i = 0; i < filtered_point_cloud->size(); ++i) { - if (i % config_->horizontal_downsample == 0) + if (i % config_->horizontal_downsample == 0 )//&& (*point_cloud)[i].phi < -M_PI) indices.emplace_back(i); } *filtered_point_cloud = From 239cdb30dae04131d78c5f03a84fb448676c7ed5 Mon Sep 17 00:00:00 2001 From: Alec Krawciw Date: Tue, 23 May 2023 16:57:50 -0400 Subject: [PATCH 31/77] Fix bug with empty topic names --- main/src/vtr_pose_graph/src/serializable/rc_graph.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/main/src/vtr_pose_graph/src/serializable/rc_graph.cpp b/main/src/vtr_pose_graph/src/serializable/rc_graph.cpp index cc13f0320..de2c37fee 100644 --- a/main/src/vtr_pose_graph/src/serializable/rc_graph.cpp +++ b/main/src/vtr_pose_graph/src/serializable/rc_graph.cpp @@ -60,7 +60,7 @@ auto RCGraph::addVertex(const Timestamp& time) -> VertexPtr { } void RCGraph::loadGraphIndex() { - GraphMsgAccessor accessor{fs::path{file_path_} / "index"}; + GraphMsgAccessor accessor{fs::path{file_path_}, "index", "vtr_pose_graph_msgs/msg/Graph"}; msg_ = accessor.readAtIndex(1); if (!msg_) { std::string err{"Graph index message does not exist."}; @@ -83,7 +83,7 @@ void RCGraph::loadGraphIndex() { void RCGraph::loadVertices() { CLOG(DEBUG, "pose_graph") << "Loading vertices from disk"; - VertexMsgAccessor accessor{fs::path{file_path_} / "vertices"}; + VertexMsgAccessor accessor{fs::path{file_path_}, "vertices", "vtr_pose_graph_msgs/msg/Vertex"}; for (int index = 1;; index++) { const auto msg = accessor.readAtIndex(index); if (!msg) break; @@ -98,7 +98,7 @@ void RCGraph::loadVertices() { void RCGraph::loadEdges() { CLOG(DEBUG, "pose_graph") << "Loading edges from disk"; - EdgeMsgAccessor accessor{fs::path{file_path_} / "edges"}; + EdgeMsgAccessor accessor{fs::path{file_path_}, "edges", "vtr_pose_graph_msgs/msg/Edge"}; for (int index = 1;; index++) { const auto msg = accessor.readAtIndex(index); if (!msg) break; From 7652e8016df852522b86617f8839e794a00e553a Mon Sep 17 00:00:00 2001 From: Alec Krawciw Date: Tue, 6 Jun 2023 17:20:27 -0400 Subject: [PATCH 32/77] Minor changes, may revert. --- config/hdl64_grizzly_default.yaml | 62 ++++++++----------- config/honeycomb_grizzly_default.yaml | 2 +- .../planning/change_detection_module_v3.cpp | 4 ++ main/src/vtr_navigation/launch/vtr.launch.py | 2 +- 4 files changed, 31 insertions(+), 39 deletions(-) diff --git a/config/hdl64_grizzly_default.yaml b/config/hdl64_grizzly_default.yaml index a69828093..91c1c0f22 100644 --- a/config/hdl64_grizzly_default.yaml +++ b/config/hdl64_grizzly_default.yaml @@ -109,7 +109,7 @@ downsample_ratio: 2 filtering: - type: lidar.preprocessing + type: lidar.preprocessing_v2 num_threads: 8 crop_range: 40.0 @@ -150,7 +150,7 @@ - 1.0 num_threads: 8 first_num_steps: 2 - initial_max_iter: 8 + initial_max_iter: 10 initial_max_pairing_dist: 1.5 initial_max_planar_dist: 1.0 refined_max_iter: 50 @@ -171,7 +171,7 @@ vertex_test: type: lidar.vertex_test max_translation: 0.5 - max_rotation: 5.0 + max_rotation: 10.0 intra_exp_merging: type: lidar.intra_exp_merging_v2 depth: 6.0 @@ -182,6 +182,7 @@ dynamic_detection: type: lidar.dynamic_detection depth: 12.0 + horizontal_resolution: 0.1728 # 0.02042 vertical_resolution: 0.00872664626 # 0.01326 max_num_observations: 10000 @@ -283,26 +284,6 @@ size_y: 20.0 run_async: true visualize: true - ground_extraction: - type: lidar.ground_extraction - z_offset: 0.2 - alpha: 0.035 - tolerance: 0.25 - Tm: 0.3 - Tm_small: 0.1 - Tb: 0.5 - Trmse: 0.1 - Tdprev: 1.0 - rmin: 2.0 - num_bins_small: 30.0 - bin_size_small: 0.5 - num_bins_large: 10.0 - bin_size_large: 1.0 - resolution: 0.6 - size_x: 40.0 - size_y: 20.0 - run_async: true - visualize: true terrain_assessment: type: lidar.terrain_assessment lookahead_distance: 15.0 @@ -315,21 +296,22 @@ run_async: true visualize: true path_planning: - type: cbit # cbit for obstacle free path tracker, cbit.lidar for obstacle avoidance version + type: cbit.lidar # teb.lidar for old teb version, cbit for obstacle free path tracker, cbit.lidar for obstacle avoidance version control_period: 100 # ms + cbit: obs_padding: 0.0 curv_to_euclid_discretization: 10 sliding_window_width: 12.0 sliding_window_freespace_padding: 0.5 - corridor_resolution: 0.2 - state_update_freq: 2.0 + corridor_resolution: 0.1 + state_update_freq: 1.0 update_state: true rand_seed: 1 # Planner Tuning Params - initial_samples: 300 - batch_samples: 100 + initial_samples: 400 + batch_samples: 200 pre_seed_resolution: 0.5 alpha: 0.5 q_max: 2.5 @@ -349,21 +331,27 @@ # Controller Params horizon_steps: 20 horizon_step_size: 0.3 - forward_vel: 1.1 - max_lin_vel: 1.5 - max_ang_vel: 1.5 + forward_vel: 0.75 + max_lin_vel: 1.25 + max_ang_vel: 0.75 robot_linear_velocity_scale: 1.1 # Used to scale the output twist linear velocity messages by some constant factor to compensate internal low level control miscalibration robot_angular_velocity_scale: 1.0 # Used to scale the output twist angular velocity messages by some constant factor to compensate internal low level control miscalibration vehicle_model: "unicycle" # Cost Function Covariance Matrices - # Cost Function Covariance Matrices - pose_error_cov: [30.0, 30.0, 30.0, 1000.0, 1000.0, 1000.0] - vel_error_cov: [100.0, 100.0] # was [0.1 2.0] # No longer using this cost term - acc_error_cov: [30.0, 30.0] - kin_error_cov: [0.004, 0.004, 0.004, 0.004, 0.004, 0.004] - lat_error_cov: [10.0] + pose_error_cov: [100.0, 100.0, 1000.0, 1000.0, 1000.0, 200.0] + vel_error_cov: [300.0, 300.0] # was [0.1 2.0] # No longer using this cost term + acc_error_cov: [100.0, 100.0] + kin_error_cov: [0.001, 0.001, 0.001, 0.001, 0.001, 0.001] + lat_error_cov: [20.0] + + # Cost Function Covariance Matrices (Tracking Backup) + #pose_error_cov: [100.0, 100.0, 1000.0, 1000.0, 1000.0, 200.0] + #vel_error_cov: [30.0, 30.0] # was [0.1 2.0] # No longer using this cost term + #acc_error_cov: [10.0, 10.0] + #kin_error_cov: [0.001, 0.001, 0.001, 0.001, 0.001, 0.001] + #lat_error_cov: [10.0] # Cost Function Weights pose_error_weight: 1.0 diff --git a/config/honeycomb_grizzly_default.yaml b/config/honeycomb_grizzly_default.yaml index 4987f3be3..50c13e1a4 100644 --- a/config/honeycomb_grizzly_default.yaml +++ b/config/honeycomb_grizzly_default.yaml @@ -4,7 +4,7 @@ log_debug: true log_enabled: #- lidar.terrain_assessment - #- navigation + - navigation #- navigation.graph_map_server #- navigation.command # tactic 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 1f1c528e6..151a9b9ec 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 @@ -167,6 +167,10 @@ void ChangeDetectionModuleV3::run_(QueryCache &qdata0, OutputCache &output0, } pcl::PointCloud query_points(points, query_indices); + if (query_points.size() == 0){ + CLOG(WARNING, "lidar.change_detection") << "No points were valid to detect changes"; + return; + } voxelDownsample(query_points, 0.2); // Eigen matrix of original data (only shallow copy of ref clouds) diff --git a/main/src/vtr_navigation/launch/vtr.launch.py b/main/src/vtr_navigation/launch/vtr.launch.py index ccb901111..d2d911404 100644 --- a/main/src/vtr_navigation/launch/vtr.launch.py +++ b/main/src/vtr_navigation/launch/vtr.launch.py @@ -22,7 +22,7 @@ def generate_launch_description(): namespace='vtr', executable='vtr_navigation', output='screen', - #prefix=['xterm -e gdb -ex run --args'], + prefix=['xterm -e gdb -ex run --args'], parameters=[ { "data_dir": LaunchConfiguration("data_dir"), From 81fceb66103b25e675651ddaebb6cafe04271f7b Mon Sep 17 00:00:00 2001 From: Alec Krawciw Date: Fri, 9 Jun 2023 15:37:27 -0400 Subject: [PATCH 33/77] Add PyTorch --- Dockerfile | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/Dockerfile b/Dockerfile index add482e74..a443c721c 100644 --- a/Dockerfile +++ b/Dockerfile @@ -3,7 +3,7 @@ FROM nvidia/cuda:11.7.1-cudnn8-devel-ubuntu22.04 CMD ["/bin/bash"] # Args for setting up non-root users, example command to use your own user: -# docker build -t \ +# docker build -t vtr3torch \ # --build-arg USERID=$(id -u) \ # --build-arg GROUPID=$(id -g) \ # --build-arg USERNAME=$(whoami) \ @@ -108,6 +108,10 @@ ENV TORCH_LIB=/opt/torch/libtorch ENV LD_LIBRARY_PATH=$TORCH_LIB/lib:${LD_LIBRARY_PATH:+:${LD_LIBRARY_PATH}} ENV CMAKE_PREFIX_PATH=$TORCH_LIB:$CMAKE_PREFIX_PATH +#Install PyTorch +RUN pip3 install torch torchvision +RUN pip3 install tqdm + ENV NVIDIA_DRIVER_CAPABILITIES compute,utility,graphics ## Switch to specified user From 81bd52ddd776c078b315ac52e56c9e7c7d2254f1 Mon Sep 17 00:00:00 2001 From: Alec Krawciw Date: Wed, 14 Jun 2023 09:05:33 -0400 Subject: [PATCH 34/77] Create range image conversion. Run 2D unet in C++ --- config/honeycomb_grizzly_learned.yaml | 97 ++++++++++--------- main/src/deps/steam | 2 +- main/src/vtr_common/vtr_include.cmake | 4 +- main/src/vtr_lidar/CMakeLists.txt | 3 +- .../include/vtr_lidar/filters/range_image.hpp | 56 +++++++++++ .../include/vtr_lidar/modules/modules.hpp | 1 + .../rangenet_change_detection_module.hpp | 70 +++++++++++++ .../rangenet_change_detection_module.cpp | 88 +++++++++++++++++ .../vtr_torch/modules/torch_module.hpp | 2 + .../vtr_torch/modules/torch_module.inl | 2 + .../vtr_torch/src/modules/torch_module.cpp | 13 +++ 11 files changed, 291 insertions(+), 47 deletions(-) create mode 100644 main/src/vtr_lidar/include/vtr_lidar/filters/range_image.hpp create mode 100644 main/src/vtr_lidar/include/vtr_lidar/modules/planning/rangenet_change_detection_module.hpp create mode 100644 main/src/vtr_lidar/src/modules/planning/rangenet_change_detection_module.cpp diff --git a/config/honeycomb_grizzly_learned.yaml b/config/honeycomb_grizzly_learned.yaml index b42c39fbd..e83c7c2af 100644 --- a/config/honeycomb_grizzly_learned.yaml +++ b/config/honeycomb_grizzly_learned.yaml @@ -27,7 +27,7 @@ # pose graph - pose_graph # pipeline specific - # "lidar.pipeline", + - lidar.range_change # "lidar.honeycomb_converter", # "lidar.preprocessing", # "lidar.odometry_icp", @@ -85,12 +85,11 @@ - dynamic_detection - inter_exp_merging - memory - - network localization: - recall - icp - safe_corridor - - change_detection_sync + - range_change - memory submap_translation_threshold: 1.5 submap_rotation_threshold: 30.0 @@ -116,11 +115,6 @@ cluster_num_sample: 10000 visualize: true odometry: - network: - type: torch.sample - use_gpu: false - abs_filepath: false - filepath: sample_model_script.pt icp: type: lidar.odometry_icp use_trajectory_estimation: true @@ -215,6 +209,12 @@ size_x: 16.0 size_y: 8.0 visualize: true + + range_change: + type: torch.range_change + use_gpu: false + abs_filepath: false + filepath: exported_range_model.pt change_detection_sync: type: lidar.change_detection_v3 detection_range: 8.0 @@ -297,8 +297,8 @@ run_async: true visualize: true path_planning: - type: cbit - control_period: 33 + type: cbit # teb.lidar for old teb version, cbit for obstacle free path tracker, cbit.lidar for obstacle avoidance version + control_period: 100 # ms teb: visualize: true extrapolate: false @@ -317,54 +317,63 @@ weight_costmap: 100.0 cbit: obs_padding: 0.0 - curv_to_euclid_discretization: 20 - sliding_window_width: 10.0 + curv_to_euclid_discretization: 10 + sliding_window_width: 12.0 sliding_window_freespace_padding: 0.5 - corridor_resolution: 0.05 - state_update_freq: 0.5 + corridor_resolution: 0.2 + state_update_freq: 1.0 update_state: true rand_seed: 1 - initial_samples: 250 + + # Planner Tuning Params + initial_samples: 300 batch_samples: 100 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: 30 # 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 - 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 + # Controller Params + horizon_steps: 20 + horizon_step_size: 0.3 + forward_vel: 1.1 + max_lin_vel: 1.5 + max_ang_vel: 1.5 + robot_linear_velocity_scale: 1.1 # Used to scale the output twist linear velocity messages by some constant factor to compensate internal low level control miscalibration + robot_angular_velocity_scale: 1.0 # Used to scale the output twist angular velocity messages by some constant factor to compensate internal low level control miscalibration + + vehicle_model: "unicycle" + + # Cost Function Covariance Matrices + pose_error_cov: [30.0, 30.0, 300.0, 300.0, 300.0, 6.50] + vel_error_cov: [10.0, 100.0] # was [0.1 2.0] # No longer using this cost term + acc_error_cov: [10.0, 10.0] + kin_error_cov: [0.004, 0.004, 0.004, 0.004, 0.004, 0.004] + lat_error_cov: [20.0] + + # Cost Function Weights + pose_error_weight: 5.0 + vel_error_weight: 5.0 + acc_error_weight: 5.0 + kin_error_weight: 1000.0 + lat_error_weight: 0.01 + + #backup for tracking mpc + #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/deps/steam b/main/src/deps/steam index 62a2188bc..632729057 160000 --- a/main/src/deps/steam +++ b/main/src/deps/steam @@ -1 +1 @@ -Subproject commit 62a2188bceb81e056e5156c1b48bd6f20b109f3e +Subproject commit 632729057b1c80e4f727b9123b5b347bcd114d58 diff --git a/main/src/vtr_common/vtr_include.cmake b/main/src/vtr_common/vtr_include.cmake index 31bc6f24b..3fc0940e2 100644 --- a/main/src/vtr_common/vtr_include.cmake +++ b/main/src/vtr_common/vtr_include.cmake @@ -10,7 +10,9 @@ add_compile_options(-march=native -O3 -pthread -Wall -Wextra) # add_compile_options(-frepo) #Add debug symbols -add_compile_options(-g -Og) +add_compile_options(-g) +#Prevent all optimization (useful for detailed debugging but really slow!) +#add_compile_options(-O0) # built time and memory report # add_compile_options(-ftime-report -fmem-report) diff --git a/main/src/vtr_lidar/CMakeLists.txt b/main/src/vtr_lidar/CMakeLists.txt index 08c295ec9..014bd4efa 100644 --- a/main/src/vtr_lidar/CMakeLists.txt +++ b/main/src/vtr_lidar/CMakeLists.txt @@ -18,6 +18,7 @@ find_package(pcl_ros REQUIRED) find_package(nav_msgs REQUIRED) find_package(visualization_msgs REQUIRED) +find_package(sensor_msgs REQUIRED) find_package(lgmath REQUIRED) find_package(steam REQUIRED) @@ -56,7 +57,7 @@ add_library(${PROJECT_NAME}_pipeline ${PIPELINE_SRC}) target_link_libraries(${PROJECT_NAME}_pipeline ${PROJECT_NAME}_components) ament_target_dependencies(${PROJECT_NAME}_pipeline Eigen3 pcl_conversions pcl_ros - nav_msgs visualization_msgs + nav_msgs visualization_msgs sensor_msgs lgmath steam vtr_torch vtr_logging vtr_tactic vtr_lidar_msgs ) diff --git a/main/src/vtr_lidar/include/vtr_lidar/filters/range_image.hpp b/main/src/vtr_lidar/include/vtr_lidar/filters/range_image.hpp new file mode 100644 index 000000000..9d9a6b4e8 --- /dev/null +++ b/main/src/vtr_lidar/include/vtr_lidar/filters/range_image.hpp @@ -0,0 +1,56 @@ +// Copyright 2023, 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 range_image.hpp + * \author Alec Krawciw, Autonomous Space Robotics Lab (ASRL) + */ +#pragma once + +#include "pcl/point_cloud.h" +#include "vtr_lidar/data_types/point.hpp" + +namespace vtr { +namespace lidar{ + +struct RangeImageParams { + double fov_up; //In radians + double fov_down; //In radians + + double getFOV() { return abs(fov_down) + abs(fov_up);} +}; + +/// \todo Parallelize +void generate_range_image(const pcl::PointCloud& point_cloud, Eigen::MatrixXf& range_image, RangeImageParams params) { + + for (size_t i = 0; i < point_cloud.size(); ++i) { + auto& point = point_cloud[i]; + double proj_x = 0.5 * (point.theta / M_PI + 1.0); + double proj_y = 1.0 - (point.phi + abs(params.fov_down)) / params.getFOV(); + + proj_x = (proj_x < 0.0) ? 0.0 : proj_x; + proj_x = (proj_x > 1.0) ? 1.0 : proj_x; + + proj_y = (proj_y < 0.0) ? 0.0 : proj_y; + proj_y = (proj_y > 1.0) ? 1.0 : proj_y; + + proj_x *= range_image.cols() - 1; + proj_y *= range_image.rows() - 1; + + range_image((int)proj_y, (int)proj_x) = point.rho; + } +} + +} // namespace lidar +} // vtr diff --git a/main/src/vtr_lidar/include/vtr_lidar/modules/modules.hpp b/main/src/vtr_lidar/include/vtr_lidar/modules/modules.hpp index 3a7d772d6..88a4ee98b 100644 --- a/main/src/vtr_lidar/include/vtr_lidar/modules/modules.hpp +++ b/main/src/vtr_lidar/include/vtr_lidar/modules/modules.hpp @@ -48,4 +48,5 @@ #include "vtr_lidar/modules/planning/safe_corridor_module.hpp" #include "vtr_lidar/modules/planning/terrain_assessment_module.hpp" #include "vtr_lidar/modules/planning/diff_generator.hpp" +#include "vtr_lidar/modules/planning/rangenet_change_detection_module.hpp" diff --git a/main/src/vtr_lidar/include/vtr_lidar/modules/planning/rangenet_change_detection_module.hpp b/main/src/vtr_lidar/include/vtr_lidar/modules/planning/rangenet_change_detection_module.hpp new file mode 100644 index 000000000..a411d443a --- /dev/null +++ b/main/src/vtr_lidar/include/vtr_lidar/modules/planning/rangenet_change_detection_module.hpp @@ -0,0 +1,70 @@ +// Copyright 2023, 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 sample_module.hpp + * \author Alec Krawciw, Autonomous Space Robotics Lab (ASRL) + */ +#pragma once + +#include "vtr_tactic/modules/base_module.hpp" +#include "vtr_tactic/task_queue.hpp" +#include +#include "vtr_lidar/cache.hpp" +#include + +#include "sensor_msgs/msg/image.hpp" + +namespace vtr { +namespace lidar { + +using Image = sensor_msgs::msg::Image; + +/** \brief Load and store Torch Models */ +class RangeChangeNetModule : public nn::TorchModule { + public: + PTR_TYPEDEFS(RangeChangeNetModule); + /** \brief Static module identifier. */ + static constexpr auto static_name = "torch.range_change"; + + /** \brief Config parameters. */ + struct Config : public nn::TorchModule::Config { + PTR_TYPEDEFS(Config); + + static ConstPtr fromROS(const rclcpp::Node::SharedPtr &node, + const std::string ¶m_prefix); + }; + + RangeChangeNetModule( + const Config::ConstPtr &config, + const std::shared_ptr &module_factory = nullptr, + const std::string &name = static_name) + : nn::TorchModule{config, module_factory, name}, config_(config) {} + + private: + void run_(tactic::QueryCache &qdata, tactic::OutputCache &output, + const tactic::Graph::Ptr &graph, + const tactic::TaskExecutor::Ptr &executor) override; + + Config::ConstPtr config_; + + rclcpp::Publisher::SharedPtr mask_pub_; + bool pub_init = false; + + VTR_REGISTER_MODULE_DEC_TYPE(RangeChangeNetModule); + +}; + +} // namespace lidar +} // namespace vtr \ No newline at end of file diff --git a/main/src/vtr_lidar/src/modules/planning/rangenet_change_detection_module.cpp b/main/src/vtr_lidar/src/modules/planning/rangenet_change_detection_module.cpp new file mode 100644 index 000000000..d2a91c15e --- /dev/null +++ b/main/src/vtr_lidar/src/modules/planning/rangenet_change_detection_module.cpp @@ -0,0 +1,88 @@ +// Copyright 2023, 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 rangenet_change_detection_module.cpp + * \author Alec Krawciw, Autonomous Space Robotics Lab (ASRL) + */ +#include +#include "vtr_lidar/filters/range_image.hpp" + + +namespace vtr { +namespace lidar { + +using namespace tactic; + +auto RangeChangeNetModule::Config::fromROS(const rclcpp::Node::SharedPtr &node, + const std::string ¶m_prefix) + -> ConstPtr { + auto config = std::make_shared(); + auto base_config = std::static_pointer_cast(config); + *base_config = *nn::TorchModule::Config::fromROS(node, param_prefix); + // clang-format off + // clang-format on + return config; +} + + +void RangeChangeNetModule::run_(QueryCache &qdata0, OutputCache &, + const Graph::Ptr &, const TaskExecutor::Ptr &) { + auto &qdata = dynamic_cast(qdata0); + + + auto& nn_point_cloud = *qdata.nn_point_cloud; + + if(!qdata.submap_loc.valid()) { + CLOG(WARNING, "lidar.range_change") << "Range image requires a map to work"; + return; + } + + if(!pub_init){ + mask_pub_ = qdata.node->create_publisher("detection_mask", 5); + } + auto& sub_map= *qdata.submap_loc; + auto& map_point_cloud = sub_map.point_cloud(); + + RangeImageParams image_params; + image_params.fov_up = 3 * M_PI / 180; + image_params.fov_down = -25 * M_PI / 180; + + Eigen::MatrixXf scan_image = Eigen::MatrixXf::Zero(64, 1024); + Eigen::MatrixXf map_image = Eigen::MatrixXf::Zero(64, 1024); + + generate_range_image(nn_point_cloud, scan_image, image_params); + generate_range_image(map_point_cloud, map_image, image_params); + + auto scan_tensor = torch::from_blob(scan_image.data(), {64, 1024}); + auto map_tensor = torch::from_blob(map_image.data(), {64, 1024}); + auto input = at::unsqueeze(at::stack({scan_tensor, map_tensor}), 0); + + auto tensor = evaluateModel(input, {1, 2, 64, 1024}); + auto mask = at::squeeze(at::argmax(tensor, 1), 0).to(torch::kUInt8); + + Image mask_im; + mask_im.width = 1024; + mask_im.height = 64; + mask_im.encoding = "mono8"; + mask_im.data = *mask.data_ptr(); + mask_im.step = 1024; + + + mask_pub_->publish(mask_im); + +} + +} // namespace nn +} // namespace vtr \ No newline at end of file diff --git a/main/src/vtr_torch/include/vtr_torch/modules/torch_module.hpp b/main/src/vtr_torch/include/vtr_torch/modules/torch_module.hpp index 0f88af978..4aa1337ae 100644 --- a/main/src/vtr_torch/include/vtr_torch/modules/torch_module.hpp +++ b/main/src/vtr_torch/include/vtr_torch/modules/torch_module.hpp @@ -95,6 +95,8 @@ class TorchModule : public tactic::BaseModule { template torch::Tensor evaluateModel(std::vector inputs, const Shape shape); + torch::Tensor evaluateModel(torch::Tensor input, const Shape shape); + }; } // namespace nn diff --git a/main/src/vtr_torch/include/vtr_torch/modules/torch_module.inl b/main/src/vtr_torch/include/vtr_torch/modules/torch_module.inl index 224c781ed..fc709eae2 100644 --- a/main/src/vtr_torch/include/vtr_torch/modules/torch_module.inl +++ b/main/src/vtr_torch/include/vtr_torch/modules/torch_module.inl @@ -20,6 +20,8 @@ namespace nn { return output.toTensor().cpu(); } + + } // namespace nn } // namespace vtr \ No newline at end of file diff --git a/main/src/vtr_torch/src/modules/torch_module.cpp b/main/src/vtr_torch/src/modules/torch_module.cpp index 0a11bde0e..6666b2587 100644 --- a/main/src/vtr_torch/src/modules/torch_module.cpp +++ b/main/src/vtr_torch/src/modules/torch_module.cpp @@ -47,5 +47,18 @@ auto TorchModule::Config::fromROS(const rclcpp::Node::SharedPtr &node, TorchModule::~TorchModule() {} + +torch::Tensor TorchModule::evaluateModel(torch::Tensor input, const Shape shape) { + torch::NoGradGuard no_grad; + std::vector jit_inputs; + + jit_inputs.push_back(input); + + + auto output = network(jit_inputs); + + return output.toTensor().cpu(); +} + } // namespace nn } // namespace vtr \ No newline at end of file From 99bb8cbd3793d2a34e4f63845db0d478bfcee897 Mon Sep 17 00:00:00 2001 From: Alec Krawciw Date: Thu, 29 Jun 2023 13:09:45 -0400 Subject: [PATCH 35/77] Running range images on the Grizzly --- config/hdl64_grizzly_learned.yaml | 338 ++++++++++++++++++ config/honeycomb_grizzly_learned.yaml | 1 + launch/offline_velodyne_grizzly.launch.yaml | 5 +- launch/online_velodyne_grizzly.launch.yaml | 6 +- .../include/vtr_lidar/filters/range_image.hpp | 59 ++- .../rangenet_change_detection_module.hpp | 8 + .../rangenet_change_detection_module.cpp | 81 ++++- .../pointmap/intra_exp_merging_module_v2.cpp | 26 +- .../vtr_torch/src/modules/torch_module.cpp | 4 +- rviz/lidar.rviz | 225 +++++++++--- 10 files changed, 677 insertions(+), 76 deletions(-) create mode 100644 config/hdl64_grizzly_learned.yaml diff --git a/config/hdl64_grizzly_learned.yaml b/config/hdl64_grizzly_learned.yaml new file mode 100644 index 000000000..0ce53e72c --- /dev/null +++ b/config/hdl64_grizzly_learned.yaml @@ -0,0 +1,338 @@ +/**: + ros__parameters: + log_to_file: true + log_debug: true + log_enabled: + - lidar.terrain_assessment + - navigation + - navigation.graph_map_server + #- navigation.command + - navigation.sensor_input + # tactic + - tactic + - tactic.pipeline + - tactic.module + #- tactic.module.live_mem_manager + #- tactic.module.graph_mem_manager + # path planner + # "path_planning", + # "path_planning.teb", + # "path_planning.cbit", + # "path_planning.cbit_planner", + # "mpc.cbit", + # "mpc_debug.cbit", + # mission planner + #- mission.server + #- mission.state_machine + # pose graph + - pose_graph + # pipeline specific + #- lidar.pipeline + - lidar.velodyne_converter_v2 + #- lidar.preprocessing + - lidar.odometry_icp + - lidar.range + # "lidar.odometry_map_maintenance", + #- lidar.vertex_test + - lidar.range_change + # "lidar.localization_map_recall", + # "lidar.localization_icp", + # "lidar.intra_exp_merging", + # "lidar.dynamic_detection", + # "lidar.inter_exp_merging", + # "lidar.change_detection", + # "lidar.ground_extraction", + # "lidar.obstacle_detection", + # "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 + origin_theta: 1.3 + scale: 1.0 + tactic: + enable_parallelization: true + preprocessing_skippable: false + odometry_mapping_skippable: false + localization_skippable: false + task_queue_num_threads: 1 + task_queue_size: -1 + + route_completion_translation_threshold: 0.5 + chain: + min_cusp_distance: 1.5 + angle_weight: 7.0 + search_depth: 5 + search_back_depth: 10 + distance_warning: 5.0 + save_odometry_result: true + save_localization_result: true + visualize: true + rviz_loc_path_offset: + - 0.0 + - 0.0 + - 0.0 + pipeline: + type: lidar + preprocessing: + - conversion + - filtering + odometry: + - icp + - mapping + - vertex_test + - intra_exp_merging + - dynamic_detection + - inter_exp_merging + - memory + localization: + - recall + - icp + # - safe_corridor + - range_change + #- change_detection_sync + - memory + submap_translation_threshold: 1.5 + submap_rotation_threshold: 30.0 + preprocessing: + conversion: + type: lidar.velodyne_converter_v2 + visualize: true + estimate_time: true + downsample_ratio: 3 + + filtering: + type: lidar.preprocessing + num_threads: 8 + + crop_range: 40.0 + + frame_voxel_size: 0.2 # grid subsampling voxel size + + #velodyne lidar has 1/3 degree upper block and 1/2 degree lower block + vertical_angle_res: 0.00872664626 # vertical angle resolution in radians (0.5deg), + 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 + #horizontal resolution 0.1728degree 0.1728/(0.5) + h_scale: 0.175 # 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 + traj_lock_prev_vel: false + traj_qc_diag: + - 1.0 + - 0.1 + - 0.1 + - 0.1 + - 0.1 + - 1.0 + num_threads: 8 + first_num_steps: 2 + initial_max_iter: 10 + initial_max_pairing_dist: 1.5 + initial_max_planar_dist: 1.0 + refined_max_iter: 50 + refined_max_pairing_dist: 1.0 + refined_max_planar_dist: 0.3 + averaging_num_steps: 2 + verbose: false + max_iterations: 1 + min_matched_ratio: 0.85 + visualize: true + mapping: + type: lidar.odometry_map_maintenance_v2 + map_voxel_size: 0.2 + crop_range_front: 40.0 + back_over_front_ratio: 0.5 + point_life_time: 20.0 + visualize: true + vertex_test: + type: lidar.vertex_test + max_translation: 0.5 + max_rotation: 10.0 + intra_exp_merging: + type: lidar.intra_exp_merging_v2 + depth: 6.0 + map_voxel_size: 0.2 + 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.1728 # 0.02042 + vertical_resolution: 0.00872664626 # 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 + max_num_experiences: 128 + distance_threshold: 0.6 + planar_threshold: 0.2 + normal_threshold: 0.8 + dynamic_obs_threshold: 1 + visualize: true + memory: + type: live_mem_manager + window_size: 5 + localization: + recall: + type: lidar.localization_map_recall + map_version: pointmap + visualize: true + icp: + type: lidar.localization_icp + use_pose_prior: true + num_threads: 8 + first_num_steps: 2 + initial_max_iter: 10 + initial_max_pairing_dist: 1.5 + initial_max_planar_dist: 1.0 + refined_max_iter: 50 + refined_max_pairing_dist: 1.0 + refined_max_planar_dist: 0.3 + averaging_num_steps: 2 + verbose: false + max_iterations: 1 + min_matched_ratio: 0.3 + safe_corridor: + type: lidar.safe_corridor + lookahead_distance: 5.0 + corridor_width: 3.5 + influence_distance: 1.0 + resolution: 0.25 + size_x: 16.0 + size_y: 8.0 + visualize: true + range_change: + type: torch.range_change + use_gpu: true + abs_filepath: false + filepath: exported_range_model.pt + visualize: true + memory: + type: graph_mem_manager + vertex_life_span: 5 + window_size: 3 + obstacle_detection: + type: lidar.obstacle_detection + z_min: 0.5 + z_max: 2.0 + resolution: 0.6 + size_x: 40.0 + size_y: 20.0 + run_async: true + visualize: true + terrain_assessment: + type: lidar.terrain_assessment + lookahead_distance: 15.0 + corridor_width: 1.0 + search_radius: 1.0 + resolution: 0.5 + size_x: 40.0 + size_y: 20.0 + run_online: false + run_async: true + visualize: true + path_planning: + type: cbit.lidar # teb.lidar for old teb version, cbit for obstacle free path tracker, cbit.lidar for obstacle avoidance version + control_period: 100 # ms + + cbit: + obs_padding: 0.0 + curv_to_euclid_discretization: 10 + sliding_window_width: 12.0 + sliding_window_freespace_padding: 0.5 + corridor_resolution: 0.1 + state_update_freq: 1.0 + update_state: true + rand_seed: 1 + + # Planner Tuning Params + initial_samples: 400 + batch_samples: 200 + pre_seed_resolution: 0.5 + alpha: 0.5 + q_max: 2.5 + frame_interval: 50 + iter_max: 10000000 + 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: 30 # Note I dont think im actually using this one anymore, am using the one in the change detection v3 now + + mpc: + # Controller Params + horizon_steps: 20 + horizon_step_size: 0.3 + forward_vel: 0.75 + max_lin_vel: 1.25 + max_ang_vel: 0.75 + robot_linear_velocity_scale: 1.1 # Used to scale the output twist linear velocity messages by some constant factor to compensate internal low level control miscalibration + robot_angular_velocity_scale: 1.0 # Used to scale the output twist angular velocity messages by some constant factor to compensate internal low level control miscalibration + + vehicle_model: "unicycle" + + # Cost Function Covariance Matrices + pose_error_cov: [100.0, 100.0, 1000.0, 1000.0, 1000.0, 200.0] + vel_error_cov: [300.0, 300.0] # was [0.1 2.0] # No longer using this cost term + acc_error_cov: [100.0, 100.0] + kin_error_cov: [0.001, 0.001, 0.001, 0.001, 0.001, 0.001] + lat_error_cov: [20.0] + + # Cost Function Covariance Matrices (Tracking Backup) + #pose_error_cov: [100.0, 100.0, 1000.0, 1000.0, 1000.0, 200.0] + #vel_error_cov: [30.0, 30.0] # was [0.1 2.0] # No longer using this cost term + #acc_error_cov: [10.0, 10.0] + #kin_error_cov: [0.001, 0.001, 0.001, 0.001, 0.001, 0.001] + #lat_error_cov: [10.0] + + # Cost Function Weights + pose_error_weight: 1.0 + vel_error_weight: 1.0 + acc_error_weight: 1.0 + kin_error_weight: 1.0 + lat_error_weight: 0.01 + + #backup for tracking mpc + #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/config/honeycomb_grizzly_learned.yaml b/config/honeycomb_grizzly_learned.yaml index e83c7c2af..cde041c57 100644 --- a/config/honeycomb_grizzly_learned.yaml +++ b/config/honeycomb_grizzly_learned.yaml @@ -215,6 +215,7 @@ use_gpu: false abs_filepath: false filepath: exported_range_model.pt + visualize: true change_detection_sync: type: lidar.change_detection_v3 detection_range: 8.0 diff --git a/launch/offline_velodyne_grizzly.launch.yaml b/launch/offline_velodyne_grizzly.launch.yaml index 485c0d2ca..f67949e35 100644 --- a/launch/offline_velodyne_grizzly.launch.yaml +++ b/launch/offline_velodyne_grizzly.launch.yaml @@ -17,9 +17,10 @@ windows: panes: - > ros2 launch vtr_navigation vtr.launch.py - base_params:=hdl64_grizzly_default.yaml + base_params:=hdl64_grizzly_learned.yaml data_dir:=${VTRTEMP}/lidar/ - start_new_graph:=true + model_dir:=${VTRMODELS} + start_new_graph:=false use_sim_time:=true - window_name: vtr_gui diff --git a/launch/online_velodyne_grizzly.launch.yaml b/launch/online_velodyne_grizzly.launch.yaml index 91cab322e..1ce7aaa33 100644 --- a/launch/online_velodyne_grizzly.launch.yaml +++ b/launch/online_velodyne_grizzly.launch.yaml @@ -19,9 +19,10 @@ windows: panes: - > ros2 launch vtr_navigation vtr.launch.py - base_params:=hdl64_grizzly_default.yaml + base_params:=hdl64_grizzly_learned.yaml data_dir:=${VTRTEMP}/lidar/$(date '+%F')/$(date '+%F') start_new_graph:=false + model_dir:=${VTRMODELS} use_sim_time:=false - window_name: vtr_gui @@ -39,5 +40,6 @@ windows: shell_command_before: - source ${VTRSRC}/main/install/setup.bash panes: - - ros2 run rviz2 rviz2 -d ${VTRSRC}/rviz/lidar.rviz + #- ros2 run rviz2 rviz2 -d ${VTRSRC}/rviz/lidar.rviz + - ros2 launch foxglove_bridge foxglove_bridge_launch.xml # - ros2 run rqt_reconfigure rqt_reconfigure diff --git a/main/src/vtr_lidar/include/vtr_lidar/filters/range_image.hpp b/main/src/vtr_lidar/include/vtr_lidar/filters/range_image.hpp index 9d9a6b4e8..9ba02f174 100644 --- a/main/src/vtr_lidar/include/vtr_lidar/filters/range_image.hpp +++ b/main/src/vtr_lidar/include/vtr_lidar/filters/range_image.hpp @@ -19,6 +19,7 @@ #pragma once #include "pcl/point_cloud.h" +#include "sensor_msgs/msg/image.hpp" #include "vtr_lidar/data_types/point.hpp" namespace vtr { @@ -27,17 +28,28 @@ namespace lidar{ struct RangeImageParams { double fov_up; //In radians double fov_down; //In radians + double crop_range = 1000.0; //m - double getFOV() { return abs(fov_down) + abs(fov_up);} + double getFOV() { return abs(fov_up - fov_down);} }; -/// \todo Parallelize +void generate_range_image(const pcl::PointCloud& point_cloud, Eigen::MatrixXf& range_image, Eigen::MatrixXi& idx_image, RangeImageParams params); + void generate_range_image(const pcl::PointCloud& point_cloud, Eigen::MatrixXf& range_image, RangeImageParams params) { + Eigen::MatrixXi temp_idx = Eigen::MatrixXi::Constant(range_image.rows(), range_image.cols(), -1); + generate_range_image(point_cloud, range_image, temp_idx, params); +} + +/// \todo Parallelize +void generate_range_image(const pcl::PointCloud& point_cloud, Eigen::MatrixXf& range_image, Eigen::MatrixXi& idx_image, RangeImageParams params) { for (size_t i = 0; i < point_cloud.size(); ++i) { auto& point = point_cloud[i]; - double proj_x = 0.5 * (point.theta / M_PI + 1.0); - double proj_y = 1.0 - (point.phi + abs(params.fov_down)) / params.getFOV(); + if (point.rho > params.crop_range) + continue; + double proj_x = (remainder(point.phi, 2 * M_PI) / 2 / M_PI + 0.5); + double proj_y = 1.0 - (M_PI/2 - point.theta + abs(params.fov_down)) / params.getFOV(); + proj_x = (proj_x < 0.0) ? 0.0 : proj_x; proj_x = (proj_x > 1.0) ? 1.0 : proj_x; @@ -49,8 +61,47 @@ void generate_range_image(const pcl::PointCloud& point_cloud, Eig proj_y *= range_image.rows() - 1; range_image((int)proj_y, (int)proj_x) = point.rho; + idx_image((int)proj_y, (int)proj_x) = i; + } + // CLOG(DEBUG, "lidar.range") << idx_image; + +} + +//Definitely parallelize, no reverse duplication +void unproject_range_image(pcl::PointCloud& point_cloud, const Eigen::MatrixXf& data_image, const Eigen::MatrixXi& idx_image) { + + for (int i = 0; i < idx_image.rows(); i++){ + //#pragma omp parallel for schedule(dynamic, 10) num_threads(num_threads) + for (int j = 0; j < idx_image.cols(); j++) { + int idx = idx_image(i, j); + if (idx >= 0) + point_cloud[idx].flex24 = data_image(i, j) + 1; + } } + +} + +sensor_msgs::msg::Image range_to_image(const Eigen::MatrixXf& data_image) { + float max_range = data_image.maxCoeff(); + CLOG(DEBUG, "lidar.range") << "Max: " << data_image.maxCoeff() << " Min: " << data_image.minCoeff(); + + + sensor_msgs::msg::Image ros_im; + ros_im.width = data_image.cols(); + ros_im.height = data_image.rows(); + ros_im.encoding = "mono8"; + ros_im.step = data_image.cols(); + size_t size = data_image.size()*sizeof(uint8_t); + ros_im.data.resize(size); + + + Eigen::Matrix scaled_image = (data_image / max_range * 255).cast(); + memcpy(&ros_im.data[0], scaled_image.data(), size); + + return ros_im; } + + } // namespace lidar } // vtr diff --git a/main/src/vtr_lidar/include/vtr_lidar/modules/planning/rangenet_change_detection_module.hpp b/main/src/vtr_lidar/include/vtr_lidar/modules/planning/rangenet_change_detection_module.hpp index a411d443a..27d2a649f 100644 --- a/main/src/vtr_lidar/include/vtr_lidar/modules/planning/rangenet_change_detection_module.hpp +++ b/main/src/vtr_lidar/include/vtr_lidar/modules/planning/rangenet_change_detection_module.hpp @@ -30,6 +30,8 @@ namespace vtr { namespace lidar { using Image = sensor_msgs::msg::Image; +using PointCloudMsg = sensor_msgs::msg::PointCloud2; + /** \brief Load and store Torch Models */ class RangeChangeNetModule : public nn::TorchModule { @@ -42,6 +44,8 @@ class RangeChangeNetModule : public nn::TorchModule { struct Config : public nn::TorchModule::Config { PTR_TYPEDEFS(Config); + bool visualize = false; + static ConstPtr fromROS(const rclcpp::Node::SharedPtr &node, const std::string ¶m_prefix); }; @@ -60,6 +64,10 @@ class RangeChangeNetModule : public nn::TorchModule { Config::ConstPtr config_; rclcpp::Publisher::SharedPtr mask_pub_; + rclcpp::Publisher::SharedPtr live_range_pub_; + rclcpp::Publisher::SharedPtr map_range_pub_; + rclcpp::Publisher::SharedPtr diffpcd_pub_; + rclcpp::Publisher::SharedPtr mappcd_pub_; bool pub_init = false; VTR_REGISTER_MODULE_DEC_TYPE(RangeChangeNetModule); diff --git a/main/src/vtr_lidar/src/modules/planning/rangenet_change_detection_module.cpp b/main/src/vtr_lidar/src/modules/planning/rangenet_change_detection_module.cpp index d2a91c15e..1ff8b991c 100644 --- a/main/src/vtr_lidar/src/modules/planning/rangenet_change_detection_module.cpp +++ b/main/src/vtr_lidar/src/modules/planning/rangenet_change_detection_module.cpp @@ -23,6 +23,24 @@ namespace vtr { namespace lidar { +namespace { + void velodyneCart2Pol(pcl::PointCloud &point_cloud) { + for (size_t i = 0; i < point_cloud.size(); i++) { + auto &p = point_cloud[i]; + auto &pm1 = i > 0 ? point_cloud[i - 1] : point_cloud[i]; + + p.rho = sqrt(p.x * p.x + p.y * p.y + p.z * p.z); + p.theta = atan2(sqrt(p.x * p.x + p.y * p.y), p.z); + p.phi = atan2(p.y, p.x); // + M_PI / 2; + + if (i > 0 && (p.phi - pm1.phi) > 1.5 * M_PI) + p.phi -= 2 * M_PI; + else if (i > 0 && (p.phi - pm1.phi) < -1.5 * M_PI) + p.phi += 2 * M_PI; + } +} +} + using namespace tactic; auto RangeChangeNetModule::Config::fromROS(const rclcpp::Node::SharedPtr &node, @@ -31,7 +49,10 @@ auto RangeChangeNetModule::Config::fromROS(const rclcpp::Node::SharedPtr &node, auto config = std::make_shared(); auto base_config = std::static_pointer_cast(config); *base_config = *nn::TorchModule::Config::fromROS(node, param_prefix); + // clang-format off + config->visualize = node->declare_parameter(param_prefix + ".visualize", config->visualize); + // clang-format on return config; } @@ -42,7 +63,12 @@ void RangeChangeNetModule::run_(QueryCache &qdata0, OutputCache &, auto &qdata = dynamic_cast(qdata0); - auto& nn_point_cloud = *qdata.nn_point_cloud; + auto nn_point_cloud = *qdata.nn_point_cloud; + const auto &T_s_r = *qdata.T_s_r; + const auto &T_r_v_loc = *qdata.T_r_v_loc; + const auto &T_v_m_loc = *qdata.T_v_m_loc; + + if(!qdata.submap_loc.valid()) { CLOG(WARNING, "lidar.range_change") << "Range image requires a map to work"; @@ -51,18 +77,39 @@ void RangeChangeNetModule::run_(QueryCache &qdata0, OutputCache &, if(!pub_init){ mask_pub_ = qdata.node->create_publisher("detection_mask", 5); + live_range_pub_ = qdata.node->create_publisher("live_range_image", 5); + map_range_pub_ = qdata.node->create_publisher("map_range_image", 5); + diffpcd_pub_ = qdata.node->create_publisher("detection_cloud", 5); + mappcd_pub_ = qdata.node->create_publisher("transformed_map", 5); } + auto& sub_map= *qdata.submap_loc; - auto& map_point_cloud = sub_map.point_cloud(); + const auto &T_lv_pm = sub_map.T_vertex_this().matrix(); + + //Need to convert frame! + auto map_point_cloud = sub_map.point_cloud(); + auto points_mat = map_point_cloud.getMatrixXfMap(4, PointWithInfo::size(), PointWithInfo::cartesian_offset()); + + CLOG(DEBUG, "lidar.range") << "Before: (" << map_point_cloud[10].x << ", " << map_point_cloud[10].y << ", "<< map_point_cloud[10].z <<")"; + const auto T_s_m = (T_s_r * T_r_v_loc * T_v_m_loc).matrix(); + points_mat = T_s_m.cast() * points_mat; + velodyneCart2Pol(map_point_cloud); + + + CLOG(DEBUG, "lidar.range") << "After: (" << map_point_cloud[10].x << ", " << map_point_cloud[10].y << ", "<< map_point_cloud[10].z <<")"; + RangeImageParams image_params; image_params.fov_up = 3 * M_PI / 180; image_params.fov_down = -25 * M_PI / 180; + image_params.crop_range = 8.0; Eigen::MatrixXf scan_image = Eigen::MatrixXf::Zero(64, 1024); + Eigen::MatrixXf mask_image = Eigen::MatrixXf::Zero(64, 1024); + Eigen::MatrixXi scan_idxs = Eigen::MatrixXi::Constant(64, 1024, -1); Eigen::MatrixXf map_image = Eigen::MatrixXf::Zero(64, 1024); - generate_range_image(nn_point_cloud, scan_image, image_params); + generate_range_image(nn_point_cloud, scan_image, scan_idxs, image_params); generate_range_image(map_point_cloud, map_image, image_params); auto scan_tensor = torch::from_blob(scan_image.data(), {64, 1024}); @@ -71,16 +118,30 @@ void RangeChangeNetModule::run_(QueryCache &qdata0, OutputCache &, auto tensor = evaluateModel(input, {1, 2, 64, 1024}); auto mask = at::squeeze(at::argmax(tensor, 1), 0).to(torch::kUInt8); + torch::from_blob(mask_image.data(), {64, 1024}) = mask; - Image mask_im; - mask_im.width = 1024; - mask_im.height = 64; - mask_im.encoding = "mono8"; - mask_im.data = *mask.data_ptr(); - mask_im.step = 1024; + unproject_range_image(nn_point_cloud, mask_image, scan_idxs); + /// publish the transformed pointcloud + if (config_->visualize) { + PointCloudMsg filter_msg; + pcl::toROSMsg(nn_point_cloud, filter_msg); + filter_msg.header.frame_id = "odo vertex frame"; + // pointcloud_msg.header.stamp = rclcpp::Time(*qdata.stamp); + diffpcd_pub_->publish(filter_msg); - mask_pub_->publish(mask_im); + PointCloudMsg map_msg; + pcl::toROSMsg(map_point_cloud, map_msg); + map_msg.header.frame_id = "odo vertex frame"; + // pointcloud_msg.header.stamp = rclcpp::Time(*qdata.stamp); + mappcd_pub_->publish(map_msg); + + + mask_pub_->publish(range_to_image(mask_image)); + live_range_pub_->publish(range_to_image(scan_image)); + map_range_pub_->publish(range_to_image(map_image)); + + } } diff --git a/main/src/vtr_lidar/src/modules/pointmap/intra_exp_merging_module_v2.cpp b/main/src/vtr_lidar/src/modules/pointmap/intra_exp_merging_module_v2.cpp index 7341ccef4..17488c966 100644 --- a/main/src/vtr_lidar/src/modules/pointmap/intra_exp_merging_module_v2.cpp +++ b/main/src/vtr_lidar/src/modules/pointmap/intra_exp_merging_module_v2.cpp @@ -92,6 +92,11 @@ void IntraExpMergingModuleV2::runAsync_(QueryCache &qdata0, OutputCache &, { const auto msg = target_vertex->retrieve( "pointmap_ptr", "vtr_lidar_msgs/msg/PointMapPointer"); + if (msg == nullptr) { + CLOG(WARNING, "lidar.intra_exp_merging") + << "This vertex does not have an associated submap pointer, skipped."; + return; + } auto locked_msg = msg->sharedLocked(); const auto &pointmap_ptr = locked_msg.get().getData(); // @@ -106,6 +111,11 @@ void IntraExpMergingModuleV2::runAsync_(QueryCache &qdata0, OutputCache &, { const auto map_msg = target_vertex->retrieve>( "pointmap", "vtr_lidar_msgs/msg/PointMap"); + if (map_msg == nullptr) { + CLOG(WARNING, "lidar.intra_exp_merging") + << "This vertex does not have an associated submap pointer, skipped."; + return; + } auto locked_map_msg_ref = map_msg->sharedLocked(); // lock the msg auto &locked_map_msg = locked_map_msg_ref.get(); @@ -143,6 +153,11 @@ void IntraExpMergingModuleV2::runAsync_(QueryCache &qdata0, OutputCache &, { const auto msg = vertex->retrieve( "pointmap_ptr", "vtr_lidar_msgs/msg/PointMapPointer"); + if (msg == nullptr) { + CLOG(WARNING, "lidar.intra_exp_merging") + << "This vertex does not have an associated submap pointer, skipped."; + continue; + } auto locked_msg = msg->sharedLocked(); const auto &pointmap_ptr = locked_msg.get().getData(); if (pointmap_ptr.map_vid != vertex->id()) continue; @@ -156,7 +171,11 @@ void IntraExpMergingModuleV2::runAsync_(QueryCache &qdata0, OutputCache &, // retrieve point map v0 (initial map) from this vertex const auto map_msg = vertex->retrieve>( "pointmap_v0", "vtr_lidar_msgs/msg/PointMap"); - + if (map_msg == nullptr) { + CLOG(WARNING, "lidar.intra_exp_merging") + << "This vertex does not have an associated submap pointer, skipped."; + continue; + } auto pointmap = map_msg->sharedLocked().get().getData(); const auto &T_v_m = (T_target_curr * pointmap.T_vertex_this()).matrix(); auto &point_cloud = pointmap.point_cloud(); @@ -207,6 +226,11 @@ void IntraExpMergingModuleV2::runAsync_(QueryCache &qdata0, OutputCache &, { const auto map_msg = target_vertex->retrieve>( "pointmap", "vtr_lidar_msgs/msg/PointMap"); + if (map_msg == nullptr) { + CLOG(WARNING, "lidar.intra_exp_merging") + << "This vertex does not have an associated submap pointer, skipped."; + return; + } auto locked_map_msg_ref = map_msg->locked(); // lock the msg auto &locked_map_msg = locked_map_msg_ref.get(); locked_map_msg.setData(updated_map); diff --git a/main/src/vtr_torch/src/modules/torch_module.cpp b/main/src/vtr_torch/src/modules/torch_module.cpp index 6666b2587..c1c381d40 100644 --- a/main/src/vtr_torch/src/modules/torch_module.cpp +++ b/main/src/vtr_torch/src/modules/torch_module.cpp @@ -33,7 +33,7 @@ auto TorchModule::Config::fromROS(const rclcpp::Node::SharedPtr &node, config->use_gpu = node->declare_parameter(param_prefix + ".use_gpu", config->use_gpu); config->abs_filepath = node->declare_parameter(param_prefix + ".abs_filepath", config->abs_filepath); - auto model_dir = node->declare_parameter("model_dir", "defalut2"); + auto model_dir = node->declare_parameter("model_dir", "default"); model_dir = common::utils::expand_user(common::utils::expand_env(model_dir)); if (config->abs_filepath){ @@ -52,7 +52,7 @@ torch::Tensor TorchModule::evaluateModel(torch::Tensor input, const Shape shape) torch::NoGradGuard no_grad; std::vector jit_inputs; - jit_inputs.push_back(input); + jit_inputs.push_back(input.to(device)); auto output = network(jit_inputs); diff --git a/rviz/lidar.rviz b/rviz/lidar.rviz index efc3623a5..b1e9f281c 100644 --- a/rviz/lidar.rviz +++ b/rviz/lidar.rviz @@ -8,18 +8,16 @@ Panels: - /TF1 - /TF1/Frames1 - /TF1/Tree1 - - /raw scan1 - - /filtered scan1 - /odometry path1/Shape1 - /curr map loc1/Topic1 - /planned path1/Offset1 - /planned path (poses)1/Topic1 - - /change detection scan1 - - /chang detection cost pcd1 - /global path1/Offset1 - /PointCloud21 + - /PointCloud21/Topic1 + - /Map1 Splitter Ratio: 0.4507042169570923 - Tree Height: 871 + Tree Height: 372 - Class: rviz_common/Selection Name: Selection - Class: rviz_common/Tool Properties @@ -37,7 +35,7 @@ Panels: Experimental: false Name: Time SyncMode: 0 - SyncSource: "" + SyncSource: PointCloud2 Visualization Manager: Class: "" Displays: @@ -71,36 +69,60 @@ Visualization Manager: Frame Timeout: 99999 Frames: All Enabled: false + base_footprint: + Value: true + base_footprint_bumper_part: + Value: true base_link: Value: true chassis_link: Value: true front_axle_link: Value: true - front_left_wheel_link: + front_bumblebee: + Value: true + front_xb3: Value: true - front_right_wheel_link: + honeycomb: Value: true imu_link: Value: true lidar: Value: false + loc vertex frame: + Value: true + loc vertex frame (offset): + Value: true + m600_base: + Value: true + microstrain: + Value: true navsat_link: Value: true + odo vertex frame: + Value: true odom: Value: true - rear_antenna_link: + planning frame: Value: true - rear_left_wheel_link: + rear_antenna_link: Value: true - rear_right_wheel_link: + rear_bumblebee: Value: true rear_sensor_base_link: Value: true rear_sensor_plate_link: Value: true + rear_xb3: + Value: true robot: Value: true + robot planning: + Value: true + robot planning (extrapolated): + Value: true + robot planning (extrapolated) mpc: + Value: true sensor_anchor_link: Value: true velodyne: @@ -116,8 +138,23 @@ Visualization Manager: Show Names: true Tree: world: - world (offset): + loc vertex frame: {} + odo vertex frame: + robot: + lidar: + velodyne: + {} + planning frame: + robot planning: + {} + robot planning (extrapolated): + {} + robot planning (extrapolated) mpc: + {} + world (offset): + loc vertex frame (offset): + {} Update Interval: 0 Value: true - Alpha: 1 @@ -132,7 +169,7 @@ Visualization Manager: Color: 255; 255; 255 Color Transformer: Intensity Decay Time: 0 - Enabled: true + Enabled: false Invert Rainbow: false Max Color: 0; 255; 0 Max Intensity: 0 @@ -153,7 +190,7 @@ Visualization Manager: Value: /vtr/raw_point_cloud Use Fixed Frame: true Use rainbow: true - Value: true + Value: false - Alpha: 1 Autocompute Intensity Bounds: true Autocompute Value Bounds: @@ -536,40 +573,6 @@ Visualization Manager: Use Fixed Frame: true Use rainbow: false Value: false - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 10 - Min Value: -10 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz_default_plugins/PointCloud2 - Color: 255; 255; 255 - Color Transformer: Intensity - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 0.8999999761581421 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: chang detection cost pcd - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.30000001192092896 - Style: Flat Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /vtr/change_detection_diff - Use Fixed Frame: true - Use rainbow: true - Value: true - Alpha: 1 Autocompute Intensity Bounds: true Autocompute Value Bounds: @@ -1196,7 +1199,7 @@ Visualization Manager: Color: 255; 255; 255 Color Transformer: Intensity Decay Time: 0 - Enabled: true + Enabled: false Invert Rainbow: false Max Color: 255; 255; 255 Max Intensity: 255 @@ -1217,6 +1220,116 @@ Visualization Manager: Value: /velodyne_points Use Fixed Frame: true Use rainbow: true + Value: false + - Alpha: 1 + Autocompute Intensity Bounds: false + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: flex24 + Class: rviz_default_plugins/PointCloud2 + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 0 + Enabled: false + Invert Rainbow: true + Max Color: 255; 255; 255 + Max Intensity: 2 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: PointCloud2 + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.10000000149011612 + Style: Flat Squares + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /vtr/detection_cloud + Use Fixed Frame: true + Use rainbow: true + Value: false + - Class: rviz_default_plugins/Image + Enabled: true + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: Image + Normalize Range: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /vtr/detection_mask + Value: true + - Class: rviz_default_plugins/Image + Enabled: true + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: Image + Normalize Range: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /vtr/map_range_image + Value: true + - Class: rviz_default_plugins/Image + Enabled: true + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: Image + Normalize Range: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /vtr/live_range_image + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: flex13 + Class: rviz_default_plugins/PointCloud2 + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 9.420014381408691 + Min Color: 0; 0; 0 + Min Intensity: -4.211626052856445 + Name: Map + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.10000000149011612 + Style: Flat Squares + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /vtr/transformed_map + Use Fixed Frame: true + Use rainbow: true Value: true Enabled: true Global Options: @@ -1265,7 +1378,7 @@ Visualization Manager: Views: Current: Class: rviz_default_plugins/Orbit - Distance: 26.967164993286133 + Distance: 34.462005615234375 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 @@ -1280,10 +1393,10 @@ Visualization Manager: Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: 0.7153981328010559 + Pitch: 1.1403958797454834 Target Frame: robot Value: Orbit (rviz_default_plugins) - Yaw: 0.4403983950614929 + Yaw: 3.3322339057922363 Saved: - Class: rviz_default_plugins/Orbit Distance: 111.39698028564453 @@ -1490,8 +1603,10 @@ Window Geometry: collapsed: false Height: 1016 Hide Left Dock: false - Hide Right Dock: false - QMainWindow State: 000000ff00000000fd000000040000000000000281000003a2fc0200000007fc0000003d000001a20000000000fffffffaffffffff0100000002fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730000000000ffffffff0000008c00fffffffb0000001200530065006c0065006300740069006f006e0000000000000002530000006c00fffffffb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b000003a2000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c0000026100000001000001c0000003a2fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003b000003a2000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000005000000006efc0100000002fb0000000800540069006d00650000000000000005000000025300fffffffb0000000800540069006d00650100000000000004500000000000000000000002eb000003a200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Hide Right Dock: true + Image: + collapsed: false + QMainWindow State: 000000ff00000000fd0000000400000000000003de000003a2fc020000000afc0000003d000001a20000000000fffffffaffffffff0100000002fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730000000000ffffffff0000008c00fffffffb0000001200530065006c0065006300740069006f006e0000000000000002530000006c00fffffffb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b000001af000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d00610067006501000001f00000007a0000002800fffffffb0000000a0049006d00610067006501000002700000009d0000002800fffffffb0000000a0049006d0061006700650100000313000000ca0000002800ffffff00000001000001c0000003a2fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003b000003a2000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000005000000006efc0100000002fb0000000800540069006d00650000000000000005000000025300fffffffb0000000800540069006d0065010000000000000450000000000000000000000354000003a200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Time: @@ -1499,7 +1614,7 @@ Window Geometry: Tool Properties: collapsed: false Views: - collapsed: false + collapsed: true Width: 1848 X: 72 Y: 27 From 4f044e9202f28f7dbb390be6d754b9e0095af812 Mon Sep 17 00:00:00 2001 From: Alec Krawciw Date: Fri, 7 Jul 2023 13:04:14 -0400 Subject: [PATCH 36/77] Fixed row-major issue with Eigen --- config/hdl64_grizzly_learned.yaml | 8 +- launch/offline_velodyne_grizzly.launch.yaml | 4 +- .../include/vtr_lidar/filters/range_image.hpp | 20 ++--- .../rangenet_change_detection_module.hpp | 6 ++ .../rangenet_change_detection_module.cpp | 26 ++++-- rviz/lidar.rviz | 80 +++++-------------- 6 files changed, 61 insertions(+), 83 deletions(-) diff --git a/config/hdl64_grizzly_learned.yaml b/config/hdl64_grizzly_learned.yaml index 0ce53e72c..00a3f4f7c 100644 --- a/config/hdl64_grizzly_learned.yaml +++ b/config/hdl64_grizzly_learned.yaml @@ -43,7 +43,6 @@ # "lidar.change_detection", # "lidar.ground_extraction", # "lidar.obstacle_detection", - # "lidar.terrain_assessment", robot_frame: base_link env_info_topic: env_info # lidar related @@ -99,11 +98,12 @@ - recall - icp # - safe_corridor - - range_change + #- range_change #- change_detection_sync - memory submap_translation_threshold: 1.5 submap_rotation_threshold: 30.0 + save_nn_point_cloud: true preprocessing: conversion: type: lidar.velodyne_converter_v2 @@ -299,8 +299,8 @@ # Controller Params horizon_steps: 20 horizon_step_size: 0.3 - forward_vel: 0.75 - max_lin_vel: 1.25 + forward_vel: 0.5 + max_lin_vel: 1.0 max_ang_vel: 0.75 robot_linear_velocity_scale: 1.1 # Used to scale the output twist linear velocity messages by some constant factor to compensate internal low level control miscalibration robot_angular_velocity_scale: 1.0 # Used to scale the output twist angular velocity messages by some constant factor to compensate internal low level control miscalibration diff --git a/launch/offline_velodyne_grizzly.launch.yaml b/launch/offline_velodyne_grizzly.launch.yaml index f67949e35..0c5bb0033 100644 --- a/launch/offline_velodyne_grizzly.launch.yaml +++ b/launch/offline_velodyne_grizzly.launch.yaml @@ -38,5 +38,7 @@ windows: shell_command_before: - source ${VTRSRC}/main/install/setup.bash panes: - - ros2 run rviz2 rviz2 -d ${VTRSRC}/rviz/lidar.rviz + #- ros2 run rviz2 rviz2 -d ${VTRSRC}/rviz/lidar.rviz + - ros2 launch foxglove_bridge foxglove_bridge_launch.xml + # - ros2 run rqt_reconfigure rqt_reconfigure diff --git a/main/src/vtr_lidar/include/vtr_lidar/filters/range_image.hpp b/main/src/vtr_lidar/include/vtr_lidar/filters/range_image.hpp index 9ba02f174..348cc389e 100644 --- a/main/src/vtr_lidar/include/vtr_lidar/filters/range_image.hpp +++ b/main/src/vtr_lidar/include/vtr_lidar/filters/range_image.hpp @@ -25,6 +25,9 @@ namespace vtr { namespace lidar{ +using RowMatrixXf = Eigen::Matrix; +using RowMatrixXuI8 = Eigen::Matrix; + struct RangeImageParams { double fov_up; //In radians double fov_down; //In radians @@ -33,15 +36,15 @@ struct RangeImageParams { double getFOV() { return abs(fov_up - fov_down);} }; -void generate_range_image(const pcl::PointCloud& point_cloud, Eigen::MatrixXf& range_image, Eigen::MatrixXi& idx_image, RangeImageParams params); +void generate_range_image(const pcl::PointCloud& point_cloud, RowMatrixXf& range_image, Eigen::MatrixXi& idx_image, RangeImageParams params); -void generate_range_image(const pcl::PointCloud& point_cloud, Eigen::MatrixXf& range_image, RangeImageParams params) { +void generate_range_image(const pcl::PointCloud& point_cloud, RowMatrixXf& range_image, RangeImageParams params) { Eigen::MatrixXi temp_idx = Eigen::MatrixXi::Constant(range_image.rows(), range_image.cols(), -1); generate_range_image(point_cloud, range_image, temp_idx, params); } /// \todo Parallelize -void generate_range_image(const pcl::PointCloud& point_cloud, Eigen::MatrixXf& range_image, Eigen::MatrixXi& idx_image, RangeImageParams params) { +void generate_range_image(const pcl::PointCloud& point_cloud, RowMatrixXf& range_image, Eigen::MatrixXi& idx_image, RangeImageParams params) { for (size_t i = 0; i < point_cloud.size(); ++i) { auto& point = point_cloud[i]; @@ -68,7 +71,7 @@ void generate_range_image(const pcl::PointCloud& point_cloud, Eig } //Definitely parallelize, no reverse duplication -void unproject_range_image(pcl::PointCloud& point_cloud, const Eigen::MatrixXf& data_image, const Eigen::MatrixXi& idx_image) { +void unproject_range_image(pcl::PointCloud& point_cloud, const RowMatrixXf& data_image, const Eigen::MatrixXi& idx_image) { for (int i = 0; i < idx_image.rows(); i++){ //#pragma omp parallel for schedule(dynamic, 10) num_threads(num_threads) @@ -81,10 +84,8 @@ void unproject_range_image(pcl::PointCloud& point_cloud, const Ei } -sensor_msgs::msg::Image range_to_image(const Eigen::MatrixXf& data_image) { +sensor_msgs::msg::Image range_to_image(const RowMatrixXf& data_image) { float max_range = data_image.maxCoeff(); - CLOG(DEBUG, "lidar.range") << "Max: " << data_image.maxCoeff() << " Min: " << data_image.minCoeff(); - sensor_msgs::msg::Image ros_im; ros_im.width = data_image.cols(); @@ -94,9 +95,10 @@ sensor_msgs::msg::Image range_to_image(const Eigen::MatrixXf& data_image) { size_t size = data_image.size()*sizeof(uint8_t); ros_im.data.resize(size); - - Eigen::Matrix scaled_image = (data_image / max_range * 255).cast(); + RowMatrixXuI8 scaled_image = (data_image / max_range * 255).cast(); memcpy(&ros_im.data[0], scaled_image.data(), size); + // Eigen::Matrix scaled_image = (data_image / max_range * 255).cast(); + // memcpy(&ros_im.data[0], scaled_image.data(), size); return ros_im; } diff --git a/main/src/vtr_lidar/include/vtr_lidar/modules/planning/rangenet_change_detection_module.hpp b/main/src/vtr_lidar/include/vtr_lidar/modules/planning/rangenet_change_detection_module.hpp index 27d2a649f..91a9f6f47 100644 --- a/main/src/vtr_lidar/include/vtr_lidar/modules/planning/rangenet_change_detection_module.hpp +++ b/main/src/vtr_lidar/include/vtr_lidar/modules/planning/rangenet_change_detection_module.hpp @@ -45,9 +45,15 @@ class RangeChangeNetModule : public nn::TorchModule { PTR_TYPEDEFS(Config); bool visualize = false; + int img_width = 1; + int img_height = 1; static ConstPtr fromROS(const rclcpp::Node::SharedPtr &node, const std::string ¶m_prefix); + + Shape getImgShape() const { + return {img_width, img_height}; + } }; RangeChangeNetModule( diff --git a/main/src/vtr_lidar/src/modules/planning/rangenet_change_detection_module.cpp b/main/src/vtr_lidar/src/modules/planning/rangenet_change_detection_module.cpp index 1ff8b991c..02026c071 100644 --- a/main/src/vtr_lidar/src/modules/planning/rangenet_change_detection_module.cpp +++ b/main/src/vtr_lidar/src/modules/planning/rangenet_change_detection_module.cpp @@ -18,6 +18,7 @@ */ #include #include "vtr_lidar/filters/range_image.hpp" +#include "vtr_common/timing/stopwatch.hpp" namespace vtr { @@ -53,6 +54,9 @@ auto RangeChangeNetModule::Config::fromROS(const rclcpp::Node::SharedPtr &node, // clang-format off config->visualize = node->declare_parameter(param_prefix + ".visualize", config->visualize); + config->img_width = node->declare_parameter(param_prefix + ".img_width", config->img_width); + config->img_height = node->declare_parameter(param_prefix + ".img_height", config->img_height); + // clang-format on return config; } @@ -84,9 +88,6 @@ void RangeChangeNetModule::run_(QueryCache &qdata0, OutputCache &, } auto& sub_map= *qdata.submap_loc; - const auto &T_lv_pm = sub_map.T_vertex_this().matrix(); - - //Need to convert frame! auto map_point_cloud = sub_map.point_cloud(); auto points_mat = map_point_cloud.getMatrixXfMap(4, PointWithInfo::size(), PointWithInfo::cartesian_offset()); @@ -104,20 +105,31 @@ void RangeChangeNetModule::run_(QueryCache &qdata0, OutputCache &, image_params.fov_down = -25 * M_PI / 180; image_params.crop_range = 8.0; - Eigen::MatrixXf scan_image = Eigen::MatrixXf::Zero(64, 1024); - Eigen::MatrixXf mask_image = Eigen::MatrixXf::Zero(64, 1024); + RowMatrixXf scan_image = Eigen::MatrixXf::Zero(64, 1024); + RowMatrixXf mask_image = Eigen::MatrixXf::Zero(64, 1024); Eigen::MatrixXi scan_idxs = Eigen::MatrixXi::Constant(64, 1024, -1); - Eigen::MatrixXf map_image = Eigen::MatrixXf::Zero(64, 1024); + RowMatrixXf map_image = Eigen::MatrixXf::Zero(64, 1024); + common::timing::Stopwatch timer; + timer.start(); generate_range_image(nn_point_cloud, scan_image, scan_idxs, image_params); generate_range_image(map_point_cloud, map_image, image_params); + timer.stop(); + CLOG(DEBUG, "lidar.range") << "Range image creation takes " << timer; + + using namespace torch::indexing; auto scan_tensor = torch::from_blob(scan_image.data(), {64, 1024}); auto map_tensor = torch::from_blob(map_image.data(), {64, 1024}); auto input = at::unsqueeze(at::stack({scan_tensor, map_tensor}), 0); + timer.reset(); auto tensor = evaluateModel(input, {1, 2, 64, 1024}); - auto mask = at::squeeze(at::argmax(tensor, 1), 0).to(torch::kUInt8); + auto mask = at::squeeze(at::argmax(tensor, 1), 0).to(at::kFloat); + timer.stop(); + CLOG(DEBUG, "lidar.range") << "Running inference takes " << timer; + + torch::from_blob(mask_image.data(), {64, 1024}) = mask; unproject_range_image(nn_point_cloud, mask_image, scan_idxs); diff --git a/rviz/lidar.rviz b/rviz/lidar.rviz index b1e9f281c..dbb5d2caf 100644 --- a/rviz/lidar.rviz +++ b/rviz/lidar.rviz @@ -35,7 +35,7 @@ Panels: Experimental: false Name: Time SyncMode: 0 - SyncSource: PointCloud2 + SyncSource: Map Visualization Manager: Class: "" Displays: @@ -69,52 +69,12 @@ Visualization Manager: Frame Timeout: 99999 Frames: All Enabled: false - base_footprint: - Value: true - base_footprint_bumper_part: - Value: true - base_link: - Value: true - chassis_link: - Value: true - front_axle_link: - Value: true - front_bumblebee: - Value: true - front_xb3: - Value: true - honeycomb: - Value: true - imu_link: - Value: true lidar: Value: false - loc vertex frame: - Value: true - loc vertex frame (offset): - Value: true - m600_base: - Value: true - microstrain: - Value: true - navsat_link: - Value: true odo vertex frame: Value: true - odom: - Value: true planning frame: Value: true - rear_antenna_link: - Value: true - rear_bumblebee: - Value: true - rear_sensor_base_link: - Value: true - rear_sensor_plate_link: - Value: true - rear_xb3: - Value: true robot: Value: true robot planning: @@ -123,10 +83,6 @@ Visualization Manager: Value: true robot planning (extrapolated) mpc: Value: true - sensor_anchor_link: - Value: true - velodyne: - Value: true world: Value: true world (offset): @@ -138,13 +94,10 @@ Visualization Manager: Show Names: true Tree: world: - loc vertex frame: - {} odo vertex frame: robot: lidar: - velodyne: - {} + {} planning frame: robot planning: {} @@ -153,8 +106,7 @@ Visualization Manager: robot planning (extrapolated) mpc: {} world (offset): - loc vertex frame (offset): - {} + {} Update Interval: 0 Value: true - Alpha: 1 @@ -1260,7 +1212,7 @@ Visualization Manager: Max Value: 1 Median window: 5 Min Value: 0 - Name: Image + Name: Diff Image Normalize Range: true Topic: Depth: 5 @@ -1274,7 +1226,7 @@ Visualization Manager: Max Value: 1 Median window: 5 Min Value: 0 - Name: Image + Name: Map Range Normalize Range: true Topic: Depth: 5 @@ -1288,7 +1240,7 @@ Visualization Manager: Max Value: 1 Median window: 5 Min Value: 0 - Name: Image + Name: Live Range Normalize Range: true Topic: Depth: 5 @@ -1304,7 +1256,7 @@ Visualization Manager: Min Value: -10 Value: true Axis: Z - Channel Name: flex13 + Channel Name: flex24 Class: rviz_default_plugins/PointCloud2 Color: 255; 255; 255 Color Transformer: Intensity @@ -1312,9 +1264,9 @@ Visualization Manager: Enabled: true Invert Rainbow: false Max Color: 255; 255; 255 - Max Intensity: 9.420014381408691 + Max Intensity: 0.9903810024261475 Min Color: 0; 0; 0 - Min Intensity: -4.211626052856445 + Min Intensity: -1 Name: Map Position Transformer: XYZ Selectable: true @@ -1378,7 +1330,7 @@ Visualization Manager: Views: Current: Class: rviz_default_plugins/Orbit - Distance: 34.462005615234375 + Distance: 48.05464172363281 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 @@ -1393,10 +1345,10 @@ Visualization Manager: Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: 1.1403958797454834 + Pitch: 0.9203958511352539 Target Frame: robot Value: Orbit (rviz_default_plugins) - Yaw: 3.3322339057922363 + Yaw: 3.1472339630126953 Saved: - Class: rviz_default_plugins/Orbit Distance: 111.39698028564453 @@ -1599,14 +1551,18 @@ Visualization Manager: Value: Orbit (rviz_default_plugins) Yaw: 2.295583963394165 Window Geometry: + Diff Image: + collapsed: false Displays: collapsed: false Height: 1016 Hide Left Dock: false Hide Right Dock: true - Image: + Live Range: + collapsed: false + Map Range: collapsed: false - QMainWindow State: 000000ff00000000fd0000000400000000000003de000003a2fc020000000afc0000003d000001a20000000000fffffffaffffffff0100000002fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730000000000ffffffff0000008c00fffffffb0000001200530065006c0065006300740069006f006e0000000000000002530000006c00fffffffb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b000001af000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d00610067006501000001f00000007a0000002800fffffffb0000000a0049006d00610067006501000002700000009d0000002800fffffffb0000000a0049006d0061006700650100000313000000ca0000002800ffffff00000001000001c0000003a2fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003b000003a2000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000005000000006efc0100000002fb0000000800540069006d00650000000000000005000000025300fffffffb0000000800540069006d0065010000000000000450000000000000000000000354000003a200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd0000000400000000000003de000003a2fc020000000afc0000003d000001a20000000000fffffffaffffffff0100000002fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730000000000ffffffff0000008c00fffffffb0000001200530065006c0065006300740069006f006e0000000000000002530000006c00fffffffb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b000001af000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb00000014004c006900760065002000520061006e0067006501000001f00000007a0000002800fffffffb00000014004400690066006600200049006d00610067006501000002700000009d0000002800fffffffb00000012004d00610070002000520061006e006700650100000313000000ca0000002800ffffff00000001000001c0000003a2fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003b000003a2000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000005000000006efc0100000002fb0000000800540069006d00650000000000000005000000025300fffffffb0000000800540069006d0065010000000000000450000000000000000000000354000003a200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Time: From 27ae21ec4119cd70b21ce7d0cb5c8be2b3f170b8 Mon Sep 17 00:00:00 2001 From: Alec Krawciw Date: Wed, 12 Jul 2023 13:13:06 -0400 Subject: [PATCH 37/77] Fix no information value from 0 to -1 --- config/hdl64_grizzly_learned.yaml | 2 +- .../src/modules/planning/rangenet_change_detection_module.cpp | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/config/hdl64_grizzly_learned.yaml b/config/hdl64_grizzly_learned.yaml index 00a3f4f7c..54f271655 100644 --- a/config/hdl64_grizzly_learned.yaml +++ b/config/hdl64_grizzly_learned.yaml @@ -98,7 +98,7 @@ - recall - icp # - safe_corridor - #- range_change + - range_change #- change_detection_sync - memory submap_translation_threshold: 1.5 diff --git a/main/src/vtr_lidar/src/modules/planning/rangenet_change_detection_module.cpp b/main/src/vtr_lidar/src/modules/planning/rangenet_change_detection_module.cpp index 02026c071..f36763d9d 100644 --- a/main/src/vtr_lidar/src/modules/planning/rangenet_change_detection_module.cpp +++ b/main/src/vtr_lidar/src/modules/planning/rangenet_change_detection_module.cpp @@ -105,10 +105,10 @@ void RangeChangeNetModule::run_(QueryCache &qdata0, OutputCache &, image_params.fov_down = -25 * M_PI / 180; image_params.crop_range = 8.0; - RowMatrixXf scan_image = Eigen::MatrixXf::Zero(64, 1024); + RowMatrixXf scan_image = Eigen::MatrixXf::Constant(64, 1024, -1); RowMatrixXf mask_image = Eigen::MatrixXf::Zero(64, 1024); Eigen::MatrixXi scan_idxs = Eigen::MatrixXi::Constant(64, 1024, -1); - RowMatrixXf map_image = Eigen::MatrixXf::Zero(64, 1024); + RowMatrixXf map_image = Eigen::MatrixXf::Constant(64, 1024, -1); common::timing::Stopwatch timer; timer.start(); From 9b69fb5d78ae920b800875501135cf286d764488 Mon Sep 17 00:00:00 2001 From: Alec Krawciw Date: Mon, 17 Jul 2023 10:15:06 -0400 Subject: [PATCH 38/77] Move range image into cpp file. Begin costmap creation. --- config/hdl64_grizzly_learned.yaml | 32 +++-- launch/online_velodyne_grizzly.launch.yaml | 2 +- main/src/vtr_lidar/CMakeLists.txt | 1 + .../include/vtr_lidar/filters/range_image.hpp | 68 +-------- .../rangenet_change_detection_module.hpp | 29 ++++ .../src/vtr_lidar/src/filters/range_image.cpp | 86 +++++++++++ .../rangenet_change_detection_module.cpp | 135 +++++++++++++++--- rviz/hdl_64.rviz | 61 +++----- 8 files changed, 272 insertions(+), 142 deletions(-) create mode 100644 main/src/vtr_lidar/src/filters/range_image.cpp diff --git a/config/hdl64_grizzly_learned.yaml b/config/hdl64_grizzly_learned.yaml index 54f271655..1a5c89c27 100644 --- a/config/hdl64_grizzly_learned.yaml +++ b/config/hdl64_grizzly_learned.yaml @@ -3,11 +3,10 @@ log_to_file: true log_debug: true log_enabled: - - lidar.terrain_assessment - navigation - navigation.graph_map_server #- navigation.command - - navigation.sensor_input + #- navigation.sensor_input # tactic - tactic - tactic.pipeline @@ -29,14 +28,14 @@ # pipeline specific #- lidar.pipeline - lidar.velodyne_converter_v2 - #- lidar.preprocessing + - lidar.preprocessing - lidar.odometry_icp - lidar.range # "lidar.odometry_map_maintenance", #- lidar.vertex_test - lidar.range_change # "lidar.localization_map_recall", - # "lidar.localization_icp", + - lidar.localization_icp # "lidar.intra_exp_merging", # "lidar.dynamic_detection", # "lidar.inter_exp_merging", @@ -103,31 +102,31 @@ - memory submap_translation_threshold: 1.5 submap_rotation_threshold: 30.0 - save_nn_point_cloud: true + save_nn_point_cloud: false preprocessing: conversion: type: lidar.velodyne_converter_v2 visualize: true estimate_time: true - downsample_ratio: 3 + downsample_ratio: 2 filtering: type: lidar.preprocessing num_threads: 8 - crop_range: 40.0 + crop_range: 15.0 frame_voxel_size: 0.2 # grid subsampling voxel size #velodyne lidar has 1/3 degree upper block and 1/2 degree lower block - vertical_angle_res: 0.00872664626 # vertical angle resolution in radians (0.5deg), - polar_r_scale: 2.0 # polar_r_scale x vertical_angle_res = nearest neighbor search radius for normal computation + vertical_angle_res: 0.00699 # vertical angle resolution in radians (0.4deg), + polar_r_scale: 3.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 #horizontal resolution 0.1728degree 0.1728/(0.5) - h_scale: 0.175 # 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 + h_scale: 0.0864 # 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 + min_norm_score1: 0.85 # 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 @@ -237,8 +236,11 @@ type: torch.range_change use_gpu: true abs_filepath: false - filepath: exported_range_model.pt + filepath: exported_model_ground_8m.pt visualize: true + + influence_distance: 0.5 # was 0.5 Jordy # Note that the total distance where grid cells have values > 0 is min dist + influence dist, not influence dist! + minimum_distance: 0.8 # was 0.3 Jordy memory: type: graph_mem_manager vertex_life_span: 5 @@ -264,7 +266,7 @@ run_async: true visualize: true path_planning: - type: cbit.lidar # teb.lidar for old teb version, cbit for obstacle free path tracker, cbit.lidar for obstacle avoidance version + type: cbit # teb.lidar for old teb version, cbit for obstacle free path tracker, cbit.lidar for obstacle avoidance version control_period: 100 # ms cbit: @@ -309,8 +311,8 @@ # Cost Function Covariance Matrices pose_error_cov: [100.0, 100.0, 1000.0, 1000.0, 1000.0, 200.0] - vel_error_cov: [300.0, 300.0] # was [0.1 2.0] # No longer using this cost term - acc_error_cov: [100.0, 100.0] + vel_error_cov: [25.0, 10.0] # was [0.1 2.0] # No longer using this cost term + acc_error_cov: [10.0, 10.0] kin_error_cov: [0.001, 0.001, 0.001, 0.001, 0.001, 0.001] lat_error_cov: [20.0] diff --git a/launch/online_velodyne_grizzly.launch.yaml b/launch/online_velodyne_grizzly.launch.yaml index 1ce7aaa33..636c60e17 100644 --- a/launch/online_velodyne_grizzly.launch.yaml +++ b/launch/online_velodyne_grizzly.launch.yaml @@ -40,6 +40,6 @@ windows: shell_command_before: - source ${VTRSRC}/main/install/setup.bash panes: - #- ros2 run rviz2 rviz2 -d ${VTRSRC}/rviz/lidar.rviz + #- ros2 run rviz2 rviz2 -d ${VTRSRC}/rviz/hdl_64.rviz - ros2 launch foxglove_bridge foxglove_bridge_launch.xml # - ros2 run rqt_reconfigure rqt_reconfigure diff --git a/main/src/vtr_lidar/CMakeLists.txt b/main/src/vtr_lidar/CMakeLists.txt index 014bd4efa..df472b49b 100644 --- a/main/src/vtr_lidar/CMakeLists.txt +++ b/main/src/vtr_lidar/CMakeLists.txt @@ -39,6 +39,7 @@ include_directories(PUBLIC # components file(GLOB_RECURSE COMPONENTS_SRC src/data_types/*.cpp + src/filters/*.cpp ) add_library(${PROJECT_NAME}_components ${COMPONENTS_SRC}) ament_target_dependencies(${PROJECT_NAME}_components diff --git a/main/src/vtr_lidar/include/vtr_lidar/filters/range_image.hpp b/main/src/vtr_lidar/include/vtr_lidar/filters/range_image.hpp index 348cc389e..6786e1789 100644 --- a/main/src/vtr_lidar/include/vtr_lidar/filters/range_image.hpp +++ b/main/src/vtr_lidar/include/vtr_lidar/filters/range_image.hpp @@ -36,72 +36,10 @@ struct RangeImageParams { double getFOV() { return abs(fov_up - fov_down);} }; +void generate_range_image(const pcl::PointCloud& point_cloud, RowMatrixXf& range_image, RangeImageParams params); void generate_range_image(const pcl::PointCloud& point_cloud, RowMatrixXf& range_image, Eigen::MatrixXi& idx_image, RangeImageParams params); - -void generate_range_image(const pcl::PointCloud& point_cloud, RowMatrixXf& range_image, RangeImageParams params) { - Eigen::MatrixXi temp_idx = Eigen::MatrixXi::Constant(range_image.rows(), range_image.cols(), -1); - generate_range_image(point_cloud, range_image, temp_idx, params); -} - -/// \todo Parallelize -void generate_range_image(const pcl::PointCloud& point_cloud, RowMatrixXf& range_image, Eigen::MatrixXi& idx_image, RangeImageParams params) { - - for (size_t i = 0; i < point_cloud.size(); ++i) { - auto& point = point_cloud[i]; - if (point.rho > params.crop_range) - continue; - double proj_x = (remainder(point.phi, 2 * M_PI) / 2 / M_PI + 0.5); - double proj_y = 1.0 - (M_PI/2 - point.theta + abs(params.fov_down)) / params.getFOV(); - - - proj_x = (proj_x < 0.0) ? 0.0 : proj_x; - proj_x = (proj_x > 1.0) ? 1.0 : proj_x; - - proj_y = (proj_y < 0.0) ? 0.0 : proj_y; - proj_y = (proj_y > 1.0) ? 1.0 : proj_y; - - proj_x *= range_image.cols() - 1; - proj_y *= range_image.rows() - 1; - - range_image((int)proj_y, (int)proj_x) = point.rho; - idx_image((int)proj_y, (int)proj_x) = i; - } - // CLOG(DEBUG, "lidar.range") << idx_image; - -} - -//Definitely parallelize, no reverse duplication -void unproject_range_image(pcl::PointCloud& point_cloud, const RowMatrixXf& data_image, const Eigen::MatrixXi& idx_image) { - - for (int i = 0; i < idx_image.rows(); i++){ - //#pragma omp parallel for schedule(dynamic, 10) num_threads(num_threads) - for (int j = 0; j < idx_image.cols(); j++) { - int idx = idx_image(i, j); - if (idx >= 0) - point_cloud[idx].flex24 = data_image(i, j) + 1; - } - } - -} - -sensor_msgs::msg::Image range_to_image(const RowMatrixXf& data_image) { - float max_range = data_image.maxCoeff(); - - sensor_msgs::msg::Image ros_im; - ros_im.width = data_image.cols(); - ros_im.height = data_image.rows(); - ros_im.encoding = "mono8"; - ros_im.step = data_image.cols(); - size_t size = data_image.size()*sizeof(uint8_t); - ros_im.data.resize(size); - - RowMatrixXuI8 scaled_image = (data_image / max_range * 255).cast(); - memcpy(&ros_im.data[0], scaled_image.data(), size); - // Eigen::Matrix scaled_image = (data_image / max_range * 255).cast(); - // memcpy(&ros_im.data[0], scaled_image.data(), size); - - return ros_im; -} +void unproject_range_image(pcl::PointCloud& point_cloud, const RowMatrixXf& data_image, const Eigen::MatrixXi& idx_image); +sensor_msgs::msg::Image range_to_image(const RowMatrixXf& data_image); diff --git a/main/src/vtr_lidar/include/vtr_lidar/modules/planning/rangenet_change_detection_module.hpp b/main/src/vtr_lidar/include/vtr_lidar/modules/planning/rangenet_change_detection_module.hpp index 91a9f6f47..5ddacfc56 100644 --- a/main/src/vtr_lidar/include/vtr_lidar/modules/planning/rangenet_change_detection_module.hpp +++ b/main/src/vtr_lidar/include/vtr_lidar/modules/planning/rangenet_change_detection_module.hpp @@ -23,9 +23,13 @@ #include #include "vtr_lidar/cache.hpp" #include +#include #include "sensor_msgs/msg/image.hpp" +#include "vtr_lidar/filters/range_image.hpp" + + namespace vtr { namespace lidar { @@ -37,6 +41,8 @@ using PointCloudMsg = sensor_msgs::msg::PointCloud2; class RangeChangeNetModule : public nn::TorchModule { public: PTR_TYPEDEFS(RangeChangeNetModule); + using OccupancyGridMsg = nav_msgs::msg::OccupancyGrid; + /** \brief Static module identifier. */ static constexpr auto static_name = "torch.range_change"; @@ -48,12 +54,31 @@ class RangeChangeNetModule : public nn::TorchModule { int img_width = 1; int img_height = 1; + float fov_up = 0; + float fov_down = 0; + float range_crop = 0; + + unsigned int costmap_history_size = 10; + float resolution = 1.0; + float size_x = 20.0; + float size_y = 20.0; + float influence_distance = 1.0; + float minimum_distance = 0.5; + static ConstPtr fromROS(const rclcpp::Node::SharedPtr &node, const std::string ¶m_prefix); Shape getImgShape() const { return {img_width, img_height}; } + + RangeImageParams getRangeImgParams() const { + RangeImageParams image_params; + image_params.fov_up = fov_up * M_PI / 180; + image_params.fov_down = fov_down * M_PI / 180; + image_params.crop_range = range_crop; + return image_params; + } }; RangeChangeNetModule( @@ -74,8 +99,12 @@ class RangeChangeNetModule : public nn::TorchModule { rclcpp::Publisher::SharedPtr map_range_pub_; rclcpp::Publisher::SharedPtr diffpcd_pub_; rclcpp::Publisher::SharedPtr mappcd_pub_; + rclcpp::Publisher::SharedPtr costmap_pub_; bool pub_init = false; + std::list, float>> costmap_history; + + VTR_REGISTER_MODULE_DEC_TYPE(RangeChangeNetModule); }; diff --git a/main/src/vtr_lidar/src/filters/range_image.cpp b/main/src/vtr_lidar/src/filters/range_image.cpp new file mode 100644 index 000000000..a44e18d2d --- /dev/null +++ b/main/src/vtr_lidar/src/filters/range_image.cpp @@ -0,0 +1,86 @@ +// Copyright 2023, 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 range_image.cpp + * \author Alec Krawciw, Autonomous Space Robotics Lab (ASRL) + */ +#include "vtr_lidar/filters/range_image.hpp" + +namespace vtr { +namespace lidar { + +void generate_range_image(const pcl::PointCloud& point_cloud, RowMatrixXf& range_image, RangeImageParams params) { + Eigen::MatrixXi temp_idx = Eigen::MatrixXi::Constant(range_image.rows(), range_image.cols(), -1); + generate_range_image(point_cloud, range_image, temp_idx, params); +} + +/// \todo Parallelize +void generate_range_image(const pcl::PointCloud& point_cloud, RowMatrixXf& range_image, Eigen::MatrixXi& idx_image, RangeImageParams params) { + + for (size_t i = 0; i < point_cloud.size(); ++i) { + auto& point = point_cloud[i]; + if (point.rho > params.crop_range) + continue; + double proj_x = (remainder(point.phi, 2 * M_PI) / 2 / M_PI + 0.5); + double proj_y = 1.0 - (M_PI/2 - point.theta + abs(params.fov_down)) / params.getFOV(); + + + proj_x = (proj_x < 0.0) ? 0.0 : proj_x; + proj_x = (proj_x > 1.0) ? 1.0 : proj_x; + + proj_y = (proj_y < 0.0) ? 0.0 : proj_y; + proj_y = (proj_y > 1.0) ? 1.0 : proj_y; + + proj_x *= range_image.cols() - 1; + proj_y *= range_image.rows() - 1; + + range_image((int)proj_y, (int)proj_x) = point.rho; + idx_image((int)proj_y, (int)proj_x) = i; + } +} + +//Definitely parallelize, no reverse duplication +void unproject_range_image(pcl::PointCloud& point_cloud, const RowMatrixXf& data_image, const Eigen::MatrixXi& idx_image) { + + for (int i = 0; i < idx_image.rows(); i++){ + //#pragma omp parallel for schedule(dynamic, 10) num_threads(num_threads) + for (int j = 0; j < idx_image.cols(); j++) { + int idx = idx_image(i, j); + if (idx >= 0) + point_cloud[idx].flex24 = data_image(i, j) + 1; + } + } + +} + +sensor_msgs::msg::Image range_to_image(const RowMatrixXf& data_image) { + float max_range = data_image.maxCoeff(); + + sensor_msgs::msg::Image ros_im; + ros_im.width = data_image.cols(); + ros_im.height = data_image.rows(); + ros_im.encoding = "mono8"; + ros_im.step = data_image.cols(); + size_t size = data_image.size()*sizeof(uint8_t); + ros_im.data.resize(size); + + RowMatrixXuI8 scaled_image = (data_image / max_range * 255).cast(); + memcpy(&ros_im.data[0], scaled_image.data(), size); + + return ros_im; +} + +} //namespace lidar +} //namespace vtr \ No newline at end of file diff --git a/main/src/vtr_lidar/src/modules/planning/rangenet_change_detection_module.cpp b/main/src/vtr_lidar/src/modules/planning/rangenet_change_detection_module.cpp index f36763d9d..22767b155 100644 --- a/main/src/vtr_lidar/src/modules/planning/rangenet_change_detection_module.cpp +++ b/main/src/vtr_lidar/src/modules/planning/rangenet_change_detection_module.cpp @@ -17,8 +17,10 @@ * \author Alec Krawciw, Autonomous Space Robotics Lab (ASRL) */ #include -#include "vtr_lidar/filters/range_image.hpp" #include "vtr_common/timing/stopwatch.hpp" +#include "vtr_lidar/utils/nanoflann_utils.hpp" +#include "vtr_lidar/data_types/costmap.hpp" + namespace vtr { @@ -26,20 +28,57 @@ namespace lidar { namespace { void velodyneCart2Pol(pcl::PointCloud &point_cloud) { - for (size_t i = 0; i < point_cloud.size(); i++) { - auto &p = point_cloud[i]; - auto &pm1 = i > 0 ? point_cloud[i - 1] : point_cloud[i]; - - p.rho = sqrt(p.x * p.x + p.y * p.y + p.z * p.z); - p.theta = atan2(sqrt(p.x * p.x + p.y * p.y), p.z); - p.phi = atan2(p.y, p.x); // + M_PI / 2; - - if (i > 0 && (p.phi - pm1.phi) > 1.5 * M_PI) - p.phi -= 2 * M_PI; - else if (i > 0 && (p.phi - pm1.phi) < -1.5 * M_PI) - p.phi += 2 * M_PI; + for (size_t i = 0; i < point_cloud.size(); i++) { + auto &p = point_cloud[i]; + auto &pm1 = i > 0 ? point_cloud[i - 1] : point_cloud[i]; + + p.rho = sqrt(p.x * p.x + p.y * p.y + p.z * p.z); + p.theta = atan2(sqrt(p.x * p.x + p.y * p.y), p.z); + p.phi = atan2(p.y, p.x); // + M_PI / 2; + + if (i > 0 && (p.phi - pm1.phi) > 1.5 * M_PI) + p.phi -= 2 * M_PI; + else if (i > 0 && (p.phi - pm1.phi) < -1.5 * M_PI) + p.phi += 2 * M_PI; + } } -} + + template + class DetectChangeOp { + public: + DetectChangeOp(const pcl::PointCloud &points, const float &d0, + const float &d1) + : d0_(d0), d1_(d1), adapter_(points) { + /// create kd-tree of the point cloud for radius search + kdtree_ = std::make_unique>(2, adapter_, + KDTreeParams(/* max leaf */ 10)); + kdtree_->buildIndex(); + // search params setup + search_params_.sorted = false; + } + + void operator()(const Eigen::Vector2f &q, float &v) const { + size_t ind; + float dist; + KDTreeResultSet result_set(1); + result_set.init(&ind, &dist); + kdtree_->findNeighbors(result_set, q.data(), search_params_); + + // update the value of v + dist = std::sqrt(dist); // convert to distance + v = std::max(1 - (dist - d1_) / d0_, 0.0f); + v = std::min(v, 0.9f); // 1 is bad for visualization + } + + private: + const float d0_; + const float d1_; + + KDTreeSearchParams search_params_; + NanoFLANNAdapter adapter_; + std::unique_ptr> kdtree_; + }; + } using namespace tactic; @@ -54,21 +93,37 @@ auto RangeChangeNetModule::Config::fromROS(const rclcpp::Node::SharedPtr &node, // clang-format off config->visualize = node->declare_parameter(param_prefix + ".visualize", config->visualize); + //Range Image details config->img_width = node->declare_parameter(param_prefix + ".img_width", config->img_width); config->img_height = node->declare_parameter(param_prefix + ".img_height", config->img_height); - + config->fov_up = node->declare_parameter(param_prefix + ".fov_up", config->fov_up); + config->fov_down = node->declare_parameter(param_prefix + ".fov_down", config->fov_down); + config->range_crop = node->declare_parameter(param_prefix + ".range_crop", config->range_crop); + + // cost map + config->costmap_history_size = (unsigned) 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); + config->influence_distance = node->declare_parameter(param_prefix + ".influence_distance", config->influence_distance); + config->minimum_distance = node->declare_parameter(param_prefix + ".minimum_distance", config->minimum_distance); + + // clang-format on return config; } -void RangeChangeNetModule::run_(QueryCache &qdata0, OutputCache &, +void RangeChangeNetModule::run_(QueryCache &qdata0, OutputCache &output0, const Graph::Ptr &, const TaskExecutor::Ptr &) { auto &qdata = dynamic_cast(qdata0); + auto &output = dynamic_cast(output0); auto nn_point_cloud = *qdata.nn_point_cloud; const auto &T_s_r = *qdata.T_s_r; + const auto &vid_loc = *qdata.vid_loc; + const auto &sid_loc = *qdata.sid_loc; const auto &T_r_v_loc = *qdata.T_r_v_loc; const auto &T_v_m_loc = *qdata.T_v_m_loc; @@ -85,15 +140,21 @@ void RangeChangeNetModule::run_(QueryCache &qdata0, OutputCache &, map_range_pub_ = qdata.node->create_publisher("map_range_image", 5); diffpcd_pub_ = qdata.node->create_publisher("detection_cloud", 5); mappcd_pub_ = qdata.node->create_publisher("transformed_map", 5); + costmap_pub_ = qdata.node->create_publisher("change_detection_costmap", 5); } auto& sub_map= *qdata.submap_loc; auto map_point_cloud = sub_map.point_cloud(); + auto live_points_mat = nn_point_cloud.getMatrixXfMap(4, PointWithInfo::size(), PointWithInfo::cartesian_offset()); auto points_mat = map_point_cloud.getMatrixXfMap(4, PointWithInfo::size(), PointWithInfo::cartesian_offset()); CLOG(DEBUG, "lidar.range") << "Before: (" << map_point_cloud[10].x << ", " << map_point_cloud[10].y << ", "<< map_point_cloud[10].z <<")"; - const auto T_s_m = (T_s_r * T_r_v_loc * T_v_m_loc).matrix(); - points_mat = T_s_m.cast() * points_mat; + const auto T_r_s = T_s_r.inverse().matrix(); + live_points_mat = T_r_s.cast() * live_points_mat; + + const auto T_r_m = (T_r_v_loc * T_v_m_loc).matrix(); + points_mat = T_r_m.cast() * points_mat; + velodyneCart2Pol(nn_point_cloud); velodyneCart2Pol(map_point_cloud); @@ -101,8 +162,8 @@ void RangeChangeNetModule::run_(QueryCache &qdata0, OutputCache &, RangeImageParams image_params; - image_params.fov_up = 3 * M_PI / 180; - image_params.fov_down = -25 * M_PI / 180; + image_params.fov_up = 20 * M_PI / 180; + image_params.fov_down = -5 * M_PI / 180; image_params.crop_range = 8.0; RowMatrixXf scan_image = Eigen::MatrixXf::Constant(64, 1024, -1); @@ -134,10 +195,35 @@ void RangeChangeNetModule::run_(QueryCache &qdata0, OutputCache &, unproject_range_image(nn_point_cloud, mask_image, scan_idxs); + // project to 2d and construct the grid map + const auto costmap = std::make_shared( + config_->resolution, config_->size_x, config_->size_y); + + // filter out non-obstacle points + std::vector indices; + indices.reserve(nn_point_cloud.size()); + for (size_t i = 0; i < nn_point_cloud.size(); ++i) { + if (nn_point_cloud[i].flex24 > 1.5) indices.emplace_back(i); + } + pcl::PointCloud obstacle_points(nn_point_cloud, indices); + + // update cost map based on change detection result + DetectChangeOp detect_change_op( + obstacle_points, config_->influence_distance, config_->minimum_distance); + costmap->update(detect_change_op); + // add transform to the localization vertex + costmap->T_vertex_this() = tactic::EdgeTransform(true); + costmap->vertex_id() = vid_loc; + costmap->vertex_sid() = sid_loc; + + auto change_detection_costmap_ref = output.change_detection_costmap.locked(); + auto &change_detection_costmap = change_detection_costmap_ref.get(); + change_detection_costmap = costmap; + /// publish the transformed pointcloud if (config_->visualize) { PointCloudMsg filter_msg; - pcl::toROSMsg(nn_point_cloud, filter_msg); + pcl::toROSMsg(obstacle_points, filter_msg); filter_msg.header.frame_id = "odo vertex frame"; // pointcloud_msg.header.stamp = rclcpp::Time(*qdata.stamp); diffpcd_pub_->publish(filter_msg); @@ -148,6 +234,13 @@ void RangeChangeNetModule::run_(QueryCache &qdata0, OutputCache &, // pointcloud_msg.header.stamp = rclcpp::Time(*qdata.stamp); mappcd_pub_->publish(map_msg); + // publish the occupancy grid + auto costmap_msg = costmap->toCostMapMsg(); + costmap_msg.header.frame_id = "loc vertex frame"; + // costmap_msg.header.stamp = rclcpp::Time(*qdata.stamp); + CLOG(DEBUG, "lidar.range") << "Publishing costmap"; + costmap_pub_->publish(costmap_msg); + mask_pub_->publish(range_to_image(mask_image)); live_range_pub_->publish(range_to_image(scan_image)); diff --git a/rviz/hdl_64.rviz b/rviz/hdl_64.rviz index 49ccad31b..b84661769 100644 --- a/rviz/hdl_64.rviz +++ b/rviz/hdl_64.rviz @@ -8,18 +8,16 @@ Panels: - /TF1 - /TF1/Frames1 - /TF1/Tree1 - - /raw scan1 - /filtered scan1 - /odometry path1/Shape1 - /curr map loc1/Topic1 - /planned path1/Offset1 - /planned path (poses)1/Topic1 - - /change detection scan1 - - /chang detection cost pcd1 - /global path1/Offset1 + - /obstacle cost map1 - /PointCloud21 Splitter Ratio: 0.4507042169570923 - Tree Height: 871 + Tree Height: 865 - Class: rviz_common/Selection Name: Selection - Class: rviz_common/Tool Properties @@ -37,7 +35,7 @@ Panels: Experimental: false Name: Time SyncMode: 0 - SyncSource: Map + SyncSource: "" Visualization Manager: Class: "" Displays: @@ -71,33 +69,29 @@ Visualization Manager: Frame Timeout: 99999 Frames: All Enabled: false - base_link: + base_footprint: + Value: true + base_footprint_bumper_part: Value: true - chassis_link: + base_link: Value: true - front_axle_link: + front_bumblebee: Value: true - front_left_wheel_link: + front_xb3: Value: true - front_right_wheel_link: + honeycomb: Value: true imu_link: Value: true lidar: Value: false - navsat_link: - Value: true - odom: - Value: true - rear_antenna_link: + m600_base: Value: true - rear_left_wheel_link: + microstrain: Value: true - rear_right_wheel_link: + rear_bumblebee: Value: true - rear_sensor_base_link: - Value: true - rear_sensor_plate_link: + rear_xb3: Value: true robot: Value: true @@ -116,17 +110,6 @@ Visualization Manager: Show Names: true Tree: world: - odo vertex frame: - robot: - lidar: - {} - planning frame: - robot planning: - {} - robot planning (extrapolated): - {} - robot planning (extrapolated) mpc: - {} world (offset): {} Update Interval: 0 @@ -143,7 +126,7 @@ Visualization Manager: Color: 255; 255; 255 Color Transformer: Intensity Decay Time: 0 - Enabled: true + Enabled: false Invert Rainbow: false Max Color: 0; 255; 0 Max Intensity: 0 @@ -164,7 +147,7 @@ Visualization Manager: Value: /vtr/raw_point_cloud Use Fixed Frame: true Use rainbow: true - Value: true + Value: false - Alpha: 1 Autocompute Intensity Bounds: true Autocompute Value Bounds: @@ -177,7 +160,7 @@ Visualization Manager: Color: 0; 255; 0 Color Transformer: FlatColor Decay Time: 0 - Enabled: false + Enabled: true Invert Rainbow: false Max Color: 255; 255; 255 Max Intensity: 999999 @@ -198,7 +181,7 @@ Visualization Manager: Value: /vtr/filtered_point_cloud Use Fixed Frame: true Use rainbow: true - Value: false + Value: true - Angle Tolerance: 0.10000000149011612 Class: rviz_default_plugins/Odometry Covariance: @@ -559,7 +542,7 @@ Visualization Manager: Color: 255; 255; 255 Color Transformer: Intensity Decay Time: 0 - Enabled: true + Enabled: false Invert Rainbow: false Max Color: 255; 255; 255 Max Intensity: 0.8999999761581421 @@ -580,7 +563,7 @@ Visualization Manager: Value: /vtr/change_detection_diff Use Fixed Frame: true Use rainbow: true - Value: true + Value: false - Alpha: 1 Autocompute Intensity Bounds: true Autocompute Value Bounds: @@ -1497,14 +1480,12 @@ Visualization Manager: Value: Orbit (rviz_default_plugins) Yaw: 2.295583963394165 Window Geometry: - Diff Image: - collapsed: false Displays: collapsed: false Height: 1016 Hide Left Dock: false Hide Right Dock: false - QMainWindow State: 000000ff00000000fd000000040000000000000281000003a2fc0200000007fc0000003d000001a20000000000fffffffaffffffff0100000002fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730000000000ffffffff0000008c00fffffffb0000001200530065006c0065006300740069006f006e0000000000000002530000006c00fffffffb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b000003a2000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c0000026100000001000001c0000003a2fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003b000003a2000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000005000000006efc0100000002fb0000000800540069006d00650000000000000005000000025300fffffffb0000000800540069006d00650100000000000004500000000000000000000002eb000003a200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd0000000400000000000002810000039efc0200000007fc0000003d000001a20000000000fffffffaffffffff0100000002fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730000000000ffffffff0000009d00fffffffb0000001200530065006c0065006300740069006f006e0000000000000002530000007400fffffffb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d0000039e000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c0000026100000001000001c00000039efc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d0000039e000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000005000000006efc0100000002fb0000000800540069006d0065000000000000000500000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000002eb0000039e00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Time: From 7504ccd1c5f31260015c90326f49baba8c3d7706 Mon Sep 17 00:00:00 2001 From: Alec Krawciw Date: Sun, 23 Jul 2023 10:53:45 -0400 Subject: [PATCH 39/77] Continue changing costmap creation. --- config/hdl64_grizzly_learned.yaml | 4 +- launch/offline_velodyne_grizzly.launch.yaml | 2 +- .../rangenet_change_detection_module.cpp | 75 +++++--------- rviz/hdl_64.rviz | 98 ++++++++++++++++--- 4 files changed, 108 insertions(+), 71 deletions(-) diff --git a/config/hdl64_grizzly_learned.yaml b/config/hdl64_grizzly_learned.yaml index 1a5c89c27..82dbe678f 100644 --- a/config/hdl64_grizzly_learned.yaml +++ b/config/hdl64_grizzly_learned.yaml @@ -18,7 +18,7 @@ # "path_planning.teb", # "path_planning.cbit", # "path_planning.cbit_planner", - # "mpc.cbit", + - mpc.cbit # "mpc_debug.cbit", # mission planner #- mission.server @@ -301,7 +301,7 @@ # Controller Params horizon_steps: 20 horizon_step_size: 0.3 - forward_vel: 0.5 + forward_vel: 0.75 max_lin_vel: 1.0 max_ang_vel: 0.75 robot_linear_velocity_scale: 1.1 # Used to scale the output twist linear velocity messages by some constant factor to compensate internal low level control miscalibration diff --git a/launch/offline_velodyne_grizzly.launch.yaml b/launch/offline_velodyne_grizzly.launch.yaml index 0c5bb0033..4a560fa48 100644 --- a/launch/offline_velodyne_grizzly.launch.yaml +++ b/launch/offline_velodyne_grizzly.launch.yaml @@ -38,7 +38,7 @@ windows: shell_command_before: - source ${VTRSRC}/main/install/setup.bash panes: - #- ros2 run rviz2 rviz2 -d ${VTRSRC}/rviz/lidar.rviz + - ros2 run rviz2 rviz2 -d ${VTRSRC}/rviz/hdl_64.rviz - ros2 launch foxglove_bridge foxglove_bridge_launch.xml # - ros2 run rqt_reconfigure rqt_reconfigure diff --git a/main/src/vtr_lidar/src/modules/planning/rangenet_change_detection_module.cpp b/main/src/vtr_lidar/src/modules/planning/rangenet_change_detection_module.cpp index 22767b155..3c82245a8 100644 --- a/main/src/vtr_lidar/src/modules/planning/rangenet_change_detection_module.cpp +++ b/main/src/vtr_lidar/src/modules/planning/rangenet_change_detection_module.cpp @@ -20,6 +20,7 @@ #include "vtr_common/timing/stopwatch.hpp" #include "vtr_lidar/utils/nanoflann_utils.hpp" #include "vtr_lidar/data_types/costmap.hpp" +#include @@ -43,42 +44,10 @@ namespace { } } - template - class DetectChangeOp { - public: - DetectChangeOp(const pcl::PointCloud &points, const float &d0, - const float &d1) - : d0_(d0), d1_(d1), adapter_(points) { - /// create kd-tree of the point cloud for radius search - kdtree_ = std::make_unique>(2, adapter_, - KDTreeParams(/* max leaf */ 10)); - kdtree_->buildIndex(); - // search params setup - search_params_.sorted = false; - } - - void operator()(const Eigen::Vector2f &q, float &v) const { - size_t ind; - float dist; - KDTreeResultSet result_set(1); - result_set.init(&ind, &dist); - kdtree_->findNeighbors(result_set, q.data(), search_params_); - - // update the value of v - dist = std::sqrt(dist); // convert to distance - v = std::max(1 - (dist - d1_) / d0_, 0.0f); - v = std::min(v, 0.9f); // 1 is bad for visualization - } - - private: - const float d0_; - const float d1_; - - KDTreeSearchParams search_params_; - NanoFLANNAdapter adapter_; - std::unique_ptr> kdtree_; - }; + costmap::PixKey pointToKey(PointWithInfo &p) { + return {std::lround(p.x), std::lround(p.y)}; + } } using namespace tactic; @@ -139,7 +108,6 @@ void RangeChangeNetModule::run_(QueryCache &qdata0, OutputCache &output0, live_range_pub_ = qdata.node->create_publisher("live_range_image", 5); map_range_pub_ = qdata.node->create_publisher("map_range_image", 5); diffpcd_pub_ = qdata.node->create_publisher("detection_cloud", 5); - mappcd_pub_ = qdata.node->create_publisher("transformed_map", 5); costmap_pub_ = qdata.node->create_publisher("change_detection_costmap", 5); } @@ -189,7 +157,7 @@ void RangeChangeNetModule::run_(QueryCache &qdata0, OutputCache &output0, auto mask = at::squeeze(at::argmax(tensor, 1), 0).to(at::kFloat); timer.stop(); CLOG(DEBUG, "lidar.range") << "Running inference takes " << timer; - + timer.reset(); torch::from_blob(mask_image.data(), {64, 1024}) = mask; @@ -206,13 +174,20 @@ void RangeChangeNetModule::run_(QueryCache &qdata0, OutputCache &output0, if (nn_point_cloud[i].flex24 > 1.5) indices.emplace_back(i); } pcl::PointCloud obstacle_points(nn_point_cloud, indices); + std::unordered_map values; + for (size_t i = 0; i < obstacle_points.size(); ++i) { + const auto pix_coord = pointToKey(obstacle_points[i]); + if (values.find(pix_coord) != values.end()) { + values[pix_coord] += 0.05; + } else { + values[pix_coord] = 0.05; + } + } - // update cost map based on change detection result - DetectChangeOp detect_change_op( - obstacle_points, config_->influence_distance, config_->minimum_distance); - costmap->update(detect_change_op); + + costmap->update(values); // add transform to the localization vertex - costmap->T_vertex_this() = tactic::EdgeTransform(true); + costmap->T_vertex_this() = T_r_v_loc; costmap->vertex_id() = vid_loc; costmap->vertex_sid() = sid_loc; @@ -222,22 +197,18 @@ void RangeChangeNetModule::run_(QueryCache &qdata0, OutputCache &output0, /// publish the transformed pointcloud if (config_->visualize) { + CLOG(DEBUG, "lidar.range") << "Getting to vis took " << timer; + PointCloudMsg filter_msg; - pcl::toROSMsg(obstacle_points, filter_msg); + pcl::toROSMsg(nn_point_cloud, filter_msg); filter_msg.header.frame_id = "odo vertex frame"; - // pointcloud_msg.header.stamp = rclcpp::Time(*qdata.stamp); + filter_msg.header.stamp = rclcpp::Time(*qdata.stamp); diffpcd_pub_->publish(filter_msg); - PointCloudMsg map_msg; - pcl::toROSMsg(map_point_cloud, map_msg); - map_msg.header.frame_id = "odo vertex frame"; - // pointcloud_msg.header.stamp = rclcpp::Time(*qdata.stamp); - mappcd_pub_->publish(map_msg); - // publish the occupancy grid auto costmap_msg = costmap->toCostMapMsg(); - costmap_msg.header.frame_id = "loc vertex frame"; - // costmap_msg.header.stamp = rclcpp::Time(*qdata.stamp); + costmap_msg.header.frame_id = "planning_frame"; + costmap_msg.header.stamp = rclcpp::Time(*qdata.stamp); CLOG(DEBUG, "lidar.range") << "Publishing costmap"; costmap_pub_->publish(costmap_msg); diff --git a/rviz/hdl_64.rviz b/rviz/hdl_64.rviz index b84661769..5983e596b 100644 --- a/rviz/hdl_64.rviz +++ b/rviz/hdl_64.rviz @@ -5,7 +5,6 @@ Panels: Property Tree Widget: Expanded: - /Global Options1 - - /TF1 - /TF1/Frames1 - /TF1/Tree1 - /filtered scan1 @@ -13,11 +12,16 @@ Panels: - /curr map loc1/Topic1 - /planned path1/Offset1 - /planned path (poses)1/Topic1 + - /change detection scan1 + - /change detection scan1/Status1 - /global path1/Offset1 - /obstacle cost map1 + - /obstacle cost map1/Status1 - /PointCloud21 + - /PointCloud22 + - /PointCloud22/Status1 Splitter Ratio: 0.4507042169570923 - Tree Height: 865 + Tree Height: 871 - Class: rviz_common/Selection Name: Selection - Class: rviz_common/Tool Properties @@ -35,7 +39,7 @@ Panels: Experimental: false Name: Time SyncMode: 0 - SyncSource: "" + SyncSource: curr map loc Visualization Manager: Class: "" Displays: @@ -85,16 +89,30 @@ Visualization Manager: Value: true lidar: Value: false + loc vertex frame: + Value: true + loc vertex frame (offset): + Value: true m600_base: Value: true microstrain: Value: true + odo vertex frame: + Value: true + planning frame: + Value: true rear_bumblebee: Value: true rear_xb3: Value: true robot: Value: true + robot planning: + Value: true + robot planning (extrapolated): + Value: true + robot planning (extrapolated) mpc: + Value: true sensor_anchor_link: Value: true velodyne: @@ -110,8 +128,22 @@ Visualization Manager: Show Names: true Tree: world: - world (offset): + loc vertex frame: {} + odo vertex frame: + robot: + lidar: + {} + planning frame: + robot planning: + {} + robot planning (extrapolated): + {} + robot planning (extrapolated) mpc: + {} + world (offset): + loc vertex frame (offset): + {} Update Interval: 0 Value: true - Alpha: 1 @@ -160,7 +192,7 @@ Visualization Manager: Color: 0; 255; 0 Color Transformer: FlatColor Decay Time: 0 - Enabled: true + Enabled: false Invert Rainbow: false Max Color: 255; 255; 255 Max Intensity: 999999 @@ -181,7 +213,7 @@ Visualization Manager: Value: /vtr/filtered_point_cloud Use Fixed Frame: true Use rainbow: true - Value: true + Value: false - Angle Tolerance: 0.10000000149011612 Class: rviz_default_plugins/Odometry Covariance: @@ -329,7 +361,7 @@ Visualization Manager: Color: 255; 0; 0 Color Transformer: FlatColor Decay Time: 0 - Enabled: false + Enabled: true Invert Rainbow: false Max Color: 255; 255; 255 Max Intensity: 3 @@ -350,7 +382,7 @@ Visualization Manager: Value: /vtr/submap_loc Use Fixed Frame: true Use rainbow: true - Value: false + Value: true - Alpha: 1 Buffer Length: 1 Class: rviz_default_plugins/Path @@ -469,12 +501,12 @@ Visualization Manager: Min Value: -10 Value: true Axis: Z - Channel Name: flex23 + Channel Name: flex24 Class: rviz_default_plugins/PointCloud2 Color: 255; 255; 0 Color Transformer: Intensity Decay Time: 0 - Enabled: false + Enabled: true Invert Rainbow: false Max Color: 255; 255; 255 Max Intensity: 1 @@ -492,10 +524,10 @@ Visualization Manager: Filter size: 10 History Policy: Keep Last Reliability Policy: Best Effort - Value: /vtr/change_detection_scan + Value: /vtr/detection_cloud Use Fixed Frame: true Use rainbow: true - Value: false + Value: true - Alpha: 0.20000000298023224 Autocompute Intensity Bounds: false Autocompute Value Bounds: @@ -1212,6 +1244,40 @@ Visualization Manager: Use Fixed Frame: true Use rainbow: true Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: PointCloud2 + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.009999999776482582 + Style: Flat Squares + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /vtr/detection_cloud + Use Fixed Frame: true + Use rainbow: true + Value: true Enabled: true Global Options: Background Color: 248; 206; 255 @@ -1259,7 +1325,7 @@ Visualization Manager: Views: Current: Class: rviz_default_plugins/Orbit - Distance: 26.967164993286133 + Distance: 52.936981201171875 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 @@ -1274,10 +1340,10 @@ Visualization Manager: Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: 0.7153981328010559 + Pitch: 0.5253980755805969 Target Frame: robot Value: Orbit (rviz_default_plugins) - Yaw: 0.4403983950614929 + Yaw: 0.7353982925415039 Saved: - Class: rviz_default_plugins/Orbit Distance: 111.39698028564453 @@ -1485,7 +1551,7 @@ Window Geometry: Height: 1016 Hide Left Dock: false Hide Right Dock: false - QMainWindow State: 000000ff00000000fd0000000400000000000002810000039efc0200000007fc0000003d000001a20000000000fffffffaffffffff0100000002fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730000000000ffffffff0000009d00fffffffb0000001200530065006c0065006300740069006f006e0000000000000002530000007400fffffffb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d0000039e000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c0000026100000001000001c00000039efc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d0000039e000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000005000000006efc0100000002fb0000000800540069006d0065000000000000000500000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000002eb0000039e00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd000000040000000000000281000003a2fc0200000007fc0000003d000001a20000000000fffffffaffffffff0100000002fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730000000000ffffffff0000008c00fffffffb0000001200530065006c0065006300740069006f006e0000000000000002530000006c00fffffffb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b000003a2000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c0000026100000001000001c0000003a2fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003b000003a2000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000005000000006efc0100000002fb0000000800540069006d00650000000000000005000000025300fffffffb0000000800540069006d00650100000000000004500000000000000000000002eb000003a200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Time: From 53ba0107de329ed3a9b677aa66fd0e8440e2c786 Mon Sep 17 00:00:00 2001 From: Alec Krawciw Date: Sun, 23 Jul 2023 12:38:12 -0400 Subject: [PATCH 40/77] Add costmap filter --- .../rangenet_change_detection_module.cpp | 19 ++++++++++++------- 1 file changed, 12 insertions(+), 7 deletions(-) diff --git a/main/src/vtr_lidar/src/modules/planning/rangenet_change_detection_module.cpp b/main/src/vtr_lidar/src/modules/planning/rangenet_change_detection_module.cpp index 3c82245a8..96d320fe4 100644 --- a/main/src/vtr_lidar/src/modules/planning/rangenet_change_detection_module.cpp +++ b/main/src/vtr_lidar/src/modules/planning/rangenet_change_detection_module.cpp @@ -185,29 +185,34 @@ void RangeChangeNetModule::run_(QueryCache &qdata0, OutputCache &output0, } + costmap->update(values); + + const auto filtered_costmap = std::make_shared( + config_->resolution, config_->size_x, config_->size_y); + filtered_costmap->update(costmap->filter(0.75)); // add transform to the localization vertex - costmap->T_vertex_this() = T_r_v_loc; - costmap->vertex_id() = vid_loc; - costmap->vertex_sid() = sid_loc; + filtered_costmap->T_vertex_this() = T_r_v_loc; + filtered_costmap->vertex_id() = vid_loc; + filtered_costmap->vertex_sid() = sid_loc; 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 = filtered_costmap; /// publish the transformed pointcloud if (config_->visualize) { CLOG(DEBUG, "lidar.range") << "Getting to vis took " << timer; PointCloudMsg filter_msg; - pcl::toROSMsg(nn_point_cloud, filter_msg); + pcl::toROSMsg(obstacle_points, filter_msg); filter_msg.header.frame_id = "odo vertex frame"; filter_msg.header.stamp = rclcpp::Time(*qdata.stamp); diffpcd_pub_->publish(filter_msg); // publish the occupancy grid - auto costmap_msg = costmap->toCostMapMsg(); - costmap_msg.header.frame_id = "planning_frame"; + auto costmap_msg = filtered_costmap->toCostMapMsg(); + costmap_msg.header.frame_id = "odo vertex frame"; costmap_msg.header.stamp = rclcpp::Time(*qdata.stamp); CLOG(DEBUG, "lidar.range") << "Publishing costmap"; costmap_pub_->publish(costmap_msg); From 2a19e0c101811d07b464a989f95dfffe7666dc83 Mon Sep 17 00:00:00 2001 From: Alec Krawciw Date: Sun, 23 Jul 2023 13:10:30 -0400 Subject: [PATCH 41/77] Begin launch file config --- main/src/vtr_navigation/launch/vtr.launch.py | 2 ++ 1 file changed, 2 insertions(+) diff --git a/main/src/vtr_navigation/launch/vtr.launch.py b/main/src/vtr_navigation/launch/vtr.launch.py index ccb901111..7c1452181 100644 --- a/main/src/vtr_navigation/launch/vtr.launch.py +++ b/main/src/vtr_navigation/launch/vtr.launch.py @@ -15,6 +15,7 @@ def generate_launch_description(): DeclareLaunchArgument('data_dir', description='data directory (store log files and result pose graph)'), DeclareLaunchArgument('start_new_graph', default_value='false', description='whether to start a new pose graph'), DeclareLaunchArgument('use_sim_time', default_value='false', description='use simulated time for playback'), + DeclareLaunchArgument('planner', default_value='cbit', description='use no planner. Publish zero'), DeclareLaunchArgument('base_params', description='base parameter file (sensor, robot specific)'), DeclareLaunchArgument('override_params', default_value='', description='scenario specific parameter overrides'), Node( @@ -28,6 +29,7 @@ def generate_launch_description(): "data_dir": LaunchConfiguration("data_dir"), "start_new_graph": LaunchConfiguration("start_new_graph"), "use_sim_time": LaunchConfiguration("use_sim_time"), + "path_planning.type": LaunchConfiguration("planner"), }, PathJoinSubstitution((config_dir, LaunchConfiguration("base_params"))), PathJoinSubstitution((config_dir, LaunchConfiguration("override_params"))), From fc7eb21db5713e34b2bbf6232f6068ee8b27f509 Mon Sep 17 00:00:00 2001 From: Alec Krawciw Date: Mon, 14 Aug 2023 18:07:17 -0400 Subject: [PATCH 42/77] Updates to range change --- config/hdl64_grizzly_learned.yaml | 20 ++++---- launch/online_velodyne_grizzly.launch.yaml | 1 + .../rangenet_change_detection_module.hpp | 2 + .../rangenet_change_detection_module.cpp | 18 +++---- .../velodyne_conversion_module_v2.cpp | 50 ++++++++++--------- 5 files changed, 49 insertions(+), 42 deletions(-) diff --git a/config/hdl64_grizzly_learned.yaml b/config/hdl64_grizzly_learned.yaml index 82dbe678f..6ff85a3d2 100644 --- a/config/hdl64_grizzly_learned.yaml +++ b/config/hdl64_grizzly_learned.yaml @@ -14,12 +14,11 @@ #- tactic.module.live_mem_manager #- tactic.module.graph_mem_manager # path planner - # "path_planning", - # "path_planning.teb", - # "path_planning.cbit", + - path_planning + - path_planning.cbit # "path_planning.cbit_planner", - mpc.cbit - # "mpc_debug.cbit", + - mpc_debug.cbit # mission planner #- mission.server #- mission.state_machine @@ -116,7 +115,7 @@ crop_range: 15.0 - frame_voxel_size: 0.2 # grid subsampling voxel size + frame_voxel_size: 0.25 # grid subsampling voxel size #velodyne lidar has 1/3 degree upper block and 1/2 degree lower block vertical_angle_res: 0.00699 # vertical angle resolution in radians (0.4deg), @@ -185,8 +184,8 @@ type: lidar.dynamic_detection depth: 12.0 - horizontal_resolution: 0.1728 # 0.02042 - vertical_resolution: 0.00872664626 # 0.01326 + horizontal_resolution: 0.00301 # 0.02042 + vertical_resolution: 0.00699 # 0.01326 max_num_observations: 10000 min_num_observations: 4 dynamic_threshold: 0.3 @@ -236,11 +235,14 @@ type: torch.range_change use_gpu: true abs_filepath: false - filepath: exported_model_ground_8m.pt + filepath: exported_model_temporal_ground_8m.pt visualize: true influence_distance: 0.5 # was 0.5 Jordy # Note that the total distance where grid cells have values > 0 is min dist + influence dist, not influence dist! minimum_distance: 0.8 # was 0.3 Jordy + resolution: 0.25 + size_x: 16.0 + size_y: 16.0 memory: type: graph_mem_manager vertex_life_span: 5 @@ -266,7 +268,7 @@ run_async: true visualize: true path_planning: - type: cbit # teb.lidar for old teb version, cbit for obstacle free path tracker, cbit.lidar for obstacle avoidance version + type: stationary # teb.lidar for old teb version, cbit for obstacle free path tracker, cbit.lidar for obstacle avoidance version control_period: 100 # ms cbit: diff --git a/launch/online_velodyne_grizzly.launch.yaml b/launch/online_velodyne_grizzly.launch.yaml index 636c60e17..4b73f2df1 100644 --- a/launch/online_velodyne_grizzly.launch.yaml +++ b/launch/online_velodyne_grizzly.launch.yaml @@ -24,6 +24,7 @@ windows: start_new_graph:=false model_dir:=${VTRMODELS} use_sim_time:=false + planner:=cbit - window_name: vtr_gui layout: main-horizontal diff --git a/main/src/vtr_lidar/include/vtr_lidar/modules/planning/rangenet_change_detection_module.hpp b/main/src/vtr_lidar/include/vtr_lidar/modules/planning/rangenet_change_detection_module.hpp index 5ddacfc56..fedc67947 100644 --- a/main/src/vtr_lidar/include/vtr_lidar/modules/planning/rangenet_change_detection_module.hpp +++ b/main/src/vtr_lidar/include/vtr_lidar/modules/planning/rangenet_change_detection_module.hpp @@ -92,6 +92,8 @@ class RangeChangeNetModule : public nn::TorchModule { const tactic::Graph::Ptr &graph, const tactic::TaskExecutor::Ptr &executor) override; + costmap::PixKey pointToKey(PointWithInfo &p); + Config::ConstPtr config_; rclcpp::Publisher::SharedPtr mask_pub_; diff --git a/main/src/vtr_lidar/src/modules/planning/rangenet_change_detection_module.cpp b/main/src/vtr_lidar/src/modules/planning/rangenet_change_detection_module.cpp index 96d320fe4..ac65df3ce 100644 --- a/main/src/vtr_lidar/src/modules/planning/rangenet_change_detection_module.cpp +++ b/main/src/vtr_lidar/src/modules/planning/rangenet_change_detection_module.cpp @@ -44,10 +44,6 @@ namespace { } } - - costmap::PixKey pointToKey(PointWithInfo &p) { - return {std::lround(p.x), std::lround(p.y)}; - } } using namespace tactic; @@ -139,7 +135,7 @@ void RangeChangeNetModule::run_(QueryCache &qdata0, OutputCache &output0, Eigen::MatrixXi scan_idxs = Eigen::MatrixXi::Constant(64, 1024, -1); RowMatrixXf map_image = Eigen::MatrixXf::Constant(64, 1024, -1); - common::timing::Stopwatch timer; + common::timing::Stopwatch timer; timer.start(); generate_range_image(nn_point_cloud, scan_image, scan_idxs, image_params); generate_range_image(map_point_cloud, map_image, image_params); @@ -178,9 +174,9 @@ void RangeChangeNetModule::run_(QueryCache &qdata0, OutputCache &output0, for (size_t i = 0; i < obstacle_points.size(); ++i) { const auto pix_coord = pointToKey(obstacle_points[i]); if (values.find(pix_coord) != values.end()) { - values[pix_coord] += 0.05; + values[pix_coord] += 0.2; } else { - values[pix_coord] = 0.05; + values[pix_coord] = 0.2; } } @@ -190,9 +186,9 @@ void RangeChangeNetModule::run_(QueryCache &qdata0, OutputCache &output0, const auto filtered_costmap = std::make_shared( config_->resolution, config_->size_x, config_->size_y); - filtered_costmap->update(costmap->filter(0.75)); + filtered_costmap->update(costmap->filter(1.0)); // add transform to the localization vertex - filtered_costmap->T_vertex_this() = T_r_v_loc; + filtered_costmap->T_vertex_this() = T_r_v_loc.inverse(); filtered_costmap->vertex_id() = vid_loc; filtered_costmap->vertex_sid() = sid_loc; @@ -226,5 +222,9 @@ void RangeChangeNetModule::run_(QueryCache &qdata0, OutputCache &output0, } +costmap::PixKey RangeChangeNetModule::pointToKey(PointWithInfo &p) { + return {std::lround(p.x / config_->resolution), std::lround(p.y / config_->resolution)}; + } + } // namespace nn } // namespace vtr \ No newline at end of file diff --git a/main/src/vtr_lidar/src/modules/preprocessing/conversions/velodyne_conversion_module_v2.cpp b/main/src/vtr_lidar/src/modules/preprocessing/conversions/velodyne_conversion_module_v2.cpp index 65cf65a3a..4ee3c2e9f 100644 --- a/main/src/vtr_lidar/src/modules/preprocessing/conversions/velodyne_conversion_module_v2.cpp +++ b/main/src/vtr_lidar/src/modules/preprocessing/conversions/velodyne_conversion_module_v2.cpp @@ -104,12 +104,35 @@ void VelodyneConversionModuleV2::run_(QueryCache &qdata0, OutputCache &, // clang-format on - // Velodyne has no polar coordinates, so compute them manually. - velodyneCart2Pol(*point_cloud); + + + + + //If a lower horizontal resolution is acceptable, then set the horizontal downsample > 1. + //A value of 2 will leave 1/2 the points, in general 1/n points will be retained. + + auto filtered_point_cloud = + std::make_shared>(*point_cloud); + CLOG(DEBUG, "lidar.velodyne_converter_v2") << "Reducing the point cloud density by " << config_->horizontal_downsample + << "original size was " << point_cloud->size(); + if (config_->horizontal_downsample > 1) { + std::vector indices; + indices.reserve(filtered_point_cloud->size()); + for (size_t i = 0; i < filtered_point_cloud->size(); ++i) { + if (i % config_->horizontal_downsample == 0 )//&& (*point_cloud)[i].phi < -M_PI) + indices.emplace_back(i); + } + *filtered_point_cloud = + pcl::PointCloud(*filtered_point_cloud, indices); + + } + + // Velodyne has no polar coordinates, so compute them manually. + velodyneCart2Pol(*filtered_point_cloud); if (config_->estimate_time){ CLOG(INFO, "lidar.velodyne_converter_v2") << "Timings wil be estimated from yaw angle"; - estimateTime(*point_cloud, *qdata.stamp, config_->angular_vel); + estimateTime(*filtered_point_cloud, *qdata.stamp, config_->angular_vel); } else { try { try{ @@ -138,27 +161,6 @@ void VelodyneConversionModuleV2::run_(QueryCache &qdata0, OutputCache &, } } - - - //If a lower horizontal resolution is acceptable, then set the horizontal downsample > 1. - //A value of 2 will leave 1/2 the points, in general 1/n points will be retained. - - auto filtered_point_cloud = - std::make_shared>(*point_cloud); - CLOG(DEBUG, "lidar.velodyne_converter_v2") << "Reducing the point cloud density by " << config_->horizontal_downsample - << "original size was " << point_cloud->size(); - if (config_->horizontal_downsample > 1) { - std::vector indices; - indices.reserve(filtered_point_cloud->size()); - for (size_t i = 0; i < filtered_point_cloud->size(); ++i) { - if (i % config_->horizontal_downsample == 0 )//&& (*point_cloud)[i].phi < -M_PI) - indices.emplace_back(i); - } - *filtered_point_cloud = - pcl::PointCloud(*filtered_point_cloud, indices); - - } - // Output qdata.raw_point_cloud = filtered_point_cloud; From 0293029059dda69e229a88f25f774b5c24e11832 Mon Sep 17 00:00:00 2001 From: a-krawciw Date: Wed, 23 Aug 2023 20:09:40 -0400 Subject: [PATCH 43/77] Change temporal history. --- config/honeycomb_grizzly_default.yaml | 6 +- launch/offline_honeycomb_grizzly.launch.yaml | 8 +- .../planning/change_detection_module_v3.hpp | 7 +- .../planning/change_detection_module_v3.cpp | 251 ++---------------- 4 files changed, 40 insertions(+), 232 deletions(-) diff --git a/config/honeycomb_grizzly_default.yaml b/config/honeycomb_grizzly_default.yaml index 9044838ef..9c006c3db 100644 --- a/config/honeycomb_grizzly_default.yaml +++ b/config/honeycomb_grizzly_default.yaml @@ -18,7 +18,7 @@ # path planner #"path_planning", #"path_planning.teb", - #"path_planning.cbit", + - path_planning.cbit #"path_planning.cbit_planner", #"mpc.cbit", - mpc_debug.cbit @@ -296,7 +296,7 @@ visualize: true path_planning: - type: cbit # cbit for obstacle free path tracker, cbit.lidar for obstacle avoidance version + type: cbit.lidar # cbit for obstacle free path tracker, cbit.lidar for obstacle avoidance version control_period: 100 # ms cbit: obs_padding: 0.0 @@ -366,4 +366,4 @@ #kin_error_cov: [0.001, 0.001, 0.001, 0.001, 0.001, 0.001] # Misc - command_history_length: 100 \ No newline at end of file + command_history_length: 100 diff --git a/launch/offline_honeycomb_grizzly.launch.yaml b/launch/offline_honeycomb_grizzly.launch.yaml index 32d4140d7..cad8a2e9e 100644 --- a/launch/offline_honeycomb_grizzly.launch.yaml +++ b/launch/offline_honeycomb_grizzly.launch.yaml @@ -17,12 +17,12 @@ windows: panes: - > ros2 launch vtr_navigation vtr.launch.py - base_params:=honeycomb_grizzly_learned.yaml + base_params:=honeycomb_grizzly_default.yaml data_dir:=${VTRTEMP}/lidar model_dir:=${VTRMODELS} - start_new_graph:=true + start_new_graph:=false use_sim_time:=true - path_planning.type:=stationary + path_planning.type:="cbit.lidar" # - htop # monitor hardware usage @@ -41,5 +41,5 @@ windows: shell_command_before: - source /opt/ros/humble/setup.bash panes: - - ros2 run rviz2 rviz2 -d ${VTRSRC}/rviz/lidar.rviz + - ros2 run rviz2 rviz2 -d ${VTRSRC}/rviz/honeycomb.rviz # - ros2 run rqt_reconfigure rqt_reconfigure 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 5a070769e..7fb9a3f00 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 @@ -18,6 +18,8 @@ */ #pragma once +#include + #include "tf2/convert.h" #include "tf2_eigen/tf2_eigen.hpp" #include "tf2_ros/transform_broadcaster.h" @@ -58,7 +60,7 @@ class ChangeDetectionModuleV3 : public tactic::BaseModule { float support_threshold = 0.0; // cost map - int costmap_history_size = 10; + unsigned int costmap_history_size = 10; float resolution = 1.0; float size_x = 20.0; float size_y = 20.0; @@ -89,7 +91,6 @@ 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_; rclcpp::Publisher::SharedPtr diffpcd_pub_; @@ -97,7 +98,7 @@ class ChangeDetectionModuleV3 : public tactic::BaseModule { // Modificatons for Temporal costmap filter - std::vector, float>> costmap_history; + std::list>> detected_history; }; } // namespace lidar 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 b90ae38dd..36d770d20 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 @@ -114,7 +114,8 @@ 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); + int hist_size = node->declare_parameter(param_prefix + ".costmap_history_size", config->costmap_history_size); + config->costmap_history_size = (unsigned) hist_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); @@ -137,7 +138,6 @@ 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); diffpcd_pub_ = qdata.node->create_publisher("change_detection_diff", 5); // clang-format on @@ -160,6 +160,7 @@ void ChangeDetectionModuleV3::run_(QueryCache &qdata0, OutputCache &output0, const auto &submap_loc = *qdata.submap_loc; const auto &map_point_cloud = submap_loc.point_cloud(); const auto &T_v_m_loc = *qdata.T_v_m_loc; + const auto &chain = *output.chain; // clang-format off CLOG(INFO, "lidar.change_detection") << "Change detection for lidar scan at stamp: " << stamp; @@ -316,229 +317,45 @@ void ChangeDetectionModuleV3::run_(QueryCache &qdata0, OutputCache &output0, } pcl::PointCloud filtered_points(aligned_points2, indices); - // update cost map based on change detection result - DetectChangeOp detect_change_op( - filtered_points, config_->influence_distance, config_->minimum_distance); - costmap->update(detect_change_op); - // add transform to the localization vertex - costmap->T_vertex_this() = tactic::EdgeTransform(true); - costmap->vertex_id() = vid_loc; - costmap->vertex_sid() = sid_loc; + auto concat_pc = pcl::PointCloud(); - // Jordy Modifications for temporal costmap filtering (UNDER DEVELOPMENT) - + detected_history.push_front(std::make_pair(sid_loc, filtered_points)); - - // 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; + if (detected_history.size() > config_->costmap_history_size) + detected_history.pop_back(); - // Filter non-obstacles - vtr::lidar::BaseCostMap::XY2ValueMap sparse_obs_map = sparse_costmap->filter(0.01); + for (auto &pair : detected_history) { + auto &p_loc_sid = pair.first; + auto &point_cloud = pair.second; + const auto &T_v_loc_v_detect = chain.T_trunk_target(p_loc_sid); - // 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; + auto point_cloud_copy = pcl::PointCloud(point_cloud); - //CLOG(DEBUG, "obstacle_detection.cbit") << "Displaying Transform: " << (sparse_costmap->T_vertex_this().inverse()); + auto aligned_point_cloud = point_cloud.getMatrixXfMap(4, PointWithInfo::size(), PointWithInfo::cartesian_offset()); + auto aligned_norms = point_cloud.getMatrixXfMap(4, PointWithInfo::size(), PointWithInfo::normal_offset()); - 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 + auto aligned_point_cloud_copy = point_cloud_copy.getMatrixXfMap(4, PointWithInfo::size(), PointWithInfo::cartesian_offset()); + auto aligned_norms_copy = point_cloud_copy.getMatrixXfMap(4, PointWithInfo::size(), PointWithInfo::normal_offset()); + aligned_point_cloud_copy = T_v_loc_v_detect.matrix().cast() * aligned_point_cloud; + aligned_norms_copy = T_v_loc_v_detect.matrix().cast() * aligned_norms; - 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(); + concat_pc += point_cloud_copy; + CLOG(DEBUG, "lidar.change_detection") << "Point cloud of size " << point_cloud.size() << " is connected to vid: " << p_loc_sid; - - // 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 - // TODO A linked list would make more sense here - - 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 (unsigned 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 (unsigned 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 declare 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"; - - - if (config_->visualize) { - // 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 - - + // update cost map based on change detection result + DetectChangeOp detect_change_op( + concat_pc, config_->influence_distance, config_->minimum_distance); + costmap->update(detect_change_op); + // add transform to the localization vertex + costmap->T_vertex_this() = tactic::EdgeTransform(true); + costmap->vertex_id() = vid_loc; + costmap->vertex_sid() = sid_loc; /// publish the transformed pointcloud @@ -572,17 +389,7 @@ 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; - - //Jordy debug, using experimental new costmaps instead - if (costmap_history.size() < 10.0) - { - change_detection_costmap = costmap; - } - else - { - change_detection_costmap = dense_costmap; - } + change_detection_costmap = costmap; CLOG(INFO, "lidar.change_detection") << "Change detection for lidar scan at stamp: " << stamp << " - DONE"; From 8891b0887b5fe88237adc01b97bacb756032f75f Mon Sep 17 00:00:00 2001 From: a-krawciw Date: Wed, 23 Aug 2023 20:49:40 -0400 Subject: [PATCH 44/77] Move costmap inflation to its own module. --- config/honeycomb_grizzly_default.yaml | 4 +- .../src/vtr_lidar/include/vtr_lidar/cache.hpp | 3 + .../include/vtr_lidar/modules/modules.hpp | 1 + .../planning/change_detection_module_v3.hpp | 13 -- .../planning/costmap_inflation_module.hpp | 89 ++++++++ .../planning/change_detection_module_v3.cpp | 109 +--------- .../planning/costmap_inflation_module.cpp | 198 ++++++++++++++++++ 7 files changed, 297 insertions(+), 120 deletions(-) create mode 100644 main/src/vtr_lidar/include/vtr_lidar/modules/planning/costmap_inflation_module.hpp create mode 100644 main/src/vtr_lidar/src/modules/planning/costmap_inflation_module.cpp diff --git a/config/honeycomb_grizzly_default.yaml b/config/honeycomb_grizzly_default.yaml index 9c006c3db..92f755dbd 100644 --- a/config/honeycomb_grizzly_default.yaml +++ b/config/honeycomb_grizzly_default.yaml @@ -93,6 +93,7 @@ - icp - safe_corridor - change_detection_sync + - costmap_inflation - memory submap_translation_threshold: 1.5 submap_rotation_threshold: 30.0 @@ -240,7 +241,8 @@ support_radius: 0.25 support_variance: 0.05 support_threshold: 2.5 - + costmap_inflation: + type: lidar.costmap_inflation influence_distance: 0.5 # was 0.5 Jordy # Note that the total distance where grid cells have values > 0 is min dist + influence dist, not influence dist! minimum_distance: 0.8 # was 0.3 Jordy diff --git a/main/src/vtr_lidar/include/vtr_lidar/cache.hpp b/main/src/vtr_lidar/include/vtr_lidar/cache.hpp index 580ea36a4..7680b5612 100644 --- a/main/src/vtr_lidar/include/vtr_lidar/cache.hpp +++ b/main/src/vtr_lidar/include/vtr_lidar/cache.hpp @@ -57,6 +57,9 @@ struct LidarQueryCache : virtual public tactic::QueryCache { tactic::Cache submap_loc_changed; tactic::Cache T_v_m_loc; + //change detection + tactic::Cache> changed_points; + // intra exp merging async tactic::Cache intra_exp_merging_async; diff --git a/main/src/vtr_lidar/include/vtr_lidar/modules/modules.hpp b/main/src/vtr_lidar/include/vtr_lidar/modules/modules.hpp index 88a4ee98b..51409ced0 100644 --- a/main/src/vtr_lidar/include/vtr_lidar/modules/modules.hpp +++ b/main/src/vtr_lidar/include/vtr_lidar/modules/modules.hpp @@ -49,4 +49,5 @@ #include "vtr_lidar/modules/planning/terrain_assessment_module.hpp" #include "vtr_lidar/modules/planning/diff_generator.hpp" #include "vtr_lidar/modules/planning/rangenet_change_detection_module.hpp" +#include "vtr_lidar/modules/planning/costmap_inflation_module.hpp" 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 7fb9a3f00..cec469742 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 @@ -59,14 +59,6 @@ class ChangeDetectionModuleV3 : public tactic::BaseModule { float support_variance = 0.1; float support_threshold = 0.0; - // cost map - unsigned int costmap_history_size = 10; - float resolution = 1.0; - float size_x = 20.0; - float size_y = 20.0; - float influence_distance = 1.0; - float minimum_distance = 0.5; - // bool visualize = false; @@ -90,15 +82,10 @@ class ChangeDetectionModuleV3 : public tactic::BaseModule { /** \brief for visualization only */ bool publisher_initialized_ = false; rclcpp::Publisher::SharedPtr scan_pub_; - rclcpp::Publisher::SharedPtr costmap_pub_; - rclcpp::Publisher::SharedPtr costpcd_pub_; rclcpp::Publisher::SharedPtr diffpcd_pub_; VTR_REGISTER_MODULE_DEC_TYPE(ChangeDetectionModuleV3); - - // Modificatons for Temporal costmap filter - std::list>> detected_history; }; } // namespace lidar diff --git a/main/src/vtr_lidar/include/vtr_lidar/modules/planning/costmap_inflation_module.hpp b/main/src/vtr_lidar/include/vtr_lidar/modules/planning/costmap_inflation_module.hpp new file mode 100644 index 000000000..f798a07eb --- /dev/null +++ b/main/src/vtr_lidar/include/vtr_lidar/modules/planning/costmap_inflation_module.hpp @@ -0,0 +1,89 @@ +// Copyright 2023, 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 change_detection_module_v3.hpp + * \author Yuchen Wu, Autonomous Space Robotics Lab (ASRL) + */ +#pragma once + +#include + +#include "tf2/convert.h" +#include "tf2_eigen/tf2_eigen.hpp" +#include "tf2_ros/transform_broadcaster.h" + +#include "nav_msgs/msg/occupancy_grid.hpp" + +#include "vtr_lidar/cache.hpp" +#include "vtr_tactic/modules/base_module.hpp" +#include "vtr_tactic/task_queue.hpp" + +namespace vtr { +namespace lidar { + +class CostmapInflationModule : public tactic::BaseModule { + public: + PTR_TYPEDEFS(CostmapInflationModule); + using PointCloudMsg = sensor_msgs::msg::PointCloud2; + using OccupancyGridMsg = nav_msgs::msg::OccupancyGrid; + + static constexpr auto static_name = "lidar.costmap_inflation"; + + /** \brief Collection of config parameters */ + struct Config : public BaseModule::Config { + PTR_TYPEDEFS(Config); + + // cost map + unsigned int costmap_history_size = 10; + float resolution = 1.0; + float size_x = 20.0; + float size_y = 20.0; + float influence_distance = 1.0; + float minimum_distance = 0.5; + + // + bool visualize = false; + + static ConstPtr fromROS(const rclcpp::Node::SharedPtr &node, + const std::string ¶m_prefix); + }; + + CostmapInflationModule( + const Config::ConstPtr &config, + const std::shared_ptr &module_factory = nullptr, + const std::string &name = static_name) + : tactic::BaseModule{module_factory, name}, config_(config) {} + + private: + void run_(tactic::QueryCache &qdata, tactic::OutputCache &output, + const tactic::Graph::Ptr &graph, + const tactic::TaskExecutor::Ptr &executor) override; + + Config::ConstPtr config_; + + /** \brief for visualization only */ + bool publisher_initialized_ = false; + rclcpp::Publisher::SharedPtr costmap_pub_; + rclcpp::Publisher::SharedPtr costpcd_pub_; + + VTR_REGISTER_MODULE_DEC_TYPE(CostmapInflationModule); + + + // Modificatons for Temporal costmap filter + std::list>> detected_history; +}; + +} // namespace lidar +} // namespace vtr 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 36d770d20..170e74d15 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 @@ -55,42 +55,6 @@ void computeCentroidAndNormal(const pcl::PointCloud &points, roughness = es.eigenvalues()(0); // variance } -template -class DetectChangeOp { - public: - DetectChangeOp(const pcl::PointCloud &points, const float &d0, - const float &d1) - : d0_(d0), d1_(d1), adapter_(points) { - /// create kd-tree of the point cloud for radius search - kdtree_ = std::make_unique>(2, adapter_, - KDTreeParams(/* max leaf */ 10)); - kdtree_->buildIndex(); - // search params setup - search_params_.sorted = false; - } - - void operator()(const Eigen::Vector2f &q, float &v) const { - size_t ind; - float dist; - KDTreeResultSet result_set(1); - result_set.init(&ind, &dist); - kdtree_->findNeighbors(result_set, q.data(), search_params_); - - // update the value of v - dist = std::sqrt(dist); // convert to distance - v = std::max(1 - (dist - d1_) / d0_, 0.0f); - v = std::min(v, 0.9f); // 1 is bad for visualization - } - - private: - const float d0_; - const float d1_; - - KDTreeSearchParams search_params_; - NanoFLANNAdapter adapter_; - std::unique_ptr> kdtree_; -}; - } // namespace using namespace tactic; @@ -113,14 +77,7 @@ auto ChangeDetectionModuleV3::Config::fromROS( config->support_radius = node->declare_parameter(param_prefix + ".support_radius", config->support_radius); 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 - int hist_size = node->declare_parameter(param_prefix + ".costmap_history_size", config->costmap_history_size); - config->costmap_history_size = (unsigned) hist_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); - config->influence_distance = node->declare_parameter(param_prefix + ".influence_distance", config->influence_distance); - config->minimum_distance = node->declare_parameter(param_prefix + ".minimum_distance", config->minimum_distance); + // general config->visualize = node->declare_parameter(param_prefix + ".visualize", config->visualize); // clang-format on @@ -137,8 +94,6 @@ void ChangeDetectionModuleV3::run_(QueryCache &qdata0, OutputCache &output0, if (config_->visualize && !publisher_initialized_) { // clang-format off scan_pub_ = qdata.node->create_publisher("change_detection_scan", 5); - costmap_pub_ = qdata.node->create_publisher("change_detection_costmap", 5); - costpcd_pub_ = qdata.node->create_publisher("change_detection_costpcd", 5); diffpcd_pub_ = qdata.node->create_publisher("change_detection_diff", 5); // clang-format on publisher_initialized_ = true; @@ -305,9 +260,6 @@ void ChangeDetectionModuleV3::run_(QueryCache &qdata0, OutputCache &output0, aligned_mat2 = T_v_m_loc.matrix().cast() * aligned_mat; aligned_norms_mat2 = T_v_m_loc.matrix().cast() * aligned_norms_mat; - // project to 2d and construct the grid map - const auto costmap = std::make_shared( - config_->resolution, config_->size_x, config_->size_y); // filter out non-obstacle points std::vector indices; @@ -316,46 +268,8 @@ void ChangeDetectionModuleV3::run_(QueryCache &qdata0, OutputCache &output0, if (aligned_points2[i].flex23 > 0.5f) indices.emplace_back(i); } pcl::PointCloud filtered_points(aligned_points2, indices); - - auto concat_pc = pcl::PointCloud(); - - detected_history.push_front(std::make_pair(sid_loc, filtered_points)); - - if (detected_history.size() > config_->costmap_history_size) - detected_history.pop_back(); - - for (auto &pair : detected_history) { - auto &p_loc_sid = pair.first; - auto &point_cloud = pair.second; - const auto &T_v_loc_v_detect = chain.T_trunk_target(p_loc_sid); - - auto point_cloud_copy = pcl::PointCloud(point_cloud); - - auto aligned_point_cloud = point_cloud.getMatrixXfMap(4, PointWithInfo::size(), PointWithInfo::cartesian_offset()); - auto aligned_norms = point_cloud.getMatrixXfMap(4, PointWithInfo::size(), PointWithInfo::normal_offset()); - - - auto aligned_point_cloud_copy = point_cloud_copy.getMatrixXfMap(4, PointWithInfo::size(), PointWithInfo::cartesian_offset()); - auto aligned_norms_copy = point_cloud_copy.getMatrixXfMap(4, PointWithInfo::size(), PointWithInfo::normal_offset()); - - - aligned_point_cloud_copy = T_v_loc_v_detect.matrix().cast() * aligned_point_cloud; - aligned_norms_copy = T_v_loc_v_detect.matrix().cast() * aligned_norms; - - concat_pc += point_cloud_copy; - - CLOG(DEBUG, "lidar.change_detection") << "Point cloud of size " << point_cloud.size() << " is connected to vid: " << p_loc_sid; - - } - - // update cost map based on change detection result - DetectChangeOp detect_change_op( - concat_pc, config_->influence_distance, config_->minimum_distance); - costmap->update(detect_change_op); - // add transform to the localization vertex - costmap->T_vertex_this() = tactic::EdgeTransform(true); - costmap->vertex_id() = vid_loc; - costmap->vertex_sid() = sid_loc; + /// output + qdata.changed_points.emplace(filtered_points); /// publish the transformed pointcloud @@ -367,18 +281,6 @@ void ChangeDetectionModuleV3::run_(QueryCache &qdata0, OutputCache &output0, // scan_msg.header.stamp = rclcpp::Time(*qdata.stamp); scan_pub_->publish(scan_msg); - // publish the occupancy grid - auto costmap_msg = costmap->toCostMapMsg(); - costmap_msg.header.frame_id = "loc vertex frame"; - // costmap_msg.header.stamp = rclcpp::Time(*qdata.stamp); - costmap_pub_->publish(costmap_msg); - - // publish the point cloud - auto pointcloud_msg = costmap->toPointCloudMsg(); - pointcloud_msg.header.frame_id = "loc vertex frame"; - // pointcloud_msg.header.stamp = rclcpp::Time(*qdata.stamp); - costpcd_pub_->publish(pointcloud_msg); - PointCloudMsg filter_msg; pcl::toROSMsg(filtered_points, filter_msg); filter_msg.header.frame_id = "loc vertex frame"; @@ -386,11 +288,6 @@ void ChangeDetectionModuleV3::run_(QueryCache &qdata0, OutputCache &output0, diffpcd_pub_->publish(filter_msg); } - /// output - auto change_detection_costmap_ref = output.change_detection_costmap.locked(); - auto &change_detection_costmap = change_detection_costmap_ref.get(); - change_detection_costmap = costmap; - CLOG(INFO, "lidar.change_detection") << "Change detection for lidar scan at stamp: " << stamp << " - DONE"; diff --git a/main/src/vtr_lidar/src/modules/planning/costmap_inflation_module.cpp b/main/src/vtr_lidar/src/modules/planning/costmap_inflation_module.cpp new file mode 100644 index 000000000..cd7d0b9d9 --- /dev/null +++ b/main/src/vtr_lidar/src/modules/planning/costmap_inflation_module.cpp @@ -0,0 +1,198 @@ +// 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 costmap_inflation_module.cpp + * \author Alec Krawciw, Autonomous Space Robotics Lab (ASRL) + */ +#include "vtr_lidar/modules/planning/costmap_inflation_module.hpp" + +#include "pcl/features/normal_3d.h" + +#include "vtr_lidar/data_types/costmap.hpp" +#include "vtr_lidar/filters/voxel_downsample.hpp" + +#include "vtr_lidar/utils/nanoflann_utils.hpp" + +namespace vtr { +namespace lidar { + +namespace { + + +template +class DetectChangeOp { + public: + DetectChangeOp(const pcl::PointCloud &points, const float &d0, + const float &d1) + : d0_(d0), d1_(d1), adapter_(points) { + /// create kd-tree of the point cloud for radius search + kdtree_ = std::make_unique>(2, adapter_, + KDTreeParams(/* max leaf */ 10)); + kdtree_->buildIndex(); + // search params setup + search_params_.sorted = false; + } + + void operator()(const Eigen::Vector2f &q, float &v) const { + size_t ind; + float dist; + KDTreeResultSet result_set(1); + result_set.init(&ind, &dist); + kdtree_->findNeighbors(result_set, q.data(), search_params_); + + // update the value of v + dist = std::sqrt(dist); // convert to distance + v = std::max(1 - (dist - d1_) / d0_, 0.0f); + v = std::min(v, 0.9f); // 1 is bad for visualization + } + + private: + const float d0_; + const float d1_; + + KDTreeSearchParams search_params_; + NanoFLANNAdapter adapter_; + std::unique_ptr> kdtree_; +}; + +} // namespace + +using namespace tactic; + +auto CostmapInflationModule::Config::fromROS( + const rclcpp::Node::SharedPtr &node, const std::string ¶m_prefix) + -> ConstPtr { + auto config = std::make_shared(); + // clang-format off + // cost map + int hist_size = node->declare_parameter(param_prefix + ".costmap_history_size", config->costmap_history_size); + config->costmap_history_size = (unsigned) hist_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); + config->influence_distance = node->declare_parameter(param_prefix + ".influence_distance", config->influence_distance); + config->minimum_distance = node->declare_parameter(param_prefix + ".minimum_distance", config->minimum_distance); + // general + config->visualize = node->declare_parameter(param_prefix + ".visualize", config->visualize); + // clang-format on + return config; +} + +void CostmapInflationModule::run_(QueryCache &qdata0, OutputCache &output0, + const Graph::Ptr & /* graph */, + const TaskExecutor::Ptr & /* executor */) { + auto &qdata = dynamic_cast(qdata0); + auto &output = dynamic_cast(output0); + + /// visualization setup + if (config_->visualize && !publisher_initialized_) { + // clang-format off + costmap_pub_ = qdata.node->create_publisher("change_detection_costmap", 5); + costpcd_pub_ = qdata.node->create_publisher("change_detection_costpcd", 5); + // clang-format on + publisher_initialized_ = true; + } + + if (!qdata.changed_points.valid()) { + CLOG(WARNING, "lidar.obstacle_inflation") << "No change detection run!"; + return; + } + + + // inputs + const auto &stamp = *qdata.stamp; + const auto &vid_loc = *qdata.vid_loc; + const auto &sid_loc = *qdata.sid_loc; + const auto &changed_points = *qdata.changed_points; + const auto &chain = *output.chain; + + // clang-format off + CLOG(INFO, "lidar.obstacle_inflation") << "Inflating obstacles at stamp: " << stamp; + + + auto concat_pc = pcl::PointCloud(); + + // project to 2d and construct the grid map + const auto costmap = std::make_shared( + config_->resolution, config_->size_x, config_->size_y); + + detected_history.push_front(std::make_pair(sid_loc, changed_points)); + + if (detected_history.size() > config_->costmap_history_size) + detected_history.pop_back(); + + for (auto &pair : detected_history) { + auto &p_loc_sid = pair.first; + auto &point_cloud = pair.second; + const auto &T_v_loc_v_detect = chain.T_trunk_target(p_loc_sid); + + auto point_cloud_copy = pcl::PointCloud(point_cloud); + + auto aligned_point_cloud = point_cloud.getMatrixXfMap(4, PointWithInfo::size(), PointWithInfo::cartesian_offset()); + auto aligned_norms = point_cloud.getMatrixXfMap(4, PointWithInfo::size(), PointWithInfo::normal_offset()); + + + auto aligned_point_cloud_copy = point_cloud_copy.getMatrixXfMap(4, PointWithInfo::size(), PointWithInfo::cartesian_offset()); + auto aligned_norms_copy = point_cloud_copy.getMatrixXfMap(4, PointWithInfo::size(), PointWithInfo::normal_offset()); + + + aligned_point_cloud_copy = T_v_loc_v_detect.matrix().cast() * aligned_point_cloud; + aligned_norms_copy = T_v_loc_v_detect.matrix().cast() * aligned_norms; + + concat_pc += point_cloud_copy; + + CLOG(DEBUG, "lidar.obstacle_inflation") << "Point cloud of size " << point_cloud.size() << " is connected to sid: " << p_loc_sid; + + } + + // update cost map based on change detection result + DetectChangeOp detect_change_op( + concat_pc, config_->influence_distance, config_->minimum_distance); + costmap->update(detect_change_op); + // add transform to the localization vertex + costmap->T_vertex_this() = tactic::EdgeTransform(true); + costmap->vertex_id() = vid_loc; + costmap->vertex_sid() = sid_loc; + + + /// publish the transformed pointcloud + if (config_->visualize) { + + // publish the occupancy grid + auto costmap_msg = costmap->toCostMapMsg(); + costmap_msg.header.frame_id = "loc vertex frame"; + // costmap_msg.header.stamp = rclcpp::Time(*qdata.stamp); + costmap_pub_->publish(costmap_msg); + + // publish the point cloud + auto pointcloud_msg = costmap->toPointCloudMsg(); + pointcloud_msg.header.frame_id = "loc vertex frame"; + // pointcloud_msg.header.stamp = rclcpp::Time(*qdata.stamp); + costpcd_pub_->publish(pointcloud_msg); + + } + + /// output + auto change_detection_costmap_ref = output.change_detection_costmap.locked(); + auto &change_detection_costmap = change_detection_costmap_ref.get(); + change_detection_costmap = costmap; + + CLOG(INFO, "lidar.obstacle_inflation") + << "Change detection for lidar scan at stamp: " << stamp << " - DONE"; + +} + +} // namespace lidar +} // namespace vtr \ No newline at end of file From 1a52875dad42223b678fb84c515a76b04ce785d9 Mon Sep 17 00:00:00 2001 From: a-krawciw Date: Thu, 24 Aug 2023 15:32:12 -0400 Subject: [PATCH 45/77] Fix the concantenation of changes. --- config/ouster_warthog_learned.yaml | 378 ++++++++++++++++++ launch/offline_ouster_warthog.launch.yaml | 44 ++ launch/online_ouster_warthog.launch.yaml | 4 +- .../planning/costmap_inflation_module.hpp | 7 +- .../rangenet_change_detection_module.hpp | 11 - .../planning/costmap_inflation_module.cpp | 8 +- .../rangenet_change_detection_module.cpp | 81 ++-- rviz/ouster.rviz | 205 ++-------- 8 files changed, 515 insertions(+), 223 deletions(-) create mode 100644 config/ouster_warthog_learned.yaml create mode 100644 launch/offline_ouster_warthog.launch.yaml diff --git a/config/ouster_warthog_learned.yaml b/config/ouster_warthog_learned.yaml new file mode 100644 index 000000000..cb27c5a97 --- /dev/null +++ b/config/ouster_warthog_learned.yaml @@ -0,0 +1,378 @@ +/**: + ros__parameters: + log_to_file: true + log_debug: true + log_enabled: + # navigator + - navigation + #- navigation.graph_map_server + #- navigation.command + + # tactic + #- tactic + - tactic.pipeline + #- tactic.module + #- tactic.module.live_mem_manager + #- tactic.module.graph_mem_manager + + # path planner + #- path_planning + - path_planning.cbit + #- path_planning.cbit_planner + - mpc.cbit + #- mpc_debug + #- obstacle detection.cbit + #- grizzly_controller_tests.cbit + - ouster + + # mission planner + #- mission.server + #- mission.state_machine + + # pose graph + #- pose_graph + + # lidar pipeline + - lidar.pipeline + #- lidar.preprocessing + #- lidar.ouster_converter + - lidar.odometry_icp + #- lidar.odometry_map_maintenance + - lidar.vertex_test + #- lidar.localization_map_recall + - lidar.localization_icp + - lidar.intra_exp_merging + #- lidar.dynamic_detection + - lidar.inter_exp_merging + - lidar.range + - lidar.costamp_inflation + #- lidar.ground_extraction + #- lidar.obstacle_detection + #- lidar.terrain_assessment + + robot_frame: base_link + env_info_topic: env_info + lidar_frame: os_sensor + lidar_topic: /ouster/points + queue_size: 1 + graph_map: + origin_lat: 43.7822 + origin_lng: -79.4661 + origin_theta: 1.5 + scale: 1.0 + tactic: + enable_parallelization: true + preprocessing_skippable: false + odometry_mapping_skippable: false + localization_skippable: false + task_queue_num_threads: 1 + task_queue_size: -1 + + route_completion_translation_threshold: 0.5 + + chain: + min_cusp_distance: 1.5 + angle_weight: 7.0 + search_depth: 5 + search_back_depth: 10 + distance_warning: 5.0 + save_odometry_result: true + save_localization_result: true + visualize: true + rviz_loc_path_offset: + - 0.0 + - 0.0 + - 0.0 + pipeline: + type: lidar + preprocessing: + - conversion + - filtering + odometry: + - icp + - mapping + - vertex_test + - intra_exp_merging + - dynamic_detection + - inter_exp_merging + - memory + localization: + - recall + - icp + - safe_corridor + - range_change + - costmap_inflation + - memory + submap_translation_threshold: 1.5 + submap_rotation_threshold: 30.0 + preprocessing: + conversion: + type: lidar.ouster_converter + visualize: false + filter_warthog: true + filter_z_min: -0.2 + filter_z_max: 0.35 + filter_radius: 0.8 + filtering: + type: lidar.preprocessing + num_threads: 8 + crop_range: 40.0 + + frame_voxel_size: 0.2 # grid subsampling voxel size + nn_voxel_size: 0.05 + + vertical_angle_res: 0.0061365 # vertical angle resolution in radians, equal to 0.3516 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: 2.0 # scale down yaw(phi) by this value so that vertical resolution ~= horizontal resolution, horizontal resolution when 5Hz spin frequence is ~0.7031 degree, so 0.7031 / 0.3516 = 2.00 + + num_sample1: 20000 # max number of sample after filtering based on planarity + min_norm_score1: 0.95 # min planarity score + + num_sample2: 20000 # 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: 20000 # 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 + traj_lock_prev_vel: false + traj_qc_diag: + - 1.0 + - 0.1 + - 0.1 + - 0.1 + - 0.1 + - 1.0 + num_threads: 8 + first_num_steps: 2 + initial_max_iter: 4 + initial_max_pairing_dist: 1.5 + initial_max_planar_dist: 1.0 + refined_max_iter: 50 + refined_max_pairing_dist: 1.0 + refined_max_planar_dist: 0.3 + averaging_num_steps: 2 + verbose: false + max_iterations: 1 + min_matched_ratio: 0.5 + visualize: true + mapping: + type: lidar.odometry_map_maintenance_v2 + + map_voxel_size: 0.2 + + crop_range_front: 40.0 + back_over_front_ratio: 0.5 + point_life_time: 20.0 + visualize: true + vertex_test: + type: lidar.vertex_test + + max_translation: 0.3 + max_rotation: 10.0 + intra_exp_merging: + type: lidar.intra_exp_merging_v2 + depth: 6.0 + + map_voxel_size: 0.2 + + 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.0122718 # 0.02042 + vertical_resolution: 0.00613587 # 0.01326 + max_num_observations: 2000 + min_num_observations: 4 + dynamic_threshold: 0.3 + visualize: true + inter_exp_merging: + type: "lidar.inter_exp_merging_v2" + + map_voxel_size: 0.2 + max_num_experiences: 128 + distance_threshold: 0.6 + planar_threshold: 0.2 + normal_threshold: 0.8 + dynamic_obs_threshold: 1 + visualize: true + memory: + type: live_mem_manager + window_size: 5 + localization: + recall: + type: lidar.localization_map_recall + map_version: pointmap + visualize: true + icp: + type: lidar.localization_icp + use_pose_prior: true + num_threads: 8 + first_num_steps: 2 + initial_max_iter: 4 + initial_max_pairing_dist: 1.5 + initial_max_planar_dist: 1.0 + refined_max_iter: 50 + refined_max_pairing_dist: 1.0 + refined_max_planar_dist: 0.3 + averaging_num_steps: 2 + verbose: false + max_iterations: 1 + min_matched_ratio: 0.5 + safe_corridor: + type: lidar.safe_corridor + lookahead_distance: 5.0 + corridor_width: 3.5 + influence_distance: 1.0 + resolution: 0.25 + size_x: 16.0 + size_y: 8.0 + visualize: true + range_change: + type: torch.range_change + use_gpu: true + abs_filepath: false + filepath: exported_model_flat_10m.pt + resolution: 0.25 + size_x: 16.0 + size_y: 16.0 + visualize: true + costmap_inflation: + type: lidar.costmap_inflation + influence_distance: 0.5 # was 0.5 Jordy # Note that the total distance where grid cells have values > 0 is min dist + influence dist, not influence dist! + minimum_distance: 0.8 # was 0.3 Jordy + resolution: 0.25 + costmap_history_size: 20 + size_x: 16.0 + size_y: 16.0 + visualize: true + memory: + type: graph_mem_manager + vertex_life_span: 5 + window_size: 3 + obstacle_detection: + type: lidar.obstacle_detection + z_min: 0.5 + z_max: 2.0 + resolution: 0.6 + size_x: 40.0 + size_y: 20.0 + run_async: true + visualize: true + ground_extraction: + type: lidar.ground_extraction + z_offset: 0.2 + alpha: 0.035 + tolerance: 0.25 + Tm: 0.3 + Tm_small: 0.1 + Tb: 0.5 + Trmse: 0.1 + Tdprev: 1.0 + rmin: 2.0 + num_bins_small: 30.0 + bin_size_small: 0.5 + num_bins_large: 10.0 + bin_size_large: 1.0 + resolution: 0.6 + size_x: 40.0 + size_y: 20.0 + run_async: true + visualize: true + terrain_assessment: + type: lidar.terrain_assessment + lookahead_distance: 15.0 + corridor_width: 1.0 + search_radius: 1.0 + resolution: 0.5 + size_x: 40.0 + size_y: 20.0 + run_online: false + run_async: true + visualize: true + + path_planning: + type: cbit.lidar # cbit for obstacle free path tracker, cbit.lidar for obstacle avoidance version + control_period: 100 # ms + cbit: + obs_padding: 0.0 + curv_to_euclid_discretization: 10 + sliding_window_width: 12.0 + sliding_window_freespace_padding: 0.5 + corridor_resolution: 0.2 + state_update_freq: 2.0 + update_state: true + rand_seed: 1 + + # Planner Tuning Params + initial_samples: 400 + batch_samples: 200 + pre_seed_resolution: 0.5 + alpha: 0.5 + q_max: 2.5 + frame_interval: 50 + iter_max: 10000000 + 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 # Note I dont think im actually using this one anymore, am using the one in the change detection v3 now + + mpc: + # Controller Params + horizon_steps: 20 + horizon_step_size: 0.3 + forward_vel: 0.6 + max_lin_vel: 1.25 + max_ang_vel: 2.0 + robot_linear_velocity_scale: 1.0 # Used to scale the output twist linear velocity messages by some constant factor to compensate internal low level control miscalibration + robot_angular_velocity_scale: 2.0 # Used to scale the output twist angular velocity messages by some constant factor to compensate internal low level control miscalibration + + vehicle_model: "unicycle" + + # Cost Function Covariance Matrices + pose_error_cov: [100.0, 100.0, 1000.0, 1000.0, 1000.0, 200.0] + vel_error_cov: [25.0, 25.0] # was [0.1 2.0] # No longer using this cost term + acc_error_cov: [10.0, 50.0] + kin_error_cov: [0.001, 0.001, 0.001, 0.001, 0.001, 0.001] + lat_error_cov: [20.0] + + # Cost Function Covariance Matrices (Tracking Backup) + #pose_error_cov: [100.0, 100.0, 1000.0, 1000.0, 1000.0, 200.0] + #vel_error_cov: [30.0, 30.0] # was [0.1 2.0] # No longer using this cost term + #acc_error_cov: [10.0, 10.0] + #kin_error_cov: [0.001, 0.001, 0.001, 0.001, 0.001, 0.001] + #lat_error_cov: [10.0] + + # Cost Function Weights + pose_error_weight: 1.0 + vel_error_weight: 1.0 + acc_error_weight: 1.0 + kin_error_weight: 1.0 + lat_error_weight: 0.01 + + #backup for tracking mpc + #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/launch/offline_ouster_warthog.launch.yaml b/launch/offline_ouster_warthog.launch.yaml new file mode 100644 index 000000000..06ebd76a0 --- /dev/null +++ b/launch/offline_ouster_warthog.launch.yaml @@ -0,0 +1,44 @@ +## Online Lidar VTR3 +session_name: vtr_online_ouster_grizzly + +environment: + CYCLONEDDS_URI: ${VTRSRC}/config/cyclonedds.default.xml # custom dds configs + # ROS_DOMAIN_ID: "1" # set this to a unique number when multiple ROS2 dependent system running on the same network + +start_directory: ${VTRTEMP} +suppress_history: false + +windows: + - window_name: vtr_navigation_system + layout: main-horizontal + shell_command_before: + - source ${VTRSRC}/main/install/setup.bash + panes: + - > + ros2 launch vtr_navigation vtr.launch.py + base_params:=ouster_warthog_learned.yaml + data_dir:=${VTRTEMP}/lidar/$(date '+%F')/$(date '+%F') + start_new_graph:=false + use_sim_time:=false + model_dir:=${VTRROOT}/models + planner:="stationary" + + #- htop # monitor hardware usage + + - window_name: vtr_gui + layout: main-horizontal + shell_command_before: + - source ${VTRSRC}/main/install/setup.bash + panes: + - ros2 run vtr_gui socket_client --ros-args -r __ns:=/vtr + - ros2 run vtr_gui socket_server --ros-args -r __ns:=/vtr + - ros2 run vtr_gui web_server --ros-args -r __ns:=/vtr + # - firefox --new-window "localhost:5200" # the webpage has to wait for everything above + + - window_name: rviz2 + layout: main-horizontal + shell_command_before: + - source ${VTRSRC}/main/install/setup.bash + panes: + - ros2 run rviz2 rviz2 -d ${VTRSRC}/rviz/ouster.rviz + # - ros2 run rqt_reconfigure rqt_reconfigure diff --git a/launch/online_ouster_warthog.launch.yaml b/launch/online_ouster_warthog.launch.yaml index 453c381c5..c172f1d49 100644 --- a/launch/online_ouster_warthog.launch.yaml +++ b/launch/online_ouster_warthog.launch.yaml @@ -16,10 +16,12 @@ windows: panes: - > ros2 launch vtr_navigation vtr.launch.py - base_params:=ouster_warthog_default.yaml + base_params:=ouster_warthog_learned.yaml data_dir:=${VTRTEMP}/lidar/$(date '+%F')/$(date '+%F') start_new_graph:=false use_sim_time:=false + model_dir:=${VTRROOT}/models + planner:="cbit.lidar" #- htop # monitor hardware usage diff --git a/main/src/vtr_lidar/include/vtr_lidar/modules/planning/costmap_inflation_module.hpp b/main/src/vtr_lidar/include/vtr_lidar/modules/planning/costmap_inflation_module.hpp index f798a07eb..d1e65f252 100644 --- a/main/src/vtr_lidar/include/vtr_lidar/modules/planning/costmap_inflation_module.hpp +++ b/main/src/vtr_lidar/include/vtr_lidar/modules/planning/costmap_inflation_module.hpp @@ -77,12 +77,13 @@ class CostmapInflationModule : public tactic::BaseModule { bool publisher_initialized_ = false; rclcpp::Publisher::SharedPtr costmap_pub_; rclcpp::Publisher::SharedPtr costpcd_pub_; - - VTR_REGISTER_MODULE_DEC_TYPE(CostmapInflationModule); + rclcpp::Publisher::SharedPtr concat_pc_pub_; - // Modificatons for Temporal costmap filter std::list>> detected_history; + + + VTR_REGISTER_MODULE_DEC_TYPE(CostmapInflationModule); }; } // namespace lidar diff --git a/main/src/vtr_lidar/include/vtr_lidar/modules/planning/rangenet_change_detection_module.hpp b/main/src/vtr_lidar/include/vtr_lidar/modules/planning/rangenet_change_detection_module.hpp index fedc67947..217e41a31 100644 --- a/main/src/vtr_lidar/include/vtr_lidar/modules/planning/rangenet_change_detection_module.hpp +++ b/main/src/vtr_lidar/include/vtr_lidar/modules/planning/rangenet_change_detection_module.hpp @@ -58,13 +58,6 @@ class RangeChangeNetModule : public nn::TorchModule { float fov_down = 0; float range_crop = 0; - unsigned int costmap_history_size = 10; - float resolution = 1.0; - float size_x = 20.0; - float size_y = 20.0; - float influence_distance = 1.0; - float minimum_distance = 0.5; - static ConstPtr fromROS(const rclcpp::Node::SharedPtr &node, const std::string ¶m_prefix); @@ -100,12 +93,8 @@ class RangeChangeNetModule : public nn::TorchModule { rclcpp::Publisher::SharedPtr live_range_pub_; rclcpp::Publisher::SharedPtr map_range_pub_; rclcpp::Publisher::SharedPtr diffpcd_pub_; - rclcpp::Publisher::SharedPtr mappcd_pub_; - rclcpp::Publisher::SharedPtr costmap_pub_; bool pub_init = false; - std::list, float>> costmap_history; - VTR_REGISTER_MODULE_DEC_TYPE(RangeChangeNetModule); diff --git a/main/src/vtr_lidar/src/modules/planning/costmap_inflation_module.cpp b/main/src/vtr_lidar/src/modules/planning/costmap_inflation_module.cpp index cd7d0b9d9..f49012237 100644 --- a/main/src/vtr_lidar/src/modules/planning/costmap_inflation_module.cpp +++ b/main/src/vtr_lidar/src/modules/planning/costmap_inflation_module.cpp @@ -101,6 +101,7 @@ void CostmapInflationModule::run_(QueryCache &qdata0, OutputCache &output0, // clang-format off costmap_pub_ = qdata.node->create_publisher("change_detection_costmap", 5); costpcd_pub_ = qdata.node->create_publisher("change_detection_costpcd", 5); + concat_pc_pub_ = qdata.node->create_publisher("change_detection_concat", 5); // clang-format on publisher_initialized_ = true; } @@ -181,7 +182,12 @@ void CostmapInflationModule::run_(QueryCache &qdata0, OutputCache &output0, pointcloud_msg.header.frame_id = "loc vertex frame"; // pointcloud_msg.header.stamp = rclcpp::Time(*qdata.stamp); costpcd_pub_->publish(pointcloud_msg); - + + PointCloudMsg concat_msg; + pcl::toROSMsg(concat_pc, concat_msg); + concat_msg.header.frame_id = "loc vertex frame"; + concat_msg.header.stamp = rclcpp::Time(*qdata.stamp); + concat_pc_pub_->publish(concat_msg); } /// output diff --git a/main/src/vtr_lidar/src/modules/planning/rangenet_change_detection_module.cpp b/main/src/vtr_lidar/src/modules/planning/rangenet_change_detection_module.cpp index ac65df3ce..7e2fa54c3 100644 --- a/main/src/vtr_lidar/src/modules/planning/rangenet_change_detection_module.cpp +++ b/main/src/vtr_lidar/src/modules/planning/rangenet_change_detection_module.cpp @@ -65,15 +65,6 @@ auto RangeChangeNetModule::Config::fromROS(const rclcpp::Node::SharedPtr &node, config->fov_down = node->declare_parameter(param_prefix + ".fov_down", config->fov_down); config->range_crop = node->declare_parameter(param_prefix + ".range_crop", config->range_crop); - // cost map - config->costmap_history_size = (unsigned) 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); - config->influence_distance = node->declare_parameter(param_prefix + ".influence_distance", config->influence_distance); - config->minimum_distance = node->declare_parameter(param_prefix + ".minimum_distance", config->minimum_distance); - - // clang-format on return config; } @@ -104,7 +95,6 @@ void RangeChangeNetModule::run_(QueryCache &qdata0, OutputCache &output0, live_range_pub_ = qdata.node->create_publisher("live_range_image", 5); map_range_pub_ = qdata.node->create_publisher("map_range_image", 5); diffpcd_pub_ = qdata.node->create_publisher("detection_cloud", 5); - costmap_pub_ = qdata.node->create_publisher("change_detection_costmap", 5); } auto& sub_map= *qdata.submap_loc; @@ -128,7 +118,7 @@ void RangeChangeNetModule::run_(QueryCache &qdata0, OutputCache &output0, RangeImageParams image_params; image_params.fov_up = 20 * M_PI / 180; image_params.fov_down = -5 * M_PI / 180; - image_params.crop_range = 8.0; + image_params.crop_range = 10.0; RowMatrixXf scan_image = Eigen::MatrixXf::Constant(64, 1024, -1); RowMatrixXf mask_image = Eigen::MatrixXf::Zero(64, 1024); @@ -159,9 +149,9 @@ void RangeChangeNetModule::run_(QueryCache &qdata0, OutputCache &output0, unproject_range_image(nn_point_cloud, mask_image, scan_idxs); - // project to 2d and construct the grid map - const auto costmap = std::make_shared( - config_->resolution, config_->size_x, config_->size_y); + // // project to 2d and construct the grid map + // const auto costmap = std::make_shared( + // config_->resolution, config_->size_x, config_->size_y); // filter out non-obstacle points std::vector indices; @@ -170,31 +160,38 @@ void RangeChangeNetModule::run_(QueryCache &qdata0, OutputCache &output0, if (nn_point_cloud[i].flex24 > 1.5) indices.emplace_back(i); } pcl::PointCloud obstacle_points(nn_point_cloud, indices); - std::unordered_map values; - for (size_t i = 0; i < obstacle_points.size(); ++i) { - const auto pix_coord = pointToKey(obstacle_points[i]); - if (values.find(pix_coord) != values.end()) { - values[pix_coord] += 0.2; - } else { - values[pix_coord] = 0.2; - } - } + auto obstacle_points_mat = obstacle_points.getMatrixXfMap(4, PointWithInfo::size(), PointWithInfo::cartesian_offset()); + obstacle_points_mat = T_r_v_loc.inverse().matrix().cast() * obstacle_points_mat; + + qdata.changed_points.emplace(obstacle_points); + // obstacle_history_.push_front(std::make_pair(vid_loc, obstacle_points)); + // std::unordered_map values; + // for (size_t i = 0; i < obstacle_points.size(); ++i) { + // const auto pix_coord = pointToKey(obstacle_points[i]); + // if (values.find(pix_coord) != values.end()) { + // values[pix_coord] += 0.2; + // } else { + // values[pix_coord] = 0.2; + // } + // } - costmap->update(values); - const auto filtered_costmap = std::make_shared( - config_->resolution, config_->size_x, config_->size_y); - filtered_costmap->update(costmap->filter(1.0)); - // add transform to the localization vertex - filtered_costmap->T_vertex_this() = T_r_v_loc.inverse(); - filtered_costmap->vertex_id() = vid_loc; - filtered_costmap->vertex_sid() = sid_loc; - auto change_detection_costmap_ref = output.change_detection_costmap.locked(); - auto &change_detection_costmap = change_detection_costmap_ref.get(); - change_detection_costmap = filtered_costmap; + // costmap->update(values); + + // const auto filtered_costmap = std::make_shared( + // config_->resolution, config_->size_x, config_->size_y); + // filtered_costmap->update(costmap->filter(1.0)); + // // add transform to the localization vertex + // filtered_costmap->T_vertex_this() = T_r_v_loc.inverse(); + // filtered_costmap->vertex_id() = vid_loc; + // filtered_costmap->vertex_sid() = sid_loc; + + // auto change_detection_costmap_ref = output.change_detection_costmap.locked(); + // auto &change_detection_costmap = change_detection_costmap_ref.get(); + // change_detection_costmap = filtered_costmap; /// publish the transformed pointcloud if (config_->visualize) { @@ -202,18 +199,10 @@ void RangeChangeNetModule::run_(QueryCache &qdata0, OutputCache &output0, PointCloudMsg filter_msg; pcl::toROSMsg(obstacle_points, filter_msg); - filter_msg.header.frame_id = "odo vertex frame"; + filter_msg.header.frame_id = "loc vertex frame"; filter_msg.header.stamp = rclcpp::Time(*qdata.stamp); diffpcd_pub_->publish(filter_msg); - // publish the occupancy grid - auto costmap_msg = filtered_costmap->toCostMapMsg(); - costmap_msg.header.frame_id = "odo vertex frame"; - costmap_msg.header.stamp = rclcpp::Time(*qdata.stamp); - CLOG(DEBUG, "lidar.range") << "Publishing costmap"; - costmap_pub_->publish(costmap_msg); - - mask_pub_->publish(range_to_image(mask_image)); live_range_pub_->publish(range_to_image(scan_image)); map_range_pub_->publish(range_to_image(map_image)); @@ -222,9 +211,9 @@ void RangeChangeNetModule::run_(QueryCache &qdata0, OutputCache &output0, } -costmap::PixKey RangeChangeNetModule::pointToKey(PointWithInfo &p) { - return {std::lround(p.x / config_->resolution), std::lround(p.y / config_->resolution)}; - } +// costmap::PixKey RangeChangeNetModule::pointToKey(PointWithInfo &p) { +// return {std::lround(p.x / config_->resolution), std::lround(p.y / config_->resolution)}; +// } } // namespace nn } // namespace vtr \ No newline at end of file diff --git a/rviz/ouster.rviz b/rviz/ouster.rviz index 2ec92ad1f..7d271ea62 100644 --- a/rviz/ouster.rviz +++ b/rviz/ouster.rviz @@ -8,10 +8,8 @@ Panels: - /TF1 - /TF1/Frames1 - /odometry path1/Shape1 - - /localization path1 - /localization path1/Topic1 - /curr map loc1/Topic1 - - /reference path1 - /reference path1/Topic1 - /planned path1/Offset1 - /planned path (poses)1/Topic1 @@ -21,6 +19,8 @@ Panels: - /Corridor Path Left1/Topic1 - /PoseArray1 - /PoseArray2 + - /PointCloud21 + - /PointCloud21/Status1 Splitter Ratio: 0.4507042169570923 Tree Height: 871 - Class: rviz_common/Selection @@ -40,7 +40,7 @@ Panels: Experimental: false Name: Time SyncMode: 0 - SyncSource: "" + SyncSource: PointCloud2 Visualization Manager: Class: "" Displays: @@ -95,13 +95,13 @@ Visualization Manager: lidar: Value: false loc vertex frame: - Value: false + Value: true loc vertex frame (offset): - Value: false + Value: true novatel: Value: true odo vertex frame: - Value: false + Value: true odom: Value: true os_imu: @@ -114,8 +114,6 @@ Visualization Manager: Value: true os_sensor_baseplate: Value: true - planning frame: - Value: false rear_left_wheel_link: Value: true rear_right_wheel_link: @@ -128,12 +126,6 @@ Visualization Manager: Value: true robot: Value: true - robot planning: - Value: false - robot planning (extrapolated): - Value: false - robot planning (extrapolated) mpc: - Value: false sensor_tower: Value: true stereo_camera: @@ -157,13 +149,6 @@ Visualization Manager: robot: lidar: {} - planning frame: - robot planning: - {} - robot planning (extrapolated): - {} - robot planning (extrapolated) mpc: - {} world (offset): loc vertex frame (offset): {} @@ -483,74 +468,6 @@ Visualization Manager: Reliability Policy: Reliable Value: /vtr/teb_poses Value: true - - Alpha: 1 - Autocompute Intensity Bounds: false - Autocompute Value Bounds: - Max Value: 10 - Min Value: -10 - Value: true - Axis: Z - Channel Name: flex24 - Class: rviz_default_plugins/PointCloud2 - Color: 255; 255; 0 - Color Transformer: Intensity - Decay Time: 0 - Enabled: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 2 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: change detection input - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.10000000149011612 - Style: Flat Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /vtr/change_detection_fake_pcd - Use Fixed Frame: true - Use rainbow: true - Value: false - - Alpha: 1 - Autocompute Intensity Bounds: false - Autocompute Value Bounds: - Max Value: 10 - Min Value: -10 - Value: true - Axis: Z - Channel Name: flex24 - Class: rviz_default_plugins/PointCloud2 - Color: 255; 255; 0 - Color Transformer: Intensity - Decay Time: 0 - Enabled: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 2 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: change detection scan - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.10000000149011612 - Style: Boxes - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /vtr/change_detection_scan - Use Fixed Frame: true - Use rainbow: true - Value: false - Alpha: 0.20000000298023224 Autocompute Intensity Bounds: false Autocompute Value Bounds: @@ -687,40 +604,6 @@ Visualization Manager: Use Fixed Frame: true Use rainbow: true Value: false - - Alpha: 1 - Autocompute Intensity Bounds: false - Autocompute Value Bounds: - Max Value: 10 - Min Value: -10 - Value: true - Axis: Z - Channel Name: flex24 - Class: rviz_default_plugins/PointCloud2 - Color: 255; 255; 255 - Color Transformer: Intensity - Decay Time: 0 - Enabled: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 2 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: temp - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.10000000149011612 - Style: Flat Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /vtr/fake_pcd - Use Fixed Frame: true - Use rainbow: true - Value: false - Alpha: 1 Autocompute Intensity Bounds: true Autocompute Value Bounds: @@ -1110,40 +993,6 @@ Visualization Manager: Use Fixed Frame: true Use rainbow: true Value: false - - Alpha: 1 - Autocompute Intensity Bounds: false - Autocompute Value Bounds: - Max Value: 10 - Min Value: -10 - Value: true - Axis: Z - Channel Name: flex24 - Class: rviz_default_plugins/PointCloud2 - Color: 255; 255; 0 - Color Transformer: Intensity - Decay Time: 0 - Enabled: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 2 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: planning fake obs. scan - Position Transformer: XYZ - Selectable: true - Size (Pixels): 2.5 - Size (m): 0.07999999821186066 - Style: Points - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /vtr/fake_obstacle_scan - Use Fixed Frame: true - Use rainbow: true - Value: false - Alpha: 0.800000011920929 Autocompute Intensity Bounds: false Autocompute Value Bounds: @@ -1442,6 +1291,40 @@ Visualization Manager: Reliability Policy: Reliable Value: /vtr/mpc_ref_pose_array2 Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: PointCloud2 + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.05000000074505806 + Style: Flat Squares + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /vtr/change_detection_concat + Use Fixed Frame: true + Use rainbow: true + Value: true Enabled: true Global Options: Background Color: 189; 189; 189 @@ -1488,7 +1371,7 @@ Visualization Manager: Views: Current: Class: rviz_default_plugins/Orbit - Distance: 41.55506134033203 + Distance: 54.51273727416992 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 @@ -1503,10 +1386,10 @@ Visualization Manager: Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: 1.5697963237762451 + Pitch: 0.8647971153259277 Target Frame: robot Value: Orbit (rviz_default_plugins) - Yaw: 3.441279411315918 + Yaw: 4.0012898445129395 Saved: - Class: rviz_default_plugins/Orbit Distance: 111.39698028564453 @@ -1724,5 +1607,5 @@ Window Geometry: Views: collapsed: true Width: 1848 - X: 72 + X: 78 Y: 27 From e4f431d0313cfd8fc75922f6c080e8f2d51badf4 Mon Sep 17 00:00:00 2001 From: a-krawciw Date: Sat, 26 Aug 2023 08:04:12 -0400 Subject: [PATCH 46/77] Add radius filter to learned pipeline --- config/ouster_warthog_learned.yaml | 11 ++++--- .../rangenet_change_detection_module.hpp | 3 ++ .../rangenet_change_detection_module.cpp | 30 +++++++++++++++---- 3 files changed, 32 insertions(+), 12 deletions(-) diff --git a/config/ouster_warthog_learned.yaml b/config/ouster_warthog_learned.yaml index cb27c5a97..8023a0b41 100644 --- a/config/ouster_warthog_learned.yaml +++ b/config/ouster_warthog_learned.yaml @@ -246,16 +246,15 @@ use_gpu: true abs_filepath: false filepath: exported_model_flat_10m.pt - resolution: 0.25 - size_x: 16.0 - size_y: 16.0 + radius_filter: 0.25 + neighbourhood: 5 visualize: true costmap_inflation: type: lidar.costmap_inflation - influence_distance: 0.5 # was 0.5 Jordy # Note that the total distance where grid cells have values > 0 is min dist + influence dist, not influence dist! - minimum_distance: 0.8 # was 0.3 Jordy + influence_distance: 0.8 # was 0.5 Jordy # Note that the total distance where grid cells have values > 0 is min dist + influence dist, not influence dist! + minimum_distance: 0.6 # was 0.3 Jordy resolution: 0.25 - costmap_history_size: 20 + costmap_history_size: 15 size_x: 16.0 size_y: 16.0 visualize: true diff --git a/main/src/vtr_lidar/include/vtr_lidar/modules/planning/rangenet_change_detection_module.hpp b/main/src/vtr_lidar/include/vtr_lidar/modules/planning/rangenet_change_detection_module.hpp index 217e41a31..ebb6c04b9 100644 --- a/main/src/vtr_lidar/include/vtr_lidar/modules/planning/rangenet_change_detection_module.hpp +++ b/main/src/vtr_lidar/include/vtr_lidar/modules/planning/rangenet_change_detection_module.hpp @@ -57,6 +57,9 @@ class RangeChangeNetModule : public nn::TorchModule { float fov_up = 0; float fov_down = 0; float range_crop = 0; + int neighbourhood = 1; + + float radius_filter = 0.1; static ConstPtr fromROS(const rclcpp::Node::SharedPtr &node, const std::string ¶m_prefix); diff --git a/main/src/vtr_lidar/src/modules/planning/rangenet_change_detection_module.cpp b/main/src/vtr_lidar/src/modules/planning/rangenet_change_detection_module.cpp index 7e2fa54c3..ab0af35f8 100644 --- a/main/src/vtr_lidar/src/modules/planning/rangenet_change_detection_module.cpp +++ b/main/src/vtr_lidar/src/modules/planning/rangenet_change_detection_module.cpp @@ -64,6 +64,7 @@ auto RangeChangeNetModule::Config::fromROS(const rclcpp::Node::SharedPtr &node, config->fov_up = node->declare_parameter(param_prefix + ".fov_up", config->fov_up); config->fov_down = node->declare_parameter(param_prefix + ".fov_down", config->fov_down); config->range_crop = node->declare_parameter(param_prefix + ".range_crop", config->range_crop); + config->neighbourhood = node->declare_parameter(param_prefix + ".neighbourhood", config->neighbourhood); // clang-format on return config; @@ -149,10 +150,6 @@ void RangeChangeNetModule::run_(QueryCache &qdata0, OutputCache &output0, unproject_range_image(nn_point_cloud, mask_image, scan_idxs); - // // project to 2d and construct the grid map - // const auto costmap = std::make_shared( - // config_->resolution, config_->size_x, config_->size_y); - // filter out non-obstacle points std::vector indices; indices.reserve(nn_point_cloud.size()); @@ -163,9 +160,30 @@ void RangeChangeNetModule::run_(QueryCache &qdata0, OutputCache &output0, auto obstacle_points_mat = obstacle_points.getMatrixXfMap(4, PointWithInfo::size(), PointWithInfo::cartesian_offset()); obstacle_points_mat = T_r_v_loc.inverse().matrix().cast() * obstacle_points_mat; - qdata.changed_points.emplace(obstacle_points); + NanoFLANNAdapter adapter(obstacle_points); + KDTreeSearchParams search_params; + KDTreeParams tree_params(10); + auto kdtree = std::make_unique>(3, adapter, tree_params); + kdtree->buildIndex(); + + std::vector radii_indices; + radii_indices.reserve(obstacle_points.size()); + + const auto sq_search_radius = config_->radius_filter * config_->radius_filter; + for (size_t i = 0; i < obstacle_points.size(); i++) { + // radius search of the closest point + std::vector dists; + std::vector indices; + NanoFLANNRadiusResultSet result(sq_search_radius, dists, indices); + kdtree->radiusSearchCustomCallback(obstacle_points[i].data, result, search_params); + + // filter based on neighbors in map + if (indices.size() > config_->neighbourhood) + radii_indices.push_back(i); + } - // obstacle_history_.push_front(std::make_pair(vid_loc, obstacle_points)); + pcl::PointCloud radius_filtered_points(obstacle_points, radii_indices); + qdata.changed_points.emplace(radius_filtered_points); // std::unordered_map values; // for (size_t i = 0; i < obstacle_points.size(); ++i) { From 05a35253dc69bd9fb5a283a538f11a16e7d5aec3 Mon Sep 17 00:00:00 2001 From: a-krawciw Date: Mon, 28 Aug 2023 13:24:12 -0400 Subject: [PATCH 47/77] Change obstacle inflation --- config/ouster_warthog_default.yaml | 4 +- config/ouster_warthog_learned.yaml | 15 +- launch/offline_ouster_warthog.launch.yaml | 4 +- launch/online_ouster_warthog.launch.yaml | 4 +- .../include/vtr_lidar/modules/modules.hpp | 2 + .../planning/blindspot_inflation_module.hpp | 77 ++++++++++ .../planning/costmap_inflation_module.hpp | 9 +- .../planning/queued_inflation_module.hpp | 81 ++++++++++ .../planning/blindspot_inflation_module.cpp | 139 ++++++++++++++++++ .../planning/costmap_inflation_module.cpp | 51 ++----- .../planning/queued_inflation_module.cpp | 121 +++++++++++++++ .../rangenet_change_detection_module.cpp | 27 +--- .../conversions/ouster_conversion_module.cpp | 2 +- main/src/vtr_navigation/launch/vtr.launch.py | 3 +- 14 files changed, 452 insertions(+), 87 deletions(-) create mode 100644 main/src/vtr_lidar/include/vtr_lidar/modules/planning/blindspot_inflation_module.hpp create mode 100644 main/src/vtr_lidar/include/vtr_lidar/modules/planning/queued_inflation_module.hpp create mode 100644 main/src/vtr_lidar/src/modules/planning/blindspot_inflation_module.cpp create mode 100644 main/src/vtr_lidar/src/modules/planning/queued_inflation_module.cpp diff --git a/config/ouster_warthog_default.yaml b/config/ouster_warthog_default.yaml index fb06338c4..d678f0bd5 100644 --- a/config/ouster_warthog_default.yaml +++ b/config/ouster_warthog_default.yaml @@ -227,7 +227,7 @@ averaging_num_steps: 2 verbose: false max_iterations: 1 - min_matched_ratio: 0.65 + min_matched_ratio: 0.45 safe_corridor: type: lidar.safe_corridor lookahead_distance: 5.0 @@ -340,7 +340,7 @@ # Controller Params horizon_steps: 20 horizon_step_size: 0.3 - forward_vel: 0.6 + forward_vel: 0.75 max_lin_vel: 1.25 max_ang_vel: 2.0 robot_linear_velocity_scale: 1.0 # Used to scale the output twist linear velocity messages by some constant factor to compensate internal low level control miscalibration diff --git a/config/ouster_warthog_learned.yaml b/config/ouster_warthog_learned.yaml index 8023a0b41..2c025d3e5 100644 --- a/config/ouster_warthog_learned.yaml +++ b/config/ouster_warthog_learned.yaml @@ -46,6 +46,7 @@ - lidar.inter_exp_merging - lidar.range - lidar.costamp_inflation + - lidar.obstacle_inflation #- lidar.ground_extraction #- lidar.obstacle_detection #- lidar.terrain_assessment @@ -250,11 +251,13 @@ neighbourhood: 5 visualize: true costmap_inflation: - type: lidar.costmap_inflation + type: lidar.costmap_blindspot influence_distance: 0.8 # was 0.5 Jordy # Note that the total distance where grid cells have values > 0 is min dist + influence dist, not influence dist! minimum_distance: 0.6 # was 0.3 Jordy resolution: 0.25 costmap_history_size: 15 + blind_spot_radius: 2.5 + lifetime: 1.5 size_x: 16.0 size_y: 16.0 visualize: true @@ -336,20 +339,20 @@ mpc: # Controller Params - horizon_steps: 20 + horizon_steps: 30 horizon_step_size: 0.3 forward_vel: 0.6 max_lin_vel: 1.25 - max_ang_vel: 2.0 + max_ang_vel: 1.0 robot_linear_velocity_scale: 1.0 # Used to scale the output twist linear velocity messages by some constant factor to compensate internal low level control miscalibration robot_angular_velocity_scale: 2.0 # Used to scale the output twist angular velocity messages by some constant factor to compensate internal low level control miscalibration vehicle_model: "unicycle" # Cost Function Covariance Matrices - pose_error_cov: [100.0, 100.0, 1000.0, 1000.0, 1000.0, 200.0] - vel_error_cov: [25.0, 25.0] # was [0.1 2.0] # No longer using this cost term - acc_error_cov: [10.0, 50.0] + pose_error_cov: [100.0, 100.0, 1000.0, 1000.0, 1000.0, 50.0] + vel_error_cov: [25.0, 5.0] + acc_error_cov: [10.0, 5.0] kin_error_cov: [0.001, 0.001, 0.001, 0.001, 0.001, 0.001] lat_error_cov: [20.0] diff --git a/launch/offline_ouster_warthog.launch.yaml b/launch/offline_ouster_warthog.launch.yaml index 06ebd76a0..2ef56333d 100644 --- a/launch/offline_ouster_warthog.launch.yaml +++ b/launch/offline_ouster_warthog.launch.yaml @@ -17,11 +17,11 @@ windows: - > ros2 launch vtr_navigation vtr.launch.py base_params:=ouster_warthog_learned.yaml - data_dir:=${VTRTEMP}/lidar/$(date '+%F')/$(date '+%F') + data_dir:=${VTRTEMP}/lidar start_new_graph:=false use_sim_time:=false model_dir:=${VTRROOT}/models - planner:="stationary" + planner:=stationary #- htop # monitor hardware usage diff --git a/launch/online_ouster_warthog.launch.yaml b/launch/online_ouster_warthog.launch.yaml index c172f1d49..079f3c726 100644 --- a/launch/online_ouster_warthog.launch.yaml +++ b/launch/online_ouster_warthog.launch.yaml @@ -16,12 +16,12 @@ windows: panes: - > ros2 launch vtr_navigation vtr.launch.py - base_params:=ouster_warthog_learned.yaml + base_params:=ouster_warthog_default.yaml data_dir:=${VTRTEMP}/lidar/$(date '+%F')/$(date '+%F') start_new_graph:=false use_sim_time:=false model_dir:=${VTRROOT}/models - planner:="cbit.lidar" + planner:="cbit" #- htop # monitor hardware usage diff --git a/main/src/vtr_lidar/include/vtr_lidar/modules/modules.hpp b/main/src/vtr_lidar/include/vtr_lidar/modules/modules.hpp index 51409ced0..de20afe7e 100644 --- a/main/src/vtr_lidar/include/vtr_lidar/modules/modules.hpp +++ b/main/src/vtr_lidar/include/vtr_lidar/modules/modules.hpp @@ -50,4 +50,6 @@ #include "vtr_lidar/modules/planning/diff_generator.hpp" #include "vtr_lidar/modules/planning/rangenet_change_detection_module.hpp" #include "vtr_lidar/modules/planning/costmap_inflation_module.hpp" +#include "vtr_lidar/modules/planning/blindspot_inflation_module.hpp" +#include "vtr_lidar/modules/planning/queued_inflation_module.hpp" diff --git a/main/src/vtr_lidar/include/vtr_lidar/modules/planning/blindspot_inflation_module.hpp b/main/src/vtr_lidar/include/vtr_lidar/modules/planning/blindspot_inflation_module.hpp new file mode 100644 index 000000000..bf7f77195 --- /dev/null +++ b/main/src/vtr_lidar/include/vtr_lidar/modules/planning/blindspot_inflation_module.hpp @@ -0,0 +1,77 @@ +// Copyright 2023, 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 blindspot_inflation_module.hpp + * \author Alec Krawciw, Yuchen Wu, Autonomous Space Robotics Lab (ASRL) + */ +#pragma once + +#include "tf2/convert.h" +#include "tf2_eigen/tf2_eigen.hpp" + +#include "vtr_lidar/cache.hpp" +#include "vtr_lidar/modules/planning/costmap_inflation_module.hpp" + +namespace vtr { +namespace lidar { + +class BlindspotCostmapModule : public CostmapInflationModule { + public: + PTR_TYPEDEFS(BlindspotCostmapModule); + using PointCloudMsg = sensor_msgs::msg::PointCloud2; + using OccupancyGridMsg = nav_msgs::msg::OccupancyGrid; + using VtrPointCloud = pcl::PointCloud; + + static constexpr auto static_name = "lidar.costmap_blindspot"; + + /** \brief Collection of config parameters */ + struct Config : public CostmapInflationModule::Config { + PTR_TYPEDEFS(Config); + + // cost map + float blind_spot_radius = 1.0; + double lifetime = 0.1; + + static ConstPtr fromROS(const rclcpp::Node::SharedPtr &node, + const std::string ¶m_prefix); + }; + + BlindspotCostmapModule( + const Config::ConstPtr &config, + const std::shared_ptr &module_factory = nullptr, + const std::string &name = static_name) + : CostmapInflationModule{config, module_factory, name}, config_(config) {} + + + + private: + VtrPointCloud assemble_pointcloud(tactic::QueryCache &qdata, + tactic::OutputCache &output, const tactic::Graph::Ptr &graph) override; + + Config::ConstPtr config_; + + /** \brief for visualization only */ + bool publisher_initialized_ = false; + rclcpp::Publisher::SharedPtr concat_pc_pub_; + + VtrPointCloud all_points_; + unsigned active_sid_; + bool first_frame_ = true; + + VTR_REGISTER_MODULE_DEC_TYPE(BlindspotCostmapModule); +}; + +} // namespace lidar +} // namespace vtr diff --git a/main/src/vtr_lidar/include/vtr_lidar/modules/planning/costmap_inflation_module.hpp b/main/src/vtr_lidar/include/vtr_lidar/modules/planning/costmap_inflation_module.hpp index d1e65f252..04dd268b2 100644 --- a/main/src/vtr_lidar/include/vtr_lidar/modules/planning/costmap_inflation_module.hpp +++ b/main/src/vtr_lidar/include/vtr_lidar/modules/planning/costmap_inflation_module.hpp @@ -46,7 +46,6 @@ class CostmapInflationModule : public tactic::BaseModule { PTR_TYPEDEFS(Config); // cost map - unsigned int costmap_history_size = 10; float resolution = 1.0; float size_x = 20.0; float size_y = 20.0; @@ -66,6 +65,10 @@ class CostmapInflationModule : public tactic::BaseModule { const std::string &name = static_name) : tactic::BaseModule{module_factory, name}, config_(config) {} + protected: + virtual pcl::PointCloud assemble_pointcloud(tactic::QueryCache &qdata, + tactic::OutputCache &output, const tactic::Graph::Ptr &graph); + private: void run_(tactic::QueryCache &qdata, tactic::OutputCache &output, const tactic::Graph::Ptr &graph, @@ -77,10 +80,6 @@ class CostmapInflationModule : public tactic::BaseModule { bool publisher_initialized_ = false; rclcpp::Publisher::SharedPtr costmap_pub_; rclcpp::Publisher::SharedPtr costpcd_pub_; - rclcpp::Publisher::SharedPtr concat_pc_pub_; - - - std::list>> detected_history; VTR_REGISTER_MODULE_DEC_TYPE(CostmapInflationModule); diff --git a/main/src/vtr_lidar/include/vtr_lidar/modules/planning/queued_inflation_module.hpp b/main/src/vtr_lidar/include/vtr_lidar/modules/planning/queued_inflation_module.hpp new file mode 100644 index 000000000..ac04d6265 --- /dev/null +++ b/main/src/vtr_lidar/include/vtr_lidar/modules/planning/queued_inflation_module.hpp @@ -0,0 +1,81 @@ +// Copyright 2023, 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 queued_inflation_module.hpp + * \author Alec Krawciw, Yuchen Wu, Autonomous Space Robotics Lab (ASRL) + */ +#pragma once + +#include + +#include "tf2/convert.h" +#include "tf2_eigen/tf2_eigen.hpp" +#include "tf2_ros/transform_broadcaster.h" + +#include "nav_msgs/msg/occupancy_grid.hpp" + +#include "vtr_lidar/cache.hpp" +#include "vtr_lidar/modules/planning/costmap_inflation_module.hpp" + +namespace vtr { +namespace lidar { + +class QueuedCostmapModule : public CostmapInflationModule { + public: + PTR_TYPEDEFS(QueuedCostmapModule); + using PointCloudMsg = sensor_msgs::msg::PointCloud2; + using OccupancyGridMsg = nav_msgs::msg::OccupancyGrid; + using VtrPointCloud = pcl::PointCloud; + + static constexpr auto static_name = "lidar.costmap_queue"; + + /** \brief Collection of config parameters */ + struct Config : public CostmapInflationModule::Config { + PTR_TYPEDEFS(Config); + + // cost map + unsigned int costmap_history_size = 10; + + static ConstPtr fromROS(const rclcpp::Node::SharedPtr &node, + const std::string ¶m_prefix); + }; + + QueuedCostmapModule( + const Config::ConstPtr &config, + const std::shared_ptr &module_factory = nullptr, + const std::string &name = static_name) + : CostmapInflationModule{config, module_factory, name}, config_(config) {} + + + + private: + VtrPointCloud assemble_pointcloud(tactic::QueryCache &qdata, + tactic::OutputCache &output, const tactic::Graph::Ptr &graph) override; + + Config::ConstPtr config_; + + /** \brief for visualization only */ + bool publisher_initialized_ = false; + rclcpp::Publisher::SharedPtr concat_pc_pub_; + + + std::list> detected_history; + + + VTR_REGISTER_MODULE_DEC_TYPE(QueuedCostmapModule); +}; + +} // namespace lidar +} // namespace vtr diff --git a/main/src/vtr_lidar/src/modules/planning/blindspot_inflation_module.cpp b/main/src/vtr_lidar/src/modules/planning/blindspot_inflation_module.cpp new file mode 100644 index 000000000..c01d54380 --- /dev/null +++ b/main/src/vtr_lidar/src/modules/planning/blindspot_inflation_module.cpp @@ -0,0 +1,139 @@ +// Copyright 2023, 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 blindspot_inflation_module.cpp + * \author Alec Krawciw, Autonomous Space Robotics Lab (ASRL) + */ +#include "vtr_lidar/modules/planning/blindspot_inflation_module.hpp" + +#include "pcl/features/normal_3d.h" + +#include "vtr_lidar/data_types/costmap.hpp" +#include "vtr_lidar/filters/voxel_downsample.hpp" + +#include "vtr_lidar/utils/nanoflann_utils.hpp" + +namespace vtr { +namespace lidar { + + +using namespace tactic; + +auto BlindspotCostmapModule::Config::fromROS( + const rclcpp::Node::SharedPtr &node, const std::string ¶m_prefix) + -> ConstPtr { + auto config = std::make_shared(); + // clang-format off + + auto base_config = CostmapInflationModule::Config::fromROS(node, param_prefix); + + auto casted_config = + std::static_pointer_cast(config); + *casted_config = *base_config; // copy over base config + + config->blind_spot_radius = node->declare_parameter(param_prefix + ".blind_spot_radius", config->blind_spot_radius); + config->lifetime = node->declare_parameter(param_prefix + ".lifetime", config->lifetime); + + // clang-format on + return config; +} + +BlindspotCostmapModule::VtrPointCloud BlindspotCostmapModule::assemble_pointcloud(tactic::QueryCache &qdata0, + tactic::OutputCache &output0, const tactic::Graph::Ptr &) { + + auto &qdata = dynamic_cast(qdata0); + auto &output = dynamic_cast(output0); + + /// visualization setup + if (config_->visualize && !publisher_initialized_) { + // clang-format off + concat_pc_pub_ = qdata.node->create_publisher("change_detection_concat", 5); + // clang-format on + publisher_initialized_ = true; + } + + // Add reset + + if (*qdata.first_frame) { + first_frame_ = true; + all_points_.clear(); + } + + + // inputs + const auto &stamp = *qdata.stamp; + const auto &sid_loc = *qdata.sid_loc; + const auto &T_r_v_loc = *qdata.T_r_v_loc; + const auto &changed_points = *qdata.changed_points; + const auto &chain = *output.chain; + + if (!chain.isLocalized()){ + return all_points_; + } else if (first_frame_){ + active_sid_ = sid_loc; + first_frame_ = false; + CLOG(WARNING, "lidar.obstacle_inflation") << chain.T_trunk_target(active_sid_); + } + + + auto concat_pc = pcl::PointCloud(changed_points); + + const auto &T_w_v_loc = chain.pose(sid_loc); + const auto T_r_w = T_r_v_loc*(T_w_v_loc.inverse()); + + auto aligned_point_cloud = concat_pc.getMatrixXfMap(4, PointWithInfo::size(), PointWithInfo::cartesian_offset()); + aligned_point_cloud = T_w_v_loc.matrix().cast() * aligned_point_cloud; + + concat_pc += all_points_; + all_points_ = pcl::PointCloud(concat_pc); + + + auto aligned_concat_cloud = concat_pc.getMatrixXfMap(4, PointWithInfo::size(), PointWithInfo::cartesian_offset()); + aligned_concat_cloud = T_r_w.matrix().cast() * aligned_concat_cloud; + + std::vector indices; + indices.reserve(concat_pc.size()); + const float r_sq_thresh = config_->blind_spot_radius * config_->blind_spot_radius; + for (size_t i = 0; i < concat_pc.size(); ++i) { + auto &p = concat_pc[i]; + auto r_sq = p.x*p.x + p.y*p.y; + if (r_sq < r_sq_thresh) indices.emplace_back(i); + else if (abs(stamp - p.timestamp) < config_->lifetime*1e9) indices.emplace_back(i); + } + pcl::PointCloud obstacle_points(concat_pc, indices); + auto aligned_obstacle_cloud = obstacle_points.getMatrixXfMap(4, PointWithInfo::size(), PointWithInfo::cartesian_offset()); + aligned_obstacle_cloud = T_r_v_loc.inverse().matrix().cast() * aligned_obstacle_cloud; + + // pcl::PointCloud reduced_world(concat_pc, indices); + all_points_ = pcl::PointCloud(all_points_, indices); + + + + CLOG(DEBUG, "lidar.obstacle_inflation") << "Point cloud of size " << all_points_.size() << " is connected to sid: " << active_sid_; + + + /// publish the transformed pointcloud + if (config_->visualize) { + PointCloudMsg concat_msg; + pcl::toROSMsg(all_points_, concat_msg); + concat_msg.header.frame_id = "world"; + concat_msg.header.stamp = rclcpp::Time(*qdata.stamp); + concat_pc_pub_->publish(concat_msg); + } + return obstacle_points; +} + +} // namespace lidar +} // namespace vtr \ No newline at end of file diff --git a/main/src/vtr_lidar/src/modules/planning/costmap_inflation_module.cpp b/main/src/vtr_lidar/src/modules/planning/costmap_inflation_module.cpp index f49012237..3dec161d4 100644 --- a/main/src/vtr_lidar/src/modules/planning/costmap_inflation_module.cpp +++ b/main/src/vtr_lidar/src/modules/planning/costmap_inflation_module.cpp @@ -77,8 +77,6 @@ auto CostmapInflationModule::Config::fromROS( auto config = std::make_shared(); // clang-format off // cost map - int hist_size = node->declare_parameter(param_prefix + ".costmap_history_size", config->costmap_history_size); - config->costmap_history_size = (unsigned) hist_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); @@ -91,7 +89,7 @@ auto CostmapInflationModule::Config::fromROS( } void CostmapInflationModule::run_(QueryCache &qdata0, OutputCache &output0, - const Graph::Ptr & /* graph */, + const Graph::Ptr &graph, const TaskExecutor::Ptr & /* executor */) { auto &qdata = dynamic_cast(qdata0); auto &output = dynamic_cast(output0); @@ -101,7 +99,6 @@ void CostmapInflationModule::run_(QueryCache &qdata0, OutputCache &output0, // clang-format off costmap_pub_ = qdata.node->create_publisher("change_detection_costmap", 5); costpcd_pub_ = qdata.node->create_publisher("change_detection_costpcd", 5); - concat_pc_pub_ = qdata.node->create_publisher("change_detection_concat", 5); // clang-format on publisher_initialized_ = true; } @@ -111,52 +108,21 @@ void CostmapInflationModule::run_(QueryCache &qdata0, OutputCache &output0, return; } - // inputs const auto &stamp = *qdata.stamp; const auto &vid_loc = *qdata.vid_loc; const auto &sid_loc = *qdata.sid_loc; - const auto &changed_points = *qdata.changed_points; - const auto &chain = *output.chain; // clang-format off CLOG(INFO, "lidar.obstacle_inflation") << "Inflating obstacles at stamp: " << stamp; - auto concat_pc = pcl::PointCloud(); + auto concat_pc = assemble_pointcloud(qdata0, output0, graph); // project to 2d and construct the grid map const auto costmap = std::make_shared( config_->resolution, config_->size_x, config_->size_y); - detected_history.push_front(std::make_pair(sid_loc, changed_points)); - - if (detected_history.size() > config_->costmap_history_size) - detected_history.pop_back(); - - for (auto &pair : detected_history) { - auto &p_loc_sid = pair.first; - auto &point_cloud = pair.second; - const auto &T_v_loc_v_detect = chain.T_trunk_target(p_loc_sid); - - auto point_cloud_copy = pcl::PointCloud(point_cloud); - - auto aligned_point_cloud = point_cloud.getMatrixXfMap(4, PointWithInfo::size(), PointWithInfo::cartesian_offset()); - auto aligned_norms = point_cloud.getMatrixXfMap(4, PointWithInfo::size(), PointWithInfo::normal_offset()); - - - auto aligned_point_cloud_copy = point_cloud_copy.getMatrixXfMap(4, PointWithInfo::size(), PointWithInfo::cartesian_offset()); - auto aligned_norms_copy = point_cloud_copy.getMatrixXfMap(4, PointWithInfo::size(), PointWithInfo::normal_offset()); - - - aligned_point_cloud_copy = T_v_loc_v_detect.matrix().cast() * aligned_point_cloud; - aligned_norms_copy = T_v_loc_v_detect.matrix().cast() * aligned_norms; - - concat_pc += point_cloud_copy; - - CLOG(DEBUG, "lidar.obstacle_inflation") << "Point cloud of size " << point_cloud.size() << " is connected to sid: " << p_loc_sid; - - } // update cost map based on change detection result DetectChangeOp detect_change_op( @@ -182,12 +148,6 @@ void CostmapInflationModule::run_(QueryCache &qdata0, OutputCache &output0, pointcloud_msg.header.frame_id = "loc vertex frame"; // pointcloud_msg.header.stamp = rclcpp::Time(*qdata.stamp); costpcd_pub_->publish(pointcloud_msg); - - PointCloudMsg concat_msg; - pcl::toROSMsg(concat_pc, concat_msg); - concat_msg.header.frame_id = "loc vertex frame"; - concat_msg.header.stamp = rclcpp::Time(*qdata.stamp); - concat_pc_pub_->publish(concat_msg); } /// output @@ -200,5 +160,12 @@ void CostmapInflationModule::run_(QueryCache &qdata0, OutputCache &output0, } + +pcl::PointCloud CostmapInflationModule::assemble_pointcloud(tactic::QueryCache &qdata0, + tactic::OutputCache &, const tactic::Graph::Ptr &) { + const auto &qdata = dynamic_cast(qdata0); + return *qdata.changed_points; +} + } // namespace lidar } // namespace vtr \ No newline at end of file diff --git a/main/src/vtr_lidar/src/modules/planning/queued_inflation_module.cpp b/main/src/vtr_lidar/src/modules/planning/queued_inflation_module.cpp new file mode 100644 index 000000000..9e549792c --- /dev/null +++ b/main/src/vtr_lidar/src/modules/planning/queued_inflation_module.cpp @@ -0,0 +1,121 @@ +// 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 queued_inflation_module.cpp + * \author Alec Krawciw, Autonomous Space Robotics Lab (ASRL) + */ +#include "vtr_lidar/modules/planning/queued_inflation_module.hpp" + +#include "pcl/features/normal_3d.h" + +#include "vtr_lidar/data_types/costmap.hpp" +#include "vtr_lidar/filters/voxel_downsample.hpp" + +namespace vtr { +namespace lidar { + + +using namespace tactic; + +auto QueuedCostmapModule::Config::fromROS( + const rclcpp::Node::SharedPtr &node, const std::string ¶m_prefix) + -> ConstPtr { + auto config = std::make_shared(); + // clang-format off + + auto base_config = CostmapInflationModule::Config::fromROS(node, param_prefix); + + auto casted_config = + std::static_pointer_cast(config); + *casted_config = *base_config; // copy over base config + + int hist_size = node->declare_parameter(param_prefix + ".costmap_history_size", config->costmap_history_size); + config->costmap_history_size = (unsigned) hist_size; + // clang-format on + return config; +} + +QueuedCostmapModule::VtrPointCloud QueuedCostmapModule::assemble_pointcloud(tactic::QueryCache &qdata0, + tactic::OutputCache &output0, const tactic::Graph::Ptr &) { + + auto &qdata = dynamic_cast(qdata0); + auto &output = dynamic_cast(output0); + + /// visualization setup + if (config_->visualize && !publisher_initialized_) { + // clang-format off + concat_pc_pub_ = qdata.node->create_publisher("change_detection_concat", 5); + // clang-format on + publisher_initialized_ = true; + } + + + // inputs + const auto &stamp = *qdata.stamp; + const auto &sid_loc = *qdata.sid_loc; + const auto &changed_points = *qdata.changed_points; + const auto &chain = *output.chain; + + // clang-format off + CLOG(DEBUG, "lidar.obstacle_inflation") << "Adding point cloud to queue: " << stamp; + + + auto concat_pc = pcl::PointCloud(); + + detected_history.push_front(std::make_pair(sid_loc, changed_points)); + + if (detected_history.size() > config_->costmap_history_size) + detected_history.pop_back(); + + for (auto &pair : detected_history) { + auto &p_loc_sid = pair.first; + auto &point_cloud = pair.second; + const auto &T_v_loc_v_detect = chain.T_trunk_target(p_loc_sid); + + auto point_cloud_copy = pcl::PointCloud(point_cloud); + + auto aligned_point_cloud = point_cloud.getMatrixXfMap(4, PointWithInfo::size(), PointWithInfo::cartesian_offset()); + auto aligned_norms = point_cloud.getMatrixXfMap(4, PointWithInfo::size(), PointWithInfo::normal_offset()); + + + auto aligned_point_cloud_copy = point_cloud_copy.getMatrixXfMap(4, PointWithInfo::size(), PointWithInfo::cartesian_offset()); + auto aligned_norms_copy = point_cloud_copy.getMatrixXfMap(4, PointWithInfo::size(), PointWithInfo::normal_offset()); + + + aligned_point_cloud_copy = T_v_loc_v_detect.matrix().cast() * aligned_point_cloud; + aligned_norms_copy = T_v_loc_v_detect.matrix().cast() * aligned_norms; + + concat_pc += point_cloud_copy; + + CLOG(DEBUG, "lidar.obstacle_inflation") << "Point cloud of size " << point_cloud.size() << " is connected to sid: " << p_loc_sid; + + } + + + /// publish the transformed pointcloud + if (config_->visualize) { + + PointCloudMsg concat_msg; + pcl::toROSMsg(concat_pc, concat_msg); + concat_msg.header.frame_id = "loc vertex frame"; + concat_msg.header.stamp = rclcpp::Time(*qdata.stamp); + concat_pc_pub_->publish(concat_msg); + } + + return concat_pc; +} + +} // namespace lidar +} // namespace vtr \ No newline at end of file diff --git a/main/src/vtr_lidar/src/modules/planning/rangenet_change_detection_module.cpp b/main/src/vtr_lidar/src/modules/planning/rangenet_change_detection_module.cpp index ab0af35f8..3108b8166 100644 --- a/main/src/vtr_lidar/src/modules/planning/rangenet_change_detection_module.cpp +++ b/main/src/vtr_lidar/src/modules/planning/rangenet_change_detection_module.cpp @@ -83,7 +83,7 @@ void RangeChangeNetModule::run_(QueryCache &qdata0, OutputCache &output0, const auto &sid_loc = *qdata.sid_loc; const auto &T_r_v_loc = *qdata.T_r_v_loc; const auto &T_v_m_loc = *qdata.T_v_m_loc; - + if(!qdata.submap_loc.valid()) { @@ -185,31 +185,6 @@ void RangeChangeNetModule::run_(QueryCache &qdata0, OutputCache &output0, pcl::PointCloud radius_filtered_points(obstacle_points, radii_indices); qdata.changed_points.emplace(radius_filtered_points); - // std::unordered_map values; - // for (size_t i = 0; i < obstacle_points.size(); ++i) { - // const auto pix_coord = pointToKey(obstacle_points[i]); - // if (values.find(pix_coord) != values.end()) { - // values[pix_coord] += 0.2; - // } else { - // values[pix_coord] = 0.2; - // } - // } - - - - // costmap->update(values); - - // const auto filtered_costmap = std::make_shared( - // config_->resolution, config_->size_x, config_->size_y); - // filtered_costmap->update(costmap->filter(1.0)); - // // add transform to the localization vertex - // filtered_costmap->T_vertex_this() = T_r_v_loc.inverse(); - // filtered_costmap->vertex_id() = vid_loc; - // filtered_costmap->vertex_sid() = sid_loc; - - // auto change_detection_costmap_ref = output.change_detection_costmap.locked(); - // auto &change_detection_costmap = change_detection_costmap_ref.get(); - // change_detection_costmap = filtered_costmap; /// publish the transformed pointcloud if (config_->visualize) { diff --git a/main/src/vtr_lidar/src/modules/preprocessing/conversions/ouster_conversion_module.cpp b/main/src/vtr_lidar/src/modules/preprocessing/conversions/ouster_conversion_module.cpp index 0b87be1d0..7946070c0 100644 --- a/main/src/vtr_lidar/src/modules/preprocessing/conversions/ouster_conversion_module.cpp +++ b/main/src/vtr_lidar/src/modules/preprocessing/conversions/ouster_conversion_module.cpp @@ -106,7 +106,7 @@ void OusterConversionModule::run_(QueryCache &qdata0, OutputCache &, //CLOG(DEBUG, "lidar.ouster_converter") << "Message Header Stamp (nanosec)" << (msg->header).stamp.nanosec; - point_cloud->at(idx).timestamp = static_cast(*iter_time * 1e9); + point_cloud->at(idx).timestamp = *qdata.stamp; //CLOG(DEBUG, "lidar.ouster_converter") << "First point info - x: " << *iter_x << " y: " << *iter_y << " z: " << *iter_z << " timestamp: " << static_cast(*iter_time * 1e9); //CLOG(DEBUG, "lidar.ouster_converter") << "Second point info - x: " << *iter_x << " y: " << *iter_y << " z: " << *iter_z << " timestamp: " << static_cast(*iter_time); } diff --git a/main/src/vtr_navigation/launch/vtr.launch.py b/main/src/vtr_navigation/launch/vtr.launch.py index 684603318..3af2c1fe0 100644 --- a/main/src/vtr_navigation/launch/vtr.launch.py +++ b/main/src/vtr_navigation/launch/vtr.launch.py @@ -26,6 +26,8 @@ def generate_launch_description(): output='screen', #prefix=['xterm -e gdb -ex run --args'], parameters=[ + PathJoinSubstitution((config_dir, LaunchConfiguration("base_params"))), + { "data_dir": LaunchConfiguration("data_dir"), "model_dir": LaunchConfiguration("model_dir"), @@ -33,7 +35,6 @@ def generate_launch_description(): "use_sim_time": LaunchConfiguration("use_sim_time"), "path_planning.type": LaunchConfiguration("planner"), }, - PathJoinSubstitution((config_dir, LaunchConfiguration("base_params"))), PathJoinSubstitution((config_dir, LaunchConfiguration("override_params"))), ], ), From c06d75e63f914323f04ed3f8e83f5d7a677fb40d Mon Sep 17 00:00:00 2001 From: a-krawciw Date: Sat, 2 Sep 2023 16:52:30 -0400 Subject: [PATCH 48/77] Minor config tuning changes --- config/ouster_warthog_default.yaml | 6 +++--- config/ouster_warthog_learned.yaml | 16 ++++++++-------- 2 files changed, 11 insertions(+), 11 deletions(-) diff --git a/config/ouster_warthog_default.yaml b/config/ouster_warthog_default.yaml index 9d2dcab23..4e7067c12 100644 --- a/config/ouster_warthog_default.yaml +++ b/config/ouster_warthog_default.yaml @@ -322,7 +322,7 @@ initial_samples: 400 batch_samples: 200 pre_seed_resolution: 0.5 - alpha: 0.5 + alpha: 0.25 q_max: 2.5 frame_interval: 50 iter_max: 10000000 @@ -340,8 +340,8 @@ # Controller Params horizon_steps: 20 horizon_step_size: 0.3 - forward_vel: 0.75 - max_lin_vel: 1.25 + forward_vel: 1.75 + max_lin_vel: 2.05 max_ang_vel: 2.0 robot_linear_velocity_scale: 1.0 # Used to scale the output twist linear velocity messages by some constant factor to compensate internal low level control miscalibration robot_angular_velocity_scale: 2.0 # Used to scale the output twist angular velocity messages by some constant factor to compensate internal low level control miscalibration diff --git a/config/ouster_warthog_learned.yaml b/config/ouster_warthog_learned.yaml index 2c025d3e5..f27ab9118 100644 --- a/config/ouster_warthog_learned.yaml +++ b/config/ouster_warthog_learned.yaml @@ -252,11 +252,11 @@ visualize: true costmap_inflation: type: lidar.costmap_blindspot - influence_distance: 0.8 # was 0.5 Jordy # Note that the total distance where grid cells have values > 0 is min dist + influence dist, not influence dist! - minimum_distance: 0.6 # was 0.3 Jordy + influence_distance: 0.3 # was 0.5 Jordy # Note that the total distance where grid cells have values > 0 is min dist + influence dist, not influence dist! + minimum_distance: 1.0 # was 0.3 Jordy resolution: 0.25 costmap_history_size: 15 - blind_spot_radius: 2.5 + blind_spot_radius: 1.5 lifetime: 1.5 size_x: 16.0 size_y: 16.0 @@ -341,18 +341,18 @@ # Controller Params horizon_steps: 30 horizon_step_size: 0.3 - forward_vel: 0.6 + forward_vel: 1.0 max_lin_vel: 1.25 max_ang_vel: 1.0 robot_linear_velocity_scale: 1.0 # Used to scale the output twist linear velocity messages by some constant factor to compensate internal low level control miscalibration - robot_angular_velocity_scale: 2.0 # Used to scale the output twist angular velocity messages by some constant factor to compensate internal low level control miscalibration + robot_angular_velocity_scale: 1.0 # Used to scale the output twist angular velocity messages by some constant factor to compensate internal low level control miscalibration vehicle_model: "unicycle" # Cost Function Covariance Matrices - pose_error_cov: [100.0, 100.0, 1000.0, 1000.0, 1000.0, 50.0] - vel_error_cov: [25.0, 5.0] - acc_error_cov: [10.0, 5.0] + pose_error_cov: [100.0, 100.0, 1000.0, 1000.0, 1000.0, 200.0] + vel_error_cov: [25.0, 100.0] + acc_error_cov: [10.0, 100.0] kin_error_cov: [0.001, 0.001, 0.001, 0.001, 0.001, 0.001] lat_error_cov: [20.0] From 18774cf5905011295db3da9859504bef829ead71 Mon Sep 17 00:00:00 2001 From: a-krawciw Date: Sat, 2 Sep 2023 16:52:59 -0400 Subject: [PATCH 49/77] Again --- config/ouster_warthog_default.yaml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/config/ouster_warthog_default.yaml b/config/ouster_warthog_default.yaml index 4e7067c12..0ac2e745e 100644 --- a/config/ouster_warthog_default.yaml +++ b/config/ouster_warthog_default.yaml @@ -340,8 +340,8 @@ # Controller Params horizon_steps: 20 horizon_step_size: 0.3 - forward_vel: 1.75 - max_lin_vel: 2.05 + forward_vel: 0.75 + max_lin_vel: 1.25 max_ang_vel: 2.0 robot_linear_velocity_scale: 1.0 # Used to scale the output twist linear velocity messages by some constant factor to compensate internal low level control miscalibration robot_angular_velocity_scale: 2.0 # Used to scale the output twist angular velocity messages by some constant factor to compensate internal low level control miscalibration From 401e091a0640d1b87fc3c16871183b62d868c18a Mon Sep 17 00:00:00 2001 From: a-krawciw Date: Sat, 2 Sep 2023 17:03:04 -0400 Subject: [PATCH 50/77] Update configs to use UI graph selection --- launch/offline_ouster_warthog.launch.yaml | 1 + launch/online_ouster_warthog.launch.yaml | 5 +++-- 2 files changed, 4 insertions(+), 2 deletions(-) diff --git a/launch/offline_ouster_warthog.launch.yaml b/launch/offline_ouster_warthog.launch.yaml index 2ef56333d..f923d754a 100644 --- a/launch/offline_ouster_warthog.launch.yaml +++ b/launch/offline_ouster_warthog.launch.yaml @@ -13,6 +13,7 @@ windows: layout: main-horizontal shell_command_before: - source ${VTRSRC}/main/install/setup.bash +# - ros2 run vtr_gui setup_server --ros-args -r __ns:=/vtr panes: - > ros2 launch vtr_navigation vtr.launch.py diff --git a/launch/online_ouster_warthog.launch.yaml b/launch/online_ouster_warthog.launch.yaml index 079f3c726..1ad5e34f4 100644 --- a/launch/online_ouster_warthog.launch.yaml +++ b/launch/online_ouster_warthog.launch.yaml @@ -1,5 +1,5 @@ ## Online Lidar VTR3 -session_name: vtr_online_ouster_grizzly +session_name: vtr_online_ouster_warthog environment: CYCLONEDDS_URI: ${VTRSRC}/config/cyclonedds.default.xml # custom dds configs @@ -13,11 +13,12 @@ windows: layout: main-horizontal shell_command_before: - source ${VTRSRC}/main/install/setup.bash + - ros2 run vtr_gui setup_server --ros-args -r __ns:=/vtr panes: - > ros2 launch vtr_navigation vtr.launch.py base_params:=ouster_warthog_default.yaml - data_dir:=${VTRTEMP}/lidar/$(date '+%F')/$(date '+%F') +# data_dir:=${VTRTEMP}/lidar/$(date '+%F')/$(date '+%F') start_new_graph:=false use_sim_time:=false model_dir:=${VTRROOT}/models From 9607bdb6383a73b646395860e873cc18b460749e Mon Sep 17 00:00:00 2001 From: a-krawciw Date: Mon, 11 Sep 2023 08:28:37 -0400 Subject: [PATCH 51/77] Fixed blindspot filter. Added the ability to create new pose graphs within vtr_temp --- config/ouster_warthog_learned.yaml | 12 ++++--- launch/online_ouster_warthog.launch.yaml | 7 ++-- .../src/vtr_lidar/include/vtr_lidar/cache.hpp | 2 +- .../rangenet_change_detection_module.hpp | 1 + .../planning/blindspot_inflation_module.cpp | 14 +++++--- .../rangenet_change_detection_module.cpp | 28 ++++++++++++--- main/src/vtr_lidar/src/pipeline.cpp | 36 +++++++++++-------- main/src/vtr_navigation/launch/vtr.launch.py | 7 ++-- .../vtr_navigation/vtr_setup.py | 2 +- 9 files changed, 71 insertions(+), 38 deletions(-) diff --git a/config/ouster_warthog_learned.yaml b/config/ouster_warthog_learned.yaml index f27ab9118..de34afcdb 100644 --- a/config/ouster_warthog_learned.yaml +++ b/config/ouster_warthog_learned.yaml @@ -106,6 +106,7 @@ - memory submap_translation_threshold: 1.5 submap_rotation_threshold: 30.0 + save_nn_point_cloud: true preprocessing: conversion: type: lidar.ouster_converter @@ -119,7 +120,7 @@ num_threads: 8 crop_range: 40.0 - frame_voxel_size: 0.2 # grid subsampling voxel size + frame_voxel_size: 0.3 # grid subsampling voxel size nn_voxel_size: 0.05 vertical_angle_res: 0.0061365 # vertical angle resolution in radians, equal to 0.3516 degree documented in the manual @@ -170,7 +171,7 @@ mapping: type: lidar.odometry_map_maintenance_v2 - map_voxel_size: 0.2 + map_voxel_size: 0.3 crop_range_front: 40.0 back_over_front_ratio: 0.5 @@ -185,7 +186,7 @@ type: lidar.intra_exp_merging_v2 depth: 6.0 - map_voxel_size: 0.2 + map_voxel_size: 0.3 crop_range_front: 40.0 back_over_front_ratio: 0.5 @@ -203,7 +204,7 @@ inter_exp_merging: type: "lidar.inter_exp_merging_v2" - map_voxel_size: 0.2 + map_voxel_size: 0.3 max_num_experiences: 128 distance_threshold: 0.6 planar_threshold: 0.2 @@ -246,9 +247,10 @@ type: torch.range_change use_gpu: true abs_filepath: false - filepath: exported_model_flat_10m.pt + filepath: exported_model_forest_m30_10m.pt radius_filter: 0.25 neighbourhood: 5 + save_nn_point_cloud: true visualize: true costmap_inflation: type: lidar.costmap_blindspot diff --git a/launch/online_ouster_warthog.launch.yaml b/launch/online_ouster_warthog.launch.yaml index 1ad5e34f4..544be8ff6 100644 --- a/launch/online_ouster_warthog.launch.yaml +++ b/launch/online_ouster_warthog.launch.yaml @@ -17,12 +17,13 @@ windows: panes: - > ros2 launch vtr_navigation vtr.launch.py - base_params:=ouster_warthog_default.yaml -# data_dir:=${VTRTEMP}/lidar/$(date '+%F')/$(date '+%F') + base_params:=ouster_warthog_learned.yaml start_new_graph:=false use_sim_time:=false model_dir:=${VTRROOT}/models - planner:="cbit" + planner:="cbit.lidar" + + # data_dir:=${VTRTEMP}/lidar/$(date '+%F')/$(date '+%F') #- htop # monitor hardware usage diff --git a/main/src/vtr_lidar/include/vtr_lidar/cache.hpp b/main/src/vtr_lidar/include/vtr_lidar/cache.hpp index 7680b5612..0880f41a5 100644 --- a/main/src/vtr_lidar/include/vtr_lidar/cache.hpp +++ b/main/src/vtr_lidar/include/vtr_lidar/cache.hpp @@ -40,7 +40,7 @@ struct LidarQueryCache : virtual public tactic::QueryCache { // preprocessing tactic::Cache> raw_point_cloud; tactic::Cache> preprocessed_point_cloud; - tactic::Cache> nn_point_cloud; + tactic::Cache> nn_point_cloud; // odometry & mapping tactic::Cache> undistorted_point_cloud; diff --git a/main/src/vtr_lidar/include/vtr_lidar/modules/planning/rangenet_change_detection_module.hpp b/main/src/vtr_lidar/include/vtr_lidar/modules/planning/rangenet_change_detection_module.hpp index ebb6c04b9..d95a42d78 100644 --- a/main/src/vtr_lidar/include/vtr_lidar/modules/planning/rangenet_change_detection_module.hpp +++ b/main/src/vtr_lidar/include/vtr_lidar/modules/planning/rangenet_change_detection_module.hpp @@ -60,6 +60,7 @@ class RangeChangeNetModule : public nn::TorchModule { int neighbourhood = 1; float radius_filter = 0.1; + bool save_nn_point_cloud = false; static ConstPtr fromROS(const rclcpp::Node::SharedPtr &node, const std::string ¶m_prefix); diff --git a/main/src/vtr_lidar/src/modules/planning/blindspot_inflation_module.cpp b/main/src/vtr_lidar/src/modules/planning/blindspot_inflation_module.cpp index c01d54380..8927c7fdd 100644 --- a/main/src/vtr_lidar/src/modules/planning/blindspot_inflation_module.cpp +++ b/main/src/vtr_lidar/src/modules/planning/blindspot_inflation_module.cpp @@ -76,6 +76,7 @@ BlindspotCostmapModule::VtrPointCloud BlindspotCostmapModule::assemble_pointclou const auto &stamp = *qdata.stamp; const auto &sid_loc = *qdata.sid_loc; const auto &T_r_v_loc = *qdata.T_r_v_loc; + const auto &T_s_r = *qdata.T_s_r; const auto &changed_points = *qdata.changed_points; const auto &chain = *output.chain; @@ -92,6 +93,8 @@ BlindspotCostmapModule::VtrPointCloud BlindspotCostmapModule::assemble_pointclou const auto &T_w_v_loc = chain.pose(sid_loc); const auto T_r_w = T_r_v_loc*(T_w_v_loc.inverse()); + const auto T_s_w = T_s_r*T_r_w; + const auto T_v_loc_s = T_w_v_loc.inverse()*T_s_w.inverse(); auto aligned_point_cloud = concat_pc.getMatrixXfMap(4, PointWithInfo::size(), PointWithInfo::cartesian_offset()); aligned_point_cloud = T_w_v_loc.matrix().cast() * aligned_point_cloud; @@ -101,20 +104,23 @@ BlindspotCostmapModule::VtrPointCloud BlindspotCostmapModule::assemble_pointclou auto aligned_concat_cloud = concat_pc.getMatrixXfMap(4, PointWithInfo::size(), PointWithInfo::cartesian_offset()); - aligned_concat_cloud = T_r_w.matrix().cast() * aligned_concat_cloud; + aligned_concat_cloud = T_s_w.matrix().cast() * aligned_concat_cloud; std::vector indices; indices.reserve(concat_pc.size()); const float r_sq_thresh = config_->blind_spot_radius * config_->blind_spot_radius; + const double elev_thres = 25*M_PI/180; for (size_t i = 0; i < concat_pc.size(); ++i) { auto &p = concat_pc[i]; - auto r_sq = p.x*p.x + p.y*p.y; - if (r_sq < r_sq_thresh) indices.emplace_back(i); + auto r = sqrt(p.x*p.x + p.y*p.y); + + auto elevation = atan2(p.z, r); + if (abs(elevation) > elev_thres) indices.emplace_back(i); else if (abs(stamp - p.timestamp) < config_->lifetime*1e9) indices.emplace_back(i); } pcl::PointCloud obstacle_points(concat_pc, indices); auto aligned_obstacle_cloud = obstacle_points.getMatrixXfMap(4, PointWithInfo::size(), PointWithInfo::cartesian_offset()); - aligned_obstacle_cloud = T_r_v_loc.inverse().matrix().cast() * aligned_obstacle_cloud; + aligned_obstacle_cloud = T_v_loc_s.matrix().cast() * aligned_obstacle_cloud; // pcl::PointCloud reduced_world(concat_pc, indices); all_points_ = pcl::PointCloud(all_points_, indices); diff --git a/main/src/vtr_lidar/src/modules/planning/rangenet_change_detection_module.cpp b/main/src/vtr_lidar/src/modules/planning/rangenet_change_detection_module.cpp index 3108b8166..019035d6f 100644 --- a/main/src/vtr_lidar/src/modules/planning/rangenet_change_detection_module.cpp +++ b/main/src/vtr_lidar/src/modules/planning/rangenet_change_detection_module.cpp @@ -65,24 +65,23 @@ auto RangeChangeNetModule::Config::fromROS(const rclcpp::Node::SharedPtr &node, config->fov_down = node->declare_parameter(param_prefix + ".fov_down", config->fov_down); config->range_crop = node->declare_parameter(param_prefix + ".range_crop", config->range_crop); config->neighbourhood = node->declare_parameter(param_prefix + ".neighbourhood", config->neighbourhood); + config->save_nn_point_cloud = node->declare_parameter(param_prefix + ".save_nn_point_cloud", config->save_nn_point_cloud); // clang-format on return config; } -void RangeChangeNetModule::run_(QueryCache &qdata0, OutputCache &output0, - const Graph::Ptr &, const TaskExecutor::Ptr &) { +void RangeChangeNetModule::run_(QueryCache &qdata0, OutputCache &, + const Graph::Ptr &graph, const TaskExecutor::Ptr &) { auto &qdata = dynamic_cast(qdata0); - auto &output = dynamic_cast(output0); auto nn_point_cloud = *qdata.nn_point_cloud; const auto &T_s_r = *qdata.T_s_r; - const auto &vid_loc = *qdata.vid_loc; - const auto &sid_loc = *qdata.sid_loc; const auto &T_r_v_loc = *qdata.T_r_v_loc; const auto &T_v_m_loc = *qdata.T_v_m_loc; + const auto &sid_loc = *qdata.sid_loc; @@ -149,6 +148,7 @@ void RangeChangeNetModule::run_(QueryCache &qdata0, OutputCache &output0, torch::from_blob(mask_image.data(), {64, 1024}) = mask; unproject_range_image(nn_point_cloud, mask_image, scan_idxs); + unproject_range_image(*qdata.nn_point_cloud, mask_image, scan_idxs); // filter out non-obstacle points std::vector indices; @@ -185,6 +185,24 @@ void RangeChangeNetModule::run_(QueryCache &qdata0, OutputCache &output0, pcl::PointCloud radius_filtered_points(obstacle_points, radii_indices); qdata.changed_points.emplace(radius_filtered_points); + + // if (config_->save_nn_point_cloud) { + // auto vertex = graph->at(*qdata.vid_odo); + + // auto nn_scan = std::make_shared>(); + // nn_scan->point_cloud() = nn_point_cloud; + // nn_scan->T_vertex_this() = qdata.T_s_r->inverse(); + // nn_scan->vertex_id() = *qdata.vid_odo; + // // + // using PointScanLM = storage::LockableMessage>; + // auto nn_scan_odo_msg = + // std::make_shared(nn_scan, *qdata.stamp); + // vertex->insert>( + // "nn_point_cloud", "vtr_lidar_msgs/msg/PointScan", nn_scan_odo_msg); + + // CLOG(DEBUG, "lidar.pipeline") << "Saved nn pointcloud to vertex" << vertex; + // } + /// publish the transformed pointcloud if (config_->visualize) { diff --git a/main/src/vtr_lidar/src/pipeline.cpp b/main/src/vtr_lidar/src/pipeline.cpp index 1ec143abb..75de2ae77 100644 --- a/main/src/vtr_lidar/src/pipeline.cpp +++ b/main/src/vtr_lidar/src/pipeline.cpp @@ -126,6 +126,26 @@ void LidarPipeline::runLocalization_(const QueryCache::Ptr &qdata0, for (const auto &module : localization_) module->run(*qdata0, *output0, graph, executor); + + // raw point cloud + const auto& vertex_test_result = *qdata->vertex_test_result; + if (vertex_test_result == VertexTestResult::CREATE_VERTEX && + config_->save_nn_point_cloud) { + auto vertex = graph->at(*qdata->vid_odo); + + auto nn_scan = std::make_shared>(); + nn_scan->point_cloud() = *qdata->nn_point_cloud; + nn_scan->T_vertex_this() = qdata->T_s_r->inverse(); + nn_scan->vertex_id() = *qdata->vid_odo; + // + using PointScanLM = storage::LockableMessage>; + auto nn_scan_odo_msg = + std::make_shared(nn_scan, *qdata->stamp); + vertex->insert>( + "nn_point_cloud", "vtr_lidar_msgs/msg/PointScan", nn_scan_odo_msg); + + CLOG(DEBUG, "lidar.pipeline") << "Saved nn pointcloud to vertex" << vertex; + } /// store the current map for localization if (qdata->submap_loc) submap_loc_ = qdata->submap_loc.ptr(); @@ -175,21 +195,7 @@ void LidarPipeline::onVertexCreation_(const QueryCache::Ptr &qdata0, CLOG(DEBUG, "lidar.pipeline") << "Saved raw pointcloud to vertex" << vertex; } - // raw point cloud - if (config_->save_nn_point_cloud) { - auto nn_scan = std::make_shared>(); - nn_scan->point_cloud() = *qdata->nn_point_cloud; - nn_scan->T_vertex_this() = qdata->T_s_r->inverse(); - nn_scan->vertex_id() = *qdata->vid_odo; - // - using PointScanLM = storage::LockableMessage>; - auto nn_scan_odo_msg = - std::make_shared(nn_scan, *qdata->stamp); - vertex->insert>( - "nn_point_cloud", "vtr_lidar_msgs/msg/PointScan", nn_scan_odo_msg); - - CLOG(DEBUG, "lidar.pipeline") << "Saved nn pointcloud to vertex" << vertex; - } + /// save the sliding map as vertex submap if we have traveled far enough const bool create_submap = [&] { diff --git a/main/src/vtr_navigation/launch/vtr.launch.py b/main/src/vtr_navigation/launch/vtr.launch.py index 5225c7a0b..4bf5f222f 100644 --- a/main/src/vtr_navigation/launch/vtr.launch.py +++ b/main/src/vtr_navigation/launch/vtr.launch.py @@ -19,7 +19,7 @@ def generate_launch_description(): "namespace": 'vtr', "executable": 'vtr_navigation', "output": 'screen', - #prefix=['xterm -e gdb -ex run --args'], + #"prefix": 'xterm -e gdb -ex run --args', } return LaunchDescription([ @@ -33,8 +33,6 @@ def generate_launch_description(): Node(**commonNodeArgs, parameters=[ PathJoinSubstitution((config_dir, LaunchConfiguration("base_params"))), - PathJoinSubstitution((config_dir, LaunchConfiguration("base_params"))), - { "data_dir": LaunchConfiguration("data_dir"), "model_dir": LaunchConfiguration("model_dir"), @@ -48,7 +46,8 @@ def generate_launch_description(): ), Node(**commonNodeArgs, parameters=[ - PathJoinSubstitution((temp_dir, "setup_params.yaml")), + PathJoinSubstitution((config_dir, LaunchConfiguration("base_params"))), + PathJoinSubstitution((temp_dir, "setup_params.yaml")), { "model_dir": LaunchConfiguration("model_dir"), "start_new_graph": LaunchConfiguration("start_new_graph"), diff --git a/main/src/vtr_navigation/vtr_navigation/vtr_setup.py b/main/src/vtr_navigation/vtr_navigation/vtr_setup.py index 27722a51b..5910fbe8f 100644 --- a/main/src/vtr_navigation/vtr_navigation/vtr_setup.py +++ b/main/src/vtr_navigation/vtr_navigation/vtr_setup.py @@ -21,7 +21,7 @@ DEFAULT_DIR = os.getenv('VTRTEMP') def confirm_setup(data): - if not osp.isdir(data["data_dir"]): + if not data["data_dir"].startswith(DEFAULT_DIR+os.sep): return False os.chdir(SETUP_DIR) From e29b1f6bebb9ad8a4e9618a6fa7904044391e454 Mon Sep 17 00:00:00 2001 From: a-krawciw Date: Tue, 13 Feb 2024 11:26:18 -0500 Subject: [PATCH 52/77] Add intensity to map. Throw exception if TF is not found. --- config/ouster_warthog_default.yaml | 16 ++++++----- config/ouster_warthog_learned.yaml | 8 +++--- config/vtr_rtps.xml | 28 +++++++++++++++++++ launch/online_ouster_warthog.launch.yaml | 5 ++-- main/src/vtr_lidar/CMakeLists.txt | 8 ++++-- .../rangenet_change_detection_module.cpp | 13 ++++++--- .../conversions/ouster_conversion_module.cpp | 3 ++ main/src/vtr_navigation/src/navigator.cpp | 2 ++ 8 files changed, 63 insertions(+), 20 deletions(-) create mode 100644 config/vtr_rtps.xml diff --git a/config/ouster_warthog_default.yaml b/config/ouster_warthog_default.yaml index a34c8aaa1..c923a0e4b 100644 --- a/config/ouster_warthog_default.yaml +++ b/config/ouster_warthog_default.yaml @@ -50,7 +50,7 @@ robot_frame: base_link env_info_topic: env_info - lidar_frame: os_sensor + lidar_frame: os_lidar lidar_topic: /ouster/points queue_size: 1 graph_map: @@ -98,7 +98,7 @@ - recall - icp - safe_corridor - - change_detection_sync + #- change_detection_sync - memory submap_translation_threshold: 1.5 submap_rotation_threshold: 30.0 @@ -110,6 +110,8 @@ filter_z_min: -0.2 filter_z_max: 0.35 filter_radius: 0.8 + + radius_filter: 0.05 filtering: type: lidar.preprocessing num_threads: 8 @@ -338,20 +340,20 @@ mpc: # Controller Params - horizon_steps: 20 + horizon_steps: 30 horizon_step_size: 0.3 forward_vel: 0.75 max_lin_vel: 1.25 - max_ang_vel: 2.0 + max_ang_vel: 1.0 robot_linear_velocity_scale: 1.0 # Used to scale the output twist linear velocity messages by some constant factor to compensate internal low level control miscalibration - robot_angular_velocity_scale: 1.8 # Used to scale the output twist angular velocity messages by some constant factor to compensate internal low level control miscalibration + robot_angular_velocity_scale: 0.75 # Used to scale the output twist angular velocity messages by some constant factor to compensate internal low level control miscalibration vehicle_model: "unicycle" # Cost Function Covariance Matrices pose_error_cov: [100.0, 100.0, 1000.0, 1000.0, 1000.0, 200.0] - vel_error_cov: [100.0, 200.0] # was [0.1 2.0] # No longer using this cost term - acc_error_cov: [100.0, 200.0] + vel_error_cov: [25.0, 100.0] + acc_error_cov: [10.0, 100.0] kin_error_cov: [0.001, 0.001, 0.001, 0.001, 0.001, 0.001] lat_error_cov: [20.0] diff --git a/config/ouster_warthog_learned.yaml b/config/ouster_warthog_learned.yaml index de34afcdb..b561826a9 100644 --- a/config/ouster_warthog_learned.yaml +++ b/config/ouster_warthog_learned.yaml @@ -11,13 +11,13 @@ # tactic #- tactic - tactic.pipeline - #- tactic.module + - tactic.module #- tactic.module.live_mem_manager #- tactic.module.graph_mem_manager # path planner #- path_planning - - path_planning.cbit + #- path_planning.cbit #- path_planning.cbit_planner - mpc.cbit #- mpc_debug @@ -36,7 +36,7 @@ - lidar.pipeline #- lidar.preprocessing #- lidar.ouster_converter - - lidar.odometry_icp + #- lidar.odometry_icp #- lidar.odometry_map_maintenance - lidar.vertex_test #- lidar.localization_map_recall @@ -53,7 +53,7 @@ robot_frame: base_link env_info_topic: env_info - lidar_frame: os_sensor + lidar_frame: os_lidar lidar_topic: /ouster/points queue_size: 1 graph_map: diff --git a/config/vtr_rtps.xml b/config/vtr_rtps.xml new file mode 100644 index 000000000..912f815ff --- /dev/null +++ b/config/vtr_rtps.xml @@ -0,0 +1,28 @@ + + + + + + robotethernet + UDPv4 + +
enx207bd29e670f
+ + + + + shm_transport + SHM + + + + + + + + shm_transport + + + + + diff --git a/launch/online_ouster_warthog.launch.yaml b/launch/online_ouster_warthog.launch.yaml index 544be8ff6..f7c26dca3 100644 --- a/launch/online_ouster_warthog.launch.yaml +++ b/launch/online_ouster_warthog.launch.yaml @@ -2,7 +2,8 @@ session_name: vtr_online_ouster_warthog environment: - CYCLONEDDS_URI: ${VTRSRC}/config/cyclonedds.default.xml # custom dds configs + RMW_IMPLEMENTATION: rmw_fastrtps_cpp + FASTRTPS_DEFAULT_PROFILES_FILE: ${VTRSRC}/config/vtr_rtps.xml # ROS_DOMAIN_ID: "1" # set this to a unique number when multiple ROS2 dependent system running on the same network start_directory: ${VTRTEMP} @@ -21,7 +22,7 @@ windows: start_new_graph:=false use_sim_time:=false model_dir:=${VTRROOT}/models - planner:="cbit.lidar" + planner:="cbit" # data_dir:=${VTRTEMP}/lidar/$(date '+%F')/$(date '+%F') diff --git a/main/src/vtr_lidar/CMakeLists.txt b/main/src/vtr_lidar/CMakeLists.txt index df472b49b..86a611d1f 100644 --- a/main/src/vtr_lidar/CMakeLists.txt +++ b/main/src/vtr_lidar/CMakeLists.txt @@ -10,6 +10,8 @@ find_package(ament_cmake_python REQUIRED) find_package(eigen3_cmake_module REQUIRED) find_package(Eigen3 REQUIRED) +find_package(OpenCV REQUIRED) + find_package(rclcpp REQUIRED) find_package(rclpy REQUIRED) @@ -43,7 +45,7 @@ file(GLOB_RECURSE COMPONENTS_SRC ) add_library(${PROJECT_NAME}_components ${COMPONENTS_SRC}) ament_target_dependencies(${PROJECT_NAME}_components - Eigen3 pcl_conversions pcl_ros + Eigen3 pcl_conversions pcl_ros OpenCV nav_msgs lgmath steam vtr_common vtr_logging vtr_tactic vtr_path_planning vtr_lidar_msgs @@ -57,7 +59,7 @@ file(GLOB_RECURSE PIPELINE_SRC add_library(${PROJECT_NAME}_pipeline ${PIPELINE_SRC}) target_link_libraries(${PROJECT_NAME}_pipeline ${PROJECT_NAME}_components) ament_target_dependencies(${PROJECT_NAME}_pipeline - Eigen3 pcl_conversions pcl_ros + Eigen3 pcl_conversions pcl_ros OpenCV nav_msgs visualization_msgs sensor_msgs lgmath steam vtr_torch vtr_logging vtr_tactic vtr_lidar_msgs @@ -86,7 +88,7 @@ ament_target_dependencies(${PROJECT_NAME}_tools ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) ament_export_dependencies( - Eigen3 pcl_conversions pcl_ros + Eigen3 pcl_conversions pcl_ros OpenCV nav_msgs visualization_msgs lgmath steam vtr_logging vtr_tactic vtr_path_planning vtr_lidar_msgs vtr_torch diff --git a/main/src/vtr_lidar/src/modules/planning/rangenet_change_detection_module.cpp b/main/src/vtr_lidar/src/modules/planning/rangenet_change_detection_module.cpp index 019035d6f..e1174365c 100644 --- a/main/src/vtr_lidar/src/modules/planning/rangenet_change_detection_module.cpp +++ b/main/src/vtr_lidar/src/modules/planning/rangenet_change_detection_module.cpp @@ -134,21 +134,26 @@ void RangeChangeNetModule::run_(QueryCache &qdata0, OutputCache &, using namespace torch::indexing; + + timer.reset(); auto scan_tensor = torch::from_blob(scan_image.data(), {64, 1024}); auto map_tensor = torch::from_blob(map_image.data(), {64, 1024}); auto input = at::unsqueeze(at::stack({scan_tensor, map_tensor}), 0); - timer.reset(); + CLOG(DEBUG, "lidar.range") << "GPU load takes " << timer; + + auto tensor = evaluateModel(input, {1, 2, 64, 1024}); auto mask = at::squeeze(at::argmax(tensor, 1), 0).to(at::kFloat); - timer.stop(); - CLOG(DEBUG, "lidar.range") << "Running inference takes " << timer; - timer.reset(); + torch::from_blob(mask_image.data(), {64, 1024}) = mask; unproject_range_image(nn_point_cloud, mask_image, scan_idxs); unproject_range_image(*qdata.nn_point_cloud, mask_image, scan_idxs); + timer.stop(); + CLOG(DEBUG, "lidar.range") << "Running inference takes " << timer; + timer.reset(); // filter out non-obstacle points std::vector indices; diff --git a/main/src/vtr_lidar/src/modules/preprocessing/conversions/ouster_conversion_module.cpp b/main/src/vtr_lidar/src/modules/preprocessing/conversions/ouster_conversion_module.cpp index 7946070c0..cc7b3b0de 100644 --- a/main/src/vtr_lidar/src/modules/preprocessing/conversions/ouster_conversion_module.cpp +++ b/main/src/vtr_lidar/src/modules/preprocessing/conversions/ouster_conversion_module.cpp @@ -20,6 +20,8 @@ #include "pcl_conversions/pcl_conversions.h" #include "sensor_msgs/point_cloud2_iterator.hpp" +#include "vtr_lidar/utils/nanoflann_utils.hpp" + namespace vtr { namespace lidar { @@ -99,6 +101,7 @@ void OusterConversionModule::run_(QueryCache &qdata0, OutputCache &, point_cloud->at(idx).y = *iter_y; point_cloud->at(idx).z = *iter_z; point_cloud->at(idx).intensity = *iter_intensity; + point_cloud->at(idx).flex23 = point_cloud->at(idx).intensity; // pointwise timestamp //CLOG(DEBUG, "lidar.ouster_converter") << "Timestamp is: " << *iter_time; diff --git a/main/src/vtr_navigation/src/navigator.cpp b/main/src/vtr_navigation/src/navigator.cpp index 5151d8e27..27a03fc74 100644 --- a/main/src/vtr_navigation/src/navigator.cpp +++ b/main/src/vtr_navigation/src/navigator.cpp @@ -65,6 +65,8 @@ EdgeTransform loadTransform(const std::string& source_frame, << " target: " << target_frame << ". Default to identity."; EdgeTransform T_source_target(Eigen::Matrix4d(Eigen::Matrix4d::Identity())); T_source_target.setCovariance(Eigen::Matrix::Zero()); + throw std::runtime_error("Transform not found"); + return T_source_target; } From 699e6e6898f367d6b4628654d9d0101d9812dcd6 Mon Sep 17 00:00:00 2001 From: a-krawciw Date: Mon, 19 Feb 2024 12:23:37 -0500 Subject: [PATCH 53/77] Update domain ID --- launch/online_ouster_warthog.launch.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/launch/online_ouster_warthog.launch.yaml b/launch/online_ouster_warthog.launch.yaml index f7c26dca3..5bfb4ec4a 100644 --- a/launch/online_ouster_warthog.launch.yaml +++ b/launch/online_ouster_warthog.launch.yaml @@ -4,7 +4,7 @@ session_name: vtr_online_ouster_warthog environment: RMW_IMPLEMENTATION: rmw_fastrtps_cpp FASTRTPS_DEFAULT_PROFILES_FILE: ${VTRSRC}/config/vtr_rtps.xml - # ROS_DOMAIN_ID: "1" # set this to a unique number when multiple ROS2 dependent system running on the same network + ROS_DOMAIN_ID: "131" # set this to a unique number when multiple ROS2 dependent system running on the same network start_directory: ${VTRTEMP} suppress_history: false From cdbb04ae85a23df5b03fceb35e796b767a7657ee Mon Sep 17 00:00:00 2001 From: a-krawciw Date: Mon, 26 Feb 2024 11:18:18 -0500 Subject: [PATCH 54/77] Add perspective rendering --- config/ouster_warthog_lasersam.yaml | 22 ++ ...online_ouster_warthog_lasersam.launch.yaml | 44 ++++ main/src/vtr_lidar/CMakeLists.txt | 7 +- .../vtr_lidar/filters/perspective_image.hpp | 61 ++++++ .../include/vtr_lidar/modules/modules.hpp | 2 +- ...ator.hpp => perspective_render_module.hpp} | 40 ++-- .../conversions/ouster_conversion_module.hpp | 1 + .../src/filters/perspective_image.cpp | 71 +++++++ .../src/modules/planning/diff_generator.cpp | 195 ------------------ .../planning/perspective_render_module.cpp | 83 ++++++++ 10 files changed, 305 insertions(+), 221 deletions(-) create mode 100644 config/ouster_warthog_lasersam.yaml create mode 100644 launch/online_ouster_warthog_lasersam.launch.yaml create mode 100644 main/src/vtr_lidar/include/vtr_lidar/filters/perspective_image.hpp rename main/src/vtr_lidar/include/vtr_lidar/modules/planning/{diff_generator.hpp => perspective_render_module.hpp} (64%) create mode 100644 main/src/vtr_lidar/src/filters/perspective_image.cpp delete mode 100644 main/src/vtr_lidar/src/modules/planning/diff_generator.cpp create mode 100644 main/src/vtr_lidar/src/modules/planning/perspective_render_module.cpp diff --git a/config/ouster_warthog_lasersam.yaml b/config/ouster_warthog_lasersam.yaml new file mode 100644 index 000000000..a8497e386 --- /dev/null +++ b/config/ouster_warthog_lasersam.yaml @@ -0,0 +1,22 @@ +/**: + ros__parameters: + log_enabled: + - lidar.perspective + + pipeline: + localization: + - recall + - icp + - safe_corridor + - perspective + #- change_detection_sync + - memory + + localization: + perspective: + type: lidar.render_perspective + img_width: 256 + img_height: 128 + h_fov: 1.5707 + v_fov: 0.7853 + visualize: true diff --git a/launch/online_ouster_warthog_lasersam.launch.yaml b/launch/online_ouster_warthog_lasersam.launch.yaml new file mode 100644 index 000000000..add898d77 --- /dev/null +++ b/launch/online_ouster_warthog_lasersam.launch.yaml @@ -0,0 +1,44 @@ +## Online Visual VTR3 with Learned Features +session_name: vtr_online_nerian_warthog + +environment: + FASTRTPS_DEFAULT_PROFILES_FILE: ${VTRSRC}/config/vtr_rtps.xml + ROS_DOMAIN_ID: "131" # set this to a unique number when multiple ROS2 dependent system running on the same network + +start_directory: ${VTRTEMP} + +suppress_history: false + +windows: + - window_name: vtr_navigation_system + layout: main-horizontal + shell_command_before: + - source ${VTRSRC}/main/install/setup.bash + - ros2 run vtr_gui setup_server --ros-args -r __ns:=/vtr + panes: + - > + ros2 launch vtr_navigation vtr.launch.py + base_params:=nerian_warthog_default.yaml + override_params:=nerian_learned_features_extras.yaml + start_new_graph:=false + use_sim_time:=false + planner:="cbit" + model_dir:=${VTRROOT}/models + + - window_name: vtr_gui + layout: main-horizontal + shell_command_before: + - source ${VTRSRC}/main/install/setup.bash + panes: + - ros2 run vtr_gui socket_client --ros-args -r __ns:=/vtr + - ros2 run vtr_gui socket_server --ros-args -r __ns:=/vtr + - ros2 run vtr_gui web_server --ros-args -r __ns:=/vtr + # - firefox --new-window "localhost:5200" # the webpage has to wait for everything above + + - window_name: rviz2 + layout: main-horizontal + shell_command_before: + - source ${VTRSRC}/main/install/setup.bash + panes: + - ros2 run rviz2 rviz2 -d ${VTRSRC}/rviz/bumblebee.rviz + diff --git a/main/src/vtr_lidar/CMakeLists.txt b/main/src/vtr_lidar/CMakeLists.txt index 80990f58a..8a4d3e55a 100644 --- a/main/src/vtr_lidar/CMakeLists.txt +++ b/main/src/vtr_lidar/CMakeLists.txt @@ -10,6 +10,8 @@ find_package(ament_cmake_python REQUIRED) find_package(eigen3_cmake_module REQUIRED) find_package(Eigen3 REQUIRED) +find_package(OpenCV REQUIRED) +find_package(cv_bridge REQUIRED) find_package(rclcpp REQUIRED) find_package(rclpy REQUIRED) @@ -39,12 +41,13 @@ include_directories(PUBLIC # components file(GLOB_RECURSE COMPONENTS_SRC src/data_types/*.cpp + src/filters/*.cpp ) add_library(${PROJECT_NAME}_components ${COMPONENTS_SRC}) ament_target_dependencies(${PROJECT_NAME}_components Eigen3 pcl_conversions pcl_ros nav_msgs - lgmath steam + lgmath steam OpenCV vtr_common vtr_logging vtr_tactic vtr_lidar_msgs ) @@ -58,7 +61,7 @@ target_link_libraries(${PROJECT_NAME}_pipeline ${PROJECT_NAME}_components) ament_target_dependencies(${PROJECT_NAME}_pipeline Eigen3 pcl_conversions pcl_ros nav_msgs visualization_msgs - lgmath steam vtr_torch + lgmath steam vtr_torch cv_bridge vtr_logging vtr_tactic vtr_lidar_msgs ) diff --git a/main/src/vtr_lidar/include/vtr_lidar/filters/perspective_image.hpp b/main/src/vtr_lidar/include/vtr_lidar/filters/perspective_image.hpp new file mode 100644 index 000000000..cc4832539 --- /dev/null +++ b/main/src/vtr_lidar/include/vtr_lidar/filters/perspective_image.hpp @@ -0,0 +1,61 @@ +// Copyright 2024, 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 range_image.hpp + * \author Alec Krawciw, Autonomous Space Robotics Lab (ASRL) + */ +#pragma once + +#include "vtr_lidar/data_types/point.hpp" +#include "math.h" +#include "opencv2/core/mat.hpp" + + +namespace vtr { +namespace lidar{ + +struct PerspectiveImageParams { + double v_fov; //In radians + double h_fov; //In radians + double crop_range = 1000.0; //m + double min_range = 0.1; //m + int width; + int height; + + double f_u(){ + return (double)(width) / 2.0 / tan(h_fov / 2); + } + + unsigned c_u() { return width / 2; } + + double f_v(){ + return (double)(height) / 2.0 / tan(v_fov / 2); + } + + unsigned c_v() { return height / 2; } +}; + +//Greyscale image with intensity +void generate_intensity_image(const pcl::PointCloud& point_cloud, cv::Mat& intesity_image, cv::Mat& idx_image, PerspectiveImageParams params); + +//HSV image. Hue is depth Saturation is constant. Value is intensity +void generate_depth_image(const pcl::PointCloud& point_cloud, cv::Mat& depth_image, cv::Mat& idx_image, PerspectiveImageParams params); + +void unproject_data_image(pcl::PointCloud& point_cloud, const cv::Mat& depth_image, const cv::Mat& idx_image); + + + +} // namespace lidar +} // vtr \ No newline at end of file diff --git a/main/src/vtr_lidar/include/vtr_lidar/modules/modules.hpp b/main/src/vtr_lidar/include/vtr_lidar/modules/modules.hpp index 3a7d772d6..cf2e726cd 100644 --- a/main/src/vtr_lidar/include/vtr_lidar/modules/modules.hpp +++ b/main/src/vtr_lidar/include/vtr_lidar/modules/modules.hpp @@ -47,5 +47,5 @@ #include "vtr_lidar/modules/planning/obstacle_detection_module.hpp" #include "vtr_lidar/modules/planning/safe_corridor_module.hpp" #include "vtr_lidar/modules/planning/terrain_assessment_module.hpp" -#include "vtr_lidar/modules/planning/diff_generator.hpp" +#include "vtr_lidar/modules/planning/perspective_render_module.hpp" diff --git a/main/src/vtr_lidar/include/vtr_lidar/modules/planning/diff_generator.hpp b/main/src/vtr_lidar/include/vtr_lidar/modules/planning/perspective_render_module.hpp similarity index 64% rename from main/src/vtr_lidar/include/vtr_lidar/modules/planning/diff_generator.hpp rename to main/src/vtr_lidar/include/vtr_lidar/modules/planning/perspective_render_module.hpp index ea6ec55f9..dd2403440 100644 --- a/main/src/vtr_lidar/include/vtr_lidar/modules/planning/diff_generator.hpp +++ b/main/src/vtr_lidar/include/vtr_lidar/modules/planning/perspective_render_module.hpp @@ -1,4 +1,4 @@ -// Copyright 2021, Autonomous Space Robotics Lab (ASRL) +// Copyright 2024, 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. @@ -13,41 +13,34 @@ // limitations under the License. /** - * \file diff_generator.hpp + * \file perspective_rnder_module.hpp * \author Alec Krawciw, Autonomous Space Robotics Lab (ASRL) */ -#pragma once -#include "tf2/convert.h" -#include "tf2_eigen/tf2_eigen.hpp" -#include "tf2_ros/transform_broadcaster.h" +#pragma once #include "vtr_lidar/cache.hpp" #include "vtr_tactic/modules/base_module.hpp" #include "vtr_tactic/task_queue.hpp" +#include "vtr_lidar/filters/perspective_image.hpp" + +#include "sensor_msgs/msg/image.hpp" namespace vtr { namespace lidar { -class DifferenceDetector : public tactic::BaseModule { +class PerspectiveRenderModule : public tactic::BaseModule { public: - PTR_TYPEDEFS(DifferenceDetector); - using PointCloudMsg = sensor_msgs::msg::PointCloud2; + PTR_TYPEDEFS(PerspectiveRenderModule); + using ImageMsg = sensor_msgs::msg::Image; - static constexpr auto static_name = "lidar.diff_generator"; + static constexpr auto static_name = "lidar.render_perspective"; /** \brief Collection of config parameters */ struct Config : public BaseModule::Config { PTR_TYPEDEFS(Config); - // change detection - float detection_range = 10.0; //m - float minimum_distance = 0.0; //m - - float neighbour_threshold = 0.05; //m - float voxel_size = 0.2; //m - - float angle_weight = 10.0/2/M_PI; + PerspectiveImageParams perspective_params; // bool visualize = false; @@ -56,11 +49,10 @@ class DifferenceDetector : public tactic::BaseModule { const std::string ¶m_prefix); }; - DifferenceDetector( + PerspectiveRenderModule( const Config::ConstPtr &config, const std::shared_ptr &module_factory = nullptr, - const std::string &name = static_name) - : tactic::BaseModule{module_factory, name}, config_(config) {} + const std::string &name = static_name); private: void run_(tactic::QueryCache &qdata, tactic::OutputCache &output, @@ -71,9 +63,11 @@ class DifferenceDetector : public tactic::BaseModule { /** \brief for visualization only */ bool publisher_initialized_ = false; - rclcpp::Publisher::SharedPtr diffpcd_pub_; - VTR_REGISTER_MODULE_DEC_TYPE(DifferenceDetector); + rclcpp::Publisher::SharedPtr img_pub_; + + + VTR_REGISTER_MODULE_DEC_TYPE(PerspectiveRenderModule); }; diff --git a/main/src/vtr_lidar/include/vtr_lidar/modules/preprocessing/conversions/ouster_conversion_module.hpp b/main/src/vtr_lidar/include/vtr_lidar/modules/preprocessing/conversions/ouster_conversion_module.hpp index 775d20ba7..3dcd32484 100644 --- a/main/src/vtr_lidar/include/vtr_lidar/modules/preprocessing/conversions/ouster_conversion_module.hpp +++ b/main/src/vtr_lidar/include/vtr_lidar/modules/preprocessing/conversions/ouster_conversion_module.hpp @@ -49,6 +49,7 @@ class OusterConversionModule : public tactic::BaseModule { float filter_z_min = 0; float filter_radius_sq = 0; + float radius_filter = 0; static ConstPtr fromROS(const rclcpp::Node::SharedPtr &node, const std::string ¶m_prefix); diff --git a/main/src/vtr_lidar/src/filters/perspective_image.cpp b/main/src/vtr_lidar/src/filters/perspective_image.cpp new file mode 100644 index 000000000..b9d618fd0 --- /dev/null +++ b/main/src/vtr_lidar/src/filters/perspective_image.cpp @@ -0,0 +1,71 @@ +// Copyright 2023, 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 perspective_image.cpp + * \author Alec Krawciw, Autonomous Space Robotics Lab (ASRL) + */ +#include "vtr_lidar/filters/perspective_image.hpp" + + +namespace vtr { +namespace lidar { + +/// \todo Parallelize +void generate_intensity_image(const pcl::PointCloud& point_cloud, cv::Mat& intensity_image, cv::Mat& idx_image, PerspectiveImageParams params) { + + params.width = intensity_image.cols; + params.height = intensity_image.rows; + + for (size_t i = 0; i < point_cloud.size(); ++i) { + auto& point = point_cloud[i]; + if (point.z > params.crop_range || point.z < params.min_range) + continue; + + int u = (int)round(params.f_u() * point.x / point.z) + params.c_u(); + int v = (int)round(params.f_v() * point.y / point.z) + params.c_v(); + + if (0 <= u && u < params.width && 0 <= v && v < params.height) { + intensity_image.at(u, v) = point.intensity; + idx_image.at(u, v) = i; + } + } + +} + +void generate_depth_image(const pcl::PointCloud& point_cloud, cv::Mat& depth_image, cv::Mat& idx_image, PerspectiveImageParams params) { + + params.width = depth_image.cols; + params.height = depth_image.rows; + + for (size_t i = 0; i < point_cloud.size(); ++i) { + auto& point = point_cloud[i]; + if (point.z > params.crop_range || point.z < params.min_range) + continue; + + int u = (int)round(params.f_u() * point.x / point.z) + params.c_u(); + int v = (int)round(params.f_v() * point.y / point.z) + params.c_v(); + + if (0 <= u && u < params.width && 0 <= v && v < params.height) { + depth_image.at(u, v, 2) = point.intensity; + depth_image.at(u, v, 0) = point.z; + + idx_image.at(u, v) = i; + } + } + +} + +} //lidar +} //vtr \ No newline at end of file diff --git a/main/src/vtr_lidar/src/modules/planning/diff_generator.cpp b/main/src/vtr_lidar/src/modules/planning/diff_generator.cpp deleted file mode 100644 index 09b5a9b64..000000000 --- a/main/src/vtr_lidar/src/modules/planning/diff_generator.cpp +++ /dev/null @@ -1,195 +0,0 @@ -// 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 diff_generator.cpp - * \author Alec Krawciw, Autonomous Space Robotics Lab (ASRL) - */ -#include "vtr_lidar/modules/planning/diff_generator.hpp" - -#include "pcl/features/normal_3d.h" -#include "vtr_lidar/filters/voxel_downsample.hpp" - -#include "vtr_lidar/utils/nanoflann_utils.hpp" - -namespace vtr { -namespace lidar { - -void velodyneCart2Pol(pcl::PointCloud& point_cloud) { - for (size_t i = 0; i < point_cloud.size(); i++) { - auto &p = point_cloud[i]; - auto &pm1 = i > 0 ? point_cloud[i - 1] : point_cloud[i]; - - p.rho = sqrt(p.x * p.x + p.y * p.y + p.z * p.z); - p.theta = atan2(sqrt(p.x * p.x + p.y * p.y), p.z); - p.phi = atan2(p.y, p.x) + M_PI / 2; - - if (i > 0 && (p.phi - pm1.phi) > 1.5 * M_PI) - p.phi -= 2 * M_PI; - else if (i > 0 && (p.phi - pm1.phi) < -1.5 * M_PI) - p.phi += 2 * M_PI; - } -} - - - -using namespace tactic; - -auto DifferenceDetector::Config::fromROS( - const rclcpp::Node::SharedPtr &node, const std::string ¶m_prefix) - -> ConstPtr { - auto config = std::make_shared(); - // clang-format off - // change detection - config->detection_range = node->declare_parameter(param_prefix + ".detection_range", config->detection_range); - config->minimum_distance = node->declare_parameter(param_prefix + ".minimum_distance", config->minimum_distance); - config->neighbour_threshold = node->declare_parameter(param_prefix + ".neighbour_threshold", config->neighbour_threshold); - - //filtering - config->voxel_size = node->declare_parameter(param_prefix + ".voxel_size", config->voxel_size); - config->angle_weight = node->declare_parameter(param_prefix + ".angle_weight", config->angle_weight); - - // general - config->visualize = node->declare_parameter(param_prefix + ".visualize", config->visualize); - // clang-format on - return config; -} - -void DifferenceDetector::run_(QueryCache &qdata0, OutputCache &output0, - const Graph::Ptr & /* graph */, - const TaskExecutor::Ptr & /* executor */) { - auto &qdata = dynamic_cast(qdata0); - auto &output = dynamic_cast(output0); - - /// visualization setup - if (config_->visualize && !publisher_initialized_) { - // clang-format off - diffpcd_pub_ = qdata.node->create_publisher("change_detection_diff", 5); - // clang-format on - publisher_initialized_ = true; - } - - // inputs - const auto &stamp = *qdata.stamp; - const auto &T_s_r = *qdata.T_s_r; - const auto &vid_loc = *qdata.vid_loc; - const auto &sid_loc = *qdata.sid_loc; - const auto &T_r_v_loc = *qdata.T_r_v_loc; - const auto &points = *qdata.raw_point_cloud; - const auto &submap_loc = *qdata.submap_loc; - const auto &map_point_cloud = submap_loc.point_cloud(); - const auto &T_v_m_loc = *qdata.T_v_m_loc; - - // clang-format off - CLOG(INFO, static_name) << "Diff detection for lidar scan at stamp: " << stamp; - - // filter out points that are too far away or too close - std::vector query_indices; - for (size_t i = 0; i < points.size(); ++i) { - const auto &pt = points.at(i); - if (pt.getVector3fMap().norm() < config_->detection_range && pt.getVector3fMap().norm() > config_->minimum_distance) - query_indices.emplace_back(i); - } - pcl::PointCloud query_points(points, query_indices); - - voxelDownsample(query_points, config_->voxel_size); - - // Eigen matrix of original data (only shallow copy of ref clouds) - const auto query_mat = query_points.getMatrixXfMap(4, PointWithInfo::size(), PointWithInfo::cartesian_offset()); - const auto query_norms_mat = query_points.getMatrixXfMap(4, PointWithInfo::size(), PointWithInfo::normal_offset()); - - - const auto map_mat = map_point_cloud.getMatrixXfMap(4, PointWithInfo::size(), PointWithInfo::cartesian_offset()); - const auto map_norms_mat = map_point_cloud.getMatrixXfMap(4, PointWithInfo::size(), PointWithInfo::normal_offset()); - - // retrieve the pre-processed scan and convert it to the local map frame - pcl::PointCloud aligned_points(query_points); - auto aligned_mat = aligned_points.getMatrixXfMap(4, PointWithInfo::size(), PointWithInfo::cartesian_offset()); - auto aligned_norms_mat = aligned_points.getMatrixXfMap(4, PointWithInfo::size(), PointWithInfo::normal_offset()); - - const auto T_s_m = (T_s_r * T_r_v_loc * T_v_m_loc).matrix(); - const auto T_m_s = (T_s_r * T_r_v_loc * T_v_m_loc).inverse().matrix(); - aligned_mat = T_m_s.cast() * query_mat; - aligned_norms_mat = T_m_s.cast() * query_norms_mat; - - pcl::PointCloud aligned_map(map_point_cloud); - auto aligned_map_mat = aligned_map.getMatrixXfMap(4, PointWithInfo::size(), PointWithInfo::cartesian_offset()); - auto aligned_map_norms_mat = aligned_map.getMatrixXfMap(4, PointWithInfo::size(), PointWithInfo::normal_offset()); - - //Put map into lidar frame - aligned_map_mat = T_s_m.cast() * map_mat; - aligned_map_norms_mat = T_s_m.cast() * map_norms_mat; - - - velodyneCart2Pol(aligned_map); - - // create kd-tree of the map - //Could this be done ahead of time and stored? - NanoFLANNAdapter adapter(aligned_map);//, config_->angle_weight); - KDTreeSearchParams search_params; - KDTreeParams tree_params(10 /* max leaf */); - auto kdtree = std::make_unique>(3, adapter, tree_params); - kdtree->buildIndex(); - - std::vector nn_inds(query_points.size()); - std::vector nn_dists(query_points.size(), -1.0f); - // compute nearest neighbors and point to point distances - for (size_t i = 0; i < query_points.size(); i++) { - KDTreeResultSet result_set(1); - result_set.init(&nn_inds[i], &nn_dists[i]); - kdtree->findNeighbors(result_set, query_points[i].data, search_params); - } - - std::vector diff_indices; - for (size_t i = 0; i < query_points.size(); i++) { - aligned_points[i].flex23 = std::sqrt(nn_dists[i]); - if (aligned_points[i].flex23 > config_->neighbour_threshold){ - diff_indices.push_back(i); - } - } - - // add support region - - // retrieve the pre-processed scan and convert it to the vertex frame - pcl::PointCloud aligned_points2(aligned_points); - // clang-format off - auto aligned_mat2 = aligned_points2.getMatrixXfMap(4, PointWithInfo::size(), PointWithInfo::cartesian_offset()); - auto aligned_norms_mat2 = aligned_points2.getMatrixXfMap(4, PointWithInfo::size(), PointWithInfo::normal_offset()); - // clang-format on - - // transform to vertex frame - aligned_mat2 = T_v_m_loc.matrix().cast() * aligned_mat; - aligned_norms_mat2 = T_v_m_loc.matrix().cast() * aligned_norms_mat; - - pcl::PointCloud filtered_points(aligned_points2, diff_indices); - - - - /// publish the transformed pointcloud - if (config_->visualize) { - PointCloudMsg filter_msg; - pcl::toROSMsg(filtered_points, filter_msg); - filter_msg.header.frame_id = "loc vertex frame"; - // pointcloud_msg.header.stamp = rclcpp::Time(*qdata.stamp); - diffpcd_pub_->publish(filter_msg); - } - - - CLOG(INFO, static_name) - << "Diff detection for lidar scan at stamp: " << stamp << " - DONE"; - -} - -} // namespace lidar -} // namespace vtr \ No newline at end of file diff --git a/main/src/vtr_lidar/src/modules/planning/perspective_render_module.cpp b/main/src/vtr_lidar/src/modules/planning/perspective_render_module.cpp new file mode 100644 index 000000000..f441cfb53 --- /dev/null +++ b/main/src/vtr_lidar/src/modules/planning/perspective_render_module.cpp @@ -0,0 +1,83 @@ +// Copyright 2024, 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 perspective_render_module.cpp + * \author Alec Krawciw, Autonomous Space Robotics Lab (ASRL) + */ +#include "vtr_lidar/modules/planning/perspective_render_module.hpp" + +#include "cv_bridge/cv_bridge.hpp" +#include "opencv2/imgproc.hpp" + + +namespace vtr { +namespace lidar { + +using namespace tactic; + +auto PerspectiveRenderModule::Config::fromROS( + const rclcpp::Node::SharedPtr &node, const std::string ¶m_prefix) + -> ConstPtr { + auto config = std::make_shared(); + // clang-format off + + auto& params = config->perspective_params; + + params.width = node->declare_parameter(param_prefix + ".img_width", 0); + params.height = node->declare_parameter(param_prefix + ".img_height", 0); + params.h_fov = node->declare_parameter(param_prefix + ".h_fov", M_PI/2); + params.v_fov = node->declare_parameter(param_prefix + ".v_fov", M_PI/4); + + config->visualize = node->declare_parameter(param_prefix + ".visualize", config->visualize); + // clang-format on + return config; +} + +PerspectiveRenderModule::PerspectiveRenderModule( + const Config::ConstPtr &config, + const std::shared_ptr &module_factory, + const std::string &name) + : tactic::BaseModule{module_factory, name}, config_(config) {} + +void PerspectiveRenderModule::run_(QueryCache &qdata0, OutputCache &output0, + const Graph::Ptr &graph, + const TaskExecutor::Ptr &executor) { + auto &qdata = dynamic_cast(qdata0); + + if (!publisher_initialized_) { + img_pub_ = qdata.node->create_publisher("live_range_coloured", 5); + publisher_initialized_ = true; + } + + cv::Mat index_img; + index_img.create(config_->perspective_params.height, config_->perspective_params.width, CV_32S); + + cv::Mat hsv_img; + hsv_img.create({config_->perspective_params.height, config_->perspective_params.width, 3}, CV_8UC3); + + generate_depth_image(*qdata.raw_point_cloud, hsv_img, index_img, config_->perspective_params); + + cv::Mat rgb_img; + + cv::cvtColor(hsv_img, rgb_img, cv::COLOR_HSV2RGB); + + if (config_->visualize) { + ImageMsg::Ptr msg = cv_bridge::CvImage(std_msgs::Header(), "rgb8", rgb_img).toImageMsg(); + img_pub_->publish(msg); + } +} + +} // namespace lidar +} // namespace vtr \ No newline at end of file From d070ba358265204a60dd02d3c0ea4b0f3d42ba91 Mon Sep 17 00:00:00 2001 From: a-krawciw Date: Mon, 26 Feb 2024 16:06:54 -0500 Subject: [PATCH 55/77] Initially working. No filtering --- config/ouster_warthog_lasersam.yaml | 16 +- ...online_ouster_warthog_lasersam.launch.yaml | 15 +- main/src/vtr_common/vtr_include.cmake | 20 ++- main/src/vtr_lidar/CMakeLists.txt | 7 +- .../vtr_lidar/filters/perspective_image.hpp | 2 +- .../planning/perspective_render_module.hpp | 3 +- .../src/filters/perspective_image.cpp | 22 +-- .../planning/perspective_render_module.cpp | 84 +++++++-- main/src/vtr_navigation/launch/vtr.launch.py | 2 +- rviz/lidar.rviz | 163 ++++++++++++++++++ 10 files changed, 294 insertions(+), 40 deletions(-) create mode 100644 rviz/lidar.rviz diff --git a/config/ouster_warthog_lasersam.yaml b/config/ouster_warthog_lasersam.yaml index a8497e386..79a243ada 100644 --- a/config/ouster_warthog_lasersam.yaml +++ b/config/ouster_warthog_lasersam.yaml @@ -2,14 +2,24 @@ ros__parameters: log_enabled: - lidar.perspective - + - navigation.sensor_input + - tactic.pipeline + - tactic.module + #- tactic.module.live_mem_manager + #- tactic.module.graph_mem_manager + + + # pose graph + #- pose_graph + + # lidar pipeline + - lidar.pipeline pipeline: localization: - recall - icp - safe_corridor - perspective - #- change_detection_sync - memory localization: @@ -19,4 +29,6 @@ img_height: 128 h_fov: 1.5707 v_fov: 0.7853 + min_range: 0.5 + max_range: 50.0 visualize: true diff --git a/launch/online_ouster_warthog_lasersam.launch.yaml b/launch/online_ouster_warthog_lasersam.launch.yaml index add898d77..28a2ed5ff 100644 --- a/launch/online_ouster_warthog_lasersam.launch.yaml +++ b/launch/online_ouster_warthog_lasersam.launch.yaml @@ -1,5 +1,5 @@ -## Online Visual VTR3 with Learned Features -session_name: vtr_online_nerian_warthog +## Online Lidar VTR3 with LaserSAM +session_name: vtr_online_ouster_warthog environment: FASTRTPS_DEFAULT_PROFILES_FILE: ${VTRSRC}/config/vtr_rtps.xml @@ -14,16 +14,17 @@ windows: layout: main-horizontal shell_command_before: - source ${VTRSRC}/main/install/setup.bash - - ros2 run vtr_gui setup_server --ros-args -r __ns:=/vtr + #- ros2 run vtr_gui setup_server --ros-args -r __ns:=/vtr panes: - > ros2 launch vtr_navigation vtr.launch.py - base_params:=nerian_warthog_default.yaml - override_params:=nerian_learned_features_extras.yaml + base_params:=ouster_warthog_default.yaml + override_params:=ouster_warthog_lasersam.yaml start_new_graph:=false use_sim_time:=false - planner:="cbit" + planner:="stationary" model_dir:=${VTRROOT}/models + data_dir:=${VTRTEMP}/lidar/sam_testing - window_name: vtr_gui layout: main-horizontal @@ -40,5 +41,5 @@ windows: shell_command_before: - source ${VTRSRC}/main/install/setup.bash panes: - - ros2 run rviz2 rviz2 -d ${VTRSRC}/rviz/bumblebee.rviz + - ros2 run rviz2 rviz2 -d ${VTRSRC}/rviz/lidar.rviz diff --git a/main/src/vtr_common/vtr_include.cmake b/main/src/vtr_common/vtr_include.cmake index dfe0f016d..b94991e44 100644 --- a/main/src/vtr_common/vtr_include.cmake +++ b/main/src/vtr_common/vtr_include.cmake @@ -10,7 +10,7 @@ add_compile_options(-march=native -O3 -pthread -Wall -Wextra) # add_compile_options(-frepo) #Add debug symbols -#add_compile_options(-g -Og) +#add_compile_options(-g) # built time and memory report # add_compile_options(-ftime-report -fmem-report) @@ -20,8 +20,6 @@ add_compile_options(-march=native -O3 -pthread -Wall -Wextra) # set(CMAKE_CXX_STANDARD_LIBRARIES -lasan) # add_compile_options(-g -Og) -add_compile_options(-g -Og) - ## Common packages setup # Boost requirement (by mission_planning but needed everywhere) find_package(Boost REQUIRED COMPONENTS system thread chrono timer) @@ -33,6 +31,9 @@ if (OpenMP_FOUND) endif() + + + #Set to VTR_PIPELINE=VISION, LIDAR, RADAR, or RADAR-LIDAR set(SelectedPipeline "$ENV{VTR_PIPELINE}") @@ -57,5 +58,16 @@ elseif(SelectedPipeline MATCHES "VISION") add_definitions(-DVTR_VISION_LEARNED) set(VTR_ENABLE_VISION true) else() - message(FATAL_ERROR "VTR_PIPELINE not set! Must select one of VTR_PIPELINE=VISION, LIDAR, RADAR, or RADAR-LIDAR") + add_definitions(-DVTR_ENABLE_LIDAR) + add_definitions(-DVTR_ENABLE_RADAR) + add_definitions(-DVTR_ENABLE_GPUSURF) # set the available flag + add_definitions(-DVTR_ENABLE_VISION) + add_definitions(-DVTR_VISION_LEARNED) + + + set(VTR_ENABLE_VISION true) + set(VTR_ENABLE_LIDAR true) + set(VTR_ENABLE_RADAR true) + + message(DEBUG "VTR_PIPELINE not set! Compiling all pipelines.") endif() \ No newline at end of file diff --git a/main/src/vtr_lidar/CMakeLists.txt b/main/src/vtr_lidar/CMakeLists.txt index 8a4d3e55a..8e4d09fd5 100644 --- a/main/src/vtr_lidar/CMakeLists.txt +++ b/main/src/vtr_lidar/CMakeLists.txt @@ -32,6 +32,9 @@ find_package(vtr_torch REQUIRED) if(DEFINED VTR_ENABLE_LIDAR) +add_compile_options(-g) + + ## C++ Libraries include_directories(PUBLIC $ @@ -60,7 +63,7 @@ add_library(${PROJECT_NAME}_pipeline ${PIPELINE_SRC}) target_link_libraries(${PROJECT_NAME}_pipeline ${PROJECT_NAME}_components) ament_target_dependencies(${PROJECT_NAME}_pipeline Eigen3 pcl_conversions pcl_ros - nav_msgs visualization_msgs + nav_msgs visualization_msgs OpenCV lgmath steam vtr_torch cv_bridge vtr_logging vtr_tactic vtr_lidar_msgs ) @@ -79,7 +82,7 @@ ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) ament_export_dependencies( Eigen3 pcl_conversions pcl_ros nav_msgs visualization_msgs - lgmath steam + lgmath steam cv_bridge vtr_logging vtr_tactic vtr_lidar_msgs vtr_torch ) diff --git a/main/src/vtr_lidar/include/vtr_lidar/filters/perspective_image.hpp b/main/src/vtr_lidar/include/vtr_lidar/filters/perspective_image.hpp index cc4832539..756a8fb6e 100644 --- a/main/src/vtr_lidar/include/vtr_lidar/filters/perspective_image.hpp +++ b/main/src/vtr_lidar/include/vtr_lidar/filters/perspective_image.hpp @@ -29,7 +29,7 @@ namespace lidar{ struct PerspectiveImageParams { double v_fov; //In radians double h_fov; //In radians - double crop_range = 1000.0; //m + double max_range = 1000.0; //m double min_range = 0.1; //m int width; int height; diff --git a/main/src/vtr_lidar/include/vtr_lidar/modules/planning/perspective_render_module.hpp b/main/src/vtr_lidar/include/vtr_lidar/modules/planning/perspective_render_module.hpp index dd2403440..f9e294cae 100644 --- a/main/src/vtr_lidar/include/vtr_lidar/modules/planning/perspective_render_module.hpp +++ b/main/src/vtr_lidar/include/vtr_lidar/modules/planning/perspective_render_module.hpp @@ -64,7 +64,8 @@ class PerspectiveRenderModule : public tactic::BaseModule { /** \brief for visualization only */ bool publisher_initialized_ = false; - rclcpp::Publisher::SharedPtr img_pub_; + rclcpp::Publisher::SharedPtr live_img_pub_; + rclcpp::Publisher::SharedPtr map_img_pub_; VTR_REGISTER_MODULE_DEC_TYPE(PerspectiveRenderModule); diff --git a/main/src/vtr_lidar/src/filters/perspective_image.cpp b/main/src/vtr_lidar/src/filters/perspective_image.cpp index b9d618fd0..c86386883 100644 --- a/main/src/vtr_lidar/src/filters/perspective_image.cpp +++ b/main/src/vtr_lidar/src/filters/perspective_image.cpp @@ -25,12 +25,9 @@ namespace lidar { /// \todo Parallelize void generate_intensity_image(const pcl::PointCloud& point_cloud, cv::Mat& intensity_image, cv::Mat& idx_image, PerspectiveImageParams params) { - params.width = intensity_image.cols; - params.height = intensity_image.rows; - for (size_t i = 0; i < point_cloud.size(); ++i) { auto& point = point_cloud[i]; - if (point.z > params.crop_range || point.z < params.min_range) + if (point.z > params.max_range || point.z < params.min_range) continue; int u = (int)round(params.f_u() * point.x / point.z) + params.c_u(); @@ -46,22 +43,25 @@ void generate_intensity_image(const pcl::PointCloud& point_cloud, void generate_depth_image(const pcl::PointCloud& point_cloud, cv::Mat& depth_image, cv::Mat& idx_image, PerspectiveImageParams params) { - params.width = depth_image.cols; - params.height = depth_image.rows; - for (size_t i = 0; i < point_cloud.size(); ++i) { auto& point = point_cloud[i]; - if (point.z > params.crop_range || point.z < params.min_range) + if (point.z > params.max_range || point.z < params.min_range) continue; int u = (int)round(params.f_u() * point.x / point.z) + params.c_u(); int v = (int)round(params.f_v() * point.y / point.z) + params.c_v(); if (0 <= u && u < params.width && 0 <= v && v < params.height) { - depth_image.at(u, v, 2) = point.intensity; - depth_image.at(u, v, 0) = point.z; + cv::Vec3b &hsv = depth_image.at(v, u); + + if (hsv[0] == 0 || hsv[0] > abs(point.z) * UINT8_MAX / params.max_range) { + hsv[2] = sqrt(point.flex23) * 15.5 < UINT8_MAX ? sqrt(point.intensity) * 15.5 : UINT8_MAX; + hsv[1] = UINT8_MAX - 1; + hsv[0] = abs(point.z) * UINT8_MAX / params.max_range; + + idx_image.at(v, u) = i; + } - idx_image.at(u, v) = i; } } diff --git a/main/src/vtr_lidar/src/modules/planning/perspective_render_module.cpp b/main/src/vtr_lidar/src/modules/planning/perspective_render_module.cpp index f441cfb53..283ed7222 100644 --- a/main/src/vtr_lidar/src/modules/planning/perspective_render_module.cpp +++ b/main/src/vtr_lidar/src/modules/planning/perspective_render_module.cpp @@ -18,7 +18,7 @@ */ #include "vtr_lidar/modules/planning/perspective_render_module.hpp" -#include "cv_bridge/cv_bridge.hpp" +#include "cv_bridge/cv_bridge.h" #include "opencv2/imgproc.hpp" @@ -39,6 +39,8 @@ auto PerspectiveRenderModule::Config::fromROS( params.height = node->declare_parameter(param_prefix + ".img_height", 0); params.h_fov = node->declare_parameter(param_prefix + ".h_fov", M_PI/2); params.v_fov = node->declare_parameter(param_prefix + ".v_fov", M_PI/4); + params.max_range = node->declare_parameter(param_prefix + ".max_range", params.max_range); + params.min_range = node->declare_parameter(param_prefix + ".min_range", params.min_range); config->visualize = node->declare_parameter(param_prefix + ".visualize", config->visualize); // clang-format on @@ -55,27 +57,87 @@ void PerspectiveRenderModule::run_(QueryCache &qdata0, OutputCache &output0, const Graph::Ptr &graph, const TaskExecutor::Ptr &executor) { auto &qdata = dynamic_cast(qdata0); + auto raw_point_cloud = *qdata.raw_point_cloud; + + if(!qdata.submap_loc.valid()) { + CLOG(WARNING, "lidar.range_change") << "Range image requires a map to work"; + return; + } + + const auto &T_s_r = *qdata.T_s_r; + const auto &T_r_v_loc = *qdata.T_r_v_loc; + const auto &T_v_m_loc = *qdata.T_v_m_loc; + const auto &sid_loc = *qdata.sid_loc; + + CLOG(DEBUG, "lidar.perspective") << "Hello world"; + if (!publisher_initialized_) { - img_pub_ = qdata.node->create_publisher("live_range_coloured", 5); + live_img_pub_ = qdata.node->create_publisher("live_range_coloured", 5); + map_img_pub_ = qdata.node->create_publisher("map_range_coloured", 5); publisher_initialized_ = true; + CLOG(DEBUG, "lidar.perspective") << "Creating publisher"; } - cv::Mat index_img; - index_img.create(config_->perspective_params.height, config_->perspective_params.width, CV_32S); + + + + + auto& sub_map= *qdata.submap_loc; + auto map_point_cloud = sub_map.point_cloud(); + auto map_points_mat = map_point_cloud.getMatrixXfMap(4, PointWithInfo::size(), PointWithInfo::cartesian_offset()); + + CLOG(DEBUG, "lidar.range") << "Before: (" << map_point_cloud[10].x << ", " << map_point_cloud[10].y << ", "<< map_point_cloud[10].z <<")"; + + const auto T_s_m = (T_s_r * T_r_v_loc * T_v_m_loc).matrix(); + - cv::Mat hsv_img; - hsv_img.create({config_->perspective_params.height, config_->perspective_params.width, 3}, CV_8UC3); + auto live_points_mat = raw_point_cloud.getMatrixXfMap(4, PointWithInfo::size(), PointWithInfo::cartesian_offset()); + + Eigen::Matrix4f T_c_s; + + T_c_s << 0, -1, 0, 0, + 0, 0, 1, 0, + -1, 0, 0, 0, + 0, 0, 0, 1; + + live_points_mat = T_c_s * live_points_mat; + map_points_mat = (T_c_s * T_s_m.cast()) * map_points_mat; + + + + cv::Mat live_index_img = cv::Mat::zeros(config_->perspective_params.height, config_->perspective_params.width, CV_32S); + cv::Mat live_hsv_img = cv::Mat::zeros(config_->perspective_params.height, config_->perspective_params.width, CV_8UC3); + cv::Mat live_rgb_img = cv::Mat::zeros(config_->perspective_params.height, config_->perspective_params.width, CV_8UC3); + + generate_depth_image(raw_point_cloud, live_hsv_img, live_index_img, config_->perspective_params); + + cv::cvtColor(live_hsv_img, live_rgb_img, cv::COLOR_HSV2RGB); + + if (config_->visualize) { + cv_bridge::CvImage live_cv_rgb_img; + live_cv_rgb_img.header.frame_id = "lidar"; + //cv_rgb_img.header.stamp = qdata.stamp->header.stamp; + live_cv_rgb_img.encoding = "rgb8"; + live_cv_rgb_img.image = live_rgb_img; + live_img_pub_->publish(*live_cv_rgb_img.toImageMsg()); + } - generate_depth_image(*qdata.raw_point_cloud, hsv_img, index_img, config_->perspective_params); + cv::Mat map_index_img = cv::Mat::zeros(config_->perspective_params.height, config_->perspective_params.width, CV_32S); + cv::Mat map_hsv_img = cv::Mat::zeros(config_->perspective_params.height, config_->perspective_params.width, CV_8UC3); + cv::Mat map_rgb_img = cv::Mat::zeros(config_->perspective_params.height, config_->perspective_params.width, CV_8UC3); - cv::Mat rgb_img; + generate_depth_image(map_point_cloud, map_hsv_img, map_index_img, config_->perspective_params); - cv::cvtColor(hsv_img, rgb_img, cv::COLOR_HSV2RGB); + cv::cvtColor(map_hsv_img, map_rgb_img, cv::COLOR_HSV2RGB); if (config_->visualize) { - ImageMsg::Ptr msg = cv_bridge::CvImage(std_msgs::Header(), "rgb8", rgb_img).toImageMsg(); - img_pub_->publish(msg); + cv_bridge::CvImage map_cv_rgb_img; + map_cv_rgb_img.header.frame_id = "lidar"; + //cv_rgb_img.header.stamp = qdata.stamp->header.stamp; + map_cv_rgb_img.encoding = "rgb8"; + map_cv_rgb_img.image = map_rgb_img; + map_img_pub_->publish(*map_cv_rgb_img.toImageMsg()); } } diff --git a/main/src/vtr_navigation/launch/vtr.launch.py b/main/src/vtr_navigation/launch/vtr.launch.py index ae106a65b..dbc00a8bb 100644 --- a/main/src/vtr_navigation/launch/vtr.launch.py +++ b/main/src/vtr_navigation/launch/vtr.launch.py @@ -19,7 +19,7 @@ def generate_launch_description(): "namespace": 'vtr', "executable": 'vtr_navigation', "output": 'screen', - "prefix": 'xterm -e gdb -ex run --args', + #"prefix": 'xterm -e gdb -ex run --args', } return LaunchDescription([ diff --git a/rviz/lidar.rviz b/rviz/lidar.rviz new file mode 100644 index 000000000..9696c8638 --- /dev/null +++ b/rviz/lidar.rviz @@ -0,0 +1,163 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + Splitter Ratio: 0.5 + Tree Height: 699 + - Class: rviz_common/Selection + Name: Selection + - Class: rviz_common/Tool Properties + Expanded: + - /2D Goal Pose1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz_common/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: "" +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz_default_plugins/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Class: rviz_default_plugins/Image + Enabled: true + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: Image + Normalize Range: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /vtr/live_range_coloured + Value: true + - Class: rviz_default_plugins/Image + Enabled: true + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: Image + Normalize Range: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /vtr/map_range_coloured + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: robot + Frame Rate: 30 + Name: root + Tools: + - Class: rviz_default_plugins/Interact + Hide Inactive Objects: true + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Covariance x: 0.25 + Covariance y: 0.25 + Covariance yaw: 0.06853891909122467 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /initialpose + - Class: rviz_default_plugins/SetGoal + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /goal_pose + - Class: rviz_default_plugins/PublishPoint + Single click: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /clicked_point + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Class: rviz_default_plugins/Orbit + Distance: 10 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 0 + Y: 0 + Z: 0 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.785398006439209 + Target Frame: + Value: Orbit (rviz) + Yaw: 0.785398006439209 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 1536 + Hide Left Dock: false + Hide Right Dock: false + Image: + collapsed: false + QMainWindow State: 000000ff00000000fd00000004000000000000034400000566fc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b00000344000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d0061006700650100000385000001080000002800fffffffb0000000a0049006d00610067006501000004930000010e0000002800ffffff000000010000010f00000566fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003b00000566000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000009b20000003efc0100000002fb0000000800540069006d00650100000000000009b20000025300fffffffb0000000800540069006d00650100000000000004500000000000000000000005530000056600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 2482 + X: 78 + Y: 27 From 8583d991a047bfd997011a34d5e71e875fa39224 Mon Sep 17 00:00:00 2001 From: a-krawciw Date: Wed, 28 Feb 2024 15:34:55 -0500 Subject: [PATCH 56/77] Interpolate perspective views --- .../vtr_lidar/filters/perspective_image.hpp | 1 + .../src/filters/perspective_image.cpp | 38 ++++++++++++++++++- .../planning/perspective_render_module.cpp | 38 ++++++++++++++----- .../conversions/ouster_conversion_module.cpp | 1 + .../vtr_torch/modules/torch_module.hpp | 2 + .../vtr_torch/src/modules/torch_module.cpp | 15 +++++++- 6 files changed, 84 insertions(+), 11 deletions(-) diff --git a/main/src/vtr_lidar/include/vtr_lidar/filters/perspective_image.hpp b/main/src/vtr_lidar/include/vtr_lidar/filters/perspective_image.hpp index 756a8fb6e..f149bf0bc 100644 --- a/main/src/vtr_lidar/include/vtr_lidar/filters/perspective_image.hpp +++ b/main/src/vtr_lidar/include/vtr_lidar/filters/perspective_image.hpp @@ -55,6 +55,7 @@ void generate_depth_image(const pcl::PointCloud& point_cloud, cv: void unproject_data_image(pcl::PointCloud& point_cloud, const cv::Mat& depth_image, const cv::Mat& idx_image); +void interpolate_hsv_image(cv::Mat& depth_image); } // namespace lidar diff --git a/main/src/vtr_lidar/src/filters/perspective_image.cpp b/main/src/vtr_lidar/src/filters/perspective_image.cpp index c86386883..aacee9098 100644 --- a/main/src/vtr_lidar/src/filters/perspective_image.cpp +++ b/main/src/vtr_lidar/src/filters/perspective_image.cpp @@ -17,6 +17,8 @@ * \author Alec Krawciw, Autonomous Space Robotics Lab (ASRL) */ #include "vtr_lidar/filters/perspective_image.hpp" +#include "opencv2/imgproc.hpp" + namespace vtr { @@ -55,7 +57,7 @@ void generate_depth_image(const pcl::PointCloud& point_cloud, cv: cv::Vec3b &hsv = depth_image.at(v, u); if (hsv[0] == 0 || hsv[0] > abs(point.z) * UINT8_MAX / params.max_range) { - hsv[2] = sqrt(point.flex23) * 15.5 < UINT8_MAX ? sqrt(point.intensity) * 15.5 : UINT8_MAX; + hsv[2] = sqrt(point.flex23) * 15.5 < UINT8_MAX ? sqrt(point.flex23) * 15.5 : UINT8_MAX; hsv[1] = UINT8_MAX - 1; hsv[0] = abs(point.z) * UINT8_MAX / params.max_range; @@ -67,5 +69,39 @@ void generate_depth_image(const pcl::PointCloud& point_cloud, cv: } +void interpolate_hsv_image(cv::Mat& depth_image) { + cv::Mat measured_pixel_mask; + + cv::Mat hsv[3]; //destination array + cv::split(depth_image, hsv);//split source + + cv::threshold(hsv[1], measured_pixel_mask, 1, UINT8_MAX, cv::THRESH_BINARY); + + cv::Mat kernel = cv::Mat::ones(3, 3, CV_32F); + + + cv::Mat smoothed; + cv::Mat weight; + + depth_image.convertTo(smoothed, CV_32FC3, 1/255.0); + measured_pixel_mask.convertTo(weight, CV_32FC3, 1/255.0); + + cv::filter2D(smoothed, smoothed, -1, kernel); + cv::filter2D(weight, weight, -1, kernel); + + cv::Mat smooth_hsv[3]; //destination array + cv::split(smoothed, smooth_hsv);//split source + for (size_t i = 0; i < 3; i++) { + cv::divide(smooth_hsv[i], weight, smooth_hsv[i]); + } + + cv::merge(smooth_hsv, 3, smoothed); + + smoothed.convertTo(smoothed, CV_8UC3, 255.0); + cv::bitwise_not(measured_pixel_mask, measured_pixel_mask); + + cv::add(depth_image, smoothed, depth_image, measured_pixel_mask); +} + } //lidar } //vtr \ No newline at end of file diff --git a/main/src/vtr_lidar/src/modules/planning/perspective_render_module.cpp b/main/src/vtr_lidar/src/modules/planning/perspective_render_module.cpp index 283ed7222..50612ba03 100644 --- a/main/src/vtr_lidar/src/modules/planning/perspective_render_module.cpp +++ b/main/src/vtr_lidar/src/modules/planning/perspective_render_module.cpp @@ -21,7 +21,6 @@ #include "cv_bridge/cv_bridge.h" #include "opencv2/imgproc.hpp" - namespace vtr { namespace lidar { @@ -60,7 +59,7 @@ void PerspectiveRenderModule::run_(QueryCache &qdata0, OutputCache &output0, auto raw_point_cloud = *qdata.raw_point_cloud; if(!qdata.submap_loc.valid()) { - CLOG(WARNING, "lidar.range_change") << "Range image requires a map to work"; + CLOG(WARNING, "lidar.perspective") << "Perspective image requires a map to work"; return; } @@ -68,9 +67,7 @@ void PerspectiveRenderModule::run_(QueryCache &qdata0, OutputCache &output0, const auto &T_r_v_loc = *qdata.T_r_v_loc; const auto &T_v_m_loc = *qdata.T_v_m_loc; const auto &sid_loc = *qdata.sid_loc; - - CLOG(DEBUG, "lidar.perspective") << "Hello world"; - + const auto &vid_loc = *qdata.vid_loc; if (!publisher_initialized_) { live_img_pub_ = qdata.node->create_publisher("live_range_coloured", 5); @@ -79,15 +76,27 @@ void PerspectiveRenderModule::run_(QueryCache &qdata0, OutputCache &output0, CLOG(DEBUG, "lidar.perspective") << "Creating publisher"; } - + auto map_vertex = graph->at(vid_loc); + auto nn_map_scan = [&] { + auto locked_nn_pc_msg = map_vertex->retrieve>( + "raw_point_cloud", "vtr_lidar_msgs/msg/PointScan"); + if (locked_nn_pc_msg != nullptr) { + auto locked_msg = locked_nn_pc_msg->sharedLocked(); + return locked_msg.get().getDataPtr(); + } + return std::make_shared>(); + }(); + auto nn_map_point_cloud = nn_map_scan->point_cloud(); + + auto map_nn_mat = nn_map_point_cloud.getMatrixXfMap(4, PointWithInfo::size(), PointWithInfo::cartesian_offset()); + const auto T_s_sm = (T_s_r * T_r_v_loc * nn_map_scan->T_vertex_this()).matrix(); + auto& sub_map= *qdata.submap_loc; auto map_point_cloud = sub_map.point_cloud(); auto map_points_mat = map_point_cloud.getMatrixXfMap(4, PointWithInfo::size(), PointWithInfo::cartesian_offset()); - - CLOG(DEBUG, "lidar.range") << "Before: (" << map_point_cloud[10].x << ", " << map_point_cloud[10].y << ", "<< map_point_cloud[10].z <<")"; const auto T_s_m = (T_s_r * T_r_v_loc * T_v_m_loc).matrix(); @@ -103,6 +112,7 @@ void PerspectiveRenderModule::run_(QueryCache &qdata0, OutputCache &output0, live_points_mat = T_c_s * live_points_mat; map_points_mat = (T_c_s * T_s_m.cast()) * map_points_mat; + map_nn_mat = (T_c_s * T_s_sm.cast()) * map_nn_mat; @@ -111,6 +121,7 @@ void PerspectiveRenderModule::run_(QueryCache &qdata0, OutputCache &output0, cv::Mat live_rgb_img = cv::Mat::zeros(config_->perspective_params.height, config_->perspective_params.width, CV_8UC3); generate_depth_image(raw_point_cloud, live_hsv_img, live_index_img, config_->perspective_params); + interpolate_hsv_image(live_hsv_img); cv::cvtColor(live_hsv_img, live_rgb_img, cv::COLOR_HSV2RGB); @@ -127,16 +138,25 @@ void PerspectiveRenderModule::run_(QueryCache &qdata0, OutputCache &output0, cv::Mat map_hsv_img = cv::Mat::zeros(config_->perspective_params.height, config_->perspective_params.width, CV_8UC3); cv::Mat map_rgb_img = cv::Mat::zeros(config_->perspective_params.height, config_->perspective_params.width, CV_8UC3); + generate_depth_image(map_point_cloud, map_hsv_img, map_index_img, config_->perspective_params); + generate_depth_image(nn_map_point_cloud, map_hsv_img, map_index_img, config_->perspective_params); + interpolate_hsv_image(map_hsv_img); + cv::cvtColor(map_hsv_img, map_rgb_img, cv::COLOR_HSV2RGB); + cv::Mat diff; + cv::subtract(live_rgb_img, map_rgb_img, diff); + + qdata.rendered_images.emplace(std::make_pair(live_rgb_img, map_rgb_img)); + if (config_->visualize) { cv_bridge::CvImage map_cv_rgb_img; map_cv_rgb_img.header.frame_id = "lidar"; //cv_rgb_img.header.stamp = qdata.stamp->header.stamp; map_cv_rgb_img.encoding = "rgb8"; - map_cv_rgb_img.image = map_rgb_img; + map_cv_rgb_img.image = diff; map_img_pub_->publish(*map_cv_rgb_img.toImageMsg()); } } diff --git a/main/src/vtr_lidar/src/modules/preprocessing/conversions/ouster_conversion_module.cpp b/main/src/vtr_lidar/src/modules/preprocessing/conversions/ouster_conversion_module.cpp index 0b87be1d0..d4fb36ef3 100644 --- a/main/src/vtr_lidar/src/modules/preprocessing/conversions/ouster_conversion_module.cpp +++ b/main/src/vtr_lidar/src/modules/preprocessing/conversions/ouster_conversion_module.cpp @@ -99,6 +99,7 @@ void OusterConversionModule::run_(QueryCache &qdata0, OutputCache &, point_cloud->at(idx).y = *iter_y; point_cloud->at(idx).z = *iter_z; point_cloud->at(idx).intensity = *iter_intensity; + point_cloud->at(idx).flex23 = point_cloud->at(idx).intensity; // pointwise timestamp //CLOG(DEBUG, "lidar.ouster_converter") << "Timestamp is: " << *iter_time; diff --git a/main/src/vtr_torch/include/vtr_torch/modules/torch_module.hpp b/main/src/vtr_torch/include/vtr_torch/modules/torch_module.hpp index 1aff3c680..6ee885918 100644 --- a/main/src/vtr_torch/include/vtr_torch/modules/torch_module.hpp +++ b/main/src/vtr_torch/include/vtr_torch/modules/torch_module.hpp @@ -96,6 +96,8 @@ class TorchModule : public tactic::BaseModule { template torch::Tensor evaluateModel(std::vector inputs, const Shape shape); + torch::Tensor evaluateModel(torch::Tensor input, const Shape shape); + }; } // namespace nn diff --git a/main/src/vtr_torch/src/modules/torch_module.cpp b/main/src/vtr_torch/src/modules/torch_module.cpp index 0a11bde0e..c1c381d40 100644 --- a/main/src/vtr_torch/src/modules/torch_module.cpp +++ b/main/src/vtr_torch/src/modules/torch_module.cpp @@ -33,7 +33,7 @@ auto TorchModule::Config::fromROS(const rclcpp::Node::SharedPtr &node, config->use_gpu = node->declare_parameter(param_prefix + ".use_gpu", config->use_gpu); config->abs_filepath = node->declare_parameter(param_prefix + ".abs_filepath", config->abs_filepath); - auto model_dir = node->declare_parameter("model_dir", "defalut2"); + auto model_dir = node->declare_parameter("model_dir", "default"); model_dir = common::utils::expand_user(common::utils::expand_env(model_dir)); if (config->abs_filepath){ @@ -47,5 +47,18 @@ auto TorchModule::Config::fromROS(const rclcpp::Node::SharedPtr &node, TorchModule::~TorchModule() {} + +torch::Tensor TorchModule::evaluateModel(torch::Tensor input, const Shape shape) { + torch::NoGradGuard no_grad; + std::vector jit_inputs; + + jit_inputs.push_back(input.to(device)); + + + auto output = network(jit_inputs); + + return output.toTensor().cpu(); +} + } // namespace nn } // namespace vtr \ No newline at end of file From 4a303885d19fd880f7d2c0ce736dc2af48c11e30 Mon Sep 17 00:00:00 2001 From: a-krawciw Date: Wed, 28 Feb 2024 15:35:28 -0500 Subject: [PATCH 57/77] Add SegmentAnything Model from torch. --- config/ouster_warthog_default.yaml | 17 +-- config/ouster_warthog_lasersam.yaml | 10 +- .../src/vtr_lidar/include/vtr_lidar/cache.hpp | 2 + .../include/vtr_lidar/modules/modules.hpp | 1 + .../planning/segmentanything_module.hpp | 80 ++++++++++++++ .../planning/segmentanything_module.cpp | 102 ++++++++++++++++++ .../vtr_torch/modules/torch_module.hpp | 3 +- 7 files changed, 203 insertions(+), 12 deletions(-) create mode 100644 main/src/vtr_lidar/include/vtr_lidar/modules/planning/segmentanything_module.hpp create mode 100644 main/src/vtr_lidar/src/modules/planning/segmentanything_module.cpp diff --git a/config/ouster_warthog_default.yaml b/config/ouster_warthog_default.yaml index 84e98fe00..914151b3a 100644 --- a/config/ouster_warthog_default.yaml +++ b/config/ouster_warthog_default.yaml @@ -34,7 +34,7 @@ # lidar pipeline - lidar.pipeline - #- lidar.preprocessing + - lidar.preprocessing #- lidar.ouster_converter #- lidar.odometry_icp #- lidar.odometry_map_maintenance @@ -91,7 +91,7 @@ - mapping - vertex_test - intra_exp_merging - - dynamic_detection + #- dynamic_detection - inter_exp_merging - memory localization: @@ -102,6 +102,7 @@ - memory submap_translation_threshold: 1.5 submap_rotation_threshold: 30.0 + save_raw_point_cloud: True preprocessing: conversion: type: lidar.ouster_converter @@ -117,7 +118,7 @@ num_threads: 8 crop_range: 40.0 - frame_voxel_size: 0.3 # grid subsampling voxel size + frame_voxel_size: 0.1 # grid subsampling voxel size vertical_angle_res: 0.0061365 # vertical angle resolution in radians, equal to 0.3516 degree documented in the manual polar_r_scale: 2.0 # polar_r_scale x vertical_angle_res = nearest neighbor search radius for normal computation @@ -167,10 +168,10 @@ mapping: type: lidar.odometry_map_maintenance_v2 - map_voxel_size: 0.3 + map_voxel_size: 0.1 crop_range_front: 40.0 - back_over_front_ratio: 0.5 + back_over_front_ratio: 1.0 point_life_time: 20.0 visualize: true vertex_test: @@ -182,10 +183,10 @@ type: lidar.intra_exp_merging_v2 depth: 6.0 - map_voxel_size: 0.3 + map_voxel_size: 0.1 crop_range_front: 40.0 - back_over_front_ratio: 0.5 + back_over_front_ratio: 1.0 visualize: true dynamic_detection: type: lidar.dynamic_detection @@ -200,7 +201,7 @@ inter_exp_merging: type: "lidar.inter_exp_merging_v2" - map_voxel_size: 0.3 + map_voxel_size: 0.1 max_num_experiences: 128 distance_threshold: 0.6 planar_threshold: 0.2 diff --git a/config/ouster_warthog_lasersam.yaml b/config/ouster_warthog_lasersam.yaml index 79a243ada..961b8b3a5 100644 --- a/config/ouster_warthog_lasersam.yaml +++ b/config/ouster_warthog_lasersam.yaml @@ -5,21 +5,20 @@ - navigation.sensor_input - tactic.pipeline - tactic.module - #- tactic.module.live_mem_manager - #- tactic.module.graph_mem_manager - # pose graph #- pose_graph # lidar pipeline - lidar.pipeline + - torch pipeline: localization: - recall - icp - safe_corridor - perspective + - segmentation - memory localization: @@ -32,3 +31,8 @@ min_range: 0.5 max_range: 50.0 visualize: true + segmentation: + type: lidar.SAM + use_gpu: true + abs_filepath: false + filepath: sam_vit_b_cuda.torchscript diff --git a/main/src/vtr_lidar/include/vtr_lidar/cache.hpp b/main/src/vtr_lidar/include/vtr_lidar/cache.hpp index 3553cbd5f..f2d63bd21 100644 --- a/main/src/vtr_lidar/include/vtr_lidar/cache.hpp +++ b/main/src/vtr_lidar/include/vtr_lidar/cache.hpp @@ -19,6 +19,7 @@ #pragma once #include "sensor_msgs/msg/point_cloud2.hpp" +#include "opencv2/core.hpp" #include "vtr_lidar/data_types/costmap.hpp" #include "vtr_lidar/data_types/point.hpp" @@ -61,6 +62,7 @@ struct LidarQueryCache : virtual public tactic::QueryCache { // dynamic detection async tactic::Cache dynamic_detection_async; + tactic::Cache> rendered_images; // inter exp merging async (priv, curr, T_priv_curr) tactic::Cache +#include "vtr_lidar/cache.hpp" +#include +#include + +#include "sensor_msgs/msg/image.hpp" + + + +namespace vtr { +namespace lidar { + +using ImageMsg = sensor_msgs::msg::Image; +using PointCloudMsg = sensor_msgs::msg::PointCloud2; + + +/** \brief Load and store Torch Models */ +class SegmentAnythingModule : public nn::TorchModule { + public: + PTR_TYPEDEFS(SegmentAnythingModule); + + /** \brief Static module identifier. */ + static constexpr auto static_name = "lidar.SAM"; + + /** \brief Config parameters. */ + struct Config : public nn::TorchModule::Config { + PTR_TYPEDEFS(Config); + + bool visualize = false; + + static ConstPtr fromROS(const rclcpp::Node::SharedPtr &node, + const std::string ¶m_prefix); + + }; + + SegmentAnythingModule( + const Config::ConstPtr &config, + const std::shared_ptr &module_factory = nullptr, + const std::string &name = static_name) + : nn::TorchModule{config, module_factory, name}, config_(config) {} + + private: + void run_(tactic::QueryCache &qdata, tactic::OutputCache &output, + const tactic::Graph::Ptr &graph, + const tactic::TaskExecutor::Ptr &executor) override; + + Config::ConstPtr config_; + + rclcpp::Publisher::SharedPtr mask_pub_; + bool pub_init_ = false; + + + VTR_REGISTER_MODULE_DEC_TYPE(SegmentAnythingModule); + +}; + +} // namespace lidar +} // namespace vtr \ No newline at end of file diff --git a/main/src/vtr_lidar/src/modules/planning/segmentanything_module.cpp b/main/src/vtr_lidar/src/modules/planning/segmentanything_module.cpp new file mode 100644 index 000000000..c5b2118ef --- /dev/null +++ b/main/src/vtr_lidar/src/modules/planning/segmentanything_module.cpp @@ -0,0 +1,102 @@ +// Copyright 2024, 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 segmentanything_module.cpp + * \author Alec Krawciw, Autonomous Space Robotics Lab (ASRL) + */ +#include + + + +namespace vtr { +namespace lidar { + +using namespace tactic; + +auto SegmentAnythingModule::Config::fromROS(const rclcpp::Node::SharedPtr &node, + const std::string ¶m_prefix) + -> ConstPtr { + auto config = std::make_shared(); + auto base_config = std::static_pointer_cast(config); + *base_config = *nn::TorchModule::Config::fromROS(node, param_prefix); + + // clang-format off + config->visualize = node->declare_parameter(param_prefix + ".visualize", config->visualize); + + // clang-format on + return config; +} + + +void SegmentAnythingModule::run_(QueryCache &qdata0, OutputCache &, + const Graph::Ptr &graph, const TaskExecutor::Ptr &) { + auto &qdata = dynamic_cast(qdata0); + + + auto nn_point_cloud = *qdata.nn_point_cloud; + const auto &T_s_r = *qdata.T_s_r; + const auto &T_r_v_loc = *qdata.T_r_v_loc; + const auto &T_v_m_loc = *qdata.T_v_m_loc; + const auto &sid_loc = *qdata.sid_loc; + + CLOG(DEBUG, "running SAM"); + + + if(!qdata.rendered_images.valid()) { + CLOG(WARNING, "lidar.perspective") << "Rendered perspective images required a map to work!"; + return; + } + + if(!pub_init_){ + mask_pub_ = qdata.node->create_publisher("detection_mask", 5); + pub_init_ = true; + } + + auto& [live_img, map_img]= *qdata.rendered_images; + + CLOG(DEBUG, "lidar.perspective") << "Received images! "; + + + using namespace torch::indexing; + + + auto input = torch::zeros({1, 3, 1024, 1024}); + input.index_put_({0, "...", Slice(0, 128), Slice(0, 256)}, torch::from_blob(live_img.ptr(), {128, 256})); + + torch::NoGradGuard no_grad; + std::vector jit_inputs; + + jit_inputs.push_back(input.to(device)); + + auto output = network(jit_inputs); + + CLOG(DEBUG, "lidar.perspective") << "Ran model!"; + + auto mask = output.toGenericDict().at("masks"); + + + + /// publish the transformed pointcloud + if (config_->visualize) { + + + // mask_pub_->publish(range_to_image(mask_image)); + + } + +} + +} // namespace nn +} // namespace vtr \ No newline at end of file diff --git a/main/src/vtr_torch/include/vtr_torch/modules/torch_module.hpp b/main/src/vtr_torch/include/vtr_torch/modules/torch_module.hpp index 6ee885918..79db49e9f 100644 --- a/main/src/vtr_torch/include/vtr_torch/modules/torch_module.hpp +++ b/main/src/vtr_torch/include/vtr_torch/modules/torch_module.hpp @@ -87,11 +87,12 @@ class TorchModule : public tactic::BaseModule { const tactic::TaskExecutor::Ptr &executor) = 0; Config::ConstPtr config_; - torch::Device device = torch::kCPU; protected: Module network; + torch::Device device = torch::kCPU; + template torch::Tensor evaluateModel(std::vector inputs, const Shape shape); From a4f1850fd87df95f727c36958846fea3dc013ab5 Mon Sep 17 00:00:00 2001 From: a-krawciw Date: Tue, 5 Mar 2024 15:49:00 -0500 Subject: [PATCH 58/77] Initial stage of running live on the robot. --- config/ouster_warthog_lasersam.yaml | 14 +- .../src/vtr_lidar/include/vtr_lidar/cache.hpp | 2 +- .../vtr_lidar/filters/perspective_image.hpp | 1 + .../planning/segmentanything_module.hpp | 16 +- .../src/filters/perspective_image.cpp | 11 +- .../planning/perspective_render_module.cpp | 4 +- .../planning/segmentanything_module.cpp | 262 ++++++++++++++++-- rviz/lidar.rviz | 92 +++++- 8 files changed, 356 insertions(+), 46 deletions(-) diff --git a/config/ouster_warthog_lasersam.yaml b/config/ouster_warthog_lasersam.yaml index 961b8b3a5..19a3e1781 100644 --- a/config/ouster_warthog_lasersam.yaml +++ b/config/ouster_warthog_lasersam.yaml @@ -17,22 +17,20 @@ - recall - icp - safe_corridor - - perspective - segmentation - memory localization: - perspective: - type: lidar.render_perspective + segmentation: + type: lidar.SAM + use_gpu: true + abs_filepath: false + filepath: sam_vit_b_cuda.torchscript img_width: 256 img_height: 128 h_fov: 1.5707 v_fov: 0.7853 min_range: 0.5 max_range: 50.0 + num_prompts: 4 visualize: true - segmentation: - type: lidar.SAM - use_gpu: true - abs_filepath: false - filepath: sam_vit_b_cuda.torchscript diff --git a/main/src/vtr_lidar/include/vtr_lidar/cache.hpp b/main/src/vtr_lidar/include/vtr_lidar/cache.hpp index f2d63bd21..a67595bcb 100644 --- a/main/src/vtr_lidar/include/vtr_lidar/cache.hpp +++ b/main/src/vtr_lidar/include/vtr_lidar/cache.hpp @@ -62,7 +62,7 @@ struct LidarQueryCache : virtual public tactic::QueryCache { // dynamic detection async tactic::Cache dynamic_detection_async; - tactic::Cache> rendered_images; + tactic::Cache> rendered_images; // inter exp merging async (priv, curr, T_priv_curr) tactic::Cache& point_cloud, const cv: void interpolate_hsv_image(cv::Mat& depth_image); +void mask_to_pointcloud(const cv::Mat& mask, const cv::Mat& index_img, pcl::PointCloud& point_cloud); } // namespace lidar } // vtr \ No newline at end of file diff --git a/main/src/vtr_lidar/include/vtr_lidar/modules/planning/segmentanything_module.hpp b/main/src/vtr_lidar/include/vtr_lidar/modules/planning/segmentanything_module.hpp index 4cd3907a0..45bd88722 100644 --- a/main/src/vtr_lidar/include/vtr_lidar/modules/planning/segmentanything_module.hpp +++ b/main/src/vtr_lidar/include/vtr_lidar/modules/planning/segmentanything_module.hpp @@ -22,6 +22,8 @@ #include "vtr_tactic/task_queue.hpp" #include #include "vtr_lidar/cache.hpp" +#include "vtr_lidar/filters/perspective_image.hpp" + #include #include @@ -49,6 +51,11 @@ class SegmentAnythingModule : public nn::TorchModule { PTR_TYPEDEFS(Config); bool visualize = false; + PerspectiveImageParams perspective_params; + + float iou_thresh = 0.5; + int num_prompts = 4; + static ConstPtr fromROS(const rclcpp::Node::SharedPtr &node, const std::string ¶m_prefix); @@ -68,9 +75,16 @@ class SegmentAnythingModule : public nn::TorchModule { Config::ConstPtr config_; - rclcpp::Publisher::SharedPtr mask_pub_; bool pub_init_ = false; + rclcpp::Publisher::SharedPtr mask_pub_; + rclcpp::Publisher::SharedPtr diff_pub_; + rclcpp::Publisher::SharedPtr live_img_pub_; + rclcpp::Publisher::SharedPtr map_img_pub_; + rclcpp::Publisher::SharedPtr filtered_pub_; + + + VTR_REGISTER_MODULE_DEC_TYPE(SegmentAnythingModule); diff --git a/main/src/vtr_lidar/src/filters/perspective_image.cpp b/main/src/vtr_lidar/src/filters/perspective_image.cpp index aacee9098..bfbbfbc1d 100644 --- a/main/src/vtr_lidar/src/filters/perspective_image.cpp +++ b/main/src/vtr_lidar/src/filters/perspective_image.cpp @@ -79,7 +79,6 @@ void interpolate_hsv_image(cv::Mat& depth_image) { cv::Mat kernel = cv::Mat::ones(3, 3, CV_32F); - cv::Mat smoothed; cv::Mat weight; @@ -103,5 +102,15 @@ void interpolate_hsv_image(cv::Mat& depth_image) { cv::add(depth_image, smoothed, depth_image, measured_pixel_mask); } +void mask_to_pointcloud(const cv::Mat& mask, const cv::Mat& index_img, pcl::PointCloud& point_cloud) { + index_img.forEach +( + [&](int32_t &idx, const int position[]) -> void + { + point_cloud[idx].flex24 = (mask.at(position[0], position[1]) > 0) ? 2 : 1; + } +); +} + } //lidar } //vtr \ No newline at end of file diff --git a/main/src/vtr_lidar/src/modules/planning/perspective_render_module.cpp b/main/src/vtr_lidar/src/modules/planning/perspective_render_module.cpp index 50612ba03..805f85d82 100644 --- a/main/src/vtr_lidar/src/modules/planning/perspective_render_module.cpp +++ b/main/src/vtr_lidar/src/modules/planning/perspective_render_module.cpp @@ -139,7 +139,7 @@ void PerspectiveRenderModule::run_(QueryCache &qdata0, OutputCache &output0, cv::Mat map_rgb_img = cv::Mat::zeros(config_->perspective_params.height, config_->perspective_params.width, CV_8UC3); - generate_depth_image(map_point_cloud, map_hsv_img, map_index_img, config_->perspective_params); + // generate_depth_image(map_point_cloud, map_hsv_img, map_index_img, config_->perspective_params); generate_depth_image(nn_map_point_cloud, map_hsv_img, map_index_img, config_->perspective_params); interpolate_hsv_image(map_hsv_img); @@ -156,7 +156,7 @@ void PerspectiveRenderModule::run_(QueryCache &qdata0, OutputCache &output0, map_cv_rgb_img.header.frame_id = "lidar"; //cv_rgb_img.header.stamp = qdata.stamp->header.stamp; map_cv_rgb_img.encoding = "rgb8"; - map_cv_rgb_img.image = diff; + map_cv_rgb_img.image = map_rgb_img; map_img_pub_->publish(*map_cv_rgb_img.toImageMsg()); } } diff --git a/main/src/vtr_lidar/src/modules/planning/segmentanything_module.cpp b/main/src/vtr_lidar/src/modules/planning/segmentanything_module.cpp index c5b2118ef..16b68c0ff 100644 --- a/main/src/vtr_lidar/src/modules/planning/segmentanything_module.cpp +++ b/main/src/vtr_lidar/src/modules/planning/segmentanything_module.cpp @@ -18,6 +18,8 @@ */ #include +#include +#include "cv_bridge/cv_bridge.h" namespace vtr { @@ -32,6 +34,19 @@ auto SegmentAnythingModule::Config::fromROS(const rclcpp::Node::SharedPtr &node, auto base_config = std::static_pointer_cast(config); *base_config = *nn::TorchModule::Config::fromROS(node, param_prefix); + + auto& params = config->perspective_params; + + params.width = node->declare_parameter(param_prefix + ".img_width", 0); + params.height = node->declare_parameter(param_prefix + ".img_height", 0); + params.h_fov = node->declare_parameter(param_prefix + ".h_fov", M_PI/2); + params.v_fov = node->declare_parameter(param_prefix + ".v_fov", M_PI/4); + params.max_range = node->declare_parameter(param_prefix + ".max_range", params.max_range); + params.min_range = node->declare_parameter(param_prefix + ".min_range", params.min_range); + + config->iou_thresh = node->declare_parameter(param_prefix + ".iou_threshold", config->iou_thresh); + config->num_prompts = node->declare_parameter(param_prefix + ".num_prompts", config->num_prompts); + // clang-format off config->visualize = node->declare_parameter(param_prefix + ".visualize", config->visualize); @@ -45,55 +60,262 @@ void SegmentAnythingModule::run_(QueryCache &qdata0, OutputCache &, auto &qdata = dynamic_cast(qdata0); - auto nn_point_cloud = *qdata.nn_point_cloud; + // if(!qdata.rendered_images.valid()) { + // CLOG(WARNING, "lidar.perspective") << "Rendered perspective images required a map to work!"; + // return; + // } + + if(!pub_init_){ + mask_pub_ = qdata.node->create_publisher("detection_mask", 5); + diff_pub_ = qdata.node->create_publisher("normed_diff", 5); + live_img_pub_ = qdata.node->create_publisher("live_range_coloured", 5); + map_img_pub_ = qdata.node->create_publisher("map_range_coloured", 5); + filtered_pub_ = qdata.node->create_publisher("changed_point_cloud", 5); + + pub_init_ = true; + } + + auto raw_point_cloud = *qdata.raw_point_cloud; + + if(!qdata.submap_loc.valid()) { + CLOG(WARNING, "lidar.perspective") << "Perspective image requires a map to work"; + return; + } + const auto &T_s_r = *qdata.T_s_r; const auto &T_r_v_loc = *qdata.T_r_v_loc; const auto &T_v_m_loc = *qdata.T_v_m_loc; - const auto &sid_loc = *qdata.sid_loc; + const auto &vid_loc = *qdata.vid_loc; + + auto map_vertex = graph->at(vid_loc); + auto nn_map_scan = [&] { + auto locked_nn_pc_msg = map_vertex->retrieve>( + "raw_point_cloud", "vtr_lidar_msgs/msg/PointScan"); + + if (locked_nn_pc_msg != nullptr) { + auto locked_msg = locked_nn_pc_msg->sharedLocked(); + return locked_msg.get().getDataPtr(); + } + return std::make_shared>(); + }(); + + auto nn_map_point_cloud = nn_map_scan->point_cloud(); + + auto map_nn_mat = nn_map_point_cloud.getMatrixXfMap(4, PointWithInfo::size(), PointWithInfo::cartesian_offset()); + const auto T_s_sm = (T_s_r * T_r_v_loc * nn_map_scan->T_vertex_this()).matrix(); - CLOG(DEBUG, "running SAM"); + auto& sub_map= *qdata.submap_loc; + auto map_point_cloud = sub_map.point_cloud(); + auto map_points_mat = map_point_cloud.getMatrixXfMap(4, PointWithInfo::size(), PointWithInfo::cartesian_offset()); + + const auto T_s_m = (T_s_r * T_r_v_loc * T_v_m_loc).matrix(); - if(!qdata.rendered_images.valid()) { - CLOG(WARNING, "lidar.perspective") << "Rendered perspective images required a map to work!"; - return; - } - if(!pub_init_){ - mask_pub_ = qdata.node->create_publisher("detection_mask", 5); - pub_init_ = true; + auto live_points_mat = raw_point_cloud.getMatrixXfMap(4, PointWithInfo::size(), PointWithInfo::cartesian_offset()); + + Eigen::Matrix4f T_c_s; + + T_c_s << 0, -1, 0, 0, + 0, 0, 1, 0, + -1, 0, 0, 0, + 0, 0, 0, 1; + + live_points_mat = T_c_s * live_points_mat; + map_points_mat = (T_c_s * T_s_m.cast()) * map_points_mat; + map_nn_mat = (T_c_s * T_s_sm.cast()) * map_nn_mat; + + + + cv::Mat live_index_img = cv::Mat::zeros(config_->perspective_params.height, config_->perspective_params.width, CV_32S); + cv::Mat live_hsv_img = cv::Mat::zeros(config_->perspective_params.height, config_->perspective_params.width, CV_8UC3); + cv::Mat live_rgb_img = cv::Mat::zeros(config_->perspective_params.height, config_->perspective_params.width, CV_8UC3); + + generate_depth_image(raw_point_cloud, live_hsv_img, live_index_img, config_->perspective_params); + interpolate_hsv_image(live_hsv_img); + + cv::cvtColor(live_hsv_img, live_rgb_img, cv::COLOR_HSV2RGB); + + if (config_->visualize) { + cv_bridge::CvImage live_cv_rgb_img; + live_cv_rgb_img.header.frame_id = "lidar"; + //cv_rgb_img.header.stamp = qdata.stamp->header.stamp; + live_cv_rgb_img.encoding = "rgb8"; + live_cv_rgb_img.image = live_rgb_img; + live_img_pub_->publish(*live_cv_rgb_img.toImageMsg()); } - auto& [live_img, map_img]= *qdata.rendered_images; + cv::Mat map_index_img = cv::Mat::zeros(config_->perspective_params.height, config_->perspective_params.width, CV_32S); + cv::Mat map_hsv_img = cv::Mat::zeros(config_->perspective_params.height, config_->perspective_params.width, CV_8UC3); + cv::Mat map_rgb_img = cv::Mat::zeros(config_->perspective_params.height, config_->perspective_params.width, CV_8UC3); + + + // generate_depth_image(map_point_cloud, map_hsv_img, map_index_img, config_->perspective_params); + generate_depth_image(nn_map_point_cloud, map_hsv_img, map_index_img, config_->perspective_params); + interpolate_hsv_image(map_hsv_img); + + + cv::cvtColor(map_hsv_img, map_rgb_img, cv::COLOR_HSV2RGB); + + // qdata.rendered_images.emplace(std::make_pair(live_rgb_img, map_rgb_img)); + + if (config_->visualize) { + cv_bridge::CvImage map_cv_rgb_img; + map_cv_rgb_img.header.frame_id = "lidar"; + //cv_rgb_img.header.stamp = qdata.stamp->header.stamp; + map_cv_rgb_img.encoding = "rgb8"; + map_cv_rgb_img.image = map_rgb_img; + map_img_pub_->publish(*map_cv_rgb_img.toImageMsg()); + } CLOG(DEBUG, "lidar.perspective") << "Received images! "; + cv::Mat big_live; + cv::Mat big_map; + + cv::resize(live_rgb_img, big_live, cv::Size(1024, 512), cv::INTER_LINEAR); + cv::resize(map_rgb_img, big_map, cv::Size(1024, 512), cv::INTER_LINEAR); + + if (!big_live.isContinuous()){ + big_live = big_live.clone(); + CLOG(WARNING, "lidar.perspective") << "Live image was not contiguous in memory!"; + } + if (!big_map.isContinuous()){ + big_map = big_map.clone(); + CLOG(WARNING, "lidar.perspective") << "Map image was not contiguous in memory!"; + } + + + torch::Tensor live_tensor = torch::from_blob(big_live.data, {big_live.rows, big_live.cols, big_live.channels()}, torch::kByte).to(torch::kFloat32).permute({2, 0, 1}).contiguous(); + torch::Tensor map_tensor = torch::from_blob(big_map.data, {big_map.rows, big_map.cols, big_map.channels()}, torch::kByte).to(torch::kFloat32).permute({2, 0, 1}).contiguous(); + + torch::Tensor diff = live_tensor - map_tensor; + diff = diff.norm(2, {0}); + const auto [topk_vals, topk_idx] = diff.flatten().topk(4l); using namespace torch::indexing; + // int data[] = { 512, 256, + // 256, 256, + // 768, 256 }; + std::vector prompts; + + auto idxs_a = topk_idx.accessor(); + + for (size_t i = 0; i < idxs_a.size(0); i++) { + const long& idx = idxs_a[i]; + int prompt[] = {idx % diff.size(1), idx / diff.size(1)}; + CLOG(DEBUG, "lidar.perspective") << "Diff Tensor prompts (" << prompt[0] << ", " << prompt[1] << ")"; - auto input = torch::zeros({1, 3, 1024, 1024}); - input.index_put_({0, "...", Slice(0, 128), Slice(0, 256)}, torch::from_blob(live_img.ptr(), {128, 256})); + if (map_tensor.index({0, idx % diff.size(1), idx / diff.size(1)}) > 0) + prompts.push_back(torch::from_blob(prompt, {2}, torch::kInt).to(device)); + else + CLOG(DEBUG, "lidar.perspective") << "Prompt point on an empty map pixel. Try again." + } + + // prompts.push_back(torch::from_blob(&data[2], {2}, torch::kInt).to(device)); + // prompts.push_back(torch::from_blob(&data[4], {2}, torch::kInt).to(device)); torch::NoGradGuard no_grad; std::vector jit_inputs; - jit_inputs.push_back(input.to(device)); + jit_inputs.push_back(map_tensor.to(device)); + jit_inputs.push_back(live_tensor.to(device)); + jit_inputs.push_back(prompts); - auto output = network(jit_inputs); + auto outputs = network(jit_inputs).toGenericDict(); CLOG(DEBUG, "lidar.perspective") << "Ran model!"; - auto mask = output.toGenericDict().at("masks"); + auto teach_masks = outputs.at("teach_masks").toTensor(); + auto repeat_masks = outputs.at("repeat_masks").toTensor(); + auto intersection = teach_masks.bitwise_and(repeat_masks); + auto unions = teach_masks.bitwise_or(repeat_masks); - - /// publish the transformed pointcloud - if (config_->visualize) { + auto ious = intersection.sum({1, 2}) / unions.sum({1, 2}); + + CLOG(DEBUG, "lidar.perspective") << "IoUS " << ious.cpu(); + + std::vector idxs_to_keep; + auto ious_a = ious.accessor(); - // mask_pub_->publish(range_to_image(mask_image)); + for (size_t i = 0; i < ious_a.size(0); i++) { + CLOG(DEBUG, "lidar.perspective") << "Iou VAL " << ious_a[i] << " < " << config_->iou_thresh; + if (ious_a[i] < config_->iou_thresh) { + idxs_to_keep.push_back(i); + } + } + + CLOG(DEBUG, "lidar.perspective") << "IDs to KEEP " << idxs_to_keep; + + cv::Mat repeat_mask = cv::Mat::zeros(teach_masks.size(1), teach_masks.size(2), CV_8UC1); + + if (idxs_to_keep.size() > 0){ + auto changes = repeat_masks.index({torch::from_blob(idxs_to_keep.data(), {idxs_to_keep.size()}, torch::kInt), Slice(), Slice()}); + CLOG(DEBUG, "lidar.perspective") << "Repeat Tensor size " << changes.sizes(); + + changes = changes.sum({0}).div(changes.max()).to(torch::kByte).mul(255); + + + teach_masks = teach_masks.squeeze().to(torch::kByte).mul(255).contiguous(); + + CLOG(DEBUG, "lidar.perspective") << "Teach Tensor size " << teach_masks.sizes(); + CLOG(DEBUG, "lidar.perspective") << "Tensor type " << teach_masks.dtype() << " with size " << teach_masks.element_size(); + + teach_masks = teach_masks.permute({1, 2, 0}).contiguous(); + + cv::Mat map_mask{teach_masks.size(0), teach_masks.size(1), CV_8UC3, teach_masks.data_ptr()}; + repeat_mask = cv::Mat{changes.size(0), changes.size(1), CV_8UC1, changes.data_ptr()}; + + + mask_to_pointcloud(repeat_mask, live_index_img, raw_point_cloud); + + auto filtered_point_cloud = + std::make_shared>(raw_point_cloud); + + /// changed cropping + { + std::vector indices; + indices.reserve(filtered_point_cloud->size()); + for (size_t i = 0; i < filtered_point_cloud->size(); ++i) { + if ((*filtered_point_cloud)[i].flex24 > 0) + indices.emplace_back(i); + } + *filtered_point_cloud = + pcl::PointCloud(*filtered_point_cloud, indices); + } + + if (config_->visualize) { + PointCloudMsg pc2_msg; + pcl::toROSMsg(*filtered_point_cloud, pc2_msg); + pc2_msg.header.frame_id = "lidar"; //"lidar" for honeycomb + pc2_msg.header.stamp = rclcpp::Time(*qdata.stamp); + filtered_pub_->publish(pc2_msg); + } + + } + + // /// publish the transformed pointcloud + if (config_->visualize) { + cv_bridge::CvImage mask_img_msg; + mask_img_msg.header.frame_id = "lidar"; + //cv_rgb_img.header.stamp = qdata.stamp->header.stamp; + mask_img_msg.encoding = "mono8"; + mask_img_msg.image = repeat_mask; + mask_pub_->publish(*mask_img_msg.toImageMsg()); + + diff = diff.div(diff.max()).mul(255).to(torch::kByte).contiguous(); + CLOG(DEBUG, "lidar.perspective") << "Max: " << diff.max() << " Min: " << diff.min(); + + cv_bridge::CvImage diff_img_msg; + diff_img_msg.header.frame_id = "lidar"; + //cv_rgb_img.header.stamp = qdata.stamp->header.stamp; + diff_img_msg.encoding = "mono8"; + diff_img_msg.image = cv::Mat{diff.size(0), diff.size(1), CV_8UC1, diff.data_ptr()};; + diff_pub_->publish(*diff_img_msg.toImageMsg()); } } diff --git a/rviz/lidar.rviz b/rviz/lidar.rviz index 9696c8638..9fcf5cebf 100644 --- a/rviz/lidar.rviz +++ b/rviz/lidar.rviz @@ -1,13 +1,17 @@ Panels: - Class: rviz_common/Displays - Help Height: 78 + Help Height: 0 Name: Displays Property Tree Widget: Expanded: - /Global Options1 - /Status1 + - /Image1 + - /Image2 + - /Image3 + - /PointCloud21 Splitter Ratio: 0.5 - Tree Height: 699 + Tree Height: 140 - Class: rviz_common/Selection Name: Selection - Class: rviz_common/Tool Properties @@ -25,7 +29,7 @@ Panels: Experimental: false Name: Time SyncMode: 0 - SyncSource: "" + SyncSource: PointCloud2 Visualization Manager: Class: "" Displays: @@ -33,7 +37,7 @@ Visualization Manager: Cell Size: 1 Class: rviz_default_plugins/Grid Color: 160; 160; 164 - Enabled: true + Enabled: false Line Style: Line Width: 0.029999999329447746 Value: Lines @@ -46,7 +50,7 @@ Visualization Manager: Plane: XY Plane Cell Count: 10 Reference Frame: - Value: true + Value: false - Class: rviz_default_plugins/Image Enabled: true Max Value: 1 @@ -61,6 +65,20 @@ Visualization Manager: Reliability Policy: Reliable Value: /vtr/live_range_coloured Value: true + - Class: rviz_default_plugins/Image + Enabled: true + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: Image + Normalize Range: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /vtr/detection_mask + Value: true - Class: rviz_default_plugins/Image Enabled: true Max Value: 1 @@ -75,6 +93,54 @@ Visualization Manager: Reliability Policy: Reliable Value: /vtr/map_range_coloured Value: true + - Class: rviz_default_plugins/Image + Enabled: true + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: Image + Normalize Range: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /vtr/normed_diff + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: false + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: flex24 + Class: rviz_default_plugins/PointCloud2 + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 2 + Min Color: 0; 0; 0 + Min Intensity: 1 + Name: PointCloud2 + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.029999999329447746 + Style: Flat Squares + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /vtr/changed_point_cloud + Use Fixed Frame: true + Use rainbow: true + Value: true Enabled: true Global Options: Background Color: 48; 48; 48 @@ -121,7 +187,7 @@ Visualization Manager: Views: Current: Class: rviz_default_plugins/Orbit - Distance: 10 + Distance: 27.068925857543945 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 @@ -136,20 +202,20 @@ Visualization Manager: Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: 0.785398006439209 + Pitch: -1.419796347618103 Target Frame: Value: Orbit (rviz) - Yaw: 0.785398006439209 + Yaw: 4.668571472167969 Saved: ~ Window Geometry: Displays: collapsed: false - Height: 1536 + Height: 1403 Hide Left Dock: false Hide Right Dock: false Image: collapsed: false - QMainWindow State: 000000ff00000000fd00000004000000000000034400000566fc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b00000344000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d0061006700650100000385000001080000002800fffffffb0000000a0049006d00610067006501000004930000010e0000002800ffffff000000010000010f00000566fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003b00000566000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000009b20000003efc0100000002fb0000000800540069006d00650100000000000009b20000025300fffffffb0000000800540069006d00650100000000000004500000000000000000000005530000056600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd000000040000000000000344000004e1fc020000000cfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b000000c7000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d00610067006501000001080000010f0000002800fffffffb0000000a0049006d006100670065010000021d0000010b0000002800fffffffb0000000a0049006d006100670065010000032e000001180000002800fffffffb0000000a0049006d006100670065010000044c000000d00000002800ffffff000000010000010f000004e1fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003b000004e1000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e1000001970000000300000a000000003efc0100000002fb0000000800540069006d0065010000000000000a000000025300fffffffb0000000800540069006d00650100000000000004500000000000000000000005a1000004e100000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Time: @@ -158,6 +224,6 @@ Window Geometry: collapsed: false Views: collapsed: false - Width: 2482 - X: 78 - Y: 27 + Width: 2560 + X: 0 + Y: 0 From 3716a51b8c90bf5a1051653564f5f5ef42d313b9 Mon Sep 17 00:00:00 2001 From: a-krawciw Date: Mon, 11 Mar 2024 14:26:05 -0400 Subject: [PATCH 59/77] Changes for visualization. --- config/ouster_warthog_lasersam.yaml | 4 +- ...online_ouster_warthog_lasersam.launch.yaml | 6 ++- .../planning/segmentanything_module.cpp | 50 +++++++++++++------ rviz/lidar.rviz | 16 +++--- 4 files changed, 49 insertions(+), 27 deletions(-) diff --git a/config/ouster_warthog_lasersam.yaml b/config/ouster_warthog_lasersam.yaml index 19a3e1781..f2b5e66ed 100644 --- a/config/ouster_warthog_lasersam.yaml +++ b/config/ouster_warthog_lasersam.yaml @@ -12,6 +12,8 @@ # lidar pipeline - lidar.pipeline - torch + tactic: + localization_skippable: true pipeline: localization: - recall @@ -32,5 +34,5 @@ v_fov: 0.7853 min_range: 0.5 max_range: 50.0 - num_prompts: 4 + num_prompts: 5 visualize: true diff --git a/launch/online_ouster_warthog_lasersam.launch.yaml b/launch/online_ouster_warthog_lasersam.launch.yaml index 28a2ed5ff..a842db0a4 100644 --- a/launch/online_ouster_warthog_lasersam.launch.yaml +++ b/launch/online_ouster_warthog_lasersam.launch.yaml @@ -14,7 +14,7 @@ windows: layout: main-horizontal shell_command_before: - source ${VTRSRC}/main/install/setup.bash - #- ros2 run vtr_gui setup_server --ros-args -r __ns:=/vtr + - ros2 run vtr_gui setup_server --ros-args -r __ns:=/vtr panes: - > ros2 launch vtr_navigation vtr.launch.py @@ -24,7 +24,9 @@ windows: use_sim_time:=false planner:="stationary" model_dir:=${VTRROOT}/models - data_dir:=${VTRTEMP}/lidar/sam_testing + + + # data_dir:=${VTRTEMP}/lidar/sam_testing - window_name: vtr_gui layout: main-horizontal diff --git a/main/src/vtr_lidar/src/modules/planning/segmentanything_module.cpp b/main/src/vtr_lidar/src/modules/planning/segmentanything_module.cpp index 16b68c0ff..3aee43a30 100644 --- a/main/src/vtr_lidar/src/modules/planning/segmentanything_module.cpp +++ b/main/src/vtr_lidar/src/modules/planning/segmentanything_module.cpp @@ -45,7 +45,7 @@ auto SegmentAnythingModule::Config::fromROS(const rclcpp::Node::SharedPtr &node, params.min_range = node->declare_parameter(param_prefix + ".min_range", params.min_range); config->iou_thresh = node->declare_parameter(param_prefix + ".iou_threshold", config->iou_thresh); - config->num_prompts = node->declare_parameter(param_prefix + ".num_prompts", config->num_prompts); + config->num_prompts = node->declare_parameter(param_prefix + ".num_prompts", config->num_prompts); // clang-format off config->visualize = node->declare_parameter(param_prefix + ".visualize", config->visualize); @@ -191,7 +191,7 @@ void SegmentAnythingModule::run_(QueryCache &qdata0, OutputCache &, torch::Tensor diff = live_tensor - map_tensor; diff = diff.norm(2, {0}); - const auto [topk_vals, topk_idx] = diff.flatten().topk(4l); + const auto [topk_vals, topk_idx] = diff.flatten().topk(config_->num_prompts); using namespace torch::indexing; @@ -205,12 +205,24 @@ void SegmentAnythingModule::run_(QueryCache &qdata0, OutputCache &, for (size_t i = 0; i < idxs_a.size(0); i++) { const long& idx = idxs_a[i]; int prompt[] = {idx % diff.size(1), idx / diff.size(1)}; - CLOG(DEBUG, "lidar.perspective") << "Diff Tensor prompts (" << prompt[0] << ", " << prompt[1] << ")"; - if (map_tensor.index({0, idx % diff.size(1), idx / diff.size(1)}) > 0) + auto& pc_idx = live_index_img.at(prompt[1] / 4, prompt[0] / 4); + + auto live_point = raw_point_cloud[pc_idx]; + Eigen::Vector4f h_point; + h_point << live_point.x, live_point.y, live_point.z, 1.0f; + h_point = T_s_sm.inverse().cast() * h_point; + + double theta = atan2(h_point[1], h_point[0]); + CLOG(DEBUG, "lidar.perspective") << "Diff Tensor prompt (" << prompt[0] << ", " << prompt[1] << ") Theta: " << theta; + + + //Prompts are x, y rather than row, column + //map_tensor.index({0, prompt[1], prompt[0]}).item().to() > 0 || + if (!((theta > 2.06 && theta < 2.18) || (theta > -2.27 && theta < -2.135))) prompts.push_back(torch::from_blob(prompt, {2}, torch::kInt).to(device)); else - CLOG(DEBUG, "lidar.perspective") << "Prompt point on an empty map pixel. Try again." + CLOG(DEBUG, "lidar.perspective") << "Prompt point on an empty map pixel. Try again."; } // prompts.push_back(torch::from_blob(&data[2], {2}, torch::kInt).to(device)); @@ -251,23 +263,20 @@ void SegmentAnythingModule::run_(QueryCache &qdata0, OutputCache &, CLOG(DEBUG, "lidar.perspective") << "IDs to KEEP " << idxs_to_keep; - cv::Mat repeat_mask = cv::Mat::zeros(teach_masks.size(1), teach_masks.size(2), CV_8UC1); + cv::Mat teach_mask = cv::Mat::zeros(teach_masks.size(1), teach_masks.size(2), CV_8UC1); + cv::Mat repeat_mask = cv::Mat::zeros(repeat_masks.size(1), repeat_masks.size(2), CV_8UC1); if (idxs_to_keep.size() > 0){ + auto bg_flat = teach_masks.sum({0}); + bg_flat = bg_flat.div(bg_flat.max()).to(torch::kByte).mul(255); + teach_mask = cv::Mat{bg_flat.size(0), bg_flat.size(1), CV_8UC1, bg_flat.data_ptr()}; + + auto changes = repeat_masks.index({torch::from_blob(idxs_to_keep.data(), {idxs_to_keep.size()}, torch::kInt), Slice(), Slice()}); CLOG(DEBUG, "lidar.perspective") << "Repeat Tensor size " << changes.sizes(); changes = changes.sum({0}).div(changes.max()).to(torch::kByte).mul(255); - - teach_masks = teach_masks.squeeze().to(torch::kByte).mul(255).contiguous(); - - CLOG(DEBUG, "lidar.perspective") << "Teach Tensor size " << teach_masks.sizes(); - CLOG(DEBUG, "lidar.perspective") << "Tensor type " << teach_masks.dtype() << " with size " << teach_masks.element_size(); - - teach_masks = teach_masks.permute({1, 2, 0}).contiguous(); - - cv::Mat map_mask{teach_masks.size(0), teach_masks.size(1), CV_8UC3, teach_masks.data_ptr()}; repeat_mask = cv::Mat{changes.size(0), changes.size(1), CV_8UC1, changes.data_ptr()}; @@ -296,6 +305,14 @@ void SegmentAnythingModule::run_(QueryCache &qdata0, OutputCache &, filtered_pub_->publish(pc2_msg); } + } else { + if (config_->visualize) { + PointCloudMsg pc2_msg; + pcl::toROSMsg(raw_point_cloud, pc2_msg); + pc2_msg.header.frame_id = "lidar"; //"lidar" for honeycomb + pc2_msg.header.stamp = rclcpp::Time(*qdata.stamp); + filtered_pub_->publish(pc2_msg); + } } // /// publish the transformed pointcloud @@ -314,7 +331,8 @@ void SegmentAnythingModule::run_(QueryCache &qdata0, OutputCache &, diff_img_msg.header.frame_id = "lidar"; //cv_rgb_img.header.stamp = qdata.stamp->header.stamp; diff_img_msg.encoding = "mono8"; - diff_img_msg.image = cv::Mat{diff.size(0), diff.size(1), CV_8UC1, diff.data_ptr()};; + // diff_img_msg.image = cv::Mat{diff.size(0), diff.size(1), CV_8UC1, diff.data_ptr()};; + diff_img_msg.image = teach_mask; diff_pub_->publish(*diff_img_msg.toImageMsg()); } diff --git a/rviz/lidar.rviz b/rviz/lidar.rviz index 9fcf5cebf..4c1565970 100644 --- a/rviz/lidar.rviz +++ b/rviz/lidar.rviz @@ -11,7 +11,7 @@ Panels: - /Image3 - /PointCloud21 Splitter Ratio: 0.5 - Tree Height: 140 + Tree Height: 162 - Class: rviz_common/Selection Name: Selection - Class: rviz_common/Tool Properties @@ -202,20 +202,20 @@ Visualization Manager: Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: -1.419796347618103 + Pitch: -1.2647966146469116 Target Frame: Value: Orbit (rviz) - Yaw: 4.668571472167969 + Yaw: 4.588586330413818 Saved: ~ Window Geometry: Displays: collapsed: false - Height: 1403 + Height: 1536 Hide Left Dock: false Hide Right Dock: false Image: collapsed: false - QMainWindow State: 000000ff00000000fd000000040000000000000344000004e1fc020000000cfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b000000c7000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d00610067006501000001080000010f0000002800fffffffb0000000a0049006d006100670065010000021d0000010b0000002800fffffffb0000000a0049006d006100670065010000032e000001180000002800fffffffb0000000a0049006d006100670065010000044c000000d00000002800ffffff000000010000010f000004e1fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003b000004e1000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e1000001970000000300000a000000003efc0100000002fb0000000800540069006d0065010000000000000a000000025300fffffffb0000000800540069006d00650100000000000004500000000000000000000005a1000004e100000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd00000004000000000000034400000566fc020000000cfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b000000dd000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d006100670065010000011e000001390000002800fffffffb0000000a0049006d006100670065010000025d0000011b0000002800fffffffb0000000a0049006d006100670065010000037e000001040000002800fffffffb0000000a0049006d0061006700650100000488000001190000002800ffffff000000010000010f00000566fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003b00000566000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000009b20000003efc0100000002fb0000000800540069006d00650100000000000009b20000025300fffffffb0000000800540069006d00650100000000000004500000000000000000000005530000056600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Time: @@ -224,6 +224,6 @@ Window Geometry: collapsed: false Views: collapsed: false - Width: 2560 - X: 0 - Y: 0 + Width: 2482 + X: 78 + Y: 27 From cbdea67a1fc3b0ad775a8484d01250a6107aecca Mon Sep 17 00:00:00 2001 From: a-krawciw Date: Mon, 11 Mar 2024 14:37:38 -0400 Subject: [PATCH 60/77] Update vtr_torch --- .../src/vtr_torch/include/vtr_torch/modules/torch_module.hpp | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/main/src/vtr_torch/include/vtr_torch/modules/torch_module.hpp b/main/src/vtr_torch/include/vtr_torch/modules/torch_module.hpp index 79db49e9f..06c88c8da 100644 --- a/main/src/vtr_torch/include/vtr_torch/modules/torch_module.hpp +++ b/main/src/vtr_torch/include/vtr_torch/modules/torch_module.hpp @@ -59,12 +59,12 @@ class TorchModule : public tactic::BaseModule { : tactic::BaseModule{module_factory, name}, config_(config) { //Load the model - //Should the system crash out if the model is loaded incorrectly? try { // Deserialize the ScriptModule from a file using torch::jit::load(). network = torch::jit::load(config_->model_filepath); } catch (const c10::Error& e) { CLOG(ERROR, "torch") << "error loading the model\n" << "Tried to load " << config_->model_filepath; + throw std::runtime_error("Error loading requested network model. Please check your filepaths in the config file."); } if (config_->use_gpu){ @@ -87,12 +87,11 @@ class TorchModule : public tactic::BaseModule { const tactic::TaskExecutor::Ptr &executor) = 0; Config::ConstPtr config_; + torch::Device device = torch::kCPU; protected: Module network; - torch::Device device = torch::kCPU; - template torch::Tensor evaluateModel(std::vector inputs, const Shape shape); From 4c6a39b20a2c5f6f5ad94f24a4ea12b8ea128e98 Mon Sep 17 00:00:00 2001 From: a-krawciw Date: Mon, 11 Mar 2024 14:52:31 -0400 Subject: [PATCH 61/77] Fix torch change. --- main/src/vtr_torch/include/vtr_torch/modules/torch_module.hpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/main/src/vtr_torch/include/vtr_torch/modules/torch_module.hpp b/main/src/vtr_torch/include/vtr_torch/modules/torch_module.hpp index 06c88c8da..337118b45 100644 --- a/main/src/vtr_torch/include/vtr_torch/modules/torch_module.hpp +++ b/main/src/vtr_torch/include/vtr_torch/modules/torch_module.hpp @@ -87,11 +87,10 @@ class TorchModule : public tactic::BaseModule { const tactic::TaskExecutor::Ptr &executor) = 0; Config::ConstPtr config_; - torch::Device device = torch::kCPU; - protected: Module network; + torch::Device device = torch::kCPU; template torch::Tensor evaluateModel(std::vector inputs, const Shape shape); From 6c35a836501f4845e01225c282daae34cab87b95 Mon Sep 17 00:00:00 2001 From: a-krawciw Date: Mon, 11 Mar 2024 15:20:00 -0400 Subject: [PATCH 62/77] Cleanup changes. Improve merge --- Dockerfile | 68 +- config/ouster_warthog_default.yaml | 2 +- config/ouster_warthog_learned.yaml | 46 +- launch/offline_honeycomb_grizzly.launch.yaml | 2 +- launch/offline_velodyne_grizzly.launch.yaml | 9 +- launch/online_ouster_warthog.launch.yaml | 5 +- .../include/vtr_lidar/modules/modules.hpp | 1 - .../modules/odometry/sample_module.hpp | 63 -- .../rangenet_change_detection_module.hpp | 2 +- .../src/modules/odometry/sample_module.cpp | 50 -- main/src/vtr_navigation/src/navigator.cpp | 82 ++- rviz/ouster.rviz | 585 +----------------- 12 files changed, 168 insertions(+), 747 deletions(-) delete mode 100644 main/src/vtr_lidar/include/vtr_lidar/modules/odometry/sample_module.hpp delete mode 100644 main/src/vtr_lidar/src/modules/odometry/sample_module.cpp diff --git a/Dockerfile b/Dockerfile index 576fc4969..99395247e 100644 --- a/Dockerfile +++ b/Dockerfile @@ -3,15 +3,17 @@ FROM nvidia/cuda:11.7.1-cudnn8-devel-ubuntu22.04 CMD ["/bin/bash"] # Args for setting up non-root users, example command to use your own user: -# docker build -t vtr3torch \ +# docker build -t \ # --build-arg USERID=$(id -u) \ # --build-arg GROUPID=$(id -g) \ # --build-arg USERNAME=$(whoami) \ -# --build-arg HOMEDIR=${HOME} . +# --build-arg HOMEDIR=${HOME} \ +# --build-arg CUDA_ARCH=${nvidia-smi --query-gpu=compute_cap --format=csv,noheader} . ARG GROUPID=0 ARG USERID=0 ARG USERNAME=root ARG HOMEDIR=/root +ARG CUDA_ARCH="8.6" RUN if [ ${GROUPID} -ne 0 ]; then addgroup --gid ${GROUPID} ${USERNAME}; fi \ && if [ ${USERID} -ne 0 ]; then adduser --disabled-password --gecos '' --uid ${USERID} --gid ${GROUPID} ${USERNAME}; fi @@ -59,6 +61,7 @@ RUN mkdir -p ${HOMEDIR}/proj && cd ${HOMEDIR}/proj \ ENV LD_LIBRARY_PATH=/usr/local/lib${LD_LIBRARY_PATH:+:${LD_LIBRARY_PATH}} + ## Install ROS2 # UTF-8 RUN apt install -q -y locales \ @@ -106,17 +109,59 @@ RUN pip3 install \ python-socketio[client] \ websocket-client + +#added by sherry + +RUN apt install wget +RUN apt install nano + + +## install opencv 4.5.1 + +RUN apt install -q -y libgtk2.0-dev pkg-config libavcodec-dev libavformat-dev libswscale-dev python3-dev python3-numpy + + +RUN mkdir -p ${HOMEDIR}/opencv && cd ${HOMEDIR}/opencv \ +&& git clone https://github.com/opencv/opencv.git . + +RUN cd ${HOMEDIR}/opencv && git checkout 4.6.0 +RUN mkdir -p ${HOMEDIR}/opencv_contrib && cd ${HOMEDIR}/opencv_contrib \ +&& git clone https://github.com/opencv/opencv_contrib.git . +RUN cd ${HOMEDIR}/opencv_contrib && git checkout 4.6.0 + + +RUN apt install -q -y build-essential cmake git libgtk2.0-dev pkg-config libavcodec-dev libavformat-dev libswscale-dev python3-dev python3-numpy +# # generate Makefiles (note that install prefix is customized to: /usr/local/opencv_cuda) + +RUN mkdir -p ${HOMEDIR}/opencv/build && cd ${HOMEDIR}/opencv/build \ +&& cmake -D CMAKE_BUILD_TYPE=RELEASE \ +-D CMAKE_INSTALL_PREFIX=/usr/local/opencv_cuda \ +-D OPENCV_EXTRA_MODULES_PATH=${HOMEDIR}/opencv_contrib/modules \ +-D PYTHON_DEFAULT_EXECUTABLE=/usr/bin/python3.10 \ +-DBUILD_opencv_python2=OFF \ +-DBUILD_opencv_python3=ON \ +-DWITH_OPENMP=ON \ +-DWITH_CUDA=ON \ +-D CUDA_TOOLKIT_ROOT_DIR=/usr/local/cuda-11.7 \ +-DOPENCV_ENABLE_NONFREE=ON \ +-D OPENCV_GENERATE_PKGCONFIG=ON \ +-DWITH_TBB=ON \ +-DWITH_GTK=ON \ +-DWITH_OPENMP=ON \ +-DWITH_FFMPEG=ON \ +-DBUILD_opencv_cudacodec=OFF \ +-D BUILD_EXAMPLES=OFF \ +-D CUDA_ARCH_BIN=$CUDA_ARCH .. && make -j16 && make install + + +ENV LD_LIBRARY_PATH=/usr/local/opencv_cuda/lib${LD_LIBRARY_PATH:+:${LD_LIBRARY_PATH}} + RUN mkdir -p ${HOMEDIR}/.matplotcpp && cd ${HOMEDIR}/.matplotcpp \ && git clone https://github.com/lava/matplotlib-cpp.git . \ && mkdir -p ${HOMEDIR}/.matplotcpp/build && cd ${HOMEDIR}/.matplotcpp/build \ && cmake .. && cmake --build . -j${NUMPROC} --target install - -RUN apt install htop -RUN apt install ros-humble-velodyne -q -y - -# Install vim -RUN apt update && apt install -q -y vim - + + ##Install LibTorch RUN curl https://download.pytorch.org/libtorch/cu117/libtorch-cxx11-abi-shared-with-deps-2.0.0%2Bcu117.zip --output libtorch.zip RUN unzip libtorch.zip -d /opt/torch @@ -127,5 +172,10 @@ ENV CMAKE_PREFIX_PATH=$TORCH_LIB:$CMAKE_PREFIX_PATH ENV NVIDIA_DRIVER_CAPABILITIES compute,utility,graphics +RUN apt install htop + +# Install vim +RUN apt update && apt install -q -y vim + ## Switch to specified user USER ${USERID}:${GROUPID} diff --git a/config/ouster_warthog_default.yaml b/config/ouster_warthog_default.yaml index e101417f0..4a2f689ec 100644 --- a/config/ouster_warthog_default.yaml +++ b/config/ouster_warthog_default.yaml @@ -352,7 +352,7 @@ max_lin_vel: 1.25 max_ang_vel: 1.0 robot_linear_velocity_scale: 1.0 # Used to scale the output twist linear velocity messages by some constant factor to compensate internal low level control miscalibration - robot_angular_velocity_scale: 0.75 # Used to scale the output twist angular velocity messages by some constant factor to compensate internal low level control miscalibration + robot_angular_velocity_scale: 1.0 # Used to scale the output twist angular velocity messages by some constant factor to compensate internal low level control miscalibration vehicle_model: "unicycle" diff --git a/config/ouster_warthog_learned.yaml b/config/ouster_warthog_learned.yaml index b561826a9..569cb8288 100644 --- a/config/ouster_warthog_learned.yaml +++ b/config/ouster_warthog_learned.yaml @@ -325,7 +325,7 @@ initial_samples: 400 batch_samples: 200 pre_seed_resolution: 0.5 - alpha: 0.5 + alpha: 0.25 q_max: 2.5 frame_interval: 50 iter_max: 10000000 @@ -339,11 +339,20 @@ costmap_filter_value: 0.01 costmap_history: 15 # Note I dont think im actually using this one anymore, am using the one in the change detection v3 now + speed_scheduler: + planar_curv_weight: 2.50 + profile_curv_weight: 0.5 + eop_weight: 1.0 + min_vel: 0.5 + mpc: # Controller Params - horizon_steps: 30 - horizon_step_size: 0.3 - forward_vel: 1.0 + extrapolate_robot_pose: true + mpc_verbosity: false + homotopy_guided_mpc: false + horizon_steps: 20 + horizon_step_size: 0.5 + forward_vel: 0.75 max_lin_vel: 1.25 max_ang_vel: 1.0 robot_linear_velocity_scale: 1.0 # Used to scale the output twist linear velocity messages by some constant factor to compensate internal low level control miscalibration @@ -352,31 +361,18 @@ vehicle_model: "unicycle" # Cost Function Covariance Matrices - pose_error_cov: [100.0, 100.0, 1000.0, 1000.0, 1000.0, 200.0] - vel_error_cov: [25.0, 100.0] - acc_error_cov: [10.0, 100.0] - kin_error_cov: [0.001, 0.001, 0.001, 0.001, 0.001, 0.001] - lat_error_cov: [20.0] - - # Cost Function Covariance Matrices (Tracking Backup) - #pose_error_cov: [100.0, 100.0, 1000.0, 1000.0, 1000.0, 200.0] - #vel_error_cov: [30.0, 30.0] # was [0.1 2.0] # No longer using this cost term - #acc_error_cov: [10.0, 10.0] - #kin_error_cov: [0.001, 0.001, 0.001, 0.001, 0.001, 0.001] - #lat_error_cov: [10.0] - + pose_error_cov: [10.0, 10.0, 100.0, 100.0, 100.0, 20.0] + vel_error_cov: [20.0, 30.0] + acc_error_cov: [20.0, 20.0] + kin_error_cov: [0.01, 0.01, 0.01, 0.01, 0.01, 0.01] + lat_error_cov: [2.0] + # Cost Function Weights pose_error_weight: 1.0 - vel_error_weight: 1.0 - acc_error_weight: 1.0 + vel_error_weight: 1.5 + acc_error_weight: 1.5 kin_error_weight: 1.0 lat_error_weight: 0.01 - #backup for tracking mpc - #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/launch/offline_honeycomb_grizzly.launch.yaml b/launch/offline_honeycomb_grizzly.launch.yaml index 754f94a2d..86be9bfc4 100644 --- a/launch/offline_honeycomb_grizzly.launch.yaml +++ b/launch/offline_honeycomb_grizzly.launch.yaml @@ -23,7 +23,7 @@ windows: model_dir:=${VTRMODELS} start_new_graph:=true use_sim_time:=true - path_planning.type:="cbit.lidar" + path_planning.type:=stationary # - htop # monitor hardware usage diff --git a/launch/offline_velodyne_grizzly.launch.yaml b/launch/offline_velodyne_grizzly.launch.yaml index 4b9c41413..b3d8f2225 100644 --- a/launch/offline_velodyne_grizzly.launch.yaml +++ b/launch/offline_velodyne_grizzly.launch.yaml @@ -18,10 +18,9 @@ windows: panes: - > ros2 launch vtr_navigation vtr.launch.py - base_params:=hdl64_grizzly_learned.yaml + base_params:=hdl64_grizzly_default.yaml data_dir:=${VTRTEMP}/lidar/ - model_dir:=${VTRMODELS} - start_new_graph:=false + start_new_graph:=true use_sim_time:=true path_planning.type:=stationary @@ -40,7 +39,5 @@ windows: shell_command_before: - source ${VTRSRC}/main/install/setup.bash panes: - - ros2 run rviz2 rviz2 -d ${VTRSRC}/rviz/hdl_64.rviz - - ros2 launch foxglove_bridge foxglove_bridge_launch.xml - + - ros2 run rviz2 rviz2 -d ${VTRSRC}/rviz/hdl64.rviz # - ros2 run rqt_reconfigure rqt_reconfigure diff --git a/launch/online_ouster_warthog.launch.yaml b/launch/online_ouster_warthog.launch.yaml index 5bfb4ec4a..06e69ab7d 100644 --- a/launch/online_ouster_warthog.launch.yaml +++ b/launch/online_ouster_warthog.launch.yaml @@ -18,7 +18,7 @@ windows: panes: - > ros2 launch vtr_navigation vtr.launch.py - base_params:=ouster_warthog_learned.yaml + base_params:=ouster_warthog_default.yaml start_new_graph:=false use_sim_time:=false model_dir:=${VTRROOT}/models @@ -43,5 +43,4 @@ windows: shell_command_before: - source ${VTRSRC}/main/install/setup.bash panes: - - ros2 run rviz2 rviz2 -d ${VTRSRC}/rviz/ouster.rviz - # - ros2 run rqt_reconfigure rqt_reconfigure + - ros2 run rviz2 rviz2 -d ${VTRSRC}/rviz/ouster.rviz \ No newline at end of file diff --git a/main/src/vtr_lidar/include/vtr_lidar/modules/modules.hpp b/main/src/vtr_lidar/include/vtr_lidar/modules/modules.hpp index de20afe7e..ceff07220 100644 --- a/main/src/vtr_lidar/include/vtr_lidar/modules/modules.hpp +++ b/main/src/vtr_lidar/include/vtr_lidar/modules/modules.hpp @@ -31,7 +31,6 @@ #include "vtr_lidar/modules/odometry/odometry_map_maintenance_module.hpp" #include "vtr_lidar/modules/odometry/odometry_map_maintenance_module_v2.hpp" #include "vtr_lidar/modules/odometry/vertex_test_module.hpp" -#include "vtr_lidar/modules/odometry/sample_module.hpp" #include "vtr_lidar/modules/localization/localization_icp_module.hpp" #include "vtr_lidar/modules/localization/localization_map_recall_module.hpp" diff --git a/main/src/vtr_lidar/include/vtr_lidar/modules/odometry/sample_module.hpp b/main/src/vtr_lidar/include/vtr_lidar/modules/odometry/sample_module.hpp deleted file mode 100644 index 264a41d3a..000000000 --- a/main/src/vtr_lidar/include/vtr_lidar/modules/odometry/sample_module.hpp +++ /dev/null @@ -1,63 +0,0 @@ -// Copyright 2023, 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 sample_module.hpp - * \author Alec Krawciw, Autonomous Space Robotics Lab (ASRL) - */ -#pragma once - -#include "vtr_tactic/modules/base_module.hpp" -#include "vtr_tactic/task_queue.hpp" -#include -#include "vtr_lidar/cache.hpp" -#include - -namespace vtr { -namespace lidar { - -/** \brief Load and store Torch Models */ -class TestNNModule : public nn::TorchModule { - public: - PTR_TYPEDEFS(TestNNModule); - /** \brief Static module identifier. */ - static constexpr auto static_name = "torch.sample"; - - /** \brief Config parameters. */ - struct Config : public nn::TorchModule::Config { - PTR_TYPEDEFS(Config); - - static ConstPtr fromROS(const rclcpp::Node::SharedPtr &node, - const std::string ¶m_prefix); - }; - - TestNNModule( - const Config::ConstPtr &config, - const std::shared_ptr &module_factory = nullptr, - const std::string &name = static_name) - : nn::TorchModule{config, module_factory, name}, config_(config) {} - - private: - void run_(tactic::QueryCache &qdata, tactic::OutputCache &output, - const tactic::Graph::Ptr &graph, - const tactic::TaskExecutor::Ptr &executor) override; - - Config::ConstPtr config_; - - VTR_REGISTER_MODULE_DEC_TYPE(TestNNModule); - -}; - -} // namespace nn -} // namespace vtr \ No newline at end of file diff --git a/main/src/vtr_lidar/include/vtr_lidar/modules/planning/rangenet_change_detection_module.hpp b/main/src/vtr_lidar/include/vtr_lidar/modules/planning/rangenet_change_detection_module.hpp index d95a42d78..2d980383f 100644 --- a/main/src/vtr_lidar/include/vtr_lidar/modules/planning/rangenet_change_detection_module.hpp +++ b/main/src/vtr_lidar/include/vtr_lidar/modules/planning/rangenet_change_detection_module.hpp @@ -13,7 +13,7 @@ // limitations under the License. /** - * \file sample_module.hpp + * \file rangenet_change_detection_module.hpp * \author Alec Krawciw, Autonomous Space Robotics Lab (ASRL) */ #pragma once diff --git a/main/src/vtr_lidar/src/modules/odometry/sample_module.cpp b/main/src/vtr_lidar/src/modules/odometry/sample_module.cpp deleted file mode 100644 index 2c282bdc9..000000000 --- a/main/src/vtr_lidar/src/modules/odometry/sample_module.cpp +++ /dev/null @@ -1,50 +0,0 @@ -// Copyright 2023, 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 torch_module.cpp - * \author Alec Krawciw, Autonomous Space Robotics Lab (ASRL) - */ -#include - - -namespace vtr { -namespace lidar { - -using namespace tactic; - -auto TestNNModule::Config::fromROS(const rclcpp::Node::SharedPtr &node, - const std::string ¶m_prefix) - -> ConstPtr { - auto config = std::make_shared(); - auto base_config = std::static_pointer_cast(config); - *base_config = *nn::TorchModule::Config::fromROS(node, param_prefix); - // clang-format off - // clang-format on - return config; -} - - -void TestNNModule::run_(QueryCache &qdata0, OutputCache &, - const Graph::Ptr &, const TaskExecutor::Ptr &) { - auto &qdata = dynamic_cast(qdata0); - - std::vector inputs {2, 5}; - auto tensor = evaluateModel(inputs, {1, 2}); - CLOG(INFO, "torch.sample") << tensor; - -} - -} // namespace nn -} // namespace vtr \ No newline at end of file diff --git a/main/src/vtr_navigation/src/navigator.cpp b/main/src/vtr_navigation/src/navigator.cpp index ef367a25e..4b94469e8 100644 --- a/main/src/vtr_navigation/src/navigator.cpp +++ b/main/src/vtr_navigation/src/navigator.cpp @@ -26,10 +26,15 @@ #include "vtr_path_planning/path_planning.hpp" #include "vtr_route_planning/route_planning.hpp" + #ifdef VTR_ENABLE_LIDAR #include "vtr_lidar/pipeline.hpp" #endif +#ifdef VTR_ENABLE_VISION +#include "vtr_vision/pipeline.hpp" +#endif + namespace vtr { namespace navigation { @@ -64,8 +69,6 @@ EdgeTransform loadTransform(const std::string& source_frame, << " target: " << target_frame << ". Default to identity."; EdgeTransform T_source_target(Eigen::Matrix4d(Eigen::Matrix4d::Identity())); T_source_target.setCovariance(Eigen::Matrix::Zero()); - throw std::runtime_error("Transform not found"); - return T_source_target; } @@ -131,6 +134,7 @@ Navigator::Navigator(const rclcpp::Node::SharedPtr& node) : node_(node) { max_queue_size_ = node->declare_parameter("queue_size", max_queue_size_); #ifdef VTR_ENABLE_LIDAR +if (pipeline->name() == "lidar"){ lidar_frame_ = node_->declare_parameter("lidar_frame", "lidar"); T_lidar_robot_ = loadTransform(lidar_frame_, robot_frame_); // static transform @@ -141,11 +145,38 @@ Navigator::Navigator(const rclcpp::Node::SharedPtr& node) : node_(node) { tf_sbc_->sendTransform(msg); // lidar pointcloud data subscription const auto lidar_topic = node_->declare_parameter("lidar_topic", "/points"); - // \note lidar point cloud data frequency is low, and we cannot afford dropping data + + auto lidar_qos = rclcpp::QoS(max_queue_size_); lidar_qos.reliable(); lidar_sub_ = node_->create_subscription(lidar_topic, lidar_qos, std::bind(&Navigator::lidarCallback, this, std::placeholders::_1), sub_opt); - //lidar_sub_ = node_->create_subscription(lidar_topic, rclcpp::SystemDefaultsQoS(), std::bind(&Navigator::lidarCallback, this, std::placeholders::_1), sub_opt); +} +#endif +#ifdef VTR_ENABLE_VISION +if (pipeline->name() == "stereo") { + using namespace std::placeholders; + + camera_frame_ = node_->declare_parameter("camera_frame", "camera"); + T_camera_robot_ = loadTransform(camera_frame_, robot_frame_); + // static transform + tf_sbc_ = std::make_shared(node_); + auto msg = tf2::eigenToTransform(Eigen::Affine3d(T_camera_robot_.inverse().matrix())); + msg.header.frame_id = "robot"; + msg.child_frame_id = "camera"; + tf_sbc_->sendTransform(msg); + // camera images subscription + const auto right_image_topic = node_->declare_parameter("camera_right_topic", "/image_right"); + const auto left_image_topic = node_->declare_parameter("camera_left_topic", "/image_left"); + + auto camera_qos = rclcpp::QoS(10); + camera_qos.reliable(); + + right_camera_sub_.subscribe(node_, right_image_topic, camera_qos.get_rmw_qos_profile()); + left_camera_sub_.subscribe(node_, left_image_topic, camera_qos.get_rmw_qos_profile()); + + sync_ = std::make_shared>(ApproximateImageSync(10), right_camera_sub_, left_camera_sub_); + sync_->registerCallback(&Navigator::cameraCallback, this); +} #endif // clang-format on @@ -196,9 +227,6 @@ void Navigator::process() { // get the front in queue auto qdata0 = queue_.front(); queue_.pop(); -#ifdef VTR_ENABLE_LIDAR - auto qdata = std::dynamic_pointer_cast(qdata0); -#endif // unlock the queue so that new data can be added lock.unlock(); @@ -260,5 +288,45 @@ void Navigator::lidarCallback( }; #endif +#ifdef VTR_ENABLE_VISION +void Navigator::cameraCallback( + const sensor_msgs::msg::Image::SharedPtr msg_r, const sensor_msgs::msg::Image::SharedPtr msg_l) { + LockGuard lock(mutex_); + CLOG(DEBUG, "navigation") << "Received an image."; + + if (image_in_queue_) { + CLOG(WARNING, "navigation") + << "Skip image message because there is already " + "one in queue."; + return; + } + + // Convert message to query_data format and store into query_data + auto query_data = std::make_shared(); + + // some modules require node for visualization + query_data->node = node_; + + query_data->left_image = msg_l; + query_data->right_image = msg_r; + + // set the timestamp + Timestamp timestamp = msg_r->header.stamp.sec * 1e9 + msg_r->header.stamp.nanosec; + query_data->stamp.emplace(timestamp); + + // add the current environment info + query_data->env_info.emplace(env_info_); + + // fill in the vehicle to sensor transform and frame names + query_data->T_s_r.emplace(T_camera_robot_); + + + // add to the queue and notify the processing thread + queue_.push(query_data); + image_in_queue_ = true; + cv_set_or_stop_.notify_one(); +}; +#endif + } // namespace navigation } // namespace vtr diff --git a/rviz/ouster.rviz b/rviz/ouster.rviz index cccc5b79d..fecd477e5 100644 --- a/rviz/ouster.rviz +++ b/rviz/ouster.rviz @@ -26,7 +26,7 @@ Panels: Experimental: false Name: Time SyncMode: 0 - SyncSource: PointCloud2 + SyncSource: "" Visualization Manager: Class: "" Displays: @@ -181,581 +181,6 @@ Visualization Manager: Use Fixed Frame: true Use rainbow: true Value: false - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 10 - Min Value: -10 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz_default_plugins/PointCloud2 - Color: 255; 0; 0 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 1.0234375 - Min Color: 0; 0; 0 - Min Intensity: 0.00022029876708984375 - Name: curr map odo - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.10000000149011612 - Style: Flat Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /vtr/submap_odo - Use Fixed Frame: true - Use rainbow: true - Value: false - - Alpha: 1 - Buffer Length: 1 - Class: rviz_default_plugins/Path - Color: 255; 0; 0 - Enabled: true - Head Diameter: 0.30000001192092896 - Head Length: 0.20000000298023224 - Length: 1 - Line Style: Billboards - Line Width: 0.10000000149011612 - Name: localization path - Offset: - X: 0 - Y: 0 - Z: 0 - Pose Color: 255; 85; 255 - Pose Style: None - Radius: 0.10000000149011612 - Shaft Diameter: 0.10000000149011612 - Shaft Length: 0.10000000149011612 - Topic: - Depth: 1 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep All - Reliability Policy: Best Effort - Value: /vtr/loc_path - Value: true - - Alpha: 1 - Autocompute Intensity Bounds: false - Autocompute Value Bounds: - Max Value: 10 - Min Value: -10 - Value: true - Axis: Z - Channel Name: icp_score - Class: rviz_default_plugins/PointCloud2 - Color: 255; 0; 0 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 3 - Min Color: 0; 0; 0 - Min Intensity: 1 - Name: curr map loc - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.10000000149011612 - Style: Flat Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /vtr/submap_loc - Use Fixed Frame: true - Use rainbow: true - Value: false - - Alpha: 1 - Buffer Length: 1 - Class: rviz_default_plugins/Path - Color: 0; 0; 255 - Enabled: true - Head Diameter: 0.30000001192092896 - Head Length: 0.20000000298023224 - Length: 1 - Line Style: Billboards - Line Width: 0.07999999821186066 - Name: reference path - Offset: - X: 0 - Y: 0 - Z: 0 - Pose Color: 255; 85; 255 - Pose Style: None - Radius: 0.10000000149011612 - 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/planning_path - Value: true - - Alpha: 1 - Buffer Length: 1 - Class: rviz_default_plugins/Path - Color: 255; 255; 0 - Enabled: true - Head Diameter: 0.30000001192092896 - Head Length: 0.20000000298023224 - Length: 1 - Line Style: Lines - Line Width: 0.029999999329447746 - Name: planned path - Offset: - X: 0 - Y: 0 - Z: 0 - Pose Color: 255; 85; 255 - Pose Style: None - Radius: 0.10000000149011612 - 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/local_plan - Value: true - - Alpha: 1 - Arrow Length: 0.5 - Axes Length: 0.30000001192092896 - Axes Radius: 0.009999999776482582 - Class: rviz_default_plugins/PoseArray - Color: 0; 255; 1 - Enabled: true - Head Length: 0.10000000149011612 - Head Radius: 0.10000000149011612 - Name: planned path (poses) - Shaft Length: 0.30000001192092896 - Shaft Radius: 0.10000000149011612 - Shape: Arrow (3D) - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /vtr/teb_poses - Value: true - - Alpha: 0.20000000298023224 - Autocompute Intensity Bounds: false - Autocompute Value Bounds: - Max Value: 10 - Min Value: -10 - Value: true - Axis: Z - Channel Name: z - Class: rviz_default_plugins/PointCloud2 - Color: 0; 0; 0 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 5 - Min Color: 0; 0; 0 - Min Intensity: 0.9500057697296143 - Name: change detection map - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.03500000014901161 - Style: Flat Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /vtr/change_detection_map - Use Fixed Frame: true - Use rainbow: false - Value: false - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 10 - Min Value: -10 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz_default_plugins/PointCloud2 - Color: 255; 0; 0 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 1.0234375 - Min Color: 0; 0; 0 - Min Intensity: 0.00022029876708984375 - Name: intra exp merging (old) - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.10000000149011612 - Style: Flat Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /vtr/intra_exp_merging_old - Use Fixed Frame: true - Use rainbow: true - Value: false - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 10 - Min Value: -10 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz_default_plugins/PointCloud2 - Color: 0; 255; 0 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 1.0234375 - Min Color: 0; 0; 0 - Min Intensity: 0.00022029876708984375 - Name: intra exp merging (new) - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.10000000149011612 - Style: Flat Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /vtr/intra_exp_merging_new - Use Fixed Frame: true - Use rainbow: true - Value: false - - Alpha: 1 - Autocompute Intensity Bounds: false - Autocompute Value Bounds: - Max Value: 10 - Min Value: -10 - Value: true - Axis: Z - Channel Name: flex21 - Class: rviz_default_plugins/PointCloud2 - Color: 255; 255; 255 - Color Transformer: Intensity - Decay Time: 0 - Enabled: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 2 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: annotation map - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.10000000149011612 - Style: Flat Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /annotation_map - Use Fixed Frame: true - Use rainbow: true - Value: false - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 10 - Min Value: -10 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz_default_plugins/PointCloud2 - Color: 255; 0; 0 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 1.0234375 - Min Color: 0; 0; 0 - Min Intensity: 0.00022029876708984375 - Name: dynaimc detection (old) - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.10000000149011612 - Style: Flat Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /vtr/dynamic_detection_old - Use Fixed Frame: true - Use rainbow: true - Value: false - - Alpha: 1 - Autocompute Intensity Bounds: false - Autocompute Value Bounds: - Max Value: 10 - Min Value: -10 - Value: true - Axis: Z - Channel Name: flex11 - Class: rviz_default_plugins/PointCloud2 - Color: 0; 255; 0 - Color Transformer: Intensity - Decay Time: 0 - Enabled: false - Invert Rainbow: false - Max Color: 0; 255; 0 - Max Intensity: 3 - Min Color: 255; 0; 0 - Min Intensity: 2 - Name: dynamic detection (new) - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.10000000149011612 - Style: Flat Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /vtr/dynamic_detection_new - Use Fixed Frame: true - Use rainbow: false - Value: false - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 10 - Min Value: -10 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz_default_plugins/PointCloud2 - Color: 0; 255; 0 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 1.0234375 - Min Color: 0; 0; 0 - Min Intensity: 0.00022029876708984375 - Name: dynamic detection (scan) - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.10000000149011612 - Style: Flat Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /vtr/dynamic_detection_scan - Use Fixed Frame: true - Use rainbow: true - Value: false - - Alpha: 1 - Autocompute Intensity Bounds: false - Autocompute Value Bounds: - Max Value: 10 - Min Value: -10 - Value: true - Axis: Z - Channel Name: flex21 - Class: rviz_default_plugins/PointCloud2 - Color: 255; 0; 0 - Color Transformer: Intensity - Decay Time: 0 - Enabled: false - Invert Rainbow: false - Max Color: 255; 0; 0 - Max Intensity: 2 - Min Color: 25; 255; 0 - Min Intensity: 1 - Name: inter exp merging priv - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.10000000149011612 - Style: Flat Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /vtr/inter_exp_merging_priv - Use Fixed Frame: true - Use rainbow: false - Value: false - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 10 - Min Value: -10 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz_default_plugins/PointCloud2 - Color: 25; 255; 0 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 1.0234375 - Min Color: 0; 0; 0 - Min Intensity: 0.00022029876708984375 - Name: inter exp merging curr - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.10000000149011612 - Style: Flat Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /vtr/inter_exp_merging_curr - Use Fixed Frame: true - Use rainbow: true - Value: false - - Alpha: 1 - Autocompute Intensity Bounds: false - Autocompute Value Bounds: - Max Value: 10 - Min Value: -10 - Value: true - Axis: Z - Channel Name: flex11 - Class: rviz_default_plugins/PointCloud2 - Color: 255; 0; 0 - Color Transformer: Intensity - Decay Time: 0 - Enabled: false - Invert Rainbow: false - Max Color: 0; 255; 0 - Max Intensity: 2 - Min Color: 255; 0; 0 - Min Intensity: 1 - Name: global map - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.20000000298023224 - Style: Boxes - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /vtr/global_map - Use Fixed Frame: true - Use rainbow: false - Value: false - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 10 - Min Value: -10 - Value: true - Axis: Z - Channel Name: z - Class: rviz_default_plugins/PointCloud2 - Color: 255; 0; 0 - Color Transformer: Intensity - Decay Time: 0 - Enabled: false - Invert Rainbow: false - Max Color: 255; 0; 0 - Max Intensity: 9.06832218170166 - Min Color: 0; 255; 0 - Min Intensity: -2.8154680728912354 - Name: global me map - Position Transformer: XYZ - Selectable: true - Size (Pixels): 1 - Size (m): 0.15000000596046448 - Style: Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /vtr/global_map - Use Fixed Frame: true - Use rainbow: true - Value: false - - Alpha: 1 - Buffer Length: 1 - Class: rviz_default_plugins/Path - Color: 255; 0; 255 - Enabled: false - Head Diameter: 0.30000001192092896 - Head Length: 0.20000000298023224 - Length: 0.30000001192092896 - Line Style: Billboards - Line Width: 0.5 - Name: global path - Offset: - X: 0 - Y: 0 - Z: 0.5 - 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/global_path - Value: false - Alpha: 1 Autocompute Intensity Bounds: false Autocompute Value Bounds: @@ -1190,7 +615,7 @@ Visualization Manager: Views: Current: Class: rviz_default_plugins/Orbit - Distance: 54.51273727416992 + Distance: 41.55506134033203 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 @@ -1205,10 +630,10 @@ Visualization Manager: Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: 0.8647971153259277 + Pitch: 1.5697963237762451 Target Frame: robot Value: Orbit (rviz_default_plugins) - Yaw: 4.0012898445129395 + Yaw: 3.441279411315918 Saved: - Class: rviz_default_plugins/Orbit Distance: 111.39698028564453 @@ -1426,5 +851,5 @@ Window Geometry: Views: collapsed: true Width: 1848 - X: 78 + X: 72 Y: 27 From eb0a453049621377597ab023d5dc11170adfc1b2 Mon Sep 17 00:00:00 2001 From: a-krawciw Date: Mon, 11 Mar 2024 15:23:35 -0400 Subject: [PATCH 63/77] More cleanup --- config/cyclonedds.default.xml | 9 ----- launch/offline_ouster_warthog.launch.yaml | 45 ---------------------- launch/online_velodyne_grizzly.launch.yaml | 5 +-- main/src/vtr_common/vtr_include.cmake | 42 ++++++++++++++++---- 4 files changed, 36 insertions(+), 65 deletions(-) delete mode 100644 config/cyclonedds.default.xml delete mode 100644 launch/offline_ouster_warthog.launch.yaml diff --git a/config/cyclonedds.default.xml b/config/cyclonedds.default.xml deleted file mode 100644 index bb0b7cfab..000000000 --- a/config/cyclonedds.default.xml +++ /dev/null @@ -1,9 +0,0 @@ - - - - - auto - - - diff --git a/launch/offline_ouster_warthog.launch.yaml b/launch/offline_ouster_warthog.launch.yaml deleted file mode 100644 index f923d754a..000000000 --- a/launch/offline_ouster_warthog.launch.yaml +++ /dev/null @@ -1,45 +0,0 @@ -## Online Lidar VTR3 -session_name: vtr_online_ouster_grizzly - -environment: - CYCLONEDDS_URI: ${VTRSRC}/config/cyclonedds.default.xml # custom dds configs - # ROS_DOMAIN_ID: "1" # set this to a unique number when multiple ROS2 dependent system running on the same network - -start_directory: ${VTRTEMP} -suppress_history: false - -windows: - - window_name: vtr_navigation_system - layout: main-horizontal - shell_command_before: - - source ${VTRSRC}/main/install/setup.bash -# - ros2 run vtr_gui setup_server --ros-args -r __ns:=/vtr - panes: - - > - ros2 launch vtr_navigation vtr.launch.py - base_params:=ouster_warthog_learned.yaml - data_dir:=${VTRTEMP}/lidar - start_new_graph:=false - use_sim_time:=false - model_dir:=${VTRROOT}/models - planner:=stationary - - #- htop # monitor hardware usage - - - window_name: vtr_gui - layout: main-horizontal - shell_command_before: - - source ${VTRSRC}/main/install/setup.bash - panes: - - ros2 run vtr_gui socket_client --ros-args -r __ns:=/vtr - - ros2 run vtr_gui socket_server --ros-args -r __ns:=/vtr - - ros2 run vtr_gui web_server --ros-args -r __ns:=/vtr - # - firefox --new-window "localhost:5200" # the webpage has to wait for everything above - - - window_name: rviz2 - layout: main-horizontal - shell_command_before: - - source ${VTRSRC}/main/install/setup.bash - panes: - - ros2 run rviz2 rviz2 -d ${VTRSRC}/rviz/ouster.rviz - # - ros2 run rqt_reconfigure rqt_reconfigure diff --git a/launch/online_velodyne_grizzly.launch.yaml b/launch/online_velodyne_grizzly.launch.yaml index 7eeb29d08..b26a39a3a 100644 --- a/launch/online_velodyne_grizzly.launch.yaml +++ b/launch/online_velodyne_grizzly.launch.yaml @@ -23,9 +23,7 @@ windows: base_params:=hdl64_grizzly_default.yaml # data_dir:=${VTRTEMP}/lidar/$(date '+%F')/$(date '+%F') start_new_graph:=false - model_dir:=${VTRMODELS} use_sim_time:=false - planner:=cbit - window_name: vtr_gui layout: main-horizontal @@ -42,6 +40,5 @@ windows: shell_command_before: - source ${VTRSRC}/main/install/setup.bash panes: - #- ros2 run rviz2 rviz2 -d ${VTRSRC}/rviz/hdl_64.rviz - - ros2 launch foxglove_bridge foxglove_bridge_launch.xml + - ros2 run rviz2 rviz2 -d ${VTRSRC}/rviz/hdl_64.rviz # - ros2 run rqt_reconfigure rqt_reconfigure diff --git a/main/src/vtr_common/vtr_include.cmake b/main/src/vtr_common/vtr_include.cmake index 23de338dc..0d7c2bae4 100644 --- a/main/src/vtr_common/vtr_include.cmake +++ b/main/src/vtr_common/vtr_include.cmake @@ -10,9 +10,7 @@ add_compile_options(-march=native -O3 -pthread -Wall -Wextra) # add_compile_options(-frepo) #Add debug symbols -add_compile_options(-g) -#Prevent all optimization (useful for detailed debugging but really slow!) -#add_compile_options(-O0) +#add_compile_options(-g -Og) # built time and memory report # add_compile_options(-ftime-report -fmem-report) @@ -34,7 +32,37 @@ if (OpenMP_FOUND) endif() -## Enable certain pipelines -add_definitions(-DVTR_ENABLE_LIDAR) -add_definitions(-DVTR_ENABLE_RADAR) -#add_definitions(-DSAVE_ALL_REPEATS) +#Set to VTR_PIPELINE=VISION, LIDAR, RADAR, or RADAR-LIDAR +set(SelectedPipeline "$ENV{VTR_PIPELINE}") + + +if(SelectedPipeline MATCHES "LIDAR") + add_definitions(-DVTR_ENABLE_LIDAR) + set(VTR_ENABLE_LIDAR true) +elseif(SelectedPipeline MATCHES "RADAR") + add_definitions(-DVTR_ENABLE_RADAR) + set(VTR_ENABLE_RADAR true) +elseif(SelectedPipeline MATCHES "RADAR-LIDAR") + add_definitions(-DVTR_ENABLE_RADAR) + set(VTR_ENABLE_RADAR true) + add_definitions(-DVTR_ENABLE_LIDAR) + set(VTR_ENABLE_LIDAR true) +elseif(SelectedPipeline MATCHES "VISION") + ## GPUSURF enable/disable flag (used by vision pipeline only) + # Note: currently assume that gpusurf is always available, because we have no + # other options, so do not disable (i.e. comment out) this flag + add_definitions(-DVTR_ENABLE_GPUSURF) # set the available flag + add_definitions(-DVTR_ENABLE_VISION) + add_definitions(-DVTR_VISION_LEARNED) + set(VTR_ENABLE_VISION true) +else() + add_definitions(-DVTR_ENABLE_RADAR) + set(VTR_ENABLE_RADAR true) + add_definitions(-DVTR_ENABLE_LIDAR) + set(VTR_ENABLE_LIDAR true) + add_definitions(-DVTR_ENABLE_GPUSURF) # set the available flag + add_definitions(-DVTR_ENABLE_VISION) + add_definitions(-DVTR_VISION_LEARNED) + set(VTR_ENABLE_VISION true) + message(WARNING "VTR_PIPELINE not set! Compiling all! Save time by selecting VTR_PIPELINE=VISION, LIDAR, RADAR, or RADAR-LIDAR") +endif() \ No newline at end of file From 3314bff6d118de4b12b6fe993520c5281d53b8ea Mon Sep 17 00:00:00 2001 From: a-krawciw Date: Wed, 13 Mar 2024 17:10:30 -0400 Subject: [PATCH 64/77] Add smoothing to diff. Improve prompting. Add cd point cloud --- config/ouster_warthog_default.yaml | 1 - config/ouster_warthog_lasersam.yaml | 14 +++ .../planning/segmentanything_module.hpp | 3 +- .../planning/segmentanything_module.cpp | 99 ++++++++++--------- main/src/vtr_lidar/src/pipeline.cpp | 2 +- rviz/lidar.rviz | 4 +- 6 files changed, 73 insertions(+), 50 deletions(-) diff --git a/config/ouster_warthog_default.yaml b/config/ouster_warthog_default.yaml index 914151b3a..3d524869f 100644 --- a/config/ouster_warthog_default.yaml +++ b/config/ouster_warthog_default.yaml @@ -102,7 +102,6 @@ - memory submap_translation_threshold: 1.5 submap_rotation_threshold: 30.0 - save_raw_point_cloud: True preprocessing: conversion: type: lidar.ouster_converter diff --git a/config/ouster_warthog_lasersam.yaml b/config/ouster_warthog_lasersam.yaml index f2b5e66ed..72f28cced 100644 --- a/config/ouster_warthog_lasersam.yaml +++ b/config/ouster_warthog_lasersam.yaml @@ -15,13 +15,16 @@ tactic: localization_skippable: true pipeline: + save_raw_point_cloud: True localization: - recall - icp - safe_corridor - segmentation + - costmap_inflation - memory + localization: segmentation: type: lidar.SAM @@ -36,3 +39,14 @@ max_range: 50.0 num_prompts: 5 visualize: true + costmap_inflation: + type: lidar.costmap_queue + influence_distance: 0.5 # was 0.5 Jordy # Note that the total distance where grid cells have values > 0 is min dist + influence dist, not influence dist! + minimum_distance: 0.8 # was 0.3 Jordy + + # cost map + costmap_history_size: 5 # Keep between 3 and 30, used for temporal filtering + resolution: 0.25 + size_x: 16.0 + size_y: 8.0 + visualize: true diff --git a/main/src/vtr_lidar/include/vtr_lidar/modules/planning/segmentanything_module.hpp b/main/src/vtr_lidar/include/vtr_lidar/modules/planning/segmentanything_module.hpp index 45bd88722..60abae12d 100644 --- a/main/src/vtr_lidar/include/vtr_lidar/modules/planning/segmentanything_module.hpp +++ b/main/src/vtr_lidar/include/vtr_lidar/modules/planning/segmentanything_module.hpp @@ -77,7 +77,8 @@ class SegmentAnythingModule : public nn::TorchModule { bool pub_init_ = false; - rclcpp::Publisher::SharedPtr mask_pub_; + rclcpp::Publisher::SharedPtr live_mask_pub_; + rclcpp::Publisher::SharedPtr map_mask_pub_; rclcpp::Publisher::SharedPtr diff_pub_; rclcpp::Publisher::SharedPtr live_img_pub_; rclcpp::Publisher::SharedPtr map_img_pub_; diff --git a/main/src/vtr_lidar/src/modules/planning/segmentanything_module.cpp b/main/src/vtr_lidar/src/modules/planning/segmentanything_module.cpp index 3aee43a30..51808e86e 100644 --- a/main/src/vtr_lidar/src/modules/planning/segmentanything_module.cpp +++ b/main/src/vtr_lidar/src/modules/planning/segmentanything_module.cpp @@ -59,14 +59,9 @@ void SegmentAnythingModule::run_(QueryCache &qdata0, OutputCache &, const Graph::Ptr &graph, const TaskExecutor::Ptr &) { auto &qdata = dynamic_cast(qdata0); - - // if(!qdata.rendered_images.valid()) { - // CLOG(WARNING, "lidar.perspective") << "Rendered perspective images required a map to work!"; - // return; - // } - if(!pub_init_){ - mask_pub_ = qdata.node->create_publisher("detection_mask", 5); + live_mask_pub_ = qdata.node->create_publisher("detection_mask", 5); + map_mask_pub_ = qdata.node->create_publisher("background_mask", 5); diff_pub_ = qdata.node->create_publisher("normed_diff", 5); live_img_pub_ = qdata.node->create_publisher("live_range_coloured", 5); map_img_pub_ = qdata.node->create_publisher("map_range_coloured", 5); @@ -191,8 +186,19 @@ void SegmentAnythingModule::run_(QueryCache &qdata0, OutputCache &, torch::Tensor diff = live_tensor - map_tensor; diff = diff.norm(2, {0}); - const auto [topk_vals, topk_idx] = diff.flatten().topk(config_->num_prompts); + namespace F = torch::nn::functional; + + torch::Tensor average = torch::ones({1, 1, 5, 5}) / 25; + + diff = F::conv2d(diff.unsqueeze(0).unsqueeze(0), average); + + const auto [smaller_diff, idx] = F::max_pool2d_with_indices(diff.squeeze(0), F::MaxPool2dFuncOptions(20)); + const auto [topk_vals, topk_idx] = smaller_diff.flatten().topk(config_->num_prompts); + + CLOG(DEBUG, "lidar.perspective") << "Max pooling " << smaller_diff.sizes() << " and " << idx.sizes(); + + diff = diff.squeeze(); using namespace torch::indexing; // int data[] = { 512, 256, @@ -200,10 +206,15 @@ void SegmentAnythingModule::run_(QueryCache &qdata0, OutputCache &, // 768, 256 }; std::vector prompts; - auto idxs_a = topk_idx.accessor(); + auto topk_idx_a = topk_idx.accessor(); + auto idx_a = idx.accessor(); + + for (size_t i = 0; i < topk_idx_a.size(0); i++) { + const long& small_idx = topk_idx_a[i]; + + const long& idx = idx_a[0][small_idx / diff.size(1)][small_idx % diff.size(1)]; + - for (size_t i = 0; i < idxs_a.size(0); i++) { - const long& idx = idxs_a[i]; int prompt[] = {idx % diff.size(1), idx / diff.size(1)}; auto& pc_idx = live_index_img.at(prompt[1] / 4, prompt[0] / 4); @@ -224,9 +235,6 @@ void SegmentAnythingModule::run_(QueryCache &qdata0, OutputCache &, else CLOG(DEBUG, "lidar.perspective") << "Prompt point on an empty map pixel. Try again."; } - - // prompts.push_back(torch::from_blob(&data[2], {2}, torch::kInt).to(device)); - // prompts.push_back(torch::from_blob(&data[4], {2}, torch::kInt).to(device)); torch::NoGradGuard no_grad; std::vector jit_inputs; @@ -267,8 +275,7 @@ void SegmentAnythingModule::run_(QueryCache &qdata0, OutputCache &, cv::Mat repeat_mask = cv::Mat::zeros(repeat_masks.size(1), repeat_masks.size(2), CV_8UC1); if (idxs_to_keep.size() > 0){ - auto bg_flat = teach_masks.sum({0}); - bg_flat = bg_flat.div(bg_flat.max()).to(torch::kByte).mul(255); + auto bg_flat = teach_masks.sum({0}).to(torch::kByte).mul(255); teach_mask = cv::Mat{bg_flat.size(0), bg_flat.size(1), CV_8UC1, bg_flat.data_ptr()}; @@ -282,59 +289,61 @@ void SegmentAnythingModule::run_(QueryCache &qdata0, OutputCache &, mask_to_pointcloud(repeat_mask, live_index_img, raw_point_cloud); - auto filtered_point_cloud = + auto obstacle_point_cloud = std::make_shared>(raw_point_cloud); /// changed cropping { std::vector indices; - indices.reserve(filtered_point_cloud->size()); - for (size_t i = 0; i < filtered_point_cloud->size(); ++i) { - if ((*filtered_point_cloud)[i].flex24 > 0) + indices.reserve(obstacle_point_cloud->size()); + for (size_t i = 0; i < obstacle_point_cloud->size(); ++i) { + if ((*obstacle_point_cloud)[i].flex24 > 0) indices.emplace_back(i); } - *filtered_point_cloud = - pcl::PointCloud(*filtered_point_cloud, indices); - } + *obstacle_point_cloud = + pcl::PointCloud(*obstacle_point_cloud, indices); - if (config_->visualize) { - PointCloudMsg pc2_msg; - pcl::toROSMsg(*filtered_point_cloud, pc2_msg); - pc2_msg.header.frame_id = "lidar"; //"lidar" for honeycomb - pc2_msg.header.stamp = rclcpp::Time(*qdata.stamp); - filtered_pub_->publish(pc2_msg); - } + auto obs_mat = obstacle_point_cloud->getMatrixXfMap(4, PointWithInfo::size(), PointWithInfo::cartesian_offset()); + obs_mat = T_s_r.inverse().matrix().cast() * T_c_s.inverse() * obs_mat; - } else { - if (config_->visualize) { - PointCloudMsg pc2_msg; - pcl::toROSMsg(raw_point_cloud, pc2_msg); - pc2_msg.header.frame_id = "lidar"; //"lidar" for honeycomb - pc2_msg.header.stamp = rclcpp::Time(*qdata.stamp); - filtered_pub_->publish(pc2_msg); + qdata.changed_points.emplace(*obstacle_point_cloud); } + } // /// publish the transformed pointcloud if (config_->visualize) { - cv_bridge::CvImage mask_img_msg; - mask_img_msg.header.frame_id = "lidar"; + cv_bridge::CvImage live_mask_img_msg; + live_mask_img_msg.header.frame_id = "lidar"; //cv_rgb_img.header.stamp = qdata.stamp->header.stamp; - mask_img_msg.encoding = "mono8"; - mask_img_msg.image = repeat_mask; - mask_pub_->publish(*mask_img_msg.toImageMsg()); + live_mask_img_msg.encoding = "mono8"; + live_mask_img_msg.image = repeat_mask; + live_mask_pub_->publish(*live_mask_img_msg.toImageMsg()); + + cv_bridge::CvImage map_mask_img_msg; + map_mask_img_msg.header.frame_id = "lidar"; + //cv_rgb_img.header.stamp = qdata.stamp->header.stamp; + map_mask_img_msg.encoding = "mono8"; + map_mask_img_msg.image = teach_mask; + map_mask_pub_->publish(*map_mask_img_msg.toImageMsg()); diff = diff.div(diff.max()).mul(255).to(torch::kByte).contiguous(); - CLOG(DEBUG, "lidar.perspective") << "Max: " << diff.max() << " Min: " << diff.min(); cv_bridge::CvImage diff_img_msg; diff_img_msg.header.frame_id = "lidar"; //cv_rgb_img.header.stamp = qdata.stamp->header.stamp; diff_img_msg.encoding = "mono8"; - // diff_img_msg.image = cv::Mat{diff.size(0), diff.size(1), CV_8UC1, diff.data_ptr()};; - diff_img_msg.image = teach_mask; + diff_img_msg.image = cv::Mat{diff.size(0), diff.size(1), CV_8UC1, diff.data_ptr()};; diff_pub_->publish(*diff_img_msg.toImageMsg()); } + + if (config_->visualize) { + PointCloudMsg pc2_msg; + pcl::toROSMsg(raw_point_cloud, pc2_msg); + pc2_msg.header.frame_id = "lidar"; //"lidar" for honeycomb + pc2_msg.header.stamp = rclcpp::Time(*qdata.stamp); + filtered_pub_->publish(pc2_msg); + } } diff --git a/main/src/vtr_lidar/src/pipeline.cpp b/main/src/vtr_lidar/src/pipeline.cpp index 40275bf96..ca7b0bc54 100644 --- a/main/src/vtr_lidar/src/pipeline.cpp +++ b/main/src/vtr_lidar/src/pipeline.cpp @@ -180,7 +180,7 @@ void LidarPipeline::onVertexCreation_(const QueryCache::Ptr &qdata0, } // raw point cloud - if (config_->save_raw_point_cloud) + if (config_->save_raw_point_cloud && (*qdata->pipeline_mode == PipelineMode::TeachBranch || *qdata->pipeline_mode == PipelineMode::TeachMetricLoc)) { auto raw_scan_odo = std::make_shared>(); raw_scan_odo->point_cloud() = *qdata->raw_point_cloud; diff --git a/rviz/lidar.rviz b/rviz/lidar.rviz index 4c1565970..f6981cb10 100644 --- a/rviz/lidar.rviz +++ b/rviz/lidar.rviz @@ -9,7 +9,7 @@ Panels: - /Image1 - /Image2 - /Image3 - - /PointCloud21 + - /Image4 Splitter Ratio: 0.5 Tree Height: 162 - Class: rviz_common/Selection @@ -105,7 +105,7 @@ Visualization Manager: Durability Policy: Volatile History Policy: Keep Last Reliability Policy: Reliable - Value: /vtr/normed_diff + Value: /vtr/background_mask Value: true - Alpha: 1 Autocompute Intensity Bounds: false From af34a0c035926830bdf22832bccb4f7788f96aa0 Mon Sep 17 00:00:00 2001 From: a-krawciw Date: Tue, 19 Mar 2024 13:25:02 -0400 Subject: [PATCH 65/77] Update costmap inflation to current style. Improe prompting. --- config/ouster_warthog_default.yaml | 4 +- config/ouster_warthog_lasersam.yaml | 11 +- ...online_ouster_warthog_lasersam.launch.yaml | 2 +- .../vtr_lidar/filters/perspective_image.hpp | 2 +- .../planning/segmentanything_module.hpp | 2 +- .../src/filters/perspective_image.cpp | 13 +- .../planning/costmap_inflation_module.cpp | 13 +- .../planning/segmentanything_module.cpp | 119 ++++++++++++------ rviz/lidar.rviz | 29 +++-- 9 files changed, 140 insertions(+), 55 deletions(-) diff --git a/config/ouster_warthog_default.yaml b/config/ouster_warthog_default.yaml index 3d524869f..5fba01c59 100644 --- a/config/ouster_warthog_default.yaml +++ b/config/ouster_warthog_default.yaml @@ -117,7 +117,7 @@ num_threads: 8 crop_range: 40.0 - frame_voxel_size: 0.1 # grid subsampling voxel size + frame_voxel_size: 0.3 # grid subsampling voxel size vertical_angle_res: 0.0061365 # vertical angle resolution in radians, equal to 0.3516 degree documented in the manual polar_r_scale: 2.0 # polar_r_scale x vertical_angle_res = nearest neighbor search radius for normal computation @@ -308,7 +308,7 @@ visualize: true path_planning: - type: cbit # cbit for obstacle free path tracker, cbit.lidar for obstacle avoidance version + type: cbit control_period: 100 # ms cbit: obstacle_avoidance: false diff --git a/config/ouster_warthog_lasersam.yaml b/config/ouster_warthog_lasersam.yaml index 72f28cced..db01ed32b 100644 --- a/config/ouster_warthog_lasersam.yaml +++ b/config/ouster_warthog_lasersam.yaml @@ -5,6 +5,7 @@ - navigation.sensor_input - tactic.pipeline - tactic.module + - cbit.obstacle_filtering # pose graph #- pose_graph @@ -30,7 +31,7 @@ type: lidar.SAM use_gpu: true abs_filepath: false - filepath: sam_vit_b_cuda.torchscript + filepath: sam_vit_b_fast_cuda.torchscript img_width: 256 img_height: 128 h_fov: 1.5707 @@ -38,6 +39,8 @@ min_range: 0.5 max_range: 50.0 num_prompts: 5 + smooth_size: 40 + iou_threshold: 1.0 visualize: true costmap_inflation: type: lidar.costmap_queue @@ -50,3 +53,9 @@ size_x: 16.0 size_y: 8.0 visualize: true + path_planning: + cbit: + obstacle_avoidance: true + mpc: + homotopy_guided_mpc: true + diff --git a/launch/online_ouster_warthog_lasersam.launch.yaml b/launch/online_ouster_warthog_lasersam.launch.yaml index a842db0a4..74c5d7006 100644 --- a/launch/online_ouster_warthog_lasersam.launch.yaml +++ b/launch/online_ouster_warthog_lasersam.launch.yaml @@ -22,7 +22,7 @@ windows: override_params:=ouster_warthog_lasersam.yaml start_new_graph:=false use_sim_time:=false - planner:="stationary" + planner:="cbit" model_dir:=${VTRROOT}/models diff --git a/main/src/vtr_lidar/include/vtr_lidar/filters/perspective_image.hpp b/main/src/vtr_lidar/include/vtr_lidar/filters/perspective_image.hpp index 0814efbf3..9f6492aec 100644 --- a/main/src/vtr_lidar/include/vtr_lidar/filters/perspective_image.hpp +++ b/main/src/vtr_lidar/include/vtr_lidar/filters/perspective_image.hpp @@ -57,7 +57,7 @@ void unproject_data_image(pcl::PointCloud& point_cloud, const cv: void interpolate_hsv_image(cv::Mat& depth_image); -void mask_to_pointcloud(const cv::Mat& mask, const cv::Mat& index_img, pcl::PointCloud& point_cloud); +void mask_to_pointcloud(const cv::Mat& mask, const cv::Mat& index_img, pcl::PointCloud& point_cloud, int selected_val=2); } // namespace lidar } // vtr \ No newline at end of file diff --git a/main/src/vtr_lidar/include/vtr_lidar/modules/planning/segmentanything_module.hpp b/main/src/vtr_lidar/include/vtr_lidar/modules/planning/segmentanything_module.hpp index 60abae12d..e9f18b67a 100644 --- a/main/src/vtr_lidar/include/vtr_lidar/modules/planning/segmentanything_module.hpp +++ b/main/src/vtr_lidar/include/vtr_lidar/modules/planning/segmentanything_module.hpp @@ -55,6 +55,7 @@ class SegmentAnythingModule : public nn::TorchModule { float iou_thresh = 0.5; int num_prompts = 4; + int smooth_size = 10; static ConstPtr fromROS(const rclcpp::Node::SharedPtr &node, @@ -86,7 +87,6 @@ class SegmentAnythingModule : public nn::TorchModule { - VTR_REGISTER_MODULE_DEC_TYPE(SegmentAnythingModule); }; diff --git a/main/src/vtr_lidar/src/filters/perspective_image.cpp b/main/src/vtr_lidar/src/filters/perspective_image.cpp index bfbbfbc1d..f7a13ee40 100644 --- a/main/src/vtr_lidar/src/filters/perspective_image.cpp +++ b/main/src/vtr_lidar/src/filters/perspective_image.cpp @@ -61,7 +61,8 @@ void generate_depth_image(const pcl::PointCloud& point_cloud, cv: hsv[1] = UINT8_MAX - 1; hsv[0] = abs(point.z) * UINT8_MAX / params.max_range; - idx_image.at(v, u) = i; + //Offset by 1, so that a value of 0 means no associated 3D point. + idx_image.at(v, u) = i + 1; } } @@ -102,12 +103,16 @@ void interpolate_hsv_image(cv::Mat& depth_image) { cv::add(depth_image, smoothed, depth_image, measured_pixel_mask); } -void mask_to_pointcloud(const cv::Mat& mask, const cv::Mat& index_img, pcl::PointCloud& point_cloud) { +void mask_to_pointcloud(const cv::Mat& mask, const cv::Mat& index_img, pcl::PointCloud& point_cloud, int selected_val) { index_img.forEach ( - [&](int32_t &idx, const int position[]) -> void + [&](const int32_t &idx, const int position[]) -> void { - point_cloud[idx].flex24 = (mask.at(position[0], position[1]) > 0) ? 2 : 1; + if(idx > 0){ + if (point_cloud[idx - 1].flex24 < 2) { + point_cloud[idx - 1].flex24 = (mask.at(position[0], position[1]) > 0) ? selected_val : 1; + } + } } ); } diff --git a/main/src/vtr_lidar/src/modules/planning/costmap_inflation_module.cpp b/main/src/vtr_lidar/src/modules/planning/costmap_inflation_module.cpp index 3dec161d4..12c7b74f0 100644 --- a/main/src/vtr_lidar/src/modules/planning/costmap_inflation_module.cpp +++ b/main/src/vtr_lidar/src/modules/planning/costmap_inflation_module.cpp @@ -151,9 +151,16 @@ void CostmapInflationModule::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; + { + // Lock the mutex before modifying output.obs_map + std::lock_guard lock(output.obsMapMutex); + output.costmap_sid = costmap->vertex_sid(); + output.obs_map = costmap->filter(0.1); // Modify output.obs_map safely + output.grid_resolution = config_->resolution; + } + // auto change_detection_costmap_ref = output.change_detection_costmap.locked(); + // auto &change_detection_costmap = change_detection_costmap_ref.get(); + // change_detection_costmap = costmap; CLOG(INFO, "lidar.obstacle_inflation") << "Change detection for lidar scan at stamp: " << stamp << " - DONE"; diff --git a/main/src/vtr_lidar/src/modules/planning/segmentanything_module.cpp b/main/src/vtr_lidar/src/modules/planning/segmentanything_module.cpp index 51808e86e..6bd940610 100644 --- a/main/src/vtr_lidar/src/modules/planning/segmentanything_module.cpp +++ b/main/src/vtr_lidar/src/modules/planning/segmentanything_module.cpp @@ -19,6 +19,7 @@ #include #include +#include #include "cv_bridge/cv_bridge.h" @@ -46,6 +47,7 @@ auto SegmentAnythingModule::Config::fromROS(const rclcpp::Node::SharedPtr &node, config->iou_thresh = node->declare_parameter(param_prefix + ".iou_threshold", config->iou_thresh); config->num_prompts = node->declare_parameter(param_prefix + ".num_prompts", config->num_prompts); + config->smooth_size = node->declare_parameter(param_prefix + ".smooth_size", config->smooth_size); // clang-format off config->visualize = node->declare_parameter(param_prefix + ".visualize", config->visualize); @@ -54,11 +56,17 @@ auto SegmentAnythingModule::Config::fromROS(const rclcpp::Node::SharedPtr &node, return config; } +using BGR = typename cv::Scalar; void SegmentAnythingModule::run_(QueryCache &qdata0, OutputCache &, const Graph::Ptr &graph, const TaskExecutor::Ptr &) { auto &qdata = dynamic_cast(qdata0); + static const std::vector color_options {BGR(230, 25, 75), BGR(60, 180, 75), BGR(255, 225, 25), BGR(0, 130, 200), BGR(245, 130, 48), BGR(145, 30, 180), + BGR(70, 240, 240), BGR(240, 50, 230), BGR(210, 245, 60), BGR(250, 190, 212), BGR(0, 128, 128), BGR(220, 190, 255), BGR(170, 110, 40), BGR(255, 250, 200), BGR(128, 0, 0), + BGR(170, 255, 195), BGR(128, 128, 0), BGR(255, 215, 180), BGR(0, 0, 128)}; + + if(!pub_init_){ live_mask_pub_ = qdata.node->create_publisher("detection_mask", 5); map_mask_pub_ = qdata.node->create_publisher("background_mask", 5); @@ -120,8 +128,6 @@ void SegmentAnythingModule::run_(QueryCache &qdata0, OutputCache &, map_points_mat = (T_c_s * T_s_m.cast()) * map_points_mat; map_nn_mat = (T_c_s * T_s_sm.cast()) * map_nn_mat; - - cv::Mat live_index_img = cv::Mat::zeros(config_->perspective_params.height, config_->perspective_params.width, CV_32S); cv::Mat live_hsv_img = cv::Mat::zeros(config_->perspective_params.height, config_->perspective_params.width, CV_8UC3); cv::Mat live_rgb_img = cv::Mat::zeros(config_->perspective_params.height, config_->perspective_params.width, CV_8UC3); @@ -152,8 +158,6 @@ void SegmentAnythingModule::run_(QueryCache &qdata0, OutputCache &, cv::cvtColor(map_hsv_img, map_rgb_img, cv::COLOR_HSV2RGB); - // qdata.rendered_images.emplace(std::make_pair(live_rgb_img, map_rgb_img)); - if (config_->visualize) { cv_bridge::CvImage map_cv_rgb_img; map_cv_rgb_img.header.frame_id = "lidar"; @@ -189,7 +193,7 @@ void SegmentAnythingModule::run_(QueryCache &qdata0, OutputCache &, namespace F = torch::nn::functional; - torch::Tensor average = torch::ones({1, 1, 5, 5}) / 25; + torch::Tensor average = torch::ones({1, 1, 10, 10}) / 100; diff = F::conv2d(diff.unsqueeze(0).unsqueeze(0), average); @@ -201,20 +205,17 @@ void SegmentAnythingModule::run_(QueryCache &qdata0, OutputCache &, diff = diff.squeeze(); using namespace torch::indexing; - // int data[] = { 512, 256, - // 256, 256, - // 768, 256 }; std::vector prompts; auto topk_idx_a = topk_idx.accessor(); - auto idx_a = idx.accessor(); + auto topk_val_a = topk_vals.accessor(); + auto idx_a = idx.accessor(); //shape (x, y, z, dtype=long) for (size_t i = 0; i < topk_idx_a.size(0); i++) { const long& small_idx = topk_idx_a[i]; const long& idx = idx_a[0][small_idx / diff.size(1)][small_idx % diff.size(1)]; - int prompt[] = {idx % diff.size(1), idx / diff.size(1)}; auto& pc_idx = live_index_img.at(prompt[1] / 4, prompt[0] / 4); @@ -225,15 +226,17 @@ void SegmentAnythingModule::run_(QueryCache &qdata0, OutputCache &, h_point = T_s_sm.inverse().cast() * h_point; double theta = atan2(h_point[1], h_point[0]); - CLOG(DEBUG, "lidar.perspective") << "Diff Tensor prompt (" << prompt[0] << ", " << prompt[1] << ") Theta: " << theta; + CLOG(DEBUG, "lidar.perspective") << "Diff Tensor prompt (" << prompt[0] << ", " << prompt[1] << ") Theta: " << theta << " DIff norm " << topk_val_a[i]; //Prompts are x, y rather than row, column //map_tensor.index({0, prompt[1], prompt[0]}).item().to() > 0 || - if (!((theta > 2.06 && theta < 2.18) || (theta > -2.27 && theta < -2.135))) + if (!((theta > 1.98 && theta < 2.18) || (theta > -2.27 && theta < -2.135)) && topk_val_a[i] > 10.){ prompts.push_back(torch::from_blob(prompt, {2}, torch::kInt).to(device)); - else + + } else { CLOG(DEBUG, "lidar.perspective") << "Prompt point on an empty map pixel. Try again."; + } } torch::NoGradGuard no_grad; @@ -245,11 +248,13 @@ void SegmentAnythingModule::run_(QueryCache &qdata0, OutputCache &, auto outputs = network(jit_inputs).toGenericDict(); - CLOG(DEBUG, "lidar.perspective") << "Ran model!"; auto teach_masks = outputs.at("teach_masks").toTensor(); auto repeat_masks = outputs.at("repeat_masks").toTensor(); + + CLOG(DEBUG, "lidar.perspective") << "Ran model! Teach size " << teach_masks.sizes() << " Repeat size " << repeat_masks.sizes(); + auto intersection = teach_masks.bitwise_and(repeat_masks); auto unions = teach_masks.bitwise_or(repeat_masks); @@ -261,33 +266,76 @@ void SegmentAnythingModule::run_(QueryCache &qdata0, OutputCache &, auto ious_a = ious.accessor(); - for (size_t i = 0; i < ious_a.size(0); i++) { - CLOG(DEBUG, "lidar.perspective") << "Iou VAL " << ious_a[i] << " < " << config_->iou_thresh; + cv::Mat teach_mask = cv::Mat::zeros(teach_masks.size(1), teach_masks.size(2), CV_8UC3); + cv::Mat repeat_mask = cv::Mat::zeros(repeat_masks.size(1), repeat_masks.size(2), CV_8UC3); + + for (int i = 0; i < prompts.size(); ++i) { + + auto prompt = prompts[i].cpu(); + auto prompt_a = prompt.accessor(); + + + auto active_teach_tensor = teach_masks.index({i, Slice(), Slice()}).to(torch::kByte).mul(255).contiguous(); + auto active_repeat_tensor = repeat_masks.index({i, Slice(), Slice()}).to(torch::kByte).mul(255).contiguous(); - if (ious_a[i] < config_->iou_thresh) { + cv::Mat active_teach_mask = cv::Mat{active_teach_tensor.size(0), active_teach_tensor.size(1), CV_8UC1, active_teach_tensor.data_ptr()}; + //mask_to_pointcloud(active_teach_mask, map_index_img, nn_map_point_cloud, 2+i); + + // auto map_change_point_cloud = [&]{ + // std::vector indices; + // indices.reserve(nn_map_point_cloud.size()); + // for (size_t i = 0; i < nn_map_point_cloud.size(); ++i) { + // if (nn_map_point_cloud[i].flex24 == 2+i) + // indices.emplace_back(i); + // } + // return pcl::PointCloud(nn_map_point_cloud, indices); + // }(); + + cv::Mat active_repeat_mask = cv::Mat{active_repeat_tensor.size(0), active_repeat_tensor.size(1), CV_8UC1, active_repeat_tensor.data_ptr()}; + //mask_to_pointcloud(active_repeat_mask, live_index_img, raw_point_cloud, 2+i); + + // auto change_point_cloud = [&]{ + // std::vector indices; + // indices.reserve(raw_point_cloud.size()); + // for (size_t i = 0; i < raw_point_cloud.size(); ++i) { + // if (raw_point_cloud[i].flex24 == 2+i) + // indices.emplace_back(i); + // } + // return pcl::PointCloud(raw_point_cloud, indices); + // }(); + + cv::Scalar mapMean, mapStdDev, liveMean, liveStdDev; + cv::meanStdDev(map_hsv_img, mapMean, mapStdDev, active_teach_mask); + cv::meanStdDev(live_hsv_img, liveMean, liveStdDev, active_repeat_mask); + + CLOG(DEBUG, "lidar.perspective") << "Map Mean" << mapMean << "Live mean " << liveMean; + + if (ious_a[i] < config_->iou_thresh && active_repeat_tensor.sum().item() < 0.1*255*128*256 && abs(liveMean[0] - mapMean[0]) > 10.) { idxs_to_keep.push_back(i); + teach_mask.setTo(color_options[2*i % color_options.size()], active_teach_mask == 255); + repeat_mask.setTo(color_options[2*i % color_options.size()], active_repeat_mask == 255); } + + cv::circle(teach_mask, cv::Point{prompt_a[0] / 4, prompt_a[1] / 4}, 6, color_options[(2*i + 1) % color_options.size()], 1); + cv::circle(repeat_mask, cv::Point{prompt_a[0] / 4, prompt_a[1] / 4}, 6, color_options[(2*i + 1) % color_options.size()], 1); + + // PointWithInfo minPt, maxPt; + // pcl::getMinMax3D(change_point_cloud, minPt, maxPt); + // CLOG(DEBUG, "lidar.perspective") << "Min" << minPt.x << " max: " << maxPt.x; } + CLOG(DEBUG, "lidar.perspective") << "IDs to KEEP " << idxs_to_keep; - cv::Mat teach_mask = cv::Mat::zeros(teach_masks.size(1), teach_masks.size(2), CV_8UC1); - cv::Mat repeat_mask = cv::Mat::zeros(repeat_masks.size(1), repeat_masks.size(2), CV_8UC1); + // cv::Mat teach_mask = cv::Mat::zeros(teach_masks.size(1), teach_masks.size(2), CV_8UC1); + // cv::Mat repeat_mask = cv::Mat::zeros(repeat_masks.size(1), repeat_masks.size(2), CV_8UC1); if (idxs_to_keep.size() > 0){ - auto bg_flat = teach_masks.sum({0}).to(torch::kByte).mul(255); - teach_mask = cv::Mat{bg_flat.size(0), bg_flat.size(1), CV_8UC1, bg_flat.data_ptr()}; - - auto changes = repeat_masks.index({torch::from_blob(idxs_to_keep.data(), {idxs_to_keep.size()}, torch::kInt), Slice(), Slice()}); - CLOG(DEBUG, "lidar.perspective") << "Repeat Tensor size " << changes.sizes(); - - changes = changes.sum({0}).div(changes.max()).to(torch::kByte).mul(255); + changes = changes.sum({0}).to(torch::kByte).mul(255).contiguous(); - repeat_mask = cv::Mat{changes.size(0), changes.size(1), CV_8UC1, changes.data_ptr()}; - - - mask_to_pointcloud(repeat_mask, live_index_img, raw_point_cloud); + auto repeat_mask_flat = cv::Mat{changes.size(0), changes.size(1), CV_8UC1, changes.data_ptr()}; + mask_to_pointcloud(repeat_mask_flat, live_index_img, raw_point_cloud, 2); auto obstacle_point_cloud = std::make_shared>(raw_point_cloud); @@ -297,18 +345,19 @@ void SegmentAnythingModule::run_(QueryCache &qdata0, OutputCache &, std::vector indices; indices.reserve(obstacle_point_cloud->size()); for (size_t i = 0; i < obstacle_point_cloud->size(); ++i) { - if ((*obstacle_point_cloud)[i].flex24 > 0) + if ((*obstacle_point_cloud)[i].flex24 > 1) indices.emplace_back(i); } *obstacle_point_cloud = pcl::PointCloud(*obstacle_point_cloud, indices); auto obs_mat = obstacle_point_cloud->getMatrixXfMap(4, PointWithInfo::size(), PointWithInfo::cartesian_offset()); - obs_mat = T_s_r.inverse().matrix().cast() * T_c_s.inverse() * obs_mat; + obs_mat = (T_s_r * T_r_v_loc).inverse().matrix().cast() * T_c_s.inverse() * obs_mat; qdata.changed_points.emplace(*obstacle_point_cloud); } - + } else { + qdata.changed_points.emplace(pcl::PointCloud()); } // /// publish the transformed pointcloud @@ -316,14 +365,14 @@ void SegmentAnythingModule::run_(QueryCache &qdata0, OutputCache &, cv_bridge::CvImage live_mask_img_msg; live_mask_img_msg.header.frame_id = "lidar"; //cv_rgb_img.header.stamp = qdata.stamp->header.stamp; - live_mask_img_msg.encoding = "mono8"; + live_mask_img_msg.encoding = "bgr8"; live_mask_img_msg.image = repeat_mask; live_mask_pub_->publish(*live_mask_img_msg.toImageMsg()); cv_bridge::CvImage map_mask_img_msg; map_mask_img_msg.header.frame_id = "lidar"; //cv_rgb_img.header.stamp = qdata.stamp->header.stamp; - map_mask_img_msg.encoding = "mono8"; + map_mask_img_msg.encoding = "bgr8"; map_mask_img_msg.image = teach_mask; map_mask_pub_->publish(*map_mask_img_msg.toImageMsg()); diff --git a/rviz/lidar.rviz b/rviz/lidar.rviz index f6981cb10..c3c7f3266 100644 --- a/rviz/lidar.rviz +++ b/rviz/lidar.rviz @@ -10,8 +10,9 @@ Panels: - /Image2 - /Image3 - /Image4 + - /PointCloud21 Splitter Ratio: 0.5 - Tree Height: 162 + Tree Height: 140 - Class: rviz_common/Selection Name: Selection - Class: rviz_common/Tool Properties @@ -108,7 +109,7 @@ Visualization Manager: Value: /vtr/background_mask Value: true - Alpha: 1 - Autocompute Intensity Bounds: false + Autocompute Intensity Bounds: true Autocompute Value Bounds: Max Value: 10 Min Value: -10 @@ -124,7 +125,7 @@ Visualization Manager: Max Color: 255; 255; 255 Max Intensity: 2 Min Color: 0; 0; 0 - Min Intensity: 1 + Min Intensity: 0 Name: PointCloud2 Position Transformer: XYZ Selectable: true @@ -141,6 +142,20 @@ Visualization Manager: Use Fixed Frame: true Use rainbow: true Value: true + - Class: rviz_default_plugins/Image + Enabled: true + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: Image + Normalize Range: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /vtr/normed_diff + Value: true Enabled: true Global Options: Background Color: 48; 48; 48 @@ -187,7 +202,7 @@ Visualization Manager: Views: Current: Class: rviz_default_plugins/Orbit - Distance: 27.068925857543945 + Distance: 18.88730812072754 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 @@ -202,10 +217,10 @@ Visualization Manager: Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: -1.2647966146469116 + Pitch: 1.5697963237762451 Target Frame: Value: Orbit (rviz) - Yaw: 4.588586330413818 + Yaw: 1.62860107421875 Saved: ~ Window Geometry: Displays: @@ -215,7 +230,7 @@ Window Geometry: Hide Right Dock: false Image: collapsed: false - QMainWindow State: 000000ff00000000fd00000004000000000000034400000566fc020000000cfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b000000dd000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d006100670065010000011e000001390000002800fffffffb0000000a0049006d006100670065010000025d0000011b0000002800fffffffb0000000a0049006d006100670065010000037e000001040000002800fffffffb0000000a0049006d0061006700650100000488000001190000002800ffffff000000010000010f00000566fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003b00000566000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000009b20000003efc0100000002fb0000000800540069006d00650100000000000009b20000025300fffffffb0000000800540069006d00650100000000000004500000000000000000000005530000056600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd00000004000000000000034400000566fc020000000dfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b000000c7000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d0061006700650100000108000001020000002800fffffffb0000000a0049006d0061006700650100000210000000e90000002800fffffffb0000000a0049006d00610067006501000002ff000000d60000002800fffffffb0000000a0049006d00610067006501000003db000000e80000002800fffffffb0000000a0049006d00610067006501000004c9000000d80000002800ffffff000000010000010f00000566fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003b00000566000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000009b20000003efc0100000002fb0000000800540069006d00650100000000000009b20000025300fffffffb0000000800540069006d00650100000000000004500000000000000000000005530000056600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Time: From aafaed639d14a7af5e2484044e055f5d582095ef Mon Sep 17 00:00:00 2001 From: a-krawciw Date: Wed, 20 Mar 2024 14:36:20 -0400 Subject: [PATCH 66/77] Fix obstacle inflation module. Prevent prompting on iterpolated pixels. --- config/ouster_warthog_lasersam.yaml | 5 +- .../planning/costmap_inflation_module.cpp | 17 ++- .../planning/segmentanything_module.cpp | 30 ++++-- main/src/vtr_path_planning/src/cbit/cbit.cpp | 2 + rviz/ouster.rviz | 102 +++++++++++++++--- 5 files changed, 121 insertions(+), 35 deletions(-) diff --git a/config/ouster_warthog_lasersam.yaml b/config/ouster_warthog_lasersam.yaml index db01ed32b..e17817d3f 100644 --- a/config/ouster_warthog_lasersam.yaml +++ b/config/ouster_warthog_lasersam.yaml @@ -6,6 +6,7 @@ - tactic.pipeline - tactic.module - cbit.obstacle_filtering + - lidar.obstacle_inflation # pose graph #- pose_graph @@ -53,9 +54,11 @@ size_x: 16.0 size_y: 8.0 visualize: true + fake_segmentation: + type: lidar.TestObstacleModule path_planning: cbit: obstacle_avoidance: true mpc: - homotopy_guided_mpc: true + homotopy_guided_mpc: false diff --git a/main/src/vtr_lidar/src/modules/planning/costmap_inflation_module.cpp b/main/src/vtr_lidar/src/modules/planning/costmap_inflation_module.cpp index 12c7b74f0..8143ec727 100644 --- a/main/src/vtr_lidar/src/modules/planning/costmap_inflation_module.cpp +++ b/main/src/vtr_lidar/src/modules/planning/costmap_inflation_module.cpp @@ -55,7 +55,7 @@ class DetectChangeOp { // update the value of v dist = std::sqrt(dist); // convert to distance v = std::max(1 - (dist - d1_) / d0_, 0.0f); - v = std::min(v, 0.9f); // 1 is bad for visualization + v = std::min(v, 1.0f); // 1 is NESSECARY! } private: @@ -152,15 +152,12 @@ void CostmapInflationModule::run_(QueryCache &qdata0, OutputCache &output0, /// output { - // Lock the mutex before modifying output.obs_map - std::lock_guard lock(output.obsMapMutex); - output.costmap_sid = costmap->vertex_sid(); - output.obs_map = costmap->filter(0.1); // Modify output.obs_map safely - output.grid_resolution = config_->resolution; - } - // auto change_detection_costmap_ref = output.change_detection_costmap.locked(); - // auto &change_detection_costmap = change_detection_costmap_ref.get(); - // change_detection_costmap = costmap; + // Lock the mutex before modifying output.obs_map + std::lock_guard lock(output.obsMapMutex); + output.costmap_sid = costmap->vertex_sid(); + output.obs_map = costmap->filter(0.01); // Modify output.obs_map safely + output.grid_resolution = config_->resolution; + } CLOG(INFO, "lidar.obstacle_inflation") << "Change detection for lidar scan at stamp: " << stamp << " - DONE"; diff --git a/main/src/vtr_lidar/src/modules/planning/segmentanything_module.cpp b/main/src/vtr_lidar/src/modules/planning/segmentanything_module.cpp index 6bd940610..f8b88606c 100644 --- a/main/src/vtr_lidar/src/modules/planning/segmentanything_module.cpp +++ b/main/src/vtr_lidar/src/modules/planning/segmentanything_module.cpp @@ -112,21 +112,25 @@ void SegmentAnythingModule::run_(QueryCache &qdata0, OutputCache &, auto map_point_cloud = sub_map.point_cloud(); auto map_points_mat = map_point_cloud.getMatrixXfMap(4, PointWithInfo::size(), PointWithInfo::cartesian_offset()); - const auto T_s_m = (T_s_r * T_r_v_loc * T_v_m_loc).matrix(); + const auto T_s_m = T_s_r * T_r_v_loc * T_v_m_loc; auto live_points_mat = raw_point_cloud.getMatrixXfMap(4, PointWithInfo::size(), PointWithInfo::cartesian_offset()); - Eigen::Matrix4f T_c_s; + Eigen::Matrix4d T_c_s_temp; - T_c_s << 0, -1, 0, 0, + T_c_s_temp << 0, -1, 0, 0, 0, 0, 1, 0, -1, 0, 0, 0, 0, 0, 0, 1; - live_points_mat = T_c_s * live_points_mat; - map_points_mat = (T_c_s * T_s_m.cast()) * map_points_mat; - map_nn_mat = (T_c_s * T_s_sm.cast()) * map_nn_mat; + const tactic::EdgeTransform T_c_s {T_c_s_temp}; + const tactic::EdgeTransform T_v_loc_c = (T_c_s * T_s_r * T_r_v_loc).inverse(); + + + live_points_mat = T_c_s.matrix().cast() * live_points_mat; + map_points_mat = (T_c_s * T_s_m).matrix().cast() * map_points_mat; + map_nn_mat = (T_c_s * T_s_sm).matrix().cast() * map_nn_mat; cv::Mat live_index_img = cv::Mat::zeros(config_->perspective_params.height, config_->perspective_params.width, CV_32S); cv::Mat live_hsv_img = cv::Mat::zeros(config_->perspective_params.height, config_->perspective_params.width, CV_8UC3); @@ -220,10 +224,15 @@ void SegmentAnythingModule::run_(QueryCache &qdata0, OutputCache &, auto& pc_idx = live_index_img.at(prompt[1] / 4, prompt[0] / 4); - auto live_point = raw_point_cloud[pc_idx]; + if (pc_idx == 0) { + CLOG(DEBUG, "lidar.perspective") << "Prompt point on an interpolated live pixel. Skipping."; + continue; + } + + auto live_point = raw_point_cloud[pc_idx - 1]; Eigen::Vector4f h_point; h_point << live_point.x, live_point.y, live_point.z, 1.0f; - h_point = T_s_sm.inverse().cast() * h_point; + h_point = T_v_loc_c.matrix().cast() * h_point; double theta = atan2(h_point[1], h_point[0]); CLOG(DEBUG, "lidar.perspective") << "Diff Tensor prompt (" << prompt[0] << ", " << prompt[1] << ") Theta: " << theta << " DIff norm " << topk_val_a[i]; @@ -231,9 +240,8 @@ void SegmentAnythingModule::run_(QueryCache &qdata0, OutputCache &, //Prompts are x, y rather than row, column //map_tensor.index({0, prompt[1], prompt[0]}).item().to() > 0 || - if (!((theta > 1.98 && theta < 2.18) || (theta > -2.27 && theta < -2.135)) && topk_val_a[i] > 10.){ + if (!((theta > 0.611 && theta < 0.96) || (theta > -0.96 && theta < -0.611)) && topk_val_a[i] > 10.){ prompts.push_back(torch::from_blob(prompt, {2}, torch::kInt).to(device)); - } else { CLOG(DEBUG, "lidar.perspective") << "Prompt point on an empty map pixel. Try again."; } @@ -352,7 +360,7 @@ void SegmentAnythingModule::run_(QueryCache &qdata0, OutputCache &, pcl::PointCloud(*obstacle_point_cloud, indices); auto obs_mat = obstacle_point_cloud->getMatrixXfMap(4, PointWithInfo::size(), PointWithInfo::cartesian_offset()); - obs_mat = (T_s_r * T_r_v_loc).inverse().matrix().cast() * T_c_s.inverse() * obs_mat; + obs_mat = T_v_loc_c.matrix().cast() * obs_mat; qdata.changed_points.emplace(*obstacle_point_cloud); } diff --git a/main/src/vtr_path_planning/src/cbit/cbit.cpp b/main/src/vtr_path_planning/src/cbit/cbit.cpp index b0adf0ae7..2ac7c7af1 100644 --- a/main/src/vtr_path_planning/src/cbit/cbit.cpp +++ b/main/src/vtr_path_planning/src/cbit/cbit.cpp @@ -241,6 +241,8 @@ void CBIT::initializeRoute(RobotState& robot_state) { { } + costmap_ptr = std::make_shared (); + lgmath::se3::TransformationWithCovariance teach_frame; std::tuple se3_vector; Pose se3_pose; diff --git a/rviz/ouster.rviz b/rviz/ouster.rviz index fecd477e5..9594ed340 100644 --- a/rviz/ouster.rviz +++ b/rviz/ouster.rviz @@ -5,10 +5,10 @@ Panels: Property Tree Widget: Expanded: - /Global Options1 - - /TF1/Status1 + - /Change Detection Scan1 - /Teach Path1/Topic1 Splitter Ratio: 0.4507042169570923 - Tree Height: 871 + Tree Height: 1391 - Class: rviz_common/Selection Name: Selection - Class: rviz_common/Tool Properties @@ -26,7 +26,7 @@ Panels: Experimental: false Name: Time SyncMode: 0 - SyncSource: "" + SyncSource: Change Detection Scan Visualization Manager: Class: "" Displays: @@ -60,10 +60,74 @@ Visualization Manager: Frame Timeout: 99999 Frames: All Enabled: false + base_link: + Value: true + chassis_link: + Value: true + diff_link: + Value: true + front_left_wheel_link: + Value: true + front_right_wheel_link: + Value: true + imu_link: + Value: true + left_diff_unit_headlight_link: + Value: true + left_diff_unit_link: + Value: true + left_diff_unit_taillight_link: + Value: true lidar: Value: false + loc vertex frame: + Value: true + loc vertex frame (offset): + Value: true + navtech_base: + Value: true + nerian_left: + Value: true + nerian_stereo: + Value: true + novatel: + Value: true + odo vertex frame: + Value: true + odom: + Value: true + os_imu: + Value: true + os_lidar: + Value: true + os_sensor: + Value: true + os_sensor_base_link: + Value: true + os_sensor_baseplate: + Value: true + planning frame: + Value: true + rear_left_wheel_link: + Value: true + rear_right_wheel_link: + Value: true + right_diff_unit_headlight_link: + Value: true + right_diff_unit_link: + Value: true + right_diff_unit_taillight_link: + Value: true robot: Value: true + robot planning: + Value: true + robot planning (extrapolated) mpc: + Value: true + sensor_tower: + Value: true + top_chassis_link: + Value: true world: Value: false world (offset): @@ -75,8 +139,20 @@ Visualization Manager: Show Names: true Tree: world: - world (offset): + loc vertex frame: {} + odo vertex frame: + robot: + lidar: + {} + planning frame: + robot planning: + {} + robot planning (extrapolated) mpc: + {} + world (offset): + loc vertex frame (offset): + {} Update Interval: 0 Value: true - Alpha: 1 @@ -295,7 +371,7 @@ Visualization Manager: Color: 255; 255; 0 Color Transformer: Intensity Decay Time: 0 - Enabled: false + Enabled: true Invert Rainbow: false Max Color: 255; 255; 255 Max Intensity: 2 @@ -313,10 +389,10 @@ Visualization Manager: Filter size: 10 History Policy: Keep Last Reliability Policy: Best Effort - Value: /vtr/change_detection_scan + Value: /vtr/change_detection_concat Use Fixed Frame: true Use rainbow: true - Value: false + Value: true - Alpha: 1 Buffer Length: 1 Class: rviz_default_plugins/Path @@ -630,10 +706,10 @@ Visualization Manager: Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: 1.5697963237762451 + Pitch: 0.7497973442077637 Target Frame: robot Value: Orbit (rviz_default_plugins) - Yaw: 3.441279411315918 + Yaw: 3.326280117034912 Saved: - Class: rviz_default_plugins/Orbit Distance: 111.39698028564453 @@ -838,10 +914,10 @@ Visualization Manager: Window Geometry: Displays: collapsed: false - Height: 1016 + Height: 1536 Hide Left Dock: false Hide Right Dock: true - QMainWindow State: 000000ff00000000fd000000040000000000000281000003a2fc0200000007fc0000003d000001a20000000000fffffffaffffffff0100000002fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730000000000ffffffff0000008c00fffffffb0000001200530065006c0065006300740069006f006e0000000000000002530000006c00fffffffb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b000003a2000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c0000026100000001000001c0000003bdfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003b000003bd000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000005000000006efc0100000002fb0000000800540069006d00650000000000000005000000025300fffffffb0000000800540069006d00650100000000000004500000000000000000000004b1000003a200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd000000040000000000000281000005aafc020000000cfc0000003d000001a20000000000fffffffaffffffff0100000002fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730000000000ffffffff0000008c00fffffffb0000001200530065006c0065006300740069006f006e0000000000000002530000006c00fffffffb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b000005aa000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d0061006700650100000357000000160000000000000000fb0000000a0049006d0061006700650100000373000000160000000000000000fb0000000a0049006d006100670065010000038f000000160000000000000000fb0000000a0049006d00610067006501000003ab000000160000000000000000fb0000000a0049006d00610067006501000003c700000016000000000000000000000001000001c0000003bdfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003b000003bd000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000005000000006efc0100000002fb0000000800540069006d00650000000000000005000000025300fffffffb0000000800540069006d006501000000000000045000000000000000000000072b000005aa00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Time: @@ -850,6 +926,6 @@ Window Geometry: collapsed: false Views: collapsed: true - Width: 1848 - X: 72 + Width: 2482 + X: 78 Y: 27 From 64ce6e4c77769d7fc13bb01ba641bc0333d8609f Mon Sep 17 00:00:00 2001 From: a-krawciw Date: Wed, 20 Mar 2024 16:11:32 -0400 Subject: [PATCH 67/77] Prompt on small image. --- .../planning/segmentanything_module.cpp | 25 +++++++++++++------ 1 file changed, 17 insertions(+), 8 deletions(-) diff --git a/main/src/vtr_lidar/src/modules/planning/segmentanything_module.cpp b/main/src/vtr_lidar/src/modules/planning/segmentanything_module.cpp index f8b88606c..432ad505c 100644 --- a/main/src/vtr_lidar/src/modules/planning/segmentanything_module.cpp +++ b/main/src/vtr_lidar/src/modules/planning/segmentanything_module.cpp @@ -135,18 +135,21 @@ void SegmentAnythingModule::run_(QueryCache &qdata0, OutputCache &, cv::Mat live_index_img = cv::Mat::zeros(config_->perspective_params.height, config_->perspective_params.width, CV_32S); cv::Mat live_hsv_img = cv::Mat::zeros(config_->perspective_params.height, config_->perspective_params.width, CV_8UC3); cv::Mat live_rgb_img = cv::Mat::zeros(config_->perspective_params.height, config_->perspective_params.width, CV_8UC3); + cv::Mat raw_rgb_img = cv::Mat::zeros(config_->perspective_params.height, config_->perspective_params.width, CV_8UC3); generate_depth_image(raw_point_cloud, live_hsv_img, live_index_img, config_->perspective_params); + cv::Mat raw_hsv_img = live_hsv_img.clone(); interpolate_hsv_image(live_hsv_img); cv::cvtColor(live_hsv_img, live_rgb_img, cv::COLOR_HSV2RGB); + cv::cvtColor(raw_hsv_img, raw_rgb_img, cv::COLOR_HSV2RGB); if (config_->visualize) { cv_bridge::CvImage live_cv_rgb_img; live_cv_rgb_img.header.frame_id = "lidar"; //cv_rgb_img.header.stamp = qdata.stamp->header.stamp; live_cv_rgb_img.encoding = "rgb8"; - live_cv_rgb_img.image = live_rgb_img; + live_cv_rgb_img.image = raw_rgb_img; live_img_pub_->publish(*live_cv_rgb_img.toImageMsg()); } @@ -192,16 +195,18 @@ void SegmentAnythingModule::run_(QueryCache &qdata0, OutputCache &, torch::Tensor live_tensor = torch::from_blob(big_live.data, {big_live.rows, big_live.cols, big_live.channels()}, torch::kByte).to(torch::kFloat32).permute({2, 0, 1}).contiguous(); torch::Tensor map_tensor = torch::from_blob(big_map.data, {big_map.rows, big_map.cols, big_map.channels()}, torch::kByte).to(torch::kFloat32).permute({2, 0, 1}).contiguous(); - torch::Tensor diff = live_tensor - map_tensor; - diff = diff.norm(2, {0}); + torch::Tensor small_live_tensor = torch::from_blob(raw_rgb_img.data, {raw_rgb_img.rows, raw_rgb_img.cols, raw_rgb_img.channels()}, torch::kByte).to(torch::kFloat32).permute({2, 0, 1}); + torch::Tensor small_map_tensor = torch::from_blob(map_rgb_img.data, {map_rgb_img.rows, map_rgb_img.cols, map_rgb_img.channels()}, torch::kByte).to(torch::kFloat32).permute({2, 0, 1}); + torch::Tensor diff = small_live_tensor - small_map_tensor; + diff = diff.clamp(0, 255.0).norm(2, {0}); namespace F = torch::nn::functional; - torch::Tensor average = torch::ones({1, 1, 10, 10}) / 100; + torch::Tensor average = torch::ones({1, 1, 3, 3}) / 9; diff = F::conv2d(diff.unsqueeze(0).unsqueeze(0), average); - const auto [smaller_diff, idx] = F::max_pool2d_with_indices(diff.squeeze(0), F::MaxPool2dFuncOptions(20)); + const auto [smaller_diff, idx] = F::max_pool2d_with_indices(diff.squeeze(0), F::MaxPool2dFuncOptions(5)); const auto [topk_vals, topk_idx] = smaller_diff.flatten().topk(config_->num_prompts); CLOG(DEBUG, "lidar.perspective") << "Max pooling " << smaller_diff.sizes() << " and " << idx.sizes(); @@ -218,17 +223,21 @@ void SegmentAnythingModule::run_(QueryCache &qdata0, OutputCache &, for (size_t i = 0; i < topk_idx_a.size(0); i++) { const long& small_idx = topk_idx_a[i]; - const long& idx = idx_a[0][small_idx / diff.size(1)][small_idx % diff.size(1)]; + const long& idx = idx_a[0][small_idx / smaller_diff.size(2)][small_idx % smaller_diff.size(2)]; int prompt[] = {idx % diff.size(1), idx / diff.size(1)}; - auto& pc_idx = live_index_img.at(prompt[1] / 4, prompt[0] / 4); + auto& pc_idx = live_index_img.at(prompt[1], prompt[0]); if (pc_idx == 0) { - CLOG(DEBUG, "lidar.perspective") << "Prompt point on an interpolated live pixel. Skipping."; + CLOG(DEBUG, "lidar.perspective") << "Prompt point (" << prompt[0] << ", " << prompt[1] << ") << on an interpolated live pixel. Skipping."; continue; } + //Scale up to whole image + prompt[0] = 4*prompt[0] + 2; + prompt[1] = 4*prompt[1] + 2; + auto live_point = raw_point_cloud[pc_idx - 1]; Eigen::Vector4f h_point; h_point << live_point.x, live_point.y, live_point.z, 1.0f; From e8334b2da62dd38b252382b73ca04c8bc518c45c Mon Sep 17 00:00:00 2001 From: a-krawciw Date: Wed, 20 Mar 2024 18:02:18 -0400 Subject: [PATCH 68/77] Add testing modules. Remove before merging --- .../include/vtr_lidar/modules/modules.hpp | 1 + .../modules/planning/test_obstacle_module.hpp | 59 +++++++++++++++++++ .../modules/planning/test_obstacle_module.cpp | 51 ++++++++++++++++ 3 files changed, 111 insertions(+) create mode 100644 main/src/vtr_lidar/include/vtr_lidar/modules/planning/test_obstacle_module.hpp create mode 100644 main/src/vtr_lidar/src/modules/planning/test_obstacle_module.cpp diff --git a/main/src/vtr_lidar/include/vtr_lidar/modules/modules.hpp b/main/src/vtr_lidar/include/vtr_lidar/modules/modules.hpp index b4eb02cd2..0126264ab 100644 --- a/main/src/vtr_lidar/include/vtr_lidar/modules/modules.hpp +++ b/main/src/vtr_lidar/include/vtr_lidar/modules/modules.hpp @@ -49,6 +49,7 @@ #include "vtr_lidar/modules/planning/perspective_render_module.hpp" #include "vtr_lidar/modules/planning/segmentanything_module.hpp" #include "vtr_lidar/modules/planning/rangenet_change_detection_module.hpp" +#include "vtr_lidar/modules/planning/test_obstacle_module.hpp" #include "vtr_lidar/modules/planning/costmap_inflation_module.hpp" #include "vtr_lidar/modules/planning/blindspot_inflation_module.hpp" diff --git a/main/src/vtr_lidar/include/vtr_lidar/modules/planning/test_obstacle_module.hpp b/main/src/vtr_lidar/include/vtr_lidar/modules/planning/test_obstacle_module.hpp new file mode 100644 index 000000000..c02783cf0 --- /dev/null +++ b/main/src/vtr_lidar/include/vtr_lidar/modules/planning/test_obstacle_module.hpp @@ -0,0 +1,59 @@ +// Copyright 2024, 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 test_obstacle_module.hpp + * \author Alec Krawciw, Autonomous Space Robotics Lab (ASRL) + */ +#pragma once + +#include "vtr_lidar/cache.hpp" +#include "vtr_tactic/modules/base_module.hpp" +#include "vtr_tactic/task_queue.hpp" + +namespace vtr { +namespace lidar { + +class TestObstacleModule : public tactic::BaseModule { + public: + PTR_TYPEDEFS(TestObstacleModule); + + static constexpr auto static_name = "lidar.TestObstacleModule"; + + /** \brief Collection of config parameters */ + struct Config : public BaseModule::Config { + PTR_TYPEDEFS(Config); + + static ConstPtr fromROS(const rclcpp::Node::SharedPtr &node, + const std::string ¶m_prefix); + }; + + TestObstacleModule( + const Config::ConstPtr &config, + const std::shared_ptr &module_factory = nullptr, + const std::string &name = static_name) : + tactic::BaseModule{module_factory, name}, config_(config) {}; + + private: + void run_(tactic::QueryCache &qdata, tactic::OutputCache &output, + const tactic::Graph::Ptr &graph, + const tactic::TaskExecutor::Ptr &executor) override; + + Config::ConstPtr config_; + + VTR_REGISTER_MODULE_DEC_TYPE(TestObstacleModule); +}; + +} // namespace lidar +} // namespace vtr diff --git a/main/src/vtr_lidar/src/modules/planning/test_obstacle_module.cpp b/main/src/vtr_lidar/src/modules/planning/test_obstacle_module.cpp new file mode 100644 index 000000000..dc51da064 --- /dev/null +++ b/main/src/vtr_lidar/src/modules/planning/test_obstacle_module.cpp @@ -0,0 +1,51 @@ +// Copyright 2024, 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 test_obstacle_module.cpp + * \author Alec Krawciw, Autonomous Space Robotics Lab (ASRL) + */ +#include +#include "vtr_lidar/data_types/costmap.hpp" + +namespace vtr { +namespace lidar { + +using namespace tactic; + +auto TestObstacleModule::Config::fromROS(const rclcpp::Node::SharedPtr &node, + const std::string ¶m_prefix) + -> ConstPtr { + // clang-format on + return std::make_shared(); +} + + +void TestObstacleModule::run_(QueryCache &qdata0, OutputCache &, + const Graph::Ptr &graph, const TaskExecutor::Ptr &) { + auto &qdata = dynamic_cast(qdata0); + + auto point_cloud = std::make_shared>(200, 1); + + for (size_t i = 0; i < point_cloud->size(); ++i) { + point_cloud->at(i).x = 3.0 + (rand() % 100) / 100.0; + point_cloud->at(i).y = 0.0 + (rand() % 100) / 100.0; + point_cloud->at(i).z = 1.0 + (rand() % 100) / 100.0; + } + + qdata.changed_points.emplace(*point_cloud); +} + +} // namespace lidar +} // namespace vtr \ No newline at end of file From 0e7c332a4cd6d7a3f8a87a7f8ca8095fd3df690b Mon Sep 17 00:00:00 2001 From: a-krawciw Date: Fri, 22 Mar 2024 15:17:44 -0400 Subject: [PATCH 69/77] Update sampling approach. --- .../planning/segmentanything_module.cpp | 73 ++++++++++++++----- 1 file changed, 55 insertions(+), 18 deletions(-) diff --git a/main/src/vtr_lidar/src/modules/planning/segmentanything_module.cpp b/main/src/vtr_lidar/src/modules/planning/segmentanything_module.cpp index 432ad505c..fe0a1717b 100644 --- a/main/src/vtr_lidar/src/modules/planning/segmentanything_module.cpp +++ b/main/src/vtr_lidar/src/modules/planning/segmentanything_module.cpp @@ -21,6 +21,9 @@ #include #include #include "cv_bridge/cv_bridge.h" +#include + + namespace vtr { @@ -156,6 +159,7 @@ void SegmentAnythingModule::run_(QueryCache &qdata0, OutputCache &, cv::Mat map_index_img = cv::Mat::zeros(config_->perspective_params.height, config_->perspective_params.width, CV_32S); cv::Mat map_hsv_img = cv::Mat::zeros(config_->perspective_params.height, config_->perspective_params.width, CV_8UC3); cv::Mat map_rgb_img = cv::Mat::zeros(config_->perspective_params.height, config_->perspective_params.width, CV_8UC3); + cv::Mat similarity_diff; // = cv::Mat::empty(config_->perspective_params.height, config_->perspective_params.width, CV_32FC1); // generate_depth_image(map_point_cloud, map_hsv_img, map_index_img, config_->perspective_params); @@ -165,6 +169,19 @@ void SegmentAnythingModule::run_(QueryCache &qdata0, OutputCache &, cv::cvtColor(map_hsv_img, map_rgb_img, cv::COLOR_HSV2RGB); + auto val = cv::quality::QualitySSIM::compute(map_hsv_img, live_hsv_img, similarity_diff); + CLOG(DEBUG, "lidar.perspective") << "Sim: " << similarity_diff.type(); + similarity_diff = similarity_diff; + similarity_diff *= 255; + similarity_diff.convertTo(similarity_diff, CV_8UC3); + + cv::Mat hsv_scores[3]; //destination array + cv::split(similarity_diff, hsv_scores);//split source + + + CLOG(DEBUG, "lidar.perspective") << "SSIM " << val; + + if (config_->visualize) { cv_bridge::CvImage map_cv_rgb_img; map_cv_rgb_img.header.frame_id = "lidar"; @@ -202,28 +219,24 @@ void SegmentAnythingModule::run_(QueryCache &qdata0, OutputCache &, namespace F = torch::nn::functional; + using namespace torch::indexing; + torch::Tensor average = torch::ones({1, 1, 3, 3}) / 9; diff = F::conv2d(diff.unsqueeze(0).unsqueeze(0), average); - const auto [smaller_diff, idx] = F::max_pool2d_with_indices(diff.squeeze(0), F::MaxPool2dFuncOptions(5)); - const auto [topk_vals, topk_idx] = smaller_diff.flatten().topk(config_->num_prompts); - - CLOG(DEBUG, "lidar.perspective") << "Max pooling " << smaller_diff.sizes() << " and " << idx.sizes(); - - diff = diff.squeeze(); - using namespace torch::indexing; + diff = diff.squeeze().squeeze(); std::vector prompts; - auto topk_idx_a = topk_idx.accessor(); - auto topk_val_a = topk_vals.accessor(); - auto idx_a = idx.accessor(); //shape (x, y, z, dtype=long) - for (size_t i = 0; i < topk_idx_a.size(0); i++) { - const long& small_idx = topk_idx_a[i]; + for (int i = 0; i < config_->num_prompts; ++i) { + const auto [max_val, idx2] = torch::max(diff.flatten(), 0); + + int idx = idx2.item(); - const long& idx = idx_a[0][small_idx / smaller_diff.size(2)][small_idx % smaller_diff.size(2)]; + diff.index_put_({Slice(idx / diff.size(1) - 3, idx / diff.size(1) + 4), + Slice(idx % diff.size(1) - 3, idx % diff.size(1) + 4) }, 0); int prompt[] = {idx % diff.size(1), idx / diff.size(1)}; @@ -233,8 +246,6 @@ void SegmentAnythingModule::run_(QueryCache &qdata0, OutputCache &, CLOG(DEBUG, "lidar.perspective") << "Prompt point (" << prompt[0] << ", " << prompt[1] << ") << on an interpolated live pixel. Skipping."; continue; } - - //Scale up to whole image prompt[0] = 4*prompt[0] + 2; prompt[1] = 4*prompt[1] + 2; @@ -244,18 +255,44 @@ void SegmentAnythingModule::run_(QueryCache &qdata0, OutputCache &, h_point = T_v_loc_c.matrix().cast() * h_point; double theta = atan2(h_point[1], h_point[0]); - CLOG(DEBUG, "lidar.perspective") << "Diff Tensor prompt (" << prompt[0] << ", " << prompt[1] << ") Theta: " << theta << " DIff norm " << topk_val_a[i]; + CLOG(DEBUG, "lidar.perspective") << "Diff Tensor prompt (" << prompt[0] << ", " << prompt[1] << ") Theta: " << theta << " DIff norm " << max_val; //Prompts are x, y rather than row, column //map_tensor.index({0, prompt[1], prompt[0]}).item().to() > 0 || - if (!((theta > 0.611 && theta < 0.96) || (theta > -0.96 && theta < -0.611)) && topk_val_a[i] > 10.){ + if (!((theta > 0.611 && theta < 0.96) || (theta > -0.96 && theta < -0.611))){ prompts.push_back(torch::from_blob(prompt, {2}, torch::kInt).to(device)); } else { CLOG(DEBUG, "lidar.perspective") << "Prompt point on an empty map pixel. Try again."; + continue; } + } + + // const auto [smaller_diff, idx] = F::max_pool2d_with_indices(diff.squeeze(0), F::MaxPool2dFuncOptions(5)); + // const auto [topk_vals, topk_idx] = smaller_diff.flatten().topk(config_->num_prompts); + + // CLOG(DEBUG, "lidar.perspective") << "Max pooling " << smaller_diff.sizes() << " and " << idx.sizes(); + + // diff = diff.squeeze(); + + + // auto topk_idx_a = topk_idx.accessor(); + // auto topk_val_a = topk_vals.accessor(); + // auto idx_a = idx.accessor(); //shape (x, y, z, dtype=long) + + // for (size_t i = 0; i < topk_idx_a.size(0); i++) { + // const long& small_idx = topk_idx_a[i]; + + // const long& idx = idx_a[0][small_idx / smaller_diff.size(2)][small_idx % smaller_diff.size(2)]; + + + + // //Scale up to whole image + + // } + torch::NoGradGuard no_grad; std::vector jit_inputs; @@ -399,7 +436,7 @@ void SegmentAnythingModule::run_(QueryCache &qdata0, OutputCache &, diff_img_msg.header.frame_id = "lidar"; //cv_rgb_img.header.stamp = qdata.stamp->header.stamp; diff_img_msg.encoding = "mono8"; - diff_img_msg.image = cv::Mat{diff.size(0), diff.size(1), CV_8UC1, diff.data_ptr()};; + diff_img_msg.image = hsv_scores[2];//cv::Mat{diff.size(0), diff.size(1), CV_8UC1, diff.data_ptr()};; diff_pub_->publish(*diff_img_msg.toImageMsg()); } From 9fa22541edf40a180be0b76cdf4c730449fda03a Mon Sep 17 00:00:00 2001 From: a-krawciw Date: Mon, 25 Mar 2024 17:08:36 -0400 Subject: [PATCH 70/77] Add corridor filter. --- ...ffline_ouster_warthog_lasersam.launch.yaml | 45 +++++++++ .../vtr_lidar/filters/corridor_filter.hpp | 32 +++++++ .../vtr_lidar/filters/perspective_image.hpp | 8 +- .../vtr_lidar/src/filters/corridor_filter.cpp | 63 +++++++++++++ .../planning/segmentanything_module.cpp | 92 +++++++++++++++---- 5 files changed, 216 insertions(+), 24 deletions(-) create mode 100644 launch/offline_ouster_warthog_lasersam.launch.yaml create mode 100644 main/src/vtr_lidar/include/vtr_lidar/filters/corridor_filter.hpp create mode 100644 main/src/vtr_lidar/src/filters/corridor_filter.cpp diff --git a/launch/offline_ouster_warthog_lasersam.launch.yaml b/launch/offline_ouster_warthog_lasersam.launch.yaml new file mode 100644 index 000000000..c5ec43b39 --- /dev/null +++ b/launch/offline_ouster_warthog_lasersam.launch.yaml @@ -0,0 +1,45 @@ +## Offline Lidar VTR3 with LaserSAM +session_name: vtr_offline_ouster_warthog + +environment: + FASTRTPS_DEFAULT_PROFILES_FILE: ${VTRSRC}/config/vtr_rtps.xml + #ROS_DOMAIN_ID: "131" # set this to a unique number when multiple ROS2 dependent system running on the same network + +start_directory: ${VTRTEMP} + +suppress_history: false + +windows: + - window_name: vtr_navigation_system + layout: main-horizontal + shell_command_before: + - source ${VTRSRC}/main/install/setup.bash + #- ros2 run vtr_gui setup_server --ros-args -r __ns:=/vtr + panes: + - > + ros2 launch vtr_navigation vtr.launch.py + base_params:=ouster_warthog_default.yaml + override_params:=ouster_warthog_lasersam.yaml + start_new_graph:=false + use_sim_time:=false + planner:="stationary" + model_dir:=${VTRROOT}/models + data_dir:=${VTRTEMP}/lidar/forest_sam_testing + + - window_name: vtr_gui + layout: main-horizontal + shell_command_before: + - source ${VTRSRC}/main/install/setup.bash + panes: + - ros2 run vtr_gui socket_client --ros-args -r __ns:=/vtr + - ros2 run vtr_gui socket_server --ros-args -r __ns:=/vtr + - ros2 run vtr_gui web_server --ros-args -r __ns:=/vtr + # - firefox --new-window "localhost:5200" # the webpage has to wait for everything above + + - window_name: rviz2 + layout: main-horizontal + shell_command_before: + - source ${VTRSRC}/main/install/setup.bash + panes: + - ros2 run rviz2 rviz2 -d ${VTRSRC}/rviz/lidar.rviz + diff --git a/main/src/vtr_lidar/include/vtr_lidar/filters/corridor_filter.hpp b/main/src/vtr_lidar/include/vtr_lidar/filters/corridor_filter.hpp new file mode 100644 index 000000000..d5869075a --- /dev/null +++ b/main/src/vtr_lidar/include/vtr_lidar/filters/corridor_filter.hpp @@ -0,0 +1,32 @@ +// Copyright 2024, 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 corridor_filter.hpp + * \author Alec Krawciw, Autonomous Space Robotics Lab (ASRL) + */ +#pragma once + +#include "vtr_lidar/data_types/point.hpp" +#include "math.h" +#include "vtr_tactic/types.hpp" + + +namespace vtr { +namespace lidar{ + +pcl::PointCloud filter_by_corridor(const pcl::PointCloud& point_cloud, long curr_sid, double path_length, const tactic::LocalizationChain& chain, double corridor_width, + const tactic::EdgeTransform &T_cam_w); +} // namespace lidar +} // vtr \ No newline at end of file diff --git a/main/src/vtr_lidar/include/vtr_lidar/filters/perspective_image.hpp b/main/src/vtr_lidar/include/vtr_lidar/filters/perspective_image.hpp index 9f6492aec..d83a14279 100644 --- a/main/src/vtr_lidar/include/vtr_lidar/filters/perspective_image.hpp +++ b/main/src/vtr_lidar/include/vtr_lidar/filters/perspective_image.hpp @@ -34,17 +34,17 @@ struct PerspectiveImageParams { int width; int height; - double f_u(){ + double f_u() const { return (double)(width) / 2.0 / tan(h_fov / 2); } - unsigned c_u() { return width / 2; } + unsigned c_u() const { return width / 2; } - double f_v(){ + double f_v() const { return (double)(height) / 2.0 / tan(v_fov / 2); } - unsigned c_v() { return height / 2; } + unsigned c_v() const { return height / 2; } }; //Greyscale image with intensity diff --git a/main/src/vtr_lidar/src/filters/corridor_filter.cpp b/main/src/vtr_lidar/src/filters/corridor_filter.cpp new file mode 100644 index 000000000..3d7b53886 --- /dev/null +++ b/main/src/vtr_lidar/src/filters/corridor_filter.cpp @@ -0,0 +1,63 @@ +// Copyright 2024, 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 corridor_filter.cpp + * \author Alec Krawciw, Autonomous Space Robotics Lab (ASRL) + */ +#include "vtr_lidar/filters/corridor_filter.hpp" + + + +namespace vtr { +namespace lidar { + +std::vector points_near_vertex(const pcl::PointCloud& point_cloud, Eigen::Vector4d path_point, double corridor_width) { + std::vector indices; + indices.reserve(point_cloud.size()); + for (size_t i = 0; i < point_cloud.size(); ++i) { + if (abs(path_point[0] - point_cloud[i].x) < corridor_width && + abs(path_point[2] - point_cloud[i].z) < 0.75) + indices.emplace_back(i); + } + return indices; +} + +pcl::PointCloud filter_by_corridor(const pcl::PointCloud& point_cloud, long curr_sid, double path_length, const tactic::LocalizationChain& chain, double corridor_width, + const tactic::EdgeTransform &T_cam_w) { + auto lock = chain.guard(); + // compute vertex lookahead + const auto distance = chain.dist(curr_sid); + std::set indices; + + // forwards + for (auto query_sid = curr_sid; + query_sid < chain.size() + && (chain.dist(query_sid) - distance) < path_length; + ++query_sid) { + const auto T_cam_query = T_cam_w * chain.pose(query_sid); + const auto p_cam_query = T_cam_query.matrix().block<4, 1>(0, 3); + CLOG(DEBUG, "lidar.perspective") << "Pose origin" << p_cam_query; + + auto idx_i = points_near_vertex(point_cloud, p_cam_query, corridor_width); + indices.insert(std::make_move_iterator(idx_i.begin()), + std::make_move_iterator(idx_i.end())); + } + return pcl::PointCloud(point_cloud, std::vector{std::make_move_iterator(indices.begin()), + std::make_move_iterator(indices.end())}); +} + + +} //lidar +} //vtr \ No newline at end of file diff --git a/main/src/vtr_lidar/src/modules/planning/segmentanything_module.cpp b/main/src/vtr_lidar/src/modules/planning/segmentanything_module.cpp index fe0a1717b..26692a54b 100644 --- a/main/src/vtr_lidar/src/modules/planning/segmentanything_module.cpp +++ b/main/src/vtr_lidar/src/modules/planning/segmentanything_module.cpp @@ -22,6 +22,7 @@ #include #include "cv_bridge/cv_bridge.h" #include +#include @@ -61,7 +62,7 @@ auto SegmentAnythingModule::Config::fromROS(const rclcpp::Node::SharedPtr &node, using BGR = typename cv::Scalar; -void SegmentAnythingModule::run_(QueryCache &qdata0, OutputCache &, +void SegmentAnythingModule::run_(QueryCache &qdata0, OutputCache &output, const Graph::Ptr &graph, const TaskExecutor::Ptr &) { auto &qdata = dynamic_cast(qdata0); @@ -92,6 +93,11 @@ void SegmentAnythingModule::run_(QueryCache &qdata0, OutputCache &, const auto &T_r_v_loc = *qdata.T_r_v_loc; const auto &T_v_m_loc = *qdata.T_v_m_loc; const auto &vid_loc = *qdata.vid_loc; + const auto &curr_sid = *qdata.sid_loc; + + auto &chain = *output.chain; + const auto T_w_curr = chain.pose(curr_sid); + auto map_vertex = graph->at(vid_loc); auto nn_map_scan = [&] { @@ -129,6 +135,7 @@ void SegmentAnythingModule::run_(QueryCache &qdata0, OutputCache &, const tactic::EdgeTransform T_c_s {T_c_s_temp}; const tactic::EdgeTransform T_v_loc_c = (T_c_s * T_s_r * T_r_v_loc).inverse(); + const auto T_cam_w = T_c_s * T_s_r * T_w_curr.inverse(); live_points_mat = T_c_s.matrix().cast() * live_points_mat; @@ -140,6 +147,9 @@ void SegmentAnythingModule::run_(QueryCache &qdata0, OutputCache &, cv::Mat live_rgb_img = cv::Mat::zeros(config_->perspective_params.height, config_->perspective_params.width, CV_8UC3); cv::Mat raw_rgb_img = cv::Mat::zeros(config_->perspective_params.height, config_->perspective_params.width, CV_8UC3); + raw_point_cloud = filter_by_corridor(raw_point_cloud, curr_sid, 20, chain, 2.0, T_cam_w); +//pcl::PointCloud filter_by_corridor(const pcl::PointCloud& point_cloud, long curr_sid, double path_length, const LocalizationChain &chain, double corridor_width, + // const tactic::EdgeTransform &T_cam_w) { generate_depth_image(raw_point_cloud, live_hsv_img, live_index_img, config_->perspective_params); cv::Mat raw_hsv_img = live_hsv_img.clone(); interpolate_hsv_image(live_hsv_img); @@ -161,7 +171,7 @@ void SegmentAnythingModule::run_(QueryCache &qdata0, OutputCache &, cv::Mat map_rgb_img = cv::Mat::zeros(config_->perspective_params.height, config_->perspective_params.width, CV_8UC3); cv::Mat similarity_diff; // = cv::Mat::empty(config_->perspective_params.height, config_->perspective_params.width, CV_32FC1); - + nn_map_point_cloud = filter_by_corridor(nn_map_point_cloud, curr_sid, 20, chain, 2.0, T_cam_w); // generate_depth_image(map_point_cloud, map_hsv_img, map_index_img, config_->perspective_params); generate_depth_image(nn_map_point_cloud, map_hsv_img, map_index_img, config_->perspective_params); interpolate_hsv_image(map_hsv_img); @@ -169,17 +179,52 @@ void SegmentAnythingModule::run_(QueryCache &qdata0, OutputCache &, cv::cvtColor(map_hsv_img, map_rgb_img, cv::COLOR_HSV2RGB); - auto val = cv::quality::QualitySSIM::compute(map_hsv_img, live_hsv_img, similarity_diff); - CLOG(DEBUG, "lidar.perspective") << "Sim: " << similarity_diff.type(); - similarity_diff = similarity_diff; - similarity_diff *= 255; - similarity_diff.convertTo(similarity_diff, CV_8UC3); + // { + // CLOG(DEBUG, "lidar.perspective") << chain.size(); + + // auto lock = chain.guard(); + // // compute vertex lookahead + // const auto distance = chain.dist(curr_sid); + // // forwards + // for (auto query_sid = curr_sid; + // query_sid < chain.size() + // && (chain.dist(query_sid) - distance) < 10; + // ++query_sid) { + // const auto T_cam_query = T_c_s * T_s_r * T_w_curr.inverse() * chain.pose(query_sid); + // const auto p_cam_query = T_cam_query.matrix().block<4, 1>(0, 3); + // // T_curr_query_vec.emplace_back(T_curr_query.matrix()); + // CLOG(DEBUG, "lidar.perspective") << "Pose origin" << p_cam_query; + + // int lu = (int)round(config_->perspective_params.f_u() * (p_cam_query[0] - 0.75) / p_cam_query[2]) + config_->perspective_params.c_u(); + // int lv = (int)round(config_->perspective_params.f_v() * (p_cam_query[1] - 0.2) / p_cam_query[2]) + config_->perspective_params.c_v(); - cv::Mat hsv_scores[3]; //destination array - cv::split(similarity_diff, hsv_scores);//split source + // int ru = (int)round(config_->perspective_params.f_u() * (p_cam_query[0] + 0.75) / p_cam_query[2]) + config_->perspective_params.c_u(); + // int rv = (int)round(config_->perspective_params.f_v() * (p_cam_query[1] + 3) / p_cam_query[2]) + config_->perspective_params.c_v(); - CLOG(DEBUG, "lidar.perspective") << "SSIM " << val; + // if ((chain.dist(query_sid) - distance) > 0.75) + // cv::rectangle(map_rgb_img, cv::Point{lu, lv}, cv::Point{ru, rv}, color_options[3], cv::FILLED); + + + // CLOG(DEBUG, "lidar.perspective") << "Recatngle " << cv::Point{lu, lv} << " " << cv::Point{ru, rv}; + + // // if (0 <= u && u < config_->perspective_params.width && 0 <= v && v < config_->perspective_params.height) { + // // cv::circle(map_rgb_img, cv::Point{u, v}, 12, color_options[color_options.size() - 1], 1); + // // } + // } + // } + + // auto val = cv::quality::QualitySSIM::compute(map_hsv_img, live_hsv_img, similarity_diff); + // CLOG(DEBUG, "lidar.perspective") << "Sim: " << similarity_diff.type(); + // similarity_diff = similarity_diff; + // similarity_diff *= 255; + // similarity_diff.convertTo(similarity_diff, CV_8UC3); + + // cv::Mat hsv_scores[3]; //destination array + // cv::split(similarity_diff, hsv_scores);//split source + + + // CLOG(DEBUG, "lidar.perspective") << "SSIM " << val; if (config_->visualize) { @@ -212,10 +257,11 @@ void SegmentAnythingModule::run_(QueryCache &qdata0, OutputCache &, torch::Tensor live_tensor = torch::from_blob(big_live.data, {big_live.rows, big_live.cols, big_live.channels()}, torch::kByte).to(torch::kFloat32).permute({2, 0, 1}).contiguous(); torch::Tensor map_tensor = torch::from_blob(big_map.data, {big_map.rows, big_map.cols, big_map.channels()}, torch::kByte).to(torch::kFloat32).permute({2, 0, 1}).contiguous(); - torch::Tensor small_live_tensor = torch::from_blob(raw_rgb_img.data, {raw_rgb_img.rows, raw_rgb_img.cols, raw_rgb_img.channels()}, torch::kByte).to(torch::kFloat32).permute({2, 0, 1}); + torch::Tensor small_live_tensor = torch::from_blob(live_rgb_img.data, {raw_rgb_img.rows, raw_rgb_img.cols, raw_rgb_img.channels()}, torch::kByte).to(torch::kFloat32).permute({2, 0, 1}); torch::Tensor small_map_tensor = torch::from_blob(map_rgb_img.data, {map_rgb_img.rows, map_rgb_img.cols, map_rgb_img.channels()}, torch::kByte).to(torch::kFloat32).permute({2, 0, 1}); torch::Tensor diff = small_live_tensor - small_map_tensor; - diff = diff.clamp(0, 255.0).norm(2, {0}); + diff = diff.norm(2, {0}); + //.clamp(0, 255.0) namespace F = torch::nn::functional; @@ -230,20 +276,19 @@ void SegmentAnythingModule::run_(QueryCache &qdata0, OutputCache &, std::vector prompts; - for (int i = 0; i < config_->num_prompts; ++i) { + for (int i = 0; prompts.size() < config_->num_prompts; ++i) { const auto [max_val, idx2] = torch::max(diff.flatten(), 0); int idx = idx2.item(); - - diff.index_put_({Slice(idx / diff.size(1) - 3, idx / diff.size(1) + 4), - Slice(idx % diff.size(1) - 3, idx % diff.size(1) + 4) }, 0); - int prompt[] = {idx % diff.size(1), idx / diff.size(1)}; auto& pc_idx = live_index_img.at(prompt[1], prompt[0]); if (pc_idx == 0) { - CLOG(DEBUG, "lidar.perspective") << "Prompt point (" << prompt[0] << ", " << prompt[1] << ") << on an interpolated live pixel. Skipping."; + // CLOG(DEBUG, "lidar.perspective") << "Prompt point (" << prompt[0] << ", " << prompt[1] << ") << on an interpolated live pixel. Skipping."; + + diff.index_put_({Slice(idx / diff.size(1), idx / diff.size(1) + 1), + Slice(idx % diff.size(1), idx % diff.size(1) + 1) }, 0); continue; } prompt[0] = 4*prompt[0] + 2; @@ -262,8 +307,15 @@ void SegmentAnythingModule::run_(QueryCache &qdata0, OutputCache &, //map_tensor.index({0, prompt[1], prompt[0]}).item().to() > 0 || if (!((theta > 0.611 && theta < 0.96) || (theta > -0.96 && theta < -0.611))){ prompts.push_back(torch::from_blob(prompt, {2}, torch::kInt).to(device)); + + + diff.index_put_({Slice(idx / diff.size(1) - 3, idx / diff.size(1) + 4), + Slice(idx % diff.size(1) - 3, idx % diff.size(1) + 4) }, 0); + } else { - CLOG(DEBUG, "lidar.perspective") << "Prompt point on an empty map pixel. Try again."; + // CLOG(DEBUG, "lidar.perspective") << "Prompt point on an empty map pixel. Try again."; + diff.index_put_({Slice(idx / diff.size(1), idx / diff.size(1) + 1), + Slice(idx % diff.size(1), idx % diff.size(1) + 1) }, 0); continue; } @@ -436,7 +488,7 @@ void SegmentAnythingModule::run_(QueryCache &qdata0, OutputCache &, diff_img_msg.header.frame_id = "lidar"; //cv_rgb_img.header.stamp = qdata.stamp->header.stamp; diff_img_msg.encoding = "mono8"; - diff_img_msg.image = hsv_scores[2];//cv::Mat{diff.size(0), diff.size(1), CV_8UC1, diff.data_ptr()};; + diff_img_msg.image = cv::Mat{diff.size(0), diff.size(1), CV_8UC1, diff.data_ptr()};; diff_pub_->publish(*diff_img_msg.toImageMsg()); } From 97b3c7ee9f70d83d0d55d1bf52fcba4f1b540beb Mon Sep 17 00:00:00 2001 From: a-krawciw Date: Tue, 9 Apr 2024 09:17:03 -0400 Subject: [PATCH 71/77] Fix localization bug in path filtering. --- config/ouster_warthog_lasersam.yaml | 12 ++++++------ main/src/vtr_lidar/src/filters/corridor_filter.cpp | 1 - .../src/modules/planning/segmentanything_module.cpp | 9 ++++----- 3 files changed, 10 insertions(+), 12 deletions(-) diff --git a/config/ouster_warthog_lasersam.yaml b/config/ouster_warthog_lasersam.yaml index e17817d3f..9e71f6aaf 100644 --- a/config/ouster_warthog_lasersam.yaml +++ b/config/ouster_warthog_lasersam.yaml @@ -44,13 +44,13 @@ iou_threshold: 1.0 visualize: true costmap_inflation: - type: lidar.costmap_queue - influence_distance: 0.5 # was 0.5 Jordy # Note that the total distance where grid cells have values > 0 is min dist + influence dist, not influence dist! - minimum_distance: 0.8 # was 0.3 Jordy - - # cost map - costmap_history_size: 5 # Keep between 3 and 30, used for temporal filtering + type: lidar.costmap_blindspot + influence_distance: 0.3 # was 0.5 Jordy # Note that the total distance where grid cells have values > 0 is min dist + influence dist, not influence dist! + minimum_distance: 1.0 # was 0.3 Jordy resolution: 0.25 + costmap_history_size: 15 + blind_spot_radius: 1.5 + lifetime: 1.5 size_x: 16.0 size_y: 8.0 visualize: true diff --git a/main/src/vtr_lidar/src/filters/corridor_filter.cpp b/main/src/vtr_lidar/src/filters/corridor_filter.cpp index 3d7b53886..cbc0c29af 100644 --- a/main/src/vtr_lidar/src/filters/corridor_filter.cpp +++ b/main/src/vtr_lidar/src/filters/corridor_filter.cpp @@ -48,7 +48,6 @@ pcl::PointCloud filter_by_corridor(const pcl::PointCloud(0, 3); - CLOG(DEBUG, "lidar.perspective") << "Pose origin" << p_cam_query; auto idx_i = points_near_vertex(point_cloud, p_cam_query, corridor_width); indices.insert(std::make_move_iterator(idx_i.begin()), diff --git a/main/src/vtr_lidar/src/modules/planning/segmentanything_module.cpp b/main/src/vtr_lidar/src/modules/planning/segmentanything_module.cpp index 26692a54b..185167eb2 100644 --- a/main/src/vtr_lidar/src/modules/planning/segmentanything_module.cpp +++ b/main/src/vtr_lidar/src/modules/planning/segmentanything_module.cpp @@ -135,7 +135,7 @@ void SegmentAnythingModule::run_(QueryCache &qdata0, OutputCache &output, const tactic::EdgeTransform T_c_s {T_c_s_temp}; const tactic::EdgeTransform T_v_loc_c = (T_c_s * T_s_r * T_r_v_loc).inverse(); - const auto T_cam_w = T_c_s * T_s_r * T_w_curr.inverse(); + const auto T_cam_w = (T_w_curr * T_v_loc_c).inverse(); live_points_mat = T_c_s.matrix().cast() * live_points_mat; @@ -148,9 +148,8 @@ void SegmentAnythingModule::run_(QueryCache &qdata0, OutputCache &output, cv::Mat raw_rgb_img = cv::Mat::zeros(config_->perspective_params.height, config_->perspective_params.width, CV_8UC3); raw_point_cloud = filter_by_corridor(raw_point_cloud, curr_sid, 20, chain, 2.0, T_cam_w); -//pcl::PointCloud filter_by_corridor(const pcl::PointCloud& point_cloud, long curr_sid, double path_length, const LocalizationChain &chain, double corridor_width, - // const tactic::EdgeTransform &T_cam_w) { generate_depth_image(raw_point_cloud, live_hsv_img, live_index_img, config_->perspective_params); + cv::Mat raw_hsv_img = live_hsv_img.clone(); interpolate_hsv_image(live_hsv_img); @@ -275,8 +274,8 @@ void SegmentAnythingModule::run_(QueryCache &qdata0, OutputCache &output, std::vector prompts; - - for (int i = 0; prompts.size() < config_->num_prompts; ++i) { + static const int MAX_ITERS = 20; + for (int i = 0; prompts.size() < config_->num_prompts && i < MAX_ITERS; ++i) { const auto [max_val, idx2] = torch::max(diff.flatten(), 0); int idx = idx2.item(); From 98cf4f18b97e9a0c959e687e8f7b03c72d8b094a Mon Sep 17 00:00:00 2001 From: a-krawciw Date: Wed, 17 Apr 2024 17:10:08 -0400 Subject: [PATCH 72/77] DEC video state. Add temporal distance checks. --- config/ouster_warthog_default.yaml | 2 +- config/ouster_warthog_lasersam.yaml | 12 +++--- main/src/deps/lgmath | 2 +- .../planning/queued_inflation_module.hpp | 2 + .../planning/queued_inflation_module.cpp | 38 +++++++++++++++++++ .../planning/segmentanything_module.cpp | 4 +- rviz/lidar.rviz | 36 +++++++++--------- rviz/ouster.rviz | 27 ++++--------- 8 files changed, 78 insertions(+), 45 deletions(-) diff --git a/config/ouster_warthog_default.yaml b/config/ouster_warthog_default.yaml index 5fba01c59..9a3139fa6 100644 --- a/config/ouster_warthog_default.yaml +++ b/config/ouster_warthog_default.yaml @@ -348,7 +348,7 @@ horizon_step_size: 0.3 forward_vel: 1.0 max_lin_vel: 1.5 - max_ang_vel: 1.25 + max_ang_vel: 0.85 robot_linear_velocity_scale: 1.0 # Used to scale the output twist linear velocity messages by some constant factor to compensate internal low level control miscalibration robot_angular_velocity_scale: 1.0 # Used to scale the output twist angular velocity messages by some constant factor to compensate internal low level control miscalibration diff --git a/config/ouster_warthog_lasersam.yaml b/config/ouster_warthog_lasersam.yaml index 9e71f6aaf..9869c71b0 100644 --- a/config/ouster_warthog_lasersam.yaml +++ b/config/ouster_warthog_lasersam.yaml @@ -38,18 +38,20 @@ h_fov: 1.5707 v_fov: 0.7853 min_range: 0.5 - max_range: 50.0 + max_range: 35.0 num_prompts: 5 smooth_size: 40 iou_threshold: 1.0 visualize: true costmap_inflation: - type: lidar.costmap_blindspot - influence_distance: 0.3 # was 0.5 Jordy # Note that the total distance where grid cells have values > 0 is min dist + influence dist, not influence dist! + type: lidar.costmap_queue + influence_distance: 0.8 # was 0.5 Jordy # Note that the total distance where grid cells have values > 0 is min dist + influence dist, not influence dist! minimum_distance: 1.0 # was 0.3 Jordy resolution: 0.25 - costmap_history_size: 15 - blind_spot_radius: 1.5 + costmap_history_size: 6 + neighbourhood: 5 + radius_filter: 0.5 + blind_spot_radius: 1.25 lifetime: 1.5 size_x: 16.0 size_y: 8.0 diff --git a/main/src/deps/lgmath b/main/src/deps/lgmath index 964531066..3fb74d0ce 160000 --- a/main/src/deps/lgmath +++ b/main/src/deps/lgmath @@ -1 +1 @@ -Subproject commit 9645310665300755bd162f1f95c63796388d3e86 +Subproject commit 3fb74d0ce10898f3d51efcfe77485851d4ff6672 diff --git a/main/src/vtr_lidar/include/vtr_lidar/modules/planning/queued_inflation_module.hpp b/main/src/vtr_lidar/include/vtr_lidar/modules/planning/queued_inflation_module.hpp index ac04d6265..60e99158f 100644 --- a/main/src/vtr_lidar/include/vtr_lidar/modules/planning/queued_inflation_module.hpp +++ b/main/src/vtr_lidar/include/vtr_lidar/modules/planning/queued_inflation_module.hpp @@ -47,6 +47,8 @@ class QueuedCostmapModule : public CostmapInflationModule { // cost map unsigned int costmap_history_size = 10; + double radius_filter = 1.0; + int neighbourhood = 10; static ConstPtr fromROS(const rclcpp::Node::SharedPtr &node, const std::string ¶m_prefix); diff --git a/main/src/vtr_lidar/src/modules/planning/queued_inflation_module.cpp b/main/src/vtr_lidar/src/modules/planning/queued_inflation_module.cpp index 9e549792c..b3221c986 100644 --- a/main/src/vtr_lidar/src/modules/planning/queued_inflation_module.cpp +++ b/main/src/vtr_lidar/src/modules/planning/queued_inflation_module.cpp @@ -23,6 +23,9 @@ #include "vtr_lidar/data_types/costmap.hpp" #include "vtr_lidar/filters/voxel_downsample.hpp" +#include "vtr_lidar/utils/nanoflann_utils.hpp" + + namespace vtr { namespace lidar { @@ -43,6 +46,8 @@ auto QueuedCostmapModule::Config::fromROS( int hist_size = node->declare_parameter(param_prefix + ".costmap_history_size", config->costmap_history_size); config->costmap_history_size = (unsigned) hist_size; + config->radius_filter = node->declare_parameter(param_prefix + ".radius_filter", config->radius_filter); + config->neighbourhood = node->declare_parameter(param_prefix + ".neighbourhood", config->neighbourhood); // clang-format on return config; } @@ -97,12 +102,45 @@ QueuedCostmapModule::VtrPointCloud QueuedCostmapModule::assemble_pointcloud(tact aligned_point_cloud_copy = T_v_loc_v_detect.matrix().cast() * aligned_point_cloud; aligned_norms_copy = T_v_loc_v_detect.matrix().cast() * aligned_norms; + for (size_t i = 0; i < point_cloud_copy.size(); ++i){ + point_cloud_copy[i].flex23 = p_loc_sid; + } + concat_pc += point_cloud_copy; CLOG(DEBUG, "lidar.obstacle_inflation") << "Point cloud of size " << point_cloud.size() << " is connected to sid: " << p_loc_sid; } + NanoFLANNAdapter adapter(concat_pc); + KDTreeSearchParams search_params; + KDTreeParams tree_params(10); + auto kdtree = std::make_unique>(3, adapter, tree_params); + kdtree->buildIndex(); + + std::vector radii_indices; + radii_indices.reserve(concat_pc.size()); + + const auto sq_search_radius = config_->radius_filter * config_->radius_filter; + for (size_t i = 0; i < concat_pc.size(); i++) { + // radius search of the closest point + std::vector dists; + std::vector indices; + NanoFLANNRadiusResultSet result(sq_search_radius, dists, indices); + kdtree->radiusSearchCustomCallback(concat_pc[i].data, result, search_params); + + int other_points_size = 0; + for (auto &idx : indices) { + if (concat_pc[idx].flex23 != concat_pc[i].flex23) + ++other_points_size; + } + // filter based on neighbors in map + if (other_points_size > config_->neighbourhood) + radii_indices.push_back(i); + } + + concat_pc = pcl::PointCloud(concat_pc, radii_indices); + /// publish the transformed pointcloud if (config_->visualize) { diff --git a/main/src/vtr_lidar/src/modules/planning/segmentanything_module.cpp b/main/src/vtr_lidar/src/modules/planning/segmentanything_module.cpp index 185167eb2..bb6535dfd 100644 --- a/main/src/vtr_lidar/src/modules/planning/segmentanything_module.cpp +++ b/main/src/vtr_lidar/src/modules/planning/segmentanything_module.cpp @@ -161,7 +161,7 @@ void SegmentAnythingModule::run_(QueryCache &qdata0, OutputCache &output, live_cv_rgb_img.header.frame_id = "lidar"; //cv_rgb_img.header.stamp = qdata.stamp->header.stamp; live_cv_rgb_img.encoding = "rgb8"; - live_cv_rgb_img.image = raw_rgb_img; + live_cv_rgb_img.image = live_rgb_img; live_img_pub_->publish(*live_cv_rgb_img.toImageMsg()); } @@ -462,6 +462,8 @@ void SegmentAnythingModule::run_(QueryCache &qdata0, OutputCache &output, qdata.changed_points.emplace(*obstacle_point_cloud); } } else { + mask_to_pointcloud(cv::Mat::zeros(repeat_masks.size(1), repeat_masks.size(2), CV_8UC1), live_index_img, raw_point_cloud, 2); + qdata.changed_points.emplace(pcl::PointCloud()); } diff --git a/rviz/lidar.rviz b/rviz/lidar.rviz index c3c7f3266..2d2159418 100644 --- a/rviz/lidar.rviz +++ b/rviz/lidar.rviz @@ -7,12 +7,12 @@ Panels: - /Global Options1 - /Status1 - /Image1 - - /Image2 - /Image3 - /Image4 - /PointCloud21 + - /Image5 Splitter Ratio: 0.5 - Tree Height: 140 + Tree Height: 254 - Class: rviz_common/Selection Name: Selection - Class: rviz_common/Tool Properties @@ -67,7 +67,7 @@ Visualization Manager: Value: /vtr/live_range_coloured Value: true - Class: rviz_default_plugins/Image - Enabled: true + Enabled: false Max Value: 1 Median window: 5 Min Value: 0 @@ -79,7 +79,7 @@ Visualization Manager: History Policy: Keep Last Reliability Policy: Reliable Value: /vtr/detection_mask - Value: true + Value: false - Class: rviz_default_plugins/Image Enabled: true Max Value: 1 @@ -95,7 +95,7 @@ Visualization Manager: Value: /vtr/map_range_coloured Value: true - Class: rviz_default_plugins/Image - Enabled: true + Enabled: false Max Value: 1 Median window: 5 Min Value: 0 @@ -107,9 +107,9 @@ Visualization Manager: History Policy: Keep Last Reliability Policy: Reliable Value: /vtr/background_mask - Value: true + Value: false - Alpha: 1 - Autocompute Intensity Bounds: true + Autocompute Intensity Bounds: false Autocompute Value Bounds: Max Value: 10 Min Value: -10 @@ -130,7 +130,7 @@ Visualization Manager: Position Transformer: XYZ Selectable: true Size (Pixels): 3 - Size (m): 0.029999999329447746 + Size (m): 0.05000000074505806 Style: Flat Squares Topic: Depth: 5 @@ -143,7 +143,7 @@ Visualization Manager: Use rainbow: true Value: true - Class: rviz_default_plugins/Image - Enabled: true + Enabled: false Max Value: 1 Median window: 5 Min Value: 0 @@ -155,7 +155,7 @@ Visualization Manager: History Policy: Keep Last Reliability Policy: Reliable Value: /vtr/normed_diff - Value: true + Value: false Enabled: true Global Options: Background Color: 48; 48; 48 @@ -202,16 +202,16 @@ Visualization Manager: Views: Current: Class: rviz_default_plugins/Orbit - Distance: 18.88730812072754 + Distance: 2.5980148315429688 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false Focal Point: - X: 0 - Y: 0 - Z: 0 + X: 0.0058396183885633945 + Y: 0.17026743292808533 + Z: -0.0027216693852096796 Focal Shape Fixed Size: true Focal Shape Size: 0.05000000074505806 Invert Z Axis: false @@ -220,17 +220,17 @@ Visualization Manager: Pitch: 1.5697963237762451 Target Frame: Value: Orbit (rviz) - Yaw: 1.62860107421875 + Yaw: 1.6086010932922363 Saved: ~ Window Geometry: Displays: collapsed: false Height: 1536 Hide Left Dock: false - Hide Right Dock: false + Hide Right Dock: true Image: collapsed: false - QMainWindow State: 000000ff00000000fd00000004000000000000034400000566fc020000000dfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b000000c7000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d0061006700650100000108000001020000002800fffffffb0000000a0049006d0061006700650100000210000000e90000002800fffffffb0000000a0049006d00610067006501000002ff000000d60000002800fffffffb0000000a0049006d00610067006501000003db000000e80000002800fffffffb0000000a0049006d00610067006501000004c9000000d80000002800ffffff000000010000010f00000566fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003b00000566000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000009b20000003efc0100000002fb0000000800540069006d00650100000000000009b20000025300fffffffb0000000800540069006d00650100000000000004500000000000000000000005530000056600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd00000004000000000000034400000450fc020000000cfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d006100670065010000003b000001c70000002800fffffffb0000000a0049006d006100670065000000003b000002820000002800fffffffb0000000a0049006d0061006700650100000208000002830000002800fffffffb0000000a0049006d00610067006500000003610000012a0000002800fffffffb0000000a0049006d0061006700650000000318000001730000002800ffffff000000010000010f00000566fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003b00000566000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000009b200000154fc0100000002fc00000000000009b20000025300fffffffa000000000200000002fb000000100044006900730070006c0061007900730100000000ffffffff000000c700fffffffb0000000800540069006d006501000005a70000003e0000003700fffffffb0000000800540069006d00650100000000000004500000000000000000000006680000045000000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Time: @@ -238,7 +238,7 @@ Window Geometry: Tool Properties: collapsed: false Views: - collapsed: false + collapsed: true Width: 2482 X: 78 Y: 27 diff --git a/rviz/ouster.rviz b/rviz/ouster.rviz index 9594ed340..e2ff345b3 100644 --- a/rviz/ouster.rviz +++ b/rviz/ouster.rviz @@ -106,8 +106,6 @@ Visualization Manager: Value: true os_sensor_baseplate: Value: true - planning frame: - Value: true rear_left_wheel_link: Value: true rear_right_wheel_link: @@ -120,10 +118,6 @@ Visualization Manager: Value: true robot: Value: true - robot planning: - Value: true - robot planning (extrapolated) mpc: - Value: true sensor_tower: Value: true top_chassis_link: @@ -145,11 +139,6 @@ Visualization Manager: robot: lidar: {} - planning frame: - robot planning: - {} - robot planning (extrapolated) mpc: - {} world (offset): loc vertex frame (offset): {} @@ -369,7 +358,7 @@ Visualization Manager: Channel Name: flex23 Class: rviz_default_plugins/PointCloud2 Color: 255; 255; 0 - Color Transformer: Intensity + Color Transformer: FlatColor Decay Time: 0 Enabled: true Invert Rainbow: false @@ -691,25 +680,25 @@ Visualization Manager: Views: Current: Class: rviz_default_plugins/Orbit - Distance: 41.55506134033203 + Distance: 27.497325897216797 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false Focal Point: - X: 12.343294143676758 - Y: -3.8882362842559814 - Z: 0.2389792650938034 + X: 11.865821838378906 + Y: -0.894321084022522 + Z: -0.5033813714981079 Focal Shape Fixed Size: false Focal Shape Size: 0.05000000074505806 Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: 0.7497973442077637 + Pitch: 0.4447973966598511 Target Frame: robot Value: Orbit (rviz_default_plugins) - Yaw: 3.326280117034912 + Yaw: 3.4912807941436768 Saved: - Class: rviz_default_plugins/Orbit Distance: 111.39698028564453 @@ -917,7 +906,7 @@ Window Geometry: Height: 1536 Hide Left Dock: false Hide Right Dock: true - QMainWindow State: 000000ff00000000fd000000040000000000000281000005aafc020000000cfc0000003d000001a20000000000fffffffaffffffff0100000002fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730000000000ffffffff0000008c00fffffffb0000001200530065006c0065006300740069006f006e0000000000000002530000006c00fffffffb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b000005aa000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d0061006700650100000357000000160000000000000000fb0000000a0049006d0061006700650100000373000000160000000000000000fb0000000a0049006d006100670065010000038f000000160000000000000000fb0000000a0049006d00610067006501000003ab000000160000000000000000fb0000000a0049006d00610067006501000003c700000016000000000000000000000001000001c0000003bdfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003b000003bd000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000005000000006efc0100000002fb0000000800540069006d00650000000000000005000000025300fffffffb0000000800540069006d006501000000000000045000000000000000000000072b000005aa00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd000000040000000000000281000005aafc020000000cfc0000003d000001a20000000000fffffffaffffffff0100000002fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730000000000ffffffff0000008c00fffffffb0000001200530065006c0065006300740069006f006e0000000000000002530000006c00fffffffb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b000005aa000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d006100670065010000055f000000160000000000000000fb0000000a0049006d006100670065010000057b000000160000000000000000fb0000000a0049006d0061006700650100000597000000160000000000000000fb0000000a0049006d00610067006501000005b3000000160000000000000000fb0000000a0049006d00610067006501000005cf00000016000000000000000000000001000001c0000003bdfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003b000003bd000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000005000000006efc0100000002fb0000000800540069006d00650000000000000005000000025300fffffffb0000000800540069006d006501000000000000045000000000000000000000072b000005aa00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Time: From 7b46fa48cfff406ddb749ce251225eec306b2ade Mon Sep 17 00:00:00 2001 From: a-krawciw Date: Mon, 22 Apr 2024 14:54:52 -0400 Subject: [PATCH 73/77] Add testing infrastructure --- .../include/vtr_lidar/modules/modules.hpp | 1 + .../planning/segmentanything_module.hpp | 1 + .../testing/change_detection_outputs.hpp | 68 ++++++++++ .../planning/change_detection_module_v3.cpp | 4 +- .../planning/queued_inflation_module.cpp | 5 +- .../planning/segmentanything_module.cpp | 24 +++- .../testing/change_detection_outputs.cpp | 119 ++++++++++++++++++ .../vtr_torch/src/modules/torch_module.cpp | 7 +- 8 files changed, 222 insertions(+), 7 deletions(-) create mode 100644 main/src/vtr_lidar/include/vtr_lidar/modules/testing/change_detection_outputs.hpp create mode 100644 main/src/vtr_lidar/src/modules/testing/change_detection_outputs.cpp diff --git a/main/src/vtr_lidar/include/vtr_lidar/modules/modules.hpp b/main/src/vtr_lidar/include/vtr_lidar/modules/modules.hpp index 0126264ab..4793e1f4f 100644 --- a/main/src/vtr_lidar/include/vtr_lidar/modules/modules.hpp +++ b/main/src/vtr_lidar/include/vtr_lidar/modules/modules.hpp @@ -55,3 +55,4 @@ #include "vtr_lidar/modules/planning/blindspot_inflation_module.hpp" #include "vtr_lidar/modules/planning/queued_inflation_module.hpp" +#include "vtr_lidar/modules/testing/change_detection_outputs.hpp" diff --git a/main/src/vtr_lidar/include/vtr_lidar/modules/planning/segmentanything_module.hpp b/main/src/vtr_lidar/include/vtr_lidar/modules/planning/segmentanything_module.hpp index e9f18b67a..ba2d7f7a1 100644 --- a/main/src/vtr_lidar/include/vtr_lidar/modules/planning/segmentanything_module.hpp +++ b/main/src/vtr_lidar/include/vtr_lidar/modules/planning/segmentanything_module.hpp @@ -56,6 +56,7 @@ class SegmentAnythingModule : public nn::TorchModule { float iou_thresh = 0.5; int num_prompts = 4; int smooth_size = 10; + float corridor_width = 2.0; static ConstPtr fromROS(const rclcpp::Node::SharedPtr &node, diff --git a/main/src/vtr_lidar/include/vtr_lidar/modules/testing/change_detection_outputs.hpp b/main/src/vtr_lidar/include/vtr_lidar/modules/testing/change_detection_outputs.hpp new file mode 100644 index 000000000..be853a8b4 --- /dev/null +++ b/main/src/vtr_lidar/include/vtr_lidar/modules/testing/change_detection_outputs.hpp @@ -0,0 +1,68 @@ +// Copyright 2024, 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 change_detection_outputs.hpp + * \author Alec Krawciw, Autonomous Space Robotics Lab (ASRL) + */ +#pragma once + +#include "vtr_lidar/cache.hpp" +#include "vtr_tactic/modules/base_module.hpp" +#include "vtr_tactic/task_queue.hpp" + +#include "vtr_lidar/filters/perspective_image.hpp" + + +namespace vtr { +namespace lidar { + +/** \brief Preprocesses raw pointcloud points and computes normals. */ +class CDTestModule : public tactic::BaseModule { + public: + /** \brief Static module identifier. */ + static constexpr auto static_name = "lidar.cd_testing"; + + /** \brief Config parameters. */ + struct Config : public BaseModule::Config { + PTR_TYPEDEFS(Config); + + bool save_point_cloud = true; + std::string suffix = ""; + + PerspectiveImageParams perspective_params; + + + static ConstPtr fromROS(const rclcpp::Node::SharedPtr &node, + const std::string ¶m_prefix); + }; + + CDTestModule( + const Config::ConstPtr &config, + const std::shared_ptr &module_factory = nullptr, + const std::string &name = static_name) + : tactic::BaseModule{module_factory, name}, config_(config) {} + + private: + void run_(tactic::QueryCache &qdata, tactic::OutputCache &output, + const tactic::Graph::Ptr &graph, + const tactic::TaskExecutor::Ptr &executor) override; + + Config::ConstPtr config_; + + VTR_REGISTER_MODULE_DEC_TYPE(CDTestModule); +}; + +} // namespace lidar +} // namespace vtr \ No newline at end of file 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 170e74d15..85ed4e566 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 @@ -111,7 +111,7 @@ void ChangeDetectionModuleV3::run_(QueryCache &qdata0, OutputCache &output0, const auto &vid_loc = *qdata.vid_loc; const auto &sid_loc = *qdata.sid_loc; const auto &T_r_v_loc = *qdata.T_r_v_loc; - const auto &points = *qdata.undistorted_point_cloud; + const auto &points = *qdata.raw_point_cloud; const auto &submap_loc = *qdata.submap_loc; const auto &map_point_cloud = submap_loc.point_cloud(); const auto &T_v_m_loc = *qdata.T_v_m_loc; @@ -133,7 +133,7 @@ void ChangeDetectionModuleV3::run_(QueryCache &qdata0, OutputCache &output0, CLOG(WARNING, "lidar.change_detection") << "No points were valid to detect changes"; return; } - voxelDownsample(query_points, 0.2); + //voxelDownsample(query_points, 0.2); // Eigen matrix of original data (only shallow copy of ref clouds) const auto query_mat = query_points.getMatrixXfMap(4, PointWithInfo::size(), PointWithInfo::cartesian_offset()); diff --git a/main/src/vtr_lidar/src/modules/planning/queued_inflation_module.cpp b/main/src/vtr_lidar/src/modules/planning/queued_inflation_module.cpp index b3221c986..4d6c1ea1e 100644 --- a/main/src/vtr_lidar/src/modules/planning/queued_inflation_module.cpp +++ b/main/src/vtr_lidar/src/modules/planning/queued_inflation_module.cpp @@ -84,6 +84,8 @@ QueuedCostmapModule::VtrPointCloud QueuedCostmapModule::assemble_pointcloud(tact if (detected_history.size() > config_->costmap_history_size) detected_history.pop_back(); + unsigned idx = 0; + for (auto &pair : detected_history) { auto &p_loc_sid = pair.first; auto &point_cloud = pair.second; @@ -103,10 +105,11 @@ QueuedCostmapModule::VtrPointCloud QueuedCostmapModule::assemble_pointcloud(tact aligned_norms_copy = T_v_loc_v_detect.matrix().cast() * aligned_norms; for (size_t i = 0; i < point_cloud_copy.size(); ++i){ - point_cloud_copy[i].flex23 = p_loc_sid; + point_cloud_copy[i].flex23 = idx; } concat_pc += point_cloud_copy; + ++idx; CLOG(DEBUG, "lidar.obstacle_inflation") << "Point cloud of size " << point_cloud.size() << " is connected to sid: " << p_loc_sid; diff --git a/main/src/vtr_lidar/src/modules/planning/segmentanything_module.cpp b/main/src/vtr_lidar/src/modules/planning/segmentanything_module.cpp index bb6535dfd..e24a742aa 100644 --- a/main/src/vtr_lidar/src/modules/planning/segmentanything_module.cpp +++ b/main/src/vtr_lidar/src/modules/planning/segmentanything_module.cpp @@ -21,7 +21,7 @@ #include #include #include "cv_bridge/cv_bridge.h" -#include +// #include #include @@ -31,6 +31,8 @@ namespace vtr { namespace lidar { using namespace tactic; +using Image_LockMsg = storage::LockableMessage; + auto SegmentAnythingModule::Config::fromROS(const rclcpp::Node::SharedPtr &node, const std::string ¶m_prefix) @@ -52,6 +54,7 @@ auto SegmentAnythingModule::Config::fromROS(const rclcpp::Node::SharedPtr &node, config->iou_thresh = node->declare_parameter(param_prefix + ".iou_threshold", config->iou_thresh); config->num_prompts = node->declare_parameter(param_prefix + ".num_prompts", config->num_prompts); config->smooth_size = node->declare_parameter(param_prefix + ".smooth_size", config->smooth_size); + config->corridor_width = node->declare_parameter(param_prefix + ".corridor_width", config->corridor_width); // clang-format off config->visualize = node->declare_parameter(param_prefix + ".visualize", config->visualize); @@ -94,6 +97,9 @@ void SegmentAnythingModule::run_(QueryCache &qdata0, OutputCache &output, const auto &T_v_m_loc = *qdata.T_v_m_loc; const auto &vid_loc = *qdata.vid_loc; const auto &curr_sid = *qdata.sid_loc; + + auto vertex = graph->at(*qdata.vid_odo); + auto &chain = *output.chain; const auto T_w_curr = chain.pose(curr_sid); @@ -108,6 +114,7 @@ void SegmentAnythingModule::run_(QueryCache &qdata0, OutputCache &output, auto locked_msg = locked_nn_pc_msg->sharedLocked(); return locked_msg.get().getDataPtr(); } + CLOG(WARNING, "lidar.perspective") << "Could not load raw view from teach"; return std::make_shared>(); }(); @@ -147,7 +154,7 @@ void SegmentAnythingModule::run_(QueryCache &qdata0, OutputCache &output, cv::Mat live_rgb_img = cv::Mat::zeros(config_->perspective_params.height, config_->perspective_params.width, CV_8UC3); cv::Mat raw_rgb_img = cv::Mat::zeros(config_->perspective_params.height, config_->perspective_params.width, CV_8UC3); - raw_point_cloud = filter_by_corridor(raw_point_cloud, curr_sid, 20, chain, 2.0, T_cam_w); + raw_point_cloud = filter_by_corridor(raw_point_cloud, curr_sid, 20, chain, config_->corridor_width, T_cam_w); generate_depth_image(raw_point_cloud, live_hsv_img, live_index_img, config_->perspective_params); cv::Mat raw_hsv_img = live_hsv_img.clone(); @@ -163,6 +170,11 @@ void SegmentAnythingModule::run_(QueryCache &qdata0, OutputCache &output, live_cv_rgb_img.encoding = "rgb8"; live_cv_rgb_img.image = live_rgb_img; live_img_pub_->publish(*live_cv_rgb_img.toImageMsg()); + if (*qdata.vertex_test_result == VertexTestResult::CREATE_VERTEX) { + auto locked_image_msg = + std::make_shared(live_cv_rgb_img.toImageMsg(), *qdata.stamp); + vertex->insert("live_depth_image", "sensor_msgs/msg/Image", locked_image_msg); + } } cv::Mat map_index_img = cv::Mat::zeros(config_->perspective_params.height, config_->perspective_params.width, CV_32S); @@ -170,7 +182,7 @@ void SegmentAnythingModule::run_(QueryCache &qdata0, OutputCache &output, cv::Mat map_rgb_img = cv::Mat::zeros(config_->perspective_params.height, config_->perspective_params.width, CV_8UC3); cv::Mat similarity_diff; // = cv::Mat::empty(config_->perspective_params.height, config_->perspective_params.width, CV_32FC1); - nn_map_point_cloud = filter_by_corridor(nn_map_point_cloud, curr_sid, 20, chain, 2.0, T_cam_w); + nn_map_point_cloud = filter_by_corridor(nn_map_point_cloud, curr_sid, 20, chain, config_->corridor_width, T_cam_w); // generate_depth_image(map_point_cloud, map_hsv_img, map_index_img, config_->perspective_params); generate_depth_image(nn_map_point_cloud, map_hsv_img, map_index_img, config_->perspective_params); interpolate_hsv_image(map_hsv_img); @@ -233,6 +245,12 @@ void SegmentAnythingModule::run_(QueryCache &qdata0, OutputCache &output, map_cv_rgb_img.encoding = "rgb8"; map_cv_rgb_img.image = map_rgb_img; map_img_pub_->publish(*map_cv_rgb_img.toImageMsg()); + + if (*qdata.vertex_test_result == VertexTestResult::CREATE_VERTEX) { + auto locked_image_msg = + std::make_shared(map_cv_rgb_img.toImageMsg(), *qdata.stamp); + vertex->insert("map_depth_image", "sensor_msgs/msg/Image", locked_image_msg); + } } CLOG(DEBUG, "lidar.perspective") << "Received images! "; diff --git a/main/src/vtr_lidar/src/modules/testing/change_detection_outputs.cpp b/main/src/vtr_lidar/src/modules/testing/change_detection_outputs.cpp new file mode 100644 index 000000000..f1a735048 --- /dev/null +++ b/main/src/vtr_lidar/src/modules/testing/change_detection_outputs.cpp @@ -0,0 +1,119 @@ +// Copyright 2024, 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 change_detection_outputs.cpp + * \author Alec Krawciw, Autonomous Space Robotics Lab (ASRL) + */ +#include "vtr_lidar/modules/testing/change_detection_outputs.hpp" +#include "cv_bridge/cv_bridge.h" +#include +#include "sensor_msgs/msg/image.hpp" + + +namespace vtr { +namespace lidar { + +using namespace tactic; + +auto CDTestModule::Config::fromROS(const rclcpp::Node::SharedPtr &node, + const std::string ¶m_prefix) + -> ConstPtr { + auto config = std::make_shared(); + // clang-format off + config->save_point_cloud = node->declare_parameter(param_prefix + ".save_point_cloud", config->save_point_cloud); + config->suffix = node->declare_parameter(param_prefix + ".suffix", config->suffix); + + auto& params = config->perspective_params; + + params.width = node->declare_parameter(param_prefix + ".img_width", 256); + params.height = node->declare_parameter(param_prefix + ".img_height", 128); + params.h_fov = node->declare_parameter(param_prefix + ".h_fov", M_PI/2); + params.v_fov = node->declare_parameter(param_prefix + ".v_fov", M_PI/4); + params.max_range = node->declare_parameter(param_prefix + ".max_range", params.max_range); + params.min_range = node->declare_parameter(param_prefix + ".min_range", params.min_range); + // clang-format on + return config; +} + +void CDTestModule::run_(QueryCache &qdata0, OutputCache &, + const Graph::Ptr &graph, const TaskExecutor::Ptr &) { + auto &qdata = dynamic_cast(qdata0); + + + if (*qdata.vertex_test_result == VertexTestResult::CREATE_VERTEX) { + + auto vertex = graph->at(*qdata.vid_odo); + + const auto &T_s_r = *qdata.T_s_r; + const auto &T_r_v_loc = *qdata.T_r_v_loc; + + Eigen::Matrix4d T_c_s_temp; + + T_c_s_temp << 0, -1, 0, 0, + 0, 0, 1, 0, + -1, 0, 0, 0, + 0, 0, 0, 1; + + const tactic::EdgeTransform T_c_s {T_c_s_temp}; + const tactic::EdgeTransform T_c_v_loc = T_c_s * T_s_r * T_r_v_loc; + + //Save the changed point cloud + + if (config_->save_point_cloud) { + auto raw_scan_odo = std::make_shared>(); + raw_scan_odo->point_cloud() = *qdata.changed_points; + raw_scan_odo->T_vertex_this() = *qdata.T_r_v_loc; + raw_scan_odo->vertex_id() = *qdata.vid_odo; + // + using PointScanLM = storage::LockableMessage>; + auto raw_scan_odo_msg = + std::make_shared(raw_scan_odo, *qdata.stamp); + vertex->insert>( + "detected_points_" + config_->suffix, "vtr_lidar_msgs/msg/PointScan", raw_scan_odo_msg); + CLOG(DEBUG, "lidar.pipeline") << "Saved raw pointcloud to vertex" << vertex; + } + + auto changed_points_copy = pcl::PointCloud(*qdata.changed_points); + + auto live_points_mat = changed_points_copy.getMatrixXfMap(4, PointWithInfo::size(), PointWithInfo::cartesian_offset()); + live_points_mat = T_c_v_loc.matrix().cast() * live_points_mat; + + cv::Mat changed_idx_img = cv::Mat::zeros(config_->perspective_params.height, config_->perspective_params.width, CV_32S); + cv::Mat changed_hsv_img = cv::Mat::zeros(config_->perspective_params.height, config_->perspective_params.width, CV_8UC3); + cv::Mat changed_thresh_img = cv::Mat::zeros(config_->perspective_params.height, config_->perspective_params.width, CV_8UC1); + + generate_depth_image(changed_points_copy, changed_hsv_img, changed_idx_img, config_->perspective_params); + + cv::Mat hsv_chans[3]; //destination array + cv::split(changed_hsv_img, hsv_chans);//split source + + + cv_bridge::CvImage live_mask_img_msg; + live_mask_img_msg.header.frame_id = "lidar"; + live_mask_img_msg.encoding = "mono8"; + live_mask_img_msg.image = hsv_chans[1]; + + using Image_LockMsg = storage::LockableMessage; + auto locked_image_msg = + std::make_shared(live_mask_img_msg.toImageMsg(), *qdata.stamp); + vertex->insert("detection_mask_" + config_->suffix, "sensor_msgs/msg/Image", locked_image_msg); + + } + + qdata.changed_points.clear(); +} + +} // namespace lidar +} // namespace vtr \ No newline at end of file diff --git a/main/src/vtr_torch/src/modules/torch_module.cpp b/main/src/vtr_torch/src/modules/torch_module.cpp index c1c381d40..602b904e9 100644 --- a/main/src/vtr_torch/src/modules/torch_module.cpp +++ b/main/src/vtr_torch/src/modules/torch_module.cpp @@ -33,7 +33,12 @@ auto TorchModule::Config::fromROS(const rclcpp::Node::SharedPtr &node, config->use_gpu = node->declare_parameter(param_prefix + ".use_gpu", config->use_gpu); config->abs_filepath = node->declare_parameter(param_prefix + ".abs_filepath", config->abs_filepath); - auto model_dir = node->declare_parameter("model_dir", "default"); + std::string model_dir = "default"; + if (!node->has_parameter("model_dir")) { + model_dir = node->declare_parameter("model_dir", "default"); + } else { + node->get_parameter("model_dir", model_dir); + } model_dir = common::utils::expand_user(common::utils::expand_env(model_dir)); if (config->abs_filepath){ From 793371a19c496ee93c9096d7bc22382e18a71a18 Mon Sep 17 00:00:00 2001 From: a-krawciw Date: Mon, 22 Apr 2024 14:56:10 -0400 Subject: [PATCH 74/77] Add new launch files --- config/ouster_warthog_default.yaml | 68 ++-- config/ouster_warthog_learned.yaml | 340 ------------------ ...online_ouster_warthog_rangenet.launch.yaml | 47 +++ rviz/ouster.rviz | 89 ++--- 4 files changed, 89 insertions(+), 455 deletions(-) create mode 100644 launch/online_ouster_warthog_rangenet.launch.yaml diff --git a/config/ouster_warthog_default.yaml b/config/ouster_warthog_default.yaml index 9a3139fa6..e1e5832df 100644 --- a/config/ouster_warthog_default.yaml +++ b/config/ouster_warthog_default.yaml @@ -11,7 +11,7 @@ # tactic #- tactic - tactic.pipeline - #- tactic.module + - tactic.module #- tactic.module.live_mem_manager #- tactic.module.graph_mem_manager @@ -34,7 +34,9 @@ # lidar pipeline - lidar.pipeline - - lidar.preprocessing + #- lidar.preprocessing + - lidar.change_detection + - lidar.obstacle_inflation #- lidar.ouster_converter #- lidar.odometry_icp #- lidar.odometry_map_maintenance @@ -62,7 +64,7 @@ enable_parallelization: true preprocessing_skippable: false odometry_mapping_skippable: false - localization_skippable: false + localization_skippable: true task_queue_num_threads: 1 task_queue_size: -1 @@ -97,8 +99,8 @@ localization: - recall - icp - - safe_corridor - #- change_detection_sync + - change_detection_sync + - obstacle_inflation - memory submap_translation_threshold: 1.5 submap_rotation_threshold: 30.0 @@ -122,7 +124,7 @@ vertical_angle_res: 0.0061365 # vertical angle resolution in radians, equal to 0.3516 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: 2.0 # scale down yaw(phi) by this value so that vertical resolution ~= horizontal resolution, horizontal resolution when 5Hz spin frequence is ~0.7031 degree, so 0.7031 / 0.3516 = 2.00 + h_scale: 1.0 # scale down yaw(phi) by this value so that vertical resolution ~= horizontal resolution, horizontal resolution currently 1024 num_sample1: 20000 # max number of sample after filtering based on planarity min_norm_score1: 0.95 # min planarity score @@ -241,10 +243,10 @@ visualize: true change_detection_sync: type: lidar.change_detection_v3 - detection_range: 8.0 + detection_range: 30.0 search_radius: 0.25 - negprob_threshold: 0.015 # was 0.015 # -1.86 without prior, 0.015 with prior # Jordy: I found I needed to bump this up abit (approx 0.075+) to reduce false positives + negprob_threshold: 1.5 # was 0.015 # -1.86 without prior, 0.015 with prior # Jordy: I found I needed to bump this up abit (approx 0.075+) to reduce false positives use_prior: true alpha0: 3.0 beta0: 0.03 @@ -252,15 +254,19 @@ support_radius: 0.25 support_variance: 0.05 support_threshold: 2.5 - - influence_distance: 0.6 # was 0.5 Jordy # Note that the total distance 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 + visualize: true + obstacle_inflation: + type: lidar.costmap_queue + influence_distance: 0.8 # Note that the total distance where grid cells have values > 0 is min dist + influence dist, not influence dist! + minimum_distance: 1.0 # was 0.3 Jordy resolution: 0.25 + costmap_history_size: 6 + neighbourhood: 5 + radius_filter: 0.5 + blind_spot_radius: 1.25 + lifetime: 1.5 size_x: 16.0 - size_y: 8.0 + size_y: 16.0 visualize: true memory: type: graph_mem_manager @@ -275,43 +281,13 @@ size_y: 20.0 run_async: true visualize: true - ground_extraction: - type: lidar.ground_extraction - z_offset: 0.2 - alpha: 0.035 - tolerance: 0.25 - Tm: 0.3 - Tm_small: 0.1 - Tb: 0.5 - Trmse: 0.1 - Tdprev: 1.0 - rmin: 2.0 - num_bins_small: 30.0 - bin_size_small: 0.5 - num_bins_large: 10.0 - bin_size_large: 1.0 - resolution: 0.6 - size_x: 40.0 - size_y: 20.0 - run_async: true - visualize: true - terrain_assessment: - type: lidar.terrain_assessment - lookahead_distance: 15.0 - corridor_width: 1.0 - search_radius: 1.0 - resolution: 0.5 - size_x: 40.0 - size_y: 20.0 - run_online: false - run_async: true visualize: true path_planning: type: cbit control_period: 100 # ms cbit: - obstacle_avoidance: false + obstacle_avoidance: true obs_padding: 0.0 curv_to_euclid_discretization: 10 sliding_window_width: 12.0 diff --git a/config/ouster_warthog_learned.yaml b/config/ouster_warthog_learned.yaml index 569cb8288..f9decab14 100644 --- a/config/ouster_warthog_learned.yaml +++ b/config/ouster_warthog_learned.yaml @@ -1,102 +1,7 @@ /**: ros__parameters: - log_to_file: true - log_debug: true - log_enabled: - # navigator - - navigation - #- navigation.graph_map_server - #- navigation.command - - # tactic - #- tactic - - tactic.pipeline - - tactic.module - #- tactic.module.live_mem_manager - #- tactic.module.graph_mem_manager - - # path planner - #- path_planning - #- path_planning.cbit - #- path_planning.cbit_planner - - mpc.cbit - #- mpc_debug - #- obstacle detection.cbit - #- grizzly_controller_tests.cbit - - ouster - - # mission planner - #- mission.server - #- mission.state_machine - - # pose graph - #- pose_graph - - # lidar pipeline - - lidar.pipeline - #- lidar.preprocessing - #- lidar.ouster_converter - #- lidar.odometry_icp - #- lidar.odometry_map_maintenance - - lidar.vertex_test - #- lidar.localization_map_recall - - lidar.localization_icp - - lidar.intra_exp_merging - #- lidar.dynamic_detection - - lidar.inter_exp_merging - - lidar.range - - lidar.costamp_inflation - - lidar.obstacle_inflation - #- lidar.ground_extraction - #- lidar.obstacle_detection - #- lidar.terrain_assessment - - robot_frame: base_link - env_info_topic: env_info - lidar_frame: os_lidar - lidar_topic: /ouster/points - queue_size: 1 - graph_map: - origin_lat: 43.7822 - origin_lng: -79.4661 - origin_theta: 1.5 - scale: 1.0 - tactic: - enable_parallelization: true - preprocessing_skippable: false - odometry_mapping_skippable: false - localization_skippable: false - task_queue_num_threads: 1 - task_queue_size: -1 - - route_completion_translation_threshold: 0.5 - - chain: - min_cusp_distance: 1.5 - angle_weight: 7.0 - search_depth: 5 - search_back_depth: 10 - distance_warning: 5.0 - save_odometry_result: true - save_localization_result: true - visualize: true - rviz_loc_path_offset: - - 0.0 - - 0.0 - - 0.0 pipeline: type: lidar - preprocessing: - - conversion - - filtering - odometry: - - icp - - mapping - - vertex_test - - intra_exp_merging - - dynamic_detection - - inter_exp_merging - - memory localization: - recall - icp @@ -107,142 +12,9 @@ submap_translation_threshold: 1.5 submap_rotation_threshold: 30.0 save_nn_point_cloud: true - preprocessing: - conversion: - type: lidar.ouster_converter - visualize: false - filter_warthog: true - filter_z_min: -0.2 - filter_z_max: 0.35 - filter_radius: 0.8 - filtering: - type: lidar.preprocessing - num_threads: 8 - crop_range: 40.0 - - frame_voxel_size: 0.3 # grid subsampling voxel size - nn_voxel_size: 0.05 - - vertical_angle_res: 0.0061365 # vertical angle resolution in radians, equal to 0.3516 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: 2.0 # scale down yaw(phi) by this value so that vertical resolution ~= horizontal resolution, horizontal resolution when 5Hz spin frequence is ~0.7031 degree, so 0.7031 / 0.3516 = 2.00 - - num_sample1: 20000 # max number of sample after filtering based on planarity - min_norm_score1: 0.95 # min planarity score - - num_sample2: 20000 # 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: 20000 # 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 - traj_lock_prev_vel: false - traj_qc_diag: - - 1.0 - - 0.1 - - 0.1 - - 0.1 - - 0.1 - - 1.0 - num_threads: 8 - first_num_steps: 2 - initial_max_iter: 4 - initial_max_pairing_dist: 1.5 - initial_max_planar_dist: 1.0 - refined_max_iter: 50 - refined_max_pairing_dist: 1.0 - refined_max_planar_dist: 0.3 - averaging_num_steps: 2 - verbose: false - max_iterations: 1 - min_matched_ratio: 0.5 - visualize: true - mapping: - type: lidar.odometry_map_maintenance_v2 - - map_voxel_size: 0.3 - - crop_range_front: 40.0 - back_over_front_ratio: 0.5 - point_life_time: 20.0 - visualize: true - vertex_test: - type: lidar.vertex_test - max_translation: 0.3 - max_rotation: 10.0 - intra_exp_merging: - type: lidar.intra_exp_merging_v2 - depth: 6.0 - map_voxel_size: 0.3 - - 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.0122718 # 0.02042 - vertical_resolution: 0.00613587 # 0.01326 - max_num_observations: 2000 - min_num_observations: 4 - dynamic_threshold: 0.3 - visualize: true - inter_exp_merging: - type: "lidar.inter_exp_merging_v2" - - map_voxel_size: 0.3 - max_num_experiences: 128 - distance_threshold: 0.6 - planar_threshold: 0.2 - normal_threshold: 0.8 - dynamic_obs_threshold: 1 - visualize: true - memory: - type: live_mem_manager - window_size: 5 localization: - recall: - type: lidar.localization_map_recall - map_version: pointmap - visualize: true - icp: - type: lidar.localization_icp - use_pose_prior: true - num_threads: 8 - first_num_steps: 2 - initial_max_iter: 4 - initial_max_pairing_dist: 1.5 - initial_max_planar_dist: 1.0 - refined_max_iter: 50 - refined_max_pairing_dist: 1.0 - refined_max_planar_dist: 0.3 - averaging_num_steps: 2 - verbose: false - max_iterations: 1 - min_matched_ratio: 0.5 - safe_corridor: - type: lidar.safe_corridor - lookahead_distance: 5.0 - corridor_width: 3.5 - influence_distance: 1.0 - resolution: 0.25 - size_x: 16.0 - size_y: 8.0 - visualize: true range_change: type: torch.range_change use_gpu: true @@ -263,116 +35,4 @@ size_x: 16.0 size_y: 16.0 visualize: true - memory: - type: graph_mem_manager - vertex_life_span: 5 - window_size: 3 - obstacle_detection: - type: lidar.obstacle_detection - z_min: 0.5 - z_max: 2.0 - resolution: 0.6 - size_x: 40.0 - size_y: 20.0 - run_async: true - visualize: true - ground_extraction: - type: lidar.ground_extraction - z_offset: 0.2 - alpha: 0.035 - tolerance: 0.25 - Tm: 0.3 - Tm_small: 0.1 - Tb: 0.5 - Trmse: 0.1 - Tdprev: 1.0 - rmin: 2.0 - num_bins_small: 30.0 - bin_size_small: 0.5 - num_bins_large: 10.0 - bin_size_large: 1.0 - resolution: 0.6 - size_x: 40.0 - size_y: 20.0 - run_async: true - visualize: true - terrain_assessment: - type: lidar.terrain_assessment - lookahead_distance: 15.0 - corridor_width: 1.0 - search_radius: 1.0 - resolution: 0.5 - size_x: 40.0 - size_y: 20.0 - run_online: false - run_async: true - visualize: true - - path_planning: - type: cbit.lidar # cbit for obstacle free path tracker, cbit.lidar for obstacle avoidance version - control_period: 100 # ms - cbit: - obs_padding: 0.0 - curv_to_euclid_discretization: 10 - sliding_window_width: 12.0 - sliding_window_freespace_padding: 0.5 - corridor_resolution: 0.2 - state_update_freq: 2.0 - update_state: true - rand_seed: 1 - - # Planner Tuning Params - initial_samples: 400 - batch_samples: 200 - pre_seed_resolution: 0.5 - alpha: 0.25 - q_max: 2.5 - frame_interval: 50 - iter_max: 10000000 - 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 # Note I dont think im actually using this one anymore, am using the one in the change detection v3 now - - speed_scheduler: - planar_curv_weight: 2.50 - profile_curv_weight: 0.5 - eop_weight: 1.0 - min_vel: 0.5 - - mpc: - # Controller Params - extrapolate_robot_pose: true - mpc_verbosity: false - homotopy_guided_mpc: false - horizon_steps: 20 - horizon_step_size: 0.5 - forward_vel: 0.75 - max_lin_vel: 1.25 - max_ang_vel: 1.0 - robot_linear_velocity_scale: 1.0 # Used to scale the output twist linear velocity messages by some constant factor to compensate internal low level control miscalibration - robot_angular_velocity_scale: 1.0 # Used to scale the output twist angular velocity messages by some constant factor to compensate internal low level control miscalibration - - vehicle_model: "unicycle" - - # Cost Function Covariance Matrices - pose_error_cov: [10.0, 10.0, 100.0, 100.0, 100.0, 20.0] - vel_error_cov: [20.0, 30.0] - acc_error_cov: [20.0, 20.0] - kin_error_cov: [0.01, 0.01, 0.01, 0.01, 0.01, 0.01] - lat_error_cov: [2.0] - - # Cost Function Weights - pose_error_weight: 1.0 - vel_error_weight: 1.5 - acc_error_weight: 1.5 - kin_error_weight: 1.0 - lat_error_weight: 0.01 - # Misc - command_history_length: 100 diff --git a/launch/online_ouster_warthog_rangenet.launch.yaml b/launch/online_ouster_warthog_rangenet.launch.yaml new file mode 100644 index 000000000..e46148760 --- /dev/null +++ b/launch/online_ouster_warthog_rangenet.launch.yaml @@ -0,0 +1,47 @@ +## Online Lidar VTR3 with LaserSAM +session_name: vtr_online_ouster_warthog + +environment: + FASTRTPS_DEFAULT_PROFILES_FILE: ${VTRSRC}/config/vtr_rtps.xml + ROS_DOMAIN_ID: "131" # set this to a unique number when multiple ROS2 dependent system running on the same network + +start_directory: ${VTRTEMP} + +suppress_history: false + +windows: + - window_name: vtr_navigation_system + layout: main-horizontal + shell_command_before: + - source ${VTRSRC}/main/install/setup.bash + - ros2 run vtr_gui setup_server --ros-args -r __ns:=/vtr + panes: + - > + ros2 launch vtr_navigation vtr.launch.py + base_params:=ouster_warthog_default.yaml + override_params:=ouster_warthog_learned.yaml + start_new_graph:=false + use_sim_time:=false + planner:="stationary" + model_dir:=${VTRROOT}/models + + + # data_dir:=${VTRTEMP}/lidar/sam_testing + + - window_name: vtr_gui + layout: main-horizontal + shell_command_before: + - source ${VTRSRC}/main/install/setup.bash + panes: + - ros2 run vtr_gui socket_client --ros-args -r __ns:=/vtr + - ros2 run vtr_gui socket_server --ros-args -r __ns:=/vtr + - ros2 run vtr_gui web_server --ros-args -r __ns:=/vtr + # - firefox --new-window "localhost:5200" # the webpage has to wait for everything above + + - window_name: rviz2 + layout: main-horizontal + shell_command_before: + - source ${VTRSRC}/main/install/setup.bash + panes: + - ros2 run rviz2 rviz2 -d ${VTRSRC}/rviz/lidar.rviz + diff --git a/rviz/ouster.rviz b/rviz/ouster.rviz index e2ff345b3..78d1ff9b0 100644 --- a/rviz/ouster.rviz +++ b/rviz/ouster.rviz @@ -5,7 +5,10 @@ Panels: Property Tree Widget: Expanded: - /Global Options1 + - /Filtered Scan1 + - /Localization Submap1 - /Change Detection Scan1 + - /Change Detection Scan1/Status1 - /Teach Path1/Topic1 Splitter Ratio: 0.4507042169570923 Tree Height: 1391 @@ -60,68 +63,16 @@ Visualization Manager: Frame Timeout: 99999 Frames: All Enabled: false - base_link: - Value: true - chassis_link: - Value: true - diff_link: - Value: true - front_left_wheel_link: - Value: true - front_right_wheel_link: - Value: true - imu_link: - Value: true - left_diff_unit_headlight_link: - Value: true - left_diff_unit_link: - Value: true - left_diff_unit_taillight_link: - Value: true lidar: Value: false loc vertex frame: Value: true loc vertex frame (offset): Value: true - navtech_base: - Value: true - nerian_left: - Value: true - nerian_stereo: - Value: true - novatel: - Value: true odo vertex frame: Value: true - odom: - Value: true - os_imu: - Value: true - os_lidar: - Value: true - os_sensor: - Value: true - os_sensor_base_link: - Value: true - os_sensor_baseplate: - Value: true - rear_left_wheel_link: - Value: true - rear_right_wheel_link: - Value: true - right_diff_unit_headlight_link: - Value: true - right_diff_unit_link: - Value: true - right_diff_unit_taillight_link: - Value: true robot: Value: true - sensor_tower: - Value: true - top_chassis_link: - Value: true world: Value: false world (offset): @@ -185,17 +136,17 @@ Visualization Manager: Min Value: -10 Value: true Axis: Z - Channel Name: time + Channel Name: normal_z Class: rviz_default_plugins/PointCloud2 Color: 0; 255; 0 - Color Transformer: FlatColor + Color Transformer: Intensity Decay Time: 0 Enabled: false Invert Rainbow: false Max Color: 255; 255; 255 - Max Intensity: 999999 + Max Intensity: 1 Min Color: 0; 0; 0 - Min Intensity: 999999 + Min Intensity: -0.9999999403953552 Name: Filtered Scan Position Transformer: XYZ Selectable: true @@ -253,12 +204,12 @@ Visualization Manager: Min Value: -10 Value: true Axis: Z - Channel Name: icp_score + Channel Name: normal_z Class: rviz_default_plugins/PointCloud2 Color: 255; 0; 0 - Color Transformer: FlatColor + Color Transformer: Intensity Decay Time: 0 - Enabled: false + Enabled: true Invert Rainbow: false Max Color: 255; 255; 255 Max Intensity: 3 @@ -268,7 +219,7 @@ Visualization Manager: Position Transformer: XYZ Selectable: true Size (Pixels): 3 - Size (m): 0.10000000149011612 + Size (m): 0.05000000074505806 Style: Flat Squares Topic: Depth: 5 @@ -279,7 +230,7 @@ Visualization Manager: Value: /vtr/submap_loc Use Fixed Frame: true Use rainbow: true - Value: false + Value: true - Alpha: 1 Autocompute Intensity Bounds: true Autocompute Value Bounds: @@ -370,8 +321,8 @@ Visualization Manager: Position Transformer: XYZ Selectable: true Size (Pixels): 5 - Size (m): 0.10000000149011612 - Style: Points + Size (m): 0.30000001192092896 + Style: Flat Squares Topic: Depth: 5 Durability Policy: Volatile @@ -680,25 +631,25 @@ Visualization Manager: Views: Current: Class: rviz_default_plugins/Orbit - Distance: 27.497325897216797 + Distance: 65.87181854248047 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false Focal Point: - X: 11.865821838378906 - Y: -0.894321084022522 - Z: -0.5033813714981079 + X: 11.6036958694458 + Y: 1.764120101928711 + Z: 0.03465702384710312 Focal Shape Fixed Size: false Focal Shape Size: 0.05000000074505806 Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: 0.4447973966598511 + Pitch: 1.0402021408081055 Target Frame: robot Value: Orbit (rviz_default_plugins) - Yaw: 3.4912807941436768 + Yaw: 3.2712206840515137 Saved: - Class: rviz_default_plugins/Orbit Distance: 111.39698028564453 From 87c8250b2e12fc352093b9a94bfb98e74e037332 Mon Sep 17 00:00:00 2001 From: a-krawciw Date: Sat, 27 Apr 2024 13:06:12 -0400 Subject: [PATCH 75/77] 3d prompted version --- .../planning/segmentanything_module.hpp | 1 + .../planning/change_detection_module_v3.cpp | 9 +- .../planning/segmentanything_module.cpp | 214 +++++++++++++----- 3 files changed, 166 insertions(+), 58 deletions(-) diff --git a/main/src/vtr_lidar/include/vtr_lidar/modules/planning/segmentanything_module.hpp b/main/src/vtr_lidar/include/vtr_lidar/modules/planning/segmentanything_module.hpp index ba2d7f7a1..5d1ef7487 100644 --- a/main/src/vtr_lidar/include/vtr_lidar/modules/planning/segmentanything_module.hpp +++ b/main/src/vtr_lidar/include/vtr_lidar/modules/planning/segmentanything_module.hpp @@ -57,6 +57,7 @@ class SegmentAnythingModule : public nn::TorchModule { int num_prompts = 4; int smooth_size = 10; float corridor_width = 2.0; + float max_size = 0.1; static ConstPtr fromROS(const rclcpp::Node::SharedPtr &node, 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 85ed4e566..35d6ca410 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 @@ -99,7 +99,10 @@ void ChangeDetectionModuleV3::run_(QueryCache &qdata0, OutputCache &output0, publisher_initialized_ = true; } - if (!qdata.undistorted_point_cloud.valid()) { + if (!*qdata.run_cd) + return; + + if (!qdata.nn_point_cloud.valid()) { CLOG(WARNING, "lidar.change_detection_v3") << "Point clouds are not aligned. Skipping Change Detection."; return; } @@ -108,10 +111,8 @@ void ChangeDetectionModuleV3::run_(QueryCache &qdata0, OutputCache &output0, // inputs const auto &stamp = *qdata.stamp; const auto &T_s_r = *qdata.T_s_r; - const auto &vid_loc = *qdata.vid_loc; - const auto &sid_loc = *qdata.sid_loc; const auto &T_r_v_loc = *qdata.T_r_v_loc; - const auto &points = *qdata.raw_point_cloud; + const auto &points = *qdata.nn_point_cloud; const auto &submap_loc = *qdata.submap_loc; const auto &map_point_cloud = submap_loc.point_cloud(); const auto &T_v_m_loc = *qdata.T_v_m_loc; diff --git a/main/src/vtr_lidar/src/modules/planning/segmentanything_module.cpp b/main/src/vtr_lidar/src/modules/planning/segmentanything_module.cpp index e24a742aa..4031d94f3 100644 --- a/main/src/vtr_lidar/src/modules/planning/segmentanything_module.cpp +++ b/main/src/vtr_lidar/src/modules/planning/segmentanything_module.cpp @@ -25,6 +25,66 @@ #include +#include + +using namespace cv; + +namespace { +// size comparison, for list sorting +bool compare_size(KeyPoint first, KeyPoint second) +{ + if (first.size > second.size) return true; + else return false; +} + +std::vector detectBlobsSortedBySize(Mat im) +{ + //Invert image + im = 255 - im; + + // Setup SimpleBlobDetector parameters. + SimpleBlobDetector::Params params; + + // Change thresholds + params.minThreshold = 10; + params.maxThreshold = 200; + + // Filter by Area. + params.filterByArea = false; + params.minArea = 25; + + // Filter by Circularity + params.filterByCircularity = false; + params.minCircularity = 0.1; + + // Filter by Convexity + params.filterByConvexity = false; + params.minConvexity = 0.87; + + // Filter by Inertia + params.filterByInertia = false; + params.minInertiaRatio = 0.01; + + + params.minDistBetweenBlobs = 1; + + + // Set up the detector with default parameters. + Ptr detector = SimpleBlobDetector::create(params); + + // Detect blobs. + std::vector keypoints; + detector->detect( im, keypoints); + + //Sort by size + std::sort (keypoints.begin(), keypoints.end(), compare_size); + + return keypoints; + +} + +} + namespace vtr { @@ -55,6 +115,7 @@ auto SegmentAnythingModule::Config::fromROS(const rclcpp::Node::SharedPtr &node, config->num_prompts = node->declare_parameter(param_prefix + ".num_prompts", config->num_prompts); config->smooth_size = node->declare_parameter(param_prefix + ".smooth_size", config->smooth_size); config->corridor_width = node->declare_parameter(param_prefix + ".corridor_width", config->corridor_width); + config->max_size = node->declare_parameter(param_prefix + ".max_size", config->max_size); // clang-format off config->visualize = node->declare_parameter(param_prefix + ".visualize", config->visualize); @@ -85,6 +146,9 @@ void SegmentAnythingModule::run_(QueryCache &qdata0, OutputCache &output, pub_init_ = true; } + if (!*qdata.run_cd) + return; + auto raw_point_cloud = *qdata.raw_point_cloud; if(!qdata.submap_loc.valid()) { @@ -175,6 +239,17 @@ void SegmentAnythingModule::run_(QueryCache &qdata0, OutputCache &output, std::make_shared(live_cv_rgb_img.toImageMsg(), *qdata.stamp); vertex->insert("live_depth_image", "sensor_msgs/msg/Image", locked_image_msg); } + + cv_bridge::CvImage raw_cv_rgb_img; + raw_cv_rgb_img.header.frame_id = "lidar"; + //cv_rgb_img.header.stamp = qdata.stamp->header.stamp; + raw_cv_rgb_img.encoding = "rgb8"; + raw_cv_rgb_img.image = raw_rgb_img; + if (*qdata.vertex_test_result == VertexTestResult::CREATE_VERTEX) { + auto locked_image_msg = + std::make_shared(raw_cv_rgb_img.toImageMsg(), *qdata.stamp); + vertex->insert("live_raw_image", "sensor_msgs/msg/Image", locked_image_msg); + } } cv::Mat map_index_img = cv::Mat::zeros(config_->perspective_params.height, config_->perspective_params.width, CV_32S); @@ -274,69 +349,84 @@ void SegmentAnythingModule::run_(QueryCache &qdata0, OutputCache &output, torch::Tensor live_tensor = torch::from_blob(big_live.data, {big_live.rows, big_live.cols, big_live.channels()}, torch::kByte).to(torch::kFloat32).permute({2, 0, 1}).contiguous(); torch::Tensor map_tensor = torch::from_blob(big_map.data, {big_map.rows, big_map.cols, big_map.channels()}, torch::kByte).to(torch::kFloat32).permute({2, 0, 1}).contiguous(); - torch::Tensor small_live_tensor = torch::from_blob(live_rgb_img.data, {raw_rgb_img.rows, raw_rgb_img.cols, raw_rgb_img.channels()}, torch::kByte).to(torch::kFloat32).permute({2, 0, 1}); - torch::Tensor small_map_tensor = torch::from_blob(map_rgb_img.data, {map_rgb_img.rows, map_rgb_img.cols, map_rgb_img.channels()}, torch::kByte).to(torch::kFloat32).permute({2, 0, 1}); - torch::Tensor diff = small_live_tensor - small_map_tensor; - diff = diff.norm(2, {0}); - //.clamp(0, 255.0) + cv::Mat binary_nearest_img; + { + const auto msg = vertex->retrieve( + "detection_mask_nearest", "sensor_msgs/msg/Image"); + if (msg == nullptr) { + CLOG(WARNING, "lidar.perspective") + << "This vertex has no 3D diff image."; + return; + } + auto locked_msg = msg->sharedLocked(); + binary_nearest_img = cv_bridge::toCvShare(locked_msg.get().getDataPtr(), "mono8")->image; + } - namespace F = torch::nn::functional; + // torch::Tensor small_live_tensor = torch::from_blob(live_rgb_img.data, {raw_rgb_img.rows, raw_rgb_img.cols, raw_rgb_img.channels()}, torch::kByte).to(torch::kFloat32).permute({2, 0, 1}); + // torch::Tensor small_raw_tensor = torch::from_blob(raw_rgb_img.data, {raw_rgb_img.rows, raw_rgb_img.cols, raw_rgb_img.channels()}, torch::kByte).to(torch::kFloat32).permute({2, 0, 1}); + // torch::Tensor small_map_tensor = torch::from_blob(map_rgb_img.data, {map_rgb_img.rows, map_rgb_img.cols, map_rgb_img.channels()}, torch::kByte).to(torch::kFloat32).permute({2, 0, 1}); + // torch::Tensor diff = small_live_tensor - small_map_tensor; + // diff = diff.norm(2, {0}); + // //.clamp(0, 255.0) - using namespace torch::indexing; + // namespace F = torch::nn::functional; - torch::Tensor average = torch::ones({1, 1, 3, 3}) / 9; + // using namespace torch::indexing; - diff = F::conv2d(diff.unsqueeze(0).unsqueeze(0), average); + // torch::Tensor average = torch::ones({1, 1, 3, 3}) / 9; - diff = diff.squeeze().squeeze(); + // diff = F::conv2d(diff.unsqueeze(0).unsqueeze(0), average); + + // diff = diff.squeeze().squeeze(); + // diff.index_put_({small_raw_tensor.norm() == 0}, 0); std::vector prompts; - static const int MAX_ITERS = 20; - for (int i = 0; prompts.size() < config_->num_prompts && i < MAX_ITERS; ++i) { - const auto [max_val, idx2] = torch::max(diff.flatten(), 0); + // static const int MAX_ITERS = 40; + // for (int i = 0; prompts.size() < config_->num_prompts && i < MAX_ITERS; ++i) { + // const auto [max_val, idx2] = torch::max(diff.flatten(), 0); - int idx = idx2.item(); - int prompt[] = {idx % diff.size(1), idx / diff.size(1)}; + // int idx = idx2.item(); + // int prompt[] = {idx % diff.size(1), idx / diff.size(1)}; - auto& pc_idx = live_index_img.at(prompt[1], prompt[0]); + // auto& pc_idx = live_index_img.at(prompt[1], prompt[0]); - if (pc_idx == 0) { - // CLOG(DEBUG, "lidar.perspective") << "Prompt point (" << prompt[0] << ", " << prompt[1] << ") << on an interpolated live pixel. Skipping."; + // if (pc_idx == 0) { + // // CLOG(DEBUG, "lidar.perspective") << "Prompt point (" << prompt[0] << ", " << prompt[1] << ") << on an interpolated live pixel. Skipping."; - diff.index_put_({Slice(idx / diff.size(1), idx / diff.size(1) + 1), - Slice(idx % diff.size(1), idx % diff.size(1) + 1) }, 0); - continue; - } - prompt[0] = 4*prompt[0] + 2; - prompt[1] = 4*prompt[1] + 2; + // diff.index_put_({Slice(idx / diff.size(1), idx / diff.size(1) + 1), + // Slice(idx % diff.size(1), idx % diff.size(1) + 1) }, 0); + // continue; + // } + // prompt[0] = 4*prompt[0] + 2; + // prompt[1] = 4*prompt[1] + 2; - auto live_point = raw_point_cloud[pc_idx - 1]; - Eigen::Vector4f h_point; - h_point << live_point.x, live_point.y, live_point.z, 1.0f; - h_point = T_v_loc_c.matrix().cast() * h_point; + // auto live_point = raw_point_cloud[pc_idx - 1]; + // Eigen::Vector4f h_point; + // h_point << live_point.x, live_point.y, live_point.z, 1.0f; + // h_point = T_v_loc_c.matrix().cast() * h_point; - double theta = atan2(h_point[1], h_point[0]); - CLOG(DEBUG, "lidar.perspective") << "Diff Tensor prompt (" << prompt[0] << ", " << prompt[1] << ") Theta: " << theta << " DIff norm " << max_val; + // double theta = atan2(h_point[1], h_point[0]); + // CLOG(DEBUG, "lidar.perspective") << "Diff Tensor prompt (" << prompt[0] << ", " << prompt[1] << ") Theta: " << theta << " DIff norm " << max_val; - //Prompts are x, y rather than row, column - //map_tensor.index({0, prompt[1], prompt[0]}).item().to() > 0 || - if (!((theta > 0.611 && theta < 0.96) || (theta > -0.96 && theta < -0.611))){ - prompts.push_back(torch::from_blob(prompt, {2}, torch::kInt).to(device)); + // //Prompts are x, y rather than row, column + // //map_tensor.index({0, prompt[1], prompt[0]}).item().to() > 0 || + // if (!((theta > 0.611 && theta < 0.96) || (theta > -0.96 && theta < -0.611))){ + // prompts.push_back(torch::from_blob(prompt, {2}, torch::kInt).to(device)); - diff.index_put_({Slice(idx / diff.size(1) - 3, idx / diff.size(1) + 4), - Slice(idx % diff.size(1) - 3, idx % diff.size(1) + 4) }, 0); + // diff.index_put_({Slice(idx / diff.size(1) - 3, idx / diff.size(1) + 4), + // Slice(idx % diff.size(1) - 3, idx % diff.size(1) + 4) }, 0); - } else { - // CLOG(DEBUG, "lidar.perspective") << "Prompt point on an empty map pixel. Try again."; - diff.index_put_({Slice(idx / diff.size(1), idx / diff.size(1) + 1), - Slice(idx % diff.size(1), idx % diff.size(1) + 1) }, 0); - continue; - } + // } else { + // // CLOG(DEBUG, "lidar.perspective") << "Prompt point on an empty map pixel. Try again."; + // diff.index_put_({Slice(idx / diff.size(1), idx / diff.size(1) + 1), + // Slice(idx % diff.size(1), idx % diff.size(1) + 1) }, 0); + // continue; + // } - } + // } // const auto [smaller_diff, idx] = F::max_pool2d_with_indices(diff.squeeze(0), F::MaxPool2dFuncOptions(5)); @@ -362,6 +452,21 @@ void SegmentAnythingModule::run_(QueryCache &qdata0, OutputCache &output, // } + + std::vector keypoints = detectBlobsSortedBySize(binary_nearest_img.clone()); + + for (const auto &kp : keypoints) { + int prompt[] = {kp.pt.x, kp.pt.y}; + + prompt[0] = 4*prompt[0] + 2; + prompt[1] = 4*prompt[1] + 2; + prompts.push_back(torch::from_blob(prompt, {2}, torch::kInt).to(device)); + + if(prompts.size() == config_->num_prompts) { + break; + } + } + torch::NoGradGuard no_grad; std::vector jit_inputs; @@ -432,12 +537,13 @@ void SegmentAnythingModule::run_(QueryCache &qdata0, OutputCache &output, cv::meanStdDev(live_hsv_img, liveMean, liveStdDev, active_repeat_mask); CLOG(DEBUG, "lidar.perspective") << "Map Mean" << mapMean << "Live mean " << liveMean; - - if (ious_a[i] < config_->iou_thresh && active_repeat_tensor.sum().item() < 0.1*255*128*256 && abs(liveMean[0] - mapMean[0]) > 10.) { +//&& abs(liveMean[0] - mapMean[0]) > 10. + if (ious_a[i] < config_->iou_thresh && active_repeat_tensor.sum().item() < config_->max_size*255*128*256 ) { idxs_to_keep.push_back(i); - teach_mask.setTo(color_options[2*i % color_options.size()], active_teach_mask == 255); - repeat_mask.setTo(color_options[2*i % color_options.size()], active_repeat_mask == 255); } + teach_mask.setTo(color_options[2*i % color_options.size()], active_teach_mask == 255); + repeat_mask.setTo(color_options[2*i % color_options.size()], active_repeat_mask == 255); + cv::circle(teach_mask, cv::Point{prompt_a[0] / 4, prompt_a[1] / 4}, 6, color_options[(2*i + 1) % color_options.size()], 1); cv::circle(repeat_mask, cv::Point{prompt_a[0] / 4, prompt_a[1] / 4}, 6, color_options[(2*i + 1) % color_options.size()], 1); @@ -501,14 +607,14 @@ void SegmentAnythingModule::run_(QueryCache &qdata0, OutputCache &output, map_mask_img_msg.image = teach_mask; map_mask_pub_->publish(*map_mask_img_msg.toImageMsg()); - diff = diff.div(diff.max()).mul(255).to(torch::kByte).contiguous(); + // diff = diff.div(diff.max()).mul(255).to(torch::kByte).contiguous(); - cv_bridge::CvImage diff_img_msg; - diff_img_msg.header.frame_id = "lidar"; - //cv_rgb_img.header.stamp = qdata.stamp->header.stamp; - diff_img_msg.encoding = "mono8"; - diff_img_msg.image = cv::Mat{diff.size(0), diff.size(1), CV_8UC1, diff.data_ptr()};; - diff_pub_->publish(*diff_img_msg.toImageMsg()); + // cv_bridge::CvImage diff_img_msg; + // diff_img_msg.header.frame_id = "lidar"; + // //cv_rgb_img.header.stamp = qdata.stamp->header.stamp; + // diff_img_msg.encoding = "mono8"; + // diff_img_msg.image = cv::Mat{diff.size(0), diff.size(1), CV_8UC1, diff.data_ptr()};; + // diff_pub_->publish(*diff_img_msg.toImageMsg()); } if (config_->visualize) { From 378131f84cd405a795685f4cbd4b4428be408c1f Mon Sep 17 00:00:00 2001 From: a-krawciw Date: Tue, 30 Apr 2024 17:22:00 -0400 Subject: [PATCH 76/77] Changes to final LaserSAM submission. --- config/ouster_warthog_default.yaml | 10 ++-- config/ouster_warthog_learned.yaml | 3 +- ...ine_ouster_warthog_lasersam_3d.launch.yaml | 46 +++++++++++++++++++ ...online_ouster_warthog_rangenet.launch.yaml | 2 +- .../rangenet_change_detection_module.cpp | 1 + .../planning/segmentanything_module.cpp | 14 +++++- .../testing/change_detection_outputs.cpp | 3 +- 7 files changed, 69 insertions(+), 10 deletions(-) create mode 100644 launch/online_ouster_warthog_lasersam_3d.launch.yaml diff --git a/config/ouster_warthog_default.yaml b/config/ouster_warthog_default.yaml index e1e5832df..5001fd03d 100644 --- a/config/ouster_warthog_default.yaml +++ b/config/ouster_warthog_default.yaml @@ -169,7 +169,7 @@ mapping: type: lidar.odometry_map_maintenance_v2 - map_voxel_size: 0.1 + map_voxel_size: 0.2 crop_range_front: 40.0 back_over_front_ratio: 1.0 @@ -184,7 +184,7 @@ type: lidar.intra_exp_merging_v2 depth: 6.0 - map_voxel_size: 0.1 + map_voxel_size: 0.2 crop_range_front: 40.0 back_over_front_ratio: 1.0 @@ -202,7 +202,7 @@ inter_exp_merging: type: "lidar.inter_exp_merging_v2" - map_voxel_size: 0.1 + map_voxel_size: 0.2 max_num_experiences: 128 distance_threshold: 0.6 planar_threshold: 0.2 @@ -243,10 +243,10 @@ visualize: true change_detection_sync: type: lidar.change_detection_v3 - detection_range: 30.0 + detection_range: 15.0 search_radius: 0.25 - negprob_threshold: 1.5 # was 0.015 # -1.86 without prior, 0.015 with prior # Jordy: I found I needed to bump this up abit (approx 0.075+) to reduce false positives + negprob_threshold: 7.5 # was 0.015 # -1.86 without prior, 0.015 with prior # Jordy: I found I needed to bump this up abit (approx 0.075+) to reduce false positives use_prior: true alpha0: 3.0 beta0: 0.03 diff --git a/config/ouster_warthog_learned.yaml b/config/ouster_warthog_learned.yaml index f9decab14..5959606e2 100644 --- a/config/ouster_warthog_learned.yaml +++ b/config/ouster_warthog_learned.yaml @@ -22,7 +22,8 @@ filepath: exported_model_forest_m30_10m.pt radius_filter: 0.25 neighbourhood: 5 - save_nn_point_cloud: true + save_nn_point_cloud: false + range_crop: 10.0 visualize: true costmap_inflation: type: lidar.costmap_blindspot diff --git a/launch/online_ouster_warthog_lasersam_3d.launch.yaml b/launch/online_ouster_warthog_lasersam_3d.launch.yaml new file mode 100644 index 000000000..0795d93ec --- /dev/null +++ b/launch/online_ouster_warthog_lasersam_3d.launch.yaml @@ -0,0 +1,46 @@ +## Online Lidar VTR3 with LaserSAM +session_name: vtr_online_ouster_warthog + +environment: + FASTRTPS_DEFAULT_PROFILES_FILE: ${VTRSRC}/config/vtr_rtps.xml + ROS_DOMAIN_ID: "131" # set this to a unique number when multiple ROS2 dependent system running on the same network + +start_directory: ${VTRTEMP} + +suppress_history: false + +windows: + - window_name: vtr_navigation_system + layout: main-horizontal + shell_command_before: + - source ${VTRSRC}/main/install/setup.bash + - ros2 run vtr_gui setup_server --ros-args -r __ns:=/vtr + panes: + - > + ros2 launch vtr_navigation vtr.launch.py + base_params:=ouster_lasersam_test.yaml + start_new_graph:=false + use_sim_time:=false + planner:="stationary" + model_dir:=${VTRROOT}/models + + + # data_dir:=${VTRTEMP}/lidar/sam_testing + + - window_name: vtr_gui + layout: main-horizontal + shell_command_before: + - source ${VTRSRC}/main/install/setup.bash + panes: + - ros2 run vtr_gui socket_client --ros-args -r __ns:=/vtr + - ros2 run vtr_gui socket_server --ros-args -r __ns:=/vtr + - ros2 run vtr_gui web_server --ros-args -r __ns:=/vtr + # - firefox --new-window "localhost:5200" # the webpage has to wait for everything above + + - window_name: rviz2 + layout: main-horizontal + shell_command_before: + - source ${VTRSRC}/main/install/setup.bash + panes: + - ros2 run rviz2 rviz2 -d ${VTRSRC}/rviz/lidar.rviz + diff --git a/launch/online_ouster_warthog_rangenet.launch.yaml b/launch/online_ouster_warthog_rangenet.launch.yaml index e46148760..660d82e58 100644 --- a/launch/online_ouster_warthog_rangenet.launch.yaml +++ b/launch/online_ouster_warthog_rangenet.launch.yaml @@ -22,7 +22,7 @@ windows: override_params:=ouster_warthog_learned.yaml start_new_graph:=false use_sim_time:=false - planner:="stationary" + planner:="cbit" model_dir:=${VTRROOT}/models diff --git a/main/src/vtr_lidar/src/modules/planning/rangenet_change_detection_module.cpp b/main/src/vtr_lidar/src/modules/planning/rangenet_change_detection_module.cpp index e1174365c..6c38614cc 100644 --- a/main/src/vtr_lidar/src/modules/planning/rangenet_change_detection_module.cpp +++ b/main/src/vtr_lidar/src/modules/planning/rangenet_change_detection_module.cpp @@ -66,6 +66,7 @@ auto RangeChangeNetModule::Config::fromROS(const rclcpp::Node::SharedPtr &node, config->range_crop = node->declare_parameter(param_prefix + ".range_crop", config->range_crop); config->neighbourhood = node->declare_parameter(param_prefix + ".neighbourhood", config->neighbourhood); config->save_nn_point_cloud = node->declare_parameter(param_prefix + ".save_nn_point_cloud", config->save_nn_point_cloud); + config->radius_filter = node->declare_parameter(param_prefix + ".radius_filter", config->radius_filter); // clang-format on return config; diff --git a/main/src/vtr_lidar/src/modules/planning/segmentanything_module.cpp b/main/src/vtr_lidar/src/modules/planning/segmentanything_module.cpp index 4031d94f3..2e57ac428 100644 --- a/main/src/vtr_lidar/src/modules/planning/segmentanything_module.cpp +++ b/main/src/vtr_lidar/src/modules/planning/segmentanything_module.cpp @@ -146,8 +146,6 @@ void SegmentAnythingModule::run_(QueryCache &qdata0, OutputCache &output, pub_init_ = true; } - if (!*qdata.run_cd) - return; auto raw_point_cloud = *qdata.raw_point_cloud; @@ -600,6 +598,12 @@ void SegmentAnythingModule::run_(QueryCache &qdata0, OutputCache &output, live_mask_img_msg.image = repeat_mask; live_mask_pub_->publish(*live_mask_img_msg.toImageMsg()); + if (*qdata.vertex_test_result == VertexTestResult::CREATE_VERTEX) { + auto locked_image_msg = + std::make_shared(live_mask_img_msg.toImageMsg(), *qdata.stamp); + vertex->insert("live_sam_mask", "sensor_msgs/msg/Image", locked_image_msg); + } + cv_bridge::CvImage map_mask_img_msg; map_mask_img_msg.header.frame_id = "lidar"; //cv_rgb_img.header.stamp = qdata.stamp->header.stamp; @@ -607,6 +611,12 @@ void SegmentAnythingModule::run_(QueryCache &qdata0, OutputCache &output, map_mask_img_msg.image = teach_mask; map_mask_pub_->publish(*map_mask_img_msg.toImageMsg()); + if (*qdata.vertex_test_result == VertexTestResult::CREATE_VERTEX) { + auto locked_image_msg = + std::make_shared(map_mask_img_msg.toImageMsg(), *qdata.stamp); + vertex->insert("map_sam_mask", "sensor_msgs/msg/Image", locked_image_msg); + } + // diff = diff.div(diff.max()).mul(255).to(torch::kByte).contiguous(); // cv_bridge::CvImage diff_img_msg; diff --git a/main/src/vtr_lidar/src/modules/testing/change_detection_outputs.cpp b/main/src/vtr_lidar/src/modules/testing/change_detection_outputs.cpp index f1a735048..070858f6a 100644 --- a/main/src/vtr_lidar/src/modules/testing/change_detection_outputs.cpp +++ b/main/src/vtr_lidar/src/modules/testing/change_detection_outputs.cpp @@ -51,6 +51,8 @@ void CDTestModule::run_(QueryCache &qdata0, OutputCache &, const Graph::Ptr &graph, const TaskExecutor::Ptr &) { auto &qdata = dynamic_cast(qdata0); + if (!*qdata.run_cd) + return; if (*qdata.vertex_test_result == VertexTestResult::CREATE_VERTEX) { @@ -92,7 +94,6 @@ void CDTestModule::run_(QueryCache &qdata0, OutputCache &, cv::Mat changed_idx_img = cv::Mat::zeros(config_->perspective_params.height, config_->perspective_params.width, CV_32S); cv::Mat changed_hsv_img = cv::Mat::zeros(config_->perspective_params.height, config_->perspective_params.width, CV_8UC3); - cv::Mat changed_thresh_img = cv::Mat::zeros(config_->perspective_params.height, config_->perspective_params.width, CV_8UC1); generate_depth_image(changed_points_copy, changed_hsv_img, changed_idx_img, config_->perspective_params); From 4cd4bf196b656670971dc4dfdc956eb83fb34e5a Mon Sep 17 00:00:00 2001 From: a-krawciw Date: Tue, 30 Apr 2024 19:02:07 -0400 Subject: [PATCH 77/77] Delete cd flag in cache. --- .../src/modules/planning/change_detection_module_v3.cpp | 3 --- .../vtr_lidar/src/modules/testing/change_detection_outputs.cpp | 2 -- 2 files changed, 5 deletions(-) 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 35d6ca410..67387ae8b 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 @@ -99,9 +99,6 @@ void ChangeDetectionModuleV3::run_(QueryCache &qdata0, OutputCache &output0, publisher_initialized_ = true; } - if (!*qdata.run_cd) - return; - if (!qdata.nn_point_cloud.valid()) { CLOG(WARNING, "lidar.change_detection_v3") << "Point clouds are not aligned. Skipping Change Detection."; return; diff --git a/main/src/vtr_lidar/src/modules/testing/change_detection_outputs.cpp b/main/src/vtr_lidar/src/modules/testing/change_detection_outputs.cpp index 070858f6a..0d09238e6 100644 --- a/main/src/vtr_lidar/src/modules/testing/change_detection_outputs.cpp +++ b/main/src/vtr_lidar/src/modules/testing/change_detection_outputs.cpp @@ -51,8 +51,6 @@ void CDTestModule::run_(QueryCache &qdata0, OutputCache &, const Graph::Ptr &graph, const TaskExecutor::Ptr &) { auto &qdata = dynamic_cast(qdata0); - if (!*qdata.run_cd) - return; if (*qdata.vertex_test_result == VertexTestResult::CREATE_VERTEX) {