drivers:gnss add imu parse. (#1351)

* drivers:gnss add imu parse.

* drivers:gnss add imu parse.

modules/drivers/gnss/src/impl/parser/novatel/novatel_messages.h

@@ -45,6 +45,7 @@ enum MessageId : uint16_t {
   PSRPOS = 47,
   PSRVEL = 100,
   RAWIMU = 268,
+  RAWIMUS = 325,
   RAWIMUX = 1461,
   RAWIMUSX = 1462,
   MARK1PVA = 1067,
@@ -473,7 +474,7 @@ static_assert(sizeof(RawImuX) == 40, "Incorrect size of RawImuX");
 struct RawImu {
   uint32_t gps_week;
   double gps_seconds;  // Seconds of week.
-  char imuStatus[4];   // Status of the IMU. The content varies with IMU type.
+  uint32_t imuStatus;   // Status of the IMU. The content varies with IMU type.
   int32_t z_velocity_change;      // change in velocity along z axis.
   int32_t y_velocity_change_neg;  // -change in velocity along y axis.
   int32_t x_velocity_change;      // change in velocity along x axis.

modules/drivers/gnss/src/impl/parser/novatel/novatel_parser.cpp

@@ -134,6 +134,8 @@ class NovatelParser : public Parser {
 
   bool handle_raw_imu_x(const novatel::RawImuX* imu);
 
+  bool handle_raw_imu(const novatel::RawImu* imu);
+
   bool handle_bds_eph(const novatel::BDS_Ephemeris* bds_emph);
 
   bool handle_gps_eph(const novatel::GPS_Ephemeris* gps_emph);
@@ -398,6 +400,19 @@ Parser::MessageType NovatelParser::prepare_message(MessagePtr& message_ptr) {
       }
       break;
 
+    case novatel::RAWIMU:
+    case novatel::RAWIMUS:
+      if (message_length != sizeof(novatel::RawImu)) {
+        ROS_ERROR("Incorrect message_length");
+        break;
+      }
+
+      if (handle_raw_imu(reinterpret_cast<novatel::RawImu*>(message))) {
+        message_ptr = &_imu;
+        return MessageType::IMU;
+      }
+      break;
+
     case novatel::INSPVAX:
       if (message_length != sizeof(novatel::InsPvaX)) {
         ROS_ERROR("Incorrect message_length");
@@ -736,6 +751,74 @@ bool NovatelParser::handle_raw_imu_x(const novatel::RawImuX* imu) {
   return true;
 }
 
+bool NovatelParser::handle_raw_imu(const novatel::RawImu* imu) {
+  double gyro_scale = 0.0;
+  double accel_scale = 0.0;
+  float imu_measurement_span = 1.0 / 200.0;
+
+  if (is_zero(_gyro_scale)) {
+    novatel::ImuParameter param = novatel::get_imu_parameter(
+                                    novatel::ImuType::ADIS16488);
+    //ROS_INFO_STREAM("IMU type: " << static_cast<unsigned>(imu->imu_type) << "; "
+    //                             << "Gyro scale: " << param.gyro_scale << "; "
+    //                             << "Accel scale: " << param.accel_scale << "; "
+    //                             << "Sampling rate: " << param.sampling_rate_hz
+    //                             << ".");
+
+    if (is_zero(param.sampling_rate_hz)) {
+      ROS_ERROR_STREAM_THROTTLE(
+          5, "Unsupported IMU type ADUS16488.");
+      return false;
+    }
+    gyro_scale = param.gyro_scale * param.sampling_rate_hz;
+    accel_scale = param.accel_scale * param.sampling_rate_hz;
+    imu_measurement_span = 1.0 / param.sampling_rate_hz;
+    _imu.set_measurement_span(imu_measurement_span);
+  } else {
+    gyro_scale = _gyro_scale;
+    accel_scale = _accel_scale;
+    imu_measurement_span = _imu_measurement_span;
+    _imu.set_measurement_span(imu_measurement_span);
+  }
+
+  double time = imu->gps_week * SECONDS_PER_WEEK + imu->gps_seconds;
+  if (_imu_measurement_time_previous > 0.0 &&
+      fabs(time - _imu_measurement_time_previous - imu_measurement_span) >
+          1e-4) {
+    ROS_WARN_STREAM("Unexpected delay between two IMU measurements at: "
+                    << time - _imu_measurement_time_previous);
+  }
+
+  _imu.set_measurement_time(time);
+  switch (_imu_frame_mapping) {
+    case 5:  // Default mapping.
+      rfu_to_flu(imu->x_velocity_change * accel_scale,
+                 -imu->y_velocity_change_neg * accel_scale,
+                 imu->z_velocity_change * accel_scale,
+                 _imu.mutable_linear_acceleration());
+      rfu_to_flu(imu->x_angle_change * gyro_scale,
+                 -imu->y_angle_change_neg * gyro_scale,
+                 imu->z_angle_change * gyro_scale,
+                 _imu.mutable_angular_velocity());
+      break;
+    case 6:
+      rfu_to_flu(-imu->y_velocity_change_neg * accel_scale,
+                 imu->x_velocity_change * accel_scale,
+                 -imu->z_velocity_change * accel_scale,
+                 _imu.mutable_linear_acceleration());
+      rfu_to_flu(-imu->y_angle_change_neg * gyro_scale,
+                 imu->x_angle_change * gyro_scale,
+                 -imu->z_angle_change * gyro_scale,
+                 _imu.mutable_angular_velocity());
+      break;
+    default:
+      ROS_ERROR_STREAM_THROTTLE(
+          5, "Unsupported IMU frame mapping: " << _imu_frame_mapping);
+  }
+  _imu_measurement_time_previous = time;
+  return true;
+}
+
 bool NovatelParser::handle_gps_eph(const novatel::GPS_Ephemeris* gps_emph) {
   _gnss_ephemeris.set_gnss_type(apollo::drivers::gnss::GnssType::GPS_SYS);
 

modules/drivers/gnss/src/parser/data_parser.cpp

@@ -265,6 +265,17 @@ void DataParser::publish_imu_message(const MessagePtr message) {
   boost::shared_ptr<::apollo::drivers::gnss::Imu> raw_imu(
       new ::apollo::drivers::gnss::Imu(
           *As<::apollo::drivers::gnss::Imu>(message)));
+
+  ::apollo::drivers::gnss::Imu *imu = As<::apollo::drivers::gnss::Imu>(message);
+
+  raw_imu->mutable_linear_acceleration()->set_x(-imu->linear_acceleration().y());
+  raw_imu->mutable_linear_acceleration()->set_y(-imu->linear_acceleration().x());
+  raw_imu->mutable_linear_acceleration()->set_z(imu->linear_acceleration().z());
+
+  raw_imu->mutable_angular_velocity()->set_x(-imu->angular_velocity().y());
+  raw_imu->mutable_angular_velocity()->set_y(-imu->angular_velocity().x());
+  raw_imu->mutable_angular_velocity()->set_z(imu->angular_velocity().z());
+
   raw_imu->mutable_header()->set_timestamp_sec(ros::Time::Time::now().toSec());
   _raw_imu_publisher.publish(raw_imu);
 }