├── .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 <project>
10 | mkdir build ; cd build;
11 | cmake .. ; make
12 | ```
13 | ## Add to your project 
14 | ### Cmake
15 | ```
16 | add_subdirectory(<path_to>Pandora_SDK)
17 | 
18 | include_directories(
19 | 	<path_to>Pandora_SDK/include
20 | 	<path_to>Pandora_SDK/src/Pandar40P/include
21 | )
22 | 
23 | target_link_libraries(<Your project>
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 <opencv2/highgui/highgui_c.h>
 21 | #include <pcl/io/pcd_io.h>
 22 | #include <pcl/point_types.h>
 23 | #include <pthread.h>
 24 | #include <semaphore.h>
 25 | 
 26 | #include <string>
 27 | #include <vector>
 28 | 
 29 | #include <boost/function.hpp>
 30 | #include <opencv2/opencv.hpp>
 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<double> cameraT;  //  x y z
 48 |   std::vector<double> 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 <real angle> * 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<void(boost::shared_ptr<PPointCloud>, double)>
 68 |               pcl_callback,
 69 |           boost::function<void(double)> gps_callback, uint16_t start_angle,
 70 |           const uint16_t pandoraCameraPort,
 71 |           boost::function<void(boost::shared_ptr<cv::Mat> 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<void(boost::shared_ptr<PPointCloud>, double)>
 34 |           pcl_callback,
 35 |       boost::function<void(double)> gps_callback, uint16_t start_angle,
 36 |       const uint16_t pandoraCameraPort,
 37 |       boost::function<void(boost::shared_ptr<cv::Mat> 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<void(boost::shared_ptr<PPointCloud>, double)> pcl_callback,
 73 |     boost::function<void(double)> gps_callback, uint16_t start_angle,
 74 |     const uint16_t pandoraCameraPort,
 75 |     boost::function<void(boost::shared_ptr<cv::Mat> 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<cv::Mat> cameras_k;
244 |         std::vector<cv::Mat> 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<std::string>(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<double>(i) = yn[cameraId]["K"][i].as<double>();
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()<<std::endl;
290 |       intrinsicD = cv::Mat::zeros(yn[cameraId]["D"].size(), 1, CV_64FC1);
291 |       for (int i = 0; i < yn[cameraId]["D"].size(); ++i) {
292 |         intrinsicD.at<double>(i) = yn[cameraId]["D"][i].as<double>();
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<double>());
306 |       calibs[id].cameraR.push_back(
307 |           yn[cameraId]["Extrinsic"]["rotation"]["x"].as<double>());
308 |       calibs[id].cameraR.push_back(
309 |           yn[cameraId]["Extrinsic"]["rotation"]["y"].as<double>());
310 |       calibs[id].cameraR.push_back(
311 |           yn[cameraId]["Extrinsic"]["rotation"]["z"].as<double>());
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<double>());
322 |       calibs[id].cameraT.push_back(
323 |           yn[cameraId]["Extrinsic"]["translation"]["y"].as<double>());
324 |       calibs[id].cameraT.push_back(
325 |           yn[cameraId]["Extrinsic"]["translation"]["z"].as<double>());
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<std::string>(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<double>(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<double>(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<std::string>(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<<node<<std::endl;
375 |   return 0;
376 | }
377 | 
378 | /*****************************************************************************************
379 | Pandora Part
380 | *****************************************************************************************/
381 | /**
382 |  * @brief Constructor
383 |  * @param device_ip         The ip of the device
384 |  *        lidar_port        The port number of lidar data
385 |  *        gps_port          The port number of gps data
386 |  *        pcl_callback      The callback of PCL data structure
387 |  *        gps_callback      The callback of GPS structure
388 |  *        start_angle       The start angle of every point cloud
389 |  *        pandoraCameraPort The port of camera data
390 |  *        cameraCallback    the call back for camera data
391 |  */
392 | Pandora::Pandora(
393 |     std::string device_ip, const uint16_t lidar_port, const uint16_t gps_port,
394 |     boost::function<void(boost::shared_ptr<PPointCloud>, double)> pcl_callback,
395 |     boost::function<void(double)> gps_callback, uint16_t start_angle,
396 |     const uint16_t pandoraCameraPort,
397 |     boost::function<void(boost::shared_ptr<cv::Mat> 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 <stdio.h>
 18 | #include <unistd.h>
 19 | 
 20 | #include <jpeglib.h>
 21 | #include <vector>
 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<PandoraPic *>(param);
 35 |   PandoraCamera *pSDK = static_cast<PandoraCamera *>(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<void(boost::shared_ptr<cv::Mat> matp, double timestamp,
 43 |                          int picid, bool distortion)>
 44 |         camera_callback,
 45 |     boost::function<void(bool connected)> 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<void *>(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<cv::Mat> 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<uint8_t *>(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<cv::Mat> cameras_k,
198 |                                    const std::vector<cv::Mat> 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<cv::Mat> 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 <pcl/io/pcd_io.h>
 21 | #include <pcl/point_types.h>
 22 | #include <pthread.h>
 23 | #include <semaphore.h>
 24 | 
 25 | #include <list>
 26 | #include <string>
 27 | #include <vector>
 28 | 
 29 | #include <boost/function.hpp>
 30 | #include <opencv2/opencv.hpp>
 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<void(boost::shared_ptr<cv::Mat> matp, double timestamp,
 56 |                            int picid, bool distortion)>
 57 |           camera_callback,
 58 |       boost::function<void(bool connected)> 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<cv::Mat> cameras_k,
 67 |                       const std::vector<cv::Mat> 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<cv::Mat> 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<PandoraPic *> pic_list_[CAMERA_NUM];
 94 |   std::vector<cv::Mat> mapx_;
 95 |   std::vector<cv::Mat> mapy_;
 96 |   boost::function<void(boost::shared_ptr<cv::Mat> matp, double timestamp,
 97 |                        int pic_id, bool distortion)>
 98 |       camera_callback_;
 99 |   boost::function<void(bool connected)> 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 <arpa/inet.h>
 18 | #include <errno.h>
 19 | #include <fcntl.h>
 20 | #include <netinet/in.h>
 21 | #include <pthread.h>
 22 | #include <setjmp.h>
 23 | #include <signal.h>
 24 | #include <stdarg.h>
 25 | #include <stdio.h>
 26 | #include <stdlib.h>
 27 | #include <string.h>
 28 | #include <strings.h>
 29 | #include <sys/ipc.h>
 30 | #include <sys/msg.h>
 31 | #include <sys/socket.h>
 32 | #include <sys/types.h>
 33 | #include <syslog.h>
 34 | #include <unistd.h>
 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 <arpa/inet.h>
 18 | #include <errno.h>
 19 | #include <fcntl.h>
 20 | #include <netinet/in.h>
 21 | #include <pthread.h>
 22 | #include <setjmp.h>
 23 | #include <signal.h>
 24 | #include <stdarg.h>
 25 | #include <stdio.h>
 26 | #include <stdlib.h>
 27 | #include <string.h>
 28 | #include <sys/ipc.h>
 29 | #include <sys/msg.h>
 30 | #include <sys/socket.h>
 31 | #include <sys/types.h>
 32 | #include <syslog.h>
 33 | #include <unistd.h>
 34 | #include <linux/sockios.h>
 35 | #include <net/if.h>
 36 | #include <sys/ioctl.h>
 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 <arpa/inet.h>
 18 | #include <errno.h>
 19 | #include <fcntl.h>
 20 | #include <netinet/in.h>
 21 | #include <pthread.h>
 22 | #include <setjmp.h>
 23 | #include <signal.h>
 24 | #include <stdarg.h>
 25 | #include <stdio.h>
 26 | #include <stdlib.h>
 27 | #include <string.h>
 28 | #include <strings.h>
 29 | #include <sys/ipc.h>
 30 | #include <sys/msg.h>
 31 | #include <sys/socket.h>
 32 | #include <sys/types.h>
 33 | #include <syslog.h>
 34 | #include <unistd.h>
 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<PPointCloud> 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<cv::Mat> 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 | }


--------------------------------------------------------------------------------