fix for the issue with the termination process when powering off (#5)

* add colcon build env. (base code is 0624e) * move some hpp files, add src folder in livox driver * trivial modification * Update README.md * remove unneccesary files * remove unneccesary header * move third-party source files * update code(20210630) * Delete different test files. * README.md add "4. running" * README crlf * HwInterface/udp_socket.hpp * modify for ros2 run error * Update README.md * accumulate_count * time_stamp * struct LivoxPointXyzrtl packed * GetEchoNumPerPoint() * cloud.width 24000 * time_base -> time_stamp * code fix * cloud.width update * code fix * code fix * StreamStart retry 30sec * code fix * rviz2 * fix format * <br> * format ros2wrapper * frame_id * frame_id * Update README.md * Update README.md * discussion_r664993039 * typo typo brief and only English in the comments. and LivoxDataCallback delete * PI -> M_PI * Revert "PI -> M_PI" This reverts commit c54bbe3b3c1954f82247d38570bdf71a3ddf38b6. * PI -> M_PI 2nd * thread pointer -> shared_ptr * modify CMakelists.txt and package.xml to using ament_cmake_auto * README.md * default node name * add composable node (#4) * add composable node * fix conflicts * fix exec node * change sensor name fix to variable * fix can use ros2_wrapper_main.cpp * README.md * default node name * change namespace * add namespace to topic names Co-authored-by: mitui.hide <h-mitsui@esol.co.jp> * package.xml maintainer and README.md * #discussion_r666683800 * #discussion_r666712110 * #discussion_r665125337 * #discussion_r664995104 * #discussion_r666078326 * #discussion_r665121366 * #discussion_r665121366 Expansion * typedef enum -> enum * typedef enum -> enum * #discussion_r665121708 * #discussion_r665124279 * #discussion_r664995791 * esol_ReviewNo6 * esol_ReviewNo26 * esol_ReviewNo24 * Revert "esol_ReviewNo24" This reverts commit 0e8f33341186d6fa4a905de45c8f9aa28b8d29cb. * Revert "esol_ReviewNo24" This reverts commit 0e8f33341186d6fa4a905de45c8f9aa28b8d29cb. * esol_ReviewNo24 * esol_ReviewNo27 * esol_ReviewNo17 * format * esol_ReviewNo21 * develop #1 6bae863876bfda52db848ed31e415a3b16a0b6a8 * tab -> space * develop marge * CMakeLists.txt remove(-g -O0) * T4PUB-358 1st * format and bug-fix * 5#discussion_r677475332 * Add comment semaphore_ * 5#discussion_r678802453 Co-authored-by: mitui.hide <h-mitsui@esol.co.jp> Co-authored-by: v-nagai7872@esol.co.jp <v-nagai7872@esol.co.jp> Co-authored-by: Takeshi Miura <57553950+1222-takeshi@users.noreply.github.com> Co-authored-by: h-mitsui-esol <57085544+h-mitsui-esol@users.noreply.github.com>
tier4 · Mar 7, 2023 · d10b249 · d10b249
1 parent 8552a8c
commit d10b249
Show file tree

Hide file tree

Showing 8 changed files with 394 additions and 279 deletions.
diff --git a/hw_interface/include/HwInterface/udp_socket.hpp b/hw_interface/include/HwInterface/udp_socket.hpp
@@ -22,7 +22,7 @@ class UdpSocket
 
   bool IsOpen() const;
 
-  int Send(std::vector<uint8_t> & buff);
+  int Send(const std::vector<uint8_t> & buff);
   int Recv(std::vector<uint8_t> & buff, int wait_msec = -1);
 
   std::string EndPointIp() const;

diff --git a/hw_interface/src/udp_socket.cpp b/hw_interface/src/udp_socket.cpp
@@ -125,7 +125,7 @@ uint16_t UdpSocket::EndPointPort() const { return ntohs(rcv_addrin_.sin_port); }
 /// @return On success, these calls return the number of bytes sent.
 /// @retval -1: sendto error
 /// @retval -2: port number not set.
-int UdpSocket::Send(std::vector<uint8_t> & buff)
+int UdpSocket::Send(const std::vector<uint8_t> & buff)
 {
   int retval;
 

diff --git a/livox_driver/include/LidarDriver/lidar_driver.hpp b/livox_driver/include/LidarDriver/lidar_driver.hpp
@@ -1,7 +1,10 @@
 #ifndef LIDARDRIVER_LIVOX_LIDARDRIVER_HPP_
 #define LIDARDRIVER_LIVOX_LIDARDRIVER_HPP_
 
+
+#include <condition_variable>
 #include <functional>
+#include <mutex>
 #include <thread>
 
 #include "HwInterface/udp_socket.hpp"
@@ -11,9 +14,6 @@
 namespace lidar_driver
 {
 const uint16_t kCommandPort = 65000;
-const uint8_t kCommandRxCommunicationSettingAck = 0x01;
-const uint8_t kCommancRxStreamingAck = 0x04;
-const uint8_t kCommandRxHeartbeatAck = 0x03;
 const uint16_t kCrcSeed16 = 0x4c49;
 const uint32_t kCrcSeed32 = 0x564f580a;
 const uint32_t kSdkPacketCrcSize = 4;
@@ -38,6 +38,27 @@ enum class LidarCommandType {
   kLidarCommandHeartbeat,
 };
 
+enum class CommandResult { kAck = 0, kNack, kTimeout, kError, kUnknownPacket };
+
+/**  Enum that represents the command id. */
+enum class GeneralCommandID : uint8_t {
+  kBroadcast = 0,                 // General command set, broadcast command.
+  kHandshake = 1,                 // General command set, query the information of device.
+  kDeviceInfo = 2,                // General command set, query the information of device.
+  kHeartbeat = 3,                 // General command set, heartbeat command.
+  kControlSample = 4,             // General command set, enable or disable the sampling.
+  kCoordinateSystem = 5,          // General command set, change the coordinate of point cloud data.
+  kDisconnect = 6,                // General command set, disconnect the device.
+  kPushAbnormalState = 7,         // General command set, a message command from a connected device
+  kConfigureStaticDynamicIp = 8,  // General command set, set the IP of the a device.
+  kGetDeviceIpInformation = 9,    // General command set, get the IP of the a device.
+  kRebootDevice = 0x0a,           // General command set, reboot a device.
+  kSetDeviceParam = 0x0b,         // Set device's parameters.
+  kGetDeviceParam = 0x0c,         // Get device's parameters.
+  kResetDeviceParam = 0x0d,       // Reset device's all parameters.
+  kCommandCount                   // Don't add command id after kCommandCount.
+};
+
 #pragma pack(1)
 struct CommandHeader
 {
@@ -94,43 +115,87 @@ struct LivoxPoint
 struct CommandHandshake
 {
   uint8_t cmd_set;
-  uint8_t cmd_id;
+  GeneralCommandID cmd_id;
   uint32_t host_ip;
   uint16_t data_port;
   uint16_t command_port;
   uint16_t imu_port;
+  uint32_t crc32;
 };
 #pragma pack()
 
 #pragma pack(1)
 struct CommandHeartbeat
 {
   uint8_t cmd_set;
-  uint8_t cmd_id;
+  GeneralCommandID cmd_id;
+  uint32_t crc32;
 };
 #pragma pack()
 
 #pragma pack(1)
 struct CommandSampling
 {
   uint8_t cmd_set;
-  uint8_t cmd_id;
+  GeneralCommandID cmd_id;
   uint8_t sample_ctrl;
+  uint32_t crc32;
+};
+#pragma pack()
+
+#pragma pack(1)
+struct CommandAll
+{
+  CommandHeader header;
+  union {
+    CommandHandshake hand_shake;
+    CommandHeartbeat heart_beat;
+    CommandSampling sampling;
+  } data;
 };
 #pragma pack()
 
 #pragma pack(1)
 struct CommandAck
 {
+  CommandHeader header;
   uint8_t cmd_set;
-  uint8_t cmd_id;
+  GeneralCommandID cmd_id;
   uint8_t result;
+  uint32_t crc32;
 };
 #pragma pack()
 
-using EulerAngle = float [3];        /**< Roll, Pitch, Yaw, unit:radian. */
-using TranslationVector = float [3]; /**< x, y, z translation, unit: m. */
-using RotationMatrix = float [3][3];
+class Semaphore
+{
+public:
+  explicit Semaphore(int count = 0) : count_(count) {}
+
+  void Signal()
+  {
+    std::unique_lock<std::mutex> lock(mutex_);
+    ++count_;
+    cv_.notify_one();
+  }
+
+  void Wait()
+  {
+    std::unique_lock<std::mutex> lock(mutex_);
+    cv_.wait(lock, [=] { return count_ > 0; });
+    --count_;
+  }
+
+  int GetCount() { return count_; }
+
+private:
+  std::mutex mutex_;
+  std::condition_variable cv_;
+  volatile int count_;
+};
+
+using EulerAngle = float[3];        /**< Roll, Pitch, Yaw, unit:radian. */
+using TranslationVector = float[3]; /**< x, y, z translation, unit: m. */
+using RotationMatrix = float[3][3];
 
 #pragma pack(1)
 struct ExtrinsicParameter
@@ -186,10 +251,10 @@ class LidarDriver
   bool SetCalibration();
   bool GetCalibration();
 
-  int StartHwRxInterface();
+  bool StartHwRxInterface();
   void StopHwRxInterface();
 
-  int StartHwTxInterface();
+  CommandResult StartHwTxInterface();
   void StopHwTxInterface();
 
   int ParsePacket(livox_driver::LivoxLidarPacket & packet);
@@ -217,17 +282,23 @@ class LidarDriver
   bool LivoxHwTxInterfaceHeartbeat();
 
   bool SendCommand(LidarCommandType command);
-  void MakeCommandHandshake(std::vector<uint8_t> & buff);
-  void MakeCommandStartStream(std::vector<uint8_t> & buff);
-  void MakeCommandStopStream(std::vector<uint8_t> & buff);
-  void MakeCommandHeartbeat(std::vector<uint8_t> & buff);
+  CommandHandshake MakeCommandHandshake();
+  CommandSampling MakeCommandStartStream();
+  CommandSampling MakeCommandStopStream();
+  CommandHeartbeat MakeCommandHeartbeat();
 
   bool UdpSocketOpen(
     HwInterface::UdpSocket & sock, const std::string & sensor_ip, uint16_t sensor_port,
     uint16_t my_port);
   void CloseAllUdpSocket();
   bool CheckRecvData(const std::vector<uint8_t> & buff);
 
+  Semaphore semaphore_;
+  std::mutex mtx_;
+  std::vector<uint8_t> MakeSendCommand(LidarCommandType command);
+  CommandResult SendAckWait(const std::vector<uint8_t> & snd_buff, GeneralCommandID cmd_id);
+  GeneralCommandID GetCommandId(LidarCommandType cmd_type);
+
 private:
   enum DeviceType {
     kHub = 0,          /**< Livox Hub. */
@@ -302,7 +373,7 @@ class LidarDriver
     dst_point.x = raw_point.x / 1000.0f;
     dst_point.y = raw_point.y / 1000.0f;
     dst_point.z = raw_point.z / 1000.0f;
-    dst_point.reflectivity = (float)raw_point.reflectivity;
+    dst_point.reflectivity = static_cast<float>(raw_point.reflectivity);
   }
   inline bool IsTripleFloatNoneZero(float x, float y, float z)
   {