坐标转换：将imu坐标系下的角速度、线速度转换到车体坐标系，参考Ａutoware

需求：获取geometry_msgs::TwistStamped格式的速度信息（角速度＋线速度）
方法１：从底盘获取线速度，imu获取偏航角速度数据，然后组合数据发布信息。
方法２：通过融合定位输出的位姿（x,y,yaw)，通过前后帧计算线速度和偏航角速度，然后发布信息
方法１实现：
头文件：

#pragma once

#include <ros/ros.h>

#include <tf2/transform_datatypes.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <tf2_ros/transform_listener.h>

#include <std_msgs/Float32.h>

#include <geometry_msgs/TwistStamped.h>
#include <geometry_msgs/TwistWithCovarianceStamped.h>
#include <sensor_msgs/Imu.h>

class GyroOdometer
{
public:
  GyroOdometer(ros::NodeHandle nh, ros::NodeHandle private_nh);
  ~GyroOdometer();

private:
  void callbackTwist(const geometry_msgs::TwistStamped::ConstPtr & twist_msg_ptr);
  void callbackImu(const sensor_msgs::Imu::ConstPtr & imu_msg_ptr);
  bool getTransform(
    const std::string & target_frame, const std::string & source_frame,
    const geometry_msgs::TransformStamped::Ptr & transform_stamped_ptr);

  ros::NodeHandle nh_;
  ros::NodeHandle private_nh_;

  ros::Subscriber vehicle_twist_sub_;
  ros::Subscriber imu_sub_;

  ros::Publisher twist_pub_;
  ros::Publisher twist_with_covariance_pub_;
  ros::Publisher linear_x_pub_;
  ros::Publisher angular_z_pub_;

  tf2_ros::Buffer tf2_buffer_;
  tf2_ros::TransformListener tf2_listener_;

  std::string output_frame_;
  geometry_msgs::TwistStamped::ConstPtr twist_msg_ptr_;
};

实现文件：

#include "gyro_odometer/gyro_odometer_core.h"

#include <cmath>

#include <tf2_geometry_msgs/tf2_geometry_msgs.h>

GyroOdometer::GyroOdometer(ros::NodeHandle nh, ros::NodeHandle private_nh)
: nh_(nh), private_nh_(private_nh), output_frame_("base_link"), tf2_listener_(tf2_buffer_)
{
  private_nh_.getParam("output_frame", output_frame_);

  vehicle_twist_sub_ = nh_.subscribe("vehicle/twist", 100, &GyroOdometer::callbackTwist, this);
  imu_sub_ = nh_.subscribe("imu", 100, &GyroOdometer::callbackImu, this);

  twist_pub_ = nh_.advertise<geometry_msgs::TwistStamped>("twist", 10);
  twist_with_covariance_pub_ =nh_.advertise<geometry_msgs::TwistWithCovarianceStamped>("twist_with_covariance", 10);
  // linear_x_pub_ = nh_.advertise<std_msgs::Float32>("linear_x", 10);
  // angular_z_pub_ = nh_.advertise<std_msgs::Float32>("angular_z", 10);

  // TODO createTimer
}

GyroOdometer::~GyroOdometer() {}

void GyroOdometer::callbackTwist(const geometry_msgs::TwistStamped::ConstPtr & twist_msg_ptr)
{
  // TODO trans from twist_msg_ptr->header to base_frame
  twist_msg_ptr_ = twist_msg_ptr;
}

void GyroOdometer::callbackImu(const sensor_msgs::Imu::ConstPtr & imu_msg_ptr)
{
  if (twist_msg_ptr_ == nullptr) 
  {
    return;
  }

  geometry_msgs::TransformStamped::Ptr tf_base2imu_ptr(new geometry_msgs::TransformStamped);
  getTransform(output_frame_, imu_msg_ptr->header.frame_id, tf_base2imu_ptr);

  geometry_msgs::Vector3Stamped angular_velocity;
  angular_velocity.header = imu_msg_ptr->header;
  angular_velocity.vector = imu_msg_ptr->angular_velocity;

  geometry_msgs::Vector3Stamped transed_angular_velocity;
  transed_angular_velocity.header = tf_base2imu_ptr->header;

  tf2::doTransform(angular_velocity, transed_angular_velocity, *tf_base2imu_ptr);

  // clear imu yaw bias if vehicle is stopped
  if (
    std::fabs(transed_angular_velocity.vector.z) < 0.01 &&
    std::fabs(twist_msg_ptr_->twist.linear.x) < 0.01) //车辆的线速度基本上为0
  {
    transed_angular_velocity.vector.z = 0.0;
  }

  // TODO move code
  geometry_msgs::TwistStamped twist;
  twist.header.stamp = imu_msg_ptr->header.stamp;
  twist.header.frame_id = output_frame_;
  twist.twist.linear = twist_msg_ptr_->twist.linear;
  twist.twist.angular.z = transed_angular_velocity.vector.z;  // TODO yaw_rate only
  twist_pub_.publish(twist);

  geometry_msgs::TwistWithCovarianceStamped twist_with_covariance;
  twist_with_covariance.header.stamp = imu_msg_ptr->header.stamp;
  twist_with_covariance.header.frame_id = output_frame_;
  twist_with_covariance.twist.twist.linear = twist_msg_ptr_->twist.linear;
  twist_with_covariance.twist.twist.angular.z =
    transed_angular_velocity.vector.z;  // TODO yaw_rate only
  // TODO temporary value
  const double vx_covariance = 0.2;
  const double wz_covariance = 0.03;
  twist_with_covariance.twist.covariance[0] = vx_covariance * vx_covariance;
  twist_with_covariance.twist.covariance[0 * 6 + 5] = vx_covariance * wz_covariance;
  twist_with_covariance.twist.covariance[5 * 6 + 0] = wz_covariance * vx_covariance;
  twist_with_covariance.twist.covariance[5 * 6 + 5] = wz_covariance * wz_covariance;
  twist_with_covariance_pub_.publish(twist_with_covariance);
}

bool GyroOdometer::getTransform(
  const std::string & target_frame, const std::string & source_frame,
  const geometry_msgs::TransformStamped::Ptr & transform_stamped_ptr)
{
  if (target_frame == source_frame) {
    transform_stamped_ptr->header.stamp = ros::Time::now();
    transform_stamped_ptr->header.frame_id = target_frame;
    transform_stamped_ptr->child_frame_id = source_frame;
    transform_stamped_ptr->transform.translation.x = 0.0;
    transform_stamped_ptr->transform.translation.y = 0.0;
    transform_stamped_ptr->transform.translation.z = 0.0;
    transform_stamped_ptr->transform.rotation.x = 0.0;
    transform_stamped_ptr->transform.rotation.y = 0.0;
    transform_stamped_ptr->transform.rotation.z = 0.0;
    transform_stamped_ptr->transform.rotation.w = 1.0;
    return true;
  }

  try {
    *transform_stamped_ptr =
      tf2_buffer_.lookupTransform(target_frame, source_frame, ros::Time(0), ros::Duration(1.0));
  } catch (tf2::TransformException & ex) {
    ROS_WARN("%s", ex.what());
    ROS_ERROR("Please publish TF %s to %s", target_frame.c_str(), source_frame.c_str());

    transform_stamped_ptr->header.stamp = ros::Time::now();
    transform_stamped_ptr->header.frame_id = target_frame;
    transform_stamped_ptr->child_frame_id = source_frame;
    transform_stamped_ptr->transform.translation.x = 0.0;
    transform_stamped_ptr->transform.translation.y = 0.0;
    transform_stamped_ptr->transform.translation.z = 0.0;
    transform_stamped_ptr->transform.rotation.x = 0.0;
    transform_stamped_ptr->transform.rotation.y = 0.0;
    transform_stamped_ptr->transform.rotation.z = 0.0;
    transform_stamped_ptr->transform.rotation.w = 1.0;
    return false;
  }
  return true;
}

main：

#include <ros/ros.h>

#include "gyro_odometer/gyro_odometer_core.h"

int main(int argc, char ** argv)
{
  ros::init(argc, argv, "gyro_odometer");
  ros::NodeHandle nh;
  ros::NodeHandle private_nh("~");

  GyroOdometer node(nh, private_nh);

  ros::spin();
  return 0;
}

启动launch:

<launch>

  <arg name="input_vehicle_twist_topic" default="/vehicle/status/twist" doc="" />
  <arg name="input_imu_topic" default="/sensing/imu/imu_data" doc="" />

  <arg name="output_twist_topic" default="gyro_twist" doc="" />
  <arg name="output_twist_with_covariance_topic" default="gyro_twist_with_covariance" doc="" />

  <arg name="output_frame" default="base_link" doc="" />

  <node pkg="gyro_odometer" type="gyro_odometer" name="gyro_odometer" output="log">
    <remap from="vehicle/twist" to="$(arg input_vehicle_twist_topic)" />
    <remap from="imu" to="$(arg input_imu_topic)" />

    <remap from="twist" to="$(arg output_twist_topic)" />
    <remap from="twist_with_covariance" to="$(arg output_twist_with_covariance_topic)" />

    <param name="output_frame" value="$(arg output_frame)" />
  </node>

</launch>

方法２实现：
头文件：

#include <ros/ros.h>

#include <std_msgs/Float32.h>

#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/TwistStamped.h>

class Pose2Twist
{
public:
  Pose2Twist(ros::NodeHandle nh, ros::NodeHandle private_nh);
  ~Pose2Twist();

private:
  void callbackPose(const geometry_msgs::PoseStamped::ConstPtr & pose_msg_ptr);

  ros::NodeHandle nh_;
  ros::NodeHandle private_nh_;

  ros::Subscriber pose_sub_;

  ros::Publisher twist_pub_;
  ros::Publisher linear_x_pub_;
  ros::Publisher angular_z_pub_;
};

实现文件：

#include "pose2twist/pose2twist_core.h"

#include <cmath>

#include <tf2_geometry_msgs/tf2_geometry_msgs.h>

Pose2Twist::Pose2Twist(ros::NodeHandle nh, ros::NodeHandle private_nh)
: nh_(nh), private_nh_(private_nh)
{
  pose_sub_ = nh_.subscribe("pose", 100, &Pose2Twist::callbackPose, this);

  twist_pub_ = nh_.advertise<geometry_msgs::TwistStamped>("twist", 10);
  linear_x_pub_ = nh_.advertise<std_msgs::Float32>("linear_x", 10);
  angular_z_pub_ = nh_.advertise<std_msgs::Float32>("angular_z", 10);
}

Pose2Twist::~Pose2Twist() {}

double calcDiffForRadian(const double lhs_rad, const double rhs_rad)
{
  double diff_rad = lhs_rad - rhs_rad;
  if (diff_rad > M_PI) {
    diff_rad = diff_rad - 2 * M_PI;
  } else if (diff_rad < -M_PI) {
    diff_rad = diff_rad + 2 * M_PI;
  }
  return diff_rad;
}

// x: roll, y: pitch, z: yaw
geometry_msgs::Vector3 getRPY(const geometry_msgs::Pose & pose)
{
  geometry_msgs::Vector3 rpy;
  tf2::Quaternion q(pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w);
  tf2::Matrix3x3(q).getRPY(rpy.x, rpy.y, rpy.z);
  return rpy;
}

geometry_msgs::Vector3 getRPY(const geometry_msgs::PoseStamped & pose) { return getRPY(pose.pose); }

geometry_msgs::TwistStamped calcTwist(
  const geometry_msgs::PoseStamped & pose_a, const geometry_msgs::PoseStamped & pose_b)
{
  const double dt = (pose_b.header.stamp - pose_a.header.stamp).toSec();

  if (dt == 0) {
    geometry_msgs::TwistStamped twist;
    twist.header = pose_b.header;
    return twist;
  }

  const auto pose_a_rpy = getRPY(pose_a);
  const auto pose_b_rpy = getRPY(pose_b);

  geometry_msgs::Vector3 diff_xyz;
  geometry_msgs::Vector3 diff_rpy;

  diff_xyz.x = pose_b.pose.position.x - pose_a.pose.position.x;
  diff_xyz.y = pose_b.pose.position.y - pose_a.pose.position.y;
  diff_xyz.z = pose_b.pose.position.z - pose_a.pose.position.z;
  diff_rpy.x = calcDiffForRadian(pose_b_rpy.x, pose_a_rpy.x);
  diff_rpy.y = calcDiffForRadian(pose_b_rpy.y, pose_a_rpy.y);
  diff_rpy.z = calcDiffForRadian(pose_b_rpy.z, pose_a_rpy.z);

  geometry_msgs::TwistStamped twist;
  twist.header = pose_b.header;
  twist.twist.linear.x =
    std::sqrt(std::pow(diff_xyz.x, 2.0) + std::pow(diff_xyz.y, 2.0) + std::pow(diff_xyz.z, 2.0)) /
    dt;
  twist.twist.linear.y = 0;
  twist.twist.linear.z = 0;
  twist.twist.angular.x = diff_rpy.x / dt;
  twist.twist.angular.y = diff_rpy.y / dt;
  twist.twist.angular.z = diff_rpy.z / dt;

  return twist;
}

void Pose2Twist::callbackPose(const geometry_msgs::PoseStamped::ConstPtr & pose_msg_ptr)
{
  // TODO check time stamp diff
  // TODO check suddenly move
  // TODO apply low pass filter

  geometry_msgs::PoseStamped current_pose_msg = *pose_msg_ptr;
  static geometry_msgs::PoseStamped prev_pose_msg = current_pose_msg;
  geometry_msgs::TwistStamped twist_msg = calcTwist(prev_pose_msg, current_pose_msg);
  prev_pose_msg = current_pose_msg;
  twist_msg.header.frame_id = "base_link";
  twist_pub_.publish(twist_msg);

  std_msgs::Float32 linear_x_msg;
  linear_x_msg.data = twist_msg.twist.linear.x;
  linear_x_pub_.publish(linear_x_msg);

  std_msgs::Float32 angular_z_msg;
  angular_z_msg.data = twist_msg.twist.angular.z;
  angular_z_pub_.publish(angular_z_msg);
}

mian文件：

#include "pose2twist/pose2twist_core.h"

int main(int argc, char ** argv)
{
  ros::init(argc, argv, "pose2twist");
  ros::NodeHandle nh;
  ros::NodeHandle private_nh("~");

  Pose2Twist node(nh, private_nh);

  ros::spin();
  return 0;
}

启动launch:

<launch>

  <arg name="input_pose_topic" default="/localization/pose_estimator/pose" doc="" />
  <arg name="output_twist_topic" default="/estimate_twist" doc="" />

  <node pkg="pose2twist" type="pose2twist" name="pose2twist" output="log">
    <remap from="pose" to="$(arg input_pose_topic)" />
    <remap from="twist" to="$(arg output_twist_topic)" />
  </node>

</launch>