Program Listing for File hri.hpp

↰ Return to documentation for file (include/hri/hri.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__HRI_HPP_
#define HRI__HRI_HPP_

#include <functional>
#include <map>
#include <memory>
#include <string>
#include <vector>

#include "hri_msgs/msg/ids_list.hpp"
#include "rclcpp/rclcpp.hpp"
#include "tf2_ros/buffer.h"
#include "tf2_ros/transform_listener.h"

#include "hri/body.hpp"
#include "hri/face.hpp"
#include "hri/feature_tracker.hpp"
#include "hri/person.hpp"
#include "hri/types.hpp"
#include "hri/voice.hpp"

namespace hri
{

class HRIListener : public std::enable_shared_from_this<HRIListener>
{
public:
  [[nodiscard]] static std::shared_ptr<HRIListener> create(NodeLikeSharedPtr node_like)
  {
    return std::shared_ptr<HRIListener>(new HRIListener(node_like));
  }

  virtual ~HRIListener();

  std::map<ID, FacePtr> getFaces() const;

  void onFace(std::function<void(FacePtr)> callback)
  {
    face_callbacks_.push_back(callback);
  }

  void onFaceLost(std::function<void(ID)> callback)
  {
    face_lost_callbacks_.push_back(callback);
  }


  std::map<ID, BodyPtr> getBodies() const;

  void onBody(std::function<void(BodyPtr)> callback)
  {
    body_callbacks_.push_back(callback);
  }

  void onBodyLost(std::function<void(ID)> callback)
  {
    body_lost_callbacks_.push_back(callback);
  }


  std::map<ID, VoicePtr> getVoices() const;

  void onVoice(std::function<void(VoicePtr)> callback)
  {
    voice_callbacks_.push_back(callback);
  }

  void onVoiceLost(std::function<void(ID)> callback)
  {
    voice_lost_callbacks_.push_back(callback);
  }


  std::map<ID, PersonPtr> getPersons() const;

  void onPerson(std::function<void(PersonPtr)> callback)
  {
    person_callbacks_.push_back(callback);
  }

  void onPersonLost(std::function<void(ID)> callback)
  {
    person_lost_callbacks_.push_back(callback);
  }

  std::map<ID, PersonPtr> getTrackedPersons() const;

  void onTrackedPerson(std::function<void(PersonPtr)> callback)
  {
    person_tracked_callbacks_.push_back(callback);
  }

  void onTrackedPersonLost(std::function<void(ID)> callback)
  {
    person_tracked_lost_callbacks_.push_back(callback);
  }

  void setReferenceFrame(const std::string & frame)
  {
    reference_frame_ = frame;
  }

  rclcpp::CallbackGroup::SharedPtr getCallbackGroup() {return callback_group_;}

  void clearData()
  {
    faces_.clear();
    bodies_.clear();
    voices_.clear();
    persons_.clear();
    tracked_persons_.clear();
    tf_buffer_.clear();
  }

  void clearCallbacks()
  {
    face_callbacks_.clear();
    face_lost_callbacks_.clear();
    body_callbacks_.clear();
    body_lost_callbacks_.clear();
    voice_callbacks_.clear();
    voice_lost_callbacks_.clear();
    person_callbacks_.clear();
    person_lost_callbacks_.clear();
    person_tracked_callbacks_.clear();
    person_tracked_lost_callbacks_.clear();
  }

protected:
  explicit HRIListener(NodeLikeSharedPtr node_like);

private:
  void onTrackedFeature(FeatureType & feature, hri_msgs::msg::IdsList::ConstSharedPtr tracked);

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

  std::map<FeatureType,
    rclcpp::Subscription<hri_msgs::msg::IdsList>::SharedPtr> feature_subscribers_;

  std::map<ID, FacePtr> faces_;
  std::vector<std::function<void(FacePtr)>> face_callbacks_;
  std::vector<std::function<void(ID)>> face_lost_callbacks_;

  std::map<ID, BodyPtr> bodies_;
  std::vector<std::function<void(BodyPtr)>> body_callbacks_;
  std::vector<std::function<void(ID)>> body_lost_callbacks_;

  std::map<ID, VoicePtr> voices_;
  std::vector<std::function<void(VoicePtr)>> voice_callbacks_;
  std::vector<std::function<void(ID)>> voice_lost_callbacks_;

  std::map<ID, PersonPtr> persons_;
  std::vector<std::function<void(PersonPtr)>> person_callbacks_;
  std::vector<std::function<void(ID)>> person_lost_callbacks_;
  std::map<ID, PersonPtr> tracked_persons_;
  std::vector<std::function<void(PersonPtr)>> person_tracked_callbacks_;
  std::vector<std::function<void(ID)>> person_tracked_lost_callbacks_;

  std::string reference_frame_;
  tf2::BufferCore tf_buffer_;
  std::unique_ptr<tf2_ros::TransformListener> tf_listener_;
};

}  // namespace hri

#endif  // HRI__HRI_HPP_