Program Listing for File feature_tracker.hpp

↰ Return to documentation for file (include/hri/feature_tracker.hpp)

// Copyright (c) 2023 PAL Robotics S.L. All rights reserved.
//
// 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.

#ifndef HRI__FEATURE_TRACKER_HPP_
#define HRI__FEATURE_TRACKER_HPP_

#include <optional>
#include <string>

#include "geometry_msgs/msg/transform_stamped.hpp"
#include "rclcpp/rclcpp.hpp"
#include "tf2_ros/buffer.h"

#include "hri/types.hpp"

namespace hri
{

class FeatureTracker : public std::enable_shared_from_this<FeatureTracker>
{
public:
  explicit FeatureTracker(
    ID id,
    std::string feature_ns,
    std::string feature_tf_prefix,
    NodeInterfaces node_interfaces,
    rclcpp::CallbackGroup::SharedPtr callback_group,
    const tf2::BufferCore & tf_buffer,
    const std::string & reference_frame);

  virtual ~FeatureTracker() = default;

  // forbids copies of our 'feature trackers', as we need to internally manage
  // if/when they disappear. Instead, access them via shared pointers to const (cf HRIListener API).
  FeatureTracker(const FeatureTracker &) = delete;

  ID id() const {return kId_;}

  std::string ns() const {return kNs_;}

  std::string frame() const {return kFrame_;}

  bool valid() const {return valid_;}

  // NOLINTNEXTLINE(build/include_what_you_use): false positive requiring #include <algorithm>
  virtual std::optional<geometry_msgs::msg::TransformStamped> transform() const
  {
    return transformFromReference(frame());
  }

  bool operator<(const FeatureTracker & other) const {return id() < other.id();}

protected:
  std::optional<geometry_msgs::msg::TransformStamped> transformFromReference(
    std::string frame_name) const;

  void invalidate() {valid_ = false;}

  const ID kId_;
  const std::string kNs_;  // topic namespace under which this feature is advertised
  const std::string kFrame_;

  NodeInterfaces node_interfaces_;
  rclcpp::CallbackGroup::SharedPtr callback_group_;

  const tf2::BufferCore & tf_buffer_;
  const std::string & reference_frame_;

  bool valid_;
};

}  // namespace hri

#endif  // HRI__FEATURE_TRACKER_HPP_