Skip to content

Commit 6a2a98c

Browse files
MaverickPeterMaverickPeter
committed
add costmap layers for elevation map and point cloud
1 parent 3e9983e commit 6a2a98c

16 files changed

+646
-0
lines changed

‎layers/.vscode/c_cpp_properties.json‎

Lines changed: 23 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,23 @@
1+
{
2+
"configurations": [
3+
{
4+
"browse": {
5+
"databaseFilename": "",
6+
"limitSymbolsToIncludedHeaders": true
7+
},
8+
"includePath": [
9+
"/home/mav-lab/Projects/SLAM/GEM_with_costmap/devel/include/**",
10+
"/opt/ros/kinetic/include/**",
11+
"/home/mav-lab/Projects/SLAM/GEM_with_costmap/src/point_map_layer/include/pointmap_layer/**",
12+
"/usr/include/**"
13+
],
14+
"name": "ROS",
15+
"intelliSenseMode": "gcc-x64",
16+
"compilerPath": "/usr/bin/gcc",
17+
"cStandard": "c11",
18+
"cppStandard": "c++17",
19+
"compileCommands": "${workspaceFolder}/build/compile_commands.json"
20+
}
21+
],
22+
"version": 4
23+
}

‎layers/.vscode/settings.json‎

Lines changed: 6 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,6 @@
1+
{
2+
"files.associations": {
3+
"*.cpp": "cpp",
4+
"random": "cpp"
5+
}
6+
}

‎layers/CMakeLists.txt‎

Lines changed: 62 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,62 @@
1+
cmake_minimum_required(VERSION 3.0.2)
2+
project(pointMap_layer)
3+
4+
## Compile as C++11, supported in ROS Kinetic and newer
5+
# add_compile_options(-std=c++11)
6+
7+
## Find catkin macros and libraries
8+
## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
9+
## is used, also find other catkin packages
10+
find_package(catkin REQUIRED COMPONENTS
11+
dynamic_reconfigure roscpp costmap_2d
12+
grid_map_core grid_map_ros grid_map_msgs rqt_gui rqt_gui_py
13+
rviz
14+
)
15+
16+
catkin_package(
17+
# INCLUDE_DIRS include
18+
# LIBRARIES simple_layers
19+
# CATKIN_DEPENDS costmap_2d dynamic_reconfigure roscpp
20+
# DEPENDS system_lib
21+
)
22+
23+
###########
24+
## Build ##
25+
###########
26+
27+
## Specify additional locations of header files
28+
## Your package locations should be listed before other locations
29+
include_directories(
30+
include
31+
${catkin_INCLUDE_DIRS}
32+
)
33+
34+
## Declare a C++ library
35+
add_library(pointMap_layer
36+
src/pointMap_layer.cpp
37+
)
38+
target_link_libraries(pointMap_layer ${catkin_LIBRARIES})
39+
target_compile_options(pointMap_layer PRIVATE -std=c++11)
40+
41+
42+
add_library(elevationMap_layer
43+
src/elevationMap_layer.cpp)
44+
target_link_libraries(elevationMap_layer ${catkin_LIBRARIES})
45+
target_compile_options(elevationMap_layer PRIVATE -std=c++11)
46+
47+
#############
48+
## Install ##
49+
#############
50+
install(FILES
51+
costmap_plugins.xml
52+
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
53+
)
54+
55+
install(
56+
TARGETS
57+
pointMap_layer
58+
elevationMap_layer
59+
RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
60+
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
61+
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
62+
)

‎layers/costmap_plugins.xml‎

Lines changed: 15 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,15 @@
1+
<class_libraries>
2+
3+
<library path="lib/libpointMap_layer">
4+
<class type="costmap_2d::PointMapLayer" base_class_type="costmap_2d::Layer">
5+
<description>Pointmap Layer that adds point cloud to costmap</description>
6+
</class>
7+
</library>
8+
9+
<library path="lib/libelevationMap_layer">
10+
<class type="costmap_2d::ElevationMapLayer" base_class_type="costmap_2d::Layer">
11+
<description>Elevation map layer</description>
12+
</class>
13+
</library>
14+
15+
</class_libraries>

‎layers/include/elevationMap_layer/elevationMap_layer.h‎

Lines changed: 39 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,39 @@
1+
#ifndef ELEVATION_MAP_LAYER_H_
2+
#define ELEVATION_MAP_LAYER_H_
3+
4+
// grid map
5+
#include <grid_map_msgs/GridMap.h>
6+
#include <grid_map_ros/grid_map_ros.hpp>
7+
8+
// costmap
9+
#include <costmap_2d/obstacle_layer.h>
10+
#include <costmap_2d/GenericPluginConfig.h>
11+
12+
namespace costmap_2d
13+
{
14+
15+
class ElevationMapLayer : public ObstacleLayer
16+
{
17+
public:
18+
ElevationMapLayer();
19+
20+
virtual ~ElevationMapLayer();
21+
22+
virtual void onInitialize();
23+
24+
virtual void updateBounds(double robot_x, double robot_y, double robot_yaw,
25+
double* min_x, double* min_y,
26+
double* max_x, double* max_y);
27+
28+
private:
29+
void elevationMapCB(const grid_map_msgs::GridMapConstPtr& msg);
30+
31+
ros::Subscriber elevation_map_sub_;
32+
grid_map::GridMap elevation_map_;
33+
34+
bool elevation_map_available_;
35+
36+
};
37+
38+
}
39+
#endif

‎layers/include/pointMap_layer/PointXYZRGBIC.hpp‎

Lines changed: 61 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,61 @@
1+
/*
2+
* PointXYZRGBIC.hpp
3+
*
4+
* Created on: Dec 1, 2020
5+
* Author: Peter XU
6+
* Institute: ZJU, Robotics 104
7+
*/
8+
9+
#define PCL_NO_PRECOMPILE
10+
11+
#include <pcl/pcl_macros.h>
12+
#include <pcl/point_cloud.h>
13+
#include <pcl/point_types.h>
14+
#include <pcl/impl/instantiate.hpp>
15+
16+
#include <pcl/filters/voxel_grid.h>
17+
#include <pcl/filters/impl/voxel_grid.hpp>
18+
#include <pcl/filters/passthrough.h>
19+
#include <pcl/filters/impl/passthrough.hpp>
20+
#include <pcl/kdtree/kdtree.h>
21+
#include <pcl/kdtree/kdtree_flann.h>
22+
#include <pcl/kdtree/impl/kdtree_flann.hpp>
23+
#include <pcl/search/impl/kdtree.hpp>
24+
#include <boost/shared_ptr.hpp>
25+
26+
struct PointXYZRGBIC
27+
{
28+
PCL_ADD_POINT4D;
29+
PCL_ADD_RGB;
30+
31+
float covariance;
32+
float intensity;
33+
34+
PointXYZRGBIC() {}
35+
PointXYZRGBIC(const PointXYZRGBIC& input)
36+
{
37+
this->x = input.x;
38+
this->y = input.y;
39+
this->z = input.z;
40+
this->rgb = input.rgb;
41+
this->intensity = input.intensity;
42+
this->covariance = input.covariance;
43+
}
44+
45+
EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
46+
} EIGEN_ALIGN16;
47+
48+
POINT_CLOUD_REGISTER_POINT_STRUCT (PointXYZRGBIC,
49+
(float, x, x)
50+
(float, y, y)
51+
(float, z, z)
52+
(float, rgb, rgb)
53+
(float, intensity, intensity)
54+
(float, covariance, covariance)
55+
);
56+
57+
PCL_INSTANTIATE(VoxelGrid, PointXYZRGBIC);
58+
PCL_INSTANTIATE(KdTree, PointXYZRGBIC);
59+
PCL_INSTANTIATE(KdTreeFLANN, PointXYZRGBIC);
60+
61+
typedef pcl::PointCloud<PointXYZRGBIC> PointCloudXYZRGBIC;

‎layers/include/pointMap_layer/PointXYZRGBICO.hpp‎

Lines changed: 64 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,64 @@
1+
/*
2+
* PointXYZRGBICO.hpp
3+
*
4+
* Created on: July 1, 2021
5+
* Author: Peter XU
6+
* Institute: ZJU, Robotics 104
7+
*/
8+
9+
#define PCL_NO_PRECOMPILE
10+
11+
#include <pcl/pcl_macros.h>
12+
#include <pcl/point_cloud.h>
13+
#include <pcl/point_types.h>
14+
#include <pcl/impl/instantiate.hpp>
15+
16+
#include <pcl/filters/voxel_grid.h>
17+
#include <pcl/filters/impl/voxel_grid.hpp>
18+
#include <pcl/filters/passthrough.h>
19+
#include <pcl/filters/impl/passthrough.hpp>
20+
#include <pcl/kdtree/kdtree.h>
21+
#include <pcl/kdtree/kdtree_flann.h>
22+
#include <pcl/kdtree/impl/kdtree_flann.hpp>
23+
#include <pcl/search/impl/kdtree.hpp>
24+
#include <boost/shared_ptr.hpp>
25+
26+
struct PointXYZRGBICO
27+
{
28+
PCL_ADD_POINT4D;
29+
PCL_ADD_RGB;
30+
31+
float covariance;
32+
float intensity;
33+
float obstacle;
34+
35+
PointXYZRGBICO() {}
36+
PointXYZRGBICO(const PointXYZRGBICO& input)
37+
{
38+
this->x = input.x;
39+
this->y = input.y;
40+
this->z = input.z;
41+
this->rgb = input.rgb;
42+
this->intensity = input.intensity;
43+
this->covariance = input.covariance;
44+
this->obstacle = input.obstacle;
45+
}
46+
47+
EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
48+
} EIGEN_ALIGN16;
49+
50+
POINT_CLOUD_REGISTER_POINT_STRUCT (PointXYZRGBICO,
51+
(float, x, x)
52+
(float, y, y)
53+
(float, z, z)
54+
(float, rgb, rgb)
55+
(float, intensity, intensity)
56+
(float, covariance, covariance)
57+
(float, obstacle, obstacle)
58+
);
59+
60+
PCL_INSTANTIATE(VoxelGrid, PointXYZRGBICO);
61+
PCL_INSTANTIATE(KdTree, PointXYZRGBICO);
62+
PCL_INSTANTIATE(KdTreeFLANN, PointXYZRGBICO);
63+
64+
typedef pcl::PointCloud<PointXYZRGBICO> PointCloudXYZRGBICO;

‎layers/include/pointMap_layer/pointMap_layer.h‎

Lines changed: 44 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,44 @@
1+
#ifndef POINTMAP_LAYER_H_
2+
#define POINTMAP_LAYER_H_
3+
#include <ros/ros.h>
4+
5+
// Costmap
6+
#include <costmap_2d/obstacle_layer.h>
7+
#include <costmap_2d/GenericPluginConfig.h>
8+
#include <dynamic_reconfigure/server.h>
9+
#include <nav_msgs/OccupancyGrid.h>
10+
11+
// PCL
12+
#include <pcl/PCLPointCloud2.h>
13+
#include <pcl_conversions/pcl_conversions.h>
14+
#include <pointMap_layer/PointXYZRGBICO.hpp>
15+
16+
17+
using namespace pcl;
18+
19+
namespace costmap_2d
20+
{
21+
22+
typedef PointXYZRGBICO Anypoint;
23+
typedef pcl::PointCloud<Anypoint> pointCloud;
24+
25+
class PointMapLayer : public ObstacleLayer
26+
{
27+
public:
28+
PointMapLayer();
29+
virtual ~PointMapLayer();
30+
31+
virtual void onInitialize();
32+
virtual void updateBounds(double robot_x, double robot_y, double robot_yaw, double* min_x, double* min_y, double* max_x,
33+
double* max_y);
34+
35+
private:
36+
void pointMapCB(const sensor_msgs::PointCloud2& msg);
37+
38+
double mark_x_, mark_y_;
39+
pointCloud ob_pointCloud;
40+
ros::Subscriber point_map_sub_;
41+
42+
};
43+
}
44+
#endif

‎layers/launch/create_globalmap.launch‎

Lines changed: 14 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,14 @@
1+
<launch>
2+
3+
<param name="/use_sim_time" value="true" />
4+
5+
<node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
6+
7+
<rosparam file="$(find pointMap_layer)/params/costmap_common_params_global.yaml" command="load" ns="global_costmap" />
8+
<rosparam file="$(find pointMap_layer)/params/costmap_common_params_local.yaml" command="load" ns="local_costmap" />
9+
<rosparam file="$(find pointMap_layer)/params/local_costmap_params.yaml" command="load" />
10+
<rosparam file="$(find pointMap_layer)/params/global_costmap_params.yaml" command="load" />
11+
12+
</node>
13+
</launch>
14+

0 commit comments

Comments
 (0)