contributed by <`cwcoscar`> From NCKU MECLAB
Date : 2023/04/12

[Reference code](https://github.com/ethanec/ublox)

## topic: esfmeas
- The following picture shows the message on `/ublox_f9k/esfmeas`
- The most important thing is to decode **data[]**

- The following pisture is from EsfMEAS.msg
- It describes what information is in the data
- `DATA_FIELD_MASK = 16777215` in binary is `111111111111111111111111` (24digits).
- Ones represent the bits which are measurement.
- `DATA_TYPE_MASK = 1056964608` in binary is `111111000000000000000000000000` (30 digits).
- Ones represent the bits which are data type.
- the number from these bits can refer to one type according to the description in the picture.


- For example: `234881757` in binary is `001110000000000000001011011101` (30 digits)
- last 6 bits is `001110`, in decimal is `14`.
- It refer to `DATA_TYPE_GYRO_ANG_RATE_X = 14`
- first 24 bits is `0000000000000001011011101`, in decimal is `+733 [deg/s *2^-12 signed]`, which means `733/4096 [deg/s]`
- If the sign bit is `0`, it's positive and vise versa.
- The 24th bit is the sign bit.

### Frequency
- From the picture of `/ublox_f9k/esfmeas`, it shows the update time interval of accelerometer and gyroscope are both 0.05 second.
- Time tag is `[sec *10^-3]`

## Topic: imu_meas
- The following code is from node.cpp at line 1386.
- The origin version will publish the message at very high rate and it's because the publish function is write in the for loop.
- After moving it outside the publish function, the message is published at approximately 100 Hz.
- Only gyro and acc data are published seperately.
- The time interval of one kind is about 0.02 sec.
- The time interval of two kind is very small.

void AdrUdrProduct::callbackEsfMEAS(const ublox_msgs::EsfMEAS &m) {
  if (enabled["esf_meas"]) {
    static ros::Publisher imu_pub = 
	nh->advertise<sensor_msgs::Imu>("imu_meas", kROSQueueSize);
    static ros::Publisher time_ref_pub =
	nh->advertise<sensor_msgs::TimeReference>("interrupt_time", kROSQueueSize);
    
    imu_.header.stamp = ros::Time::now();
    imu_.header.frame_id = frame_id;
    
    static const float rad_per_sec = pow(2, -12) * M_PI / 180.0F;
    static const float m_per_sec_sq = pow(2, -10);
    static const float deg_c = 1e-2;
     
    std::vector<uint32_t> imu_data = m.data;
    for (int i=0; i < imu_data.size(); i++){
      unsigned int data_type = imu_data[i] >> 24; //grab the last six bits of data
      int32_t data_value = static_cast<int32_t>(imu_data[i] << 8); //shift to extend sign from 24 to 32 bit integer
      data_value >>= 8;

      imu_.orientation_covariance[0] = -1;
      imu_.linear_acceleration_covariance[0] = -1;
      imu_.angular_velocity_covariance[0] = -1;

      if (data_type == 14) {
          imu_.angular_velocity.x = data_value * rad_per_sec;
      } else if (data_type == 16) {
          imu_.linear_acceleration.x = data_value * m_per_sec_sq;
      } else if (data_type == 13) {
          imu_.angular_velocity.y = data_value * rad_per_sec;
      } else if (data_type == 17) {
          imu_.linear_acceleration.y = data_value * m_per_sec_sq;
      } else if (data_type == 5) {
          imu_.angular_velocity.z = data_value * rad_per_sec;
      } else if (data_type == 18) {
          imu_.linear_acceleration.z = data_value * m_per_sec_sq;
      } else if (data_type == 12) {
        //ROS_INFO("Temperature in celsius: %f", data_value * deg_c); 
      } else {
        // ROS_INFO("data_type: %u", data_type);
        // ROS_INFO("data_value: %u", data_value);
      }

      t_ref_.header.stamp = ros::Time::now(); // create a new timestamp
      t_ref_.header.frame_id = frame_id;
   
      time_ref_pub.publish(t_ref_);
      // cwc
      // imu_pub.publish(imu_);
    }
    // cwc
    /* 
    If publish function is in the for loop, there will be several message at the same time stamp with several idenical data
    After observing the "data[]" in this topic, there are 3 condition.
    1st: one data in the array, and it's wheel tick
    2nd: three data in the array, they are gyro data (time intervel is 0.05 sec)
    3rd: four data in the array, they are acc data and temperature data (time intervel is 0.05 sec)
    Only 2nd and 3rd message are what we are concerned.
    Thus only if the array length is over 1, the message will be published.
    The publishing rate of this topic will be approximately at 100 Hz.
    */
    if (imu_data.size() > 1){
      imu_pub.publish(imu_);
    }
    // cwc
  }
  
  updater->force_update();
}