feat:filter RMC packets from NMEA-0183 messages

README.md

@@ -283,5 +283,5 @@ After replacing "/home/livox/test.lvx" in the above command with the local lvx d
 
 You can get support from Livox with the following methods :
 
-* Send email to dev@livoxtech.com with a clear description of your problem and your setup
+* Send email to cs@livoxtech.com with a clear description of your problem and your setup
 * Report issue on github

README_CN.md

@@ -292,5 +292,5 @@ HUB 配置参数说明
 
 你可以通过以下方式获取 Livox 的技术支持 :
 
-* 发送邮件到 dev@livoxtech.com ，详细描述您遇到的问题和使用场景
+* 发送邮件到 cs@livoxtech.com ，详细描述您遇到的问题和使用场景
 * 提交此代码仓的 github issues

livox_ros_driver/CMakeLists.txt

@@ -212,8 +212,10 @@ install(TARGETS ${PROJECT_NAME}_node
 	RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
 	)
 
-install(DIRECTORY launch/
-    DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch
+install(DIRECTORY
+  launch
+  config
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
 	)
 
 #---------------------------------------------------------------------------------------

livox_ros_driver/livox_ros_driver/lddc.cpp

@@ -594,7 +594,7 @@ ros::Publisher *Lddc::GetCurrentPublisher(uint8_t handle) {
 
   if (use_multi_topic_) {
     pub = &private_pub_[handle];
-    queue_size = queue_size / 8; // queue size is 4 for only one lidar
+    queue_size = queue_size * 2; // queue size is 64 for only one lidar
   } else {
     pub = &global_pub_;
     queue_size = queue_size * 8; // shared queue size is 256, for all lidars
@@ -643,7 +643,7 @@ ros::Publisher *Lddc::GetCurrentImuPublisher(uint8_t handle) {
 
   if (use_multi_topic_) {
     pub = &private_imu_pub_[handle];
-    queue_size = queue_size / 8; // queue size is 4 for only one lidar
+    queue_size = queue_size * 2; // queue size is 64 for only one lidar
   } else {
     pub = &global_imu_pub_;
     queue_size = queue_size * 8; // shared queue size is 256, for all lidars

livox_ros_driver/livox_ros_driver/lds.cpp

@@ -43,6 +43,30 @@ bool IsFilePathValid(const char *path_str) {
   }
 }
 
+/** Replace nonstardard function "timegm" with mktime.
+ *  For a portable version of timegm, set the TZ environment variable to UTC,
+ *  call mktime and restore the value of TZ.
+ *  "localtime" and "timegm" are nonstandard GNU extensions that are also present
+ *  on the BSDs. Avoid their use!!!
+ */
+time_t replace_timegm(struct tm *tm) {
+  time_t ret;
+  char *tz;
+
+  tz = getenv("TZ");
+  setenv("TZ", "", 1);
+  tzset();
+  ret = mktime(tm);
+
+  if (tz)
+    setenv("TZ", tz, 1);
+  else
+    unsetenv("TZ");
+  tzset();
+
+  return ret;
+}
+
 uint64_t RawLdsStampToNs(LdsStamp &timestamp, uint8_t timestamp_type) {
   if (timestamp_type == kTimestampTypePps) {
     return timestamp.stamp;
@@ -618,18 +642,20 @@ uint8_t Lds::GetDeviceType(uint8_t handle) {
   }
 }
 
-void Lds::UpdateLidarInfoByEthPacket(LidarDevice *p_lidar, \
+void Lds::UpdateLidarInfoByEthPacket(LidarDevice *p_lidar,
     LivoxEthPacket* eth_packet) {
   if (p_lidar->raw_data_type != eth_packet->data_type) {
     p_lidar->raw_data_type = eth_packet->data_type;
-    p_lidar->packet_interval = GetPacketInterval(p_lidar->info.type, \
+    p_lidar->packet_interval = GetPacketInterval(p_lidar->info.type,
         eth_packet->data_type);
+    p_lidar->timestamp_type = eth_packet->timestamp_type;
     p_lidar->packet_interval_max = p_lidar->packet_interval * 1.8f;
-    p_lidar->onetime_publish_packets = \
-        GetPacketNumPerSec(p_lidar->info.type, \
+    p_lidar->onetime_publish_packets =
+        GetPacketNumPerSec(p_lidar->info.type,
         p_lidar->raw_data_type) * buffer_time_ms_ / 1000;
-    printf("Lidar[%d][%s] DataType[%d] PacketInterval[%d] PublishPackets[%d]\n",
-        p_lidar->handle, p_lidar->info.broadcast_code, p_lidar->raw_data_type,
+    printf("Lidar[%d][%s] DataType[%d] timestamp_type[%d] PacketInterval[%d] "
+        "PublishPackets[%d]\n", p_lidar->handle, p_lidar->info.broadcast_code,
+        p_lidar->raw_data_type, p_lidar->timestamp_type,
         p_lidar->packet_interval, p_lidar->onetime_publish_packets);
   }
 }

livox_ros_driver/livox_ros_driver/lds.h

@@ -172,9 +172,10 @@ typedef struct {
 typedef struct {
   uint8_t handle;                    /**< Lidar access handle. */
   uint8_t data_src;                  /**< From raw lidar or livox file. */
-  uint8_t raw_data_type;             /**< The data type in eth packaet */
+  uint8_t raw_data_type;             /**< The data type in eth packaet. */
   bool data_is_pubulished;           /**< Indicate the data of lidar whether is
                                           pubulished. */
+  uint32_t timestamp_type;           /**< timestamp type of the current eth packet. */
   volatile uint32_t packet_interval; /**< The time interval between packets
                                         of current lidar, unit:ns */
   volatile uint32_t packet_interval_max; /**< If more than it,
@@ -263,6 +264,7 @@ const ProductTypePointInfoPair product_type_info_pair_table[kMaxProductType] = {
  * Global function for general use.
  */
 bool IsFilePathValid(const char *path_str);
+time_t replace_timegm(struct tm *tm);
 uint64_t RawLdsStampToNs(LdsStamp &timestamp, uint8_t timestamp_type);
 uint64_t GetStoragePacketTimestamp(StoragePacket *packet, uint8_t data_src);
 uint32_t CalculatePacketQueueSize(uint32_t interval_ms, uint8_t product_type,

livox_ros_driver/timesync/timesync.cpp

@@ -27,6 +27,7 @@
 #include <stdint.h>
 #include <string.h>
 #include <chrono>
+#include <cstdio>
 #include <functional>
 #include <thread>
 
@@ -139,8 +140,12 @@ void TimeSync::PollDataLoop() {
           CommPacket packet;
           memset(&packet, 0, sizeof(packet));
           while ((kParseSuccess == comm_->ParseCommStream(&packet))) {
-            if ((fn_cb_ != nullptr) || (client_data_ != nullptr)) {
+            if (((fn_cb_ != nullptr) || (client_data_ != nullptr))) {
+              if ((strstr((const char *)packet.data, "$GPRMC")) ||
+                      (strstr((const char *)packet.data , "$GNRMC"))){
                 fn_cb_((const char *)packet.data, packet.data_len, client_data_);
+                printf("RMC data parse success!.\n");
+              }
             }
           }
         }