Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Reinitialize global and storage maps with odometery #187

Open
wants to merge 2 commits into
base: master
Choose a base branch
from
Open

Reinitialize global and storage maps with odometery #187

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
ccbd01c
Update global and storage map origins; Need to test
david-yan Feb 10, 2023
7de7d70
Reinitialize maps after odometer reaches a certain distance from map …
david-yan Feb 10, 2023
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
4 changes: 2 additions & 2 deletions autonomy_core/map_plan/map_plan_launch/config/mapper.yaml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Please revert to default values (25)

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
Expand Down
1 change: 1 addition & 0 deletions autonomy_core/map_plan/mapper/include/mapper/local_global_mapper.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
43 changes: 36 additions & 7 deletions autonomy_core/map_plan/mapper/src/local_global_mapper.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand All @@ -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<int>(
ceil((global_map_dim_d_x_) / global_map_info_.resolution));
global_map_info_.dim.y = static_cast<int>(
Expand Down Expand Up @@ -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;
Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Dividing by 40 will make the update small. Maybe we should make this threshold a parameter in the mapper YAML file, maybe name it something like map_reinit_distance, and make the default value -1 (which means no reinitialization at all).

ROS_INFO("[Mapper]: distance is %.2lf, threshold is %.2lf", distance, threshold);
if (distance > threshold) {
ROS_INFO("[Mapper]: resetting global and storage map");
Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Please add a TODO here stating that we need to publish a stopping policy state trigger

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;
Expand Down