How to use Local Position Updates?

[XXLiu-HNU]

Hi, this work is vary useful for us to change the position of UAVs from GPS to local position. But when use the code we meet some errors.

BackGround: We use RTK for UAV swarm location. But we don't know how to use the same coordinate for each UAV. Becase each UAV treats the starting position as the coordinate origin. So, we want set the local_position by calculating the latitude and longitude height of the UAV. Methods to try: Fristly, we build the lib and it looks like good, and we change the value of "EKF2_EV_CTRL" from 0 to 15 and we run : ros2 run example_local_navigation_cpp example_local_navigation , and it also looks like good .

But when we use : ros2 topic echo /fmu/out/vehicle_local_position , the locai_position is don`t change:

and here is the code we use, just change px4_ros2::VelocityFrame::LocalNED from to px4_ros2::VelocityFrame::Unkown, and set the position for test, and here is the code:

/****************************************************************************
 * Copyright (c) 2023 PX4 Development Team.
 * SPDX-License-Identifier: BSD-3-Clause
 ****************************************************************************/
#pragma once

#include <rclcpp/rclcpp.hpp>
#include <px4_ros2/navigation/experimental/local_position_measurement_interface.hpp>

using namespace std::chrono_literals; // NOLINT

class LocalNavigationTest : public px4_ros2::LocalPositionMeasurementInterface
{
public:
  explicit LocalNavigationTest(rclcpp::Node & node)
  : LocalPositionMeasurementInterface(node, px4_ros2::PoseFrame::LocalNED,
      px4_ros2::VelocityFrame::Unknown)
  {
    _timer =
      node.create_wall_timer(1s, [this] {updateLocalPosition();});

    RCLCPP_INFO(node.get_logger(), "example_local_navigation_node running!");
  }

  void updateLocalPosition()
  {
    px4_ros2::LocalPositionMeasurement local_position_measurement {};

    local_position_measurement.timestamp_sample = _node.get_clock()->now();

    // local_position_measurement.velocity_xy = Eigen::Vector2f {1.f, 2.f};
    // local_position_measurement.velocity_xy_variance = Eigen::Vector2f {0.3f, 0.4f};

    local_position_measurement.position_z = 12.3f;
    local_position_measurement.position_xy = Eigen::Vector2f {1.f, 2.f};
    local_position_measurement.position_xy_variance = Eigen::Vector2f {0.01f, 0.01f};
    local_position_measurement.position_z_variance = 0.33F;

    local_position_measurement.attitude_quaternion = Eigen::Quaternionf {0.1, -0.2, 0.3, 0.25};
    local_position_measurement.attitude_variance = Eigen::Vector3f {0.2, 0.1, 0.05};

    try {
      update(local_position_measurement);
      RCLCPP_DEBUG(
        _node.get_logger(),
        "Successfully sent position update to navigation interface.");
    } catch (const px4_ros2::NavigationInterfaceInvalidArgument & e) {
      RCLCPP_ERROR_THROTTLE(
        _node.get_logger(),
        *_node.get_clock(), 1000, "Exception caught: %s", e.what());
    }
  }

private:
  rclcpp::TimerBase::SharedPtr _timer;
};

class ExampleLocalNavigationNode : public rclcpp::Node
{
public:
  ExampleLocalNavigationNode()
  : Node("example_local_navigation_node")
  {
    // Enable debug output
    auto ret =
      rcutils_logging_set_logger_level(get_logger().get_name(), RCUTILS_LOG_SEVERITY_DEBUG);

    if (ret != RCUTILS_RET_OK) {
      RCLCPP_ERROR(get_logger(), "Error setting severity: %s", rcutils_get_error_string().str);
      rcutils_reset_error();
    }

    _interface = std::make_unique<LocalNavigationTest>(*this);

    if (!_interface->doRegister()) {
      throw std::runtime_error("Registration failed");
    }
  }

private:
  std::unique_ptr<LocalNavigationTest> _interface;
};

Desired Effect :

when change position_xy and position_z, the UAV's local Position is also change . Hope for useful ideas T-T

[XXLiu-HNU]

We find the position_xy from the topic /fmu/in/vehicle_visual_odometry by read the code:examples/cpp/navigation/local_navigation. By the way, the name of the topic is should be "/xxx/out/xxxx" for is a Publisher?

And how to change the /fmu/out/vehicle_local_position is still confused.

[bkueng]

I'm not quite sure what you want to achieve. If you want to have a reference position between all UAVs, why not use the global position directly?