mode_registration.hpp

// Copyright 2024 Robin Müller
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//      http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
 
#pragma once
 
#include <cstdint>
#include <memory>
#include <string>
#include <type_traits>
 
#include "auto_apms_px4_interfaces/msg/registered_mode.hpp"
#include "px4_ros2/components/mode.hpp"
#include "px4_ros2/components/mode_executor.hpp"
#include "rclcpp/rclcpp.hpp"
 
namespace auto_apms_px4
{

class ModeRegistrationHandler
{
public:
  static constexpr auto TOPIC_NAME = "registered_modes";

  explicit ModeRegistrationHandler(rclcpp::Node::SharedPtr node_ptr);

  ~ModeRegistrationHandler();

  void registerMode(px4_ros2::ModeBase & mode, const std::string & mode_name);

  void registerMode(px4_ros2::ModeExecutorBase & executor, const std::string & mode_name);

  void stopModeAnnouncement();
 
private:
  void waitForFmu();

  void announce(const std::string & mode_name, uint8_t nav_state);
 
  const rclcpp::Node::SharedPtr node_ptr_;
  auto_apms_px4_interfaces::msg::RegisteredMode registered_mode_msg_;
  rclcpp::Publisher<auto_apms_px4_interfaces::msg::RegisteredMode>::SharedPtr registered_mode_pub_;
  rclcpp::TimerBase::SharedPtr announce_timer_;
};

template <class ModeT>
class ModeRegistrationFactory
{
public:
  ModeRegistrationFactory(const std::string & mode_name, const rclcpp::NodeOptions & options);
 
  rclcpp::node_interfaces::NodeBaseInterface::SharedPtr get_node_base_interface();
 
private:
  rclcpp::Node::SharedPtr node_ptr_;
  std::unique_ptr<ModeT> mode_ptr_;
  ModeRegistrationHandler registration_handler_;
};
 
// #####################################################################################################################
// ################################              DEFINITIONS              ##############################################
// #####################################################################################################################
 
template <class ModeT>
ModeRegistrationFactory<ModeT>::ModeRegistrationFactory(
  const std::string & mode_name, const rclcpp::NodeOptions & options)
: node_ptr_(std::make_shared<rclcpp::Node>(mode_name + "_node", options)),
  mode_ptr_(std::make_unique<ModeT>(*node_ptr_, px4_ros2::ModeBase::Settings(mode_name))),
  registration_handler_(node_ptr_)
{
  static_assert(
    std::is_base_of<px4_ros2::ModeBase, ModeT>::value,
    "Template argument ModeT must publicly inherit px4_ros2::ModeBase.");
 
  registration_handler_.registerMode(*mode_ptr_, mode_name);
}
 
template <class ModeT>
rclcpp::node_interfaces::NodeBaseInterface::SharedPtr ModeRegistrationFactory<ModeT>::get_node_base_interface()
{
  return node_ptr_->get_node_base_interface();
}
 
}  // namespace auto_apms_px4