6_基础建图测试 | Mian的笔记

尝试在无人机上使用octomap进行建图，并于RVIZ中查看结果

仿真测试

深度相机模型

修改之前的启动仿真的launch文件，将其中vehicle修改为iris_depth_camera，启用深度相机模型
可能返回报错ERROR [init] Unknown model iris_depth_camera (not found by name on ~/.ros/etc/init.d-posix/airframes)然后无法启动gazebo。说明PX4与gazebo对于模型名称的理解产生了分歧，即PX4的官方固件中只有iris型号。需要分离“机型”与“外观”，如下

<!-- 【关键修改 1】让 PX4 固件加载它认识的机型 iris --> 
<arg name="vehicle" default="iris"/> 
<!-- 【关键修改 2】让 Gazebo 加载带相机的模型文件 --> 
<arg name="sdf" default="~/UAV/PX4-Autopilot/Tools/sitl_gazebo/models/iris_depth_camera/iris_depth_camera.sdf"/>

点云

在gazebo由深度相机插件生成，从深度图像转换到点云。当选择使用深度相机模型将默认启动转换节点，从/camera/depth/image_raw转换得到/camera/depth/points。
而在实机上上使用时，具体内容可以参考 [[工程知识/占据栅格图 Octomap]]

占据栅格图

同样参考上述内容，使用octomap包（八叉树）来实现占据栅格图，可以通过launch文件来启动。其接受点云图作为输入，使用参数
<remap from="cloud_in" to="/camera/depth/color/points" />，然后查询tf树进行坐标变换，输出占据栅格图。

RVIZ显示

在本实验中，直接在仿真中启动mavros_posix_sitl.launch，然后启动octomap_server，再打开rviz，添加pointcloud2，并订阅点云话题尝试观察点云。结果status显示error，提示frame link无法回到map。即坐标转换出现了断点，需要检查tf树，rosrun tf view_frames。

发现odom、map、baselink三者的tf树之间没有连接，于是尝试修复。首先是baselink与cameralink，两者相对位置是固定的，于是采用静态转换，直接写在启动octomap的launch文件中。

1 2	<node pkg="tf" type="static_transform_publisher" name="base_to_camera" args="0.1 0 0 -1.57 0 -1.57 base_link camera_link 100" />

而此处仿真中由于没有采用里程计，故odom可以认为是完美值，与map相等

1	<node pkg="tf" type="static_transform_publisher" name="map_to_odom" args="0 0 0 0 0 0 map odom 100" />

而baselink到odom则是一个动态的变换，虽然也可以采用上面相同的静态转换，但是这样相当于假设无人机在仿真中是不能运动的，否则octomap的累计地图会出问题。

这里可以直接采用/mavros/local_position/pose的值作为转换值，但是为了节省带宽和cpu资源，MAVROS默认将local_position/tf/send设置为false，这样算出的位置信息只会以话题发布，而不会作为坐标变换广播。
而由于是使用mavros_posix_sitl启动的mavros，所以无法直接修改参数，而又由于mavros使用的是私有参数，故也无法添加全局参数，所以直接修改默认配置文件/opt/ros/noetic/share/mavros/launch/px4_config.yaml。找到其中local_position部分，首先将frame_id修改为odom，其次将send改为true。

修改后启动仿真与octomap，查看tf树结构发现三者都连接在一棵树了，然后启动rviz发现能成功显示点云图与占据栅格图，且手动旋转无人机，占据栅格图能够正常运行

TF树

tf树，即transformer坐标系变换树，是ROS机器人的一个特点。在ROS机器人模型中存在大量的部件，称为link，每一个link都有自己的坐标系，称为frame。而tf树就是一个树状结构，维护坐标系之间的关系，从而实现系统中任意一个点在所有坐标系中的转换。而为了高效，ROS采用树的结构来表示这个坐标系转换。

以本实验为例，存在map，odom，base link，camera link几个坐标系，分别为世界坐标系（以某个固定点为原点），无人机起始点为原点，飞控中心为原点，摄像机中心为原点

odom坐标系也是一种世界坐标系，但是由无人机自己给出坐标，例如无人机绕了一个圈回到原点，那么在map下坐标回到原点，但是无人机可能估计自己没有回到原点。

而tf树中有两种frame之间的连接，分别为动态转换与静态转换。例如camera link到base link，两个部件之间是物理固定的，那么可以在launch文件中使用tf程序包来构建frame之间的连接；而baselink到odom之间则是动态的转换关系，需要不断发布消息。

TF程序包

rosrun tf tf_viewer在命令行查看tf树结构
rosrun tf view_frames生成一张当前tf树的pdf图像

构建静态tf连接，在launch文件中

1 2	<node pkg="tf" type="static_transform_publisher" name="base_to_camera" args="0.1 0 0 -1.57 0 -1.57 base_link camera_link 100" />

发布动态tf连接，在节点中

#include <tf/transform_broadcaster.h>

// 1. 创建广播员对象
static tf::TransformBroadcaster br;
tf::Transform transform;

// 2. 将收到的 Pose 赋值给 Transform
transform.setOrigin( tf::Vector3(msg.x, msg.y, msg.z) );
transform.setRotation( tf::Quaternion(msg.qx, msg.qy, msg.qz, msg.qw) );

// 3. 广播出去（这一步会让 /tf 话题产生数据）
br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "odom", "base_link"));

仿真部分

简单的offb+建图没有在执行上没有问题，但是细节上存在问题。一是能否消除地面，二是随着无人机的起飞与降落，地面的位置发生了变换

而square+建图之前出现了无人机速度异常提醒，而且好像都没有飞起来就提醒了，需要检查。

实机测试

实机测试时不止需要启动相机，故按照之前启动里程计的方式启动，但是需要进行一定的修改。

roslaunch mavros px4.launch \
    gcs_url:=udp://@192.168.1.1 \
    local_position/tf/send:=false \
    global_position/tf/send:=false &
    
roslaunch vins d435_run.launch \
    enable_depth:=true \
    filters:=pointcloud,temporal,spatial,decimation \
    enable_pointcloud:=true \
    enable_color:=false &

TF树与命名冲突

vins的代码默认使用的坐标系名称与mavros的坐标系名称不同，vins使用world ，body，camera而mavros使用map，base_link,camera_link。建议去vins_estimator/src/utility/visualization.cpp中修改，否则tf树中会存在两套不连接的体系。

纯深度点云生成

相机默认生成的点云为彩色点云，而实机octomap建图时并不会使用彩色信息，故可以使用深度图像以节约CPU计算量。但是要注意，无论是否使用深度信息，相机生成的点云话题始终是/cmaera/depth/color/points.

修改相机的启动文件参数


<include file="$(find realsense2_camera)/launch/rs_camera.launch">
	<arg name="enable_color" value="false"/>
	
	<arg name="allow_no_texture_points" value="true"/>
	<arg name="pointcloud_texture_stream" value="RS2_STREAM_ANY"/>
	
	<arg name="filters" value="pointcloud,temporal,spatial,decimation"/>

在rviz中查看点云图像是否还有颜色来检验。注意修改后查看tf树，其中color_frame将消失

rviz坐标轴旋转

在rviz中查看点云图，发现图像逆时针旋转了90度；而里程计的读数在各方向上又正常增长。原因为vins会发布一次旋转，而realsense驱动也会发布一次旋转。
故修改相机配置文件关闭相机的旋转即可，设置参数

1	<arg name="publish_tf" value="false">

注意在关闭了相机发布tf信息后，tf树会失去camera_link以下所有frame，而为了在rviz中正常查看点云图像，需要手动添加optical_frame的静态变换

修改mapping.launch

<!-- 1. 连接 camera_link 到 camera_depth_frame -->
  <node pkg="tf" type="static_transform_publisher" name="link_to_depth_frame" 
      args="0 0 0 0 0 0 1 camera_link camera_depth_frame 100" />

  <!-- 2. 连接 camera_depth_frame 到 camera_depth_optical_frame -->
  <node pkg="tf" type="static_transform_publisher" name="depth_to_optical_identity" 
      args="0 0 0 0 0 0 1 camera_depth_frame camera_depth_optical_frame 100" />