Program Listing for File socket_can_receiver.hpp
↰ Return to documentation for file (/tmp/ws/src/ros2_socketcan/ros2_socketcan/include/ros2_socketcan/socket_can_receiver.hpp
)
// Copyright 2021 the Autoware Foundation
//
// 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.
//
// Co-developed by Tier IV, Inc. and Apex.AI, Inc.
#ifndef ROS2_SOCKETCAN__SOCKET_CAN_RECEIVER_HPP_
#define ROS2_SOCKETCAN__SOCKET_CAN_RECEIVER_HPP_
#include <linux/can.h>
#include <array>
#include <chrono>
#include <cstdint>
#include <cstring>
#include <string>
#include <vector>
#include "ros2_socketcan/visibility_control.hpp"
#include "ros2_socketcan/socket_can_id.hpp"
namespace drivers
{
namespace socketcan
{
class SOCKETCAN_PUBLIC SocketCanReceiver
{
public:
explicit SocketCanReceiver(const std::string & interface = "can0", const bool enable_fd = false);
~SocketCanReceiver() noexcept;
struct CanFilterList
{
std::vector<struct can_filter> filters;
can_err_mask_t error_mask = 0;
bool join_filters = false;
CanFilterList() = default;
explicit CanFilterList(const char * str);
explicit CanFilterList(const std::string & str);
static CanFilterList ParseFilters(const std::string & str);
};
void SetCanFilters(const CanFilterList & filters);
CanId receive(
void * const data,
const std::chrono::nanoseconds timeout = std::chrono::nanoseconds::zero()) const;
template<typename T, typename = std::enable_if_t<!std::is_pointer<T>::value>>
CanId receive(
T & data,
const std::chrono::nanoseconds timeout = std::chrono::nanoseconds::zero()) const
{
static_assert(sizeof(data) <= MAX_DATA_LENGTH, "Data type too large for CAN");
std::array<uint8_t, MAX_DATA_LENGTH> data_raw{};
const auto ret = receive(&data_raw[0U], timeout);
if (ret.length() != sizeof(data)) {
throw std::runtime_error{"Received CAN data is of size incompatible with provided type!"};
}
(void)std::memcpy(&data, &data_raw[0U], ret.length());
return ret;
}
CanId receive_fd(
void * const data,
const std::chrono::nanoseconds timeout = std::chrono::nanoseconds::zero()) const;
template<typename T, typename = std::enable_if_t<!std::is_pointer<T>::value>>
CanId receive_fd(
T & data,
const std::chrono::nanoseconds timeout = std::chrono::nanoseconds::zero()) const
{
static_assert(sizeof(data) <= MAX_FD_DATA_LENGTH, "Data type too large for CAN FD");
std::array<uint8_t, MAX_FD_DATA_LENGTH> data_raw{};
const auto ret = receive_fd(&data_raw[0U], timeout);
if (ret.length() != sizeof(data)) {
throw std::runtime_error{"Received CAN FD data is of size incompatible with provided type!"};
}
(void)std::memcpy(&data, &data_raw[0U], ret.length());
return ret;
}
private:
// Wait for file descriptor to be available to send data via select()
SOCKETCAN_LOCAL void wait(const std::chrono::nanoseconds timeout) const;
int32_t m_file_descriptor;
bool m_enable_fd;
}; // class SocketCanReceiver
} // namespace socketcan
} // namespace drivers
#endif // ROS2_SOCKETCAN__SOCKET_CAN_RECEIVER_HPP_