转载自：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

版权

文章目录

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的根路径

在[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>

在[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

拷贝模型文件到px4

cp realsense_ros_gazebo/sdf/* [px4]/Tools/sitl_gazebo/models/ -r

启动

roscd px4/launch
roslaunch iris_realsense_camera_px4_mavros_vo.launch

slam启动

需要将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 grougtruthmsg_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 sentmsg_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 framecamera_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坐标系下。