Skip to content

Instantly share code, notes, and snippets.

@matlabbe
Created March 2, 2020 19:52
Show Gist options
  • Star 0 You must be signed in to star a gist
  • Fork 0 You must be signed in to fork a gist
  • Save matlabbe/08716c1e364b72692ac22985a1266845 to your computer and use it in GitHub Desktop.
Save matlabbe/08716c1e364b72692ac22985a1266845 to your computer and use it in GitHub Desktop.
Ouster fix to use host timestamp and imu gravity inverted
diff --git a/ouster_ros/src/os1_cloud_node.cpp b/ouster_ros/src/os1_cloud_node.cpp
index 8a7e8ec..964cfa6 100644
--- a/ouster_ros/src/os1_cloud_node.cpp
+++ b/ouster_ros/src/os1_cloud_node.cpp
@@ -59,6 +59,7 @@ int main(int argc, char** argv) {
[&](uint64_t scan_ts) mutable {
msg = ouster_ros::OS1::cloud_to_cloud_msg(
cloud, std::chrono::nanoseconds{scan_ts}, lidar_frame);
+ msg.header.stamp = ros::Time::now();
lidar_pub.publish(msg);
});
@@ -67,7 +68,9 @@ int main(int argc, char** argv) {
};
auto imu_handler = [&](const PacketMsg& p) {
- imu_pub.publish(ouster_ros::OS1::packet_to_imu_msg(p, imu_frame));
+ sensor_msgs::Imu msg = ouster_ros::OS1::packet_to_imu_msg(p, imu_frame);
+ msg.header.stamp = ros::Time::now();
+ imu_pub.publish(msg);
};
auto lidar_packet_sub = nh.subscribe<PacketMsg, const PacketMsg&>(
diff --git a/ouster_ros/src/os1_ros.cpp b/ouster_ros/src/os1_ros.cpp
index abb437c..d6d7a08 100644
--- a/ouster_ros/src/os1_ros.cpp
+++ b/ouster_ros/src/os1_ros.cpp
@@ -26,7 +26,7 @@ bool read_lidar_packet(const client& cli, PacketMsg& m) {
sensor_msgs::Imu packet_to_imu_msg(const PacketMsg& p,
const std::string& frame) {
- const double standard_g = 9.80665;
+ const double standard_g = -9.80665;
sensor_msgs::Imu m;
const uint8_t* buf = p.buf.data();
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment