display.hpp

Go to the documentation of this file.

// Copyright 2024 Kotaro Uetake.
//
// 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 AWVIZ_COMMON__DISPLAY_HPP_
#define AWVIZ_COMMON__DISPLAY_HPP_
 
#include "awviz_common/property.hpp"
 
#include <rclcpp/qos.hpp>
#include <rclcpp/rclcpp.hpp>
#include <rerun.hpp>
#include <rosidl_runtime_cpp/traits.hpp>
 
#include <memory>
#include <string>
#include <unordered_map>
 
namespace awviz_common
{
class Display
{
public:
  Display() = default;
 
  virtual ~Display() = default;
 
  virtual void initialize(
    rclcpp::Node::SharedPtr node, std::shared_ptr<rerun::RecordingStream> stream) = 0;
 
  virtual void set_property(
    const std::string & topic,
    const std::shared_ptr<std::unordered_map<std::string, std::string>> entity_roots) = 0;
 
  virtual void start() = 0;
 
  virtual void end() = 0;
 
  virtual bool is_initialized() const { return is_initialized_; }
 
protected:
  rclcpp::Node::SharedPtr node_;                    
  std::shared_ptr<rerun::RecordingStream> stream_;  
  bool is_initialized_;                             
};
 
template <typename MsgType>
class RosTopicDisplay : public Display
{
public:
  RosTopicDisplay()
  {
    const auto msg_type = rosidl_generator_traits::name<MsgType>();
    property_.set_type(msg_type);
  }
 
  ~RosTopicDisplay() override { unsubscribe(); }
 
  void initialize(
    rclcpp::Node::SharedPtr node, std::shared_ptr<rerun::RecordingStream> stream) override
  {
    node_ = node;
    stream_ = stream;
    is_initialized_ = true;
  };
 
  void set_property(
    const std::string & topic,
    const std::shared_ptr<std::unordered_map<std::string, std::string>> entity_roots) override
  {
    property_.set_topic(topic);
    property_.set_entity_roots(entity_roots);
  }
 
  void start() override { subscribe(); }
 
  void end() override { unsubscribe(); }
 
  bool is_initialized() const override { return is_initialized_ && property_.is_initialized(); }
 
protected:
  static constexpr const char * TIMELINE_NAME = "timestamp";  
 
  virtual void subscribe()
  {
    if (!is_initialized()) {
      return;
    }
 
    // TODO(ktro2828): QoS setting
    subscription_ = node_->create_subscription<MsgType>(
      property_.topic(), rclcpp::SensorDataQoS{},
      [this](const typename MsgType::ConstSharedPtr msg) { log_message(msg); });
  };
 
  virtual void unsubscribe() { subscription_.reset(); }
 
  virtual void log_message(typename MsgType::ConstSharedPtr msg) = 0;
 
protected:
  typename rclcpp::Subscription<MsgType>::SharedPtr subscription_;  
  RosTopicProperty property_;                                       
};
}  // namespace awviz_common
#endif  // AWVIZ_COMMON__DISPLAY_HPP_