File continental_srr520_hw_interface.hpp

File List > include > nebula_hw_interfaces > nebula_hw_interfaces_continental > continental_srr520_hw_interface.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.

#ifndef NEBULA_CONTINENTAL_SRR520_HW_INTERFACE_H
#define NEBULA_CONTINENTAL_SRR520_HW_INTERFACE_H

#include "nebula_hw_interfaces/nebula_hw_interfaces_common/nebula_hw_interface_base.hpp"

#include <nebula_common/continental/continental_srr520.hpp>
#include <rclcpp/rclcpp.hpp>
#include <ros2_socketcan/socket_can_receiver.hpp>
#include <ros2_socketcan/socket_can_sender.hpp>

#include <nebula_msgs/msg/nebula_packet.hpp>
#include <nebula_msgs/msg/nebula_packets.hpp>

#include <memory>
#include <mutex>
#include <string>
#include <vector>

namespace nebula
{
namespace drivers
{
namespace continental_srr520
{
class ContinentalSRR520HwInterface
{
public:
  ContinentalSRR520HwInterface();

  Status SensorInterfaceStart();

  Status SensorInterfaceStop();

  Status SetSensorConfiguration(
    const std::shared_ptr<
      const nebula::drivers::continental_srr520::ContinentalSRR520SensorConfiguration>
      new_config_ptr);

  Status RegisterPacketCallback(
    std::function<void(std::unique_ptr<nebula_msgs::msg::NebulaPacket>)> packet_callback);

  void SensorSync();

  void SensorSyncFollowUp(builtin_interfaces::msg::Time stamp);

  Status ConfigureSensor(
    uint8_t sensor_id, float longitudinal_autosar, float lateral_autosar, float vertical_autosar,
    float yaw_autosar, float longitudinal_cog, float wheelbase, float cover_damping,
    bool plug_bottom, bool reset);

  Status SetVehicleDynamics(
    float longitudinal_acceleration, float lateral_acceleration, float yaw_rate,
    float longitudinal_velocity, bool standstill);

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

private:
  template <std::size_t N>
  bool SendFrame(const std::array<uint8_t, N> & data, int can_frame_id);

  void PrintInfo(std::string info);

  void PrintError(std::string error);

  void PrintDebug(std::string debug);

  void ReceiveLoop();

  std::unique_ptr<::drivers::socketcan::SocketCanReceiver> can_receiver_ptr_;
  std::unique_ptr<::drivers::socketcan::SocketCanSender> can_sender_ptr_;
  std::unique_ptr<std::thread> receiver_thread_ptr_;

  std::shared_ptr<const ContinentalSRR520SensorConfiguration> config_ptr_;
  std::function<void(std::unique_ptr<nebula_msgs::msg::NebulaPacket> buffer)>
    nebula_packet_callback_;

  std::mutex receiver_mutex_;
  bool sensor_interface_active_{};

  uint8_t sync_counter_{0};
  bool sync_follow_up_sent_{true};
  builtin_interfaces::msg::Time last_sync_stamp_;

  std::shared_ptr<rclcpp::Logger> parent_node_logger_ptr_;
};
}  // namespace continental_srr520
}  // namespace drivers
}  // namespace nebula

#endif  // NEBULA_CONTINENTAL_SRR520_HW_INTERFACE_H