Support sending GPS_GLOBAL_ORIGIN and DO_SET_HOME commands

[tedlin0913]

Hello!
Thank you for this great work. My goal is to use this library together with VIO for position estimation in order to fly my drone in GNSS-denied areas. To achieve this, I followed the px4 guide on enable auto mode with local position, which suggests sending the VEHICLE_CMD_SET_GPS_GLOBAL_ORIGIN command. In addition, VEHICLE_CMD_DO_SET_HOME can be used to set the home position once the global position estimate is available.
I am now trying to figure out how to send these two VehicleCommands correctly using this library. So far, I have attempted to send the commands via sendCommandSync in my mode executor ModeExecutorTest, which inherits from ModeExecutorBase. The functions I written for sending the command looks like this:

void ModeExecutorTest::setHome(const CompletedCallback &on_completed)
{
    if (is_home_set_)
    {
        on_completed(Result::Success);
        return;
    }
    // TODO: check if global position is available before sending the command? 
    const Result result = sendCommandSync(
        px4_msgs::msg::VehicleCommand::VEHICLE_CMD_DO_SET_HOME,
        1.f);

    // TODO: maybe subscribe HomePosition.msg to check if home position is availible?

    if (result != Result::Success)
    {
        on_completed(result);
        return;
    }
}

void ModeExecutorTest::setGlobalOrigin(const CompletedCallback &on_completed)
{
    if (is_origin_set_)
    {
        on_completed(Result::Success);
        return;
    }
   
    // VEHICLE_CMD_SET_GPS_GLOBAL_ORIGIN does not send back a VehicleCommandAck.
    sendCommandSync(
        px4_msgs::msg::VehicleCommand::VEHICLE_CMD_SET_GPS_GLOBAL_ORIGIN,
        NAN, NAN, NAN, NAN,
        0.0, 0.0, 0.0);
}

VEHICLE_CMD_DO_SET_HOME sends back a VehicleCommandAck, whereas VEHICLE_CMD_SET_GPS_GLOBAL_ORIGIN does not.

I plan to use these commands in runState. Two follow up questions occur:

1. How can I check, inside the mode executor, when the global position estimate becomes available, before setting the home position?
2. How can I subscribe to HomePosition.msg to detect when the home position becomes available, and then set on_completed to Result::Success using this library?

[tedlin0913]

After some trial and error, I managed to get it to work. I’m not entirely sure if this is the recommended way to do so.
If it is, I would be happy to create a PR and maybe add it to the examples or directly to ModeExecutorBase.

#include "executor.hpp"

ModeExecutorTest::ModeExecutorTest(
    rclcpp::Node &node,
    px4_ros2::ModeBase &owned_mode)
    : ModeExecutorBase(
          node,
          px4_ros2::ModeExecutorBase::Settings{
              px4_ros2::ModeExecutorBase::Settings::Activation::ActivateAlways},
          owned_mode),
      node_(node),
      is_origin_set_(false),
      is_home_set_(false),
      got_global_origin_valid_(false),
      got_home_position_valid_(false)
{

    global_position_sub_ =
        node.create_subscription<px4_msgs::msg::VehicleGlobalPosition>(
            "fmu/out/vehicle_global_position" +
                px4_ros2::getMessageNameVersion<px4_msgs::msg::VehicleGlobalPosition>(),
            rclcpp::QoS(1).best_effort(),
            [this](px4_msgs::msg::VehicleGlobalPosition::UniquePtr msg)
            {
                if (!got_global_origin_valid_ &&
                    !std::isnan(msg->lat) && !std::isnan(msg->lon))
                {
                    got_global_origin_valid_ = true;
                    // cancel the timeout and report success
                    global_origin_timeout_timer_->cancel();
                    on_global_origin_callback_(px4_ros2::Result::Success);
                }
            });

    home_position_sub_ = node.create_subscription<px4_msgs::msg::HomePosition>(
        "fmu/out/home_position" +
            px4_ros2::getMessageNameVersion<px4_msgs::msg::HomePosition>(),
        rclcpp::QoS(1).best_effort(),
        [this](px4_msgs::msg::HomePosition::UniquePtr msg)
        {
            if (!got_home_position_valid_ && msg->valid_hpos)
            {
                got_home_position_valid_ = true;
                home_position_timeout_timer_->cancel();
                on_home_position_callback_(px4_ros2::Result::Success);
            }
        });
}

void ModeExecutorTest::onActivate()
{
    runState(State::SetGlobal, px4_ros2::Result::Success);
}

void ModeExecutorTest::runState(State state, px4_ros2::Result previous_result)
{
// ......

    switch (state)
    {
    case State::SetGlobal:
    {
        setGlobalOrigin([this](px4_ros2::Result result)
                        { runState(State::SetHome, result); });
        break;
    }
    case State::SetHome:
    {
        setHome([this](px4_ros2::Result result)
                { runState(State::WaitReadyToArm, result); });
        break;
    }

//......
    }
}

void ModeExecutorTest::setGlobalOrigin(const CompletedCallback &on_completed)
{
    if (is_origin_set_)
    {
        on_completed(px4_ros2::Result::Success);
        return;
    }

    on_global_origin_callback_ = on_completed;
    got_global_origin_valid_ = false;

    sendCommandSync(
        px4_msgs::msg::VehicleCommand::VEHICLE_CMD_SET_GPS_GLOBAL_ORIGIN,
        NAN, NAN, NAN, NAN, 0.0, 0.0, 0.0);

    global_origin_timeout_timer_ = node_.create_wall_timer(
        3s,
        [this]()
        {
            if (!got_global_origin_valid_)
            {
                got_global_origin_valid_ = true;
                on_global_origin_callback_(px4_ros2::Result::Timeout);
            }
            global_origin_timeout_timer_->cancel();
        });
}

void ModeExecutorTest::setHome(const CompletedCallback &on_completed)
{
    if (is_home_set_)
    {
        on_completed(px4_ros2::Result::Success);
        return;
    }

    on_home_position_callback_ = on_completed;
    got_home_position_valid_ = false;

    px4_ros2::Result cmd_res = sendCommandSync(
        px4_msgs::msg::VehicleCommand::VEHICLE_CMD_DO_SET_HOME,
        1.f);

    home_position_timeout_timer_ = node_.create_wall_timer(
        3s,
        [this]()
        {
            if (!got_home_position_valid_)
            {
                got_home_position_valid_ = true;
                on_home_position_callback_(px4_ros2::Result::Timeout);
            }
            home_position_timeout_timer_->cancel();
        });
}

[bkueng]

Hi
Glad to see you got it to work.
For the global position there's already a class here: https://github.com/Auterion/px4-ros2-interface-lib/blob/main/px4_ros2_cpp/include/px4_ros2/odometry/global_position.hpp. And the home position is here: https://github.com/Auterion/px4-ros2-interface-lib/blob/main/px4_ros2_cpp/include/px4_ros2/vehicle_state/home_position.hpp.

We should also check why VEHICLE_CMD_SET_GPS_GLOBAL_ORIGIN does not return an ack on the PX4 side.

You're welcome to create a PR, but I'd rather not add them to ModeExecutorBase to avoid bloating the base class. Maybe something like a HomePositionSetter class, which takes a ModeExecutorBase reference to send the command.

[tedlin0913]

@bkueng Thank you for the comment!

Glad to see you got it to work.
For the global position there's already a class here: https://github.com/Auterion/px4-ros2-interface-lib/blob/main/px4_ros2_cpp/include/px4_ros2/odometry/global_position.hpp. And the home position is here: https://github.com/Auterion/px4-ros2-interface-lib/blob/main/px4_ros2_cpp/include/px4_ros2/vehicle_state/home_position.hpp.

I am aware that there are existing classes, and I agree that using them would be more consistent.
From the examples, I noticed that shared pointers to these classes are usually created inside the constructor of a class derived from ModeBase. Since I am using sendCommandSync, maybe I should pass in a reference of a custom mode, such as using ownedMode() in this case?

px4-ros2-interface-lib/px4_ros2_cpp/include/px4_ros2/odometry/global_position.hpp

Line 25 in f619f50

explicit OdometryGlobalPosition(Context & context);

We should also check why VEHICLE_CMD_SET_GPS_GLOBAL_ORIGIN does not return an ack on the PX4 side.

After some investigation, I discovered that this feature wasn’t included in v1.15.4 (the version I am using), but it has been fixed in the main branch. (I guess I’ll give v1.16.0-rc a try.)

tag v1.15.4
https://github.com/PX4/PX4-Autopilot/blob/99c40407ffd7ac184e2d7b4b293f36f10fe561ef/src/modules/ekf2/EKF2.cpp#L465C1-L483C1

main
https://github.com/PX4/PX4-Autopilot/blob/cadf69f73c2212e409ab9f8228a077eef914cadf/src/modules/ekf2/EKF2.cpp#L502C1-L532C1

You're welcome to create a PR, but I'd rather not add them to ModeExecutorBase to avoid bloating the base class. Maybe something like a HomePositionSetter class, which takes a ModeExecutorBase reference to send the command.

A Setter class sounds like a great solution—perhaps something like a SetterBase, dedicated to sending commands and receiving command ack?

[bkueng]

Since I am using sendCommandSync, maybe I should pass in a reference of a custom mode, such as using ownedMode() in this case?

Correct.
Or you could add them as a member to your mode and then access that from the ModeExecutor through the owned mode. This is slightly more cumbersome though.

A Setter class sounds like a great solution—perhaps something like a SetterBase, dedicated to sending commands and receiving command ack?

Yes, sounds good.

[bkueng]

@tedlin0913 were you able to do the changes?

[tedlin0913]

Hi @bkueng, I'm planning to create a PR around mid-August when I'll have more time to dedicate to this work. In the meantime, feel free to close this issue if you'd like, as I've already provided some workable examples here that should help others in the interim. Thank you!

[bkueng]

Sounds good, we can keep it open until then.