Skip to content

Fix imu 2d transform #268

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
wants to merge 7 commits into
base: devel
Choose a base branch
from
Open

Fix imu 2d transform #268

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
7 commits
Select commit Hold shift + click to select a range
2cd0381
Add diffbot
efernandez Apr 12, 2022
238dc89
Add IMU test
efernandez Apr 12, 2022
02d2323
Fix IMU transform
efernandez Apr 13, 2022
d9e246b
Fix -Werror=sign-compare warnings
efernandez May 12, 2022
75753e2
Include mutex header
efernandez May 18, 2022
60a1328
Fix roslint issues
efernandez May 19, 2022
0dbc4ad
Add missed xacro test depend
efernandez Jun 14, 2022
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
18 changes: 18 additions & 0 deletions fuse_models/CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -11,6 +11,7 @@ find_package(
fuse_publishers
fuse_variables
geometry_msgs
imu_transformer
message_generation
nav_msgs
pluginlib
Expand Down Expand Up @@ -149,6 +150,7 @@ install(

if(CATKIN_ENABLE_TESTING)
find_package(rostest REQUIRED)
find_package(catkin REQUIRED COMPONENTS angles)

# Lint tests
roslint_add_test()
Expand Down Expand Up @@ -227,6 +229,22 @@ if(CATKIN_ENABLE_TESTING)
CXX_STANDARD_REQUIRED YES
)

# Imu2D tests
add_rostest_gtest(
test_imu_2d
test/imu_2d.test
test/test_imu_2d.cpp
)
target_link_libraries(test_imu_2d
${PROJECT_NAME}
${catkin_LIBRARIES}
)
set_target_properties(test_imu_2d
PROPERTIES
CXX_STANDARD 14
CXX_STANDARD_REQUIRED YES
)

# Unicycle2D Ignition tests
add_rostest_gtest(
test_unicycle_2d_ignition
Expand Down
8 changes: 8 additions & 0 deletions fuse_models/package.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -24,6 +24,7 @@
<depend>fuse_publishers</depend>
<depend>fuse_variables</depend>
<depend>geometry_msgs</depend>
<depend>imu_transformer</depend>
<depend>nav_msgs</depend>
<depend>pluginlib</depend>
<depend>roscpp</depend>
Expand All @@ -39,6 +40,7 @@

<exec_depend>message_runtime</exec_depend>

<test_depend>angles</test_depend>
<!-- The official rosdep has an entry for Google's benchmark deb
(https://github.com/ros/rosdistro/blob/2cbcebdedcc1e827501acfecaf2386cd33f26809/rosdep/base.yaml#L232-L239)
since https://github.com/ros/rosdistro/pull/23163 got merged.
Expand All @@ -52,7 +54,13 @@
For this reason we conditionally test_depend on benchmark only if the $ROS_DISTRO is ROS Melodic or newer.
-->
<test_depend condition="$ROS_DISTRO >= melodic">benchmark</test_depend>
<test_depend>controller_manager</test_depend>
<test_depend>diff_drive_controller</test_depend>
<test_depend>gazebo_ros</test_depend>
<test_depend>joint_state_controller</test_depend>
<test_depend>robot_state_publisher</test_depend>
<test_depend>rostest</test_depend>
<test_depend>xacro</test_depend>

<export>
<fuse_core plugin="${prefix}/fuse_plugins.xml" />
Expand Down
153 changes: 72 additions & 81 deletions fuse_models/src/imu_2d.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -40,7 +40,8 @@
#include <geometry_msgs/AccelWithCovarianceStamped.h>
#include <geometry_msgs/PoseWithCovarianceStamped.h>
#include <geometry_msgs/TwistWithCovarianceStamped.h>
#include <pluginlib/class_list_macros.hpp>
#include <imu_transformer/tf2_sensor_msgs.h>
#include <pluginlib/class_list_macros.h>
#include <ros/ros.h>
#include <sensor_msgs/Imu.h>

Expand Down Expand Up @@ -105,47 +106,61 @@ void Imu2D::process(const sensor_msgs::Imu::ConstPtr& msg)
auto transaction = fuse_core::Transaction::make_shared();
transaction->stamp(msg->header.stamp);

// Transform to target frame
sensor_msgs::Imu imu_transformed;
try
{
tf_buffer_.transform(*msg, imu_transformed, params_.twist_target_frame);
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

There are currently multiple target frames supported:
https://github.com/locusrobotics/fuse/blob/devel/fuse_models/include/fuse_models/parameters/imu_2d_params.h#L99-L101

...though I'm not entirely sure why. I would think that both the twist and acceleration would want to use the same target/body frame.

And it's been a long time since I thought about IMUs, but my recollection is that transforming the IMU pose is complicated. I need to review the imu_transformer package in detail. It was not clear to me how the orientation was being transformed.

Copy link
Collaborator Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Yes, that's something I wanted to discuss in this PR and I forgot to mention it.

Right now there are three different target frames:

  • acceleration_target_frame
  • orientation_target_frame
  • twist_target_frame

However, the implementation in imu_transformer transforms everything in the IMU all at once with a single target frame.

I wonder if it really makes sense to have multiple target frames. 🤔

In that case, we could call the imu_transformer code three times and only take the components we're interested in, with each target frame. However, in that case, it'd be more efficient, and I believe still clean, to steal some of the code in imu_transformer to perform the same transformation for each component separately.

I believe the twist is already done correctly, and probably the acceleration as well. The orientation is definitely incorrect.

Just let me know whether you want to keep the three target frames or not, and I'll make the changes mentioned above?

If we remove the existing target frames and create a singal target_frame, then we need some deprecation warnings, and set the new target_frame with them. In that case, if the different target frames aren't the same, I'm not sure which one should be used. I've been using twist_target_frame in this initial implementation.

}
catch (const tf2::TransformException& ex)
{
ROS_WARN_STREAM_THROTTLE(5.0, "Cannot transform IMU message with stamp " << msg->header.stamp << " to target frame "
<< params_.twist_target_frame
<< ". Error: " << ex.what());
return;
}

// Handle the orientation data (treat it as a pose, but with only orientation indices used)
auto pose = std::make_unique<geometry_msgs::PoseWithCovarianceStamped>();
pose->header = msg->header;
pose->pose.pose.orientation = msg->orientation;
pose->pose.covariance[21] = msg->orientation_covariance[0];
pose->pose.covariance[22] = msg->orientation_covariance[1];
pose->pose.covariance[23] = msg->orientation_covariance[2];
pose->pose.covariance[27] = msg->orientation_covariance[3];
pose->pose.covariance[28] = msg->orientation_covariance[4];
pose->pose.covariance[29] = msg->orientation_covariance[5];
pose->pose.covariance[33] = msg->orientation_covariance[6];
pose->pose.covariance[34] = msg->orientation_covariance[7];
pose->pose.covariance[35] = msg->orientation_covariance[8];
geometry_msgs::PoseWithCovarianceStamped pose;
pose.header = imu_transformed.header;
pose.pose.pose.orientation = imu_transformed.orientation;
pose.pose.covariance[21] = imu_transformed.orientation_covariance[0];
pose.pose.covariance[22] = imu_transformed.orientation_covariance[1];
pose.pose.covariance[23] = imu_transformed.orientation_covariance[2];
pose.pose.covariance[27] = imu_transformed.orientation_covariance[3];
pose.pose.covariance[28] = imu_transformed.orientation_covariance[4];
pose.pose.covariance[29] = imu_transformed.orientation_covariance[5];
pose.pose.covariance[33] = imu_transformed.orientation_covariance[6];
pose.pose.covariance[34] = imu_transformed.orientation_covariance[7];
pose.pose.covariance[35] = imu_transformed.orientation_covariance[8];

geometry_msgs::TwistWithCovarianceStamped twist;
twist.header = msg->header;
twist.twist.twist.angular = msg->angular_velocity;
twist.twist.covariance[21] = msg->angular_velocity_covariance[0];
twist.twist.covariance[22] = msg->angular_velocity_covariance[1];
twist.twist.covariance[23] = msg->angular_velocity_covariance[2];
twist.twist.covariance[27] = msg->angular_velocity_covariance[3];
twist.twist.covariance[28] = msg->angular_velocity_covariance[4];
twist.twist.covariance[29] = msg->angular_velocity_covariance[5];
twist.twist.covariance[33] = msg->angular_velocity_covariance[6];
twist.twist.covariance[34] = msg->angular_velocity_covariance[7];
twist.twist.covariance[35] = msg->angular_velocity_covariance[8];
twist.header = imu_transformed.header;
twist.twist.twist.angular = imu_transformed.angular_velocity;
twist.twist.covariance[21] = imu_transformed.angular_velocity_covariance[0];
twist.twist.covariance[22] = imu_transformed.angular_velocity_covariance[1];
twist.twist.covariance[23] = imu_transformed.angular_velocity_covariance[2];
twist.twist.covariance[27] = imu_transformed.angular_velocity_covariance[3];
twist.twist.covariance[28] = imu_transformed.angular_velocity_covariance[4];
twist.twist.covariance[29] = imu_transformed.angular_velocity_covariance[5];
twist.twist.covariance[33] = imu_transformed.angular_velocity_covariance[6];
twist.twist.covariance[34] = imu_transformed.angular_velocity_covariance[7];
twist.twist.covariance[35] = imu_transformed.angular_velocity_covariance[8];

const bool validate = !params_.disable_checks;

if (params_.differential)
{
processDifferential(*pose, twist, validate, *transaction);
processDifferential(pose, twist, validate, *transaction);
}
else
{
common::processAbsolutePoseWithCovariance(
name(),
device_id_,
*pose,
pose,
params_.pose_loss,
params_.orientation_target_frame,
params_.twist_target_frame,
{},
params_.orientation_indices,
tf_buffer_,
Expand All @@ -171,17 +186,17 @@ void Imu2D::process(const sensor_msgs::Imu::ConstPtr& msg)

// Handle the acceleration data
geometry_msgs::AccelWithCovarianceStamped accel;
accel.header = msg->header;
accel.accel.accel.linear = msg->linear_acceleration;
accel.accel.covariance[0] = msg->linear_acceleration_covariance[0];
accel.accel.covariance[1] = msg->linear_acceleration_covariance[1];
accel.accel.covariance[2] = msg->linear_acceleration_covariance[2];
accel.accel.covariance[6] = msg->linear_acceleration_covariance[3];
accel.accel.covariance[7] = msg->linear_acceleration_covariance[4];
accel.accel.covariance[8] = msg->linear_acceleration_covariance[5];
accel.accel.covariance[12] = msg->linear_acceleration_covariance[6];
accel.accel.covariance[13] = msg->linear_acceleration_covariance[7];
accel.accel.covariance[14] = msg->linear_acceleration_covariance[8];
accel.header = imu_transformed.header;
accel.accel.accel.linear = imu_transformed.linear_acceleration;
accel.accel.covariance[0] = imu_transformed.linear_acceleration_covariance[0];
accel.accel.covariance[1] = imu_transformed.linear_acceleration_covariance[1];
accel.accel.covariance[2] = imu_transformed.linear_acceleration_covariance[2];
accel.accel.covariance[6] = imu_transformed.linear_acceleration_covariance[3];
accel.accel.covariance[7] = imu_transformed.linear_acceleration_covariance[4];
accel.accel.covariance[8] = imu_transformed.linear_acceleration_covariance[5];
accel.accel.covariance[12] = imu_transformed.linear_acceleration_covariance[6];
accel.accel.covariance[13] = imu_transformed.linear_acceleration_covariance[7];
accel.accel.covariance[14] = imu_transformed.linear_acceleration_covariance[8];

// Optionally remove the acceleration due to gravity
if (params_.remove_gravitational_acceleration)
Expand All @@ -190,7 +205,7 @@ void Imu2D::process(const sensor_msgs::Imu::ConstPtr& msg)
accel_gravity.z = params_.gravitational_acceleration;
geometry_msgs::TransformStamped orientation_trans;
tf2::Quaternion imu_orientation;
tf2::fromMsg(msg->orientation, imu_orientation);
tf2::fromMsg(imu_transformed.orientation, imu_orientation);
orientation_trans.transform.rotation = tf2::toMsg(imu_orientation.inverse());
tf2::doTransform(accel_gravity, accel_gravity, orientation_trans); // Doesn't use the stamp
accel.accel.accel.linear.x -= accel_gravity.x;
Expand All @@ -203,7 +218,7 @@ void Imu2D::process(const sensor_msgs::Imu::ConstPtr& msg)
device_id_,
accel,
params_.linear_acceleration_loss,
params_.acceleration_target_frame,
params_.twist_target_frame,
params_.linear_acceleration_indices,
tf_buffer_,
validate,
Expand All @@ -218,60 +233,36 @@ void Imu2D::processDifferential(const geometry_msgs::PoseWithCovarianceStamped&
const geometry_msgs::TwistWithCovarianceStamped& twist, const bool validate,
fuse_core::Transaction& transaction)
{
auto transformed_pose = std::make_unique<geometry_msgs::PoseWithCovarianceStamped>();
transformed_pose->header.frame_id =
params_.orientation_target_frame.empty() ? pose.header.frame_id : params_.orientation_target_frame;

if (!common::transformMessage(tf_buffer_, pose, *transformed_pose))
{
ROS_WARN_STREAM_THROTTLE(5.0, "Cannot transform pose message with stamp " << pose.header.stamp
<< " to orientation target frame "
<< params_.orientation_target_frame);
return;
}

if (!previous_pose_)
{
previous_pose_ = std::move(transformed_pose);
previous_pose_ = std::make_unique<geometry_msgs::PoseWithCovarianceStamped>();
*previous_pose_ = pose;
return;
}

if (params_.use_twist_covariance)
{
geometry_msgs::TwistWithCovarianceStamped transformed_twist;
transformed_twist.header.frame_id =
params_.twist_target_frame.empty() ? twist.header.frame_id : params_.twist_target_frame;

if (!common::transformMessage(tf_buffer_, twist, transformed_twist))
{
ROS_WARN_STREAM_THROTTLE(5.0, "Cannot transform twist message with stamp " << twist.header.stamp
<< " to twist target frame "
<< params_.twist_target_frame);
}
else
{
common::processDifferentialPoseWithTwistCovariance(
name(),
device_id_,
*previous_pose_,
*transformed_pose,
twist,
params_.minimum_pose_relative_covariance,
params_.twist_covariance_offset,
params_.pose_loss,
{},
params_.orientation_indices,
validate,
transaction);
}
common::processDifferentialPoseWithTwistCovariance(
name(),
device_id_,
*previous_pose_,
pose,
twist,
params_.minimum_pose_relative_covariance,
params_.twist_covariance_offset,
params_.pose_loss,
{},
params_.orientation_indices,
validate,
transaction);
}
else
{
common::processDifferentialPoseWithCovariance(
name(),
device_id_,
*previous_pose_,
*transformed_pose,
pose,
params_.independent,
params_.minimum_pose_relative_covariance,
params_.pose_loss,
Expand All @@ -281,7 +272,7 @@ void Imu2D::processDifferential(const geometry_msgs::PoseWithCovarianceStamped&
transaction);
}

previous_pose_ = std::move(transformed_pose);
*previous_pose_ = pose;
}

} // namespace fuse_models
30 changes: 30 additions & 0 deletions fuse_models/test/diffbot.launch
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,30 @@
<launch>
<arg name="gui" default="false"/>

<!-- Start world -->
<include file="$(find gazebo_ros)/launch/empty_world.launch">
<arg name="gui" value="$(arg gui)"/>
</include>

<!-- Load diffbot model -->
<param name="robot_description"
command="$(find xacro)/xacro '$(find fuse_models)/test/diffbot.xacro'"/>

<!-- Load diffbot config, e.g. PID gains -->
<rosparam command="load" file="$(find fuse_models)/test/diffbot.yaml"/>

<!-- Spawn diffbot robot in gazebo -->
<node name="spawn_diffbot" pkg="gazebo_ros" type="spawn_model"
args="-urdf -param robot_description -model diffbot -z 0.5"/>

<!-- Load controller config -->
<rosparam command="load" file="$(find fuse_models)/test/diffbot_controllers.yaml"/>

<!-- Spawn controller -->
<node name="controller_spawner" pkg="controller_manager" type="spawner"
args="joint_state_controller
diffbot_controller"/>

<!-- Convert joint states to TF transforms -->
<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher"/>
</launch>
Loading