坐标转换：将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_frametwist_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 stoppedif (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 codegeometry_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 onlytwist_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 valueconst 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 filtergeometry_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>

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

mavros 基于体轴坐标系下的无人机行人跟踪
该任务是为了实现无人机对于行人的跟踪,飞控采用的是px4,机载的板子是使用jetson nano.利用darknet_ros和自己训练的行人检测的yolov3 tiny模型,在jetson nano板 ...
kitti数据集_KITTI数据集激光雷达坐标系下的里程计真值
由KITTI数据集Odometry模块的devkit_odometrydevkitreadme.txt所述,KITII数据集提供里程计的真值是在左相机表坐标系下的,并没有提供激光雷达坐标系下的真值.因 ...
dq坐标系下无功功率表达式_一种dq旋转坐标系下谐波电流计算方法与流程
本发明涉及一种谐波电流计算方法,具体涉及一种dq旋转坐标系下谐波电流计算方法. 背景技术: 自20世纪七十年代起,电网中的晶闸管.二极管整流器.变频器.电气化铁路及各种电力电子设备用量不断增加,这些设 ...
平行坐标系下采用CHT方法检测自然图像中的消失点(VanishingPoint)
--------------------20210826更新-------------------- code和paper链接:https://pan.baidu.com/s/13RyRu0rg7Fh ...
不同坐标系下角速度_技术 | 西安80坐标与地方坐标系的转换方法技巧
提示:点击上方"壹伴编辑器"↑免费订阅本刊 1.在实施GIS项目中,我们常常会遇到坐标系转换的问题,对于地方坐标系,如果没有提供转换参数,经常需要用控制点来进行坐标几何纠正,下面 ...
neu坐标系和xyz坐标系转换_ArcGIS投影坐标系下坐标转换成地理坐标系经纬度
一.背景最近由于项目原因,需要用ArcGIS进行坐标转换由于提供的管网位置数据是坐标系是:"CGCS2000_3_Degree_GK_CM_117E "下excel表格数据,处 ...
不同坐标系下角速度_星空的经纬线：浅谈赤道坐标系
本文是"天狼观星者"第76篇原创文章经纬度是非常重要的地理标尺.我们可以利用经度和纬度,来描述地球上任何地点的位置.卫星定位.测绘勘探.航空航海等现代科技,都离不开经纬度的帮助. ...
不同坐标系下角速度_「分析」驱动桥传动系典型工况下的一体化动力学仿真
驱动桥是汽车总成中重要的组成部分,主要由主减速器总成.差速器.半轴和驱动桥壳等部分构成,其主要作用是将由传动轴传导来的转矩和力通过半轴分配到左右车轮,实现降低速度增大转矩的作用,同时改变转矩方向:当车 ...
不同坐标系下角速度_坐标系统及常见坐标系
测绘学习点击上方蓝字关注小白 01 坐标系统 1 坐标系统分类 2 2.椭球相关定义 (1)地球椭球--代表地球形状和大小的数学曲面,一般只旋转椭球.参考椭球--有确定椭球参数,经局部定位和定向,并同 ...

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

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

最新文章

热门文章