├── .gitignore ├── .gitmodules ├── CMakeLists.txt ├── README.md ├── include └── pandora │ └── pandora.h ├── src ├── pandora.cc ├── pandora_camera.cc ├── pandora_camera.h ├── pandora_client.c ├── pandora_client.h ├── tcp_command_client.c ├── tcp_command_client.h ├── util.c └── util.h └── test └── test.cc /.gitignore: -------------------------------------------------------------------------------- 1 | build 2 | -------------------------------------------------------------------------------- /.gitmodules: -------------------------------------------------------------------------------- 1 | [submodule "src/Pandar40P"] 2 | path = src/Pandar40P 3 | url = https://github.com/HesaiTechnology/Pandar40P_Apollo.git 4 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8) 2 | project(Pandora) 3 | 4 | 5 | find_package( Boost REQUIRED ) 6 | find_package( PCL REQUIRED COMPONENTS common ) 7 | find_package( OpenCV REQUIRED ) 8 | 9 | set (CMAKE_CXX_FLAGS "-fPIC --std=c++11") 10 | 11 | add_subdirectory(src/Pandar40P) 12 | 13 | include_directories( 14 | . 15 | include 16 | src/Pandar40P/include 17 | ${Boost_INCLUDE_DIRS} 18 | ${PCL_INCLUDE_DIRS} 19 | ${OpenCV_INCLUDE_DIRS} 20 | ) 21 | 22 | 23 | add_library( ${PROJECT_NAME} SHARED 24 | src/pandora.cc 25 | src/pandora_client.c 26 | src/pandora_camera.cc 27 | src/util.c 28 | src/tcp_command_client.c 29 | ) 30 | 31 | target_link_libraries(${PROJECT_NAME} 32 | Pandar40P 33 | ${Boost_LIBRARIES} 34 | ${PCL_IO_LIBRARIES} 35 | ${OpenCV_LIBS} 36 | yaml-cpp 37 | jpeg 38 | ) 39 | 40 | if(${CMAKE_SOURCE_DIR} STREQUAL ${CMAKE_CURRENT_SOURCE_DIR}) 41 | 42 | 43 | add_executable(pandora_test 44 | test/test.cc 45 | ) 46 | 47 | target_link_libraries(pandora_test 48 | Pandar40P 49 | ${PROJECT_NAME} 50 | ${Boost_LIBRARIES} 51 | ${PCL_IO_LIBRARIES} 52 | ${OpenCV_LIBS} 53 | yaml-cpp 54 | jpeg 55 | ) 56 | 57 | endif(${CMAKE_SOURCE_DIR} STREQUAL ${CMAKE_CURRENT_SOURCE_DIR}) 58 | 59 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Pandora_SDK 2 | ## Clone 3 | ``` 4 | git clone https://github.com/HesaiTechnology/Pandora_SDK.git --recursive 5 | ``` 6 | 7 | ## Build 8 | ``` 9 | cd 10 | mkdir build ; cd build; 11 | cmake .. ; make 12 | ``` 13 | ## Add to your project 14 | ### Cmake 15 | ``` 16 | add_subdirectory(Pandora_SDK) 17 | 18 | include_directories( 19 | Pandora_SDK/include 20 | Pandora_SDK/src/Pandar40P/include 21 | ) 22 | 23 | target_link_libraries( 24 | Pandora 25 | ) 26 | 27 | ``` 28 | ### C++ 29 | ``` 30 | #include "pandora/pandora.h" 31 | ``` 32 | 33 | -------------------------------------------------------------------------------- /include/pandora/pandora.h: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * Copyright 2018 The Apollo Authors. 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 | #ifndef INCLUDE_PANDORA_PANDORA_H_ 18 | #define INCLUDE_PANDORA_PANDORA_H_ 19 | 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | 26 | #include 27 | #include 28 | 29 | #include 30 | #include 31 | 32 | #include "pandar40p/pandar40p.h" 33 | #include "pandar40p/point_types.h" 34 | 35 | namespace apollo { 36 | namespace drivers { 37 | namespace hesai { 38 | 39 | class Pandora_Internal; 40 | 41 | class CameraCalibration { 42 | public: 43 | // intrinsic 44 | cv::Mat cameraK; 45 | cv::Mat cameraD; 46 | // extrinsic 47 | std::vector cameraT; // x y z 48 | std::vector cameraR; // q0(w) q1(x) q2(y) q3(z) 49 | }; 50 | 51 | class Pandora { 52 | public: 53 | /** 54 | * @brief Constructor 55 | * @param device_ip The ip of the device 56 | * lidar_port The port number of lidar data 57 | * gps_port The port number of gps data 58 | * pcl_callback The callback of PCL data structure 59 | * gps_callback The callback of GPS structure 60 | * start_angle The start angle of every point cloud 61 | * should be * 100. 62 | * pandoraCameraPort The port of camera data 63 | * cameraCallback the call back for camera data 64 | */ 65 | Pandora(std::string device_ip, const uint16_t lidar_port, 66 | const uint16_t gps_port, 67 | boost::function, double)> 68 | pcl_callback, 69 | boost::function gps_callback, uint16_t start_angle, 70 | const uint16_t pandoraCameraPort, 71 | boost::function matp, 72 | double timestamp, int picid, bool distortion)> 73 | cameraCallback, 74 | bool enable_camera = true, int tz = 0, 75 | std::string frame_id = std::string("hesai40")); 76 | /** 77 | * @brief deconstructor 78 | */ 79 | ~Pandora(); 80 | 81 | /** 82 | * @brief load the lidar correction file 83 | * @param contents The correction contents of lidar correction 84 | */ 85 | int LoadLidarCorrectionFile(std::string contents); 86 | 87 | /** 88 | * @brief Reset Lidar's start angle. 89 | * @param angle The start angle 90 | */ 91 | void ResetLidarStartAngle(uint16_t start_angle); 92 | 93 | /** 94 | * @brief Upload the camera calibration contents to Pandora Device. 95 | * @param calibs calibration contents , include camera intrinsics and 96 | * extrinsics. 97 | */ 98 | int UploadCameraCalibration(const CameraCalibration calibs[5]); 99 | 100 | /** 101 | * @brief Get Camera's Calibration, include camera intrinsics and extrinsics. 102 | * @param calibs calibration contents , include camera intrinsics and 103 | * extrinsics. 104 | */ 105 | int GetCameraCalibration(CameraCalibration calibs[5]); 106 | 107 | /** 108 | * @brief Reset camera calibration as factory-set. 109 | */ 110 | int ResetCameraClibration(); 111 | 112 | /** 113 | * @brief Run SDK. 114 | */ 115 | int Start(); 116 | 117 | /** 118 | * @brief Stop SDK. 119 | */ 120 | void Stop(); 121 | 122 | private: 123 | Pandora_Internal *internal_; 124 | }; 125 | 126 | } // namespace hesai 127 | } // namespace drivers 128 | } // namespace apollo 129 | 130 | #endif // INCLUDE_PANDORA_PANDORA_H_ 131 | -------------------------------------------------------------------------------- /src/pandora.cc: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * Copyright 2018 The Apollo Authors. 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 | #include "pandora/pandora.h" 18 | #include "src/pandora_camera.h" 19 | #include "src/pandora_client.h" 20 | #include "src/tcp_command_client.h" 21 | #include "yaml-cpp/yaml.h" 22 | 23 | namespace apollo { 24 | namespace drivers { 25 | namespace hesai { 26 | 27 | #define PANDORA_TCP_COMMAND_PORT (9347) 28 | 29 | class Pandora_Internal { 30 | public: 31 | Pandora_Internal( 32 | std::string device_ip, const uint16_t lidar_port, const uint16_t gps_port, 33 | boost::function, double)> 34 | pcl_callback, 35 | boost::function gps_callback, uint16_t start_angle, 36 | const uint16_t pandoraCameraPort, 37 | boost::function matp, double timestamp, 38 | int picid, bool distortion)> 39 | cameraCallback, 40 | bool enable_camera, int tz, std::string frame_id); 41 | ~Pandora_Internal(); 42 | int LoadLidarCorrectionFile(std::string correction_content); 43 | void ResetLidarStartAngle(uint16_t start_angle); 44 | int UploadCameraCalibrationFile(const CameraCalibration calibs[5]); 45 | int GetCameraCalibration(CameraCalibration calibs[5]); 46 | int ResetCameraClibration(); 47 | void GetCalibrationFromDevice(); 48 | int ParseCameraCalibration(const std::string contents, 49 | CameraCalibration calibs[5]); 50 | int GenerateCameraCalibration(const CameraCalibration calibs[5], 51 | std::string *contents); 52 | int Start(); 53 | void Stop(); 54 | 55 | private: 56 | Pandar40P *pandar40p_; 57 | PandoraCamera *pandora_camera_; 58 | void *tcp_command_client_; 59 | boost::thread *get_calibration_thr_; 60 | bool enable_get_calibration_thr_; 61 | 62 | bool got_lidar_calibration_; 63 | bool got_camera_calibration_; 64 | 65 | CameraCalibration camera_calibs_[5]; 66 | 67 | bool enable_camera_; 68 | }; 69 | 70 | Pandora_Internal::Pandora_Internal( 71 | std::string device_ip, const uint16_t lidar_port, const uint16_t gps_port, 72 | boost::function, double)> pcl_callback, 73 | boost::function gps_callback, uint16_t start_angle, 74 | const uint16_t pandoraCameraPort, 75 | boost::function matp, double timestamp, 76 | int picid, bool distortion)> 77 | cameraCallback, 78 | bool enable_camera, int tz, std::string frame_id) { 79 | enable_camera_ = enable_camera; 80 | pandora_camera_ = NULL; 81 | pandar40p_ = NULL; 82 | 83 | pandar40p_ = new Pandar40P(device_ip, lidar_port, gps_port, pcl_callback, 84 | gps_callback, start_angle, tz, frame_id); 85 | 86 | if (enable_camera_) { 87 | pandora_camera_ = new PandoraCamera(device_ip, pandoraCameraPort, 88 | cameraCallback, NULL, tz); 89 | } 90 | 91 | tcp_command_client_ = 92 | TcpCommandClientNew(device_ip.c_str(), PANDORA_TCP_COMMAND_PORT); 93 | if (!tcp_command_client_) { 94 | std::cout << "Init TCP Command Client Failed" << std::endl; 95 | } 96 | get_calibration_thr_ = NULL; 97 | enable_get_calibration_thr_ = false; 98 | 99 | got_lidar_calibration_ = false; 100 | got_camera_calibration_ = false; 101 | } 102 | 103 | Pandora_Internal::~Pandora_Internal() { 104 | Stop(); 105 | if (pandar40p_) { 106 | delete pandar40p_; 107 | } 108 | 109 | if (pandora_camera_) { 110 | delete pandora_camera_; 111 | } 112 | } 113 | 114 | /** 115 | * @brief load the correction file 116 | * @param file The path of correction file 117 | */ 118 | int Pandora_Internal::LoadLidarCorrectionFile(std::string correction_content) { 119 | return pandar40p_->LoadCorrectionFile(correction_content); 120 | } 121 | 122 | /** 123 | * @brief load the correction file 124 | * @param angle The start angle 125 | */ 126 | void Pandora_Internal::ResetLidarStartAngle(uint16_t start_angle) { 127 | if (!pandar40p_) return; 128 | pandar40p_->ResetStartAngle(start_angle); 129 | } 130 | 131 | int Pandora_Internal::Start() { 132 | Stop(); 133 | if (pandora_camera_) { 134 | pandora_camera_->Start(); 135 | } 136 | 137 | if (pandar40p_) { 138 | pandar40p_->Start(); 139 | } 140 | 141 | enable_get_calibration_thr_ = true; 142 | get_calibration_thr_ = new boost::thread( 143 | boost::bind(&Pandora_Internal::GetCalibrationFromDevice, this)); 144 | } 145 | 146 | void Pandora_Internal::Stop() { 147 | if (pandar40p_) pandar40p_->Stop(); 148 | if (pandora_camera_) pandora_camera_->Stop(); 149 | 150 | enable_get_calibration_thr_ = false; 151 | if (get_calibration_thr_) { 152 | get_calibration_thr_->join(); 153 | } 154 | } 155 | 156 | int Pandora_Internal::UploadCameraCalibrationFile( 157 | const CameraCalibration calibs[5]) { 158 | std::string contents; 159 | int ret = 0; 160 | ret = GenerateCameraCalibration(calibs, &contents); 161 | if (ret != 0) { 162 | std::cout << "Generate Camera Calibration Failed" << std::endl; 163 | return ret; 164 | } 165 | 166 | return TcpCommandSetCalibration(tcp_command_client_, contents.c_str(), 167 | contents.size()); 168 | } 169 | 170 | int Pandora_Internal::GetCameraCalibration(CameraCalibration calibs[5]) { 171 | if (!got_camera_calibration_) { 172 | return -1; 173 | } 174 | 175 | for (int i = 0; i < 5; ++i) { 176 | /* assign */ 177 | calibs[i].cameraK = camera_calibs_[i].cameraK; 178 | calibs[i].cameraD = camera_calibs_[i].cameraD; 179 | calibs[i].cameraT = camera_calibs_[i].cameraT; 180 | calibs[i].cameraR = camera_calibs_[i].cameraR; 181 | } 182 | } 183 | 184 | int Pandora_Internal::ResetCameraClibration() { 185 | if (!tcp_command_client_) { 186 | std::cout << "Pandora Tcp Command Client is NULL" << std::endl; 187 | return -1; 188 | } 189 | return TcpCommandResetCalibration(tcp_command_client_); 190 | } 191 | 192 | void Pandora_Internal::GetCalibrationFromDevice() { 193 | if (!tcp_command_client_) { 194 | return; 195 | } 196 | int32_t ret = 0; 197 | while (enable_get_calibration_thr_ && !got_lidar_calibration_ && 198 | !got_camera_calibration_) { 199 | if (!got_lidar_calibration_) { 200 | // get lidar calibration. 201 | char *buffer = NULL; 202 | uint32_t len = 0; 203 | 204 | ret = TcpCommandGetLidarCalibration(tcp_command_client_, &buffer, &len); 205 | if (ret == 0 && buffer) { 206 | // success; 207 | got_lidar_calibration_ = true; 208 | if (pandar40p_) { 209 | ret = pandar40p_->LoadCorrectionFile(std::string(buffer)); 210 | if (ret != 0) { 211 | std::cout << "Parse Lidar Correction Error" << std::endl; 212 | got_lidar_calibration_ = false; 213 | } else { 214 | std::cout << "Parse Lidar Correction Success!!!" << std::endl; 215 | } 216 | } 217 | free(buffer); 218 | } 219 | } 220 | 221 | // If got lidar's calibration , and camera is disabled 222 | // stop to get camera's calibration 223 | if (!enable_camera_ && got_lidar_calibration_) break; 224 | 225 | if (!got_camera_calibration_) { 226 | // get camera calibration. 227 | char *buffer = NULL; 228 | uint32_t len = 0; 229 | 230 | ret = TcpCommandGetCalibration(tcp_command_client_, &buffer, &len); 231 | if (ret == 0 && buffer) { 232 | // success; 233 | ret = ParseCameraCalibration(std::string(buffer), camera_calibs_); 234 | if (ret == 0) { 235 | got_camera_calibration_ = true; 236 | std::cout << "Parse Camera Calibration Success!!!" << std::endl; 237 | } else { 238 | std::cout << "Parse Camera Calibration Error" << std::endl; 239 | free(buffer); 240 | continue; 241 | } 242 | 243 | std::vector cameras_k; 244 | std::vector cameras_d; 245 | 246 | for (int i = 0; i < 5; ++i) { 247 | cameras_k.push_back(camera_calibs_[i].cameraK); 248 | cameras_d.push_back(camera_calibs_[i].cameraD); 249 | } 250 | if (pandora_camera_) { 251 | pandora_camera_->loadIntrinsics(cameras_k, cameras_d); 252 | } 253 | 254 | free(buffer); 255 | std::cout << "TcpCommandGetCalibration Success" << std::endl; 256 | } 257 | } 258 | 259 | sleep(1); 260 | } 261 | } 262 | 263 | int Pandora_Internal::ParseCameraCalibration(const std::string contents, 264 | CameraCalibration calibs[5]) { 265 | std::cout << "Parse Camera Calibration..." << std::endl; 266 | if (contents.empty()) { 267 | std::cout << "string is empty" << std::endl; 268 | return -1; 269 | } 270 | YAML::Node yn = YAML::Load(contents); 271 | std::string cameraId; 272 | for (int id = 0; id < 5; ++id) { 273 | // cameraId = std::to_string(i); 274 | cameraId = boost::lexical_cast(id); 275 | cv::Mat intrinsicK, intrinsicD; 276 | // get intrinsicK 277 | if (yn[cameraId]["K"].IsDefined()) { 278 | intrinsicK = cv::Mat::zeros(3, 3, CV_64FC1); 279 | for (int i = 0; i < yn[cameraId]["K"].size(); ++i) { 280 | intrinsicK.at(i) = yn[cameraId]["K"][i].as(); 281 | } 282 | calibs[id].cameraK = intrinsicK; 283 | } else { 284 | printf("invalid intrinsicFile content\n"); 285 | return -1; 286 | } 287 | // GET intrinsicD 288 | if (yn[cameraId]["D"].IsDefined()) { 289 | // std::cout<<"type: " << yn[cameraId]["D"].Type()<(i) = yn[cameraId]["D"][i].as(); 293 | } 294 | calibs[id].cameraD = intrinsicD; 295 | } else { 296 | printf("invalid intrinsicFile content\n"); 297 | return -1; 298 | } 299 | // get camera 300 | if (yn[cameraId]["Extrinsic"]["rotation"]["x"].IsDefined() && 301 | yn[cameraId]["Extrinsic"]["rotation"]["y"].IsDefined() && 302 | yn[cameraId]["Extrinsic"]["rotation"]["z"].IsDefined() && 303 | yn[cameraId]["Extrinsic"]["rotation"]["w"].IsDefined()) { 304 | calibs[id].cameraR.push_back( 305 | yn[cameraId]["Extrinsic"]["rotation"]["w"].as()); 306 | calibs[id].cameraR.push_back( 307 | yn[cameraId]["Extrinsic"]["rotation"]["x"].as()); 308 | calibs[id].cameraR.push_back( 309 | yn[cameraId]["Extrinsic"]["rotation"]["y"].as()); 310 | calibs[id].cameraR.push_back( 311 | yn[cameraId]["Extrinsic"]["rotation"]["z"].as()); 312 | } else { 313 | printf("invalid intrinsicFile content\n"); 314 | return -1; 315 | } 316 | // get cameraR 317 | if (yn[cameraId]["Extrinsic"]["translation"]["x"].IsDefined() && 318 | yn[cameraId]["Extrinsic"]["translation"]["y"].IsDefined() && 319 | yn[cameraId]["Extrinsic"]["translation"]["z"].IsDefined()) { 320 | calibs[id].cameraT.push_back( 321 | yn[cameraId]["Extrinsic"]["translation"]["x"].as()); 322 | calibs[id].cameraT.push_back( 323 | yn[cameraId]["Extrinsic"]["translation"]["y"].as()); 324 | calibs[id].cameraT.push_back( 325 | yn[cameraId]["Extrinsic"]["translation"]["z"].as()); 326 | } else { 327 | printf("invalid intrinsicFile content\n"); 328 | return -1; 329 | } 330 | } 331 | return 0; 332 | } 333 | 334 | int Pandora_Internal::GenerateCameraCalibration( 335 | const CameraCalibration calibs[5], std::string *contents) { 336 | std::string cameraId; 337 | YAML::Node node; 338 | for (int id = 0; id < 5; ++id) { 339 | YAML::Node nodeK, nodeD, nodeT, nodeR; 340 | // nodeK.SetStyle(YAML::EmitterStyle::Flow); 341 | // nodeD.SetStyle(YAML::EmitterStyle::Flow); 342 | // nodeT.SetStyle(YAML::EmitterStyle::Flow); 343 | // nodeR.SetStyle(YAML::EmitterStyle::Flow); 344 | std::string id_str = boost::lexical_cast(id); 345 | int rowsK = calibs[id].cameraK.rows; 346 | int colsK = calibs[id].cameraK.cols; 347 | int rowsD = calibs[id].cameraD.rows; 348 | int colsD = calibs[id].cameraD.cols; 349 | 350 | for (int i = 0; i < rowsK * colsK; i++) { 351 | nodeK[i] = calibs[id].cameraK.at(i); 352 | } 353 | node[id_str]["K"] = nodeK; 354 | 355 | for (int i = 0; i < rowsD * colsD; i++) { 356 | nodeD[i] = calibs[id].cameraD.at(i); 357 | } 358 | node[id_str]["D"] = nodeD; 359 | 360 | for (int i = 0; i < calibs[id].cameraT.size(); i++) { 361 | nodeT[i] = boost::lexical_cast(calibs[id].cameraT[i]); 362 | 363 | node[id_str]["Extrinsic"]["rotation"]["w"] = calibs[id].cameraR[0]; 364 | node[id_str]["Extrinsic"]["rotation"]["x"] = calibs[id].cameraR[1]; 365 | node[id_str]["Extrinsic"]["rotation"]["y"] = calibs[id].cameraR[2]; 366 | node[id_str]["Extrinsic"]["rotation"]["z"] = calibs[id].cameraR[3]; 367 | 368 | node[id_str]["Extrinsic"]["translation"]["x"] = calibs[id].cameraT[0]; 369 | node[id_str]["Extrinsic"]["translation"]["y"] = calibs[id].cameraT[1]; 370 | node[id_str]["Extrinsic"]["translation"]["z"] = calibs[id].cameraT[2]; 371 | } 372 | } 373 | *contents = YAML::Dump(node); 374 | // std::cout<, double)> pcl_callback, 395 | boost::function gps_callback, uint16_t start_angle, 396 | const uint16_t pandoraCameraPort, 397 | boost::function matp, double timestamp, 398 | int picid, bool distortion)> 399 | cameraCallback, 400 | bool enable_camera, int tz, std::string frame_id) { 401 | internal_ = new Pandora_Internal( 402 | device_ip, lidar_port, gps_port, pcl_callback, gps_callback, start_angle, 403 | pandoraCameraPort, cameraCallback, enable_camera, tz, frame_id); 404 | } 405 | 406 | /** 407 | * @brief deconstructor 408 | */ 409 | Pandora::~Pandora() { delete internal_; } 410 | 411 | /** 412 | * @brief load the lidar correction file 413 | * @param contents The correction contents of lidar correction 414 | */ 415 | int Pandora::LoadLidarCorrectionFile(std::string correction_content) { 416 | internal_->LoadLidarCorrectionFile(correction_content); 417 | } 418 | 419 | /** 420 | * @brief Reset Lidar's start angle. 421 | * @param angle The start angle 422 | */ 423 | void Pandora::ResetLidarStartAngle(uint16_t start_angle) { 424 | internal_->ResetLidarStartAngle(start_angle); 425 | } 426 | 427 | /** 428 | * @brief Upload the camera calibration contents to Pandora Device. 429 | * @param calibs calibration contents , include camera intrinsics and 430 | * extrinsics. 431 | */ 432 | int Pandora::UploadCameraCalibration(const CameraCalibration calibs[5]) { 433 | internal_->UploadCameraCalibrationFile(calibs); 434 | } 435 | 436 | /** 437 | * @brief Get Camera's Calibration, include camera intrinsics and extrinsics. 438 | * @param calibs calibration contents , include camera intrinsics and 439 | * extrinsics. 440 | */ 441 | int Pandora::GetCameraCalibration(CameraCalibration calibs[5]) { 442 | internal_->GetCameraCalibration(calibs); 443 | } 444 | 445 | /** 446 | * @brief Reset camera calibration as factory-set. 447 | */ 448 | int Pandora::ResetCameraClibration() { internal_->ResetCameraClibration(); } 449 | 450 | /** 451 | * @brief Run SDK. 452 | */ 453 | int Pandora::Start() { internal_->Start(); } 454 | 455 | /** 456 | * @brief Stop SDK. 457 | */ 458 | void Pandora::Stop() { internal_->Stop(); } 459 | 460 | } // namespace hesai 461 | } // namespace drivers 462 | } // namespace apollo 463 | -------------------------------------------------------------------------------- /src/pandora_camera.cc: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * Copyright 2018 The Apollo Authors. 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 | #include 18 | #include 19 | 20 | #include 21 | #include 22 | 23 | #include "src/pandora_camera.h" 24 | #include "src/pandora_client.h" 25 | 26 | namespace apollo { 27 | namespace drivers { 28 | namespace hesai { 29 | 30 | cv::Size HesaiLidarSDK_IMAGE_SIZE(IMAGE_WIDTH, IMAGE_HEIGHT); 31 | 32 | static int CameraClientCallback(void *handle, int cmd, void *param, 33 | void *userp) { 34 | PandoraPic *pic = static_cast(param); 35 | PandoraCamera *pSDK = static_cast(userp); 36 | pSDK->pushPicture(pic); 37 | return 0; 38 | } 39 | 40 | PandoraCamera::PandoraCamera( 41 | std::string device_ip, const uint16_t pandoraCameraPort, 42 | boost::function matp, double timestamp, 43 | int picid, bool distortion)> 44 | camera_callback, 45 | boost::function connectionChanged, int tz) { 46 | ip_ = device_ip; 47 | 48 | for (int i = 0; i < CAMERA_NUM; ++i) { 49 | /* init pic lock / sem */ 50 | sem_init(&pic_sem_[i], 0, 0); 51 | pthread_mutex_init(&pic_lock_[i], NULL); 52 | process_pic_thread_[i] = NULL; 53 | } 54 | 55 | continue_process_pic_ = false; 56 | need_remap_ = false; 57 | camera_port_ = pandoraCameraPort; 58 | pandora_client_ = NULL; 59 | camera_callback_ = camera_callback; 60 | connection_changed_ = connectionChanged; 61 | tz_second_ = tz * 3600; 62 | } 63 | 64 | PandoraCamera::~PandoraCamera() { 65 | Stop(); 66 | for (int i = 0; i < CAMERA_NUM; ++i) 67 | { 68 | sem_destroy(&pic_sem_[i]); 69 | pthread_mutex_destroy(&pic_lock_[i]); 70 | } 71 | } 72 | 73 | int PandoraCamera::Start() { 74 | continue_process_pic_ = true; 75 | pandora_client_ = 76 | PandoraClientNew(ip_.c_str(), camera_port_, CameraClientCallback, 77 | static_cast(this)); 78 | if (!pandora_client_) { 79 | continue_process_pic_ = false; 80 | return -1; 81 | } 82 | 83 | for (int i = 0; i < CAMERA_NUM; ++i) { 84 | process_pic_thread_[i] = 85 | new boost::thread(boost::bind(&PandoraCamera::processPic, this , i)); 86 | if (!process_pic_thread_[i]) { 87 | continue_process_pic_ = false; 88 | PandoraClientDestroy(pandora_client_); 89 | pandora_client_ = NULL; 90 | return -1; 91 | } 92 | } 93 | return 0; 94 | } 95 | 96 | void PandoraCamera::Stop() { 97 | continue_process_pic_ = false; 98 | if (pandora_client_) { 99 | PandoraClientDestroy(pandora_client_); 100 | } 101 | pandora_client_ = NULL; 102 | 103 | for (int i = 0; i < CAMERA_NUM; ++i) { 104 | if (process_pic_thread_[i]) { 105 | process_pic_thread_[i]->join(); 106 | delete process_pic_thread_[i]; 107 | } 108 | process_pic_thread_[i] = NULL; 109 | } 110 | 111 | } 112 | 113 | void PandoraCamera::pushPicture(PandoraPic *pic) { 114 | pthread_mutex_lock(&pic_lock_[pic->header.pic_id]); 115 | pic_list_[pic->header.pic_id].push_back(pic); 116 | pthread_mutex_unlock(&pic_lock_[pic->header.pic_id]); 117 | sem_post(&pic_sem_[pic->header.pic_id]); 118 | } 119 | 120 | void PandoraCamera::processPic(int pic_id) { 121 | while (continue_process_pic_) { 122 | struct timespec ts; 123 | if (clock_gettime(CLOCK_REALTIME, &ts) == -1) { 124 | printf("processPic, get time error\n"); 125 | } 126 | 127 | ts.tv_sec += 1; 128 | if (sem_timedwait(&pic_sem_[pic_id], &ts) == -1) { 129 | continue; 130 | } 131 | 132 | pthread_mutex_lock(&pic_lock_[pic_id]); 133 | PandoraPic *pic = pic_list_[pic_id].front(); 134 | pic_list_[pic_id].pop_front(); 135 | pthread_mutex_unlock(&pic_lock_[pic_id]); 136 | 137 | if (pic == NULL) { 138 | printf("pic is NULL\n"); 139 | return; 140 | } 141 | 142 | struct tm t; 143 | t.tm_sec = pic->header.UTC_Time.UTC_Second; 144 | t.tm_min = pic->header.UTC_Time.UTC_Minute; 145 | t.tm_hour = pic->header.UTC_Time.UTC_Hour; 146 | t.tm_mday = pic->header.UTC_Time.UTC_Day; 147 | t.tm_mon = pic->header.UTC_Time.UTC_Month - 1; 148 | t.tm_year = pic->header.UTC_Time.UTC_Year + 2000 - 1900; 149 | t.tm_isdst = 0; 150 | double timestamp = mktime(&t) + pic->header.timestamp / 1000000.0; 151 | boost::shared_ptr cvMatPic(new cv::Mat()); 152 | switch (pic->header.pic_id) { 153 | case 0: 154 | //yuv422ToCvmat(cvMatPic, pic->yuv, pic->header.width, pic->header.height, 155 | // 8); 156 | //if (need_remap_) 157 | // remap(cvMatPic->clone(), *cvMatPic, mapx_[pic->header.pic_id], 158 | // mapy_[pic->header.pic_id], CV_INTER_LINEAR); 159 | //break; 160 | case 1: 161 | case 2: 162 | case 3: 163 | case 4: 164 | uint8_t *bmp; 165 | uint32_t bmpSize; 166 | decompressJpeg(static_cast(pic->yuv), pic->header.len, &bmp, 167 | &bmpSize); 168 | 169 | *cvMatPic = cv::Mat(IMAGE_HEIGHT, IMAGE_WIDTH, CV_8UC3, bmp).clone(); 170 | if (need_remap_) 171 | remap(cvMatPic->clone(), *cvMatPic, mapx_[pic->header.pic_id], 172 | mapy_[pic->header.pic_id], CV_INTER_LINEAR); 173 | 174 | free(bmp); 175 | bmp = NULL; 176 | break; 177 | 178 | default: 179 | free(pic->yuv); 180 | pic->yuv = NULL; 181 | free(pic); 182 | pic = NULL; 183 | printf("wrong pic id\n"); 184 | return; 185 | } 186 | if (camera_callback_) { 187 | camera_callback_(cvMatPic, timestamp + tz_second_, pic->header.pic_id, 188 | need_remap_); 189 | } 190 | free(pic->yuv); 191 | pic->yuv = NULL; 192 | free(pic); 193 | pic = NULL; 194 | } 195 | } 196 | 197 | bool PandoraCamera::loadIntrinsics(const std::vector cameras_k, 198 | const std::vector cameras_d) { 199 | for (int i = 0; i < CAMERA_NUM; i++) { 200 | cv::Mat mapx = cv::Mat(HesaiLidarSDK_IMAGE_SIZE, CV_32FC1); 201 | cv::Mat mapy = cv::Mat(HesaiLidarSDK_IMAGE_SIZE, CV_32FC1); 202 | cv::Mat R = cv::Mat::eye(3, 3, CV_32F); 203 | initUndistortRectifyMap(cameras_k[i], cameras_d[i], R, cameras_k[i], 204 | HesaiLidarSDK_IMAGE_SIZE, CV_32FC1, mapx, mapy); 205 | mapx_.push_back(mapx); 206 | mapy_.push_back(mapy); 207 | } 208 | need_remap_ = true; 209 | return true; 210 | } 211 | 212 | void PandoraCamera::yuvToRgb(const int iY, const int iU, const int iV, int *iR, 213 | int *iG, int *iB) { 214 | assert(&iR != NULL && &iG != NULL && &iB != NULL); 215 | 216 | *iR = iY + 1.13983 * (iV - 128); 217 | *iG = iY - 0.39465 * (iU - 128) - 0.58060 * (iV - 128); 218 | *iB = iY + 2.03211 * (iU - 128); 219 | 220 | *iR = *iR > 255 ? 255 : *iR; 221 | *iR = *iR < 0 ? 0 : *iR; 222 | 223 | *iG = *iG > 255 ? 255 : *iG; 224 | *iG = *iG < 0 ? 0 : *iG; 225 | 226 | *iB = *iB > 255 ? 255 : *iB; 227 | *iB = *iB < 0 ? 0 : *iB; 228 | } 229 | 230 | void PandoraCamera::yuv422ToRgb24(const unsigned char *uyvy422, 231 | unsigned char *rgb24, const int width, 232 | const int height) { 233 | int iR, iG, iB; 234 | int iY0, iY1, iU, iV; 235 | int i = 0; 236 | int j = 0; 237 | for (i = 0; i < width * height * 2; i += 4) { 238 | iU = uyvy422[i + 0]; 239 | iY0 = uyvy422[i + 1]; 240 | iV = uyvy422[i + 2]; 241 | iY1 = uyvy422[i + 3]; 242 | 243 | yuvToRgb(iY0, iU, iV, &iR, &iG, &iB); 244 | rgb24[j++] = iB; 245 | rgb24[j++] = iG; 246 | rgb24[j++] = iR; 247 | yuvToRgb(iY1, iU, iV, &iR, &iG, &iB); 248 | rgb24[j++] = iB; 249 | rgb24[j++] = iG; 250 | rgb24[j++] = iR; 251 | } 252 | } 253 | 254 | void PandoraCamera::yuv422ToCvmat(boost::shared_ptr dst, 255 | const void *pYUV422, const int nWidth, 256 | const int nHeight, const int bitDepth) { 257 | if (!pYUV422) { 258 | return; 259 | } 260 | unsigned char *rgb24_buffer = new unsigned char[nWidth * nHeight * 3]; 261 | yuv422ToRgb24((unsigned char *)pYUV422, rgb24_buffer, nWidth, nHeight); 262 | *dst = cv::Mat(nHeight, nWidth, CV_8UC3, rgb24_buffer).clone(); 263 | delete[] rgb24_buffer; 264 | } 265 | 266 | static void my_output_message(j_common_ptr ptr) { return; } 267 | 268 | static void print_mem(unsigned char *mem, unsigned int size) { 269 | int i = 0; 270 | for (i = 0; i < size; i++) { 271 | printf("%02x ", mem[i]); 272 | } 273 | printf("\n"); 274 | } 275 | 276 | int PandoraCamera::decompressJpeg(uint8_t *jpgBuffer, const uint32_t jpgSize, 277 | uint8_t **bmp, uint32_t *bmpSize) { 278 | struct jpeg_decompress_struct cinfo; 279 | struct jpeg_error_mgr jerr; 280 | uint8_t *bmpBuffer; 281 | int rowStride, width, height, pixelSize; 282 | 283 | cinfo.err = jpeg_std_error(&jerr); 284 | cinfo.err->output_message = my_output_message; 285 | cinfo.err->error_exit = my_output_message; 286 | 287 | jpeg_create_decompress(&cinfo); 288 | jpeg_mem_src(&cinfo, jpgBuffer, jpgSize); 289 | 290 | int rc = jpeg_read_header(&cinfo, TRUE); 291 | if (rc != 1) { 292 | return -1; 293 | } 294 | 295 | jpeg_start_decompress(&cinfo); 296 | 297 | width = cinfo.output_width; 298 | height = cinfo.output_height; 299 | pixelSize = cinfo.output_components; 300 | *bmpSize = width * height * pixelSize; 301 | bmpBuffer = (unsigned char *)malloc(*bmpSize); 302 | rowStride = width * pixelSize; 303 | 304 | while (cinfo.output_scanline < cinfo.output_height) { 305 | unsigned char *buffer_array[1]; 306 | buffer_array[0] = bmpBuffer + (cinfo.output_scanline) * rowStride; 307 | 308 | jpeg_read_scanlines(&cinfo, buffer_array, 1); 309 | } 310 | *bmp = bmpBuffer; 311 | jpeg_finish_decompress(&cinfo); 312 | jpeg_destroy_decompress(&cinfo); 313 | return 0; 314 | } 315 | 316 | } // namespace hesai 317 | } // namespace drivers 318 | } // namespace apollo 319 | -------------------------------------------------------------------------------- /src/pandora_camera.h: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * Copyright 2018 The Apollo Authors. 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 | #ifndef SRC_PANDORA_CAMERA_H_ 18 | #define SRC_PANDORA_CAMERA_H_ 19 | 20 | #include 21 | #include 22 | #include 23 | #include 24 | 25 | #include 26 | #include 27 | #include 28 | 29 | #include 30 | #include 31 | 32 | #include "src/pandora_client.h" 33 | 34 | namespace apollo { 35 | namespace drivers { 36 | namespace hesai { 37 | 38 | #define CAMERA_NUM 5 39 | #define IMAGE_WIDTH 1280 40 | #define IMAGE_HEIGHT 720 41 | 42 | class PandoraCamera { 43 | public: 44 | /** 45 | * @brief Constructor 46 | * @param device_ip The ip of the device 47 | * lidar_port The port number of lidar data 48 | * gps_port The port number of gps data 49 | * pcl_callback The callback of PCL data structure 50 | * gps_callback The callback of GPS structure 51 | * type The device type 52 | */ 53 | PandoraCamera( 54 | std::string device_ip, const uint16_t pandoraCameraPort, 55 | boost::function matp, double timestamp, 56 | int picid, bool distortion)> 57 | camera_callback, 58 | boost::function connection_changed, int tz); 59 | ~PandoraCamera(); 60 | 61 | /** 62 | * @brief load the correction file 63 | * @param angle The start angle 64 | */ 65 | 66 | bool loadIntrinsics(const std::vector cameras_k, 67 | const std::vector cameras_d); 68 | 69 | int Start(); 70 | void Stop(); 71 | void pushPicture(PandoraPic *pic); 72 | 73 | private: 74 | void processPic(int pic_id); 75 | 76 | int decompressJpeg(uint8_t *jpgBuffer, const uint32_t jpgSize, uint8_t **bmp, 77 | uint32_t *bmpSize); 78 | void yuv422ToCvmat(boost::shared_ptr dst, const void *pYUV422, 79 | const int nWidth, const int nHeight, const int bitDepth); 80 | void yuv422ToRgb24(const uint8_t *uyvy422, uint8_t *rgb24, const int width, 81 | const int height); 82 | void yuvToRgb(const int iY, const int iU, const int iV, int *iR, int *iG, 83 | int *iB); 84 | 85 | pthread_mutex_t pic_lock_[CAMERA_NUM]; 86 | sem_t pic_sem_[CAMERA_NUM]; 87 | boost::thread *process_pic_thread_[CAMERA_NUM]; 88 | bool continue_process_pic_; 89 | bool need_remap_; 90 | std::string ip_; 91 | uint16_t camera_port_; 92 | void *pandora_client_; 93 | std::list pic_list_[CAMERA_NUM]; 94 | std::vector mapx_; 95 | std::vector mapy_; 96 | boost::function matp, double timestamp, 97 | int pic_id, bool distortion)> 98 | camera_callback_; 99 | boost::function connection_changed_; 100 | 101 | int tz_second_; 102 | }; 103 | 104 | } // namespace hesai 105 | } // namespace drivers 106 | } // namespace apollo 107 | 108 | #endif // SRC_PANDORA_CAMERA_H_ 109 | -------------------------------------------------------------------------------- /src/pandora_client.c: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * Copyright 2018 The Apollo Authors. 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 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | #include 27 | #include 28 | #include 29 | #include 30 | #include 31 | #include 32 | #include 33 | #include 34 | #include 35 | 36 | #include "src/pandora_client.h" 37 | #include "src/util.h" 38 | 39 | typedef struct _PandoraClient_s { 40 | pthread_mutex_t cliSocketLock; 41 | int cliSocket; 42 | 43 | pthread_t receiveTask; 44 | pthread_t heartBeatTask; 45 | 46 | int exit; 47 | char* ip; 48 | int port; 49 | 50 | CallBack callback; 51 | void* userp; 52 | 53 | unsigned int position[PANDORA_CAMERA_UNIT]; 54 | unsigned int startTimestamp[PANDORA_CAMERA_UNIT]; 55 | PandoraPic* pics[PANDORA_CAMERA_UNIT]; 56 | } PandoraClient; 57 | 58 | void PandoraClientTask(void* handle); 59 | void PandoraClientHeartBeatTask(void* handle); 60 | void parseHeader(char* header, int len, PandoraPicHeader* picHeader) { 61 | int index = 0; 62 | picHeader->SOP[0] = header[index]; 63 | picHeader->SOP[1] = header[index + 1]; 64 | index += 2; 65 | 66 | picHeader->pic_id = header[index]; 67 | picHeader->type = header[index + 1]; 68 | index += 2; 69 | 70 | picHeader->width = 71 | (header[index + 0] & 0xff) << 24 | (header[index + 1] & 0xff) << 16 | 72 | (header[index + 2] & 0xff) << 8 | (header[index + 3] & 0xff) << 0; 73 | index += 4; 74 | 75 | picHeader->height = 76 | (header[index + 0] & 0xff) << 24 | (header[index + 1] & 0xff) << 16 | 77 | (header[index + 2] & 0xff) << 8 | (header[index + 3] & 0xff) << 0; 78 | index += 4; 79 | 80 | picHeader->timestamp = 81 | (header[index + 0] & 0xff) << 24 | (header[index + 1] & 0xff) << 16 | 82 | (header[index + 2] & 0xff) << 8 | (header[index + 3] & 0xff) << 0; 83 | index += 4; 84 | 85 | picHeader->len = 86 | (header[index + 0] & 0xff) << 24 | (header[index + 1] & 0xff) << 16 | 87 | (header[index + 2] & 0xff) << 8 | (header[index + 3] & 0xff) << 0; 88 | index += 4; 89 | 90 | picHeader->totalLen = 91 | (header[index + 0] & 0xff) << 24 | (header[index + 1] & 0xff) << 16 | 92 | (header[index + 2] & 0xff) << 8 | (header[index + 3] & 0xff) << 0; 93 | 94 | index += 4; 95 | 96 | picHeader->position = 97 | (header[index + 0] & 0xff) << 24 | (header[index + 1] & 0xff) << 16 | 98 | (header[index + 2] & 0xff) << 8 | (header[index + 3] & 0xff) << 0; 99 | #ifdef UTC_TIME 100 | index += 4; 101 | picHeader->UTC_Time.UTC_Year = header[index++]; 102 | picHeader->UTC_Time.UTC_Month = header[index++]; 103 | picHeader->UTC_Time.UTC_Day = header[index++]; 104 | picHeader->UTC_Time.UTC_Hour = header[index++]; 105 | picHeader->UTC_Time.UTC_Minute = header[index++]; 106 | picHeader->UTC_Time.UTC_Second = header[index]; 107 | #endif 108 | } 109 | void* PandoraClientNew(const char* ip, int port, CallBack callback, 110 | void* userp) { 111 | if (!ip || !callback || !userp) { 112 | printf("Bad Parameter\n"); 113 | return NULL; 114 | } 115 | 116 | int ret = 0; 117 | 118 | PandoraClient* client = (PandoraClient*)malloc(sizeof(PandoraClient)); 119 | if (!client) { 120 | printf("No Memory\n"); 121 | return NULL; 122 | } 123 | 124 | memset(client, 0, sizeof(PandoraClient)); 125 | client->callback = callback; 126 | client->userp = userp; 127 | client->cliSocket = -1; 128 | client->ip = strdup(ip); 129 | client->port = port; 130 | 131 | pthread_mutex_init(&client->cliSocketLock, NULL); 132 | 133 | ret = pthread_create(&client->receiveTask, NULL, (void*)PandoraClientTask, 134 | (void*)client); 135 | if (ret != 0) { 136 | printf("Create Task Failed\n"); 137 | free(client); 138 | return NULL; 139 | } 140 | 141 | ret = pthread_create(&client->heartBeatTask, NULL, 142 | (void*)PandoraClientHeartBeatTask, (void*)client); 143 | if (ret != 0) { 144 | printf("Create heart beat Task Failed\n"); 145 | client->exit = 1; 146 | pthread_join(client->receiveTask, NULL); 147 | free(client); 148 | return NULL; 149 | } 150 | return (void*)client; 151 | } 152 | 153 | void PandoraClientDestroy(void* handle) { 154 | PandoraClient* client = (PandoraClient*)handle; 155 | if (!client) { 156 | printf("Bad Parameter\n"); 157 | return; 158 | } 159 | 160 | client->exit = 1; 161 | pthread_join(client->heartBeatTask, NULL); 162 | pthread_join(client->receiveTask, NULL); 163 | close(client->cliSocket); 164 | free(client); 165 | } 166 | 167 | void PandoraClientHeartBeatTask(void* handle) { 168 | PandoraClient* client = (PandoraClient*)handle; 169 | if (!client) { 170 | printf("Bad Parameter\n"); 171 | return; 172 | } 173 | int ret = 0; 174 | 175 | 176 | while (!client->exit) { 177 | int clifd = -1; 178 | 179 | pthread_mutex_lock(&client->cliSocketLock); 180 | if(client->cliSocket != -1) { 181 | clifd = client->cliSocket; 182 | } else { 183 | pthread_mutex_unlock(&client->cliSocketLock); 184 | sleep(1); 185 | continue; 186 | } 187 | pthread_mutex_unlock(&client->cliSocketLock); 188 | 189 | ret = select_fd(clifd, 1, WAIT_FOR_WRITE); 190 | if (ret > 0) { 191 | ret = write(clifd, "HEARTBEAT", strlen("HEARTBEAT")); 192 | if (ret < 0) { 193 | // printf("Write Error\n"); 194 | } 195 | } 196 | 197 | sleep(1); 198 | } 199 | } 200 | 201 | void PandoraClientTask(void* handle) { 202 | PandoraClient* client = (PandoraClient*)handle; 203 | if (!client) { 204 | printf("Bad Parameter\n"); 205 | return; 206 | } 207 | 208 | int connfd = client->cliSocket; 209 | 210 | while (!client->exit) { 211 | if (client->cliSocket == -1) { 212 | printf("Camera: Connecting......\n"); 213 | connfd = tcp_open(client->ip, client->port); 214 | if (connfd < 0) { 215 | printf("Connect to server failed\n"); 216 | sleep(1); 217 | continue; 218 | } 219 | pthread_mutex_lock(&client->cliSocketLock); 220 | client->cliSocket = connfd; 221 | pthread_mutex_unlock(&client->cliSocketLock); 222 | printf("Camera: connect to server successfully!\n"); 223 | struct timeval tv; 224 | tv.tv_sec = 5; /* 5 Secs Timeout */ 225 | tv.tv_usec = 0; // Not init'ing this can cause strange errors 226 | setsockopt(client->cliSocket, SOL_SOCKET, SO_RCVTIMEO, (const char*)&tv, 227 | sizeof(struct timeval)); 228 | connfd = client->cliSocket; 229 | } 230 | 231 | int ret = 0; 232 | // printf("start select_fd for read\n"); 233 | ret = select_fd(connfd, 1, WAIT_FOR_READ); 234 | if (ret == 0) { 235 | printf("select for read wrong\n"); 236 | pthread_mutex_lock(&client->cliSocketLock); 237 | close(client->cliSocket); 238 | client->cliSocket = -1; 239 | pthread_mutex_unlock(&client->cliSocketLock); 240 | sleep(5); 241 | continue; 242 | } else if (ret > 0) { 243 | char header[64]; 244 | int n = sys_readn(connfd, header, 2); 245 | if (n < 0) { 246 | printf("read header wrong!\n"); 247 | pthread_mutex_lock(&client->cliSocketLock); 248 | close(client->cliSocket); 249 | client->cliSocket = -1; 250 | pthread_mutex_unlock(&client->cliSocketLock); 251 | continue; 252 | } 253 | 254 | if (header[0] != 0x47 || header[1] != 0x74) { 255 | printf("InValid Header SOP\n"); 256 | printf("%02x %02x \n", header[0], header[1]); 257 | exit(0); 258 | continue; 259 | } 260 | 261 | n = sys_readn(connfd, header + 2, PANDORA_CLIENT_HEADER_SIZE - 2); 262 | if (n <= 0) { 263 | printf("read header2 wrong!\n"); 264 | pthread_mutex_lock(&client->cliSocketLock); 265 | close(client->cliSocket); 266 | client->cliSocket = -1; 267 | pthread_mutex_unlock(&client->cliSocketLock); 268 | continue; 269 | } 270 | PandoraPic* pic = (PandoraPic*)malloc(sizeof(PandoraPic)); 271 | if (!pic) { 272 | printf("No Memory\n"); 273 | continue; 274 | } 275 | 276 | parseHeader(header, n + 2, &pic->header); 277 | 278 | pic->yuv = malloc(pic->header.len); 279 | if (!pic->yuv) { 280 | printf("No Memory\n"); 281 | free(pic); 282 | continue; 283 | } 284 | 285 | n = sys_readn(connfd, pic->yuv, pic->header.len); 286 | if (n <= 0) { 287 | printf("read pic_yuv wrong!\n"); 288 | pthread_mutex_lock(&client->cliSocketLock); 289 | close(client->cliSocket); 290 | client->cliSocket = -1; 291 | pthread_mutex_unlock(&client->cliSocketLock); 292 | continue; 293 | } 294 | if (n != pic->header.len) { 295 | printf("picLength wrong\n"); 296 | free(pic->yuv); 297 | free(pic); 298 | continue; 299 | } 300 | 301 | // check frame id 302 | if (pic->header.pic_id < 0 || pic->header.pic_id >= PANDORA_CAMERA_UNIT) { 303 | free(pic->yuv); 304 | free(pic); 305 | continue; 306 | } 307 | 308 | // check the desired frame postion packet. 309 | if (pic->header.position != client->position[pic->header.pic_id]) { 310 | client->position[pic->header.pic_id] = 0; 311 | free(pic->yuv); 312 | free(pic); 313 | continue; 314 | } 315 | 316 | if (!client->pics[pic->header.pic_id]) { 317 | client->pics[pic->header.pic_id] = malloc(sizeof(PandoraPic)); 318 | memcpy(client->pics[pic->header.pic_id], pic, sizeof(PandoraPic)); 319 | client->pics[pic->header.pic_id]->yuv = malloc(pic->header.totalLen); 320 | } 321 | 322 | memcpy(client->pics[pic->header.pic_id]->yuv + 323 | client->position[pic->header.pic_id], 324 | pic->yuv, pic->header.len); 325 | 326 | if (client->position[pic->header.pic_id] == 0) { 327 | client->startTimestamp[pic->header.pic_id] = pic->header.timestamp; 328 | } 329 | 330 | client->position[pic->header.pic_id] += pic->header.len; 331 | 332 | if (client->position[pic->header.pic_id] == pic->header.totalLen) { 333 | client->pics[pic->header.pic_id]->header.len = pic->header.totalLen; 334 | client->pics[pic->header.pic_id]->header.timestamp = 335 | pic->header.timestamp; 336 | client->pics[pic->header.pic_id]->header.UTC_Time = 337 | pic->header.UTC_Time; 338 | // a whole frame 339 | if (client->callback) 340 | client->callback(client, 0, client->pics[pic->header.pic_id], 341 | client->userp); 342 | client->pics[pic->header.pic_id] = NULL; 343 | client->position[pic->header.pic_id] = 0; 344 | } 345 | free(pic->yuv); 346 | free(pic); 347 | 348 | } else { 349 | printf("Read Error\n"); 350 | pthread_mutex_lock(&client->cliSocketLock); 351 | close(client->cliSocket); 352 | client->cliSocket = -1; 353 | pthread_mutex_unlock(&client->cliSocketLock); 354 | continue; 355 | } 356 | } 357 | } 358 | -------------------------------------------------------------------------------- /src/pandora_client.h: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * Copyright 2018 The Apollo Authors. 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 | #ifndef SRC_PANDORA_CLIENT_H_ 18 | #define SRC_PANDORA_CLIENT_H_ 19 | 20 | #ifdef __cplusplus 21 | extern "C" { 22 | #endif 23 | 24 | #define PANDORA_CAMERA_UNIT (5) 25 | 26 | #define UTC_TIME 27 | #ifdef UTC_TIME 28 | typedef struct { 29 | unsigned char UTC_Year; 30 | unsigned char UTC_Month; 31 | unsigned char UTC_Day; 32 | unsigned char UTC_Hour; 33 | unsigned char UTC_Minute; 34 | unsigned char UTC_Second; 35 | } UTC_Time_T; 36 | #endif 37 | typedef struct _PandoraPicHeader_s { 38 | char SOP[2]; 39 | unsigned char pic_id; 40 | unsigned char type; 41 | unsigned int width; 42 | unsigned int height; 43 | unsigned timestamp; 44 | unsigned len; 45 | unsigned int totalLen; 46 | unsigned int position; 47 | #ifdef UTC_TIME 48 | UTC_Time_T UTC_Time; 49 | #endif 50 | } PandoraPicHeader; 51 | 52 | typedef struct _PandoraPic { 53 | PandoraPicHeader header; 54 | void* yuv; 55 | } PandoraPic; 56 | 57 | #ifdef UTC_TIME 58 | #define PANDORA_CLIENT_HEADER_SIZE (34) 59 | #else 60 | #define PANDORA_CLIENT_HEADER_SIZE (28) 61 | #endif 62 | 63 | typedef int (*CallBack)(void* handle, int cmd, void* param, void* userp); 64 | 65 | void* PandoraClientNew(const char* ip, int port, CallBack callback, 66 | void* userp); 67 | void PandoraClientDestroy(void* handle); 68 | 69 | #ifdef __cplusplus 70 | } 71 | #endif 72 | 73 | #endif // SRC_PANDORA_CLIENT_H_ 74 | -------------------------------------------------------------------------------- /src/tcp_command_client.c: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * Copyright 2018 The Apollo Authors. 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 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | #include 27 | #include 28 | #include 29 | #include 30 | #include 31 | #include 32 | #include 33 | #include 34 | #include 35 | #include 36 | #include 37 | #include "src/util.h" 38 | #include "src/tcp_command_client.h" 39 | 40 | typedef struct TcpCommandClient_s { 41 | pthread_mutex_t lock; 42 | pthread_t tid; 43 | 44 | int exit; 45 | 46 | char ip[256]; 47 | unsigned short port; 48 | 49 | int fd; 50 | } TcpCommandClient; 51 | 52 | #ifdef DEBUG 53 | static void print_mem(char* mem, int len) { 54 | int i = 0; 55 | for (int i = 0; i < len; ++i) { 56 | printf("%02x ", mem[i]); 57 | } 58 | printf("\n"); 59 | } 60 | #else 61 | static void print_mem(char* mem, int len) {} 62 | #endif 63 | 64 | static int tcpCommandHeaderParser(unsigned char* buffer, int len, 65 | TcpCommandHeader* header) { 66 | int index = 0; 67 | header->cmd = buffer[index++]; 68 | header->ret_code = buffer[index++]; 69 | header->len = 70 | ((buffer[index] & 0xff) << 24) | ((buffer[index + 1] & 0xff) << 16) | 71 | ((buffer[index + 2] & 0xff) << 8) | ((buffer[index + 3] & 0xff) << 0); 72 | return 0; 73 | } 74 | 75 | static int tcpCommandReadCommand(int connfd, TC_Command* cmd) { 76 | int ret = 0; 77 | if (!cmd) { 78 | return -1; 79 | } 80 | memset(cmd, 0, sizeof(TC_Command)); 81 | unsigned char buffer[1500]; 82 | ret = sys_readn(connfd, buffer, 2); 83 | if (ret <= 0 || buffer[0] != 0x47 || buffer[1] != 0x74) { 84 | printf("Server Read failed\n"); 85 | return -1; 86 | } 87 | 88 | ret = sys_readn(connfd, buffer + 2, 6); 89 | if (ret != 6) { 90 | printf("Server Read failed\n"); 91 | return -1; 92 | } 93 | 94 | print_mem(buffer, 8); 95 | 96 | tcpCommandHeaderParser(buffer + 2, 6, &cmd->header); 97 | 98 | if (cmd->header.len > 0) { 99 | cmd->data = malloc(cmd->header.len); 100 | if (!cmd->data) { 101 | printf("malloc data error\n"); 102 | return -1; 103 | } 104 | } 105 | 106 | ret = sys_readn(connfd, cmd->data, cmd->header.len); 107 | if (ret != cmd->header.len) { 108 | free(cmd->data); 109 | printf("Server Read failed\n"); 110 | return -1; 111 | } 112 | 113 | // cmd->ret_size = cmd->header.len; 114 | 115 | print_mem(cmd->data, cmd->header.len); 116 | 117 | return 0; 118 | } 119 | 120 | static int TcpCommand_buildHeader(char* buffer, TC_Command* cmd) { 121 | if (!buffer) { 122 | return -1; 123 | } 124 | int index = 0; 125 | buffer[index++] = 0x47; 126 | buffer[index++] = 0x74; 127 | buffer[index++] = cmd->header.cmd; 128 | buffer[index++] = cmd->header.ret_code; // color or mono 129 | buffer[index++] = (cmd->header.len >> 24) & 0xff; 130 | buffer[index++] = (cmd->header.len >> 16) & 0xff; 131 | buffer[index++] = (cmd->header.len >> 8) & 0xff; 132 | buffer[index++] = (cmd->header.len >> 0) & 0xff; 133 | 134 | return index; 135 | } 136 | 137 | static PTC_ErrCode tcpCommandClient_SendCmd(TcpCommandClient* client, 138 | TC_Command* cmd) { 139 | if (!client && !cmd) { 140 | printf("Bad Parameter\n"); 141 | return PTC_ERROR_BAD_PARAMETER; 142 | } 143 | 144 | if (cmd->header.len != 0 && cmd->data == NULL) { 145 | printf("Bad Parameter : payload is null\n"); 146 | return PTC_ERROR_BAD_PARAMETER; 147 | } 148 | 149 | pthread_mutex_lock(&client->lock); 150 | int fd = tcp_open(client->ip, client->port); 151 | if (fd < 0) { 152 | pthread_mutex_unlock(&client->lock); 153 | return PTC_ERROR_CONNECT_SERVER_FAILED; 154 | } 155 | 156 | unsigned char buffer[128]; 157 | int size = TcpCommand_buildHeader(buffer, cmd); 158 | 159 | print_mem(buffer, size); 160 | int ret = write(fd, buffer, size); 161 | if (ret != size) { 162 | close(fd); 163 | pthread_mutex_unlock(&client->lock); 164 | printf("Write header error\n"); 165 | return PTC_ERROR_TRANSFER_FAILED; 166 | } 167 | 168 | if (cmd->header.len > 0 && cmd->data) { 169 | print_mem(cmd->data, cmd->header.len); 170 | ret = write(fd, cmd->data, cmd->header.len); 171 | if (ret != cmd->header.len) { 172 | printf("Write Payload error\n"); 173 | close(fd); 174 | pthread_mutex_unlock(&client->lock); 175 | return PTC_ERROR_TRANSFER_FAILED; 176 | } 177 | } 178 | 179 | TC_Command feedBack; 180 | ret = tcpCommandReadCommand(fd, &feedBack); 181 | if (ret != 0) { 182 | printf("Receive feed back failed!!!\n"); 183 | close(fd); 184 | pthread_mutex_unlock(&client->lock); 185 | return PTC_ERROR_TRANSFER_FAILED; 186 | } 187 | // printf("feed back : %d %d %d \n", feedBack.header.len , 188 | // cmd->header.ret_code, 189 | // cmd->header.cmd); 190 | 191 | cmd->ret_data = feedBack.data; 192 | cmd->ret_size = feedBack.header.len; 193 | cmd->header.ret_code = feedBack.header.ret_code; 194 | 195 | close(fd); 196 | pthread_mutex_unlock(&client->lock); 197 | return PTC_ERROR_NO_ERROR; 198 | } 199 | 200 | void* TcpCommandClientNew(const char* ip, const unsigned short port) { 201 | if (!ip) { 202 | printf("Bad Parameter\n"); 203 | return NULL; 204 | } 205 | 206 | TcpCommandClient* client = 207 | (TcpCommandClient*)malloc(sizeof(TcpCommandClient)); 208 | if (!client) { 209 | printf("No Memory!!!\n"); 210 | return NULL; 211 | } 212 | memset(client, 0, sizeof(TcpCommandClient)); 213 | client->fd = -1; 214 | strcpy(client->ip, ip); 215 | client->port = port; 216 | 217 | pthread_mutex_init(&client->lock, NULL); 218 | 219 | printf("TCP Command Client Init Success!!!\n"); 220 | return (void*)client; 221 | } 222 | 223 | PTC_ErrCode TcpCommandSetCalibration(const void* handle, const char* buffer, 224 | unsigned int len) { 225 | if (!handle || !buffer || len <= 0) { 226 | printf("Bad Parameter!!!\n"); 227 | return PTC_ERROR_BAD_PARAMETER; 228 | } 229 | TcpCommandClient* client = (TcpCommandClient*)handle; 230 | 231 | TC_Command cmd; 232 | memset(&cmd, 0, sizeof(TC_Command)); 233 | cmd.header.cmd = PTC_COMMAND_SET_CALIBRATION; 234 | cmd.header.len = len; 235 | cmd.data = strdup(buffer); 236 | 237 | PTC_ErrCode errorCode = tcpCommandClient_SendCmd(client, &cmd); 238 | if (errorCode != PTC_ERROR_NO_ERROR) { 239 | free(cmd.data); 240 | return errorCode; 241 | } 242 | free(cmd.data); 243 | 244 | if (cmd.ret_data) { 245 | // useless data; 246 | free(cmd.ret_data); 247 | } 248 | 249 | return cmd.header.ret_code; 250 | } 251 | 252 | PTC_ErrCode TcpCommandGetCalibration(const void* handle, char** buffer, 253 | unsigned int* len) { 254 | if (!handle || !buffer || !len) { 255 | printf("Bad Parameter!!!\n"); 256 | return PTC_ERROR_BAD_PARAMETER; 257 | } 258 | TcpCommandClient* client = (TcpCommandClient*)handle; 259 | 260 | TC_Command cmd; 261 | memset(&cmd, 0, sizeof(TC_Command)); 262 | cmd.header.cmd = PTC_COMMAND_GET_CALIBRATION; 263 | cmd.header.len = 0; 264 | cmd.data = NULL; 265 | 266 | PTC_ErrCode errorCode = tcpCommandClient_SendCmd(client, &cmd); 267 | if (errorCode != PTC_ERROR_NO_ERROR) { 268 | return errorCode; 269 | } 270 | 271 | char* ret_str = (char*)malloc(cmd.ret_size + 1); 272 | memcpy(ret_str, cmd.ret_data, cmd.ret_size); 273 | ret_str[cmd.ret_size] = '\0'; 274 | 275 | free(cmd.ret_data); 276 | 277 | *buffer = ret_str; 278 | *len = cmd.ret_size + 1; 279 | 280 | return cmd.header.ret_code; 281 | } 282 | PTC_ErrCode TcpCommandGetLidarCalibration(const void* handle, char** buffer, 283 | unsigned int* len) { 284 | if (!handle || !buffer || !len) { 285 | printf("Bad Parameter!!!\n"); 286 | return PTC_ERROR_BAD_PARAMETER; 287 | } 288 | TcpCommandClient* client = (TcpCommandClient*)handle; 289 | 290 | TC_Command cmd; 291 | memset(&cmd, 0, sizeof(TC_Command)); 292 | cmd.header.cmd = PTC_COMMAND_GET_LIDAR_CALIBRATION; 293 | cmd.header.len = 0; 294 | cmd.data = NULL; 295 | 296 | PTC_ErrCode errorCode = tcpCommandClient_SendCmd(client, &cmd); 297 | if (errorCode != PTC_ERROR_NO_ERROR) { 298 | return errorCode; 299 | } 300 | 301 | char* ret_str = (char*)malloc(cmd.ret_size + 1); 302 | memcpy(ret_str, cmd.ret_data, cmd.ret_size); 303 | ret_str[cmd.ret_size] = '\0'; 304 | 305 | free(cmd.ret_data); 306 | 307 | *buffer = ret_str; 308 | *len = cmd.ret_size + 1; 309 | 310 | return cmd.header.ret_code; 311 | } 312 | 313 | PTC_ErrCode TcpCommandResetCalibration(const void* handle) { 314 | if (!handle) { 315 | printf("Bad Parameter!!!\n"); 316 | return PTC_ERROR_BAD_PARAMETER; 317 | } 318 | TcpCommandClient* client = (TcpCommandClient*)handle; 319 | 320 | TC_Command cmd; 321 | memset(&cmd, 0, sizeof(TC_Command)); 322 | cmd.header.cmd = PTC_COMMAND_RESET_CALIBRATION; 323 | cmd.header.len = 0; 324 | cmd.data = NULL; 325 | 326 | PTC_ErrCode errorCode = tcpCommandClient_SendCmd(client, &cmd); 327 | if (errorCode != PTC_ERROR_NO_ERROR) { 328 | return errorCode; 329 | } 330 | 331 | if (cmd.ret_data) { 332 | // useless data; 333 | free(cmd.ret_data); 334 | } 335 | 336 | return cmd.header.ret_code; 337 | } 338 | 339 | void TcpCommandClientDestroy(const void* handle) {} 340 | -------------------------------------------------------------------------------- /src/tcp_command_client.h: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * Copyright 2018 The Apollo Authors. 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 | #ifndef SRC_TCP_COMMAND_CLIENT_H_ 18 | #define SRC_TCP_COMMAND_CLIENT_H_ 19 | 20 | #ifdef __cplusplus 21 | extern "C" { 22 | #endif 23 | 24 | typedef enum { 25 | PTC_COMMAND_GET_CALIBRATION = 0, 26 | PTC_COMMAND_SET_CALIBRATION, 27 | PTC_COMMAND_HEARTBEAT, 28 | PTC_COMMAND_RESET_CALIBRATION, 29 | PTC_COMMAND_TEST, 30 | PTC_COMMAND_GET_LIDAR_CALIBRATION, 31 | } PTC_COMMAND; 32 | 33 | typedef enum { 34 | PTC_ERROR_NO_ERROR = 0, 35 | PTC_ERROR_BAD_PARAMETER, 36 | PTC_ERROR_CONNECT_SERVER_FAILED, 37 | PTC_ERROR_TRANSFER_FAILED, 38 | PTC_ERROR_NO_MEMORY, 39 | } PTC_ErrCode; 40 | 41 | typedef struct TcpCommandHeader_s { 42 | unsigned char cmd; 43 | unsigned char ret_code; 44 | unsigned int len; 45 | } TcpCommandHeader; 46 | 47 | typedef struct TC_Command_s { 48 | TcpCommandHeader header; 49 | unsigned char* data; 50 | 51 | unsigned char* ret_data; 52 | unsigned int ret_size; 53 | } TC_Command; 54 | 55 | void* TcpCommandClientNew(const char* ip, const unsigned short port); 56 | PTC_ErrCode TcpCommandSetCalibration(const void* handle, const char* buffer, 57 | unsigned int len); 58 | PTC_ErrCode TcpCommandGetCalibration(const void* handle, char** buffer, 59 | unsigned int* len); 60 | PTC_ErrCode TcpCommandGetLidarCalibration(const void* handle, char** buffer, 61 | unsigned int* len); 62 | PTC_ErrCode TcpCommandResetCalibration(const void* handle); 63 | void TcpCommandClientDestroy(const void* handle); 64 | 65 | #ifdef __cplusplus 66 | } 67 | #endif 68 | 69 | #endif // SRC_TCP_COMMAND_CLIENT_H_ 70 | -------------------------------------------------------------------------------- /src/util.c: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * Copyright 2018 The Apollo Authors. 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 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | #include 27 | #include 28 | #include 29 | #include 30 | #include 31 | #include 32 | #include 33 | #include 34 | #include 35 | 36 | #include "src/util.h" 37 | 38 | #define DEFAULT_TIMEOUT 10 /*secondes waitting for read/write*/ 39 | 40 | int sys_readn(int fd, void* vptr, int n) { 41 | // printf("start sys_readn: %d....\n", n); 42 | int nleft, nread; 43 | char* ptr; 44 | 45 | ptr = vptr; 46 | nleft = n; 47 | while (nleft > 0) { 48 | // printf("start read\n"); 49 | if ((nread = read(fd, ptr, nleft)) < 0) { 50 | if (errno == EINTR) 51 | nread = 0; 52 | else 53 | return -1; 54 | } else if (nread == 0) { 55 | break; 56 | } 57 | // printf("end read, read: %d\n", nread); 58 | nleft -= nread; 59 | ptr += nread; 60 | } 61 | // printf("stop sys_readn....\n"); 62 | 63 | return n - nleft; 64 | } 65 | 66 | int sys_writen(int fd, const void* vptr, int n) { 67 | int nleft; 68 | int nwritten; 69 | const char* ptr; 70 | 71 | ptr = vptr; 72 | nleft = n; 73 | while (nleft > 0) { 74 | if ((nwritten = write(fd, ptr, nleft)) <= 0) { 75 | if (nwritten < 0 && errno == EINTR) 76 | nwritten = 0; /* and call write() again */ 77 | else 78 | return (-1); /* error */ 79 | } 80 | 81 | nleft -= nwritten; 82 | ptr += nwritten; 83 | } 84 | 85 | return n; 86 | } 87 | 88 | int tcp_open(const char* ipaddr, int port) { 89 | int sockfd; 90 | struct sockaddr_in servaddr; 91 | 92 | if ((sockfd = socket(AF_INET, SOCK_STREAM, 0)) == -1) return -1; 93 | 94 | bzero(&servaddr, sizeof(servaddr)); 95 | servaddr.sin_family = AF_INET; 96 | servaddr.sin_port = htons(port); 97 | if (inet_pton(AF_INET, ipaddr, &servaddr.sin_addr) <= 0) { 98 | close(sockfd); 99 | return -1; 100 | } 101 | 102 | if (connect(sockfd, (struct sockaddr*)&servaddr, sizeof(servaddr)) == -1) { 103 | close(sockfd); 104 | return -1; 105 | } 106 | 107 | return sockfd; 108 | } 109 | 110 | /** 111 | *Description:check the socket state 112 | * 113 | * @param 114 | * fd: socket 115 | * timeout:the time out of select 116 | * wait_for:socket state(r,w,conn) 117 | * 118 | * @return 1 if everything was ok, 0 otherwise 119 | */ 120 | int select_fd(int fd, int timeout, int wait_for) { 121 | fd_set fdset; 122 | fd_set *rd = NULL, *wr = NULL; 123 | struct timeval tmo; 124 | int result; 125 | 126 | FD_ZERO(&fdset); 127 | FD_SET(fd, &fdset); 128 | if (wait_for == WAIT_FOR_READ) { 129 | rd = &fdset; 130 | } 131 | if (wait_for == WAIT_FOR_WRITE) { 132 | wr = &fdset; 133 | } 134 | if (wait_for == WAIT_FOR_CONN) { 135 | rd = &fdset; 136 | wr = &fdset; 137 | } 138 | 139 | tmo.tv_sec = timeout; 140 | tmo.tv_usec = 0; 141 | do { 142 | result = select(fd + 1, rd, wr, NULL, &tmo); 143 | } while (result < 0 && errno == EINTR); 144 | 145 | return result; 146 | } 147 | -------------------------------------------------------------------------------- /src/util.h: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * Copyright 2018 The Apollo Authors. 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 | #ifndef SRC_UTIL_H_ 18 | #define SRC_UTIL_H_ 19 | 20 | #ifdef __cplusplus 21 | extern "C" { 22 | #endif 23 | 24 | int sys_readn(int fd, void* vptr, int n); 25 | int sys_writen(int fd, const void* vptr, int n); 26 | int tcp_open(const char* ipaddr, int port); 27 | int select_fd(int fd, int timeout, int wait_for); 28 | 29 | enum { WAIT_FOR_READ, WAIT_FOR_WRITE, WAIT_FOR_CONN }; 30 | 31 | #ifdef __cplusplus 32 | } 33 | #endif 34 | 35 | #endif // SRC_UTIL_H_ 36 | -------------------------------------------------------------------------------- /test/test.cc: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * Copyright 2018 The Apollo Authors. 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 | #include "pandora/pandora.h" 18 | 19 | using namespace apollo::drivers::hesai; 20 | 21 | FILE* lidarTimestampFile = fopen("lidar-timestamp.txt", "w"); 22 | 23 | double pandoraToSysTimeGap = 0; 24 | int gpsTimestamp = 0; 25 | 26 | void gpsCallback(int timestamp) { 27 | struct timeval ts; 28 | gettimeofday(&ts, NULL); 29 | gpsTimestamp = timestamp; 30 | pandoraToSysTimeGap = 31 | (double)ts.tv_sec + ((double)ts.tv_usec / 1000000.0) - (double)timestamp; 32 | printf("gps: %d, gap: %f\n", timestamp, pandoraToSysTimeGap); 33 | } 34 | 35 | void lidarCallback(boost::shared_ptr cld, double timestamp) { 36 | struct timeval ts; 37 | gettimeofday(&ts, NULL); 38 | printf("lidar: %lf with frame id : %s \n", timestamp, 39 | cld->header.frame_id.c_str()); 40 | // fprintf(lidarTimestampFile, "%d, %f,%f\n", gpsTimestamp, timestamp, 41 | // ts.tv_sec + (double)ts.tv_usec / 1000000 - pandoraToSysTimeGap - 42 | // timestamp); 43 | } 44 | 45 | void cameraCallback(boost::shared_ptr matp, double timestamp, 46 | int picid, bool distortion) { 47 | printf("callback %d %d timestamp %lf \n", picid, distortion, timestamp); 48 | } 49 | 50 | int main(int argc, char** argv) { 51 | Pandora pandora(std::string("192.168.20.51"), 2368, 10110, lidarCallback, 52 | gpsCallback, 13500, 9870, cameraCallback, 1, 0, 53 | std::string("hesai40")); 54 | pandora.Start(); 55 | while (true) { 56 | sleep(100); 57 | } 58 | } --------------------------------------------------------------------------------