玩转四旋翼无人机（在rviz中显示飞机)

如何在rviz中显示一个四旋翼?

rviz中可以将订阅的姿态轨迹信息三维显示，这样不需要任何代码就可以三维可视化定位结果，十分方便。但是如何加载一个机器人（四旋翼）的模型呢？
所以这里借鉴hector-quadrotor中的urdf，得到一个飞机的urdf模型。因此可以启动hector-quadrotor-gazebo中的spawn_quad的模型。

roslaunch hector_quadrotor_gazebo spawn_quadrotor.launch
<arg name="model" value="$(find hector_quadrotor_description)/urdf/quadrotor_hokuyo_utm30lx.gazebo.xacro"/>

然后启动rviz，使用配置文件out_flight.rviz即可，该文件在hector-quadrotor-demo中的rviz_cfg文件夹下。
这样我们就可以在rviz显示一个静态的四旋翼。然后想要的这个飞机动起来，需要广播/tf，tf是一个TrnansformStamped消息，类似于PoseStamped。

import rospy
import numpy as np
import tf
from  geometry_msgs.msg  import  TransformStamped
if __name__ == '__main__':
    rospy.init_node('drone_tf_broadcaster')
    br =tf.TransformBroadcaster()
    tfs = TransformStamped()
    rate = rospy.Rate(10.0)  # 10hz
    idx = 0
    while not rospy.is_shutdown():
        tfs.header.stamp = rospy.Time.now()
        tfs.header.frame_id = 'base_link'
        tfs.child_frame_id = 'world'
        idx += 1
        peroid = np.pi*4
        radius = 10
        tfs.transform.translation.x = radius * np.sin(idx/peroid)
        tfs.transform.translation.y = radius * np.cos(idx/peroid)
        tfs.transform.translation.z = 10
        tfs.transform.rotation.w = 1
        tfs.transform.rotation.x = 0
        tfs.transform.rotation.y = 0
        tfs.transform.rotation.z = 0
        br.sendTransformMessage(tfs)

        rate.sleep()

这里按照10Hz的频率广播一个tf，这个message需要指定frame_id和child_frame_id，这里一个是world，一个是base_link。tf的姿态没有改变，水平做圆周运动，竖直方向位置是10。
如果想要在rviz中留下轨迹，可以使用Path，这样就动态显示了飞机运行的轨迹。

#!/usr/bin/env python
import rospy
from nav_msgs.msg import  Path
from geometry_msgs.msg import PoseStamped,TransformStamped
import numpy as np
from copy import deepcopy
import time
import tf
class path(object):
    def __init__(self):
        self.pose_sub = rospy.Subscriber('/mavros/local_position/pose',PoseStamped,self.add_pose_path)
        self.path_pub = rospy.Publisher('/uav_pose/aPath', Path, queue_size=10)
        self.mav_path = Path()
        self.mav_path.header.frame_id = 'fcu'
        self.br =tf.TransformBroadcaster()
        self.tfs = TransformStamped()
    def add_pose_path(self,pose):
        self.mav_path.poses.append(pose)
        self.path_pub.publish(self.mav_path)
        self.br.sendTransform((pose.pose.position.x, pose.pose.position.y, pose.pose.position.z), (pose.pose.orientation.x, pose.pose.orientation.y, pose.pose.orientation.z,pose.pose.orientation.w), rospy.Time.now(), "base_link", "fcu")
        self.br.sendTransformMessage(self.tfs)

if __name__=='__main__':
    rospy.init_node('mav_path')
    mpath = path()
    rospy.spin()

这里首先订阅一个pose message，然后将这个pose message累积成一个path即可。