文章目录

realsense D435i gazebo slam仿真

包含realsense T265 D435i的urdf和sdf文件、realsense_gazebo_plugin包及realsense 模型文件使用示例。

下载realsense 仿真模型

[catkin_ws]表示自定义的工作目录

mkdir -p [catkin_ws]/src
cd [catkin_ws]/src
git clone https://gitee.com/nie_xun/realsense_ros_gazebo.git
cd [catkin_ws]
catkin_make
source devel/setup.sh

运行D435仿真环境测试

D435

roslaunch realsense_ros_gazebo simulation_sdf.launch
运行结果：
在这里插入图片描述

D435i

相比D435多一个camera/imu topic
roslaunch realsense_ros_gazebo simulation_D435i_sdf.launch
在这里插入图片描述

slam仿真示例

UAV模型启动

本示例使用px4的iris 无人机模型作为示例，本文使用的Firmware为v1.8版本。
在realsense_ros_gazebo的sdf文件夹下已存放携带D435i的iris sdf文件。
以下[px4]为px4 对应的Firmware的根路径
在这里插入图片描述

  1. 在[px4]/launch下生成文件iris_realsense_camera_px4_mavros_vo.launch，内容如下：
<?xml version="1.0"?>
<launch>
    <arg name="x" default="0"/>
    <arg name="y" default="0"/>
    <arg name="z" default="0"/>
    <arg name="R" default="0"/>
    <arg name="P" default="0"/>
    <arg name="Y" default="0"/>
    <!-- vehicle model and world -->
    <arg name="est" default="ekf2"/>
    <arg name="vehicle" default="iris"/>
    <arg name="world" default="$(find px4)/Tools/sitl_gazebo/worlds/empty.world"/>
    <arg name="sdf" default="$(find px4)/Tools/sitl_gazebo/models/iris_realsense_camera/iris_realsense_camera.sdf"/>
    <arg name="rcS" default="$(find px4)/posix-configs/SITL/init/$(arg est)/$(arg vehicle)_vo"/>
    <!-- gazebo configs -->
    <arg name="gui" default="true"/>
    <arg name="debug" default="false"/>
    <arg name="verbose" default="false"/>
    <arg name="paused" default="false"/>
    <arg name="respawn_gazebo" default="false"/>
    <!-- MAVROS configs -->
    <arg name="fcu_url" default="udp://:14540@localhost:14557"/>
    <arg name="respawn_mavros" default="false"/>
    <!-- PX4 configs -->
    <arg name="interactive" default="true"/>
    <!-- PX4 SITL and Gazebo -->
    <include file="$(find px4)/launch/posix_sitl.launch">
        <arg name="x" value="$(arg x)"/>
        <arg name="y" value="$(arg y)"/>
        <arg name="z" value="$(arg z)"/>
        <arg name="R" value="$(arg R)"/>
        <arg name="P" value="$(arg P)"/>
        <arg name="Y" value="$(arg Y)"/>
        <arg name="world" value="$(arg world)"/>
        <arg name="vehicle" value="$(arg vehicle)"/>
        <arg name="sdf" value="$(arg sdf)"/>
        <arg name="rcS" value="$(arg rcS)"/>
        <arg name="gui" value="$(arg gui)"/>
        <arg name="interactive" value="$(arg interactive)"/>
        <arg name="debug" value="$(arg debug)"/>
        <arg name="verbose" value="$(arg verbose)"/>
        <arg name="paused" value="$(arg paused)"/>
        <arg name="respawn_gazebo" value="$(arg respawn_gazebo)"/>
    </include>
    <!-- MAVROS -->
    <include file="$(find mavros)/launch/px4.launch">
        <arg name="gcs_url" value=""/>
        <arg name="fcu_url" value="$(arg fcu_url)"/>
        <arg name="respawn_mavros" value="$(arg respawn_mavros)"/>
    </include>
</launch>
  1. 在[px4]//posix-configs/SITL/init/ekf2下生成iris_vo，操作如下：
cd [px4]//posix-configs/SITL/init/ekf2
cp iris iris_vo

将EKF2_AID_MASK和EKF2_HGT_MODE对应项改成：

param set EKF2_AID_MASK 24
param set EKF2_HGT_MODE 3

在这里插入图片描述

  1. 拷贝模型文件到px4
cp realsense_ros_gazebo/sdf/* [px4]/Tools/sitl_gazebo/models/ -r
  1. 启动
roscd px4/launch
roslaunch iris_realsense_camera_px4_mavros_vo.launch

在这里插入图片描述

slam启动

  1. 需要将slam的odom输出remap到/mavros/vision_pose/pose
    1）如果slam输出使用的odom消息类型为geometry_msgs::PoseStamped，且坐标系为(NWU: x:前；y:左；z:上)则直接在slam的launch文件中发布odom消息的node下加入以下内容：
<remap from="/camera/odom" to="/mavros/vision_pose/pose" />

2）本文使用的slam坐标系为WUN，发布的odom消息类型为/camera/odometry
使用的转换代码文件如下：
nav_msg_to_mavros.cpp

#include <iostream>
#include <ros/ros.h>
#include <geometry_msgs/Pose.h>
#include <geometry_msgs/PoseStamped.h>
#include <nav_msgs/Odometry.h>

ros::Publisher camera_pose_publisher;

int data_source;
enum {
  GAZEBO_GT = 0,
  CAMERA_VO = 1 
};

void vision_cb(const nav_msgs::Odometry::ConstPtr &msg)
{
    geometry_msgs::PoseStamped msg_body_pose;

    if (data_source == GAZEBO_GT) {
        // 0 means use gazebo grougtruth
        msg_body_pose.header.stamp = msg->header.stamp;
        msg_body_pose.header.frame_id = "world";
        msg_body_pose.pose.position.x = msg->pose.pose.position.x;
        msg_body_pose.pose.position.y = msg->pose.pose.position.y;
        msg_body_pose.pose.position.z = msg->pose.pose.position.z;
        msg_body_pose.pose.orientation.x = msg->pose.pose.orientation.x;
        msg_body_pose.pose.orientation.y = msg->pose.pose.orientation.y;
        msg_body_pose.pose.orientation.z = msg->pose.pose.orientation.z;
        msg_body_pose.pose.orientation.w = msg->pose.pose.orientation.w;
    } else if (data_source == CAMERA_VO) {
        // 1 means use camera vo
        // Create PoseStamped message to be sent
        msg_body_pose.header.stamp = msg->header.stamp;
        msg_body_pose.header.frame_id = "world";
        msg_body_pose.pose.position.x = msg->pose.pose.position.z;
        msg_body_pose.pose.position.y = msg->pose.pose.position.x;
        msg_body_pose.pose.position.z = msg->pose.pose.position.y;
        msg_body_pose.pose.orientation.x = msg->pose.pose.orientation.z;
        msg_body_pose.pose.orientation.y = msg->pose.pose.orientation.x;
        msg_body_pose.pose.orientation.z = msg->pose.pose.orientation.y;
        msg_body_pose.pose.orientation.w = msg->pose.pose.orientation.w;
    }   

    // Publish pose of body frame in world frame
    camera_pose_publisher.publish(msg_body_pose);
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "vision_to_mavros");
    ros::NodeHandle nh("~");
    nh.param<int>("data_source", data_source, 0);

    ros::Subscriber gazebo_sub = nh.subscribe<nav_msgs::Odometry>("/camera/odometry", 50, vision_cb);
    camera_pose_publisher = nh.advertise<geometry_msgs::PoseStamped>("/mavros/vision_pose/pose", 50);

    ros::spin();

}

2.启动想使用的slam

结果示例：
在这里插入图片描述

其他

mavros px4 坐标转换

mavros：enu
px4：ned

在外部传感器定位，非GPS自身定位，世界坐标系由外部传感器定义。
如optirack，使用自身的定义的原点坐标系，由定义原点时，使用的定位杆长短轴及motive软件的Y/Z up综合定义。

Y up: 前(Z+), 左(X+), 上(Y+) 。
Z up: 在Y up的坐标系基础上绕x轴旋转-90度。Z up即使用的enu的坐标系。

坐标系的定位也有相对之分，比如optirack相对于GPS坐标系就是相对坐标系，GPS作为地球坐标系(更大的坐标系)则作为绝对坐标系，当使用相机定位做对比实验，则又能将optirack看做绝对坐标系，相机坐标系作为相对坐标系。

f:first l:left b:back u:up

因此像enu end这种坐标系更多的是说明xyz三轴的相对关系，比如(flu, lbu)都是一种enu坐标系，更多的是说明机体坐标系来用的。如下图所示：通过旋转后的坐标系，满足三轴规则，还是enu坐标系。
在这里插入图片描述

当optirack vo等相对局部的坐标系只要满足xyz的右手关系，即，食指指向x，对应中指就指向y, 大拇指就是z方向。则就满足enu坐标系。

当无人机以固件启动时作为原点，偏航初始化为0，当无人机以外部传感器作为定位及yaw的输入时(roll pitch由无人机自身imu获得)，需要对准机头方向，使外部传感器得到的偏航和机体得到的偏航一致。

例子1：外部定位使用optirack，Zup(enu), 将初始化时将机头于optirack的x轴对齐，则直接将/vrpn_client_node/<rigid_body_name>/poseremap到mavros/vision_pose/pose即可。
使用mavros/vision_pose发送，mavros采用的enu坐标系，mavros会自动两enu转换到ned坐标系，则初始化时将机头于optirack的x轴对齐，从而能够使px4初始化时，px4解算偏航与optirack解算的机体偏航一致，约为0。或者不对齐时，需减去optirack的偏航测量值再赋值给mavros/vision_pose

例子2：外部定位使用vo，相机前向与无人机绑定，坐标系为(LUF)，将vo输出转换到ENU坐标系赋值给mavros/vision_pose/pose，则对应的转换：类似将(LUF->FLU)

poseX = voZ
poseY = voX
poseZ = voY

此时，直接将机头与mavros的enu的初始化x轴对齐了，即将相机的前向(机头方向)作为mavros的x轴。

总结：无论是vo还是optitrack，要保证的是初始化时，机头要对准赋值给mavros/vision_pose的x。mavros/vision_pose在ENU坐标系下。

附vins 的配置文件
vins-fusion，仅作参考，实际应根据realsense_ros_gazebo/sdf/D435i/model.sdf计算得。
本人只用了单目、imu及深度图，因此没有去解算双目的变换矩阵。以下仅供参考：建议将estimate_extrinsic设为1。

realsense_stereo_imu_config.yaml

imu_topic: "/camera/imu"
image0_topic: "/camera/infra1/image_raw"
image1_topic: "/camera/infra2/image_raw"
output_path: "/home/xxx/output"

cam0_calib: "left.yaml"
cam1_calib: "right.yaml"
image_width: 640 
image_height: 480 
   
# Extrinsic parameter between IMU and Camera.
estimate_extrinsic: 1   # 0  Have an accurate extrinsic parameters. We will trust the following imu^R_cam, imu^T_cam, don't change it.
                        # 1  Have an initial guess about extrinsic parameters. We will optimize around your initial guess.

body_T_cam0: !!opencv-matrix
   rows: 4
   cols: 4
   dt: d
   data: [ -5.7586305857286746e-03, -4.0463318787729019e-03,
       9.9997523237933461e-01, 2.0329267950355900e-02,
       -9.9998287214160420e-01, -1.0224590553211677e-03,
       -5.7628118925283633e-03, 7.9325209639615653e-03,
       1.0457519809151661e-03, -9.9999129084997906e-01,
       -4.0403746097850135e-03, 2.8559824645148020e-03, 0., 0., 0., 1. ]

body_T_cam1: !!opencv-matrix
   rows: 4
   cols: 4
   dt: d
   data: [ -1.0021770212322867e-03, 3.6313480322730518e-04,
       9.9999943188700535e-01, 1.5285779565991807e-02,
       -9.9999216342926500e-01, -3.8303422615924010e-03,
       -1.0007788055728661e-03, -5.2435791444330505e-02,
       3.8299766679101843e-03, -9.9999259827824449e-01,
       3.6697063849344680e-04, 8.6931302450199057e-03, 0., 0., 0., 1. ]

left.yaml/right.yaml

model_type: PINHOLE
camera_name: camera
image_width: 640 
image_height: 480 
distortion_parameters:
   k1: 0.0 
   k2: 0.0 
   p1: 0.0 
   p2: 0.0 
projection_parameters:
   fx: 383.6
   fy: 383.3
   cx: 317.0
   cy: 240.0
# 自动驾驶
Logo
GitCode 开源社区

旨在为数千万中国开发者提供一个无缝且高效的云端环境，以支持学习、使用和贡献开源项目。

更多推荐

【目标检测】目标检测的一些常用神经网络模型及方法

我的阶段性总结????文章目录1.概述1.2 目标检测的任务1.3 目标检测的分类2.R-CNN系列2.1 [R-CNN（Region with CNN features）](https://arxiv.org/pdf/1311.2524.pdf)2.2 [Fast R-CNN](https://www.cv-foundation.org/openaccess/content_iccv_2015/

avatar GitCode 开源社区
cover

用C++实现4种YOLO目标检测（内附完整代码）

avatar GitCode 开源社区
cover

静态时序分析：SDC约束命令create_generated_clock详解（上）

avatar GitCode 开源社区