From 4de2cea834c8ccbaa241bc3da9544373e25a378a Mon Sep 17 00:00:00 2001 From: Steven Peters Date: Thu, 26 Mar 2020 02:20:54 +0100 Subject: [PATCH] Measure IMU orientation with respect to world (dashing) (#1065) Report the IMU orientation from the sensor plugin with respect to the world frame. This complies with convention documented in REP 145: https://www.ros.org/reps/rep-0145.html In order to not break existing behavior, users should opt-in by adding a new SDF tag. Co-authored-by: Jacob Perron --- gazebo_plugins/src/gazebo_ros_imu_sensor.cpp | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/gazebo_plugins/src/gazebo_ros_imu_sensor.cpp b/gazebo_plugins/src/gazebo_ros_imu_sensor.cpp index c85f19058..0c09eb882 100644 --- a/gazebo_plugins/src/gazebo_ros_imu_sensor.cpp +++ b/gazebo_plugins/src/gazebo_ros_imu_sensor.cpp @@ -66,6 +66,16 @@ void GazeboRosImuSensor::Load(gazebo::sensors::SensorPtr _sensor, sdf::ElementPt return; } + if (_sdf->HasElement("initial_orientation_as_reference")) { + bool initial_orientation_as_reference = _sdf->Get("initial_orientation_as_reference"); + RCLCPP_INFO(impl_->ros_node_->get_logger(), + " set to: %d", initial_orientation_as_reference); + if (!initial_orientation_as_reference) { + // This complies with REP 145 + impl_->sensor_->SetWorldToReferenceOrientation(ignition::math::Quaterniond::Identity); + } + } + impl_->pub_ = impl_->ros_node_->create_publisher("~/out", rclcpp::SensorDataQoS());