navigation: adds global navigation interface implementation, adds glo…

…bal interface usage node

examples/cpp/navigation/global_navigation/CMakeLists.txt

@@ -0,0 +1,34 @@
+cmake_minimum_required(VERSION 3.5)
+project(example_global_navigation_cpp)
+
+if(NOT CMAKE_CXX_STANDARD)
+    set(CMAKE_CXX_STANDARD 17)
+endif()
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+    add_compile_options(-Wall -Wextra -Wpedantic -Werror -Wno-unused-parameter)
+endif()
+
+set(CMAKE_EXPORT_COMPILE_COMMANDS ON)
+
+find_package(eigen3_cmake_module REQUIRED)
+find_package(Eigen3 REQUIRED)
+find_package(ament_cmake REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(px4_ros2_cpp REQUIRED)
+
+include_directories(include ${Eigen3_INCLUDE_DIRS})
+add_executable(example_global_navigation_cpp
+        src/main.cpp)
+ament_target_dependencies(example_global_navigation_cpp Eigen3 px4_ros2_cpp rclcpp)
+
+install(TARGETS
+        example_global_navigation_cpp
+        DESTINATION lib/${PROJECT_NAME})
+
+if(BUILD_TESTING)
+    find_package(ament_lint_auto REQUIRED)
+    ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_package()

examples/cpp/navigation/global_navigation/include/global_navigation.hpp

@@ -0,0 +1,56 @@
+/****************************************************************************
+ * Copyright (c) 2023 PX4 Development Team.
+ * SPDX-License-Identifier: BSD-3-Clause
+ ****************************************************************************/
+#pragma once
+
+
+#include <rclcpp/rclcpp.hpp>
+#include <px4_ros2/navigation/experimental/global_navigation_interface.hpp>
+
+#include <Eigen/Core>
+
+using namespace std::chrono_literals; // NOLINT
+
+class ExampleGlobalNavigationNode : public rclcpp::Node
+{
+public:
+  ExampleGlobalNavigationNode()
+  : Node("example_global_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();
+    }
+
+    // Instantiate global navigation interface
+    _global_navigation_interface = std::make_shared<px4_ros2::GlobalNavigationInterface>(*this);
+
+    _timer =
+      create_wall_timer(1s, std::bind(&ExampleGlobalNavigationNode::updateAuxGlobalPosition, this));
+
+    RCLCPP_INFO(get_logger(), "example_global_navigation_node running!");
+  }
+
+  void updateAuxGlobalPosition()
+  {
+    px4_ros2::GlobalPositionEstimate global_position_estimate {};
+
+    global_position_estimate.timestamp_sample = this->now().nanoseconds() * 1e-3;
+
+    global_position_estimate.position_lat_lon = Eigen::Vector2f {12.34321, 23.45432};
+    global_position_estimate.position_variance = 0.4f;
+
+    global_position_estimate.altitude_agl = 12.4f;
+
+    _global_navigation_interface->update(global_position_estimate);
+  }
+
+private:
+  std::shared_ptr<px4_ros2::GlobalNavigationInterface> _global_navigation_interface;
+  rclcpp::TimerBase::SharedPtr _timer;
+};

examples/cpp/navigation/global_navigation/package.xml

@@ -0,0 +1,26 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>example_global_navigation_cpp</name>
+  <version>0.0.1</version>
+  <description>Example navigation: global navigation</description>
+  <maintainer email="[email protected]">Beat Kueng</maintainer>
+  <license>BSD-3-Clause</license>
+
+  <buildtool_depend>eigen3_cmake_module</buildtool_depend>
+  <buildtool_depend>ament_cmake</buildtool_depend>
+  <buildtool_export_depend>eigen3_cmake_module</buildtool_export_depend>
+
+  <build_depend>eigen</build_depend>
+  <build_depend>rclcpp</build_depend>
+  <build_export_depend>eigen</build_export_depend>
+
+  <depend>px4_ros2_cpp</depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

examples/cpp/navigation/global_navigation/src/main.cpp

@@ -0,0 +1,17 @@
+/****************************************************************************
+ * Copyright (c) 2023 PX4 Development Team.
+ * SPDX-License-Identifier: BSD-3-Clause
+ ****************************************************************************/
+
+#include "rclcpp/rclcpp.hpp"
+
+#include <global_navigation.hpp>
+
+
+int main(int argc, char * argv[])
+{
+  rclcpp::init(argc, argv);
+  rclcpp::spin(std::make_shared<ExampleGlobalNavigationNode>());
+  rclcpp::shutdown();
+  return 0;
+}

examples/cpp/navigation/local_navigation/include/local_navigation.hpp

@@ -6,7 +6,6 @@
 
 
 #include <rclcpp/rclcpp.hpp>
-#include <px4_ros2/common/context.hpp>
 #include <px4_ros2/navigation/experimental/local_navigation_interface.hpp>
 
 #include <Eigen/Core>

px4_ros2_cpp/include/px4_ros2/navigation/experimental/global_navigation_interface.hpp

@@ -22,12 +22,19 @@ struct GlobalPositionEstimate
 {
   uint64_t timestamp_sample {};
 
+  // Lon lat
+  std::optional<Vector2f> position_lat_lon {std::nullopt};
+  std::optional<float> position_variance {std::nullopt};
+
+  // Altitude (AGL frame)
+  std::optional<float> altitude_agl {std::nullopt};
+  // std::optional<float> altitude_variance {std::nullopt};
 };
 
 class GlobalNavigationInterface
 {
 public:
-  explicit GlobalNavigationInterface(rclcpp::Node & node, uint8_t altitude_frame);
+  explicit GlobalNavigationInterface(rclcpp::Node & node);
   ~GlobalNavigationInterface() = default;
 
   /**
@@ -38,10 +45,25 @@ class GlobalNavigationInterface
   const std::string AUX_GLOBAL_POSITION_TOPIC = "/fmu/in/aux_global_position";
 
 private:
+  /**
+   * @brief Check that at least one estimate value is defined.
+   */
+  bool _checkEstimateEmpty(const GlobalPositionEstimate & estimate) const;
+
+  /**
+   * @brief Check that if an estimate value is defined, its variance is also defined and strictly greater than zero.
+   */
+  bool _checkVarianceValid(const GlobalPositionEstimate & estimate) const;
+
+  /**
+   * @brief Check that if an estimate value is defined, its associated frame is not *FRAME_UNKNOWN.
+   */
+  bool _checkFrameValid(const GlobalPositionEstimate & estimate) const;
+
   rclcpp::Node & _node;
   rclcpp::Publisher<AuxGlobalPosition>::SharedPtr _aux_global_position_pub;
 
-  uint8_t _altitude_frame;
+  // uint8_t _altitude_frame;
 };
 
 } // namespace px4_ros2

px4_ros2_cpp/src/navigation/experimental/global_navigation_interface.cpp

@@ -9,25 +9,74 @@
 namespace px4_ros2
 {
 
-GlobalNavigationInterface::GlobalNavigationInterface(rclcpp::Node & node, uint8_t altitude_frame)
-: _node(node),
-  _altitude_frame(altitude_frame)
+GlobalNavigationInterface::GlobalNavigationInterface(rclcpp::Node & node)
+: _node(node)
 {
   _aux_global_position_pub =
     node.create_publisher<AuxGlobalPosition>(AUX_GLOBAL_POSITION_TOPIC, 10);
 }
 
 int GlobalNavigationInterface::update(const GlobalPositionEstimate & global_position_estimate) const
 {
-  AuxGlobalPosition aux_global_position;
+  // Run basic sanity checks on global position estimate
+  if (_checkEstimateEmpty(global_position_estimate)) {
+    RCLCPP_WARN(_node.get_logger(), "Estimate values are all empty.");
+    return static_cast<int>(NavigationInterfaceCodes::ESTIMATE_EMPTY);
+  }
+
+  if (!_checkVarianceValid(global_position_estimate)) {
+    return static_cast<int>(NavigationInterfaceCodes::ESTIMATE_VARIANCE_INVALID);
+  }
+
+  // Populate aux global position
+  px4_msgs::msg::AuxGlobalPosition aux_global_position;
 
   aux_global_position.timestamp_sample = global_position_estimate.timestamp_sample;
 
+  // Lat lon
+  const Vector2f position_lat_lon = global_position_estimate.position_lat_lon.value_or(
+    Vector2f{NAN,
+      NAN});
+  aux_global_position.latitude = position_lat_lon[0];
+  aux_global_position.longitude = position_lat_lon[1];
+  aux_global_position.positional_uncertainty = global_position_estimate.position_variance.value_or(
+    NAN);
+
+  // Altitude
+  aux_global_position.altitude_agl = global_position_estimate.altitude_agl.value_or(NAN);
+
+  // Valid flag
+  aux_global_position.valid = true;
+
   // Publish
   aux_global_position.timestamp = _node.get_clock()->now().nanoseconds() * 1e-3;
   _aux_global_position_pub->publish(aux_global_position);
 
   return static_cast<int>(NavigationInterfaceCodes::SUCCESS);
 }
 
+bool GlobalNavigationInterface::_checkEstimateEmpty(const GlobalPositionEstimate & estimate) const
+{
+  return !estimate.position_lat_lon.has_value() && !estimate.altitude_agl.has_value();
+}
+
+bool GlobalNavigationInterface::_checkVarianceValid(const GlobalPositionEstimate & estimate) const
+{
+  if (estimate.position_lat_lon.has_value() &&
+    (!estimate.position_variance.has_value() || estimate.position_variance.value() <= 0))
+  {
+    RCLCPP_WARN(
+      _node.get_logger(),
+      "Estimate value position_lat_lon has an invalid variance value.");
+    return false;
+  }
+
+  return true;
+}
+
+bool GlobalNavigationInterface::_checkFrameValid(const GlobalPositionEstimate & estimate) const
+{
+  return true;
+}
+
 } // namespace px4_ros2

px4_ros2_cpp/src/navigation/experimental/local_navigation_interface.cpp

@@ -16,7 +16,7 @@ LocalNavigationInterface::LocalNavigationInterface(
   _pose_frame(pose_frame),
   _velocity_frame(velocity_frame)
 {
-  // Check that pose and velocity reference frames are valid
+  // Check that selected pose and velocity reference frames are valid
   auto it_pose_frame = std::find(
     std::begin(_available_pose_frames), std::end(
       _available_pose_frames), pose_frame);