├── .gitmodules ├── CMakeLists.txt ├── README.md ├── launch └── pandora_projection.launch ├── package.xml └── src └── pandora_projection.cc /.gitmodules: -------------------------------------------------------------------------------- 1 | [submodule "src/Pandora_Apollo"] 2 | path = src/Pandora_Apollo 3 | url = https://github.com/HesaiTechnology/Pandora_Apollo.git 4 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(pandora_projection) 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 | 11 | 12 | find_package(catkin REQUIRED COMPONENTS 13 | cv_bridge 14 | image_transport 15 | pcl_ros 16 | roscpp 17 | sensor_msgs 18 | std_msgs 19 | pcl_conversions 20 | ) 21 | 22 | catkin_package( 23 | CATKIN_DEPENDS std_msgs 24 | ) 25 | 26 | 27 | INCLUDE_DIRECTORIES( 28 | ./src/Pandora_Apollo/src/Pandar40P/include 29 | ./src/Pandora_Apollo/include 30 | ./src 31 | ) 32 | 33 | add_subdirectory(src/Pandora_Apollo) 34 | 35 | add_executable(pandora_projection_node 36 | src/pandora_projection.cc 37 | ) 38 | 39 | 40 | 41 | 42 | target_link_libraries(pandora_projection_node 43 | ${catkin_LIBRARIES} 44 | Pandora 45 | ) 46 | 47 | ## System dependencies are found with CMake's conventions 48 | # find_package(Boost REQUIRED COMPONENTS system) 49 | 50 | 51 | ## Uncomment this if the package has a setup.py. This macro ensures 52 | ## modules and global scripts declared therein get installed 53 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 54 | # catkin_python_setup() 55 | 56 | ################################################ 57 | ## Declare ROS messages, services and actions ## 58 | ################################################ 59 | 60 | ## To declare and build messages, services or actions from within this 61 | ## package, follow these steps: 62 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 63 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 64 | ## * In the file package.xml: 65 | ## * add a build_depend tag for "message_generation" 66 | ## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET 67 | ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in 68 | ## but can be declared for certainty nonetheless: 69 | ## * add a run_depend tag for "message_runtime" 70 | ## * In this file (CMakeLists.txt): 71 | ## * add "message_generation" and every package in MSG_DEP_SET to 72 | ## find_package(catkin REQUIRED COMPONENTS ...) 73 | ## * add "message_runtime" and every package in MSG_DEP_SET to 74 | ## catkin_package(CATKIN_DEPENDS ...) 75 | ## * uncomment the add_*_files sections below as needed 76 | ## and list every .msg/.srv/.action file to be processed 77 | ## * uncomment the generate_messages entry below 78 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 79 | 80 | ## Generate messages in the 'msg' folder 81 | # add_message_files( 82 | # FILES 83 | # Message1.msg 84 | # Message2.msg 85 | # ) 86 | 87 | ## Generate services in the 'srv' folder 88 | # add_service_files( 89 | # FILES 90 | # Service1.srv 91 | # Service2.srv 92 | # ) 93 | 94 | ## Generate actions in the 'action' folder 95 | # add_action_files( 96 | # FILES 97 | # Action1.action 98 | # Action2.action 99 | # ) 100 | 101 | ## Generate added messages and services with any dependencies listed here 102 | # generate_messages( 103 | # DEPENDENCIES 104 | # sensor_msgs# std_msgs 105 | # ) 106 | 107 | ################################################ 108 | ## Declare ROS dynamic reconfigure parameters ## 109 | ################################################ 110 | 111 | ## To declare and build dynamic reconfigure parameters within this 112 | ## package, follow these steps: 113 | ## * In the file package.xml: 114 | ## * add a build_depend and a run_depend tag for "dynamic_reconfigure" 115 | ## * In this file (CMakeLists.txt): 116 | ## * add "dynamic_reconfigure" to 117 | ## find_package(catkin REQUIRED COMPONENTS ...) 118 | ## * uncomment the "generate_dynamic_reconfigure_options" section below 119 | ## and list every .cfg file to be processed 120 | 121 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 122 | # generate_dynamic_reconfigure_options( 123 | # cfg/DynReconf1.cfg 124 | # cfg/DynReconf2.cfg 125 | # ) 126 | 127 | ################################### 128 | ## catkin specific configuration ## 129 | ################################### 130 | ## The catkin_package macro generates cmake config files for your package 131 | ## Declare things to be passed to dependent projects 132 | ## INCLUDE_DIRS: uncomment this if your package contains header files 133 | ## LIBRARIES: libraries you create in this project that dependent projects also need 134 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 135 | ## DEPENDS: system dependencies of this project that dependent projects also need 136 | catkin_package( 137 | # INCLUDE_DIRS include 138 | # LIBRARIES pandora_projection 139 | # CATKIN_DEPENDS cv_bridge image_transport pcl roscpp sensor_msgs std_msgs 140 | # DEPENDS system_lib 141 | ) 142 | 143 | ########### 144 | ## Build ## 145 | ########### 146 | 147 | ## Specify additional locations of header files 148 | ## Your package locations should be listed before other locations 149 | include_directories( 150 | # include 151 | ${catkin_INCLUDE_DIRS} 152 | ) 153 | 154 | ## Declare a C++ library 155 | # add_library(${PROJECT_NAME} 156 | # src/${PROJECT_NAME}/pandora_projection.cpp 157 | # ) 158 | 159 | ## Add cmake target dependencies of the library 160 | ## as an example, code may need to be generated before libraries 161 | ## either from message generation or dynamic reconfigure 162 | # add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 163 | 164 | ## Declare a C++ executable 165 | ## With catkin_make all packages are built within a single CMake context 166 | ## The recommended prefix ensures that target names across packages don't collide 167 | # add_executable(${PROJECT_NAME}_node src/pandora_projection_node.cpp) 168 | 169 | ## Rename C++ executable without prefix 170 | ## The above recommended prefix causes long target names, the following renames the 171 | ## target back to the shorter version for ease of user use 172 | ## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" 173 | # set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") 174 | 175 | ## Add cmake target dependencies of the executable 176 | ## same as for the library above 177 | # add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 178 | 179 | ## Specify libraries to link a library or executable target against 180 | # target_link_libraries(${PROJECT_NAME}_node 181 | # ${catkin_LIBRARIES} 182 | # ) 183 | 184 | ############# 185 | ## Install ## 186 | ############# 187 | 188 | # all install targets should use catkin DESTINATION variables 189 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 190 | 191 | ## Mark executable scripts (Python etc.) for installation 192 | ## in contrast to setup.py, you can choose the destination 193 | # install(PROGRAMS 194 | # scripts/my_python_script 195 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 196 | # ) 197 | 198 | ## Mark executables and/or libraries for installation 199 | # install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node 200 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 201 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 202 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 203 | # ) 204 | 205 | ## Mark cpp header files for installation 206 | # install(DIRECTORY include/${PROJECT_NAME}/ 207 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 208 | # FILES_MATCHING PATTERN "*.h" 209 | # PATTERN ".svn" EXCLUDE 210 | # ) 211 | 212 | ## Mark other files for installation (e.g. launch and bag files, etc.) 213 | # install(FILES 214 | # # myfile1 215 | # # myfile2 216 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 217 | # ) 218 | 219 | ############# 220 | ## Testing ## 221 | ############# 222 | 223 | ## Add gtest based cpp test target and link libraries 224 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_pandora_projection.cpp) 225 | # if(TARGET ${PROJECT_NAME}-test) 226 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 227 | # endif() 228 | 229 | ## Add folders to be run by python nosetests 230 | # catkin_add_nosetests(test) 231 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Pandora_Projected_ROS 2 | 3 | ## Build 4 | ``` 5 | mkdir -p rosworkspace/src 6 | cd rosworkspace/src 7 | git clone https://github.com/HesaiTechnology/Pandora_Projected_ROS.git --recursive 8 | cd ../ 9 | catkin_make 10 | ``` 11 | 12 | ## Run 13 | ``` 14 | source devel/setup.sh 15 | roslaunch pandora_projection pandora_projection.launch 16 | ``` 17 | 18 | ## ROS Topic name 19 | ``` 20 | /pandora_projection0 21 | /pandora_projection1 22 | /pandora_projection2 23 | /pandora_projection3 24 | /pandora_projection4 25 | ``` 26 | -------------------------------------------------------------------------------- /launch/pandora_projection.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | pandora_projection 4 | 0.0.0 5 | The pandora_projection package 6 | 7 | 8 | 9 | 10 | Philip.Pi 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 | cv_bridge 53 | image_transport 54 | pcl 55 | roscpp 56 | sensor_msgs 57 | std_msgs 58 | cv_bridge 59 | image_transport 60 | pcl 61 | roscpp 62 | sensor_msgs 63 | std_msgs 64 | cv_bridge 65 | image_transport 66 | pcl 67 | roscpp 68 | sensor_msgs 69 | std_msgs 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | -------------------------------------------------------------------------------- /src/pandora_projection.cc: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * Copyright 2018 The Hesai Technology. All Rights Reserved. 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | *****************************************************************************/ 16 | 17 | 18 | 19 | 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | #include 27 | #include 28 | #include 29 | #include 30 | 31 | #include 32 | #include 33 | #include 34 | #include 35 | 36 | #include 37 | #include "pandora/pandora.h" 38 | 39 | 40 | using namespace apollo::drivers::hesai; 41 | 42 | void convert_rot(const double array[], double rr[]) { 43 | cv::Mat rvec = (cv::Mat_(3, 1) << array[0], array[1], array[2]); 44 | cv::Mat rot; 45 | cv::Rodrigues(rvec, rot); 46 | for (int i = 0; i < 9; ++i) { 47 | rr[i] = rot.at(i); 48 | } 49 | } 50 | 51 | void convert_rot_trans(const double array[], double rr[], double tt[]) { 52 | convert_rot(array, rr); 53 | for (int i = 0; i < 3; ++i) { 54 | tt[i] = array[3 + i]; 55 | } 56 | } 57 | 58 | void convert_quater_to_axisd(const std::vector q, double axsid[]) { 59 | Eigen::Quaternion rotation(q[0], q[1], q[2], q[3]); 60 | Eigen::AngleAxis aa(rotation); 61 | Eigen::Vector3d avec = aa.axis() * aa.angle(); 62 | 63 | axsid[0] = avec[0]; 64 | axsid[1] = avec[1]; 65 | axsid[2] = avec[2]; 66 | } 67 | 68 | bool convert_inv_extrinsics(const double extrinsics_src[], 69 | double extrinsics_inv[]) { 70 | double rr[9]; 71 | double tt[3]; 72 | convert_rot_trans(extrinsics_src, rr, tt); 73 | extrinsics_inv[0] = -extrinsics_src[0]; 74 | extrinsics_inv[1] = -extrinsics_src[1]; 75 | extrinsics_inv[2] = -extrinsics_src[2]; 76 | extrinsics_inv[3] = -rr[0] * tt[0] - rr[3] * tt[1] - rr[6] * tt[2]; 77 | extrinsics_inv[4] = -rr[1] * tt[0] - rr[4] * tt[1] - rr[7] * tt[2]; 78 | extrinsics_inv[5] = -rr[2] * tt[0] - rr[5] * tt[1] - rr[8] * tt[2]; 79 | } 80 | 81 | void convert_quater_to_trans(const std::vector q, 82 | const std::vector tt, double array[]) { 83 | convert_quater_to_axisd(q, array); 84 | array[3] = tt[0]; 85 | array[4] = tt[1]; 86 | array[5] = tt[2]; 87 | } 88 | 89 | void convert_cv_vec(const double array[], cv::Mat &rvec, cv::Mat &tvec) { 90 | rvec = (cv::Mat_(3, 1) << array[0], array[1], array[2]); 91 | tvec = (cv::Mat_(3, 1) << array[3], array[4], array[5]); 92 | } 93 | 94 | void convert_camera_matrix(const cv::Mat &camera_matrix, double array[]) { 95 | assert(!camera_matrix.empty() && camera_matrix.rows == 3 && 96 | camera_matrix.cols == 3); 97 | array[0] = camera_matrix.at(0, 0); 98 | array[1] = camera_matrix.at(1, 1); 99 | array[2] = camera_matrix.at(0, 2); 100 | array[3] = camera_matrix.at(1, 2); 101 | } 102 | 103 | void convert_camera_matrix(const double array[], cv::Mat &camera_matrix) { 104 | camera_matrix = (cv::Mat_(3, 3) << array[0], 0.0, array[2], 0.0, 105 | array[1], array[3], 0.0, 0.0, 1.0); 106 | } 107 | 108 | void convert_camera_dist(const double array[], cv::Mat &camera_dist) { 109 | camera_dist = (cv::Mat_(5, 1) << array[0], array[1], array[2], 110 | array[3], array[4]); 111 | } 112 | 113 | void compute_color_map(int color_mode, std::vector &clr_vec) { 114 | cv::Mat color_map_gray = cv::Mat::zeros(256, 1, CV_8UC1); 115 | for (int i = 0; i < color_map_gray.rows; ++i) { 116 | color_map_gray.at(i) = 255 - i; 117 | } 118 | if (color_mode >= 0) { 119 | cv::Mat color_map; 120 | cv::applyColorMap(color_map_gray, color_map, color_mode); 121 | for (int i = 0; i < color_map.total(); ++i) { 122 | cv::Vec3b clr = color_map.at(i); 123 | clr_vec.push_back(cv::Scalar(clr[0], clr[1], clr[2], 255)); 124 | } 125 | } else { 126 | for (int i = 0; i < color_map_gray.total(); ++i) { 127 | clr_vec.push_back(cv::Scalar(i, i, i, 255)); 128 | } 129 | } 130 | } 131 | 132 | bool project_cloud_to_image(boost::shared_ptr cloud, 133 | const double intrinsic[], const double distortion[], 134 | const double extrinsic[], int render_mode, 135 | double min_v, double max_v, 136 | const std::vector &clr_vec, 137 | int point_size, int thinkness, int point_type, 138 | cv::Mat &out_image) { 139 | if (cloud->empty() || out_image.empty() || clr_vec.empty()) { 140 | return false; 141 | } 142 | point_size = point_size <= 0 ? 1 : point_size; 143 | int point_count = cloud->points.size(); 144 | std::vector pt3d_vec; 145 | std::vector inten_vec; 146 | double rr[9]; 147 | double tt[3]; 148 | convert_rot_trans(extrinsic, rr, tt); 149 | double xx[3]; 150 | double yy[3]; 151 | float img_pt[2]; 152 | int width = out_image.cols; 153 | int height = out_image.rows; 154 | float ratio = 0.5; 155 | float min_w = -ratio * width; 156 | float max_w = (1 + ratio) * width; 157 | float min_h = -ratio * height; 158 | float max_h = (1 + ratio) * height; 159 | for (int i = 0; i < point_count; ++i) { 160 | xx[0] = cloud->points[i].x; 161 | xx[1] = cloud->points[i].y; 162 | xx[2] = cloud->points[i].z; 163 | if (std::isnan(xx[0]) || std::isnan(xx[1]) || std::isnan(xx[2])) { 164 | continue; 165 | } 166 | yy[2] = rr[6] * xx[0] + rr[7] * xx[1] + rr[8] * xx[2] + tt[2]; 167 | if (yy[2] < 1.0) { 168 | continue; 169 | } 170 | yy[2] = 1.0 / yy[2]; 171 | yy[0] = rr[0] * xx[0] + rr[1] * xx[1] + rr[2] * xx[2] + tt[0]; 172 | img_pt[0] = yy[0] * yy[2] * intrinsic[0] + intrinsic[2]; 173 | if (img_pt[0] < min_w || img_pt[0] > max_w) { 174 | continue; 175 | } 176 | yy[1] = rr[3] * xx[0] + rr[4] * xx[1] + rr[5] * xx[2] + tt[1]; 177 | img_pt[1] = yy[1] * yy[2] * intrinsic[1] + intrinsic[3]; 178 | if (img_pt[1] < min_h || img_pt[1] > max_h) { 179 | continue; 180 | } 181 | pt3d_vec.push_back(cv::Point3f(xx[0], xx[1], xx[2])); 182 | inten_vec.push_back(cloud->points[i].intensity); 183 | } 184 | if (pt3d_vec.empty()) { 185 | return false; 186 | } 187 | 188 | cv::Mat rvec; 189 | cv::Mat tvec; 190 | convert_cv_vec(extrinsic, rvec, tvec); 191 | cv::Mat camera_k; 192 | convert_camera_matrix(intrinsic, camera_k); 193 | cv::Mat camera_d; 194 | convert_camera_dist(distortion, camera_d); 195 | std::vector img_pts; 196 | cv::projectPoints(pt3d_vec, rvec, tvec, camera_k, camera_d, img_pts); 197 | 198 | int clr_count = clr_vec.size(); 199 | double kk = clr_count * 1.0 / (max_v - min_v); 200 | double bb = -min_v * clr_count / (max_v - min_v); 201 | for (int i = 0; i < img_pts.size(); ++i) { 202 | int idx = 0; 203 | if (render_mode == 1) { 204 | idx = int(kk * pt3d_vec[i].z + bb); 205 | } else if (render_mode == 2) { 206 | double distance = cv::norm(pt3d_vec[i]); 207 | idx = int(kk * distance + bb); 208 | } else if (render_mode == 3) { 209 | idx = int(kk * inten_vec[i] + bb); 210 | } else { 211 | idx = int(kk * inten_vec[i] + bb); 212 | } 213 | // std::cout << max_v << " " << min_v << " " << kk << " " << bb<< " " << 214 | // pt3d_vec[i].z << " " << idx << std::endl; 215 | idx = idx < 0 ? 0 : idx; 216 | idx = idx >= clr_count ? clr_count - 1 : idx; 217 | cv::Scalar clr = clr_vec[idx]; 218 | if (point_type == 1) { 219 | float x = img_pts[i].x; 220 | float y = img_pts[i].y; 221 | cv::line(out_image, cv::Point2f(x + point_size, y), 222 | cv::Point2f(x - point_size, y), clr, thinkness); 223 | cv::line(out_image, cv::Point2f(x, y + point_size), 224 | cv::Point2f(x, y - point_size), clr, thinkness); 225 | } else { 226 | cv::circle(out_image, img_pts[i], point_size, clr, thinkness); 227 | } 228 | } 229 | return true; 230 | } 231 | 232 | using namespace apollo::drivers::hesai; 233 | 234 | class CameraData { 235 | public: 236 | boost::shared_ptr matp; 237 | double timestamp; 238 | int picid; 239 | bool distortion; 240 | }; 241 | 242 | class LidarData { 243 | public: 244 | boost::shared_ptr cld; 245 | double timestamp; 246 | }; 247 | 248 | class ProjectionData { 249 | public: 250 | LidarData lidar; 251 | CameraData camera; 252 | }; 253 | 254 | class Projection { 255 | public: 256 | Projection(ros::NodeHandle node, ros::NodeHandle private_nh, std::string ip, 257 | int render_mode = 0) { 258 | pandora_cameras_output[0] = 259 | node.advertise("pandora_projection0", 10); 260 | pandora_cameras_output[1] = 261 | node.advertise("pandora_projection1", 10); 262 | pandora_cameras_output[2] = 263 | node.advertise("pandora_projection2", 10); 264 | pandora_cameras_output[3] = 265 | node.advertise("pandora_projection3", 10); 266 | pandora_cameras_output[4] = 267 | node.advertise("pandora_projection4", 10); 268 | ip_ = ip; 269 | pthread_mutex_init(&cld_lock_, NULL); 270 | pthread_mutex_init(&pic_lock_, NULL); 271 | pthread_mutex_init(&proj_lock_, NULL); 272 | 273 | sem_init(&proj_sem_, 0, 0); 274 | 275 | render_mode_ = render_mode; // 0, intensity; 1, height; 2, distance 276 | min_v = 1; 277 | max_v = 30; 278 | point_size = 1; 279 | clr_mode = 4; 280 | point_type = 0; 281 | thinkness = 1; 282 | got_calibration_ = 0; 283 | 284 | int lidar_port = 2368; 285 | int gps_port = 10110; 286 | int camera_port = 9870; 287 | double start_angle = 135.0f; 288 | 289 | bool ret = ParseParameter(private_nh, &ip_, &lidar_port, &gps_port, \ 290 | &camera_port, &start_angle, &render_mode_); 291 | 292 | if (!ret) { 293 | ROS_INFO("Parse parameters failed, please check parameters above."); 294 | return; 295 | } 296 | 297 | compute_color_map(clr_mode, clr_vec); 298 | 299 | // init distortion as 0.0 300 | for (int i = 0; i < 5; ++i) { 301 | for (int j = 0; j < 5; ++j) { 302 | distortion[i][j] = 0.0; 303 | } 304 | } 305 | 306 | proj_thr_ = 307 | new boost::thread(boost::bind(&Projection::ProjectionTask, this)); 308 | 309 | pandora = new Pandora(ip_, lidar_port, gps_port, 310 | boost::bind(&Projection::LidarCallBack, this, _1, _2), 311 | NULL, start_angle * 100.0f, camera_port, 312 | boost::bind(&Projection::CameraCallBack, this, _1, _2, _3, _4), 1, 0, 313 | std::string("hesai40")); 314 | pandora->Start(); 315 | } 316 | 317 | void LidarCallBack(boost::shared_ptr, double); 318 | void CameraCallBack(boost::shared_ptr matp, double timestamp, 319 | int picid, bool distortion); 320 | 321 | private: 322 | void TryToProjection(); 323 | void ProjectionTask(); 324 | bool ParseParameter(ros::NodeHandle nh, std::string *pandora_ip, \ 325 | int *lidar_port, int *gps_port, int *camera_port, \ 326 | double *start_angle, int *render_mode); 327 | bool checkPort(int port); 328 | bool checkRenderMode(int render_mode); 329 | 330 | pthread_mutex_t cld_lock_; 331 | pthread_mutex_t pic_lock_; 332 | pthread_mutex_t proj_lock_; 333 | 334 | sem_t proj_sem_; 335 | 336 | std::list cld_list_; 337 | std::list pic_list_; 338 | std::list proj_list_; 339 | 340 | std::string ip_; 341 | 342 | Pandora *pandora; 343 | 344 | int render_mode_; 345 | double min_v; 346 | double max_v; 347 | int point_size; 348 | int thinkness; 349 | int point_type; 350 | int clr_mode; 351 | std::vector clr_vec; 352 | 353 | double intrinsic[5][4]; 354 | double distortion[5][5]; 355 | double extrinsic[5][6]; 356 | 357 | int got_calibration_; 358 | 359 | ros::Publisher pandora_cameras_output[5]; 360 | 361 | boost::thread *proj_thr_; 362 | }; 363 | 364 | double gap_map[5][2] = { 365 | {0.000, 0.025}, {0.000, 0.025}, {0.025, 0.050}, 366 | {0.050, 0.075}, {0.075, 0.100}, 367 | }; 368 | 369 | void Projection::ProjectionTask() { 370 | while (true) { 371 | sem_wait(&proj_sem_); 372 | ProjectionData proj_data; 373 | 374 | pthread_mutex_lock(&proj_lock_); 375 | proj_data = proj_list_.front(); 376 | proj_list_.pop_front(); 377 | pthread_mutex_unlock(&proj_lock_); 378 | 379 | cv_bridge::CvImage cv_ptr = 380 | cv_bridge::CvImage(std_msgs::Header(), "bgr8", *proj_data.camera.matp); 381 | if (!project_cloud_to_image( 382 | proj_data.lidar.cld, intrinsic[proj_data.camera.picid], 383 | distortion[proj_data.camera.picid], 384 | extrinsic[proj_data.camera.picid], render_mode_, min_v, max_v, 385 | clr_vec, point_size, thinkness, point_type, cv_ptr.image)) { 386 | std::cout << "error projection" << std::endl; 387 | } 388 | 389 | sensor_msgs::ImagePtr msg = cv_ptr.toImageMsg(); 390 | pandora_cameras_output[proj_data.camera.picid].publish(msg); 391 | } 392 | } 393 | 394 | bool Projection::ParseParameter(ros::NodeHandle nh, \ 395 | std::string *pandora_ip, int *lidar_port, int *gps_port, \ 396 | int *camera_port, double *start_angle, int *render_mode) 397 | { 398 | if (nh.hasParam("pandora_ip")) { 399 | nh.getParam("pandora_ip", *pandora_ip); 400 | } 401 | if (nh.hasParam("lidar_port")) { 402 | nh.getParam("lidar_port", *lidar_port); 403 | } 404 | if (nh.hasParam("gps_port")) { 405 | nh.getParam("gps_port", *gps_port); 406 | } 407 | if (nh.hasParam("camera_port")) { 408 | nh.getParam("camera_port", *camera_port); 409 | } 410 | if (nh.hasParam("start_angle")) { 411 | nh.getParam("start_angle", *start_angle); 412 | } 413 | if (nh.hasParam("render_mode")) { 414 | nh.getParam("render_mode", *render_mode); 415 | } 416 | 417 | std::cout << "Configs: pandora_ip: " << *pandora_ip << \ 418 | ", lidar_port: " << *lidar_port << ", gps_port: " << *lidar_port << \ 419 | ", camera_port: " << *camera_port << ", start_angle: " << \ 420 | *start_angle << ", render_mode: " << *render_mode << std::endl; 421 | 422 | struct sockaddr_in sa; 423 | 424 | return checkPort(*lidar_port) && checkPort(*gps_port) && \ 425 | checkPort(*camera_port) && checkRenderMode(*render_mode) && \ 426 | (*start_angle < 360) && (*start_angle >= 0) && \ 427 | (1 == inet_pton(AF_INET, pandora_ip->c_str(), &(sa.sin_addr))); 428 | } 429 | 430 | bool Projection::checkRenderMode(int render_mode) 431 | { 432 | if (render_mode < 0 || render_mode > 3) 433 | return false; 434 | else 435 | return true; 436 | } 437 | 438 | bool Projection::checkPort(int port) 439 | { 440 | if (port <= 0 || port >= 65535) 441 | { 442 | return false; 443 | } 444 | else 445 | { 446 | return true; 447 | } 448 | } 449 | 450 | void Projection::TryToProjection() { 451 | int cld_size = 0; 452 | pthread_mutex_lock(&cld_lock_); 453 | cld_size = cld_list_.size(); 454 | pthread_mutex_unlock(&cld_lock_); 455 | 456 | // empty cloud 457 | if (cld_size == 0) { 458 | pthread_mutex_lock(&pic_lock_); 459 | while (pic_list_.size() > 5) { 460 | // drop when cloud is empty; 461 | pic_list_.pop_front(); 462 | } 463 | pthread_mutex_unlock(&pic_lock_); 464 | return; 465 | } 466 | 467 | pthread_mutex_lock(&cld_lock_); 468 | pthread_mutex_lock(&pic_lock_); 469 | 470 | while (pic_list_.size() > 0) { 471 | int found = false; 472 | CameraData camera_data = pic_list_.front(); 473 | double front_gap = camera_data.timestamp - cld_list_.front().timestamp; 474 | 475 | // picture is too late !!! 476 | if (front_gap < 0) { 477 | printf("Camera [%d]'s data is too late\n", camera_data.picid); 478 | pic_list_.pop_front(); 479 | continue; 480 | } 481 | 482 | std::list::iterator it; 483 | for (it = cld_list_.begin(); it != cld_list_.end(); ++it) { 484 | /* code */ 485 | double gap = camera_data.timestamp - it->timestamp; 486 | if (gap > gap_map[camera_data.picid][0] && 487 | gap < gap_map[camera_data.picid][1]) { 488 | pic_list_.pop_front(); 489 | 490 | ProjectionData proj_data; 491 | proj_data.camera = camera_data; 492 | proj_data.lidar = *it; 493 | 494 | pthread_mutex_lock(&proj_lock_); 495 | proj_list_.push_back(proj_data); 496 | pthread_mutex_unlock(&proj_lock_); 497 | 498 | sem_post(&proj_sem_); 499 | 500 | found = true; 501 | break; 502 | } 503 | } 504 | 505 | if (!found) break; 506 | } 507 | 508 | pthread_mutex_unlock(&pic_lock_); 509 | pthread_mutex_unlock(&cld_lock_); 510 | } 511 | 512 | void Projection::LidarCallBack(boost::shared_ptr cld, 513 | double timestamp) { 514 | LidarData lidar_data; 515 | lidar_data.cld = cld; 516 | lidar_data.timestamp = timestamp; 517 | 518 | pthread_mutex_lock(&cld_lock_); 519 | 520 | cld_list_.push_back(lidar_data); 521 | 522 | // only cache 1 frames; 523 | if (cld_list_.size() > 1) { 524 | cld_list_.pop_front(); 525 | } 526 | 527 | pthread_mutex_unlock(&cld_lock_); 528 | } 529 | 530 | void Projection::CameraCallBack(boost::shared_ptr matp, 531 | double timestamp, int picid, bool distortion) { 532 | if (distortion && !got_calibration_) { 533 | // get parameters; 534 | CameraCalibration calibs[5]; 535 | pandora->GetCameraCalibration(calibs); 536 | 537 | for (int i = 0; i < 5; ++i) { 538 | /* code */ 539 | convert_camera_matrix(calibs[i].cameraK, intrinsic[i]); 540 | 541 | convert_quater_to_trans(calibs[i].cameraR, calibs[i].cameraT, 542 | extrinsic[i]); 543 | 544 | convert_inv_extrinsics(extrinsic[i], extrinsic[i]); 545 | } 546 | got_calibration_ = 1; 547 | } 548 | CameraData camera_data; 549 | camera_data.matp = matp; 550 | camera_data.timestamp = timestamp; 551 | camera_data.picid = picid; 552 | camera_data.distortion = distortion; 553 | 554 | pthread_mutex_lock(&pic_lock_); 555 | pic_list_.push_back(camera_data); 556 | pthread_mutex_unlock(&pic_lock_); 557 | 558 | TryToProjection(); 559 | } 560 | 561 | int main(int argc, char **argv) { 562 | ros::init(argc, argv, "pandora_projection"); 563 | ros::NodeHandle nh("~"); 564 | ros::NodeHandle node; 565 | 566 | Projection projection(node, nh, std::string("192.168.20.51")); 567 | ros::Rate loop_rate(5); 568 | 569 | ros::spin(); 570 | } 571 | --------------------------------------------------------------------------------