PX4 ROS2 导航接口
PX4 v1.15 ExperimentalWARNING
Experimental 在撰写本文时,PX4 ROS 2 接口库的部分内容仍处于试验阶段,因此可能会发生变动。
PX4 ROS 2 Interface Library 中的导航接口,支持开发者直接从 ROS 2 应用(如视觉惯性里程计系统或地图匹配系统)向 PX4 发送位置测量数据。 该接口提供了对 PX4 和 uORB 消息框架的抽象层,并对通过该接口发送的请求状态估计更新引入了一些合理性检查。 这些测量数据随后会被融合到扩展EKF中,其处理方式与 PX4 内部生成的测量数据完全一致。
库提供两个类,LocalPositionMeasurementInterface
和 GlobalPositionMeasureInterface
它都会暴露出一个类似的 "update" 方法来提供一个本地位置或全球位置更新到 PX4。 update
方法需要一个位置测量struct
(LocalPositionMeasure
](https://auterion.github.io/px4-ros2-interface-lib/structpx4__ros2_1_1LocalPositionMeasurement.html)或`GlobalPositionMeasure`],开发者可以在其中填入自己生成的位置测量数据。
安装与首次测试
开始使用前,需完成以下步骤:
请确保您在 ROS 2 工作区中有 ROS 2 设置 与 [
px4_msgs
](https://github.com/PX4/px4_msgs]。将代码仓库克隆到工作空间中
shcd $ros_workspace/src git clone --recursive https://github.com/Auterion/px4-ros2-interface-lib
提示信息 为确保兼容性,请使用 PX4、px4_msgs(PX4 消息包)及该库的最新 main 分支。 另请参阅 [here](https://github.com/Auterion/px4-ros2-interface-lib#compatibility-with-px4)
:::
构建工作空间:
shcd .. colcon版本
在另一个外壳中,启动 PX4 SITL:
shcd $px4-autopilot make px4_sitl gazebo-classic
(这里我们使用Gazebo-Classic,但你可以使用任何模型或模拟器)
在另一个独立的终端中,运行 micro XRCE 代理(运行后可保持后台持续运行):
shMicroXRCEAgent udp4 -p 8888
返回ROS2终端,为您刚刚构建的工作空间(步骤3),运行 global_navigation 示例。 它会周期性地发送虚拟的全球位置更新(数据):
shsource install/setup.bash ros2 run example_global_navigation_cpp example_global_navigation
你应会看到如下所示的输出,该输出表明全球位置接口正成功发送位置更新:
sh[INFO] [1702030701.836897756] [example_global_navigation_node]: example_global_navigation_node running! [DEBUG] [1702030702.837279784] [example_global_navigation_node]: Successfully sent position update to navigation interface. [DEBUG] [1702030703.837223884] [example_global_navigation_node]: Successfully sent position update to navigation interface.
在 PX4 终端(PX4 shell)中,你可以通过以下操作检查 PX4 是否接收到全球位置更新(数据)
shlistener aux_global_position
输出内容应如下所示:
shTOPIC: aux_global_position aux_global_position timestamp: 46916000 (0.528000 seconds ago) timestamp_sample: 46916000 (0 us before timestamp) lat: 12.343210 lon: 23.454320 alt: 12.40000 alt_ellipsoid: 0.00000 delta_alt: 0.00000 eph: 0.31623 epv: 0.44721 terrain_alt: 0.00000 lat_lon_reset_counter: 0 alt_reset_counter: 0 terrain_alt_valid: False dead_reckoning: False
现在你已准备好使用该导航接口发送自己的位置更新数据了。
如何使用代码库
要发送位置测量数据,你需要用所测量的值填充位置结构体。 然后以此结构调用接口的更新功能作为参数。
关于如何使用此接口的基本示例,请在“Auterion/px4-ros2-interface-lib”仓库中查看 examples 例如示例/cpp/navigation/local_navigation或示例/cpp/navigation/global_navigation。
局部位置更新
首先确保正确配置 PX4 参数EKF2_EV_CTRL
通过将相应的位设置为true,可以正确配置(系统)以融合外部局部测量数据:
0
: 水平位置数据1
:垂直位置数据2
:速度数据3
:偏航角数据
向PX4发送局部位置测量:
- 通过提供一个 ROS节点,创建一个
localPositionMeasureInterface
实例:您测量的位置和速度参考框架。 - 包含一个
本地定位测量
structt
,包含你测量的数据。 - 将
struct
传入LocalPositionMeasurementInterface
update()
方法中。
你的测量数据可用的位置和速度参考坐标系由以下枚举(enum)定义:
cpp
enum class PoseFrame
{
Unknown,
LocalNED,
LocalFRD
};
enum class VelocityFrame
{
Unknown,
LocalNED,
LocalFRD,
BodyFRD
};
LocalPositionMeasurement
结构定义如下:
cpp
struct LocalPositionMeasurement
{
rclcpp::Time timestamp_sample {};
std::optional<0> position_xy {std::nullopt};
std::optional<0> position_xy_variance {std::nullopt};
std::optional<1> position_z {std::nullopt};
std::optional<1> position_z_variance {std::nullopt};
std::optional<0> velocity_xy {std::nullopt};
std::optional<0> velocity_xy_variance {std::nullopt};
std::optional<1> velocity_z {std::nullopt};
std::optional<1> velocity_z_variance {std::nullopt};
std::optional<2> attitude_quaternion {std::nullopt};
std::optional<3> attitude_variance {std::nullopt};
};
局部接口的update()方法要求LocalPositionMeasurement(局部位置测量结构体)满足以下条件:
- 示例时间戳已定义。
- 数值不得包含
NAN
。 - 如果提供了某个测量值,则其关联的方差值必须明确定义(例如,若position_xy已定义,则position_xy_variance也必须定义)。
- 如果提供了某个测量值,那么其关联的参考坐标系不得为 “未知”(例如,若position_xy已定义,则接口必须以不同于PoseFrame::Unknown的位置坐标系进行初始化)。
以下是一个 ROS 2 节点的示例代码片段,该节点使用局部导航接口向 PX4 发送东北天(NED)参考坐标系下的 3D 位姿更新:
cpp
class MyLocalMeasurementUpdateNode : public rclcpp::Node
{
public:
MyLocalMeasurementUpdateNode()
: Node("my_node_name")
{
// Set pose measurement reference frame to north-east-down
const px4_ros2::PoseFrame pose_frame = px4_ros2::PoseFrame::LocalNED;
// We will only send pose measurements in this example
// Set velocity measurement reference frame to unknown
const px4_ros2::VelocityFrame velocity_frame = px4_ros2::VelocityFrame::Unknown;
// Initialize local interface [1]
_local_position_measurement_interface =
std::make_shared<Eigen::Vector2f>(*this, pose_frame, velocity_frame);
}
void sendUpdate()
{
while (running) { // Potentially make method run as a callback or on a timer
// Generate local position measurement
rclcpp::Time timestamp_sample = ...
Eigen::Vector2f position_xy = ...
Eigen::Vector2f position_xy_variance = ...
float position_z = ...
float position_z_variance = ...
// Populate the local position measurement struct [2]
px4_ros2::LocalPositionMeasurement local_position_measurement{};
local_position_measurement.timestamp_sample = timestamp_sample;
local_position_measurement.position_xy = position_xy;
local_position_measurement.position_xy_variance = position_xy_variance;
local_position_measurement.position_z = position_z;
local_position_measurement.position_z_variance = position_z_variance;
// Send measurement to PX4 using the interface [3]
try {
_local_position_measurement_interface->update(local_position_measurement);
} catch (const px4_ros2::NavigationInterfaceInvalidArgument & e) {
// Handle exceptions caused by invalid local_position_measurement definition
RCLCPP_ERROR(get_logger(), "Exception caught: %s", e.what());
}
}
}
private:
std::shared_ptr<Eigen::Vector2f> _local_position_measurement_interface;
};
全局位置更新
首先确保正确配置 PX4 参数EKF2_EV_CTRL
通过将相应的位设置为true,可以正确配置(系统)以融合外部全部测量数据:
0
: 水平位置数据1
:垂直位置数据
向PX4发送全局位置测量:
- 创建一个
GlobalPositionMeasureInterface
实例,并提供一个 ROS节点。 - 包含一个
GlobalPositionMeasurement
struct
,包含你测量的数据。 - 将结构移至
GlobalPositionMeasurementInterface
update() 方法中。
GlobalPositionMeasurement
结构定义如下:
cpp
struct GlobalPositionMeasurement
{
rclcpp::Time timestamp_sample {};
std::optional<px4_ros2::LocalPositionMeasurementInterface> lat_lon {std::nullopt};
std::optional<1> horizontal_variance {std::nullopt};
std::optional<1> altitude_msl {std::nullopt};
std::optional<1> vertical_variance {std::nullopt};
全局接口的 update()
方法预计在 GlobalPositionMeasurement
中保留以下条件:
- 样本
timestamp_sample
已定义。 - 数据不得包含NAN。
- 如果提供了某个测量值,那么其关联的方差值必须明确定义(例如,若lat_lon(经纬度)已定义,则horizontal_variance(水平方差)也必须定义)。
下面的代码片段是一个 ROS 2 节点的示例,该节点使用全局导航接口来发送一个测量纬度。 经度和高度到 PX4:
cpp
class MyGlobalMeasurementUpdateNode : public rclcpp::Node
{
public:
MyGlobalMeasurementUpdateNode()
: Node("my_node_name")
{
// Initialize global interface [1]
_global_position_measurement_interface =
std::make_shared<Eigen::Vector2d>(*this);
}
void sendUpdate()
{
while (running) { // Potentially make method run as a callback or on a timer
// Generate global position measurement
rclcpp::Time timestamp_sample = ...
Eigen::Vector2d lat_lon = ...
float horizontal_variance = ...
float altitude_msl = ...
float vertical_variance = ...
// Populate the global position measurement struct [2]
px4_ros2::GlobalPositionMeasurement global_position_measurement{};
global_position_measurement.timestamp_sample = timestamp_sample;
global_position_measurement.lat_lon = lat_lon;
global_position_measurement.horizontal_variance = horizontal_variance;
global_position_measurement.altitude_msl = altitude_msl;
global_position_measurement.vertical_variance = vertical_variance;
// Send measurement to PX4 using the interface [3]
try {
_global_position_measurement_interface->update(global_position_measurement);
} catch (const px4_ros2::NavigationInterfaceInvalidArgument & e) {
// Handle exceptions caused by invalid global_position_measurement definition
RCLCPP_ERROR(get_logger(), "Exception caught: %s", e.what());
}
}
}
private:
std::shared_ptr<0> _global_position_measurement_interface;
};
接口的多个实例
使用同一接口的多个实例(例如,多个局部接口实例)发送估计更新时,会将所有更新消息发送到同一个主题,从而导致串扰。 这不应影响计量并入EKF,但不同的计量来源将无法区分。