From af8e031d75e69ccc2bc0895b399ccfef4b0244e6 Mon Sep 17 00:00:00 2001 From: Steven Peters Date: Thu, 12 Mar 2020 01:51:45 +0100 Subject: [PATCH 1/2] Measure IMU orientation with respect to world 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..38aff446c 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_STREAM(impl_->ros_node_->get_logger(), + " set to: " << 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()); From 1cc408317ba4916b005dded4ac668e3df316b9bd Mon Sep 17 00:00:00 2001 From: Steven Peters Date: Wed, 25 Mar 2020 15:23:16 -0700 Subject: [PATCH 2/2] use RCLCPP_INFO with format string --- gazebo_plugins/src/gazebo_ros_imu_sensor.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/gazebo_plugins/src/gazebo_ros_imu_sensor.cpp b/gazebo_plugins/src/gazebo_ros_imu_sensor.cpp index 38aff446c..0c09eb882 100644 --- a/gazebo_plugins/src/gazebo_ros_imu_sensor.cpp +++ b/gazebo_plugins/src/gazebo_ros_imu_sensor.cpp @@ -68,8 +68,8 @@ void GazeboRosImuSensor::Load(gazebo::sensors::SensorPtr _sensor, sdf::ElementPt if (_sdf->HasElement("initial_orientation_as_reference")) { bool initial_orientation_as_reference = _sdf->Get("initial_orientation_as_reference"); - RCLCPP_INFO_STREAM(impl_->ros_node_->get_logger(), - " set to: " << 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);