Sending gps data from Ros 2 to PX4 via Micro XRCE-DDS

Hi everyone! I am a total newbie and in need of help. I know some of my questions may have been answered here before and I have searched for them but am still unsure of what I am doing. I am a student and currently working on a research project where I am in charge of flying a drone indoor that should be getting its GPS location from opti track cameras. I have the gps coordinates from opti track and am trying to send this data to the PX4 Flight controller to update the drone gps location. I am trying to send this information via ROS2 Micro XRCE-DDS client. This is being done on a pi 3 with ubuntu server 22.04 lts that will be physically attached and wired to the Pixhawk. A few of my questions are:

  1. Does it sound like Im going about this in the right way? Can the Micro XRCE-DDS do what I am looking for
  2. Does the PixHawk firmware need to be changed to get the client on the PixHawk? I am unsure of how I am supposed to get the firmware onto the controller or if it comes default
  3. What is the best way to go about connecting ROS2 to the Pixhawk controller and updating the drones gps location - is there a tutorial somewhere or suggestions on how to complete this

Thank you all I have attached a picture of my pixhawk controller for reference

Hi @matthewhood14

Yes, a solid yes.


when you say

You literally want to send raw GPS data, like you have a GPS receiver on your drone?
Or the opti track system is acquing local position data, such as x,y,z in some local reference frame and the orientation?
because the two approaches are quite different.

Thanks for your reply! I have made a lot of progress from this point and wanted to provide some more clarification because I have more questions. I am sending pose data (x,y,z and orientation) from an external camera source (mocap_optitrack) to the px4 via the XRCE-DDS bridge. I have successfully setup my DDS bridge and am posting the mocap_optitrack pose data to /fmu/in/vehicle_visual_odometry. I can confirm the px4 is recieving the topic as when I go into mavlink console in QGroundControl and run ‘listener vehicle_visual_odometry’ I can see the data present and being updated. However, when I go into the mavlink inspector the actual Odometry topic does not show the updated data. Additionally the drone will not arm in position mode. It seems the EKF2 is not using the data but I am unsure why this might be

@Benja Any help is greatly appreciated, Me and two others are hoping get the system up and running before the 11th for a presentation. If you need any information let me know. It feels like it is very close to working as the visual odometry data is being recieved by the px4 I am just unsure of why it is not using it - thanks for any help

/fum/in/vehicle_visual_odometry is the right way :slight_smile:

Now you probably have some frame mismatches:
PX4 expects odom data in NED & RFD references while ROS normally uses ENU & FLU, see ROS 2 User Guide | PX4 User Guide for references.

Then make sure that the EKF2 is configured to use the EV (external vision) data and not the GPS Using the ECL EKF | PX4 User Guide

  • EKF2_EV_CTRL = 11 (fuse horizontal position, vertical position and yaw) or EKF2_EV_CTRL = 3 (fuse horizontal position, vertical position) depending on your setup
  • EKF2_GPS_CTRL = 0 (no GPS fusion)
  • EKF2_HGT_REF = 3 (height reference coming from EV)

@Benja Thanks for your reply! I have changed my frames to ensure they are in NED and FRD format. I have EKF2_EV_CTRL = 3, gps fusion turned off, and height reference set to Vision. EKF2 is still not using the data - I have linked my code that translates the data aswell

Edit: I have SYS_HAS_GPS = disabled because there is no gps on board. Is this correct?

#include
#include <rclcpp/rclcpp.hpp>
#include <geometry_msgs/msg/pose_stamped.hpp>
#include <px4_msgs/msg/vehicle_odometry.hpp>

using namespace std::chrono_literals;

class OptitrackToOdometryMapper : public rclcpp::Node
{
public:
OptitrackToOdometryMapper() : Node(“optitrack_to_odometry_mapper”)
{
// Initialize the subscriber
subscription_ = this->create_subscription<geometry_msgs::msg::PoseStamped>(
“/Drone/pose”,
10,
std::bind(&OptitrackToOdometryMapper::listener_callback, this, std::placeholders::_1));

    // Publisher for the uORB topic (vehicle_odometry)
    publisher_ = this->create_publisher<px4_msgs::msg::VehicleOdometry>("/fmu/in/vehicle_visual_odometry", 10);

    // Set up timer callback
    timer_ = this->create_wall_timer(20ms, std::bind(&OptitrackToOdometryMapper::timer_callback, this));
}

private:
void listener_callback(const geometry_msgs::msg::PoseStamped::SharedPtr msg)
{
// Store latest message for timer callback use
last_received_pose_ = msg;
}

void timer_callback()
{
    if (last_received_pose_)
    {
        auto odometry_msg = px4_msgs::msg::VehicleOdometry();
        odometry_msg.timestamp = std::chrono::time_point_cast<std::chrono::microseconds>(
            std::chrono::steady_clock::now()).time_since_epoch().count();

        // Manually convert from ENU to NED for position
        odometry_msg.position[0] = last_received_pose_->pose.position.y;
        odometry_msg.position[1] = last_received_pose_->pose.position.x;
        odometry_msg.position[2] = -last_received_pose_->pose.position.z;

        // Manually convert from ENU to NED for orientation
        odometry_msg.q[0] = last_received_pose_->pose.orientation.w;
        odometry_msg.q[1] = last_received_pose_->pose.orientation.y;
        odometry_msg.q[2] = last_received_pose_->pose.orientation.x;
        odometry_msg.q[3] = -last_received_pose_->pose.orientation.z;

        // Assuming you don't have the velocity info, set them to NaN
        odometry_msg.velocity[0] = std::nan("");
        odometry_msg.velocity[1] = std::nan("");
        odometry_msg.velocity[2] = std::nan("");

        RCLCPP_INFO(this->get_logger(), "Publishing Vehicle Odometry: timestamp: %lu, position: [%f, %f, %f], orientation: [%f, %f, %f, %f]",
                    odometry_msg.timestamp, odometry_msg.position[0], odometry_msg.position[1], odometry_msg.position[2],
                    odometry_msg.q[0], odometry_msg.q[1], odometry_msg.q[2], odometry_msg.q[3]);

        publisher_->publish(odometry_msg);
    }
}

rclcpp::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr subscription_;
rclcpp::Publisher<px4_msgs::msg::VehicleOdometry>::SharedPtr publisher_;
rclcpp::TimerBase::SharedPtr timer_;
geometry_msgs::msg::PoseStamped::SharedPtr last_received_pose_ = nullptr;

};

int main(int argc, char *argv)
{
std::cout << “Starting optitrack_to_odometry_mapper node…” << std::endl;
setvbuf(stdout, NULL, _IONBF, BUFSIZ);
rclcpp::init(argc, argv);
rclcpp::spin(std::make_shared());

rclcpp::shutdown();
return 0;

}

The orientation transformation is incorrect. You can use le frame_transform library of px4_ros_com to transform the frames.
Please see this discussion for instructions:

In particular:

  1. I convert the position with ned_to_enu_local_frame or enu_to_ned_local_frame. In both cases, the argument (position) must be Eigen::Vector3d
  2. I convert the orientation with px4_to_ros_orientation or ros_to_px4_orientation. In both cases, the argument (orientation) must be Eigen::Quaterniond

Hello, I’ve been trying to achieve the same thing, but in my case, I’m sending GPS data as in latitude and longitude. I can successfully see that my message is received by PX4, as I see the GPS_RAW_INT message appear in Mavlink Inspector in QGC, but global position estimation is not there. Is there an additional step in that process?

1 Like

Hey, could you please elaborate on “sending GPS data” ? Are you publishing GPS lat and long using a ros2 node?
I am trying to get GPS data from pixhawk to raspberry pi 3b ( Ubuntu 22.04.4, ros2 humble, uxrce-dds, px4 1.14.0 ). When connected using a fdti chip I can see the data published on some topics like /fmu/out/sensor_combined, vehicle_attitude, etc using ros2 topic echo command. But on /vehicle_gps_position no data is being published.
The message is being sent from px4 as I can see data when I run ‘listener vehicle_gps_position’ on nsh terminal on pixhawk

Hello, i have had the same problems ( some topics was present, some no) , you need to compile the correct verion off px4_msg, if you are using the stable version off px4 you need to change the branch of px4_msgs and compile the 1.14 version , now i can echo all topics witout problem.