File continental_srr520_decoder.hpp

File List > decoders > continental_srr520_decoder.hpp

Go to the documentation of this file

// Copyright 2024 TIER IV, Inc.
//
// 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.

#pragma once

#include "nebula_decoders/nebula_decoders_continental/decoders/continental_packets_decoder.hpp"

#include <nebula_common/continental/continental_srr520.hpp>
#include <rclcpp/rclcpp.hpp>

#include <continental_msgs/msg/continental_srr520_detection_list.hpp>
#include <continental_msgs/msg/continental_srr520_object_list.hpp>
#include <diagnostic_msgs/msg/diagnostic_array.hpp>
#include <nebula_msgs/msg/nebula_packet.hpp>
#include <nebula_msgs/msg/nebula_packets.hpp>

#include <array>
#include <memory>
#include <string>
#include <vector>

namespace nebula::drivers::continental_srr520
{
class ContinentalSRR520Decoder : public ContinentalPacketsDecoder
{
public:
  explicit ContinentalSRR520Decoder(
    const std::shared_ptr<const ContinentalSRR520SensorConfiguration> & sensor_configuration);

  Status get_status() override;

  bool process_packet(std::unique_ptr<nebula_msgs::msg::NebulaPacket> packet_msg) override;

  Status register_near_detection_list_callback(
    std::function<void(std::unique_ptr<continental_msgs::msg::ContinentalSrr520DetectionList>)>
      detection_list_callback);

  Status register_hrr_detection_list_callback(
    std::function<void(std::unique_ptr<continental_msgs::msg::ContinentalSrr520DetectionList>)>
      detection_list_callback);

  Status register_object_list_callback(
    std::function<void(std::unique_ptr<continental_msgs::msg::ContinentalSrr520ObjectList>)>
      object_list_callback);

  Status register_status_callback(
    std::function<void(std::unique_ptr<diagnostic_msgs::msg::DiagnosticArray>)> status_callback);

  Status register_sync_follow_up_callback(
    std::function<void(builtin_interfaces::msg::Time)> sync_follow_up_callback);

  Status register_packets_callback(
    std::function<void(std::unique_ptr<nebula_msgs::msg::NebulaPackets>)> nebula_packets_callback);

  void set_logger(std::shared_ptr<rclcpp::Logger> node);

private:
  void process_near_header_packet(std::unique_ptr<nebula_msgs::msg::NebulaPacket> packet_msg);

  void process_near_element_packet(std::unique_ptr<nebula_msgs::msg::NebulaPacket> packet_msg);

  void process_hrr_header_packet(std::unique_ptr<nebula_msgs::msg::NebulaPacket> packet_msg);

  void process_hrr_element_packet(std::unique_ptr<nebula_msgs::msg::NebulaPacket> packet_msg);

  void process_object_header_packet(std::unique_ptr<nebula_msgs::msg::NebulaPacket> packet_msg);

  void process_object_element_packet(std::unique_ptr<nebula_msgs::msg::NebulaPacket> packet_msg);

  void process_crc_list_packet(std::unique_ptr<nebula_msgs::msg::NebulaPacket> packet_msg);

  void process_near_crc_list_packet(std::unique_ptr<nebula_msgs::msg::NebulaPacket> packet_msg);

  void process_hrrcrc_list_packet(
    std::unique_ptr<nebula_msgs::msg::NebulaPacket> packet_msg);  // cspell:ignore HRRCRC

  void process_object_crc_list_packet(std::unique_ptr<nebula_msgs::msg::NebulaPacket> packet_msg);

  void process_sensor_status_packet(std::unique_ptr<nebula_msgs::msg::NebulaPacket> packet_msg);

  void process_sync_follow_up_packet(std::unique_ptr<nebula_msgs::msg::NebulaPacket> packet_msg);

  void print_info(std::string info);

  void print_error(std::string error);

  void print_debug(std::string debug);

  std::function<void(std::unique_ptr<continental_msgs::msg::ContinentalSrr520DetectionList> msg)>
    near_detection_list_callback_{};
  std::function<void(std::unique_ptr<continental_msgs::msg::ContinentalSrr520DetectionList> msg)>
    hrr_detection_list_callback_{};
  std::function<void(std::unique_ptr<continental_msgs::msg::ContinentalSrr520ObjectList> msg)>
    object_list_callback_{};
  std::function<void(std::unique_ptr<diagnostic_msgs::msg::DiagnosticArray> msg)>
    status_callback_{};
  std::function<void(builtin_interfaces::msg::Time)> sync_follow_up_callback_{};
  std::function<void(std::unique_ptr<nebula_msgs::msg::NebulaPackets> msg)>
    nebula_packets_callback_{};

  std::unique_ptr<nebula_msgs::msg::NebulaPackets> rdi_near_packets_ptr_{};
  std::unique_ptr<nebula_msgs::msg::NebulaPackets> rdi_hrr_packets_ptr_{};
  std::unique_ptr<nebula_msgs::msg::NebulaPackets> object_packets_ptr_{};

  std::unique_ptr<continental_msgs::msg::ContinentalSrr520DetectionList> near_detection_list_ptr_{};
  std::unique_ptr<continental_msgs::msg::ContinentalSrr520DetectionList> hrr_detection_list_ptr_{};
  std::unique_ptr<continental_msgs::msg::ContinentalSrr520ObjectList> object_list_ptr_{};

  bool first_rdi_near_packet_{true};
  bool first_rdi_hrr_packet_{true};
  bool first_object_packet_{true};

  ScanHeaderPacket rdi_near_header_packet_{};
  ScanHeaderPacket rdi_hrr_header_packet_{};
  ObjectHeaderPacket object_header_packet_{};

  std::shared_ptr<const continental_srr520::ContinentalSRR520SensorConfiguration>
    sensor_configuration_{};

  std::shared_ptr<rclcpp::Logger> parent_node_logger_ptr_{};
};

}  // namespace nebula::drivers::continental_srr520