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..423dae2b 100644 --- a/autonomy_core/map_plan/map_plan_launch/config/mapper.yaml +++ b/autonomy_core/map_plan/map_plan_launch/config/mapper.yaml @@ -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: 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/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..c7a92c0f 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,38 @@ 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(); + 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); + + 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; + + // Reinitialize 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;