XTDrone+VINs+fast-planner

接下来的工作需要把XTDrone,VINS和fast-planner集成到一起。
在XTDrone集成VINs按照XTDrone使用手册来就可以了，按照仿真平台基础配置，PX4飞控与EKF配置和视觉惯性里程计（VIO）这三节配置就可以了。
接下来是配置fast-planner，首先将fast-planner源码clone下来，要改的地方也不多，主要就是第一：要把fastplanner接受到的里程计，深度相机的位姿，深度相机的点云信息换成XTDrone环境下发布出来的话题，具体见下面。

  <arg name="odom_topic" default="vins_estimator/odometry" />
  <arg name="camera_pose_topic" default="/iris_0/camera_pose"/>
  <arg name="depth_topic" default="/iris_0/realsense/depth_camera/depth/image_raw"/>

然后将fast-planner发布出来的指令话题remap成XTDrone需要的话题名字，我找的时候没找到fast-planner发出来的XTDrone能用的，所以就又写了一个话题发布，在traj_server.cpp这个文件里，具体就是下面的代码，具体代码是参考ego-planner写的

ros::Publisher pose_cmd_pub;
geometry_msgs::Pose pose_cmd;

  pose_cmd.position.x = pos(0);
  pose_cmd.position.y = pos(1);
  pose_cmd.position.z = pos(2);

  pose_cmd.orientation.x = 0.0; 
  pose_cmd.orientation.y = 0.0;
  pose_cmd.orientation.z = sin(yaw_yawdot.first/2);
  pose_cmd.orientation.w = cos(yaw_yawdot.first/2);
  //发送一个geometry_msgs::Pose类型的消息
  pose_cmd_pub.publish(pose_cmd);
  
  pose_cmd_pub = nh.advertise<geometry_msgs::Pose>("/pose_cmd", 50);

然后在launch文件里remap一下就可以了。

  <!-- trajectory server -->
  <node pkg="plan_manage" name="traj_server" type="traj_server" output="screen">
    <remap from="pose_cmd" to="/xtdrone/iris_0/cmd_pose_enu"/>
    <remap from="odom_world" to="$(arg odom_topic)"/>
    <param name="traj_server/time_forward" value="1.0" type="double"/>
  </node>

这个时候我发现地图显示不出来，可能是坐标系转换的问题，我参考了官方给的ego-planner文档，就是下面这个文件。

import rospy
from geometry_msgs.msg import PoseStamped
import math
from pyquaternion import Quaternion
import tf
import sys

vehicle_type = sys.argv[1]
vehicle_id = sys.argv[2]
local_pose = PoseStamped()
local_pose.header.frame_id = 'world'
quaternion = tf.transformations.quaternion_from_euler(-math.pi/2, 0, -math.pi/2)
q = Quaternion([quaternion[3],quaternion[0],quaternion[1],quaternion[2]])

def vision_callback(data):    
    local_pose.pose.position.x = data.pose.position.x
    local_pose.pose.position.y = data.pose.position.y
    local_pose.pose.position.z = data.pose.position.z
    q_= Quaternion([data.pose.orientation.w,data.pose.orientation.x,data.pose.orientation.y,data.pose.orientation.z])
    q_ = q_*q
    local_pose.pose.orientation.w = q_[0]
    local_pose.pose.orientation.x = q_[1]
    local_pose.pose.orientation.y = q_[2]
    local_pose.pose.orientation.z = q_[3]
    
rospy.init_node(vehicle_type+"_"+vehicle_id+'_fast_transfer')
rospy.Subscriber(vehicle_type+"_"+vehicle_id+"/mavros/vision_pose/pose", PoseStamped, vision_callback,queue_size=1)
position_pub = rospy.Publisher(vehicle_type+"_"+vehicle_id+"/camera_pose", PoseStamped, queue_size=1)
rate = rospy.Rate(60) 

while not rospy.is_shutdown():
    local_pose.header.stamp = rospy.Time.now()
    position_pub.publish(local_pose) 
    rate.sleep()

最终的结果图是这样的：

fastplanner