imu loop back

[yangqifan913]

频繁地报错imu loop back，clear buffer。看IMU数据经常会回滚到过去一段时间，然后又恢复正常。不进行时间同步就不会出现这个问题。

[yangqifan913]

忘了说用的是mid360

[xuankuzcr]

1. 检查下是否供地，以及买的网线是否有屏蔽层。
2. 如果实在无法修复的话，可以在imu的回调函数里面加一句，如果上一个imu msg的时间戳小于当前帧imu的话，就把回退的时间补偿回来。
3. 也可以直接在livox_ros_driver里面修复，用下面代码替换这个函数。[但接线没问题的话是没有回退的]

uint32_t Lddc::PublishImuData(LidarDataQueue *queue, uint32_t packet_num,
                              uint8_t handle) {
  volatile uint64_t timestamp = 0;
  uint32_t published_packet = 0;

  sensor_msgs::Imu imu_data;
  imu_data.header.frame_id = "livox_frame";

  uint8_t data_source = lds_->lidars_[handle].data_src;
  StoragePacket storage_packet;
  QueuePrePop(queue, &storage_packet);
  LivoxEthPacket *raw_packet =
      reinterpret_cast<LivoxEthPacket *>(storage_packet.raw_data);
  timestamp = GetStoragePacketTimestamp(&storage_packet, data_source);
  if (timestamp >= 0) {

    if (timestamp_last_imu_ > timestamp) {
      diff_t_imu_ = timestamp_last_imu_ - timestamp;
    }

    if (diff_t_imu_ > 39809238968) {
      cnt_imu_ = ((int)((float)diff_t_imu_ / 40000000000.0));
      timestamp = timestamp + (cnt_imu_ + 1) * 40000000000;
    }
    imu_data.header.stamp =
        ros::Time(timestamp / 1000000000.0); // to ros time stamp
    timestamp_last_imu_ = timestamp;
  }

  uint8_t point_buf[2048];
  LivoxImuDataProcess(point_buf, raw_packet);

  LivoxImuPoint *imu = (LivoxImuPoint *)point_buf;
  imu_data.angular_velocity.x = imu->gyro_x;
  imu_data.angular_velocity.y = imu->gyro_y;
  imu_data.angular_velocity.z = imu->gyro_z;
  imu_data.linear_acceleration.x = imu->acc_x;
  imu_data.linear_acceleration.y = imu->acc_y;
  imu_data.linear_acceleration.z = imu->acc_z;

  QueuePopUpdate(queue);
  ++published_packet;

  ros::Publisher *p_publisher = Lddc::GetCurrentImuPublisher(handle);
  if (kOutputToRos == output_type_) {
    p_publisher->publish(imu_data);
  } else {
    if (bag_ && enable_imu_bag_) {
      bag_->write(p_publisher->getTopic(), ros::Time(timestamp / 1000000000.0),
                  imu_data);
    }
  }
  return published_packet;
}

[xuankuzcr]

别忘了定义这几个成员变量在class Lddc里。

volatile uint64_t timestamp_last_imu_ = 0;
volatile uint64_t diff_t_imu_;
volatile int cnt_imu_;