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 <optional>
#include <string>
#include <unordered_map>
#include <utility>
 
namespace awviz_common
{
enum class DisplayState : uint8_t {
  kCreated = 0,
  kInitialized = 1,
  kConfigured = 2,
  kRunning = 3,
};
 
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 state_ != DisplayState::kCreated; }
 
protected:
  void set_state(DisplayState state) { state_ = state; }
  DisplayState state() const { return state_; }
 
  rclcpp::Node::SharedPtr node_;                    
  std::shared_ptr<rerun::RecordingStream> stream_;  
  DisplayState state_{DisplayState::kCreated};      
};
 
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_ = std::move(node);
    stream_ = std::move(stream);
    set_state(DisplayState::kInitialized);
  };
 
  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);
    set_state(DisplayState::kConfigured);
  }
 
  void start() override
  {
    // Only transition to running when the display is properly initialized/configured.
    if (!is_initialized()) {
      return;
    }
    subscribe();
    set_state(DisplayState::kRunning);
  }
 
  void end() override
  {
    unsubscribe();
    set_state(DisplayState::kConfigured);
  }
 
  bool is_initialized() const override
  {
    return (state() == DisplayState::kConfigured || state() == DisplayState::kRunning) &&
           property_.is_initialized();
  }
 
protected:
  static constexpr const char * TIMELINE_NAME = "timestamp";  
 
  void log_timestamp(const rclcpp::Time & stamp)
  {
    if (!stream_) {
      return;
    }
    stream_->set_time_duration_secs(TIMELINE_NAME, stamp.seconds());
  }
 
  std::optional<std::string> resolve_entity_path(
    const std::string & frame_id, bool include_topic = true) const
  {
    return include_topic ? property_.entity(frame_id) : property_.entity_without_topic(frame_id);
  }
 
  void log_warning_for_missing_entity(const std::string & frame_id) const
  {
    if (!stream_) {
      return;
    }
 
    std::string message = "There is no corresponding entity path";
 
    if (!frame_id.empty()) {
      message += " for frame_id: " + frame_id;
    }
 
    const auto & topic = property_.topic();
    if (!topic.empty()) {
      message += " on topic: " + topic;
    }
 
    stream_->log(topic, rerun::TextLog(message));
  }
 
  void log_warning_text(const std::string & message) const
  {
    if (!stream_) {
      return;
    }
 
    stream_->log(property_.topic(), rerun::TextLog(message));
  }
 
  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_