程序员经验分享-hwhale

realsense D435i gazebo slam(px4)仿真（转载）

2023-05-16

转载自：https://blog.csdn.net/weixin_41469272/article/details/117919845?utm_medium=distribute.pc_relevant_t0.none-task-blog-2%7Edefault%7ECTRLIST%7Edefault-1.essearch_pc_relevant&depth_1-utm_source=distribute.pc_relevant_t0.none-task-blog-2%7Edefault%7ECTRLIST%7Edefault-1.essearch_pc_relevant

realsense D435i gazebo slam(px4)仿真

Nie_Xun 2021-06-15 11:27:31

306

收藏 1

版权

文章目录

    • realsense D435i gazebo slam仿真
      • 下载realsense 仿真模型
      • 运行D435仿真环境测试
        • D435
        • D435i
    • slam仿真示例
      • UAV模型启动
      • slam启动
      • 其他
        • mavros px4 坐标转换

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坐标系下。

本文内容由网友自发贡献，版权归原作者所有，本站不承担相应法律责任。如您发现有涉嫌抄袭侵权的内容，请联系:hwhale#tublm.com(使用前将#替换为@)

Realsense

D435i

Gazebo

Slam

PX4

realsense D435i gazebo slam(px4)仿真（转载） 的相关文章

  • 三维刚体变换

    欢迎访问我的博客首页 三维刚体变换 1 坐标系 1 1 空间坐标系 1 2 右手坐标系与像素坐标系 2 旋转与平移 2 1 推导旋转 2 2 推导平移 2 3 推导变换 2 4 刚体变换 2 5 坐标系旋转与向量旋转 3 链式变换 4 Ei

  • 激光SLAM直接线性方法里程计运动模型及标定

    原创作者 W Tortoise 原创作者文章 https blog csdn net learning tortosie article details 107763626 1 里程计运动模型 1 1 两轮差分底盘的运动模型 1 2 三轮全

  • SLAM评估工具evo的使用

    evo官方指南 参考博客 lt 官方手册 这篇参考博客 完全可以掌握evo的基本操作 gt Then 实践出真知 1 安装evo sudo apt install python pip pip install evo upgrade no

  • 对最小二乘法的一点理解 - slam学习笔记

    我对最小二乘法的理解 在给定参数个数和函数模型之后 根据测试数据 找出与所有测试数据的偏差的平方和最小的参数 这里面应该有两个问题 1 为什么选取与真实数据平方和最小的拟合函数 2 如何求参数 为什么选取与真实数据平方和最小的拟合函数 极大

  • 单目视觉里程记代码

    在Github上发现了一个简单的单目vo 有接近500星 链接如下 https github com avisingh599 mono vo 这个单目里程计主要依靠opencv实现 提取fast角点并进行光流跟踪 然后求取本质矩阵并恢复两帧

  • vscode配置eigen3

    目录 1 头文件包含 2 c cpp properties json 3 CMakeList txt 4 完整代码 1 头文件包含 Eigen 核心部分 include

  • 激光SLAM7-基于已知位姿的构图算法

    1 通过覆盖栅格建图算法进行栅格地图的构建 1 1 Theory 1 2 code 这里没有判断idx和hitPtIndex是否有效 start of TODO 对对应的map的cell信息进行更新 1 2 3题内容 GridIndex h

  • 关于GPS、惯导、视觉里程计的几个定义

    1 首先写几个定义 惯性导航系统 Inertial Navigation System INS 全球定位卫星系统 Global Navigation Satellite System GNSS GNSS 包括全球定位系统 Global Po

  • Sophus安装踩坑

    装SLAM十四讲第二版提供的Sophus Eigen版本3 4 0 报错 home ch 下载 Sophus 13fb3288311485dc94e3226b69c9b59cd06ff94e test core test so2 cpp 9

  • 动态场景下基于实例分割的SLAM（毕业设计开题及语义分割部分）

    动态场景下基于实例分割的SLAM 毕业论文设计思路及流水 前言 今年选了个比较难的毕设题目 这里记录一下自己思路和流程 为之后的学弟学妹 划掉 铺个方向 会按日期不定期的更新 一 开题 2019 12 24 考研前选择课题是 利用深度学习对

  • lego-LOAM跑自己的数据包无法显示全局点云地图解决（速腾聚创RS-LiDAR-16 雷达 ）---SLAM不学无术小问题

    LeGo LOAM跑自己的数据包无法显示全局地图问题 注意 本文笔者使用环境 Ubuntu18 04 ROS melodic 版本 背景 3D SLAM新手 在看到了各种狂拽炫酷的3D点云图的之后决定亲自上手一试 首先当然的是最为经典的LO

  • Ubuntu20.04安装各种库----简洁版

    目录 Eigen3 Sophus Pangolin Ceres g2o 建议先装anaconda再装ros python opencv啥该有的都有了 下面仅仅安装ros没有的库 Eigen3 作用 线性代数开源库 提供了有关线性代数 矩阵和

  • 1-如何安装ROS

    如何安装ROS 大家好 我是如何 今天尝试在Ubantu下安装ROS Robot Operating System 测试环境 虚拟机VMware Ubantu20 04 准备步骤 添加ROS软件源 sudo sh c echo deb ht

  • 用Eigen库练习代数运算方式以便后续对刚体旋转和移动做基础

    include

  • 快看！那个学vSLAM的上吊了！ —— （一）综述

    不同于之前发布的文章 我将使用一种全新的方式 iPad Notability Blog的方式打开这个板块的大门 原因有两个 1 Notability更方便手写长公式 也方便手绘坐标系变换等等 2 之前Apple Pencil找不到了新破费买

  • LeGO-LOAM中的数学公式推导

    LeGO LOAM是一种在LOAM之上进行改进的激光雷达建图方法 建图效果比LOAM要好 但是建图较为稀疏 计算量也更小了 本文原地址 wykxwyc的博客 github注释后LeGO LOAM源码 LeGO LOAM NOTED 关于代码

  • BLAM跑自己的数据包无法显示全局点云地图解决（速腾聚创RS-LiDAR-16 雷达 ）-SLAM不学无术小问题

    BLAM算法跑自己的数据包无法显示全局点云地图解决 适配速腾聚创RS LiDAR 16 雷达 提示 本文笔者使用环境Ubuntu18 04 ROS melodic版本 首先放一个效果链接 由b站up VladimirDuan上传 非官方 官

  • ORB_SLAM2运行官方数据集/自己数据集

    官方数据集运行结果 WeChat 20230210194425 可以正常运行 自己数据集运行结果 自己的数据集 主要是用手机摄像头采集的实验室进行了一下简单的运行 可以成功运行 但是由于查看的相关程序的是死循环不能像运行官方数据集那样完整保

  • 什么是深度学习的无监督学习与有监督学习

    无监督学习 深度学习中的无监督学习方法是一种训练算法 它在没有标注输出的情况下从输入数据中学习模式和特征 这种方法的核心是探索和理解数据的内在结构和分布 而不是通过已知的输出来指导学习过程 无监督学习在深度学习领域有许多不同的形式和应用 以

  • gazebo(fortress) set the path of sdf file

    This method only satisfied with gazebo fortress not harmenic

随机推荐

热门标签