实现激光塔和雷达的轨迹对比

一、录制bag包

1、使用FAST-LIO2运行mid360

安装ROS驱动，需要把livox_ros_driver2放到已有的ros工程下

（1）下载fast-lio2

（2）运行fast_lio2_ws和livox_mid360_driver_ws

roslaunch fast_lio mapping_avia.launch
roslaunch livox_ros_driver2 msg_MID360.launch 

2、运行px4_command

roscore

source devel/setup.bash 
rosrun px4_command pub_vive_pose

source devel/setup.bash 
rosrun px4_command px4_pos_estimator

 3、录制bag包

终端执行：rosbag record -o(包含时间） xxx.bag 话题名1 话题名2 …如

rosbag record -o zh.bag /mavros/vision_pose/pose /Odometry 

使用ctrl+c终止录制，不然会生存.bag.active后缀的文件，需要进行转换

转换方法：

①切换到xxx.bag.active文件所在的目录下； 
②命令行输入rosbag reindex xxx.bag.active； 
③输入rosbag fix xxx.bag.active result.bag；

二、安装和使用rpg_trajectory_evaluation

1、编译

mkdir catkin_ws
cd catkin_ws
mkdir src
cd src
git clone https://github.com/uzh-rpg/rpg_trajectory_evaluation.git
git clone https://github.com/catkin/catkin_simple.git
cd ..
catkin_make 

2、安装依赖库

pip install numpy
pip install matplotlib
pip install colorama
pip install ruamel.yaml
sudo apt install texlive-fonts-recommended texlive-fonts-extra
sudo apt install dvipng

3、将rosbag转换为需要的文件，通过bag_to_pose.py

python3 /home/zhong/projects/rpg/src/rpg_trajectory_evaluation/scripts/dataset_tools/bag_to_pose.py /home/zhong/Bag/zh.bag --msg_type PoseStamped --output stamped_traj_estimate.txt /mavros/vision_pose/pose 
 
python3 /home/zhong/projects/rpg/src/rpg_trajectory_evaluation/scripts/dataset_tools/bag_to_pose.py /home/zhong/Bag/zh.bag --msg_type PoseWithCovarianceStamped  --output stamped_groundtruth.txt /Odometry

语句：pythonx(对应的版本） 
因为是对比不同话题的轨迹，所以要生成两个txt文件

4、使用analyze_trajectory_single.py进行分析

python3 scripts/analyze_trajectory_single.py /home/zhong/xxx(包含两个txt文件的文件夹名）

三、在rviz中可视化对比轨迹

1、启动rviz：

rviz

2、播放bag包：

指令：rosbag play -loop(循环播放） xxx.bag;

3、在rviz中订阅相应的话题

4、修改参考系：在如图位置可以查看





四、ROS里程计消息nav_msgs/Odometry的可视化方法

此处参照：

ROS里程计消息nav_msgs/Odometry的可视化方法_nav_msgs::odometry_Studying Cui的博客-CSDN博客

可视化的方法为：

①在一个节点中订阅发布的里程计话题消息nav_msgs/Odometry

②创建geometry_msgs::PoseStamped对象接收里程计的位姿

③创建nav_msgs/Path对象作为容器，将赋值后的对象push_back进nav_msgs/Path中并发布

然后即可在rviz中订阅包含nav_msgs/Path的话题并可视化轨迹

1.新建ROS工作空间

mkdir -p path_ws/src
cd path_ws
catkin_make
cd src
catkin_create_pkg path_3d roscpp rospy std_msgs nav_msgs geometry_msgs

2.在path_3d/src中编写消息收发节点文件path_3d.cpp

#include <ros/ros.h>
#include <nav_msgs/Path.h>
#include <std_msgs/String.h>
#include <nav_msgs/Odometry.h>
#include <geometry_msgs/PoseStamped.h>
#include <tf/transform_broadcaster.h>
#include <tf/tf.h>
 

nav_msgs::Path path;
ros::Publisher path_pub;
 

void pathCallback(const nav_msgs::Odometry::ConstPtr& odom_3d)
{
 geometry_msgs::PoseStamped position_3d;
 position_3d.pose.position.x = odom_3d->pose.pose.position.x; 
 position_3d.pose.position.y = odom_3d->pose.pose.position.y; 
 position_3d.pose.position.z = odom_3d->pose.pose.position.z;
 position_3d.pose.orientation = odom_3d->pose.pose.orientation;
 position_3d.header.stamp = odom_3d->header.stamp;
 position_3d.header.frame_id = "map";
 

 path.poses.push_back(position_3d);
 path.header.stamp = position_3d.header.stamp;
 path.header.frame_id = "map";
 path_pub.publish(path);
 
 std::cout << odom_3d -> header.stamp << ' ' << odom_3d->pose.pose.position.x << ' ' << odom_3d->pose.pose.position.y << ' ' << odom_3d->pose.pose.position.z << std::endl;
}
 

int main (int argc, char **argv)
{
 ros::init (argc, argv, "showpath");
 ros::NodeHandle ph;
 

 path_pub = ph.advertise<nav_msgs::Path>("odom3d_path", 10, true);
 ros::Subscriber odomSub = ph.subscribe<nav_msgs::Odometry>("/odometry_3d", 10, pathCallback); //订阅里程计话题信息,其中"/odometry_3d"是自己发布的里程计话题名，别忘了修改
 
 ros::Rate loop_rate(1000);
 while(ros::ok())
 {
 ros::spinOnce();
 loop_rate.sleep();
 }
 return 0;
}

3.path_3d文件夹目录中的CMakeLists.txt如下

cmake_minimum_required(VERSION 2.8.3)
project(path_3d)
 
## Compile as C++11, supported in ROS Kinetic and newer
# add_compile_options(-std=c++11)
 
find_package(catkin REQUIRED COMPONENTS
 geometry_msgs
 roscpp
 rospy
 std_msgs
message_generation
)
 
## Generate added messages and services with any dependencies listed here
 generate_messages(
 DEPENDENCIES
 geometry_msgs std_msgs
 )
catkin_package(
 INCLUDE_DIRS include
 LIBRARIES path_3d
 CATKIN_DEPENDS geometry_msgs roscpp rospy std_msgs
 DEPENDS system_lib
)
 
include_directories(
 include
 ${catkin_INCLUDE_DIRS}
)
 
add_executable(path_3d src/path_3d.cpp) #${PROJECT_NAME}_node
target_link_libraries(path_3d ${catkin_LIBRARIES}) # ${PROJECT_NAME}_node
add_dependencies(path_3d beginner_tutorials_generate_messages_cpp) #path_3d_node

4.在工作空间中编译功能包

// 打开命令行
// 进入工作空间最上层目录
cd path_ws
// 执行一下 source 命令
source devel/setup.bash
// 编译工作空间下的所有功能包
catkin_make
// 单独编译工作空间下的 path_3d 功能包
catkin_make -DCATKIN_WHITELIST_PACKAGES="path_3d"

四、对数据进行补偿

1、对path_3d.cpp文件进行修改

（1）先补偿z轴方向的差值，然后在二维中进行向量的旋转

（2）通过四元数进行旋转：

此处参照：

ROS TF2 中的 四元数 基础部分 - 古月居

a、将一个姿态（用四元数表示） 做一个 旋转（用四元数表示） ，只需要将 姿态的四元数 乘以旋转的四元数，例子：

#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
//q_orig  是原姿态转换的tf的四元数
//q_rot   旋转四元数
//q_new   旋转后的姿态四元数
tf2::Quaternion q_orig, q_rot, q_new;

// commanded_pose.pose.orientation  这个比如说 是 订阅的别的节点的topic 是一个  姿态的 msg 四元数
//通过tf2::convert()  转换成 tf 的四元数
tf2::convert(commanded_pose.pose.orientation , q_orig);

// 设置 绕 x 轴 旋转180度
double r=3.14159, p=0, y=0;  
q_rot.setRPY(r, p, y);//求得 tf 的旋转四元数

q_new = q_rot*q_orig;  // 通过 姿态的四元数 乘以旋转的四元数 即为 旋转 后的  四元数
q_new.normalize(); // 归一化

//  将 旋转后的 tf 四元数 转换 为 msg 四元数
tf2::convert(q_new, commanded_pose.pose.orientation);

b、四元数转置：将w参数加上一个负号

c、求两个姿态（四元数）的旋转角

假如在一个坐标系下有两个 姿态 用四元数 表示的q_1和q_2，那如何求这两个姿态的旋转四元数q_r呢。

q_2 = q_r*q_1

可以类似于求解矩阵方程来求解q_r。颠倒q_1并将两边右乘。同样，乘法的顺序很重要：

q_r = q_2*q_1_inverse

例子：

q1_inv[0] = prev_pose.pose.orientation.x
q1_inv[1] = prev_pose.pose.orientation.y
q1_inv[2] = prev_pose.pose.orientation.z
q1_inv[3] = -prev_pose.pose.orientation.w //注意这个负号

q2[0] = current_pose.pose.orientation.x
q2[1] = current_pose.pose.orientation.y
q2[2] = current_pose.pose.orientation.z
q2[3] = current_pose.pose.orientation.w

qr = tf.transformations.quaternion_multiply(q2, q1_inv)