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::drivers::continental_srr520
{
class ContinentalSRR520HwInterface
{
public:
ContinentalSRR520HwInterface();
Status sensor_interface_start();
Status sensor_interface_stop();
Status set_sensor_configuration(
const std::shared_ptr<
const nebula::drivers::continental_srr520::ContinentalSRR520SensorConfiguration>
new_config_ptr);
Status register_packet_callback(
std::function<void(std::unique_ptr<nebula_msgs::msg::NebulaPacket>)> packet_callback);
void sensor_sync();
void sensor_sync_follow_up(builtin_interfaces::msg::Time stamp);
Status configure_sensor(
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 set_vehicle_dynamics(
float longitudinal_acceleration, float lateral_acceleration, float yaw_rate,
float longitudinal_velocity, bool standstill);
void set_logger(std::shared_ptr<rclcpp::Logger> node);
private:
template <std::size_t N>
bool send_frame(const std::array<uint8_t, N> & data, int can_frame_id);
void print_info(std::string info);
void print_error(std::string error);
void print_debug(std::string debug);
void receive_loop();
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 nebula::drivers::continental_srr520
#endif // NEBULA_CONTINENTAL_SRR520_HW_INTERFACE_H