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
{
namespace drivers
{
namespace continental_srr520
{
class ContinentalSRR520Decoder : public ContinentalPacketsDecoder
{
public:
  explicit ContinentalSRR520Decoder(
    const std::shared_ptr<const ContinentalSRR520SensorConfiguration> & sensor_configuration);

  Status GetStatus() override;

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

  void PrintInfo(std::string info);

  void PrintError(std::string error);

  void PrintDebug(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 continental_srr520
}  // namespace drivers
}  // namespace nebula