send_vehicle_command.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 <functional>
#include <memory>
#include <string>
#include <utility>
#include <vector>
 
#include "auto_apms_behavior_tree_core/node.hpp"
#include "auto_apms_behavior_tree_core/node/base/ros_action_node_base.hpp"
#include "px4_msgs/msg/vehicle_command.hpp"
#include "px4_msgs/msg/vehicle_command_ack.hpp"
#include "rclcpp/rclcpp.hpp"
 
namespace auto_apms_px4
{

class SendVehicleCommand : public auto_apms_behavior_tree::core::RosActionNodeBase
{
public:
  static constexpr auto PORT_KEY_COMMAND = "command";
  static constexpr auto PORT_KEY_PARAM1 = "param1";
  static constexpr auto PORT_KEY_PARAM2 = "param2";
  static constexpr auto PORT_KEY_PARAM3 = "param3";
  static constexpr auto PORT_KEY_PARAM4 = "param4";
  static constexpr auto PORT_KEY_PARAM5 = "param5";
  static constexpr auto PORT_KEY_PARAM6 = "param6";
  static constexpr auto PORT_KEY_PARAM7 = "param7";
  static constexpr auto PORT_KEY_CONFIRMATION = "confirmation";
  static constexpr auto PORT_KEY_TARGET_SYSTEM = "target_system";
  static constexpr auto PORT_KEY_TARGET_COMPONENT = "target_component";
 
  SendVehicleCommand(const std::string & instance_name, const Config & config, const Context & context);
 
  ~SendVehicleCommand() override;
 
  static BT::PortsList providedPorts();
 
  BT::NodeStatus tick() override;
 
  void halt() override;

  virtual bool setMessage(px4_msgs::msg::VehicleCommand & msg);
 
protected:
  uint16_t resolveSourceComponent();
 
private:
  struct AckSubscription
  {
    using Ack = px4_msgs::msg::VehicleCommandAck;
 
    AckSubscription(
      rclcpp::Node::SharedPtr node, rclcpp::CallbackGroup::SharedPtr group, const std::string & topic_name);
 
    void addListener(const void * owner, std::function<void(const Ack::SharedPtr &)> callback);
    void removeListener(const void * owner);
    void broadcast(const Ack::SharedPtr & msg);
 
    rclcpp::Subscription<Ack>::SharedPtr subscription;
    std::vector<std::pair<const void *, std::function<void(const Ack::SharedPtr &)>>> listeners;
    std::string name;
  };

  std::string resolveCommandTopicName();

  rclcpp::Publisher<px4_msgs::msg::VehicleCommand>::SharedPtr command_pub_;
  std::shared_ptr<AckSubscription> ack_sub_;

  px4_msgs::msg::VehicleCommand tx_msg_;
  std::shared_ptr<px4_msgs::msg::VehicleCommandAck> last_ack_;
  bool waiting_for_ack_ = false;
  rclcpp::Time send_time_;
  double ack_timeout_s_;
};
 
}  // namespace auto_apms_px4