navigation: adds example usage of local navigation interface

examples/cpp/navigation/local_navigation/CMakeLists.txt

@@ -0,0 +1,34 @@
+cmake_minimum_required(VERSION 3.5)
+project(example_local_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_local_navigation_cpp
+        src/main.cpp)
+ament_target_dependencies(example_local_navigation_cpp Eigen3 px4_ros2_cpp rclcpp)
+
+install(TARGETS
+        example_local_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/local_navigation/include/local_navigation.hpp

@@ -0,0 +1,80 @@
+/****************************************************************************
+ * Copyright (c) 2023 PX4 Development Team.
+ * SPDX-License-Identifier: BSD-3-Clause
+ ****************************************************************************/
+#pragma once
+
+
+#include <rclcpp/rclcpp.hpp>
+#include <px4_ros2/common/context.hpp>
+#include <px4_ros2/navigation/experimental/local_navigation_interface.hpp>
+
+#include <Eigen/Core>
+
+using namespace std::chrono_literals; // NOLINT
+
+static const std::string kName = "local_navigation_test";
+static const std::string kNodeName = "example_local_navigation_node";
+
+class LocalNavigationTest : public px4_ros2::Context
+{
+public:
+  explicit LocalNavigationTest(rclcpp::Node & node)
+  : Context(node), _node(node)
+  {
+    const uint8_t pose_frame = 0;
+    const uint8_t velocity_frame = 0;
+
+    _local_navigation_interface = std::make_shared<px4_ros2::LocalNavigationInterface>(
+      *this,
+      pose_frame,
+      velocity_frame);
+
+  }
+
+  void updateAuxLocalPosition()
+  {
+    px4_ros2::LocalPositionEstimate local_position_estimate {};
+
+    local_position_estimate.timestamp_sample = _node.get_clock()->now().nanoseconds() * 1e-3;
+    local_position_estimate.velocity_xy = Eigen::Vector2f{1.f, 2.f};
+    local_position_estimate.velocity_xy_variance = Eigen::Vector2f{0.3f, 0.4f};
+
+    _local_navigation_interface->update(local_position_estimate);
+  }
+
+private:
+  rclcpp::Node & _node;
+  std::shared_ptr<px4_ros2::LocalNavigationInterface> _local_navigation_interface;
+};
+
+class TestNode : public rclcpp::Node
+{
+public:
+  TestNode()
+  : Node(kNodeName)
+  {
+    // 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();
+    }
+
+    _local_navigation_test = std::make_unique<LocalNavigationTest>(*this);
+
+    RCLCPP_INFO(get_logger(), "TestNode running!");
+
+    int counter = 10;
+    while (counter-- > 0) {
+      _local_navigation_test->updateAuxLocalPosition();
+      sleep(1);
+    }
+
+  }
+
+private:
+  std::unique_ptr<LocalNavigationTest> _local_navigation_test;
+};

examples/cpp/navigation/local_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_local_navigation_cpp</name>
+  <version>0.0.1</version>
+  <description>Example navigation: local 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/local_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 <local_navigation.hpp>
+
+
+int main(int argc, char * argv[])
+{
+  rclcpp::init(argc, argv);
+  rclcpp::spin(std::make_shared<TestNode>());
+  rclcpp::shutdown();
+  return 0;
+}

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

@@ -9,6 +9,7 @@
 #include <px4_msgs/msg/vehicle_odometry.hpp>
 #include <rclcpp/rclcpp.hpp>
 #include <Eigen/Eigen>
+#include <px4_ros2/common/context.hpp>
 
 using namespace Eigen;
 using AuxLocalPosition = px4_msgs::msg::VehicleOdometry;
@@ -37,17 +38,18 @@ struct LocalPositionEstimate
   std::optional<Vector3f> attitude_variance {std::nullopt};
 };
 
-class LocalNavigationInterface : public rclcpp::Node
+class LocalNavigationInterface
 {
 public:
-  explicit LocalNavigationInterface(uint8_t pose_frame, uint8_t velocity_frame);
-  ~LocalNavigationInterface();
+  explicit LocalNavigationInterface(Context & context, uint8_t pose_frame, uint8_t velocity_frame);
+  ~LocalNavigationInterface() = default;
 
   void update(LocalPositionEstimate & local_position_estimate);
 
 private:
   const std::string AUX_LOCAL_POSITION_TOPIC = "/fmu/in/vehicle_odometry";
 
+  rclcpp::Node & _node;
   rclcpp::Publisher<AuxLocalPosition>::SharedPtr _aux_local_position_pub;
 
   uint8_t _pose_frame;

px4_ros2_cpp/src/navigation/experimental/local_navigation_interface.cpp

@@ -3,17 +3,24 @@
  * SPDX-License-Identifier: BSD-3-Clause
  ****************************************************************************/
 
+#include <rclcpp/rclcpp.hpp>
 #include <px4_ros2/navigation/experimental/local_navigation_interface.hpp>
 
-
 namespace px4_ros2
 {
 
-LocalNavigationInterface::LocalNavigationInterface(uint8_t pose_frame, uint8_t velocity_frame)
-: Node("local_navigation_interface_node"),
-  _aux_local_position_pub(create_publisher<AuxLocalPosition>(AUX_LOCAL_POSITION_TOPIC, 10)),
+LocalNavigationInterface::LocalNavigationInterface(
+  Context & context, uint8_t pose_frame,
+  uint8_t velocity_frame)
+: _node(context.node()),
   _pose_frame(pose_frame),
-  _velocity_frame(velocity_frame) {}
+  _velocity_frame(velocity_frame)
+{
+  _aux_local_position_pub =
+    context.node().create_publisher<AuxLocalPosition>(
+    context.topicNamespacePrefix() + AUX_LOCAL_POSITION_TOPIC, 10);
+
+}
 
 void LocalNavigationInterface::update(LocalPositionEstimate & local_position_estimate)
 {
@@ -66,8 +73,10 @@ void LocalNavigationInterface::update(LocalPositionEstimate & local_position_est
     local_position_estimate.velocity_z_variance.value_or(NAN);
 
   // Publish
-  aux_local_position.timestamp = this->now().nanoseconds() * 1e-3;
+  aux_local_position.timestamp = _node.get_clock()->now().nanoseconds() * 1e-3;
   _aux_local_position_pub->publish(aux_local_position);
+
+  std::cout << "Published aux local position!\n";
 }
 
 } // namespace px4_ros2