From ccbd01c396810453ec83fe204e94962bf0a6cd4b Mon Sep 17 00:00:00 2001 From: David Yan Date: Thu, 9 Feb 2023 23:04:19 -0500 Subject: [PATCH 1/2] Update global and storage map origins; Need to test --- .../map_plan_launch/config/mapper.yaml | 8 ++-- .../include/mapper/local_global_mapper.h | 1 + .../mapper/src/local_global_mapper.cpp | 40 +++++++++++++++---- 3 files changed, 38 insertions(+), 11 deletions(-) diff --git a/autonomy_core/map_plan/map_plan_launch/config/mapper.yaml b/autonomy_core/map_plan/map_plan_launch/config/mapper.yaml index 8428b527..b02e8f19 100644 --- a/autonomy_core/map_plan/map_plan_launch/config/mapper.yaml +++ b/autonomy_core/map_plan/map_plan_launch/config/mapper.yaml @@ -5,8 +5,8 @@ robot_h: 0.25 # robot height, local and global maps will be dilated in z directi global: center_x: 0.0 # center of the map in x-axis center_y: 0.0 # center of the map in y-axis - range_x: 200.0 # range of the map in x-axis, map will span over [center_x - 0.5*range_x, center_x + 0.5*range_x] (high impact on memory and computation usage, recommended value: 500) - range_y: 200.0 # range of the map in y-axis, map will span over [center_y - 0.5*range_y, center_y + 0.5*range_y] (high impact on memory and computation usage, recommended value: 500 + range_x: 10.0 # range of the map in x-axis, map will span over [center_x - 0.5*range_x, center_x + 0.5*range_x] (high impact on memory and computation usage, recommended value: 500) + range_y: 10.0 # range of the map in y-axis, map will span over [center_y - 0.5*range_y, center_y + 0.5*range_y] (high impact on memory and computation usage, recommended value: 500 center_z: 5.0 # center of the map in z-axis, should be similar to the robot take_off_height range_z: 10.0 # range of the map in z-axis, map will span over [center_z - 0.5*range_z, center_z + 0.5*range_z] (high impact on memory and computation usage, recommended value: 10) resolution: 0.5 # resolution of the map (high impact on memory and computation usage, recommended value: 1.0~2.0 for sparse environments, 0.5 for dense environments) @@ -19,8 +19,8 @@ global: local: # local map range z and center_z will be the same as storage map # local planning horizon will be from robot to the local voxel map boundary # center of local map will be the same as robot current odometry along x and y direction, and the same as global map along z direction - range_x: 25.0 # range of the map in x-axis (high impact on memory and computation usage, recommended value: 40) - range_y: 25.0 # range of the map in x-axis (high impact on memory and computation usage, recommended value: 40) + range_x: 5.0 # range of the map in x-axis (high impact on memory and computation usage, recommended value: 40) + range_y: 5.0 # range of the map in x-axis (high impact on memory and computation usage, recommended value: 40) range_z: 12.0 # range of the map in z-axis (recommended value: slightly larger than global map range z to handle overshoot along z-axis upon stopping policy) resolution: 0.25 # resolution of the map (high impact on memory and computation usage, recommended value: 0.5~1.0 for sparse environments, 0.1~0.5 for dense environments) max_raycast_range: 13.0 # max raycast range, the smaller, the less computation demand, but shorter perception range diff --git a/autonomy_core/map_plan/mapper/include/mapper/local_global_mapper.h b/autonomy_core/map_plan/mapper/include/mapper/local_global_mapper.h index 12bd0801..7b663ec2 100644 --- a/autonomy_core/map_plan/mapper/include/mapper/local_global_mapper.h +++ b/autonomy_core/map_plan/mapper/include/mapper/local_global_mapper.h @@ -128,6 +128,7 @@ class LocalGlobalMapperNode { bool global_use_robot_dim_xy_; bool global_use_robot_dim_z_; double global_map_dim_d_x_, global_map_dim_d_y_, global_map_dim_d_z_; + double global_map_cx_, global_map_cy_, global_map_cz_; double local_map_dim_d_x_, local_map_dim_d_y_, local_map_dim_d_z_; double local_max_raycast_, global_max_raycast_; // maximum raycasting range diff --git a/autonomy_core/map_plan/mapper/src/local_global_mapper.cpp b/autonomy_core/map_plan/mapper/src/local_global_mapper.cpp index 70066c57..e2431a07 100644 --- a/autonomy_core/map_plan/mapper/src/local_global_mapper.cpp +++ b/autonomy_core/map_plan/mapper/src/local_global_mapper.cpp @@ -66,11 +66,11 @@ void LocalGlobalMapperNode::initParams() { nh_.param("robot_r", robot_r_, 0.2); nh_.param("robot_h", robot_h_, 0.0); - double global_map_cx, global_map_cy, global_map_cz; + double global_map_cx_, global_map_cy_, global_map_cz_; nh_.param("global/resolution", global_map_info_.resolution, 2.0f); - nh_.param("global/center_x", global_map_cx, 0.0); - nh_.param("global/center_y", global_map_cy, 0.0); - nh_.param("global/center_z", global_map_cz, 0.0); + nh_.param("global/center_x", global_map_cx_, 0.0); + nh_.param("global/center_y", global_map_cy_, 0.0); + nh_.param("global/center_z", global_map_cz_, 0.0); nh_.param("global/range_x", global_map_dim_d_x_, 500.0); nh_.param("global/range_y", global_map_dim_d_y_, 500.0); nh_.param("global/range_z", global_map_dim_d_z_, 2.0); @@ -84,9 +84,9 @@ void LocalGlobalMapperNode::initParams() { // map origin is the left lower corner of the voxel map, therefore, adding // an offset make the map centered around the given position - global_map_info_.origin.x = global_map_cx - global_map_dim_d_x_ / 2; - global_map_info_.origin.y = global_map_cy - global_map_dim_d_y_ / 2; - global_map_info_.origin.z = global_map_cz - global_map_dim_d_z_ / 2; + global_map_info_.origin.x = global_map_cx_ - global_map_dim_d_x_ / 2; + global_map_info_.origin.y = global_map_cy_ - global_map_dim_d_y_ / 2; + global_map_info_.origin.z = global_map_cz_ - global_map_dim_d_z_ / 2; global_map_info_.dim.x = static_cast( ceil((global_map_dim_d_x_) / global_map_info_.resolution)); global_map_info_.dim.y = static_cast( @@ -288,9 +288,35 @@ void LocalGlobalMapperNode::processCloud( T_odom_lidar.translation().y(), T_odom_lidar.translation().z()); + const Eigen::Vector3d global_center(global_map_cx_, + global_map_cy_, + global_map_cz_); + ros::Time t0; cpu_times tc; + // Reset global and storage maps if the robot is far from the map centers + const auto diff = lidar_position_odom - global_center; + const auto distance = diff.norm(); + if (distance > std::min(global_map_dim_d_x_, global_map_dim_d_x_) / 2) { + global_map_cx_ = lidar_position_odom(0, 0); + global_map_cy_ = lidar_position_odom(1, 0); + global_map_cz_ = lidar_position_odom(2, 0); + + global_map_info_.origin.x = global_map_cx_ - global_map_dim_d_x_ / 2; + global_map_info_.origin.y = global_map_cy_ - global_map_dim_d_y_ / 2; + global_map_info_.origin.z = global_map_cz_ - global_map_dim_d_z_ / 2; + + // storage map should have same x y z center and x_dim y_dim as global map + storage_map_info_.origin.x = global_map_info_.origin.x; + storage_map_info_.origin.y = global_map_info_.origin.y; + storage_map_info_.origin.z = global_map_info_.origin.z; + + // Initialize maps. + globalMapInit(); + storageMapInit(); + } + double min_range = 0.75; // points within this distance will be discarded double min_range_squared; min_range_squared = min_range * min_range; From 7de7d70b06aeed7fbf2b678ad78034980c6df65a Mon Sep 17 00:00:00 2001 From: David Yan Date: Fri, 10 Feb 2023 14:38:09 -0500 Subject: [PATCH 2/2] Reinitialize maps after odometer reaches a certain distance from map center --- autonomy_core/map_plan/map_plan_launch/config/mapper.yaml | 8 ++++---- autonomy_core/map_plan/mapper/src/local_global_mapper.cpp | 7 +++++-- 2 files changed, 9 insertions(+), 6 deletions(-) diff --git a/autonomy_core/map_plan/map_plan_launch/config/mapper.yaml b/autonomy_core/map_plan/map_plan_launch/config/mapper.yaml index b02e8f19..423dae2b 100644 --- a/autonomy_core/map_plan/map_plan_launch/config/mapper.yaml +++ b/autonomy_core/map_plan/map_plan_launch/config/mapper.yaml @@ -5,8 +5,8 @@ robot_h: 0.25 # robot height, local and global maps will be dilated in z directi global: center_x: 0.0 # center of the map in x-axis center_y: 0.0 # center of the map in y-axis - range_x: 10.0 # range of the map in x-axis, map will span over [center_x - 0.5*range_x, center_x + 0.5*range_x] (high impact on memory and computation usage, recommended value: 500) - range_y: 10.0 # range of the map in y-axis, map will span over [center_y - 0.5*range_y, center_y + 0.5*range_y] (high impact on memory and computation usage, recommended value: 500 + range_x: 200.0 # range of the map in x-axis, map will span over [center_x - 0.5*range_x, center_x + 0.5*range_x] (high impact on memory and computation usage, recommended value: 500) + range_y: 200.0 # range of the map in y-axis, map will span over [center_y - 0.5*range_y, center_y + 0.5*range_y] (high impact on memory and computation usage, recommended value: 500 center_z: 5.0 # center of the map in z-axis, should be similar to the robot take_off_height range_z: 10.0 # range of the map in z-axis, map will span over [center_z - 0.5*range_z, center_z + 0.5*range_z] (high impact on memory and computation usage, recommended value: 10) resolution: 0.5 # resolution of the map (high impact on memory and computation usage, recommended value: 1.0~2.0 for sparse environments, 0.5 for dense environments) @@ -19,8 +19,8 @@ global: local: # local map range z and center_z will be the same as storage map # local planning horizon will be from robot to the local voxel map boundary # center of local map will be the same as robot current odometry along x and y direction, and the same as global map along z direction - range_x: 5.0 # range of the map in x-axis (high impact on memory and computation usage, recommended value: 40) - range_y: 5.0 # range of the map in x-axis (high impact on memory and computation usage, recommended value: 40) + range_x: 50.0 # range of the map in x-axis (high impact on memory and computation usage, recommended value: 40) + range_y: 50.0 # range of the map in x-axis (high impact on memory and computation usage, recommended value: 40) range_z: 12.0 # range of the map in z-axis (recommended value: slightly larger than global map range z to handle overshoot along z-axis upon stopping policy) resolution: 0.25 # resolution of the map (high impact on memory and computation usage, recommended value: 0.5~1.0 for sparse environments, 0.1~0.5 for dense environments) max_raycast_range: 13.0 # max raycast range, the smaller, the less computation demand, but shorter perception range diff --git a/autonomy_core/map_plan/mapper/src/local_global_mapper.cpp b/autonomy_core/map_plan/mapper/src/local_global_mapper.cpp index e2431a07..c7a92c0f 100644 --- a/autonomy_core/map_plan/mapper/src/local_global_mapper.cpp +++ b/autonomy_core/map_plan/mapper/src/local_global_mapper.cpp @@ -298,7 +298,10 @@ void LocalGlobalMapperNode::processCloud( // Reset global and storage maps if the robot is far from the map centers const auto diff = lidar_position_odom - global_center; const auto distance = diff.norm(); - if (distance > std::min(global_map_dim_d_x_, global_map_dim_d_x_) / 2) { + const auto threshold = std::min(global_map_dim_d_x_, global_map_dim_d_x_) / 40; + ROS_INFO("[Mapper]: distance is %.2lf, threshold is %.2lf", distance, threshold); + if (distance > threshold) { + ROS_INFO("[Mapper]: resetting global and storage map"); global_map_cx_ = lidar_position_odom(0, 0); global_map_cy_ = lidar_position_odom(1, 0); global_map_cz_ = lidar_position_odom(2, 0); @@ -312,7 +315,7 @@ void LocalGlobalMapperNode::processCloud( storage_map_info_.origin.y = global_map_info_.origin.y; storage_map_info_.origin.z = global_map_info_.origin.z; - // Initialize maps. + // Reinitialize maps. globalMapInit(); storageMapInit(); }