（3-4-03）速度控制与动态响应优化：无人机速度控制器 - 详解
3.3.3 无人机速度控制器
接下来的实例是一个开源项目，旨在通过 ROS2和PX4 控制模拟无人机的速度。本实例为初学者提供了一个简单易懂的入门示例，通过键盘控制无人机的速度和方向，同时集成了Gazebo和RVIZ用于可视化。
实例3-6：基于ROS 2的无人机速度控制器（源码路径：codes\3\ROS2_PX4_Offboard）
1. 配置文件
文件package.xml是ROS 2包的元数据文件，它定义了包的基本信息和构建依赖。对于px4_offboard 包，它提供了无人机速度控制的 ROS2接口。该包依赖于px4_msgs消息包来与PX4无人机通信，并在构建时使用ament_python作为构建类型。此外，它还指定了一些执行时依赖和测试依赖，包括ros2launch以及一些用于代码质量和版权测试的工具。
px4_offboard
0.0.0
ROS2 PX4 Interface Velocity Control
braden
BSD-3
px4_msgs
ros2launch
ament_copyright
ament_flake8
ament_pep257
python3-pytest
ament_python
文件offboard_velocity_control.launch.py
from launch import LaunchDescription
from launch.actions import ExecuteProcess
from launch_ros.actions import Node
from ament_index_python.packages import get_package_share_directory
import os
def generate_launch_description():
package_dir = get_package_share_directory('px4_offboard')
# bash_script_path = os.path.join(package_dir, 'scripts', 'TerminatorScript.sh')
return LaunchDescription([
# ExecuteProcess(cmd=['bash', bash_script_path], output='screen'),
Node(
package='px4_offboard',
namespace='px4_offboard',
executable='visualizer',
name='visualizer'
),
Node(
package='px4_offboard',
namespace='px4_offboard',
executable='processes',
name='processes',
prefix='gnome-terminal --'
),
Node(
package='px4_offboard',
namespace='px4_offboard',
executable='control',
name='control',
prefix='gnome-terminal --',
),
Node(
package='px4_offboard',
namespace='px4_offboard',
executable='velocity_control',
name='velocity'
),
Node(
package='rviz2',
namespace='',
executable='rviz2',
name='rviz2',
arguments=['-d', [os.path.join(package_dir, 'visualize.rviz')]]
)
])2. 启动文件
文件offboard_velocity_control.launch.py是一个ROS 2启动文件，用于初始化和运行px4_offboard包中的各个节点以及RViz可视化工具。该启动文件定义了多个节点的启动配置，包括一个可视化节点（visualizer）、一个处理进程节点（processes）、一个控制节点（control）和一个速度控制节点（velocity），它们均在 px4_offboard 命名空间下运行。此外，它还启动了RViz 2，并加载了位于px4_offboard包共享目录下的 visualize.rviz 配置文件以进行数据可视化。所有这些节点的启动都通过 gnome-terminal 终端模拟器进行，以便用户可以在图形界面中看到它们的输出。这个启动文件为用户设置了一个完整的环境，用于通过键盘控制模拟无人机的速度，并实时观察其在 RViz 中的表现。
from launch import LaunchDescription
from launch.actions import ExecuteProcess
from launch_ros.actions import Node
from ament_index_python.packages import get_package_share_directory
import os
def generate_launch_description():
# 获取px4_offboard包的共享目录路径
package_dir = get_package_share_directory('px4_offboard')
# bash_script_path = os.path.join(package_dir, 'scripts', 'TerminatorScript.sh')
# 返回LaunchDescription对象，包含启动文件中定义的所有节点和进程
return LaunchDescription([
# ExecuteProcess(cmd=['bash', bash_script_path], output='screen'),
# 启动名为visualizer的节点，位于px4_offboard包中，运行在px4_offboard命名空间下
Node(
package='px4_offboard',
namespace='px4_offboard',
executable='visualizer',
name='visualizer'
),
# 启动名为processes的节点，位于px4_offboard包中，运行在px4_offboard命名空间下，使用gnome-terminal终端运行
Node(
package='px4_offboard',
namespace='px4_offboard',
executable='processes',
name='processes',
prefix='gnome-terminal --'
),
# 启动名为control的节点，位于px4_offboard包中，运行在px4_offboard命名空间下，使用gnome-terminal终端运行
Node(
package='px4_offboard',
namespace='px4_offboard',
executable='control',
name='control',
prefix='gnome-terminal --',
),
# 启动名为velocity的节点，位于px4_offboard包中，运行在px4_offboard命名空间下
Node(
package='px4_offboard',
namespace='px4_offboard',
executable='velocity_control',
name='velocity'
),
# 启动rviz2节点，加载位于px4_offboard包共享目录下的visualize.rviz配置文件
Node(
package='rviz2',
namespace='',
executable='rviz2',
name='rviz2',
arguments=['-d', os.path.join(package_dir, 'visualize.rviz')]
)
])3. 速度和方向控制
文件control.py是一个ROS 2节点，用于从键盘接收输入并将其发布为Twist消息，控制无人机的速度和方向。用户可以通过键盘上的箭头键和 WASD 键来控制无人机的移动，空格键用于武装或解除武装无人机。该节点还负责发布速度和转向指令到 /offboard_velocity_cmd 主题，并根据键盘输入实时更新无人机的状态。
import sys
import geometry_msgs.msg
import rclpy
import std_msgs.msg
from rclpy.qos import QoSProfile, QoSReliabilityPolicy, QoSHistoryPolicy, QoSDurabilityPolicy
if sys.platform == 'win32':
import msvcrt
else:
import termios
import tty
msg = """
该节点从键盘接收按键输入，并将其作为Twist消息发布。
使用箭头键和WASD，你可以获得模式2的遥控控制。
W: 向上
S: 向下
A: 偏航向左
D: 偏航向右
上箭头: 俯仰向前
下箭头: 俯仰向后
左箭头: 横滚向左
右箭头: 横滚向右
按空格键来武装/解除武装无人机
"""
moveBindings = {
'w': (0, 0, 1, 0), #Z+
's': (0, 0, -1, 0),#Z-
'a': (0, 0, 0, -1), #偏航+
'd': (0, 0, 0, 1),#偏航-
'\x1b[A' : (0, 1, 0, 0), #上箭头
'\x1b[B' : (0, -1, 0, 0), #下箭头
'\x1b[C' : (-1, 0, 0, 0), #右箭头
'\x1b[D' : (1, 0, 0, 0), #左箭头
}
speedBindings = {
# 'q': (1.1, 1.1),
# 'z': (.9, .9),
# 'w': (1.1, 1),
# 'x': (.9, 1),
# 'e': (1, 1.1),
# 'c': (1, .9),
}
def getKey(settings):
if sys.platform == 'win32':
# getwch()在Windows上返回字符串
key = msvcrt.getwch()
else:
tty.setraw(sys.stdin.fileno())
# sys.stdin.read()在Linux上返回字符串
key = sys.stdin.read(1)
if key == '\x1b': # 如果第一个字符是\x1b，我们可能在处理箭头键
additional_chars = sys.stdin.read(2) # 读取接下来的两个字符
key += additional_chars # 将这些字符附加到键上
termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)
return key
def saveTerminalSettings():
if sys.platform == 'win32':
return None
return termios.tcgetattr(sys.stdin)
def restoreTerminalSettings(old_settings):
if sys.platform == 'win32':
return
termios.tcsetattr(sys.stdin, termios.TCSADRAIN, old_settings)
def vels(speed, turn):
return '当前：\t速度 %s\t转向 %s ' % (speed, turn)
def main():
settings = saveTerminalSettings()
rclpy.init()
node = rclpy.create_node('teleop_twist_keyboard')
qos_profile = QoSProfile(
reliability=QoSReliabilityPolicy.BEST_EFFORT,
durability=QoSDurabilityPolicy.TRANSIENT_LOCAL,
history=QoSHistoryPolicy.KEEP_LAST,
depth=10
)
pub = node.create_publisher(geometry_msgs.msg.Twist, '/offboard_velocity_cmd', qos_profile)
arm_toggle = False
arm_pub = node.create_publisher(std_msgs.msg.Bool, '/arm_message', qos_profile)
speed = 0.5
turn = .2
x = 0.0
y = 0.0
z = 0.0
th = 0.0
status = 0.0
x_val = 0.0
y_val = 0.0
z_val = 0.0
yaw_val = 0.0
try:
print(msg)
# print(vels(speed, turn))
while True:
key = getKey(settings)
if key in moveBindings.keys():
x = moveBindings[key][0]
y = moveBindings[key][1]
z = moveBindings[key][2]
th = moveBindings[key][3]
else:
x = 0.0
y = 0.0
z = 0.0
th = 0.0
if (key == '\x03'):
break
if key == ' ': # 空格键的ASCII值
arm_toggle = not arm_toggle # 翻转arm_toggle的值
arm_msg = std_msgs.msg.Bool()
arm_msg.data = arm_toggle
arm_pub.publish(arm_msg)
print(f"武装切换现在是：{arm_toggle}")
twist = geometry_msgs.msg.Twist()
x_val = (x * speed) + x_val
y_val = (y * speed) + y_val
z_val = (z * speed) + z_val
yaw_val = (th * turn) + yaw_val
twist.linear.x = x_val
twist.linear.y = y_val
twist.linear.z = z_val
twist.angular.x = 0.0
twist.angular.y = 0.0
twist.angular.z = yaw_val
pub.publish(twist)
print("X:",twist.linear.x, " Y:",twist.linear.y, " Z:",twist.linear.z, " 偏航:",twist.angular.z)
except Exception as e:
print(e)
finally :
twist = geometry_msgs.msg.Twist()
twist.linear.x = 0.0
twist.linear.y = 0.0
twist.linear.z = 0.0
twist.angular.x = 0.0
twist.angular.y = 0.0
twist.angular.z = 0.0
pub.publish(twist)
restoreTerminalSettings(settings)
if __name__ == '__main__':
main()4. 自动化启动无人机模拟
文件processes.py用于自动化启动一系列与无人机模拟相关的进程。通过在新的 gnome-terminal 标签页中执行预定义的命令列表来运行 Micro XRCE-DDS Agent、PX4 软件在环（SITL）模拟，并且可以选择性地运行 QGroundControl 地面站软件。
import subprocess
import time
# 要运行的命令列表
commands = [
# 运行 Micro XRCE-DDS Agent
"MicroXRCEAgent udp4 -p 8888",
# 运行 PX4 SITL 模拟
"cd ~/PX4-Autopilot && make px4_sitl gz_x500"
# 运行 QGroundControl
# "cd ~/QGroundControl && ./QGroundControl.AppImage"
]
# 遍历列表中的每个命令
for command in commands:
# 每个命令都在 gnome-terminal 的新标签页中运行
subprocess.run(["gnome-terminal", "--tab", "--", "bash", "-c", command + "; exec bash"])
# 每个命令之间暂停
time.sleep(1)5. 无人机控制
文件velocity_control.py定义了一个名为 OffboardControl的ROS 2节点类，用于控制无人机的起飞、悬停和离板（offboard）模式。它通过订阅无人机的状态、姿态和速度指令，发布控制模式和轨迹设定点消息，以实现无人机的自动控制。
class OffboardControl(Node):
def __init__(self):
super().__init__('minimal_publisher')
qos_profile = QoSProfile(
reliability=QoSReliabilityPolicy.BEST_EFFORT,
durability=QoSDurabilityPolicy.TRANSIENT_LOCAL,
history=QoSHistoryPolicy.KEEP_LAST,
depth=1
)
# 创建订阅
self.status_sub = self.create_subscription(
VehicleStatus,
'/fmu/out/vehicle_status',
self.vehicle_status_callback,
qos_profile)
self.offboard_velocity_sub = self.create_subscription(
Twist,
'/offboard_velocity_cmd',
self.offboard_velocity_callback,
qos_profile)
self.attitude_sub = self.create_subscription(
VehicleAttitude,
'/fmu/out/vehicle_attitude',
self.attitude_callback,
qos_profile)
self.my_bool_sub = self.create_subscription(
Bool,
'/arm_message',
self.arm_message_callback,
qos_profile)
# 创建发布者
self.publisher_offboard_mode = self.create_publisher(OffboardControlMode, '/fmu/in/offboard_control_mode', qos_profile)
self.publisher_velocity = self.create_publisher(Twist, '/fmu/in/setpoint_velocity/cmd_vel_unstamped', qos_profile)
self.publisher_trajectory = self.create_publisher(TrajectorySetpoint, '/fmu/in/trajectory_setpoint', qos_profile)
self.vehicle_command_publisher_ = self.create_publisher(VehicleCommand, "/fmu/in/vehicle_command", 10)
# 创建武装定时器的回调函数
# 周期是任意的，只要超过2Hz即可
arm_timer_period = .1 # 秒
self.arm_timer_ = self.create_timer(arm_timer_period, self.arm_timer_callback)
# 创建命令循环的回调函数
# 周期是任意的，只要超过2Hz即可。因为实时控制依赖于此，所以推荐更高的频率
# 如果飞行器不在离板模式下，cmdloop_callback中的命令将不会被执行
timer_period = 0.02 # 秒
self.timer = self.create_timer(timer_period, self.cmdloop_callback)
self.nav_state = VehicleStatus.NAVIGATION_STATE_MAX
self.arm_state = VehicleStatus.ARMING_STATE_ARMED
self.velocity = Vector3()
self.yaw = 0.0 # 发送命令时的偏航值
self.trueYaw = 0.0 # 无人机当前的偏航值
self.offboardMode = False
self.flightCheck = False
self.myCnt = 0
self.arm_message = False
self.failsafe = False
self.current_state = "IDLE"
self.last_state = self.current_state
def arm_message_callback(self, msg):
self.arm_message = msg.data
self.get_logger().info(f"武装消息：{self.arm_message}")
# 武装、起飞和切换到离板模式的回调函数
# 实现有限状态机
def arm_timer_callback(self):
match self.current_state:
case "IDLE":
if(self.flightCheck and self.arm_message == True):
self.current_state = "ARMING"
self.get_logger().info(f"正在武装")
case "ARMING":
if(not(self.flightCheck)):
self.current_state = "IDLE"
self.get_logger().info(f"武装，飞行检查失败")
elif(self.arm_state == VehicleStatus.ARMING_STATE_ARMED and self.myCnt > 10):
self.current_state = "TAKEOFF"
self.get_logger().info(f"武装，起飞")
self.arm() # 发送武装命令
case "TAKEOFF":
if(not(self.flightCheck)):
self.current_state = "IDLE"
self.get_logger().info(f"起飞，飞行检查失败")
elif(self.nav_state == VehicleStatus.NAVIGATION_STATE_AUTO_TAKEOFF):
self.current_state = "LOITER"
self.get_logger().info(f"起飞，悬停")
self.arm() # 发送武装命令
self.take_off() # 发送起飞命令
# 在此状态下等待起飞，一旦VehicleStatus切换到悬停状态，它将切换到离板模式
case "LOITER":
if(not(self.flightCheck)):
self.current_state = "IDLE"
self.get_logger().info(f"悬停，飞行检查失败")
elif(self.nav_state == VehicleStatus.NAVIGATION_STATE_AUTO_LOITER):
self.current_state = "OFFBOARD"
self.get_logger().info(f"悬停，离板")
self.arm()
case "OFFBOARD":
if(not(self.flightCheck) or self.arm_state != VehicleStatus.ARMING_STATE_ARMED or self.failsafe == True):
self.current_state = "IDLE"
self.get_logger().info(f"离板，飞行检查失败")
self.state_offboard()
if(self.arm_state != VehicleStatus.ARMING_STATE_ARMED):
self.arm_message = False
if (self.last_state != self.current_state):
self.last_state = self.current_state
self.get_logger().info(self.current_state)
self.myCnt += 1
def state_offboard(self):
self.myCnt = 0
self.publish_vehicle_command(VehicleCommand.VEHICLE_CMD_DO_SET_MODE, 1., 6.)
self.offboardMode = True
# 武装飞行器
def arm(self):
self.publish_vehicle_command(VehicleCommand.VEHICLE_CMD_COMPONENT_ARM_DISARM, 1.0)
self.get_logger().info("已发送武装命令")
# 起飞到用户指定的高度（米）
def take_off(self):
self.publish_vehicle_command(VehicleCommand.VEHICLE_CMD_NAV_TAKEOFF, param1 = 1.0, param7=5.0) # param7 是高度（米）
self.get_logger().info("已发送起飞命令")
# 发布到 /fmu/in/vehicle_command
def publish_vehicle_command(self, command, param1=0.0, param2=0.0, param7=0.0):
msg = VehicleCommand()
msg.param1 = param1
msg.param2 = param2
msg.param7 = param7 # 起飞命令中的高度值
msg.command = command # 命令ID
msg.target_system = 1 # 应执行命令的系统
msg.target_component = 1 # 应执行命令的组件，0 表示所有组件
msg.source_system = 1 # 发送命令的系统
msg.source_component = 1 # 发送命令的组件
msg.from_external = True
msg.timestamp = int(Clock().now().nanoseconds / 1000) # 时间（微秒）
self.vehicle_command_publisher_.publish(msg)
# 接收并设置飞行器状态值
def vehicle_status_callback(self, msg):
if (msg.nav_state != self.nav_state):
self.get_logger().info(f"导航状态：{msg.nav_state}")
if (msg.arming_state != self.arm_state):
self.get_logger().info(f"武装状态：{msg.arming_state}")
if (msg.failsafe != self.failsafe):
self.get_logger().info(f"故障安全：{msg.failsafe}")
if (msg.pre_flight_checks_pass != self.flightCheck):
self.get_logger().info(f"飞行检查：{msg.pre_flight_checks_pass}")
self.nav_state = msg.nav_state
self.arm_state = msg.arming_state
self.failsafe = msg.failsafe
self.flightCheck = msg.pre_flight_checks_pass
# 从 Teleop 接收 Twist 命令并转换 NED -> FLU
def offboard_velocity_callback(self, msg):
# 实现 NED -> FLU 转换
# X (FLU) 是 -Y (NED)
self.velocity.x = -msg.linear.y
# Y (FLU) 是 X (NED)
self.velocity.y = msg.linear.x
# Z (FLU) 是 -Z (NED)
self.velocity.z = -msg.linear.z
# 偏航 z 的转换在 attitude_callback 函数中完成（在 self.trueYaw 前面的 '-'）
self.yaw = msg.angular.z
# 从无人机接收当前轨迹值并获取方向的偏航值
def attitude_callback(self, msg):
orientation_q = msg.q
# trueYaw 是无人机当前的偏航值
self.trueYaw = -(np.arctan2(2.0*(orientation_q[3]*orientation_q[0] + orientation_q[1]*orientation_q[2]),
1.0 - 2.0*(orientation_q[0]*orientation_q[0] + orientation_q[1]*orientation_q[1])))
# 发布离板控制模式和速度作为轨迹设定点
def cmdloop_callback(self):
if(self.offboardMode == True):
# 发布离板控制模式
offboard_msg = OffboardControlMode()
offboard_msg.timestamp = int(Clock().now().nanoseconds / 1000)
offboard_msg.position = False
offboard_msg.velocity = True
offboard_msg.acceleration = False
self.publisher_offboard_mode.publish(offboard_msg)
def main(args=None):
rclpy.init(args=args)
offboard_control = OffboardControl()
rclpy.spin(offboard_control)
offboard_control.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()6. 可视化
文件visualizer.py实现了一个ROS 2节点PX4Visualizer，用于可视化展示PX4无人机的飞行状态和轨迹。文件visualizer.py订阅了无人机的姿态、局部位置和轨迹设定点，然后发布无人机的姿态、速度和路径信息，以及轨迹设定点的路径信息。此外，它还发布表示无人机速度的箭头标记。
from re import M
import numpy as np
import rclpy
from rclpy.node import Node
from rclpy.clock import Clock
from rclpy.qos import QoSProfile, QoSReliabilityPolicy, QoSHistoryPolicy
from px4_msgs.msg import VehicleAttitude
from px4_msgs.msg import VehicleLocalPosition
from px4_msgs.msg import TrajectorySetpoint
from geometry_msgs.msg import PoseStamped, Point
from nav_msgs.msg import Path
from visualization_msgs.msg import Marker
def vector2PoseMsg(frame_id, position, attitude):
pose_msg = PoseStamped()
# msg.header.stamp = Clock().now().nanoseconds / 1000
pose_msg.header.frame_id=frame_id
pose_msg.pose.orientation.w = attitude[0]
pose_msg.pose.orientation.x = attitude[1]
pose_msg.pose.orientation.y = attitude[2]
pose_msg.pose.orientation.z = attitude[3]
pose_msg.pose.position.x = position[0]
pose_msg.pose.position.y = position[1]
pose_msg.pose.position.z = position[2]
return pose_msg
class PX4Visualizer(Node):
def __init__(self):
super().__init__('px4_visualizer')
## 配置订阅
qos_profile = QoSProfile(
reliability=QoSReliabilityPolicy.RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT,
history=QoSHistoryPolicy.RMW_QOS_POLICY_HISTORY_KEEP_LAST,
depth=1
)
self.attitude_sub = self.create_subscription(
VehicleAttitude,
'/fmu/out/vehicle_attitude',
self.vehicle_attitude_callback,
qos_profile)
self.local_position_sub = self.create_subscription(
VehicleLocalPosition,
'/fmu/out/vehicle_local_position',
self.vehicle_local_position_callback,
qos_profile)
self.setpoint_sub = self.create_subscription(
TrajectorySetpoint,
'/fmu/in/trajectory_setpoint',
self.trajectory_setpoint_callback,
qos_profile)
self.vehicle_pose_pub = self.create_publisher(PoseStamped, '/px4_visualizer/vehicle_pose', 10)
self.vehicle_vel_pub = self.create_publisher(Marker, '/px4_visualizer/vehicle_velocity', 10)
self.vehicle_path_pub = self.create_publisher(Path, '/px4_visualizer/vehicle_path', 10)
self.setpoint_path_pub = self.create_publisher(Path, '/px4_visualizer/setpoint_path', 10)
self.vehicle_attitude = np.array([1.0, 0.0, 0.0, 0.0])
self.vehicle_local_position = np.array([0.0, 0.0, 0.0])
self.vehicle_local_velocity = np.array([0.0, 0.0, 0.0])
self.setpoint_position = np.array([0.0, 0.0, 0.0])
self.vehicle_path_msg = Path()
self.setpoint_path_msg = Path()
timer_period = 0.05 # 秒
self.timer = self.create_timer(timer_period, self.cmdloop_callback)
def vehicle_attitude_callback(self, msg):
# TODO: 处理 NED->ENU 转换
self.vehicle_attitude[0] = msg.q[0]
self.vehicle_attitude[1] = msg.q[1]
self.vehicle_attitude[2] = -msg.q[2]
self.vehicle_attitude[3] = -msg.q[3]
def vehicle_local_position_callback(self, msg):
# TODO: 处理 NED->ENU 转换
self.vehicle_local_position[0] = msg.x
self.vehicle_local_position[1] = -msg.y
self.vehicle_local_position[2] = -msg.z
self.vehicle_local_velocity[0] = msg.vx
self.vehicle_local_velocity[1] = -msg.vy
self.vehicle_local_velocity[2] = -msg.vz
def trajectory_setpoint_callback(self, msg):
self.setpoint_position[0] = msg.position[0]
self.setpoint_position[1] = -msg.position[1]
self.setpoint_position[2] = -msg.position[2]
def create_arrow_marker(self, id, tail, vector):
msg = Marker()
msg.action = Marker.ADD
msg.header.frame_id = 'map'
# msg.header.stamp = Clock().now().nanoseconds / 1000
msg.ns = 'arrow'
msg.id = id
msg.type = Marker.ARROW
msg.scale.x = 0.1
msg.scale.y = 0.2
msg.scale.z = 0.0
msg.color.r = 0.5
msg.color.g = 0.5
msg.color.b = 0.0
msg.color.a = 1.0
dt = 0.3
tail_point = Point()
tail_point.x = tail[0]
tail_point.y = tail[1]
tail_point.z = tail[2]
head_point = Point()
head_point.x = tail[0] + dt * vector[0]
head_point.y = tail[1] + dt * vector[1]
head_point.z = tail[2] + dt * vector[2]
msg.points = [tail_point, head_point]
return msg
def cmdloop_callback(self):
vehicle_pose_msg = vector2PoseMsg('map', self.vehicle_local_position, self.vehicle_attitude)
self.vehicle_pose_pub.publish(vehicle_pose_msg)
# 发布无人机路径的时间历史
self.vehicle_path_msg.header = vehicle_pose_msg.header
self.vehicle_path_msg.poses.append(vehicle_pose_msg)
self.vehicle_path_pub.publish(self.vehicle_path_msg)
# 发布轨迹设定点的路径时间历史
setpoint_pose_msg = vector2PoseMsg('map', self.setpoint_position, self.vehicle_attitude)
self.setpoint_path_msg.header = setpoint_pose_msg.header
self.setpoint_path_msg.poses.append(setpoint_pose_msg)
self.setpoint_path_pub.publish(self.setpoint_path_msg)
# 发布速度的箭头标记
velocity_msg = self.create_arrow_marker(1, self.vehicle_local_position, self.vehicle_local_velocity)
self.vehicle_vel_pub.publish(velocity_msg)
def main(args=None):
rclpy.init(args=args)
px4_visualizer = PX4Visualizer()
rclpy.spin(px4_visualizer)
px4_visualizer.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()7. 运行
通过以下命令启动项目中的所有节点：
ros2 launch px4_offboard offboard_velocity_control.launch.py可以使用RViz2来可视化无人机的状态和轨迹：
rviz2RViz2的可视化效果如图3-8所示。
图3-8 无人飞行器的可视化效果
浙公网安备 33010602011771号