Use correct units in sensor_msgs::Imu

Chenh333 · Jul 24, 2018 · 874b8f7 · 874b8f7
1 parent b72a874
commit 874b8f7
Showing 1 changed file with 4 additions and 3 deletions.
diff --git a/ouster_ros/src/os1_ros.cpp b/ouster_ros/src/os1_ros.cpp
@@ -28,6 +28,7 @@ bool read_lidar_packet(const client& cli, PacketMsg& m) {
 }
 
 sensor_msgs::Imu packet_to_imu_msg(const PacketMsg& p) {
+    const double standard_g = 9.80665;
     sensor_msgs::Imu m;
     const uint8_t* buf = p.buf.data();
 
@@ -39,9 +40,9 @@ sensor_msgs::Imu packet_to_imu_msg(const PacketMsg& p) {
     m.orientation.z = 0;
     m.orientation.w = 0;
 
-    m.linear_acceleration.x = imu_la_x(buf);
-    m.linear_acceleration.y = imu_la_y(buf);
-    m.linear_acceleration.z = imu_la_z(buf);
+    m.linear_acceleration.x = imu_la_x(buf) * standard_g;
+    m.linear_acceleration.y = imu_la_y(buf) * standard_g;
+    m.linear_acceleration.z = imu_la_z(buf) * standard_g;
 
     m.angular_velocity.x = imu_av_x(buf) * M_PI / 180.0;
     m.angular_velocity.y = imu_av_y(buf) * M_PI / 180.0;