get_mode_nav_state.cpp

// 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.
 
#include <cstdint>
#include <map>
#include <memory>
#include <string>
 
#include "auto_apms_behavior_tree_core/node.hpp"
#include "auto_apms_px4_interfaces/msg/registered_mode.hpp"
#include "px4_msgs/msg/vehicle_status.hpp"
 
#define INPUT_KEY_MODE_NAME "mode_name"
#define OUTPUT_KEY_NAV_STATE "nav_state"
 
namespace auto_apms_px4
{

class GetModeNavState
: public auto_apms_behavior_tree::core::RosSubscriberNode<auto_apms_px4_interfaces::msg::RegisteredMode>
{
  static std::map<std::string, uint8_t> standardModes()
  {
    using VehicleStatus = px4_msgs::msg::VehicleStatus;
    return {
      {"Manual", VehicleStatus::NAVIGATION_STATE_MANUAL},
      {"Altitude", VehicleStatus::NAVIGATION_STATE_ALTCTL},
      {"Position", VehicleStatus::NAVIGATION_STATE_POSCTL},
      {"Mission", VehicleStatus::NAVIGATION_STATE_AUTO_MISSION},
      {"Hold", VehicleStatus::NAVIGATION_STATE_AUTO_LOITER},
      {"RTL", VehicleStatus::NAVIGATION_STATE_AUTO_RTL},
      {"Acro", VehicleStatus::NAVIGATION_STATE_ACRO},
      {"Descend", VehicleStatus::NAVIGATION_STATE_DESCEND},
      {"Termination", VehicleStatus::NAVIGATION_STATE_TERMINATION},
      {"Offboard", VehicleStatus::NAVIGATION_STATE_OFFBOARD},
      {"Stabilized", VehicleStatus::NAVIGATION_STATE_STAB},
      {"Takeoff", VehicleStatus::NAVIGATION_STATE_AUTO_TAKEOFF},
      {"Land", VehicleStatus::NAVIGATION_STATE_AUTO_LAND},
      {"FollowTarget", VehicleStatus::NAVIGATION_STATE_AUTO_FOLLOW_TARGET},
      {"PrecisionLand", VehicleStatus::NAVIGATION_STATE_AUTO_PRECLAND},
      {"Orbit", VehicleStatus::NAVIGATION_STATE_ORBIT},
      {"VtolTakeoff", VehicleStatus::NAVIGATION_STATE_AUTO_VTOL_TAKEOFF},
    };
  }
 
  // Seeded with the builtin modes; announcements from custom mode components are merged in on tick.
  std::map<std::string, uint8_t> known_modes_ = standardModes();
 
public:
  GetModeNavState(
    const std::string & instance_name, const BT::NodeConfig & config,
    const auto_apms_behavior_tree::core::RosNodeContext & context)
  : RosSubscriberNode{instance_name, config, context, rclcpp::QoS(10).transient_local()}
  {
  }
 
  static BT::PortsList providedPorts()
  {
    return providedBasicPorts({
      BT::InputPort<std::string>(INPUT_KEY_MODE_NAME, "Name of the registered PX4 mode to resolve."),
      BT::OutputPort<int>(OUTPUT_KEY_NAV_STATE, "Resolved PX4 navigation state (mode id) of the named mode."),
    });
  }
 
  BT::NodeStatus onTick(const std::shared_ptr<auto_apms_px4_interfaces::msg::RegisteredMode> & last_msg_ptr) final
  {
    // Accumulate the latest announcement (each mode component publishes its own mapping on the shared topic).
    if (last_msg_ptr) known_modes_[last_msg_ptr->name] = last_msg_ptr->nav_state;
 
    const BT::Expected<std::string> expected_mode_name = getInput<std::string>(INPUT_KEY_MODE_NAME);
    if (!expected_mode_name) {
      RCLCPP_ERROR(
        logger_, "%s - Missing required input '%s': %s", context_.getFullyQualifiedTreeNodeName(this).c_str(),
        INPUT_KEY_MODE_NAME, expected_mode_name.error().c_str());
      return BT::NodeStatus::FAILURE;
    }
 
    const auto it = known_modes_.find(expected_mode_name.value());
    if (it == known_modes_.end()) {
      // Not a builtin mode and not announced (yet) - retry to keep waiting for a custom mode to register.
      return BT::NodeStatus::FAILURE;
    }
 
    setOutput(OUTPUT_KEY_NAV_STATE, static_cast<int>(it->second));
    return BT::NodeStatus::SUCCESS;
  }
};
 
}  // namespace auto_apms_px4
 
AUTO_APMS_BEHAVIOR_TREE_REGISTER_NODE(auto_apms_px4::GetModeNavState)