Livox Diagnostics (#10)

* Livox Diagnostics. Initial non working commit Signed-off-by: amc-nu <abrahammonrroy@yahoo.com> * clang. add tier4 format Signed-off-by: amc-nu <abrahammonrroy@yahoo.com> * gitignore. Add Tier4 ignore file Signed-off-by: amc-nu <abrahammonrroy@yahoo.com> * wip Signed-off-by: amc-nu <abrahammonrroy@yahoo.com> * wip 2 Signed-off-by: amc-nu <abrahammonrroy@yahoo.com> * code adaptations to new main branch Signed-off-by: Abraham Cano <abrahammonrroy@yahoo.com> * wip 3 get device status from sensor Signed-off-by: Abraham Cano <abrahammonrroy@yahoo.com> * adaptations for diagnostics from eth packet Signed-off-by: Abraham Cano <abrahammonrroy@yahoo.com> * RosWrapper. parse and report Diagnostics Signed-off-by: amc-nu <abrahammonrroy@yahoo.com> * lidar_driver. fix getcommandid switch Signed-off-by: amc-nu <abrahammonrroy@yahoo.com> * ros2_driver_wrapper. fix switch indent Signed-off-by: amc-nu <abrahammonrroy@yahoo.com> * git workflow. test CI token Signed-off-by: amc-nu <abrahammonrroy@yahoo.com> * livox_command. Remove unused CommandPushAbnormalState struct Signed-off-by: amc-nu <abrahammonrroy@yahoo.com> * add fallthroughts Signed-off-by: amc-nu <abrahammonrroy@yahoo.com> * apply t4 formatting Signed-off-by: amc-nu <abrahammonrroy@yahoo.com> * livox_diagnostics. remove unnecessary code for boolean vars Signed-off-by: amc-nu <abrahammonrroy@yahoo.com> * ros2_driver_wrapper. clarify level comparison with enum Signed-off-by: amc-nu <abrahammonrroy@yahoo.com> * livox_diagnostics. remove unncessary comma at the end of enum Signed-off-by: amc-nu <abrahammonrroy@yahoo.com> * ros2_driver_wrapper. group diagnostics members Signed-off-by: amc-nu <abrahammonrroy@yahoo.com> * gitignore. remove aw related entries Signed-off-by: amc-nu <abrahammonrroy@yahoo.com> * ros2_driver_wrapper. simplify ros diagnostics string Signed-off-by: amc-nu <abrahammonrroy@yahoo.com> * revert fallthroughs on switch Signed-off-by: Abraham Cano <abrahammonrroy@yahoo.com> * replace shift logic for bit fields Signed-off-by: Abraham Cano <abrahammonrroy@yahoo.com> * add private keyword to clarify Signed-off-by: Abraham Cano <abrahammonrroy@yahoo.com> * separate diagnostics, remove static definition Signed-off-by: Abraham Cano <abrahammonrroy@yahoo.com> * add missing StatusCodeToLivoxMotorStatus Signed-off-by: Abraham Cano <abrahammonrroy@yahoo.com> * Separate diagnostics to multiple reports Signed-off-by: Abraham Cano <abrahammonrroy@yahoo.com> * update status from sensor Signed-off-by: amc-nu <abrahammonrroy@yahoo.com> * wrapper. remove the namespace from the diagnostic task Signed-off-by: amc-nu <abrahammonrroy@yahoo.com> * ros2_wrapper. remove unncessary lidar_identifier Signed-off-by: amc-nu <abrahammonrroy@yahoo.com>
tier4 · Mar 7, 2023 · 119bcef · 119bcef
1 parent 2bd9534
commit 119bcef
Show file tree

Hide file tree

Showing 14 changed files with 921 additions and 35 deletions.
diff --git a/.github/workflows/build_and_test.yml b/.github/workflows/build_and_test.yml
@@ -52,4 +52,4 @@ jobs:
           package-name: ${{ steps.list_packages.outputs.package_list }}
           target-ros2-distro: ${{ matrix.ros_distribution }}
           vcs-repo-file-url: build_depends.repos
-          import-token: ${{ secrets.REPO_TOKEN }}
+          import-token: ${{ secrets.GITHUB_TOKEN }}
diff --git a/livox_driver/CMakeLists.txt b/livox_driver/CMakeLists.txt
@@ -17,6 +17,7 @@ set(FASTCRC_DIR ${CMAKE_CURRENT_SOURCE_DIR}/src/third_party/FastCRC)
 ament_auto_add_library(${PROJECT_NAME} SHARED
   ${CMAKE_CURRENT_SOURCE_DIR}/src/lidar_driver.cpp
   ${CMAKE_CURRENT_SOURCE_DIR}/src/livox_data_recv.cpp
+  ${CMAKE_CURRENT_SOURCE_DIR}/src/livox_diagnostics.cpp
   ${FASTCRC_DIR}/FastCRCsw.cpp
 )
 

diff --git a/livox_driver/include/LidarDriver/lidar_driver.hpp b/livox_driver/include/LidarDriver/lidar_driver.hpp
@@ -2,6 +2,7 @@
 #define LIDARDRIVER_LIVOX_LIDARDRIVER_HPP_
 
 #include <functional>
+#include <map>
 #include <mutex>
 #include <thread>
 
@@ -15,6 +16,20 @@ namespace lidar_driver
 class LidarDriver
 {
 public:
+  enum class LidarDriverStatus {
+    kDisconnect = 0,
+    kConnect,
+    kStreaming,
+  };
+  const std::map<LidarDriverStatus, const char *> lidar_driver_status_dict_ = {
+    {LidarDriverStatus::kDisconnect, "Disconnected"},
+    {LidarDriverStatus::kConnect, "Connected"},
+    {LidarDriverStatus::kStreaming, "Streaming"}};
+
+  using PublishLidarDataCallback = std::function<void(
+    const std::vector<uint8_t> & buff, int pkt_len, uint64_t timestamp, uint32_t point_num)>;
+  using PublishImuPacketCallback =
+    std::function<void(const livox_driver::LivoxImuPoint & imu_pkt, uint64_t timestamp)>;
   LidarDriver();
   virtual ~LidarDriver();
 
@@ -44,14 +59,15 @@ class LidarDriver
   int ParsePacket(livox_driver::LivoxLidarPacket & packet);
   std::unique_ptr<livox_driver::LivoxPublishData> GenerateCloud();
 
-  using PublishLidarDataCallback = std::function<void(
-    const std::vector<uint8_t> & buff, int pkt_len, uint64_t timestamp, uint32_t point_num)>;
+  bool Initialize();
+
+  LidarDriverStatus GetLidarStatus() { return lidarstatus_; }
+
   void SetPublishLidarDataFunc(const PublishLidarDataCallback & func)
   {
     publish_lidar_data_cb_ = func;
   }
-  using PublishImuPacketCallback =
-    std::function<void(const livox_driver::LivoxImuPoint & imu_pkt, uint64_t timestamp)>;
+
   void SetPublishImuPacketFunc(const PublishImuPacketCallback & func)
   {
     publish_imu_packet_cb_ = func;
@@ -85,22 +101,19 @@ class LidarDriver
     kRunning = 0,
     kTerminate,
   };
+  const std::map<DriverStatus, const char *> driver_status_dict_ = {
+    {DriverStatus::kRunning, "Running"}, {DriverStatus::kTerminate, "Not Running"}};
   volatile DriverStatus driverstatus_{DriverStatus::kTerminate};
 
-  enum class LidarDriverStatus {
-    kDisconnect = 0,
-    kConnect,
-    kStreaming,
-  };
   volatile LidarDriverStatus lidarstatus_{LidarDriverStatus::kDisconnect};
 
   std::vector<uint8_t> MakeSendCommand(LidarCommandType cmd_type);
   CommandHandshake MakeCommandHandshake();
-  CommandSampling MakeCommandStartStream();
-  CommandSampling MakeCommandStopStream();
-  CommandHeartbeat MakeCommandHeartbeat();
+  static CommandSampling MakeCommandStartStream();
+  static CommandSampling MakeCommandStopStream();
+  static CommandHeartbeat MakeCommandHeartbeat();
   CommandResult SendAckWait(std::vector<uint8_t> & snd_buff, GeneralCommandID cmd_id);
-  GeneralCommandID GetCommandId(LidarCommandType cmd_type);
+  static GeneralCommandID GetCommandId(LidarCommandType cmd_type);
 
   FastCRC16 crc16_{kCrcSeed16};
   FastCRC32 crc32_{kCrcSeed32};
@@ -141,6 +154,8 @@ class LidarDriver
     uint32_t line_num;
     LidarPacketStatistic statistic_info_data;
     LidarPacketStatistic statistic_info_imu;
+    uint32_t lidar_status_code; /**< Stores the lidar device status */
+    bool status_code_ready_{};
     bool data_is_published;
   } lidar_device_;
 
@@ -152,6 +167,11 @@ class LidarDriver
     LidarPacketStatistic & packet_statistic, uint8_t timestamp_type, int64_t cur_timestamp_stamp);
   void StorageRawPacket(const std::vector<uint8_t> & buff, int rcv_len);
   void UpdateLidarInfoByEthPacket(uint8_t data_type);
+  /**
+   * Updates the LivoxStatusCode union (error_status) from Livox Ethernet Packet, and sets the received flag to true.
+   * @param lidar_status_code The 32 bit word received in the packet containing the error status.
+   */
+  void UpdateLidarStatusCode(uint32_t lidar_status_code);
 
 private:  // Parse
   uint32_t buffer_time_ms_;
@@ -168,6 +188,15 @@ class LidarDriver
   void LivoxExtendRawPointToPxyzrtl(
     uint8_t * point_buf, const std::vector<uint8_t> & raw_packet, uint32_t line_num);
 
+public:
+  bool GetLidarStatusCode(uint32_t & out_lidar_status_code) const
+  {
+    if (lidar_device_.status_code_ready_) {
+      out_lidar_status_code = lidar_device_.lidar_status_code;
+      return true;
+    }
+    return false;
+  }
 #ifdef UTEST
 public:
   friend class TestFriend_LivoxDriver;

diff --git a/livox_driver/include/LidarDriver/livox_command.hpp b/livox_driver/include/LidarDriver/livox_command.hpp
@@ -13,7 +13,7 @@ enum class LidarCommandType {
   kHandshake = 0,
   kStartStreaming,
   kStopStreaming,
-  kHeartbeat,
+  kHeartbeat
 };
 
 enum class CommandResult { kAck = 0, kNack, kTimeout, kError, kUnknownPacket };

diff --git a/livox_driver/include/LidarDriver/livox_common.hpp b/livox_driver/include/LidarDriver/livox_common.hpp
@@ -1,6 +1,8 @@
 #ifndef LIVOX_DRIVER_LIVOX_COMMON_HPP_
 #define LIVOX_DRIVER_LIVOX_COMMON_HPP_
+#include <bitset>
 #include <string>
+#include <vector>
 
 namespace livox_driver
 {