Hello, I’m a beginner developer working with PX4.
I’m currently developing a simulation using PX4 SITL version 1.15 with Gazebo Harmonic. I’ve modified the drone model to use a Livox LiDAR and am using a custom model named gz_x500_mid360.
Both point cloud and IMU data are being published correctly.
I’m forwarding the Gazebo topics to ROS 2 and using fast_lio for processing. As a result, /Odometry is being published, and the mapping appears to work well in RViz.
The issue arises when PX4 tries to consume the /Odometry data produced by fast_lio.
#include <rclcpp/rclcpp.hpp>
#include <nav_msgs/msg/odometry.hpp>
#include “px4_msgs/msg/vehicle_odometry.hpp”
#include <tf2/LinearMath/Quaternion.h>
#include <tf2/LinearMath/Matrix3x3.h>
#include <tf2/LinearMath/Transform.h> // For tf2::Transform
#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp> // For converting geometry_msgs::msg::Quaternion to tf2::Quaternion
class OdometryBridge : public rclcpp::Node
{
public:
OdometryBridge()
: Node(“odometry_bridge”)
{
odom_sub_ = this->create_subscription<nav_msgs::msg::Odometry>(
“/Odometry”,
rclcpp::QoS(rclcpp::KeepLast(10)),
std::bind(&OdometryBridge::odometry_callback, this, std::placeholders::1));
px4_odom_pub = this->create_publisher<px4_msgs::msg::VehicleOdometry>(
“/px4_1/fmu/in/vehicle_visual_odometry”, // For PX4 SITL, default input is /fmu/in/vehicle_visual_odometry. For real vehicles, this may vary.
rclcpp::QoS(rclcpp::KeepLast(10)));
}
private:
rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr odom_sub_;
rclcpp::Publisher<px4_msgs::msg::VehicleOdometry>::SharedPtr px4_odom_pub_;
void odometry_callback(const nav_msgs::msg::Odometry::SharedPtr msg){
px4_msgs::msg::VehicleOdometry px4_odom_msg{};
// 1. Timestamp (microseconds)
// PX4 expects timestamps in microseconds.
px4_odom_msg.timestamp = this->get_clock()->now().nanoseconds() / 1000;
rclcpp::Time stamp_time(msg->header.stamp);
px4_odom_msg.timestamp_sample = stamp_time.nanoseconds() / 1000;
// 2. Frame setting
px4_odom_msg.pose_frame = px4_msgs::msg::VehicleOdometry::POSE_FRAME_NED;
px4_odom_msg.velocity_frame = px4_msgs::msg::VehicleOdometry::VELOCITY_FRAME_NED;
// 3. Position conversion (ENU -> NED)
// ROS: x = East, y = North, z = Up
// PX4: x = North, y = East, z = Down
px4_odom_msg.position[0] = msg->pose.pose.position.y; // ROS Y (North) -> PX4 X (North)
px4_odom_msg.position[1] = msg->pose.pose.position.x; // ROS X (East) -> PX4 Y (East)
px4_odom_msg.position[2] = -msg->pose.pose.position.z; // ROS Z (Up) -> PX4 Z (Down)
// 4. Orientation conversion (ENU Quaternion -> NED Quaternion)
// Transformation quaternion from ENU to NED (X<->Y swap, Z negation)
tf2::Quaternion q_enu_orig;
tf2::fromMsg(msg->pose.pose.orientation, q_enu_orig); // Convert geometry_msgs::Quaternion to tf2::Quaternion
// ENU -> NED rotation matrix
tf2::Matrix3x3 R_enu_to_ned;
R_enu_to_ned[0][0] = 0; R_enu_to_ned[0][1] = 1; R_enu_to_ned[0][2] = 0; // new X = old Y (North)
R_enu_to_ned[1][0] = 1; R_enu_to_ned[1][1] = 0; R_enu_to_ned[1][2] = 0; // new Y = old X (East)
R_enu_to_ned[2][0] = 0; R_enu_to_ned[2][1] = 0; R_enu_to_ned[2][2] = -1; // new Z = -old Z (Down)
// Convert original ENU quaternion to rotation matrix
tf2::Matrix3x3 R_enu_orig;
R_enu_orig.setRotation(q_enu_orig);
// Final NED rotation = R_ENU_to_NED * R_ENU
tf2::Matrix3x3 R_ned_final = R_enu_to_ned * R_enu_orig;
tf2::Quaternion q_ned;
R_ned_final.getRotation(q_ned); // Extract quaternion from final NED rotation matrix
px4_odom_msg.q[0] = q_ned.w();
px4_odom_msg.q[1] = q_ned.x();
px4_odom_msg.q[2] = q_ned.y();
px4_odom_msg.q[3] = q_ned.z();
// 5. Velocity conversion (ENU -> NED)
px4_odom_msg.velocity[0] = msg->twist.twist.linear.y; // ROS Y (North) -> PX4 X (North)
px4_odom_msg.velocity[1] = msg->twist.twist.linear.x; // ROS X (East) -> PX4 Y (East)
px4_odom_msg.velocity[2] = -msg->twist.twist.linear.z; // ROS Z (Up) -> PX4 Z (Down)
// Angular velocity conversion (ENU -> NED)
tf2::Vector3 angular_vel_enu;
tf2::fromMsg(msg->twist.twist.angular, angular_vel_enu);
// Apply rotation
tf2::Vector3 angular_vel_ned = R_enu_to_ned * angular_vel_enu;
px4_odom_msg.angular_velocity[0] = angular_vel_ned.x();
px4_odom_msg.angular_velocity[1] = angular_vel_ned.y();
px4_odom_msg.angular_velocity[2] = angular_vel_ned.z();
// 6. Covariance
// Covariances should ideally be transformed: C_NED = R * C_ENU * R^T
// For now, use diagonal terms and apply axis remapping.
// Example: setting manually for test
px4_odom_msg.position_variance[0] = 0.05;
px4_odom_msg.position_variance[1] = 0.05;
px4_odom_msg.position_variance[2] = 0.05;
px4_odom_msg.orientation_variance[0] = 0.05;
px4_odom_msg.orientation_variance[1] = 0.05;
px4_odom_msg.orientation_variance[2] = 0.05;
px4_odom_msg.velocity_variance[0] = 0.05;
px4_odom_msg.velocity_variance[1] = 0.05;
px4_odom_msg.velocity_variance[2] = 0.05;
px4_odom_msg.reset_counter = 0;
px4_odom_msg.quality = 100; // 0–100, 100 = highest quality
px4_odom_pub_->publish(px4_odom_msg);
// Debug logs
RCLCPP_INFO(this->get_logger(), "ROS ENU Pos: x=%.4f, y=%.4f, z=%.4f",
msg->pose.pose.position.x, msg->pose.pose.position.y, msg->pose.pose.position.z);
RCLCPP_INFO(this->get_logger(), "PX4 NED Pos: x=%.4f, y=%.4f, z=%.4f",
px4_odom_msg.position[0], px4_odom_msg.position[1], px4_odom_msg.position[2]);
RCLCPP_INFO(this->get_logger(), "ROS ENU Q: x=%.4f, y=%.4f, z=%.4f, w=%.4f",
msg->pose.pose.orientation.x, msg->pose.pose.orientation.y,
msg->pose.pose.orientation.z, msg->pose.pose.orientation.w);
RCLCPP_INFO(this->get_logger(), "PX4 NED Q: x=%.4f, y=%.4f, z=%.4f, w=%.4f",
px4_odom_msg.q[1], px4_odom_msg.q[2], px4_odom_msg.q[3], px4_odom_msg.q[0]);
}
};
int main(int argc, char *argv)
{
rclcpp::init(argc, argv);
rclcpp::spin(std::make_shared());
rclcpp::shutdown();
return 0;
}
When I command the drone to take off to 5 meters, it fails to localize properly and begins drifting uncontrollably until it crashes into obstacles and falls.
I’m unsure if this is due to a problem in the coordinate frame conversion or incorrect EKF parameter settings.
The parameters I’ve set are as follows:
bash
param set EKF2_EV_CTRL 3
param set EKF2_EVP_NOISE 0.2
param set EKF2_EVV_NOISE 0.2
param set EKF2_EVA_NOISE 0.2
param set EKF2_AID_MASK 24 # This one does not seem to exist in the current PX4 version, which may be part of the problem
param set EKF2_HGT_MODE 3
I would greatly appreciate any help in diagnosing or resolving this issue.
Thank you.