接下来的工作需要把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