.. _program_listing_file_include_hri_feature_tracker.hpp: Program Listing for File feature_tracker.hpp ============================================ |exhale_lsh| :ref:`Return to documentation for file ` (``include/hri/feature_tracker.hpp``) .. |exhale_lsh| unicode:: U+021B0 .. UPWARDS ARROW WITH TIP LEFTWARDS .. code-block:: cpp // 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 #include #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 { 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 virtual std::optional transform() const { return transformFromReference(frame()); } bool operator<(const FeatureTracker & other) const {return id() < other.id();} protected: std::optional 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_