├── README.md
├── package.xml
├── CMakeLists.txt
└── src
└── curb_detec.cpp
/README.md:
--------------------------------------------------------------------------------
1 | # 3DLidar_curb_detection
2 | 1- download the rosbag for the VELODYNE 3D lidar
3 | [click here](http://db3.ertl.jp/autoware/sample_data/sample_moriyama_150324.tar.gz)
4 |
5 | ```Bash
6 | $ cd
7 | $ cp ~/Downloads/sample_moriyama_* .
8 | $ tar zxfv sample_moriyama_150324.tar.gz
9 |
10 | ```
11 |
12 | 2- clone the repo into your workspace
13 |
14 | ```Bash
15 | $ cd
16 | $ cd catkin_ws/src/
17 | $ git clone https://github.com/SohaibAl-emara/3D_Lidar_Curb_Detection.git
18 | ```
19 |
20 | 3- build the code
21 | ```Bash
22 | $ cd
23 | $ cd catkin_ws/
24 | $ catkin_make
25 | ```
26 | 4- run rosmaster
27 | ```BASH
28 | $ roscore
29 | ```
30 |
31 | 5- in a different terminal, play the rosbag
32 | ```BASH
33 | $ cd
34 | $ rosbag play sample_moriyama_150324.bag
35 | ```
36 |
37 | 6- in a different terminal, open rviz
38 | ```
39 | $ rosrun rviz rviz -f velodyne
40 | ```
41 |
42 | 7- in a different terminal, run the curb detection node
43 | ```Bash
44 | $ rosrun curb_detec_cpp curb_detec
45 | ```
46 |
47 | 8- setup rviz to see the pointcloud2 data
48 |
49 | 
50 |
51 | this will show you the curbs detection only,
52 |
53 | if you want to see the lidar data, do the same thing again add a new pointcloud2 data, the topic name is /points_raw
54 | keep the size of the points smaller and than the curb detection points, and make them different color. This way you can see the different between the point clouds easily.
55 |
--------------------------------------------------------------------------------
/package.xml:
--------------------------------------------------------------------------------
1 |
2 |
3 | curb_detec_cpp
4 | 0.0.0
5 | The curb_detec_cpp package
6 |
7 |
8 |
9 |
10 | xh
11 |
12 |
13 |
14 |
15 |
16 | TODO
17 |
18 |
19 |
20 |
21 |
22 |
23 |
24 |
25 |
26 |
27 |
28 |
29 |
30 |
31 |
32 |
33 |
34 |
35 |
36 |
37 |
38 |
39 |
40 |
41 |
42 |
43 |
44 |
45 |
46 |
47 |
48 |
49 |
50 |
51 | catkin
52 | roscpp
53 | rospy
54 | std_msgs
55 | libpcl-all-dev
56 | libpcl-all
57 | roscpp
58 | rospy
59 | std_msgs
60 | roscpp
61 | rospy
62 | std_msgs
63 | libpcl-all-dev
64 | libpcl-all
65 |
66 |
67 |
68 |
69 |
70 |
71 |
72 |
73 |
--------------------------------------------------------------------------------
/CMakeLists.txt:
--------------------------------------------------------------------------------
1 | cmake_minimum_required(VERSION 2.8.3)
2 | project(curb_detec_cpp)
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 | roscpp
12 | rospy
13 | std_msgs
14 | pcl_conversions
15 | pcl_ros
16 | )
17 | ## System dependencies are found with CMake's conventions
18 | # find_package(Boost REQUIRED COMPONENTS system)
19 |
20 |
21 | ## Uncomment this if the package has a setup.py. This macro ensures
22 | ## modules and global scripts declared therein get installed
23 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
24 | # catkin_python_setup()
25 |
26 | ################################################
27 | ## Declare ROS messages, services and actions ##
28 | ################################################
29 |
30 | ## To declare and build messages, services or actions from within this
31 | ## package, follow these steps:
32 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in
33 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
34 | ## * In the file package.xml:
35 | ## * add a build_depend tag for "message_generation"
36 | ## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET
37 | ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in
38 | ## but can be declared for certainty nonetheless:
39 | ## * add a exec_depend tag for "message_runtime"
40 | ## * In this file (CMakeLists.txt):
41 | ## * add "message_generation" and every package in MSG_DEP_SET to
42 | ## find_package(catkin REQUIRED COMPONENTS ...)
43 | ## * add "message_runtime" and every package in MSG_DEP_SET to
44 | ## catkin_package(CATKIN_DEPENDS ...)
45 | ## * uncomment the add_*_files sections below as needed
46 | ## and list every .msg/.srv/.action file to be processed
47 | ## * uncomment the generate_messages entry below
48 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
49 |
50 | ## Generate messages in the 'msg' folder
51 | # add_message_files(
52 | # FILES
53 | # Message1.msg
54 | # Message2.msg
55 | # )
56 |
57 | ## Generate services in the 'srv' folder
58 | # add_service_files(
59 | # FILES
60 | # Service1.srv
61 | # Service2.srv
62 | # )
63 |
64 | ## Generate actions in the 'action' folder
65 | # add_action_files(
66 | # FILES
67 | # Action1.action
68 | # Action2.action
69 | # )
70 |
71 | ## Generate added messages and services with any dependencies listed here
72 | # generate_messages(
73 | # DEPENDENCIES
74 | # std_msgs
75 | # )
76 |
77 | ################################################
78 | ## Declare ROS dynamic reconfigure parameters ##
79 | ################################################
80 |
81 | ## To declare and build dynamic reconfigure parameters within this
82 | ## package, follow these steps:
83 | ## * In the file package.xml:
84 | ## * add a build_depend and a exec_depend tag for "dynamic_reconfigure"
85 | ## * In this file (CMakeLists.txt):
86 | ## * add "dynamic_reconfigure" to
87 | ## find_package(catkin REQUIRED COMPONENTS ...)
88 | ## * uncomment the "generate_dynamic_reconfigure_options" section below
89 | ## and list every .cfg file to be processed
90 |
91 | ## Generate dynamic reconfigure parameters in the 'cfg' folder
92 | # generate_dynamic_reconfigure_options(
93 | # cfg/DynReconf1.cfg
94 | # cfg/DynReconf2.cfg
95 | # )
96 |
97 | ###################################
98 | ## catkin specific configuration ##
99 | ###################################
100 | ## The catkin_package macro generates cmake config files for your package
101 | ## Declare things to be passed to dependent projects
102 | ## INCLUDE_DIRS: uncomment this if your package contains header files
103 | ## LIBRARIES: libraries you create in this project that dependent projects also need
104 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need
105 | ## DEPENDS: system dependencies of this project that dependent projects also need
106 | catkin_package(
107 | # INCLUDE_DIRS include
108 | # LIBRARIES curb_detec_cpp
109 | # CATKIN_DEPENDS roscpp rospy std_msgs
110 | # DEPENDS system_lib
111 | )
112 |
113 | ###########
114 | ## Build ##
115 | ###########
116 |
117 | ## Specify additional locations of header files
118 | ## Your package locations should be listed before other locations
119 | include_directories(include ${catkin_INCLUDE_DIRS})
120 |
121 | ## Declare a C++ library
122 | # add_library(${PROJECT_NAME}
123 | # src/${PROJECT_NAME}/curb_detec_cpp.cpp
124 | # )
125 |
126 | ## Add cmake target dependencies of the library
127 | ## as an example, code may need to be generated before libraries
128 | ## either from message generation or dynamic reconfigure
129 | # add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
130 |
131 | ## Declare a C++ executable
132 | ## With catkin_make all packages are built within a single CMake context
133 | ## The recommended prefix ensures that target names across packages don't collide
134 | add_executable(curb_detec src/curb_detec.cpp)
135 |
136 | ## Rename C++ executable without prefix
137 | ## The above recommended prefix causes long target names, the following renames the
138 | ## target back to the shorter version for ease of user use
139 | ## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
140 | # set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")
141 |
142 | ## Add cmake target dependencies of the executable
143 | ## same as for the library above
144 | # add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
145 |
146 | ## Specify libraries to link a library or executable target against
147 | target_link_libraries(curb_detec ${catkin_LIBRARIES})
148 |
149 | #############
150 | ## Install ##
151 | #############
152 |
153 | # all install targets should use catkin DESTINATION variables
154 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
155 |
156 | ## Mark executable scripts (Python etc.) for installation
157 | ## in contrast to setup.py, you can choose the destination
158 | # install(PROGRAMS
159 | # scripts/my_python_script
160 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
161 | # )
162 |
163 | ## Mark executables and/or libraries for installation
164 | # install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node
165 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
166 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
167 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
168 | # )
169 |
170 | ## Mark cpp header files for installation
171 | # install(DIRECTORY include/${PROJECT_NAME}/
172 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
173 | # FILES_MATCHING PATTERN "*.h"
174 | # PATTERN ".svn" EXCLUDE
175 | # )
176 |
177 | ## Mark other files for installation (e.g. launch and bag files, etc.)
178 | # install(FILES
179 | # # myfile1
180 | # # myfile2
181 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
182 | # )
183 |
184 | #############
185 | ## Testing ##
186 | #############
187 |
188 | ## Add gtest based cpp test target and link libraries
189 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_curb_detec_cpp.cpp)
190 | # if(TARGET ${PROJECT_NAME}-test)
191 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
192 | # endif()
193 |
194 | ## Add folders to be run by python nosetests
195 | # catkin_add_nosetests(test)
196 |
--------------------------------------------------------------------------------
/src/curb_detec.cpp:
--------------------------------------------------------------------------------
1 | #include
2 | #include "ros/ros.h"
3 | #include
4 | #include
5 | #include
6 | #include
7 | #include
8 | #include
9 | using namespace std;
10 |
11 |
12 | // Help sort point clouds.
13 | // Ascending order. (For points where y is greater than 0)
14 |
15 | bool comp_up(const pcl::PointXYZ &a, const pcl::PointXYZ &b)
16 | {
17 | return a.y < b.y;
18 | }
19 |
20 | // Descending. (For points where y is less than 0)
21 | bool comp_down(const pcl::PointXYZ &a, const pcl::PointXYZ &b)
22 | {
23 | return a.y > b.y;
24 | }
25 |
26 | // This class is used to detect curb.
27 | // Input a point cloud and return a point cloud consisting of detected curb points.
28 | // The main execution function is detection.
29 | class curbDetector
30 | {
31 | public:
32 | curbDetector(){}
33 |
34 | pcl::PointCloud detector(const std::vector > input)
35 | {
36 | pc_in = input;
37 | pcl::PointCloud look_test;
38 |
39 |
40 | // Process and detect each layer. Since we took 10 layers of 64 lidar here, the number of layers here is 10.
41 | for (int i = 0; i < 10; i++)
42 | {
43 | pcl::PointCloud pointsInTheRing = pc_in[i]; // Save the points on this line.
44 | pcl::PointCloud pc_left; // Store the point where y is greater than 0 (the left point).
45 | pcl::PointCloud pc_right; // Store the point where y is less than 0 (the right point).
46 |
47 | pcl::PointCloud cleaned_pc_left;
48 | pcl::PointCloud cleaned_pc_right;
49 |
50 | pcl::PointXYZ point;
51 | size_t numOfPointsInTheRing = pointsInTheRing.size();
52 |
53 | // Separate the left and right points and save them to the corresponding point cloud.
54 | for (int idx = 0; idx < numOfPointsInTheRing; idx++)
55 | {
56 | point = pointsInTheRing[idx];
57 | if (point.y >= 0)
58 | {pc_left.push_back(point);}
59 | else
60 | {pc_right.push_back(point);}
61 | }
62 |
63 | // Sort. (In ascending order of absolute value)
64 | sort(pc_left.begin(), pc_left.end(), comp_up);
65 | sort(pc_right.begin(), pc_right.end(), comp_down);
66 |
67 |
68 | slideForGettingPoints(pc_left, true);
69 | slideForGettingPoints(pc_right, false);
70 |
71 | }
72 | return curb_left + curb_right;
73 |
74 | }
75 |
76 | int slideForGettingPoints(pcl::PointCloud points, bool isLeftLine)
77 | {
78 | int w_0 = 10;
79 | int w_d = 30;
80 | int i = 0;
81 |
82 | // some important parameters influence the final performance.
83 | float xy_thresh = 0.1;
84 | float z_thresh = 0.06;
85 |
86 | int points_num = points.size();
87 |
88 | while((i + w_d) < points_num)
89 | {
90 | float z_max = points[i].z;
91 | float z_min = points[i].z;
92 |
93 | int idx_ = 0;
94 | float z_dis = 0;
95 |
96 | for (int i_ = 0; i_ < w_d; i_++)
97 | {
98 | float dis = fabs(points[i+i_].z - points[i+i_+1].z);
99 | if (dis > z_dis) {z_dis = dis; idx_ = i+i_;}
100 | if (points[i+i_].z < z_min){z_min = points[i+i_].z;}
101 | if (points[i+i_].z > z_max){z_max = points[i+i_].z;}
102 | }
103 |
104 | if (fabs(z_max - z_min) >= z_thresh)
105 | {
106 | for (int i_ = 0; i_ < (w_d - 1); i_++)
107 | {
108 | float p_dist = sqrt(((points[i + i_].y - points[i + 1 + i_].y) * (points[i + i_].y - points[i + 1 + i_].y))
109 | + ((points[i + i_].x - points[i + 1 + i_].x) *(points[i + i_].x - points[i + 1 + i_].x)));
110 | if (p_dist >= xy_thresh)
111 | {
112 | if (isLeftLine) {curb_left.push_back(points[i_ + i]);return 0;}
113 | else {curb_right.push_back(points[i_ + i]);return 0;}
114 | }
115 | }
116 | if (isLeftLine) {curb_left.push_back(points[idx_]);return 0;}
117 | else {curb_right.push_back(points[idx_]);return 0;}
118 | }
119 | i += w_0;
120 | }
121 | }
122 |
123 | private:
124 | std::vector > pc_in;
125 | pcl::PointCloud curb_left;
126 | pcl::PointCloud curb_right;
127 | };
128 |
129 |
130 | class rosTransPoints
131 | {
132 | public:
133 | rosTransPoints()
134 | {
135 | //Set the parameters of the area of interest. The four parameters set the rectangle of this region of interest.
136 | // X is used to define the distance of the detection infront of the car.
137 | // Y is used to define the distance from the car to the curb.
138 | // x_up means infront of the car. x_down behid the car.
139 | // y_up means how far from the middle of the car to the left
140 | // y_down means how far from the middle of the car to the right
141 | // z, i am not very sure about it, but it is most likely the hight of the curb
142 | // ring_idx is the layer id. The lidar that they are using has 64 layers, and they selected 10 layers for detection.
143 | x_range_up = 30;
144 | x_range_down = 0;
145 | y_range_up = 20;
146 | y_range_down = -10;
147 | z_range_down = -1.5;
148 | int ring_idx_[10] = {25, 27, 30, 32, 35, 38, 40, 41, 42, 43};
149 | memcpy(ring_idx, ring_idx_, sizeof(ring_idx_));
150 |
151 | // define the publisher and the subscriber
152 | pub = nh.advertise("/curb_detection_result", 1);
153 | sub = nh.subscribe("/points_raw",3,&rosTransPoints::call_back,this);
154 | }
155 |
156 | void call_back(const boost::shared_ptr& input)
157 | {
158 | // this fuction is the callback for the subscriber, it gets excuted everytime a point cloud data is obtained through topic /points_raw
159 | clock_t startTime,endTime;
160 | startTime = ros::Time::now().toNSec();
161 | curbDetector cd;
162 |
163 | pcl::fromROSMsg(*input, cloud_in);
164 |
165 | // Here to add the code for processing the pc(pointcloud).
166 | cloud_cleaned = cleanPoints(cloud_in);
167 | cloud_out = cd.detector(cloud_cleaned);
168 |
169 | pcl::toROSMsg(cloud_out, cloud_final);
170 | cloud_final.header.stamp = ros::Time::now();
171 | cloud_final.header.frame_id = "velodyne";
172 | pub.publish (cloud_final);
173 |
174 | endTime = ros::Time::now().toNSec();
175 | cout << "The run time is:" << (double)(endTime - startTime) / 10e6 << "ms" << endl;
176 | }
177 |
178 | int hiPoints_WhereAreYouFrom(pcl::PointXYZ p)
179 | {
180 | // find which to which layer the point belongs
181 | // refer to this code to understand it：https://github.com/luhongquan66/loam_velodyne/blob/master/src/lib/MultiScanRegistration.cpp
182 | double angle;
183 | int scanID;
184 | angle = atan(p.z / sqrt(p.x * p.x + p.y * p.y));
185 | // double test = sqrt(p.x * p.x + p.y * p.y);
186 | // cout << test << endl;
187 | scanID = (int)(angle * 134.18714161056457 + 58.81598513011153);
188 |
189 | return scanID;
190 | }
191 |
192 | std::vector > cleanPoints(pcl::PointCloud pc)
193 | {
194 | // this function cleans the points, this way you can have only the 10 layers we want
195 | // 1. gets rid of unwanted layers
196 | // 2. takes the points according to the layer
197 | // 3. save the new layers in a new pointcloud mesage and return them
198 | size_t cloudSize = pc.size();
199 | // size_t ringSize;
200 | pcl::PointXYZ point;
201 | int scanID_;
202 | // pcl::PointCloud _laserCloud;
203 | std::vector > laserCloudScans(10);
204 |
205 | for (int i = 0; i < cloudSize; i++)
206 | {
207 | point.x = pc[i].x;
208 | point.y = pc[i].y;
209 | point.z = pc[i].z;
210 |
211 | if (!pcl_isfinite(point.x) ||
212 | !pcl_isfinite(point.y) ||
213 | !pcl_isfinite(point.z))
214 | {continue;}
215 | if ((point.x < x_range_down) || (point.x > x_range_up) || (point.y < y_range_down) || (point.y > y_range_up) || (point.z > z_range_down))
216 | {continue;}
217 |
218 | scanID_ = hiPoints_WhereAreYouFrom(point);
219 |
220 | for (int ring_num = 0;ring_num < 10; ring_num++)
221 | {
222 | if (scanID_ == ring_idx[ring_num])
223 | {laserCloudScans[ring_num].push_back(point);}
224 | }
225 | }
226 |
227 | return laserCloudScans;
228 | }
229 |
230 | private:
231 | ros::NodeHandle nh;
232 | ros::Publisher pub;
233 | ros::Subscriber sub;
234 | pcl::PointCloud cloud_in;
235 | pcl::PointCloud cloud_out;
236 | std::vector > cloud_cleaned;
237 | sensor_msgs::PointCloud2 cloud_final;
238 |
239 | // parameters help to select the detection scope.
240 | int x_range_up;
241 | int x_range_down;
242 | int y_range_up;
243 | int y_range_down;
244 | float z_range_down;
245 | int ring_idx[10];
246 |
247 | };
248 |
249 |
250 | int main(int argc, char **argv)
251 | {
252 | ros::init(argc, argv, "curb_detector");
253 | rosTransPoints start_detec;
254 | ros::spin();
255 | }
256 |
257 |
--------------------------------------------------------------------------------