Local Costmap Generator Node (ROS, C++)
Generate Cost Map using Point Cloud Height Map algorithm
After building this ROS package using 'catkin_make' in your catkin workspace, launch the heightmap_node with heightmap_costmap_node:
roslaunch local_costmap_generator run.launch
TODO: find and change the topic name of the point cloud data
|
// subscribe to Velodyne data points |
|
// TODO: find and change the topic name of the point cloud data |
|
velodyne_scan_ = node.subscribe("/points", 10, |
|
&HeightMap::processData, this, |
|
ros::TransportHints().tcpNoDelay(true)); |
TODO: convert the position of the points from the camera to the local coordinate
|
// TODO: convert position of points from camera to local coordinate |
|
// - camera coordinate (x,y,z): scan->points[i].x, scan->points[i].y, scan->points[i].z |
|
// - local coordinate (x,y,z): local_x, local_y, local_z |
|
double local_x = scan->points[i].x; // currently, wrong transform. you need to fix this! |
|
double local_y = scan->points[i].y; // currently, wrong transform. you need to fix this! |
|
double local_z = scan->points[i].z; // currently, wrong transform. you need to fix this! |
|
// TODO: convert position of points from camera to local coordinate |
|
// - camera coordinate (x,y,z): scan->points[i].x, scan->points[i].y, scan->points[i].z |
|
// - local coordinate (x,y,z): local_x, local_y, local_z |
|
double local_x = scan->points[i].x; // currently, wrong transform. you need to fix this! |
|
double local_y = scan->points[i].y; // currently, wrong transform. you need to fix this! |
|
double local_z = scan->points[i].z; // currently, wrong transform. you need to fix this! |
|
// TODO: convert position of points from camera to local coordinate |
|
// - camera coordinate (x,y,z): scan->points[i].x, scan->points[i].y, scan->points[i].z |
|
// - local coordinate (x,y,z): local_x, local_y, local_z |
|
double local_x = scan->points[i].x; // currently, wrong transform. you need to fix this! |
|
double local_y = scan->points[i].y; // currently, wrong transform. you need to fix this! |
|
double local_z = scan->points[i].z; // currently, wrong transform. you need to fix this! |
|
// TODO: convert position of points from camera to local coordinate |
|
// - camera coordinate (x,y,z): scan->points[i].x, scan->points[i].y, scan->points[i].z |
|
// - local coordinate (x,y,z): local_x, local_y, local_z |
|
double local_x = scan->points[i].x; // currently, wrong transform. you need to fix this! |
|
double local_y = scan->points[i].y; // currently, wrong transform. you need to fix this! |
Parameters (in heightmap.cpp)
|
priv_nh.param("cell_size", m_per_cell_, 0.1); // [m / cell] |
|
priv_nh.param("full_clouds", full_clouds_, false); |
|
priv_nh.param("grid_dimensions", grid_dim_, 200); // [cell] size of map; 200 cell = 20 [m] / 0.1 [m/cell]; 20 is calculated from MAP_MAX_X - MAP_MIN_X at 'heightmap_to_costmap.cpp' |
|
priv_nh.param("height_threshold", height_diff_threshold_, 0.25); // [m] |
Parameters (in heightmap_to_costmap.cpp)
|
bool DO_INFLATION = true; // true |
|
float RESOLUTION_ = 0.1; // [m / cell] |
|
float MAP_MIN_X = -5; // map min x position |
|
float MAP_MAX_X = 15; // map max x position |
|
float MAP_MIN_Y = -10; // map min y position |
|
float MAP_MAX_Y = 10; // map max y position |
|
|
|
float INFLATION_RADIUS = 0.2; // [m] size of inflation |
|
float INFLATION_RES = RESOLUTION_; // [m] resolution of inflation |
|
int INFLATION_BINS = INFLATION_RADIUS / INFLATION_RES; |
- /points <-- change to correct topic name
- /points/velodyne_obstacles
- /points/velodyne_clear
- /map/local_map/obstacle
React
Typescript
Django
Laravel
Facebook
Microsoft
Google
Alibaba
D3
Tencent