diff --git a/.travis.yml b/.travis.yml index 98466cbe9..986b89f77 100644 --- a/.travis.yml +++ b/.travis.yml @@ -10,8 +10,8 @@ env: global: - CCACHE_DIR=$HOME/.ccache matrix: - - ROS_DISTRO="crystal" ROS_REPO=testing - - ROS_DISTRO="crystal" ROS_REPO=main + - ROS_DISTRO="crystal" ROS_REPO=testing UPSTREAM_WORKSPACE="github:ament/ament_lint#dashing" + - ROS_DISTRO="crystal" ROS_REPO=main UPSTREAM_WORKSPACE="github:ament/ament_lint#dashing" - ROS_DISTRO="dashing" ROS_REPO=testing - ROS_DISTRO="dashing" ROS_REPO=main - ROS_DISTRO="eloquent" ROS_REPO=testing diff --git a/LICENSE b/LICENSE index bde60cebd..0a041280b 100644 --- a/LICENSE +++ b/LICENSE @@ -1,7 +1,7 @@ -GNU LESSER GENERAL PUBLIC LICENSE + GNU LESSER GENERAL PUBLIC LICENSE Version 3, 29 June 2007 - Copyright (C) 2007 Free Software Foundation, Inc. + Copyright (C) 2007 Free Software Foundation, Inc. Everyone is permitted to copy and distribute verbatim copies of this license document, but changing it is not allowed. @@ -162,4 +162,4 @@ General Public License ever published by the Free Software Foundation. whether future versions of the GNU Lesser General Public License shall apply, that proxy's public statement of acceptance of any version is permanent authorization for you to choose that version for the -Library. \ No newline at end of file +Library. diff --git a/canopen_master/include/canopen_master/canopen.h b/canopen_master/include/canopen_master/canopen.h index a6f397620..0c9901fcf 100644 --- a/canopen_master/include/canopen_master/canopen.h +++ b/canopen_master/include/canopen_master/canopen.h @@ -206,7 +206,6 @@ class Node : public Layer{ bool prepare(); using StateFunc = std::function; - using StateDelegate [[deprecated("use StateFunc instead")]] = can::DelegateHelper; typedef can::Listener StateListener; typedef StateListener::ListenerConstSharedPtr StateListenerConstSharedPtr; diff --git a/canopen_master/include/canopen_master/objdict.h b/canopen_master/include/canopen_master/objdict.h index d03f262c9..a643741e1 100644 --- a/canopen_master/include/canopen_master/objdict.h +++ b/canopen_master/include/canopen_master/objdict.h @@ -235,9 +235,6 @@ class ObjectDict{ typedef ObjectDict::ObjectDictSharedPtr ObjectDictSharedPtr; typedef std::shared_ptr ObjectDictConstSharedPtr; -[[deprecated]] -std::size_t hash_value(ObjectDict::Key const& k); - template class NodeIdOffset{ T offset; T (*adder)(const uint8_t &, const T &); @@ -278,10 +275,8 @@ class AccessException : public Exception{ class ObjectStorage{ public: using ReadFunc = std::function; - using ReadDelegate [[deprecated("use ReadFunc instead")]] = can::DelegateHelper; using WriteFunc = std::function; - using WriteDelegate [[deprecated("use WriteFunc instead")]] = can::DelegateHelper; typedef std::shared_ptr ObjectStorageSharedPtr; diff --git a/canopen_master/include/canopen_master/timer.h b/canopen_master/include/canopen_master/timer.h index 0853a506b..88cc7d546 100644 --- a/canopen_master/include/canopen_master/timer.h +++ b/canopen_master/include/canopen_master/timer.h @@ -15,7 +15,6 @@ namespace canopen{ class Timer{ public: using TimerFunc = std::function; - using TimerDelegate [[deprecated("use TimerFunc instead")]] = can::DelegateHelper; Timer():work(io), timer(io),thread(std::bind( static_cast(&boost::asio::io_service::run), &io)) diff --git a/socketcan_interface/AMENT_IGNORE b/socketcan_interface/AMENT_IGNORE deleted file mode 100644 index e69de29bb..000000000 diff --git a/socketcan_interface/CMakeLists.txt b/socketcan_interface/CMakeLists.txt index e3b45fe6b..bec292f44 100644 --- a/socketcan_interface/CMakeLists.txt +++ b/socketcan_interface/CMakeLists.txt @@ -1,4 +1,4 @@ -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.5) project(socketcan_interface) if(NOT CMAKE_CXX_STANDARD) @@ -6,146 +6,120 @@ if(NOT CMAKE_CXX_STANDARD) set(CMAKE_CXX_STANDARD 14) endif() -find_package(catkin REQUIRED - COMPONENTS - class_loader -) - -find_package(console_bridge REQUIRED) - +find_package(ament_cmake REQUIRED) +find_package(class_loader REQUIRED) find_package(Boost REQUIRED COMPONENTS chrono system thread ) - find_package(Threads REQUIRED) -catkin_package( - INCLUDE_DIRS - include - LIBRARIES - ${PROJECT_NAME}_string - CATKIN_DEPENDS - DEPENDS - Boost - console_bridge -) - -include_directories( - include +include_directories(include + ${class_loader_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS} - ${catkin_INCLUDE_DIRS} - ${console_bridge_INCLUDE_DIRS} ) # ${PROJECT_NAME}_string -add_library(${PROJECT_NAME}_string - src/string.cpp +add_library(${PROJECT_NAME}_string SHARED + src/string.cpp ) # socketcan_dump -add_executable(socketcan_dump - src/candump.cpp -) - +add_executable(socketcan_dump src/candump.cpp) target_link_libraries(socketcan_dump - ${PROJECT_NAME}_string - ${console_bridge_LIBRARIES} - ${catkin_LIBRARIES} - ${Boost_LIBRARIES} - ${CMAKE_THREAD_LIBS_INIT} + ${PROJECT_NAME}_string + ${class_loader_LIBRARIES} + ${Boost_LIBRARIES} + ${CMAKE_THREAD_LIBS_INIT} ) # socketcan_bcm -add_executable(socketcan_bcm - src/canbcm.cpp -) - +add_executable(socketcan_bcm src/canbcm.cpp) target_link_libraries(socketcan_bcm - ${PROJECT_NAME}_string - ${console_bridge_LIBRARIES} - ${catkin_LIBRARIES} - ${Boost_LIBRARIES} + ${PROJECT_NAME}_string + ${Boost_LIBRARIES} ) # ${PROJECT_NAME}_plugin -add_library(${PROJECT_NAME}_plugin +add_library(${PROJECT_NAME}_plugin SHARED src/${PROJECT_NAME}_plugin.cpp ) target_link_libraries(${PROJECT_NAME}_plugin - ${console_bridge_LIBRARIES} - ${catkin_LIBRARIES} + ${class_loader_LIBRARIES} ${Boost_LIBRARIES} ) +class_loader_hide_library_symbols(${PROJECT_NAME}_plugin) +install(FILES socketcan_interface_plugin.xml DESTINATION share/${PROJECT_NAME}/) +ament_index_register_resource(socketcan_interface__pluginlib__plugin CONTENT "share/${PROJECT_NAME}/socketcan_interface_plugin.xml\n") + +ament_export_dependencies( + class_loader + Boost +) + +ament_export_include_directories(include) +ament_export_libraries(${PROJECT_NAME}_string) + install( TARGETS socketcan_bcm socketcan_dump - ${PROJECT_NAME}_plugin ${PROJECT_NAME}_string - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} + ${PROJECT_NAME}_plugin + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin + INCLUDES DESTINATION include ) install( - DIRECTORY - include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} - FILES_MATCHING PATTERN "*.h" + DIRECTORY include/ + DESTINATION include ) -install( - FILES - ${PROJECT_NAME}_plugin.xml - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -) -if(CATKIN_ENABLE_TESTING) +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + + ament_lint_auto_find_test_dependencies() - catkin_add_gtest(${PROJECT_NAME}-test_dummy_interface + find_package(ament_cmake_gtest REQUIRED) + + ament_add_gtest(${PROJECT_NAME}-test_dummy_interface test/test_dummy_interface.cpp ) target_link_libraries(${PROJECT_NAME}-test_dummy_interface ${PROJECT_NAME}_string - ${console_bridge_LIBRARIES} - ${catkin_LIBRARIES} + ${Boost_LIBRARIES} ) - catkin_add_gtest(${PROJECT_NAME}-test_delegates - test/test_delegates.cpp - ) - target_link_libraries(${PROJECT_NAME}-test_delegates - ${PROJECT_NAME}_string - ${catkin_LIBRARIES} - ) - - catkin_add_gtest(${PROJECT_NAME}-test_string + ament_add_gtest(${PROJECT_NAME}-test_string test/test_string.cpp ) target_link_libraries(${PROJECT_NAME}-test_string ${PROJECT_NAME}_string - ${console_bridge_LIBRARIES} - ${catkin_LIBRARIES} + ${Boost_LIBRARIES} ) - catkin_add_gtest(${PROJECT_NAME}-test_filter + ament_add_gtest(${PROJECT_NAME}-test_filter test/test_filter.cpp ) target_link_libraries(${PROJECT_NAME}-test_filter ${PROJECT_NAME}_string - ${console_bridge_LIBRARIES} - ${catkin_LIBRARIES} + ${Boost_LIBRARIES} ) - catkin_add_gtest(${PROJECT_NAME}-test_dispatcher + ament_add_gtest(${PROJECT_NAME}-test_dispatcher test/test_dispatcher.cpp ) target_link_libraries(${PROJECT_NAME}-test_dispatcher ${PROJECT_NAME}_string - ${catkin_LIBRARIES} + ${Boost_LIBRARIES} ) - target_compile_options(${PROJECT_NAME}-test_dispatcher PRIVATE -Wno-deprecated-declarations) endif() + +ament_package( + CONFIG_EXTRAS socketcan_interface-extras.cmake +) diff --git a/socketcan_interface/include/socketcan_interface/asio_base.h b/socketcan_interface/include/socketcan_interface/asio_base.h deleted file mode 100644 index 8bd2eebe8..000000000 --- a/socketcan_interface/include/socketcan_interface/asio_base.h +++ /dev/null @@ -1,132 +0,0 @@ -#ifndef H_ASIO_BASE -#define H_ASIO_BASE - -#include -#include -#include -#include -#include -#include - -namespace can{ - - -template class AsioDriver : public DriverInterface{ - using FrameDispatcher = FilteredDispatcher; - using StateDispatcher = SimpleDispatcher; - FrameDispatcher frame_dispatcher_; - StateDispatcher state_dispatcher_; - - State state_; - boost::mutex state_mutex_; - boost::mutex socket_mutex_; - -protected: - boost::asio::io_service io_service_; -#if BOOST_ASIO_VERSION >= 101200 // Boost 1.66+ - boost::asio::io_context::strand strand_; -#else - boost::asio::strand strand_; -#endif - Socket socket_; - Frame input_; - - virtual void triggerReadSome() = 0; - virtual bool enqueue(const Frame & msg) = 0; - - void dispatchFrame(const Frame &msg){ - strand_.post([this, msg]{ frame_dispatcher_.dispatch(msg.key(), msg);} ); // copies msg - } - void setErrorCode(const boost::system::error_code& error){ - boost::mutex::scoped_lock lock(state_mutex_); - if(state_.error_code != error){ - state_.error_code = error; - state_dispatcher_.dispatch(state_); - } - } - void setInternalError(unsigned int internal_error){ - boost::mutex::scoped_lock lock(state_mutex_); - if(state_.internal_error != internal_error){ - state_.internal_error = internal_error; - state_dispatcher_.dispatch(state_); - } - } - - void setDriverState(State::DriverState state){ - boost::mutex::scoped_lock lock(state_mutex_); - if(state_.driver_state != state){ - state_.driver_state = state; - state_dispatcher_.dispatch(state_); - } - } - void setNotReady(){ - setDriverState(socket_.is_open()?State::open : State::closed); - } - - void frameReceived(const boost::system::error_code& error){ - if(!error){ - dispatchFrame(input_); - triggerReadSome(); - }else{ - setErrorCode(error); - setNotReady(); - } - } - - AsioDriver() - : strand_(io_service_), socket_(io_service_) - {} - -public: - virtual ~AsioDriver() { shutdown(); } - - State getState(){ - boost::mutex::scoped_lock lock(state_mutex_); - return state_; - } - virtual void run(){ - setNotReady(); - - if(getState().driver_state == State::open){ - io_service_.reset(); - boost::asio::io_service::work work(io_service_); - setDriverState(State::ready); - - boost::thread post_thread([this]() { io_service_.run(); }); - - triggerReadSome(); - - boost::system::error_code ec; - io_service_.run(ec); - setErrorCode(ec); - - setNotReady(); - } - state_dispatcher_.dispatch(getState()); - } - virtual bool send(const Frame & msg){ - return getState().driver_state == State::ready && enqueue(msg); - } - - virtual void shutdown(){ - if(socket_.is_open()){ - socket_.cancel(); - socket_.close(); - } - io_service_.stop(); - } - - virtual FrameListenerConstSharedPtr createMsgListener(const FrameFunc &delegate){ - return frame_dispatcher_.createListener(delegate); - } - virtual FrameListenerConstSharedPtr createMsgListener(const Frame::Header&h , const FrameFunc &delegate){ - return frame_dispatcher_.createListener(h.key(), delegate); - } - virtual StateListenerConstSharedPtr createStateListener(const StateFunc &delegate){ - return state_dispatcher_.createListener(delegate); - } - -}; - -} // namespace can -#endif diff --git a/socketcan_interface/include/socketcan_interface/asio_base.hpp b/socketcan_interface/include/socketcan_interface/asio_base.hpp new file mode 100644 index 000000000..4bcf28c67 --- /dev/null +++ b/socketcan_interface/include/socketcan_interface/asio_base.hpp @@ -0,0 +1,171 @@ +// Copyright (c) 2016-2019, Fraunhofer, Mathias Lüdtke, AutonomouStuff +// +// This program is free software: you can redistribute it and/or modify +// it under the terms of the GNU Lesser General Public License as published by +// the Free Software Foundation, either version 3 of the License, or +// (at your option) any later version. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU Lesser General Public License for more details. +// +// You should have received a copy of the GNU Lesser General Public License +// along with this program. If not, see . + +#ifndef SOCKETCAN_INTERFACE__ASIO_BASE_HPP_ +#define SOCKETCAN_INTERFACE__ASIO_BASE_HPP_ + +#include +#include +#include +#include + +#include "socketcan_interface/dispatcher.hpp" +#include "socketcan_interface/interface.hpp" + +namespace can +{ + +template +class AsioDriver : public DriverInterface +{ + using FrameDispatcher = FilteredDispatcher; + using StateDispatcher = SimpleDispatcher; + FrameDispatcher frame_dispatcher_; + StateDispatcher state_dispatcher_; + + State state_; + boost::mutex state_mutex_; + boost::mutex socket_mutex_; + +protected: + boost::asio::io_service io_service_; +#if BOOST_ASIO_VERSION >= 101200 // Boost 1.66+ + boost::asio::io_context::strand strand_; +#else + boost::asio::strand strand_; +#endif + Socket socket_; + Frame input_; + + virtual void triggerReadSome() = 0; + virtual bool enqueue(const Frame & msg) = 0; + + void dispatchFrame(const Frame & msg) + { + strand_.post([this, msg] {frame_dispatcher_.dispatch(msg.key(), msg);}); // copies msg + } + void setErrorCode(const boost::system::error_code & error) + { + boost::mutex::scoped_lock lock(state_mutex_); + if (state_.error_code != error) { + state_.error_code = error; + state_dispatcher_.dispatch(state_); + } + } + void setInternalError(unsigned int internal_error) + { + boost::mutex::scoped_lock lock(state_mutex_); + if (state_.internal_error != internal_error) { + state_.internal_error = internal_error; + state_dispatcher_.dispatch(state_); + } + } + + void setDriverState(State::DriverState state) + { + boost::mutex::scoped_lock lock(state_mutex_); + if (state_.driver_state != state) { + state_.driver_state = state; + state_dispatcher_.dispatch(state_); + } + } + void setNotReady() + { + setDriverState(socket_.is_open() ? State::open : State::closed); + } + + void frameReceived(const boost::system::error_code & error) + { + if (!error) { + dispatchFrame(input_); + triggerReadSome(); + } else { + setErrorCode(error); + setNotReady(); + } + } + + AsioDriver() + : strand_(io_service_), socket_(io_service_) + {} + +public: + virtual ~AsioDriver() + { + shutdown(); + } + + State getState() + { + boost::mutex::scoped_lock lock(state_mutex_); + return state_; + } + virtual void run() + { + setNotReady(); + + if (getState().driver_state == State::open) { + io_service_.reset(); + boost::asio::io_service::work work(io_service_); + setDriverState(State::ready); + + boost::thread post_thread([this]() + { + io_service_.run(); + }); + + triggerReadSome(); + + boost::system::error_code ec; + io_service_.run(ec); + setErrorCode(ec); + + setNotReady(); + } + state_dispatcher_.dispatch(getState()); + } + virtual bool send(const Frame & msg) + { + return getState().driver_state == State::ready && enqueue(msg); + } + + virtual void shutdown() + { + if (socket_.is_open()) { + socket_.cancel(); + socket_.close(); + } + + io_service_.stop(); + } + + virtual FrameListenerConstSharedPtr createMsgListener(const FrameFunc & delegate) + { + return frame_dispatcher_.createListener(delegate); + } + virtual FrameListenerConstSharedPtr createMsgListener( + const Frame::Header & h, const FrameFunc & delegate) + { + return frame_dispatcher_.createListener(h.key(), delegate); + } + virtual StateListenerConstSharedPtr createStateListener(const StateFunc & delegate) + { + return state_dispatcher_.createListener(delegate); + } +}; + +} // namespace can + +#endif // SOCKETCAN_INTERFACE__ASIO_BASE_HPP_ diff --git a/socketcan_interface/include/socketcan_interface/bcm.h b/socketcan_interface/include/socketcan_interface/bcm.h deleted file mode 100644 index cbf456f9c..000000000 --- a/socketcan_interface/include/socketcan_interface/bcm.h +++ /dev/null @@ -1,120 +0,0 @@ -#ifndef H_CAN_BCM -#define H_CAN_BCM - -#include - -#include -#include -#include -#include - -#include -#include -#include - -#include - -#include - -namespace can { - -class BCMsocket{ - int s_; - struct Message { - size_t size; - uint8_t *data; - Message(size_t n) - : size(sizeof(bcm_msg_head) + sizeof(can_frame)*n), data(new uint8_t[size]) - { - assert(n<=256); - std::memset(data, 0, size); - head().nframes = n; - } - bcm_msg_head& head() { - return *(bcm_msg_head*)data; - } - template void setIVal2(T period){ - long long usec = boost::chrono::duration_cast(period).count(); - head().ival2.tv_sec = usec / 1000000; - head().ival2.tv_usec = usec % 1000000; - } - void setHeader(Header header){ - head().can_id = header.id | (header.is_extended?CAN_EFF_FLAG:0); - } - bool write(int s){ - return ::write(s, data, size) > 0; - } - ~Message(){ - delete[] data; - data = 0; - size = 0; - } - }; -public: - BCMsocket():s_(-1){ - } - bool init(const std::string &device){ - s_ = socket(PF_CAN, SOCK_DGRAM, CAN_BCM); - - if(s_ < 0 ) return false; - struct ifreq ifr; - std::strcpy(ifr.ifr_name, device.c_str()); - int ret = ioctl(s_, SIOCGIFINDEX, &ifr); - - if(ret != 0){ - shutdown(); - return false; - } - - struct sockaddr_can addr = {0}; - addr.can_family = AF_CAN; - addr.can_ifindex = ifr.ifr_ifindex; - - ret = connect(s_, (struct sockaddr *)&addr, sizeof(addr)); - - if(ret < 0){ - shutdown(); - return false; - } - return true; - } - template bool startTX(DurationType period, Header header, size_t num, Frame *frames) { - Message msg(num); - msg.setHeader(header); - msg.setIVal2(period); - - bcm_msg_head &head = msg.head(); - - head.opcode = TX_SETUP; - head.flags |= SETTIMER | STARTTIMER; - - for(size_t i=0; i < num; ++i){ // msg nr - head.frames[i].can_dlc = frames[i].dlc; - head.frames[i].can_id = head.can_id; - for(size_t j = 0; j < head.frames[i].can_dlc; ++j){ // byte nr - head.frames[i].data[j] = frames[i].data[j]; - } - } - return msg.write(s_); - } - bool stopTX(Header header){ - Message msg(0); - msg.head().opcode = TX_DELETE; - msg.setHeader(header); - return msg.write(s_); - } - void shutdown(){ - if(s_ > 0){ - close(s_); - s_ = -1; - } - } - - virtual ~BCMsocket(){ - shutdown(); - } -}; - -} - -#endif diff --git a/socketcan_interface/include/socketcan_interface/bcm.hpp b/socketcan_interface/include/socketcan_interface/bcm.hpp new file mode 100644 index 000000000..27103601d --- /dev/null +++ b/socketcan_interface/include/socketcan_interface/bcm.hpp @@ -0,0 +1,164 @@ +// Copyright (c) 2016-2019, Fraunhofer, Mathias Lüdtke, AutonomouStuff +// +// This program is free software: you can redistribute it and/or modify +// it under the terms of the GNU Lesser General Public License as published by +// the Free Software Foundation, either version 3 of the License, or +// (at your option) any later version. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU Lesser General Public License for more details. +// +// You should have received a copy of the GNU Lesser General Public License +// along with this program. If not, see . + +#ifndef SOCKETCAN_INTERFACE__BCM_HPP_ +#define SOCKETCAN_INTERFACE__BCM_HPP_ + +#include +#include +#include +#include + +#include +#include +#include + +#include +#include + +#include "socketcan_interface/interface.hpp" + +namespace can +{ + +class BCMsocket +{ + int s_; + struct Message + { + size_t size; + uint8_t * data; + + explicit Message(size_t n) + : size(sizeof(bcm_msg_head) + sizeof(can_frame) * n), data(new uint8_t[size]) + { + assert(n <= 256); + memset(data, 0, size); + head().nframes = n; + } + + bcm_msg_head & head() + { + return *reinterpret_cast(data); + } + + template + void setIVal2(T period) + { + int64_t usec = boost::chrono::duration_cast(period).count(); + head().ival2.tv_sec = usec / 1000000; + head().ival2.tv_usec = usec % 1000000; + } + + void setHeader(Header header) + { + head().can_id = header.id | (header.is_extended ? CAN_EFF_FLAG : 0); + } + + bool write(int s) + { + return ::write(s, data, size) > 0; + } + + ~Message() + { + delete[] data; + data = 0; + size = 0; + } + }; + +public: + BCMsocket() + : s_(-1) + { + } + + bool init(const std::string & device) + { + s_ = socket(PF_CAN, SOCK_DGRAM, CAN_BCM); + + if (s_ < 0) { + return false; + } + + struct ifreq ifr; + snprintf(ifr.ifr_name, sizeof(ifr.ifr_name), "%s", device.c_str()); + int ret = ioctl(s_, SIOCGIFINDEX, &ifr); + + if (ret != 0) { + shutdown(); + return false; + } + + struct sockaddr_can addr = {}; + addr.can_family = AF_CAN; + addr.can_ifindex = ifr.ifr_ifindex; + + ret = connect(s_, (struct sockaddr *)&addr, sizeof(addr)); + + if (ret < 0) { + shutdown(); + return false; + } + + return true; + } + + template + bool startTX(DurationType period, Header header, size_t num, Frame * frames) + { + Message msg(num); + msg.setHeader(header); + msg.setIVal2(period); + + bcm_msg_head & head = msg.head(); + + head.opcode = TX_SETUP; + head.flags |= SETTIMER | STARTTIMER; + + for (size_t i = 0; i < num; ++i) { // msg nr + head.frames[i].can_dlc = frames[i].dlc; + head.frames[i].can_id = head.can_id; + for (size_t j = 0; j < head.frames[i].can_dlc; ++j) { // byte nr + head.frames[i].data[j] = frames[i].data[j]; + } + } + return msg.write(s_); + } + bool stopTX(Header header) + { + Message msg(0); + msg.head().opcode = TX_DELETE; + msg.setHeader(header); + return msg.write(s_); + } + void shutdown() + { + if (s_ > 0) { + close(s_); + s_ = -1; + } + } + + virtual ~BCMsocket() + { + shutdown(); + } +}; + +} // namespace can + +#endif // SOCKETCAN_INTERFACE__BCM_HPP_ diff --git a/socketcan_interface/include/socketcan_interface/delegates.h b/socketcan_interface/include/socketcan_interface/delegates.h deleted file mode 100644 index ffd9c818d..000000000 --- a/socketcan_interface/include/socketcan_interface/delegates.h +++ /dev/null @@ -1,24 +0,0 @@ -#ifndef SOCKETCAN_INTERFACE_DELEGATES_H_ -#define SOCKETCAN_INTERFACE_DELEGATES_H_ - -#include - -namespace can -{ - -template class DelegateHelper : public T { -public: - template - DelegateHelper(Object &&o, typename T::result_type (Instance::*member)(Args... args)) : - T([o, member](Args... args) -> typename T::result_type { return ((*o).*member)(args...); }) - { - } - template - DelegateHelper(Callable &&c) : T(c) - { - } -}; - -} // namespace can - -#endif // SOCKETCAN_INTERFACE_DELEGATES_H_ diff --git a/socketcan_interface/include/socketcan_interface/dispatcher.h b/socketcan_interface/include/socketcan_interface/dispatcher.h deleted file mode 100644 index 7c65784a7..000000000 --- a/socketcan_interface/include/socketcan_interface/dispatcher.h +++ /dev/null @@ -1,114 +0,0 @@ -#ifndef H_CAN_DISPATCHER -#define H_CAN_DISPATCHER - -#include -#include -#include -#include - -#include -#include - -namespace can{ - -template< typename Listener > class SimpleDispatcher{ -public: - using Callable = typename Listener::Callable; - using Type = typename Listener::Type; - using ListenerConstSharedPtr = typename Listener::ListenerConstSharedPtr; -protected: - class DispatcherBase; - using DispatcherBaseSharedPtr = std::shared_ptr; - class DispatcherBase { - DispatcherBase(const DispatcherBase&) = delete; // prevent copies - DispatcherBase& operator=(const DispatcherBase&) = delete; - - class GuardedListener: public Listener{ - std::weak_ptr guard_; - public: - GuardedListener(DispatcherBaseSharedPtr g, const Callable &callable): Listener(callable), guard_(g){} - virtual ~GuardedListener() { - DispatcherBaseSharedPtr d = guard_.lock(); - if(d){ - d->remove(this); - } - } - }; - - boost::mutex &mutex_; - std::list listeners_; - public: - DispatcherBase(boost::mutex &mutex) : mutex_(mutex) {} - void dispatch_nolock(const Type &obj) const{ - for(typename std::list::const_iterator it=listeners_.begin(); it != listeners_.end(); ++it){ - (**it)(obj); - } - } - void remove(Listener *d){ - boost::mutex::scoped_lock lock(mutex_); - listeners_.remove(d); - } - size_t numListeners(){ - boost::mutex::scoped_lock lock(mutex_); - return listeners_.size(); - } - - static ListenerConstSharedPtr createListener(DispatcherBaseSharedPtr dispatcher, const Callable &callable){ - ListenerConstSharedPtr l(new GuardedListener(dispatcher,callable)); - dispatcher->listeners_.push_back(l.get()); - return l; - } - }; - boost::mutex mutex_; - DispatcherBaseSharedPtr dispatcher_; -public: - SimpleDispatcher() : dispatcher_(new DispatcherBase(mutex_)) {} - ListenerConstSharedPtr createListener(const Callable &callable){ - boost::mutex::scoped_lock lock(mutex_); - return DispatcherBase::createListener(dispatcher_, callable); - } - void dispatch(const Type &obj){ - boost::mutex::scoped_lock lock(mutex_); - dispatcher_->dispatch_nolock(obj); - } - size_t numListeners(){ - return dispatcher_->numListeners(); - } - operator Callable() { return Callable(this,&SimpleDispatcher::dispatch); } -}; - -template > class FilteredDispatcher: public SimpleDispatcher{ - using BaseClass = SimpleDispatcher; - std::unordered_map filtered_; -public: - using BaseClass::createListener; - typename BaseClass::ListenerConstSharedPtr createListener(const K &key, const typename BaseClass::Callable &callable){ - boost::mutex::scoped_lock lock(BaseClass::mutex_); - typename BaseClass::DispatcherBaseSharedPtr &ptr = filtered_[key]; - if(!ptr) ptr.reset(new typename BaseClass::DispatcherBase(BaseClass::mutex_)); - return BaseClass::DispatcherBase::createListener(ptr, callable); - } - - template - [[deprecated("provide key explicitly")]] - typename BaseClass::ListenerConstSharedPtr createListener(const T &key, const typename BaseClass::Callable &callable){ - return createListener(static_cast(key), callable); - } - - void dispatch(const K &key, const typename BaseClass::Type &obj){ - boost::mutex::scoped_lock lock(BaseClass::mutex_); - typename BaseClass::DispatcherBaseSharedPtr &ptr = filtered_[key]; - if(ptr) ptr->dispatch_nolock(obj); - BaseClass::dispatcher_->dispatch_nolock(obj); - } - - [[deprecated("provide key explicitly")]] - void dispatch(const typename BaseClass::Type &obj){ - return dispatch(static_cast(obj), obj); - } - - operator typename BaseClass::Callable() { return typename BaseClass::Callable(this,&FilteredDispatcher::dispatch); } -}; - -} // namespace can -#endif diff --git a/socketcan_interface/include/socketcan_interface/dispatcher.hpp b/socketcan_interface/include/socketcan_interface/dispatcher.hpp new file mode 100644 index 000000000..9ac377b5e --- /dev/null +++ b/socketcan_interface/include/socketcan_interface/dispatcher.hpp @@ -0,0 +1,175 @@ +// Copyright (c) 2016-2019, Fraunhofer, Mathias Lüdtke, AutonomouStuff +// +// This program is free software: you can redistribute it and/or modify +// it under the terms of the GNU Lesser General Public License as published by +// the Free Software Foundation, either version 3 of the License, or +// (at your option) any later version. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU Lesser General Public License for more details. +// +// You should have received a copy of the GNU Lesser General Public License +// along with this program. If not, see . + +#ifndef SOCKETCAN_INTERFACE__DISPATCHER_HPP_ +#define SOCKETCAN_INTERFACE__DISPATCHER_HPP_ + +#include + +#include +#include +#include +#include + +#include "socketcan_interface/interface.hpp" + +namespace can +{ + +template +class SimpleDispatcher +{ +public: + using Callable = typename Listener::Callable; + using Type = typename Listener::Type; + using ListenerConstSharedPtr = typename Listener::ListenerConstSharedPtr; + + SimpleDispatcher() + : dispatcher_(new DispatcherBase(mutex_)) {} + + ListenerConstSharedPtr createListener(const Callable & callable) + { + boost::mutex::scoped_lock lock(mutex_); + return DispatcherBase::createListener(dispatcher_, callable); + } + + void dispatch(const Type & obj) + { + boost::mutex::scoped_lock lock(mutex_); + dispatcher_->dispatch_nolock(obj); + } + + size_t numListeners() + { + return dispatcher_->numListeners(); + } + + operator Callable() + { + return Callable(this, &SimpleDispatcher::dispatch); + } + +protected: + class DispatcherBase; + using DispatcherBaseSharedPtr = std::shared_ptr; + + class DispatcherBase + { + DispatcherBase(const DispatcherBase &) = delete; // prevent copies + DispatcherBase & operator=(const DispatcherBase &) = delete; + + boost::mutex & mutex_; + std::list listeners_; + +public: + explicit DispatcherBase(boost::mutex & mutex) + : mutex_(mutex) {} + + void dispatch_nolock(const Type & obj) const + { + for ( + typename std::list::const_iterator it = listeners_.begin(); + it != listeners_.end(); + ++it) + { + (**it)(obj); + } + } + + void remove(Listener * d) + { + boost::mutex::scoped_lock lock(mutex_); + listeners_.remove(d); + } + + size_t numListeners() + { + boost::mutex::scoped_lock lock(mutex_); + return listeners_.size(); + } + + static ListenerConstSharedPtr createListener( + DispatcherBaseSharedPtr dispatcher, const Callable & callable) + { + ListenerConstSharedPtr l(new GuardedListener(dispatcher, callable)); + dispatcher->listeners_.push_back(l.get()); + return l; + } + }; + + class GuardedListener + : public Listener + { + std::weak_ptr guard_; + +public: + GuardedListener(DispatcherBaseSharedPtr g, const Callable & callable) + : Listener(callable), guard_(g) {} + + virtual ~GuardedListener() + { + DispatcherBaseSharedPtr d = guard_.lock(); + if (d) { + d->remove(this); + } + } + }; + + boost::mutex mutex_; + DispatcherBaseSharedPtr dispatcher_; +}; + +template> +class FilteredDispatcher + : public SimpleDispatcher +{ + typedef SimpleDispatcher BaseClass; + std::unordered_map filtered_; + +public: + using BaseClass::createListener; + typename BaseClass::ListenerConstSharedPtr createListener( + const K & key, const typename BaseClass::Callable & callable) + { + boost::mutex::scoped_lock lock(BaseClass::mutex_); + typename BaseClass::DispatcherBaseSharedPtr & ptr = filtered_[key]; + if (!ptr) { + ptr.reset(new typename BaseClass::DispatcherBase(BaseClass::mutex_)); + } + return BaseClass::DispatcherBase::createListener(ptr, callable); + } + + void dispatch(const K & key, const typename BaseClass::Type & obj) + { + boost::mutex::scoped_lock lock(BaseClass::mutex_); + typename BaseClass::DispatcherBaseSharedPtr & ptr = filtered_[key]; + + if (ptr) { + ptr->dispatch_nolock(obj); + } + + BaseClass::dispatcher_->dispatch_nolock(obj); + } + void dispatch(const typename BaseClass::Type & obj) = delete; + + operator typename BaseClass::Callable() + { + return typename BaseClass::Callable(this, &FilteredDispatcher::dispatch); + } +}; + +} // namespace can + +#endif // SOCKETCAN_INTERFACE__DISPATCHER_HPP_ diff --git a/socketcan_interface/include/socketcan_interface/dummy.h b/socketcan_interface/include/socketcan_interface/dummy.h deleted file mode 100644 index 1baaed2e7..000000000 --- a/socketcan_interface/include/socketcan_interface/dummy.h +++ /dev/null @@ -1,102 +0,0 @@ -#ifndef SOCKETCAN_INTERFACE_DUMMY_H -#define SOCKETCAN_INTERFACE_DUMMY_H - -#include - -#include "interface.h" -#include "dispatcher.h" -#include "string.h" -#include - -namespace can{ - -class DummyInterface : public DriverInterface{ - using FrameDispatcher = FilteredDispatcher; - using StateDispatcher = SimpleDispatcher; - using Map = std::unordered_map; - FrameDispatcher frame_dispatcher_; - StateDispatcher state_dispatcher_; - State state_; - Map map_; - bool loopback_; - - bool add_noconv(const std::string &k, const Frame &v, bool multi){ - if(multi || map_.find(k) == map_.end()){ - map_.insert( std::make_pair(boost::to_lower_copy(k), v)); - return true; - } - return false; - } -public: - DummyInterface(bool loopback) : loopback_(loopback) {} - - bool add(const std::string &k, const Frame &v, bool multi){ - return add_noconv(boost::to_lower_copy(k), v, multi); - } - bool add(const Frame &k, const Frame &v, bool multi){ - return add_noconv(tostring(k,true), v, multi); - } - bool add(const std::string &k, const std::string &v, bool multi){ - return add(k, toframe(v), multi); - } - bool add(const Frame &k, const std::string &v, bool multi){ - return add(k, toframe(v), multi); - } - virtual bool send(const Frame & msg){ - if(loopback_) frame_dispatcher_.dispatch(msg.key(), msg); - try{ - std::pair r = map_.equal_range(tostring(msg, true)); - for (Map::iterator it=r.first; it!=r.second; ++it){ - frame_dispatcher_.dispatch(it->second.key(), it->second); - } - } - catch(const std::out_of_range &e){ - } - return true; - } - - virtual FrameListenerConstSharedPtr createMsgListener(const FrameFunc &delegate){ - return frame_dispatcher_.createListener(delegate); - } - virtual FrameListenerConstSharedPtr createMsgListener(const Frame::Header&h , const FrameFunc &delegate){ - return frame_dispatcher_.createListener(h.key(), delegate); - } - - // methods from StateInterface - virtual bool recover(){return false;}; - - virtual State getState(){return state_;}; - - virtual void shutdown(){}; - - virtual bool translateError(unsigned int internal_error, std::string & str){ - if (!internal_error) { - str = "OK"; - return true; - } - return false; - }; - - virtual bool doesLoopBack() const {return loopback_;}; - - virtual void run(){}; - - bool init(const std::string &device, bool loopback){ - loopback_ = loopback; - state_.driver_state = State::ready; - state_.internal_error = 0; - state_dispatcher_.dispatch(state_); - return true; - }; - - virtual StateListenerConstSharedPtr createStateListener(const StateFunc &delegate){ - return state_dispatcher_.createListener(delegate); - }; - -}; -using DummyInterfaceSharedPtr = std::shared_ptr; - - -} - -#endif diff --git a/socketcan_interface/include/socketcan_interface/dummy.hpp b/socketcan_interface/include/socketcan_interface/dummy.hpp new file mode 100644 index 000000000..b5652d748 --- /dev/null +++ b/socketcan_interface/include/socketcan_interface/dummy.hpp @@ -0,0 +1,156 @@ +// Copyright (c) 2016-2019, Fraunhofer, Mathias Lüdtke, AutonomouStuff +// +// This program is free software: you can redistribute it and/or modify +// it under the terms of the GNU Lesser General Public License as published by +// the Free Software Foundation, either version 3 of the License, or +// (at your option) any later version. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU Lesser General Public License for more details. +// +// You should have received a copy of the GNU Lesser General Public License +// along with this program. If not, see . + +#ifndef SOCKETCAN_INTERFACE__DUMMY_HPP_ +#define SOCKETCAN_INTERFACE__DUMMY_HPP_ + +#include + +#include +#include +#include +#include + +#include "socketcan_interface/interface.hpp" +#include "socketcan_interface/dispatcher.hpp" +#include "socketcan_interface/string.hpp" + +namespace can +{ + +class DummyInterface : public DriverInterface +{ + using FrameDispatcher = FilteredDispatcher; + using StateDispatcher = SimpleDispatcher; + using Map = std::unordered_map; + FrameDispatcher frame_dispatcher_; + StateDispatcher state_dispatcher_; + State state_; + Map map_; + bool loopback_; + + bool add_noconv(const std::string & k, const Frame & v, bool multi) + { + if (multi || map_.find(k) == map_.end()) { + map_.insert(std::make_pair(boost::to_lower_copy(k), v)); + return true; + } + + return false; + } + +public: + explicit DummyInterface(bool loopback) + : loopback_(loopback) {} + + bool add(const std::string & k, const Frame & v, bool multi) + { + return add_noconv(boost::to_lower_copy(k), v, multi); + } + + bool add(const Frame & k, const Frame & v, bool multi) + { + return add_noconv(tostring(k, true), v, multi); + } + + bool add(const std::string & k, const std::string & v, bool multi) + { + return add(k, toframe(v), multi); + } + + bool add(const Frame & k, const std::string & v, bool multi) + { + return add(k, toframe(v), multi); + } + + virtual bool send(const Frame & msg) + { + if (loopback_) { + frame_dispatcher_.dispatch(msg.key(), msg); + } + + try { + std::pair r = map_.equal_range(tostring(msg, true)); + + for (Map::iterator it = r.first; it != r.second; ++it) { + frame_dispatcher_.dispatch(it->second.key(), it->second); + } + } catch (const std::out_of_range & e) { + } + + return true; + } + + virtual FrameListenerConstSharedPtr createMsgListener(const FrameFunc & delegate) + { + return frame_dispatcher_.createListener(delegate); + } + + virtual FrameListenerConstSharedPtr createMsgListener( + const Frame::Header & h, const FrameFunc & delegate) + { + return frame_dispatcher_.createListener(h.key(), delegate); + } + + // methods from StateInterface + virtual bool recover() + { + return false; + } + + virtual State getState() + { + return state_; + } + + virtual void shutdown() {} + + virtual bool translateError(unsigned int internal_error, std::string & str) + { + if (!internal_error) { + str = "OK"; + return true; + } + + return false; + } + + virtual bool doesLoopBack() const + { + return loopback_; + } + + virtual void run() {} + + bool init(const std::string & device, bool loopback) + { + (void)device; + loopback_ = loopback; + state_.driver_state = State::ready; + state_.internal_error = 0; + state_dispatcher_.dispatch(state_); + return true; + } + + virtual StateListenerConstSharedPtr createStateListener(const StateFunc & delegate) + { + return state_dispatcher_.createListener(delegate); + } +}; +using DummyInterfaceSharedPtr = std::shared_ptr; + +} // namespace can + +#endif // SOCKETCAN_INTERFACE__DUMMY_HPP_ diff --git a/socketcan_interface/include/socketcan_interface/filter.h b/socketcan_interface/include/socketcan_interface/filter.h deleted file mode 100644 index 81e96f091..000000000 --- a/socketcan_interface/include/socketcan_interface/filter.h +++ /dev/null @@ -1,70 +0,0 @@ -#ifndef SOCKETCAN_INTERFACE_FILTER_H -#define SOCKETCAN_INTERFACE_FILTER_H - -#include - -#include "interface.h" - -namespace can { - -class FrameFilter { -public: - virtual bool pass(const can::Frame &frame) const = 0; - virtual ~FrameFilter() {} -}; -using FrameFilterSharedPtr = std::shared_ptr; - -class FrameMaskFilter : public FrameFilter { -public: - static const uint32_t MASK_ALL = 0xffffffff; - static const uint32_t MASK_RELAXED = ~Frame::EXTENDED_MASK; - FrameMaskFilter(uint32_t can_id, uint32_t mask = MASK_RELAXED, bool invert = false) - : mask_(mask), masked_id_(can_id & mask), invert_(invert) - {} - virtual bool pass(const can::Frame &frame) const{ - const uint32_t k = frame.key(); - return ((mask_ & k) == masked_id_) != invert_; - } -private: - const uint32_t mask_; - const uint32_t masked_id_; - const bool invert_; -}; - -class FrameRangeFilter : public FrameFilter { -public: - FrameRangeFilter(uint32_t min_id, uint32_t max_id, bool invert = false) - : min_id_(min_id), max_id_(max_id), invert_(invert) - {} - virtual bool pass(const can::Frame &frame) const{ - const uint32_t k = frame.key(); - return (min_id_ <= k && k <= max_id_) != invert_; - } -private: - const uint32_t min_id_; - const uint32_t max_id_; - const bool invert_; -}; - -class FilteredFrameListener : public CommInterface::FrameListener { -public: - using FilterVector = std::vector; - FilteredFrameListener(CommInterfaceSharedPtr comm, const Callable &callable, const FilterVector &filters) - : CommInterface::FrameListener(callable), - filters_(filters), - listener_(comm->createMsgListener([this](const Frame &frame) { - for(FilterVector::const_iterator it=this->filters_.begin(); it != this->filters_.end(); ++it) { - if((*it)->pass(frame)){ - (*this)(frame); - break; - } - } - })) - {} - const std::vector filters_; - CommInterface::FrameListenerConstSharedPtr listener_; -}; - -} // namespace can - -#endif /*SOCKETCAN_INTERFACE_FILTER_H*/ diff --git a/socketcan_interface/include/socketcan_interface/filter.hpp b/socketcan_interface/include/socketcan_interface/filter.hpp new file mode 100644 index 000000000..38304f148 --- /dev/null +++ b/socketcan_interface/include/socketcan_interface/filter.hpp @@ -0,0 +1,114 @@ +// Copyright (c) 2016-2019, Fraunhofer, Mathias Lüdtke, AutonomouStuff +// +// This program is free software: you can redistribute it and/or modify +// it under the terms of the GNU Lesser General Public License as published by +// the Free Software Foundation, either version 3 of the License, or +// (at your option) any later version. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU Lesser General Public License for more details. +// +// You should have received a copy of the GNU Lesser General Public License +// along with this program. If not, see . + +#ifndef SOCKETCAN_INTERFACE__FILTER_HPP_ +#define SOCKETCAN_INTERFACE__FILTER_HPP_ + +#include +#include + +#include "socketcan_interface/interface.hpp" + +namespace can +{ + +class FrameFilter +{ +public: + virtual bool pass(const can::Frame & frame) const = 0; + virtual ~FrameFilter() {} +}; +using FrameFilterSharedPtr = std::shared_ptr; + +class FrameMaskFilter : public FrameFilter +{ +public: + static const uint32_t MASK_ALL = 0xffffffff; + static const uint32_t MASK_RELAXED = ~Frame::EXTENDED_MASK; + explicit FrameMaskFilter( + uint32_t can_id, uint32_t mask = MASK_RELAXED, bool invert = false) + : mask_(mask), masked_id_(can_id & mask), invert_(invert) + {} + virtual bool pass(const can::Frame & frame) const + { + const uint32_t k = frame.key(); + return ((mask_ & k) == masked_id_) != invert_; + } + +private: + const uint32_t mask_; + const uint32_t masked_id_; + const bool invert_; +}; + +class FrameRangeFilter + : public FrameFilter +{ +public: + FrameRangeFilter(uint32_t min_id, uint32_t max_id, bool invert = false) + : min_id_(min_id), max_id_(max_id), invert_(invert) + {} + + virtual bool pass(const can::Frame & frame) const + { + const uint32_t k = frame.key(); + return (min_id_ <= k && k <= max_id_) != invert_; + } + +private: + const uint32_t min_id_; + const uint32_t max_id_; + const bool invert_; +}; + +class FilteredFrameListener : public CommInterface::FrameListener +{ +public: + using FilterVector = std::vector; + FilteredFrameListener( + CommInterfaceSharedPtr comm, const Callable & callable, const FilterVector & filters) + : CommInterface::FrameListener(callable), + filters_(filters), + listener_(comm->createMsgListener([this](const Frame & frame) + { + for (FilterVector::const_iterator it = this->filters_.begin(); + it != this->filters_.end(); ++it) + { + if ((*it)->pass(frame)) { + (*this)(frame); + break; + } + } + })) + {} + +private: + void filter(const Frame & frame) + { + for (FilterVector::const_iterator it = filters_.begin(); it != filters_.end(); ++it) { + if ((*it)->pass(frame)) { + (*this)(frame); + break; + } + } + } + + const std::vector filters_; + CommInterface::FrameListenerConstSharedPtr listener_; +}; + +} // namespace can + +#endif // SOCKETCAN_INTERFACE__FILTER_HPP_ diff --git a/socketcan_interface/include/socketcan_interface/interface.h b/socketcan_interface/include/socketcan_interface/interface.h deleted file mode 100644 index 9fd747348..000000000 --- a/socketcan_interface/include/socketcan_interface/interface.h +++ /dev/null @@ -1,222 +0,0 @@ -#ifndef H_CAN_INTERFACE -#define H_CAN_INTERFACE - -#include -#include -#include - -#include - -#include "socketcan_interface/delegates.h" - -namespace can{ - -/** Header for CAN id an meta data*/ -struct Header{ - static const unsigned int ID_MASK = (1u << 29)-1; - static const unsigned int ERROR_MASK = (1u << 29); - static const unsigned int RTR_MASK = (1u << 30); - static const unsigned int EXTENDED_MASK = (1u << 31); - - unsigned int id:29; ///< CAN ID (11 or 29 bits valid, depending on is_extended member - unsigned int is_error:1; ///< marks an error frame (only used internally) - unsigned int is_rtr:1; ///< frame is a remote transfer request - unsigned int is_extended:1; ///< frame uses 29 bit CAN identifier - /** check if frame header is valid*/ - bool isValid() const{ - return id < (is_extended?(1<<29):(1<<11)); - } - unsigned int fullid() const { return id | (is_error?ERROR_MASK:0) | (is_rtr?RTR_MASK:0) | (is_extended?EXTENDED_MASK:0); } - unsigned int key() const { return is_error ? (ERROR_MASK) : fullid(); } - [[deprecated("use key() instead")]] explicit operator unsigned int() const { return key(); } - - /** constructor with default parameters - * @param[in] i: CAN id, defaults to 0 - * @param[in] extended: uses 29 bit identifier, defaults to false - * @param[in] rtr: is rtr frame, defaults to false - */ - - Header() - : id(0),is_error(0),is_rtr(0), is_extended(0) {} - - Header(unsigned int i, bool extended, bool rtr, bool error) - : id(i),is_error(error?1:0),is_rtr(rtr?1:0), is_extended(extended?1:0) {} -}; - - -struct MsgHeader : public Header{ - MsgHeader(unsigned int i=0, bool rtr = false) : Header(i, false, rtr, false) {} -}; -struct ExtendedHeader : public Header{ - ExtendedHeader(unsigned int i=0, bool rtr = false) : Header(i, true, rtr, false) {} -}; -struct ErrorHeader : public Header{ - ErrorHeader(unsigned int i=0) : Header(i, false, false, true) {} -}; - - - -/** representation of a CAN frame */ -struct Frame: public Header{ - using value_type = unsigned char; - std::array data; ///< array for 8 data bytes with bounds checking - unsigned char dlc; ///< len of data - - /** check if frame header and length are valid*/ - bool isValid() const{ - return (dlc <= 8) && Header::isValid(); - } - /** - * constructor with default parameters - * @param[in] i: CAN id, defaults to 0 - * @param[in] l: number of data bytes, defaults to 0 - * @param[in] extended: uses 29 bit identifier, defaults to false - * @param[in] rtr: is rtr frame, defaults to false - */ - Frame() : Header(), dlc(0) {} - Frame(const Header &h, unsigned char l = 0) : Header(h), dlc(l) {} - - value_type * c_array() { return data.data(); } - const value_type * c_array() const { return data.data(); } -}; - -/** extended error information */ -class State{ -public: - enum DriverState{ - closed, open, ready - } driver_state; - boost::system::error_code error_code; ///< device access error - unsigned int internal_error; ///< driver specific error - - State() : driver_state(closed), internal_error(0) {} - virtual bool isReady() const { return driver_state == ready; } - virtual ~State() {} -}; - -/** template for Listener interface */ -template class Listener{ - const T callable_; -public: - using Type = U; - using Callable = T; - using ListenerConstSharedPtr = std::shared_ptr; - - Listener(const T &callable):callable_(callable){ } - void operator()(const U & u) const { if(callable_) callable_(u); } - virtual ~Listener() {} -}; - -class StateInterface{ -public: - using StateFunc = std::function; - using StateDelegate [[deprecated("use StateFunc instead")]] = DelegateHelper; - using StateListener = Listener; - using StateListenerConstSharedPtr = StateListener::ListenerConstSharedPtr; - - /** - * acquire a listener for the specified delegate, that will get called for all state changes - * - * @param[in] delegate: delegate to be bound by the listener - * @return managed pointer to listener - */ - virtual StateListenerConstSharedPtr createStateListener(const StateFunc &delegate) = 0; - template inline StateListenerConstSharedPtr createStateListenerM(Instance inst, Callable callable) { - return this->createStateListener(std::bind(callable, inst, std::placeholders::_1)); - } - - virtual ~StateInterface() {} -}; -using StateInterfaceSharedPtr = std::shared_ptr; -using StateListenerConstSharedPtr = StateInterface::StateListenerConstSharedPtr; - -class CommInterface{ -public: - using FrameFunc = std::function; - using FrameDelegate [[deprecated("use FrameFunc instead")]] = DelegateHelper; - using FrameListener = Listener; - using FrameListenerConstSharedPtr = FrameListener::ListenerConstSharedPtr; - - /** - * enqueue frame for sending - * - * @param[in] msg: message to be enqueued - * @return true if frame was enqueued succesfully, otherwise false - */ - virtual bool send(const Frame & msg) = 0; - - /** - * acquire a listener for the specified delegate, that will get called for all messages - * - * @param[in] delegate: delegate to be bound by the listener - * @return managed pointer to listener - */ - virtual FrameListenerConstSharedPtr createMsgListener(const FrameFunc &delegate) = 0; - template inline FrameListenerConstSharedPtr createMsgListenerM(Instance inst, Callable callable) { - return this->createMsgListener(std::bind(callable, inst, std::placeholders::_1)); - } - - /** - * acquire a listener for the specified delegate, that will get called for messages with demanded ID - * - * @param[in] header: CAN header to restrict listener on - * @param[in] delegate: delegate to be bound listener - * @return managed pointer to listener - */ - virtual FrameListenerConstSharedPtr createMsgListener(const Frame::Header&, const FrameFunc &delegate) = 0; - template inline FrameListenerConstSharedPtr createMsgListenerM(const Frame::Header& header, Instance inst, Callable callable) { - return this->createMsgListener(header, std::bind(callable, inst, std::placeholders::_1)); - } - - virtual ~CommInterface() {} -}; -using CommInterfaceSharedPtr = std::shared_ptr; -using FrameListenerConstSharedPtr = CommInterface::FrameListenerConstSharedPtr; - - -class DriverInterface : public CommInterface, public StateInterface { -public: - /** - * initialize interface - * - * @param[in] device: driver-specific device name/path - * @param[in] loopback: loop-back own messages - * @return true if device was initialized succesfully, false otherwise - */ - virtual bool init(const std::string &device, bool loopback) = 0; - - /** - * Recover interface after errors and emergency stops - * - * @return true if device was recovered succesfully, false otherwise - */ - virtual bool recover() = 0; - - /** - * @return current state of driver - */ - virtual State getState() = 0; - - /** - * shutdown interface - * - * @return true if shutdown was succesful, false otherwise - */ - virtual void shutdown() = 0; - - virtual bool translateError(unsigned int internal_error, std::string & str) = 0; - - virtual bool doesLoopBack() const = 0; - - virtual void run() = 0; - - virtual ~DriverInterface() {} -}; -using DriverInterfaceSharedPtr = std::shared_ptr; - - -} // namespace can - -#include "logging.h" - -#endif diff --git a/socketcan_interface/include/socketcan_interface/interface.hpp b/socketcan_interface/include/socketcan_interface/interface.hpp new file mode 100644 index 000000000..e29bc2162 --- /dev/null +++ b/socketcan_interface/include/socketcan_interface/interface.hpp @@ -0,0 +1,297 @@ +// Copyright (c) 2016-2019, Mathias Lüdtke, AutonomouStuff +// +// This program is free software: you can redistribute it and/or modify +// it under the terms of the GNU Lesser General Public License as published by +// the Free Software Foundation, either version 3 of the License, or +// (at your option) any later version. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU Lesser General Public License for more details. +// +// You should have received a copy of the GNU Lesser General Public License +// along with this program. If not, see . + +#ifndef SOCKETCAN_INTERFACE__INTERFACE_HPP_ +#define SOCKETCAN_INTERFACE__INTERFACE_HPP_ + +#include +#include + +#include +#include +#include +#include +#include + +#include "socketcan_interface/logging.hpp" + +namespace can +{ + +/** Header for CAN id an meta data*/ +struct Header +{ + static const unsigned int ID_MASK = (1u << 29) - 1; + static const unsigned int ERROR_MASK = (1u << 29); + static const unsigned int RTR_MASK = (1u << 30); + static const unsigned int EXTENDED_MASK = (1u << 31); + + unsigned int id : 29; // CAN ID (11 or 29 bits valid, depending on is_extended member + unsigned int is_error : 1; // marks an error frame (only used internally) + unsigned int is_rtr : 1; // frame is a remote transfer request + unsigned int is_extended : 1; // frame uses 29 bit CAN identifier + + /** check if frame header is valid*/ + bool isValid() const + { + return id < (is_extended ? (1 << 29) : (1 << 11)); + } + + unsigned int fullid() const + { + return + id | + (is_error ? ERROR_MASK : 0) | + (is_rtr ? RTR_MASK : 0) | + (is_extended ? EXTENDED_MASK : 0); + } + + unsigned int key() const + { + return is_error ? (ERROR_MASK) : fullid(); + } + + /** constructor with default parameters + * @param[in] i: CAN id, defaults to 0 + * @param[in] extended: uses 29 bit identifier, defaults to false + * @param[in] rtr: is rtr frame, defaults to false + */ + + Header() + : id(0), is_error(0), is_rtr(0), is_extended(0) {} + + Header(unsigned int i, bool extended, bool rtr, bool error) + : id(i), is_error(error ? 1 : 0), is_rtr(rtr ? 1 : 0), is_extended(extended ? 1 : 0) {} +}; + +struct MsgHeader : public Header +{ + explicit MsgHeader(unsigned int i = 0, bool rtr = false) + : Header(i, false, rtr, false) {} +}; +struct ExtendedHeader : public Header +{ + explicit ExtendedHeader(unsigned int i = 0, bool rtr = false) + : Header(i, true, rtr, false) {} +}; +struct ErrorHeader : public Header +{ + explicit ErrorHeader(unsigned int i = 0) + : Header(i, false, false, true) {} +}; + +/** representation of a CAN frame */ +struct Frame : public Header +{ + typedef unsigned char value_type; + std::array data; //< array for 8 data bytes with bounds checking + unsigned char dlc; //< len of data + + /** check if frame header and length are valid*/ + bool isValid() const + { + return (dlc <= 8) && Header::isValid(); + } + /** + * constructor with default parameters + * @param[in] i: CAN id, defaults to 0 + * @param[in] l: number of data bytes, defaults to 0 + * @param[in] extended: uses 29 bit identifier, defaults to false + * @param[in] rtr: is rtr frame, defaults to false + */ + Frame() + : Header(), dlc(0) {} + + explicit Frame(const Header & h, unsigned char l = 0) + : Header(h), dlc(l) {} + + value_type * c_array() + { + return data.data(); + } + + const value_type * c_array() const + { + return data.data(); + } +}; + +/** extended error information */ +class State +{ +public: + enum DriverState + { + closed, open, ready + } driver_state; + boost::system::error_code error_code; //< device access error + unsigned int internal_error; //< driver specific error + + State() + : driver_state(closed), internal_error(0) {} + virtual bool isReady() const + { + return driver_state == ready; + } + virtual ~State() {} +}; + +/** template for Listener interface */ +template +class Listener +{ + const T callable_; + +public: + using Type = U; + using Callable = T; + using ListenerConstSharedPtr = std::shared_ptr; + + explicit Listener(const T & callable) + : callable_(callable) {} + + void operator()(const U & u) const + { + if (callable_) { + callable_(u); + } + } + + virtual ~Listener() {} +}; + +class StateInterface +{ +public: + using StateFunc = std::function; + using StateListener = Listener; + using StateListenerConstSharedPtr = StateListener::ListenerConstSharedPtr; + + /** + * acquire a listener for the specified delegate, that will get called for all state changes + * + * @param[in] delegate: delegate to be bound by the listener + * @return managed pointer to listener + */ + virtual StateListenerConstSharedPtr createStateListener(const StateFunc & delegate) = 0; + + template + inline StateListenerConstSharedPtr createStateListenerM(Instance inst, Callable callable) + { + return this->createStateListener(std::bind(callable, inst, std::placeholders::_1)); + } + + virtual ~StateInterface() {} +}; +using StateInterfaceSharedPtr = std::shared_ptr; +using StateListenerConstSharedPtr = StateInterface::StateListenerConstSharedPtr; + +class CommInterface +{ +public: + using FrameFunc = std::function; + using FrameListener = Listener; + using FrameListenerConstSharedPtr = FrameListener::ListenerConstSharedPtr; + + /** + * enqueue frame for sending + * + * @param[in] msg: message to be enqueued + * @return true if frame was enqueued succesfully, otherwise false + */ + virtual bool send(const Frame & msg) = 0; + + /** + * acquire a listener for the specified delegate, that will get called for all messages + * + * @param[in] delegate: delegate to be bound by the listener + * @return managed pointer to listener + */ + virtual FrameListenerConstSharedPtr createMsgListener(const FrameFunc & delegate) = 0; + + template + inline FrameListenerConstSharedPtr createMsgListenerM(Instance inst, Callable callable) + { + return this->createMsgListener(std::bind(callable, inst, std::placeholders::_1)); + } + + /** + * acquire a listener for the specified delegate, that will get called for messages with demanded ID + * + * @param[in] header: CAN header to restrict listener on + * @param[in] delegate: delegate to be bound listener + * @return managed pointer to listener + */ + virtual FrameListenerConstSharedPtr createMsgListener( + const Frame::Header & header, const FrameFunc & delegate) = 0; + + template + inline FrameListenerConstSharedPtr createMsgListenerM( + const Frame::Header & header, Instance inst, Callable callable) + { + return this->createMsgListener(header, std::bind(callable, inst, std::placeholders::_1)); + } + + virtual ~CommInterface() {} +}; +using CommInterfaceSharedPtr = std::shared_ptr; +using FrameListenerConstSharedPtr = CommInterface::FrameListenerConstSharedPtr; + +class DriverInterface + : public CommInterface, public StateInterface +{ +public: + /** + * initialize interface + * + * @param[in] device: driver-specific device name/path + * @param[in] loopback: loop-back own messages + * @return true if device was initialized succesfully, false otherwise + */ + virtual bool init(const std::string & device, bool loopback) = 0; + + /** + * Recover interface after errors and emergency stops + * + * @return true if device was recovered succesfully, false otherwise + */ + virtual bool recover() = 0; + + /** + * @return current state of driver + */ + virtual State getState() = 0; + + /** + * shutdown interface + * + * @return true if shutdown was succesful, false otherwise + */ + virtual void shutdown() = 0; + + virtual bool translateError(unsigned int internal_error, std::string & str) = 0; + + virtual bool doesLoopBack() const = 0; + + virtual void run() = 0; + + virtual ~DriverInterface() {} +}; + +using DriverInterfaceSharedPtr = std::shared_ptr; + +} // namespace can + +#endif // SOCKETCAN_INTERFACE__INTERFACE_HPP_ diff --git a/socketcan_interface/include/socketcan_interface/logging.h b/socketcan_interface/include/socketcan_interface/logging.h deleted file mode 100644 index caf8d2ea9..000000000 --- a/socketcan_interface/include/socketcan_interface/logging.h +++ /dev/null @@ -1,17 +0,0 @@ -#ifndef SOCKETCAN_INTERFACE_LOGGING_H -#define SOCKETCAN_INTERFACE_LOGGING_H - -#include -#include - -#define ROSCANOPEN_LOG(name, file, line, level, args) { std::stringstream sstr; sstr << name << ": " << args; console_bridge::getOutputHandler()->log(sstr.str(), level, file, line); } - -#define ROSCANOPEN_ERROR(name, args) ROSCANOPEN_LOG(name, __FILE__, __LINE__, console_bridge::CONSOLE_BRIDGE_LOG_ERROR, args) -#define ROSCANOPEN_INFO(name, args) ROSCANOPEN_LOG(name, __FILE__, __LINE__, console_bridge::CONSOLE_BRIDGE_LOG_INFO, args) -#define ROSCANOPEN_WARN(name, args) ROSCANOPEN_LOG(name, __FILE__, __LINE__, console_bridge::CONSOLE_BRIDGE_LOG_WARN, args) -#define ROSCANOPEN_DEBUG(name, args) ROSCANOPEN_LOG(name, __FILE__, __LINE__,console_bridge::CONSOLE_BRIDGE_LOG_DEBUG, args) - -// extra function to mark it as deprecated -inline __attribute__ ((deprecated("please use ROSCANOPEN_* macros"))) void roscanopen_log_deprecated(const std::string s, const char* f, int l) { console_bridge::getOutputHandler()->log(s, console_bridge::CONSOLE_BRIDGE_LOG_ERROR, f, l); } -#define LOG(args) { std::stringstream sstr; sstr << "LOG: " << args; roscanopen_log_deprecated(sstr.str(), __FILE__, __LINE__); } -#endif diff --git a/socketcan_interface/include/socketcan_interface/logging.hpp b/socketcan_interface/include/socketcan_interface/logging.hpp new file mode 100644 index 000000000..81f9da9f1 --- /dev/null +++ b/socketcan_interface/include/socketcan_interface/logging.hpp @@ -0,0 +1,57 @@ +// Copyright (c) 2016-2019, Fraunhofer, Mathias Lüdtke, AutonomouStuff +// +// This program is free software: you can redistribute it and/or modify +// it under the terms of the GNU Lesser General Public License as published by +// the Free Software Foundation, either version 3 of the License, or +// (at your option) any later version. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU Lesser General Public License for more details. +// +// You should have received a copy of the GNU Lesser General Public License +// along with this program. If not, see . + +#ifndef SOCKETCAN_INTERFACE__LOGGING_HPP_ +#define SOCKETCAN_INTERFACE__LOGGING_HPP_ + +#include + +#include +#include + +#define ROSCANOPEN_LOG(name, file, line, level, args) \ + { \ + std::stringstream sstr; \ + sstr << name << ": " << args; \ + console_bridge::getOutputHandler()->log(sstr.str(), level, file, line); \ + } + +#define ROSCANOPEN_ERROR(name, args) ROSCANOPEN_LOG( \ + name, __FILE__, __LINE__, \ + console_bridge::CONSOLE_BRIDGE_LOG_ERROR, args) +#define ROSCANOPEN_INFO(name, args) ROSCANOPEN_LOG( \ + name, __FILE__, __LINE__, \ + console_bridge::CONSOLE_BRIDGE_LOG_INFO, args) +#define ROSCANOPEN_WARN(name, args) ROSCANOPEN_LOG( \ + name, __FILE__, __LINE__, \ + console_bridge::CONSOLE_BRIDGE_LOG_WARN, args) +#define ROSCANOPEN_DEBUG(name, args) ROSCANOPEN_LOG( \ + name, __FILE__, __LINE__, \ + console_bridge::CONSOLE_BRIDGE_LOG_DEBUG, args) + +// extra function to mark it as deprecated +inline __attribute__ ((deprecated("please use ROSCANOPEN_* macros"))) +void roscanopen_log_deprecated(const std::string s, const char * f, int l) +{ + console_bridge::getOutputHandler()->log(s, console_bridge::CONSOLE_BRIDGE_LOG_ERROR, f, l); +} + +#define LOG(args) \ + { \ + std::stringstream sstr; sstr << "LOG: " << args; \ + roscanopen_log_deprecated(sstr.str(), __FILE__, __LINE__); \ + } + +#endif // SOCKETCAN_INTERFACE__LOGGING_HPP_ diff --git a/socketcan_interface/include/socketcan_interface/make_shared.h b/socketcan_interface/include/socketcan_interface/make_shared.h deleted file mode 100644 index aedb0ba1a..000000000 --- a/socketcan_interface/include/socketcan_interface/make_shared.h +++ /dev/null @@ -1,7 +0,0 @@ -#ifndef SOCKETCAN_INTERFACE_MAKE_SHARED_H -#define SOCKETCAN_INTERFACE_MAKE_SHARED_H - -#include -#define ROSCANOPEN_MAKE_SHARED std::make_shared - -#endif // ! SOCKETCAN_INTERFACE_MAKE_SHARED_H diff --git a/socketcan_interface/include/socketcan_interface/make_shared.hpp b/socketcan_interface/include/socketcan_interface/make_shared.hpp new file mode 100644 index 000000000..763b93f45 --- /dev/null +++ b/socketcan_interface/include/socketcan_interface/make_shared.hpp @@ -0,0 +1,22 @@ +// Copyright (c) 2019 Mathias Lüdtke +// +// This program is free software: you can redistribute it and/or modify +// it under the terms of the GNU Lesser General Public License as published by +// the Free Software Foundation, either version 3 of the License, or +// (at your option) any later version. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU Lesser General Public License for more details. +// +// You should have received a copy of the GNU Lesser General Public License +// along with this program. If not, see . + +#ifndef SOCKETCAN_INTERFACE__MAKE_SHARED_HPP_ +#define SOCKETCAN_INTERFACE__MAKE_SHARED_HPP_ + +#include +#define ROSCANOPEN_MAKE_SHARED std::make_shared + +#endif // SOCKETCAN_INTERFACE__MAKE_SHARED_HPP_ diff --git a/socketcan_interface/include/socketcan_interface/reader.h b/socketcan_interface/include/socketcan_interface/reader.h deleted file mode 100644 index 8c6f2aa69..000000000 --- a/socketcan_interface/include/socketcan_interface/reader.h +++ /dev/null @@ -1,114 +0,0 @@ -#ifndef H_CAN_BUFFERED_READER -#define H_CAN_BUFFERED_READER - -#include -#include - -#include -#include -#include - -namespace can{ - -class BufferedReader { - std::deque buffer_; - boost::mutex mutex_; - boost::condition_variable cond_; - CommInterface::FrameListenerConstSharedPtr listener_; - bool enabled_; - size_t max_len_; - - void trim(){ - if(max_len_ > 0){ - while(buffer_.size() > max_len_){ - ROSCANOPEN_ERROR("socketcan_interface", "buffer overflow, discarded oldest message " /*<< tostring(buffer_.front())*/); // enable message printing - buffer_.pop_front(); - } - } - } - void handleFrame(const can::Frame & msg){ - boost::mutex::scoped_lock lock(mutex_); - if(enabled_){ - buffer_.push_back(msg); - trim(); - cond_.notify_one(); - }else{ - ROSCANOPEN_WARN("socketcan_interface", "discarded message " /*<< tostring(msg)*/); // enable message printing - } - } -public: - class ScopedEnabler{ - BufferedReader &reader_; - bool before_; - public: - ScopedEnabler(BufferedReader &reader) : reader_(reader), before_(reader_.setEnabled(true)) {} - ~ScopedEnabler() { reader_.setEnabled(before_); } - }; - - BufferedReader() : enabled_(true), max_len_(0) {} - BufferedReader(bool enable, size_t max_len = 0) : enabled_(enable), max_len_(max_len) {} - - void flush(){ - boost::mutex::scoped_lock lock(mutex_); - buffer_.clear(); - } - void setMaxLen(size_t max_len){ - boost::mutex::scoped_lock lock(mutex_); - max_len_ = max_len; - trim(); - } - bool isEnabled(){ - boost::mutex::scoped_lock lock(mutex_); - return enabled_; - } - bool setEnabled(bool enabled){ - boost::mutex::scoped_lock lock(mutex_); - bool before = enabled_; - enabled_ = enabled; - return before; - } - void enable(){ - boost::mutex::scoped_lock lock(mutex_); - enabled_ = true; - } - - void disable(){ - boost::mutex::scoped_lock lock(mutex_); - enabled_ = false; - } - - void listen(CommInterfaceSharedPtr interface){ - boost::mutex::scoped_lock lock(mutex_); - listener_ = interface->createMsgListenerM(this, &BufferedReader::handleFrame); - buffer_.clear(); - } - void listen(CommInterfaceSharedPtr interface, const Frame::Header& h){ - boost::mutex::scoped_lock lock(mutex_); - listener_ = interface->createMsgListenerM(h, this, &BufferedReader::handleFrame); - buffer_.clear(); - } - - template bool read(can::Frame * msg, const DurationType &duration){ - return readUntil(msg, boost::chrono::high_resolution_clock::now() + duration); - } - bool readUntil(can::Frame * msg, boost::chrono::high_resolution_clock::time_point abs_time){ - boost::mutex::scoped_lock lock(mutex_); - - while(buffer_.empty() && cond_.wait_until(lock,abs_time) != boost::cv_status::timeout) - {} - - if(buffer_.empty()){ - return false; - } - - if(msg){ - *msg = buffer_.front(); - buffer_.pop_front(); - } - return true; - } - -}; - -} // namespace can -#endif diff --git a/socketcan_interface/include/socketcan_interface/reader.hpp b/socketcan_interface/include/socketcan_interface/reader.hpp new file mode 100644 index 000000000..ae77b7ba3 --- /dev/null +++ b/socketcan_interface/include/socketcan_interface/reader.hpp @@ -0,0 +1,164 @@ +// Copyright (c) 2016-2019, Fraunhofer, Mathias Lüdtke, AutonomouStuff +// +// This program is free software: you can redistribute it and/or modify +// it under the terms of the GNU Lesser General Public License as published by +// the Free Software Foundation, either version 3 of the License, or +// (at your option) any later version. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU Lesser General Public License for more details. +// +// You should have received a copy of the GNU Lesser General Public License +// along with this program. If not, see . + +#ifndef SOCKETCAN_INTERFACE__READER_HPP_ +#define SOCKETCAN_INTERFACE__READER_HPP_ + +#include +#include +#include + +#include + +#include + +namespace can +{ + +class BufferedReader +{ + std::deque buffer_; + boost::mutex mutex_; + boost::condition_variable cond_; + CommInterface::FrameListenerConstSharedPtr listener_; + bool enabled_; + size_t max_len_; + + void trim() + { + if (max_len_ > 0) { + while (buffer_.size() > max_len_) { + ROSCANOPEN_ERROR( + "socketcan_interface", + "buffer overflow, discarded oldest message "); + buffer_.pop_front(); + } + } + } + + void handleFrame(const can::Frame & msg) + { + boost::mutex::scoped_lock lock(mutex_); + if (enabled_) { + buffer_.push_back(msg); + trim(); + cond_.notify_one(); + } else { + ROSCANOPEN_WARN( + "socketcan_interface", + "discarded message "); + } + } + +public: + class ScopedEnabler + { + BufferedReader & reader_; + bool before_; + +public: + explicit ScopedEnabler(BufferedReader & reader) + : reader_(reader), before_(reader_.setEnabled(true)) + {} + + ~ScopedEnabler() + { + reader_.setEnabled(before_); + } + }; + + BufferedReader() + : enabled_(true), max_len_(0) {} + + explicit BufferedReader(bool enable, size_t max_len = 0) + : enabled_(enable), max_len_(max_len) {} + + void flush() + { + boost::mutex::scoped_lock lock(mutex_); + buffer_.clear(); + } + void setMaxLen(size_t max_len) + { + boost::mutex::scoped_lock lock(mutex_); + max_len_ = max_len; + trim(); + } + bool isEnabled() + { + boost::mutex::scoped_lock lock(mutex_); + return enabled_; + } + bool setEnabled(bool enabled) + { + boost::mutex::scoped_lock lock(mutex_); + bool before = enabled_; + enabled_ = enabled; + return before; + } + void enable() + { + boost::mutex::scoped_lock lock(mutex_); + enabled_ = true; + } + + void disable() + { + boost::mutex::scoped_lock lock(mutex_); + enabled_ = false; + } + + void listen(CommInterfaceSharedPtr interface) + { + boost::mutex::scoped_lock lock(mutex_); + listener_ = interface->createMsgListenerM(this, &BufferedReader::handleFrame); + buffer_.clear(); + } + void listen(CommInterfaceSharedPtr interface, const Frame::Header & h) + { + boost::mutex::scoped_lock lock(mutex_); + listener_ = interface->createMsgListenerM(h, this, &BufferedReader::handleFrame); + buffer_.clear(); + } + + template + bool read(can::Frame * msg, const DurationType & duration) + { + return readUntil(msg, boost::chrono::high_resolution_clock::now() + duration); + } + + bool readUntil(can::Frame * msg, boost::chrono::high_resolution_clock::time_point abs_time) + { + boost::mutex::scoped_lock lock(mutex_); + + while (buffer_.empty() && cond_.wait_until(lock, abs_time) != boost::cv_status::timeout) + {} + + if (buffer_.empty()) { + return false; + } + + if (msg) { + *msg = buffer_.front(); + buffer_.pop_front(); + } + + return true; + } +}; + +} // namespace can + +#endif // SOCKETCAN_INTERFACE__READER_HPP_ diff --git a/socketcan_interface/include/socketcan_interface/socketcan.h b/socketcan_interface/include/socketcan_interface/socketcan.h deleted file mode 100644 index 10ae56db3..000000000 --- a/socketcan_interface/include/socketcan_interface/socketcan.h +++ /dev/null @@ -1,234 +0,0 @@ -#ifndef H_SOCKETCAN_DRIVER -#define H_SOCKETCAN_DRIVER - -#include -#include - -#include -#include -#include -#include - -#include -#include -#include - -#include - -#include -#include - -namespace can { - -class SocketCANInterface : public AsioDriver { - bool loopback_; - int sc_; -public: - SocketCANInterface() - : loopback_(false), sc_(-1) - {} - - virtual bool doesLoopBack() const{ - return loopback_; - } - - virtual bool init(const std::string &device, bool loopback){ - State s = getState(); - if(s.driver_state == State::closed){ - sc_ = 0; - device_ = device; - loopback_ = loopback; - - int sc = socket( PF_CAN, SOCK_RAW, CAN_RAW ); - if(sc < 0){ - setErrorCode(boost::system::error_code(sc,boost::system::system_category())); - return false; - } - - struct ifreq ifr; - strcpy(ifr.ifr_name, device_.c_str()); - int ret = ioctl(sc, SIOCGIFINDEX, &ifr); - - if(ret != 0){ - setErrorCode(boost::system::error_code(ret,boost::system::system_category())); - close(sc); - return false; - } - can_err_mask_t err_mask = - ( CAN_ERR_TX_TIMEOUT /* TX timeout (by netdevice driver) */ - | CAN_ERR_LOSTARB /* lost arbitration / data[0] */ - | CAN_ERR_CRTL /* controller problems / data[1] */ - | CAN_ERR_PROT /* protocol violations / data[2..3] */ - | CAN_ERR_TRX /* transceiver status / data[4] */ - | CAN_ERR_ACK /* received no ACK on transmission */ - | CAN_ERR_BUSOFF /* bus off */ - //CAN_ERR_BUSERROR /* bus error (may flood!) */ - | CAN_ERR_RESTARTED /* controller restarted */ - ); - - ret = setsockopt(sc, SOL_CAN_RAW, CAN_RAW_ERR_FILTER, - &err_mask, sizeof(err_mask)); - - if(ret != 0){ - setErrorCode(boost::system::error_code(ret,boost::system::system_category())); - close(sc); - return false; - } - - if(loopback_){ - int recv_own_msgs = 1; /* 0 = disabled (default), 1 = enabled */ - ret = setsockopt(sc, SOL_CAN_RAW, CAN_RAW_RECV_OWN_MSGS, &recv_own_msgs, sizeof(recv_own_msgs)); - - if(ret != 0){ - setErrorCode(boost::system::error_code(ret,boost::system::system_category())); - close(sc); - return false; - } - } - - struct sockaddr_can addr = {0}; - addr.can_family = AF_CAN; - addr.can_ifindex = ifr.ifr_ifindex; - ret = bind( sc, (struct sockaddr*)&addr, sizeof(addr) ); - - if(ret != 0){ - setErrorCode(boost::system::error_code(ret,boost::system::system_category())); - close(sc); - return false; - } - - boost::system::error_code ec; - socket_.assign(sc,ec); - - setErrorCode(ec); - - if(ec){ - close(sc); - return false; - } - setInternalError(0); - setDriverState(State::open); - sc_ = sc; - return true; - } - return getState().isReady(); - } - virtual bool recover(){ - if(!getState().isReady()){ - shutdown(); - return init(device_, doesLoopBack()); - } - return getState().isReady(); - } - virtual bool translateError(unsigned int internal_error, std::string & str){ - - bool ret = false; - if(!internal_error){ - str = "OK"; - ret = true; - } - if( internal_error & CAN_ERR_TX_TIMEOUT){ - str += "TX timeout (by netdevice driver);"; - ret = true; - } - if( internal_error & CAN_ERR_LOSTARB){ - str += "lost arbitration;"; - ret = true; - } - if( internal_error & CAN_ERR_CRTL){ - str += "controller problems;"; - ret = true; - } - if( internal_error & CAN_ERR_PROT){ - str += "protocol violations;"; - ret = true; - } - if( internal_error & CAN_ERR_TRX){ - str += "transceiver status;"; - ret = true; - } - if( internal_error & CAN_ERR_BUSOFF){ - str += "bus off;"; - ret = true; - } - if( internal_error & CAN_ERR_RESTARTED){ - str += "controller restarted;"; - ret = true; - } - return ret; - } - int getInternalSocket() { - return sc_; - } -protected: - std::string device_; - can_frame frame_; - - virtual void triggerReadSome(){ - boost::mutex::scoped_lock lock(send_mutex_); - socket_.async_read_some(boost::asio::buffer(&frame_, sizeof(frame_)), boost::bind( &SocketCANInterface::readFrame,this, boost::asio::placeholders::error)); - } - - virtual bool enqueue(const Frame & msg){ - boost::mutex::scoped_lock lock(send_mutex_); //TODO: timed try lock - - can_frame frame = {0}; - frame.can_id = msg.id | (msg.is_extended?CAN_EFF_FLAG:0) | (msg.is_rtr?CAN_RTR_FLAG:0);; - frame.can_dlc = msg.dlc; - - - for(int i=0; i < frame.can_dlc;++i) - frame.data[i] = msg.data[i]; - - boost::system::error_code ec; - boost::asio::write(socket_, boost::asio::buffer(&frame, sizeof(frame)),boost::asio::transfer_all(), ec); - if(ec){ - ROSCANOPEN_ERROR("socketcan_interface", "FAILED " << ec); - setErrorCode(ec); - setNotReady(); - return false; - } - - return true; - } - - void readFrame(const boost::system::error_code& error){ - if(!error){ - input_.dlc = frame_.can_dlc; - for(int i=0;i; -using SocketCANInterfaceSharedPtr = std::shared_ptr; - -template class ThreadedInterface; -using ThreadedSocketCANInterface = ThreadedInterface; -using ThreadedSocketCANInterfaceSharedPtr = std::shared_ptr; - - -} // namespace can -#endif diff --git a/socketcan_interface/include/socketcan_interface/socketcan.hpp b/socketcan_interface/include/socketcan_interface/socketcan.hpp new file mode 100644 index 000000000..a4ebbdffc --- /dev/null +++ b/socketcan_interface/include/socketcan_interface/socketcan.hpp @@ -0,0 +1,293 @@ +// Copyright (c) 2016-2019, Fraunhofer, Mathias Lüdtke, AutonomouStuff +// +// This program is free software: you can redistribute it and/or modify +// it under the terms of the GNU Lesser General Public License as published by +// the Free Software Foundation, either version 3 of the License, or +// (at your option) any later version. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU Lesser General Public License for more details. +// +// You should have received a copy of the GNU Lesser General Public License +// along with this program. If not, see . + +#ifndef SOCKETCAN_INTERFACE__SOCKETCAN_HPP_ +#define SOCKETCAN_INTERFACE__SOCKETCAN_HPP_ + +#include + +#include +#include +#include +#include + +#include +#include +#include + +#include +#include + +#include "socketcan_interface/asio_base.hpp" +#include "socketcan_interface/dispatcher.hpp" + +namespace can +{ + +class SocketCANInterface + : public AsioDriver +{ + bool loopback_; + int sc_; + +public: + SocketCANInterface() + : loopback_(false), sc_(-1) + {} + + virtual bool doesLoopBack() const + { + return loopback_; + } + + virtual bool init(const std::string & device, bool loopback) + { + State s = getState(); + + if (s.driver_state == State::closed) { + sc_ = 0; + device_ = device; + loopback_ = loopback; + + int sc = socket(PF_CAN, SOCK_RAW, CAN_RAW); + + if (sc < 0) { + setErrorCode(boost::system::error_code(sc, boost::system::system_category())); + return false; + } + + struct ifreq ifr; + snprintf(ifr.ifr_name, sizeof(ifr.ifr_name), "%s", device_.c_str()); + int ret = ioctl(sc, SIOCGIFINDEX, &ifr); + + if (ret != 0) { + setErrorCode(boost::system::error_code(ret, boost::system::system_category())); + close(sc); + return false; + } + + can_err_mask_t err_mask = ( + CAN_ERR_TX_TIMEOUT | /* TX timeout (by netdevice driver) */ + CAN_ERR_LOSTARB | /* lost arbitration / data[0] */ + CAN_ERR_CRTL | /* controller problems / data[1] */ + CAN_ERR_PROT | /* protocol violations / data[2..3] */ + CAN_ERR_TRX | /* transceiver status / data[4] */ + CAN_ERR_ACK | /* received no ACK on transmission */ + CAN_ERR_BUSOFF | /* bus off */ + // CAN_ERR_BUSERROR | /* bus error (may flood!) */ + CAN_ERR_RESTARTED /* controller restarted */ + ); + + ret = setsockopt(sc, SOL_CAN_RAW, CAN_RAW_ERR_FILTER, &err_mask, sizeof(err_mask)); + + if (ret != 0) { + setErrorCode(boost::system::error_code(ret, boost::system::system_category())); + close(sc); + return false; + } + + if (loopback_) { + int recv_own_msgs = 1; // 0 = disabled (default), 1 = enabled + ret = setsockopt( + sc, + SOL_CAN_RAW, + CAN_RAW_RECV_OWN_MSGS, + &recv_own_msgs, + sizeof(recv_own_msgs)); + + if (ret != 0) { + setErrorCode(boost::system::error_code(ret, boost::system::system_category())); + close(sc); + return false; + } + } + + struct sockaddr_can addr = {}; + addr.can_family = AF_CAN; + addr.can_ifindex = ifr.ifr_ifindex; + ret = bind(sc, (struct sockaddr *)&addr, sizeof(addr)); + + if (ret != 0) { + setErrorCode(boost::system::error_code(ret, boost::system::system_category())); + close(sc); + return false; + } + + boost::system::error_code ec; + socket_.assign(sc, ec); + + setErrorCode(ec); + + if (ec) { + close(sc); + return false; + } + + setInternalError(0); + setDriverState(State::open); + sc_ = sc; + return true; + } + + return getState().isReady(); + } + + virtual bool recover() + { + if (!getState().isReady()) { + shutdown(); + return init(device_, doesLoopBack()); + } + + return getState().isReady(); + } + + virtual bool translateError(unsigned int internal_error, std::string & str) + { + bool ret = false; + + if (!internal_error) { + str = "OK"; + ret = true; + } + + if (internal_error & CAN_ERR_TX_TIMEOUT) { + str += "TX timeout (by netdevice driver);"; + ret = true; + } + + if (internal_error & CAN_ERR_LOSTARB) { + str += "lost arbitration;"; + ret = true; + } + + if (internal_error & CAN_ERR_CRTL) { + str += "controller problems;"; + ret = true; + } + if (internal_error & CAN_ERR_PROT) { + str += "protocol violations;"; + ret = true; + } + + if (internal_error & CAN_ERR_TRX) { + str += "transceiver status;"; + ret = true; + } + + if (internal_error & CAN_ERR_BUSOFF) { + str += "bus off;"; + ret = true; + } + + if (internal_error & CAN_ERR_RESTARTED) { + str += "ontroller restarted;"; + ret = true; + } + + return ret; + } + + int getInternalSocket() + { + return sc_; + } + +protected: + std::string device_; + can_frame frame_; + + virtual void triggerReadSome() + { + boost::mutex::scoped_lock lock(send_mutex_); + socket_.async_read_some( + boost::asio::buffer( + &frame_, + sizeof(frame_)), + boost::bind(&SocketCANInterface::readFrame, this, boost::asio::placeholders::error)); + } + + virtual bool enqueue(const Frame & msg) + { + boost::mutex::scoped_lock lock(send_mutex_); + + can_frame frame = {}; + frame.can_id = msg.id | (msg.is_extended ? CAN_EFF_FLAG : 0) | (msg.is_rtr ? CAN_RTR_FLAG : 0); + frame.can_dlc = msg.dlc; + + for (int i = 0; i < frame.can_dlc; ++i) { + frame.data[i] = msg.data[i]; + } + + boost::system::error_code ec; + boost::asio::write( + socket_, + boost::asio::buffer(&frame, sizeof(frame)), + boost::asio::transfer_all(), + ec); + + if (ec) { + ROSCANOPEN_ERROR("socketcan_interface", "FAILED " << ec); + setErrorCode(ec); + setNotReady(); + return false; + } + + return true; + } + + void readFrame(const boost::system::error_code & error) + { + if (!error) { + input_.dlc = frame_.can_dlc; + + for (int i = 0; i < frame_.can_dlc && i < 8; ++i) { + input_.data[i] = frame_.data[i]; + } + + if (frame_.can_id & CAN_ERR_FLAG) { // error message + input_.id = frame_.can_id & CAN_EFF_MASK; + input_.is_error = 1; + + ROSCANOPEN_ERROR("socketcan_interface", "error: " << input_.id); + setInternalError(input_.id); + setNotReady(); + } else { + input_.is_extended = (frame_.can_id & CAN_EFF_FLAG) ? 1 : 0; + input_.id = frame_.can_id & (input_.is_extended ? CAN_EFF_MASK : CAN_SFF_MASK); + input_.is_error = 0; + input_.is_rtr = (frame_.can_id & CAN_RTR_FLAG) ? 1 : 0; + } + } + + frameReceived(error); + } + +private: + boost::mutex send_mutex_; +}; + +using SocketCANDriver = SocketCANInterface; +using SocketCANDriverSharedPtr = std::shared_ptr; +using SocketCANInterfaceSharedPtr = std::shared_ptr; + +template +class ThreadedInterface; +using ThreadedSocketCANInterface = ThreadedInterface; +using ThreadedSocketCANInterfaceSharedPtr = std::shared_ptr; + +} // namespace can + +#endif // SOCKETCAN_INTERFACE__SOCKETCAN_HPP_ diff --git a/socketcan_interface/include/socketcan_interface/string.h b/socketcan_interface/include/socketcan_interface/string.h deleted file mode 100644 index 390e8967a..000000000 --- a/socketcan_interface/include/socketcan_interface/string.h +++ /dev/null @@ -1,47 +0,0 @@ -#ifndef SOCKETCAN_INTERFACE_STRING_H -#define SOCKETCAN_INTERFACE_STRING_H - -#include "interface.h" -#include "filter.h" -#include - -namespace can { - -bool hex2dec(uint8_t& d, const char& h); - -bool hex2buffer(std::string& out, const std::string& in_raw, bool pad); - -bool dec2hex(char& h, const uint8_t& d, bool lc); - -std::string byte2hex(const uint8_t& d, bool pad, bool lc); - - -std::string buffer2hex(const std::string& in, bool lc); - -std::string tostring(const Header& h, bool lc); - -Header toheader(const std::string& s); - -std::string tostring(const Frame& f, bool lc); - -Frame toframe(const std::string& s); - -template FrameFilterSharedPtr tofilter(const T &ct); -template<> FrameFilterSharedPtr tofilter(const std::string &s); -template<> FrameFilterSharedPtr tofilter(const uint32_t &id); - -FrameFilterSharedPtr tofilter(const char* s); - -template FilteredFrameListener::FilterVector tofilters(const T& v) { - FilteredFrameListener::FilterVector filters; - for(size_t i = 0; i < static_cast(v.size()); ++i){ - filters.push_back(tofilter(v[i])); - } - return filters; -} - -std::ostream& operator <<(std::ostream& stream, const Header& h); -std::ostream& operator <<(std::ostream& stream, const Frame& f); - -} -#endif diff --git a/socketcan_interface/include/socketcan_interface/string.hpp b/socketcan_interface/include/socketcan_interface/string.hpp new file mode 100644 index 000000000..fe30af1d9 --- /dev/null +++ b/socketcan_interface/include/socketcan_interface/string.hpp @@ -0,0 +1,69 @@ +// Copyright (c) 2016-2019, Fraunhofer, Mathias Lüdtke, AutonomouStuff +// +// This program is free software: you can redistribute it and/or modify +// it under the terms of the GNU Lesser General Public License as published by +// the Free Software Foundation, either version 3 of the License, or +// (at your option) any later version. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU Lesser General Public License for more details. +// +// You should have received a copy of the GNU Lesser General Public License +// along with this program. If not, see . + +#ifndef SOCKETCAN_INTERFACE__STRING_HPP_ +#define SOCKETCAN_INTERFACE__STRING_HPP_ + +#include +#include + +#include "socketcan_interface/interface.hpp" +#include "socketcan_interface/filter.hpp" + +namespace can +{ +bool hex2dec(uint8_t & d, const char & h); + +bool hex2buffer(std::string & out, const std::string & in_raw, bool pad); + +bool dec2hex(char & h, const uint8_t & d, bool lc); + +std::string byte2hex(const uint8_t & d, bool pad, bool lc); + +std::string buffer2hex(const std::string & in, bool lc); + +std::string tostring(const Header & h, bool lc); + +Header toheader(const std::string & s); + +std::string tostring(const Frame & f, bool lc); + +Frame toframe(const std::string & s); + +template +FrameFilterSharedPtr tofilter(const T & ct); +template<> +FrameFilterSharedPtr tofilter(const std::string & s); +template<> +FrameFilterSharedPtr tofilter(const uint32_t & id); + +FrameFilterSharedPtr tofilter(const char * s); + +template +FilteredFrameListener::FilterVector tofilters(const T & v) +{ + FilteredFrameListener::FilterVector filters; + for (size_t i = 0; i < static_cast(v.size()); ++i) { + filters.push_back(tofilter(v[i])); + } + + return filters; +} + +std::ostream & operator<<(std::ostream & stream, const can::Header & h); +std::ostream & operator<<(std::ostream & stream, const can::Frame & f); +} // namespace can + +#endif // SOCKETCAN_INTERFACE__STRING_HPP_ diff --git a/socketcan_interface/include/socketcan_interface/threading.h b/socketcan_interface/include/socketcan_interface/threading.h deleted file mode 100644 index 778464e0e..000000000 --- a/socketcan_interface/include/socketcan_interface/threading.h +++ /dev/null @@ -1,75 +0,0 @@ -#ifndef H_CAN_THREADING_BASE -#define H_CAN_THREADING_BASE - -#include -#include - -namespace can{ - -class StateWaiter{ - boost::mutex mutex_; - boost::condition_variable cond_; - can::StateInterface::StateListenerConstSharedPtr state_listener_; - can::State state_; - void updateState(const can::State &s){ - boost::mutex::scoped_lock lock(mutex_); - state_ = s; - lock.unlock(); - cond_.notify_all(); - } -public: - template StateWaiter(InterfaceType *interface){ - state_ = interface->getState(); - state_listener_ = interface->createStateListener(std::bind(&StateWaiter::updateState, this, std::placeholders::_1)); - } - template bool wait(const can::State::DriverState &s, const DurationType &duration){ - boost::mutex::scoped_lock cond_lock(mutex_); - boost::system_time abs_time = boost::get_system_time() + duration; - while(s != state_.driver_state) - { - if(!cond_.timed_wait(cond_lock,abs_time)) - { - return false; - } - } - return true; - } -}; - -template class ThreadedInterface : public WrappedInterface{ - std::shared_ptr thread_; - void run_thread(){ - WrappedInterface::run(); - } -public: - virtual bool init(const std::string &device, bool loopback) { - if(!thread_ && WrappedInterface::init(device, loopback)){ - StateWaiter waiter(this); - thread_.reset(new boost::thread(&ThreadedInterface::run_thread, this)); - return waiter.wait(can::State::ready, boost::posix_time::seconds(1)); - } - return WrappedInterface::getState().isReady(); - } - virtual void shutdown(){ - WrappedInterface::shutdown(); - if(thread_){ - thread_->interrupt(); - thread_->join(); - thread_.reset(); - } - } - void join(){ - if(thread_){ - thread_->join(); - } - } - virtual ~ThreadedInterface() {} - ThreadedInterface(): WrappedInterface() {} - template ThreadedInterface(const T1 &t1): WrappedInterface(t1) {} - template ThreadedInterface(const T1 &t1, const T2 &t2): WrappedInterface(t1, t2) {} - -}; - - -} // namespace can -#endif diff --git a/socketcan_interface/include/socketcan_interface/threading.hpp b/socketcan_interface/include/socketcan_interface/threading.hpp new file mode 100644 index 000000000..8019f1044 --- /dev/null +++ b/socketcan_interface/include/socketcan_interface/threading.hpp @@ -0,0 +1,122 @@ +// Copyright (c) 2016-2019, Fraunhofer, Mathias Lüdtke, AutonomouStuff +// +// This program is free software: you can redistribute it and/or modify +// it under the terms of the GNU Lesser General Public License as published by +// the Free Software Foundation, either version 3 of the License, or +// (at your option) any later version. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU Lesser General Public License for more details. +// +// You should have received a copy of the GNU Lesser General Public License +// along with this program. If not, see . + +#ifndef SOCKETCAN_INTERFACE__THREADING_HPP_ +#define SOCKETCAN_INTERFACE__THREADING_HPP_ + +#include + +#include +#include + +#include "socketcan_interface/interface.hpp" + +namespace can +{ + +class StateWaiter +{ + boost::mutex mutex_; + boost::condition_variable cond_; + can::StateInterface::StateListenerConstSharedPtr state_listener_; + can::State state_; + void updateState(const can::State & s) + { + boost::mutex::scoped_lock lock(mutex_); + state_ = s; + lock.unlock(); + cond_.notify_all(); + } + +public: + template + explicit StateWaiter(InterfaceType * interface) + { + state_ = interface->getState(); + state_listener_ = interface->createStateListener( + std::bind(&StateWaiter::updateState, this, std::placeholders::_1)); + } + + template + bool wait(const can::State::DriverState & s, const DurationType & duration) + { + boost::mutex::scoped_lock cond_lock(mutex_); + boost::system_time abs_time = boost::get_system_time() + duration; + + while (s != state_.driver_state) { + if (!cond_.timed_wait(cond_lock, abs_time)) { + return false; + } + } + + return true; + } +}; + +template +class ThreadedInterface + : public WrappedInterface +{ + std::shared_ptr thread_; + + void run_thread() + { + WrappedInterface::run(); + } + +public: + virtual bool init(const std::string & device, bool loopback) + { + if (!thread_ && WrappedInterface::init(device, loopback)) { + StateWaiter waiter(this); + thread_.reset(new boost::thread(&ThreadedInterface::run_thread, this)); + return waiter.wait(can::State::ready, boost::posix_time::seconds(1)); + } + + return WrappedInterface::getState().isReady(); + } + + virtual void shutdown() + { + WrappedInterface::shutdown(); + + if (thread_) { + thread_->interrupt(); + thread_->join(); + thread_.reset(); + } + } + void join() + { + if (thread_) { + thread_->join(); + } + } + + virtual ~ThreadedInterface() {} + ThreadedInterface() + : WrappedInterface() {} + template + explicit ThreadedInterface(const T1 & t1) + : WrappedInterface(t1) {} + template + ThreadedInterface(const T1 & t1, const T2 & t2) + : WrappedInterface(t1, t2) {} +}; + + +} // namespace can + +#endif // SOCKETCAN_INTERFACE__THREADING_HPP_ diff --git a/socketcan_interface/package.xml b/socketcan_interface/package.xml index 7deba3d36..caad5ac24 100644 --- a/socketcan_interface/package.xml +++ b/socketcan_interface/package.xml @@ -1,11 +1,11 @@ + socketcan_interface 0.8.2 Generic CAN interface description with helpers for filtering and driver implementation. Further a socketcan implementation based on boost::asio is included. Mathias Lüdtke - Mathias Lüdtke LGPLv3 @@ -13,16 +13,20 @@ https://github.com/ros-industrial/ros_canopen https://github.com/ros-industrial/ros_canopen/issues - catkin + Mathias Lüdtke + + ament_cmake_auto boost class_loader libconsole-bridge-dev linux-kernel-headers - rosunit + ament_lint_auto + ament_lint_common + ament_cmake_gtest - + ament_cmake diff --git a/socketcan_interface/socketcan_interface-extras.cmake b/socketcan_interface/socketcan_interface-extras.cmake new file mode 100644 index 000000000..5fe96c014 --- /dev/null +++ b/socketcan_interface/socketcan_interface-extras.cmake @@ -0,0 +1,21 @@ +# Copyright (c) 2020, Ramon Wijnands, RUVU Robotics B.V. +# +# This program is free software: you can redistribute it and/or modify +# it under the terms of the GNU Lesser General Public License as published by +# the Free Software Foundation, either version 3 of the License, or +# (at your option) any later version. +# +# This program is distributed in the hope that it will be useful, +# but WITHOUT ANY WARRANTY; without even the implied warranty of +# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +# GNU Lesser General Public License for more details. +# +# You should have received a copy of the GNU Lesser General Public License +# along with this program. If not, see . + +find_package(Boost REQUIRED + COMPONENTS + chrono + system + thread +) diff --git a/socketcan_interface/socketcan_interface_plugin.xml b/socketcan_interface/socketcan_interface_plugin.xml index 98f79aaa3..d756fdcd3 100644 --- a/socketcan_interface/socketcan_interface_plugin.xml +++ b/socketcan_interface/socketcan_interface_plugin.xml @@ -1,5 +1,8 @@ - - + + SocketCAN inteface plugin. - \ No newline at end of file + diff --git a/socketcan_interface/src/canbcm.cpp b/socketcan_interface/src/canbcm.cpp index 2afe0a03e..8db44a279 100644 --- a/socketcan_interface/src/canbcm.cpp +++ b/socketcan_interface/src/canbcm.cpp @@ -1,39 +1,56 @@ -#include -#include - -using namespace can; +// Copyright (c) 2016-2019, Mathias Lüdtke, AutonomouStuff +// +// This program is free software: you can redistribute it and/or modify +// it under the terms of the GNU Lesser General Public License as published by +// the Free Software Foundation, either version 3 of the License, or +// (at your option) any later version. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU Lesser General Public License for more details. +// +// You should have received a copy of the GNU Lesser General Public License +// along with this program. If not, see . #include -int main(int argc, char *argv[]){ - BCMsocket bcm; +#include "socketcan_interface/bcm.hpp" +#include "socketcan_interface/string.hpp" - int extra_frames = argc - 4; +int main(int argc, char * argv[]) +{ + can::BCMsocket bcm; - if(extra_frames < 0){ - std::cout << "usage: "<< argv[0] << " DEVICE PERIOD HEADER#DATA [DATA*]" << std::endl; - return 1; - } + int extra_frames = argc - 4; - if(!bcm.init(argv[1])){ - return 2; - } + if (extra_frames < 0) { + std::cout << "usage: " << argv[0] << " DEVICE PERIOD HEADER#DATA [DATA*]" << std::endl; + return 1; + } - int num_frames = extra_frames+1; - Frame *frames = new Frame[num_frames]; - Header header = frames[0] = toframe(argv[3]); + if (!bcm.init(argv[1])) { + return 2; + } - if(extra_frames > 0){ - for(int i=0; i< extra_frames; ++i){ - frames[1+i] = toframe(tostring(header,true)+"#"+argv[4+i]); - } - } - for(int i = 0; i < num_frames; ++i){ - std::cout << frames[i] << std::endl; - } - if(bcm.startTX(boost::chrono::duration(atof(argv[2])), header, num_frames, frames)){ - pause(); - return 0; + int num_frames = extra_frames + 1; + can::Frame * frames = new can::Frame[num_frames]; + can::Header header = frames[0] = can::toframe(argv[3]); + + if (extra_frames > 0) { + for (int i = 0; i < extra_frames; ++i) { + frames[1 + i] = can::toframe(can::tostring(header, true) + "#" + argv[4 + i]); } - return 4; -} \ No newline at end of file + } + + for (int i = 0; i < num_frames; ++i) { + std::cout << frames[i] << std::endl; + } + + if (bcm.startTX(boost::chrono::duration(atof(argv[2])), header, num_frames, frames)) { + pause(); + return 0; + } + + return 4; +} diff --git a/socketcan_interface/src/candump.cpp b/socketcan_interface/src/candump.cpp index 27f95df87..5f126b26d 100644 --- a/socketcan_interface/src/candump.cpp +++ b/socketcan_interface/src/candump.cpp @@ -1,92 +1,99 @@ -#include -#include -#include +// Copyright (c) 2016-2019, Mathias Lüdtke, AutonomouStuff +// +// This program is free software: you can redistribute it and/or modify +// it under the terms of the GNU Lesser General Public License as published by +// the Free Software Foundation, either version 3 of the License, or +// (at your option) any later version. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU Lesser General Public License for more details. +// +// You should have received a copy of the GNU Lesser General Public License +// along with this program. If not, see . #include #include -#include - -using namespace can; #include +#include +#include +#include -void print_error(const State & s); +#include "socketcan_interface/socketcan.hpp" -void print_frame(const Frame &f){ +void print_error(const can::State & s); - if(f.is_error){ - std::cout << "E " << std::hex << f.id << std::dec; - }else if(f.is_extended){ - std::cout << "e " << std::hex << f.id << std::dec; - }else{ - std::cout << "s " << std::hex << f.id << std::dec; - } +void print_frame(const can::Frame & f) +{ + if (f.is_error) { + std::cout << "E " << std::hex << f.id << std::dec; + } else if (f.is_extended) { + std::cout << "e " << std::hex << f.id << std::dec; + } else { + std::cout << "s " << std::hex << f.id << std::dec; + } - std::cout << "\t"; + std::cout << "\t"; - if(f.is_rtr){ - std::cout << "r"; - }else{ - std::cout << (int) f.dlc << std::hex; + if (f.is_rtr) { + std::cout << "r"; + } else { + std::cout << static_cast(f.dlc) << std::hex; - for(int i=0; i < f.dlc; ++i){ - std::cout << std::hex << " " << (int) f.data[i]; - } + for (int i = 0; i < f.dlc; ++i) { + std::cout << std::hex << " " << static_cast(f.data[i]); } + } - std::cout << std::dec << std::endl; + std::cout << std::dec << std::endl; } - std::shared_ptr g_loader; -DriverInterfaceSharedPtr g_driver; - -void print_error(const State & s){ - std::string err; - g_driver->translateError(s.internal_error,err); - std::cout << "ERROR: state=" << s.driver_state << " internal_error=" << s.internal_error << "('" << err << "') asio: " << s.error_code << std::endl; +can::DriverInterfaceSharedPtr g_driver; + +void print_error(const can::State & s) +{ + std::string err; + g_driver->translateError(s.internal_error, err); + std::cout << "ERROR: state=" << s.driver_state; + std::cout << " internal_error=" << s.internal_error; + std::cout << "('" << err << "') asio: " << s.error_code << std::endl; } - -int main(int argc, char *argv[]){ - - if(argc != 2 && argc != 4){ - std::cout << "usage: "<< argv[0] << " DEVICE [PLUGIN_PATH PLUGIN_NAME]" << std::endl; - return 1; - } - - if(argc == 4 ){ - try - { - g_loader = std::make_shared(argv[2]); - g_driver = g_loader->createUniqueInstance(argv[3]); - } - - catch(std::exception& ex) - { - std::cerr << boost::diagnostic_information(ex) << std::endl;; - return 1; - } - }else{ - g_driver = std::make_shared(); +int main(int argc, char * argv[]) +{ + if (argc != 2 && argc != 4) { + std::cout << "usage: " << argv[0] << " DEVICE [PLUGIN_PATH PLUGIN_NAME]" << std::endl; + return 1; + } + + if (argc == 4) { + try { + g_loader = std::make_shared(argv[2]); + g_driver = g_loader->createUniqueInstance(argv[3]); + } catch (std::exception & ex) { + std::cerr << boost::diagnostic_information(ex) << std::endl; + return 1; } + } else { + g_driver = std::make_shared(); + } + can::FrameListenerConstSharedPtr frame_printer = g_driver->createMsgListener(print_frame); + can::StateListenerConstSharedPtr error_printer = g_driver->createStateListener(print_error); + if (!g_driver->init(argv[1], false)) { + print_error(g_driver->getState()); + return 1; + } - FrameListenerConstSharedPtr frame_printer = g_driver->createMsgListener(print_frame); - StateListenerConstSharedPtr error_printer = g_driver->createStateListener(print_error); - - if(!g_driver->init(argv[1], false)){ - print_error(g_driver->getState()); - return 1; - } - - g_driver->run(); - - g_driver->shutdown(); - g_driver.reset(); - g_loader.reset(); + g_driver->run(); - return 0; + g_driver->shutdown(); + g_driver.reset(); + g_loader.reset(); + return 0; } diff --git a/socketcan_interface/src/socketcan_interface_plugin.cpp b/socketcan_interface/src/socketcan_interface_plugin.cpp index b4fb43273..b2820a542 100644 --- a/socketcan_interface/src/socketcan_interface_plugin.cpp +++ b/socketcan_interface/src/socketcan_interface_plugin.cpp @@ -1,4 +1,19 @@ -#include -#include +// Copyright (c) 2016-2019, Mathias Lüdtke, AutonomouStuff +// +// This program is free software: you can redistribute it and/or modify +// it under the terms of the GNU Lesser General Public License as published by +// the Free Software Foundation, either version 3 of the License, or +// (at your option) any later version. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU Lesser General Public License for more details. +// +// You should have received a copy of the GNU Lesser General Public License +// along with this program. If not, see . -CLASS_LOADER_REGISTER_CLASS(can::SocketCANInterface, can::DriverInterface); +#include +#include "socketcan_interface/socketcan.hpp" + +CLASS_LOADER_REGISTER_CLASS(can::SocketCANInterface, can::DriverInterface) diff --git a/socketcan_interface/src/string.cpp b/socketcan_interface/src/string.cpp index 5af94cb62..f328436f6 100644 --- a/socketcan_interface/src/string.cpp +++ b/socketcan_interface/src/string.cpp @@ -1,182 +1,237 @@ -#include -#include - -namespace can { - -bool hex2dec(uint8_t& d, const char& h) { - if ('0' <= h && h <= '9') - d = h - '0'; - else if ('a' <= h && h <= 'f') - d = h - 'a' + 10; - else if ('A' <= h && h <= 'F') - d = h - 'A' + 10; - else - return false; - - return true; -} +// Copyright (c) 2016-2019, Mathias Lüdtke, AutonomouStuff +// +// This program is free software: you can redistribute it and/or modify +// it under the terms of the GNU Lesser General Public License as published by +// the Free Software Foundation, either version 3 of the License, or +// (at your option) any later version. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU Lesser General Public License for more details. +// +// You should have received a copy of the GNU Lesser General Public License +// along with this program. If not, see . -bool hex2buffer(std::string& out, const std::string& in_raw, bool pad) { - std::string in(in_raw); - if ((in.size() % 2) != 0) { - if (pad) - in.insert(0, "0"); - else - return false; - } - out.resize(in.size() >> 1); - for (size_t i = 0; i < out.size(); ++i) { - uint8_t hi, lo; - if (!hex2dec(hi, in[i << 1]) || !hex2dec(lo, in[(i << 1) + 1])) - return false; - - out[i] = (hi << 4) | lo; - } - return true; -} - -bool dec2hex(char& h, const uint8_t& d, bool lc) { - if (d < 10) { - h = '0' + d; - } else if (d < 16 && lc) { - h = 'a' + (d - 10); - } else if (d < 16 && !lc) { - h = 'A' + (d - 10); - } else { - h='?'; - return false; - } - return true; -} +#include +#include -std::string byte2hex(const uint8_t& d, bool pad, bool lc) { - uint8_t hi = d >> 4; - char c=0; - std::string s; - if (hi || pad) { - dec2hex(c, hi, lc); - s += c; - } - dec2hex(c, d & 0xf, lc); - s += c; - return s; -} +#include "socketcan_interface/string.hpp" -std::string buffer2hex(const std::string& in, bool lc) { - std::string s; - s.reserve(in.size() * 2); - for (size_t i = 0; i < in.size(); ++i) { - std::string b = byte2hex(in[i], true, lc); - if (b.empty()) - return b; - - s += b; - } - return s; -} +namespace can +{ -std::string tostring(const Header& h, bool lc) { - std::string s; - s.reserve(8); - std::stringstream buf; - buf << std::hex; - if (lc) - buf << std::nouppercase; - else - buf << std::uppercase; - - if (h.is_extended) - buf << std::setfill('0') << std::setw(8); - - buf << (h.fullid() & ~Header::EXTENDED_MASK); - return buf.str(); -} +bool hex2dec(uint8_t & d, const char & h) +{ + if ('0' <= h && h <= '9') { + d = h - '0'; + } else if ('a' <= h && h <= 'f') { + d = h - 'a' + 10; + } else if ('A' <= h && h <= 'F') { + d = h - 'A' + 10; + } else { + return false; + } -uint32_t tohex(const std::string& s) { - unsigned int h = 0; - std::stringstream stream; - stream << std::hex << s; - stream >> h; - return h; + return true; } -Header toheader(const std::string& s) { - unsigned int h = tohex(s); - unsigned int id = h & Header::ID_MASK; - return Header(id, h & Header::EXTENDED_MASK || (s.size() == 8 && id >= (1<<11)), - h & Header::RTR_MASK, h & Header::ERROR_MASK); -} +bool hex2buffer(std::string & out, const std::string & in_raw, bool pad) +{ + std::string in(in_raw); -std::string tostring(const Frame& f, bool lc) { - std::string s; - s.resize(f.dlc); - for (uint8_t i = 0; i < f.dlc; ++i) { - s[i] = f.data[i]; - } - return tostring((const Header&) (f), lc) + '#' + buffer2hex(s, lc); -} + if ((in.size() % 2) != 0) { + if (pad) { + in.insert(0, "0"); + } else { + return false; + } + } -Frame toframe(const std::string& s) { - size_t sep = s.find('#'); - if (sep == std::string::npos) - return MsgHeader(0xfff); - - Header header = toheader(s.substr(0, sep)); - Frame frame(header); - std::string buffer; - if (header.isValid() && hex2buffer(buffer, s.substr(sep + 1), false)) { - if (buffer.size() > 8) - return MsgHeader(0xfff); - - for (size_t i = 0; i < buffer.size(); ++i) { - frame.data[i] = buffer[i]; - } - frame.dlc = buffer.size(); - } - return frame; -} + out.resize(in.size() >> 1); -template<> FrameFilterSharedPtr tofilter(const std::string &s){ - FrameFilter * filter = 0; - size_t delim = s.find_first_of(":~-_"); - - uint32_t second = FrameMaskFilter::MASK_RELAXED; - bool invert = false; - char type = ':'; - - if(delim != std::string::npos) { - type = s.at(delim); - second = tohex(s.substr(delim +1)); - } - uint32_t first = toheader(s.substr(0, delim)).fullid(); - switch (type) { - case '~': - invert = true; - case ':': - filter = new FrameMaskFilter(first, second, invert); - break; - case '_': - invert = true; - case '-': - filter = new FrameRangeFilter(first, second, invert); - break; - } - return FrameFilterSharedPtr(filter); -} -template<> FrameFilterSharedPtr tofilter(const uint32_t &id){ - return FrameFilterSharedPtr(new FrameMaskFilter(id)); -} + for (size_t i = 0; i < out.size(); ++i) { + uint8_t hi, lo; -FrameFilterSharedPtr tofilter(const char* s){ - return tofilter(s); -} + if (!hex2dec(hi, in[i << 1]) || !hex2dec(lo, in[(i << 1) + 1])) { + return false; + } -std::ostream& operator <<(std::ostream& stream, const Header& h) { - return stream << can::tostring(h, true); -} + out[i] = (hi << 4) | lo; + } -std::ostream& operator <<(std::ostream& stream, const Frame& f) { - return stream << can::tostring(f, true); + return true; } -} +bool dec2hex(char & h, const uint8_t & d, bool lc) +{ + if (d < 10) { + h = '0' + d; + } else if (d < 16 && lc) { + h = 'a' + (d - 10); + } else if (d < 16 && !lc) { + h = 'A' + (d - 10); + } else { + h = '?'; + return false; + } + + return true; +} + +std::string byte2hex(const uint8_t & d, bool pad, bool lc) +{ + uint8_t hi = d >> 4; + char c = 0; + std::string s; + + if (hi || pad) { + dec2hex(c, hi, lc); + s += c; + } + + dec2hex(c, d & 0xf, lc); + s += c; + + return s; +} + +std::string buffer2hex(const std::string & in, bool lc) +{ + std::string s; + s.reserve(in.size() * 2); + + for (size_t i = 0; i < in.size(); ++i) { + std::string b = byte2hex(in[i], true, lc); + if (b.empty()) { + return b; + } + + s += b; + } + + return s; +} + +std::string tostring(const Header & h, bool lc) +{ + std::string s; + s.reserve(8); + std::stringstream buf; + buf << std::hex; + if (lc) { + buf << std::nouppercase; + } else { + buf << std::uppercase; + } + + if (h.is_extended) { + buf << std::setfill('0') << std::setw(8); + } + + buf << (h.fullid() & ~Header::EXTENDED_MASK); + return buf.str(); +} + +uint32_t tohex(const std::string & s) +{ + unsigned int h = 0; + std::stringstream stream; + stream << std::hex << s; + stream >> h; + return h; +} + +Header toheader(const std::string & s) +{ + unsigned int h = tohex(s); + unsigned int id = h & Header::ID_MASK; + return Header( + id, h & Header::EXTENDED_MASK || (s.size() == 8 && id >= (1 << 11)), + h & Header::RTR_MASK, h & Header::ERROR_MASK); +} + +std::string tostring(const Frame & f, bool lc) +{ + std::string s; + s.resize(f.dlc); + for (uint8_t i = 0; i < f.dlc; ++i) { + s[i] = f.data[i]; + } + return tostring((const Header &) (f), lc) + '#' + buffer2hex(s, lc); +} + +Frame toframe(const std::string & s) +{ + size_t sep = s.find('#'); + if (sep == std::string::npos) { + return Frame(MsgHeader(0xfff)); + } + + Header header = toheader(s.substr(0, sep)); + Frame frame(header); + std::string buffer; + if (header.isValid() && hex2buffer(buffer, s.substr(sep + 1), false)) { + if (buffer.size() > 8) { + return Frame(MsgHeader(0xfff)); + } + + for (size_t i = 0; i < buffer.size(); ++i) { + frame.data[i] = buffer[i]; + } + frame.dlc = buffer.size(); + } + return frame; +} + +template<> +FrameFilterSharedPtr tofilter(const std::string & s) +{ + FrameFilter * filter = 0; + size_t delim = s.find_first_of(":~-_"); + + uint32_t second = FrameMaskFilter::MASK_RELAXED; + bool invert = false; + char type = ':'; + + if (delim != std::string::npos) { + type = s.at(delim); + second = tohex(s.substr(delim + 1)); + } + uint32_t first = toheader(s.substr(0, delim)).fullid(); + switch (type) { + case '~': + invert = true; + case ':': + filter = new FrameMaskFilter(first, second, invert); + break; + case '_': + invert = true; + case '-': + filter = new FrameRangeFilter(first, second, invert); + break; + } + return FrameFilterSharedPtr(filter); +} +template<> +FrameFilterSharedPtr tofilter(const uint32_t & id) +{ + return FrameFilterSharedPtr(new FrameMaskFilter(id)); +} + +FrameFilterSharedPtr tofilter(const char * s) +{ + return tofilter(s); +} + +std::ostream & operator<<(std::ostream & stream, const Header & h) +{ + return stream << can::tostring(h, true); +} + +std::ostream & operator<<(std::ostream & stream, const Frame & f) +{ + return stream << can::tostring(f, true); +} + +} // namespace can diff --git a/socketcan_interface/test/test_delegates.cpp b/socketcan_interface/test/test_delegates.cpp deleted file mode 100644 index ed738ece0..000000000 --- a/socketcan_interface/test/test_delegates.cpp +++ /dev/null @@ -1,99 +0,0 @@ -// Bring in my package's API, which is what I'm testing -#include -#include -#include - -// Bring in gtest -#include - -class Receiver { -public: - std::list responses; - void handle(const can::Frame &f){ - responses.push_back(can::tostring(f, true)); - } - Receiver() = default; - ~Receiver() = default; - -private: - Receiver(const Receiver&) = delete; - Receiver& operator=(const Receiver&) = delete; -}; - -Receiver g_r2; - -void fill_r2(const can::Frame &f){ - g_r2.handle(f); -} - -void fill_any(Receiver &r, const can::Frame &f){ - r.handle(f); -} - -TEST(DelegatesTest, testFrameDelegate) -{ - - can::DummyInterface dummy(true); - Receiver r1, r3, r4, r5; - boost::shared_ptr r6(new Receiver()); - std::shared_ptr r7(new Receiver()); - - can::FrameListenerConstSharedPtr l1 = dummy.createMsgListener(can::CommInterface::FrameDelegate(&r1, &Receiver::handle)); - can::FrameListenerConstSharedPtr l2 = dummy.createMsgListener(can::CommInterface::FrameDelegate(fill_r2)); - can::FrameListenerConstSharedPtr l3 = dummy.createMsgListener(can::CommInterface::FrameDelegate(std::bind(fill_any, std::ref(r3), std::placeholders::_1))); - can::FrameListenerConstSharedPtr l4 = dummy.createMsgListener(std::bind(fill_any, std::ref(r4), std::placeholders::_1)); - can::FrameListenerConstSharedPtr l5 = dummy.createMsgListener(boost::bind(fill_any, boost::ref(r5), boost::placeholders::_1)); - can::FrameListenerConstSharedPtr l6 = dummy.createMsgListener(can::CommInterface::FrameDelegate(r6, &Receiver::handle)); - can::FrameListenerConstSharedPtr l7 = dummy.createMsgListener(can::CommInterface::FrameDelegate(r7, &Receiver::handle)); - - std::list expected; - dummy.send(can::toframe("0#8200")); - expected.push_back("0#8200"); - - EXPECT_EQ(expected, r1.responses); - EXPECT_EQ(expected, g_r2.responses); - EXPECT_EQ(expected, r3.responses); - EXPECT_EQ(expected, r4.responses); - EXPECT_EQ(expected, r5.responses); - EXPECT_EQ(expected, r6->responses); - EXPECT_EQ(expected, r7->responses); -} - -class BoolTest { - bool ret_; -public: - bool test_bool() { return ret_; } - BoolTest(bool ret) : ret_(ret) {} -public: - BoolTest(const BoolTest&) = delete; - BoolTest& operator=(const BoolTest&) = delete; -}; - -TEST(DelegatesTest, testBoolFunc) -{ - - using BoolFunc = std::function; - using BoolDelegate = can::DelegateHelper; - - BoolDelegate d1([]() { return false; }); - BoolDelegate d2([]() { return true; }); - - EXPECT_FALSE(d1()); - EXPECT_TRUE(d2()); - - BoolTest b1(false); - BoolTest b2(true); - - BoolDelegate d3(&b1, &BoolTest::test_bool); - BoolDelegate d4(&b2, &BoolTest::test_bool); - EXPECT_FALSE(d3()); - EXPECT_TRUE(d4()); - -} - - -// Run all the tests that were declared with TEST() -int main(int argc, char **argv){ -testing::InitGoogleTest(&argc, argv); -return RUN_ALL_TESTS(); -} diff --git a/socketcan_interface/test/test_dispatcher.cpp b/socketcan_interface/test/test_dispatcher.cpp index aaa668423..e59f70e8c 100644 --- a/socketcan_interface/test/test_dispatcher.cpp +++ b/socketcan_interface/test/test_dispatcher.cpp @@ -1,84 +1,98 @@ -// Bring in my package's API, which is what I'm testing -#include +// Copyright (c) 2016-2019, Fraunhofer, Mathias Lüdtke, AutonomouStuff +// +// This program is free software: you can redistribute it and/or modify +// it under the terms of the GNU Lesser General Public License as published by +// the Free Software Foundation, either version 3 of the License, or +// (at your option) any later version. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU Lesser General Public License for more details. +// +// You should have received a copy of the GNU Lesser General Public License +// along with this program. If not, see . -// Bring in gtest #include -class Counter { +#include + +#include + +class Counter +{ public: - size_t counter_; - Counter() : counter_(0) {} - void count(const can::Frame &msg) { - ++counter_; - } + size_t counter_; + + Counter() + : counter_(0) + {} + + void count(const can::Frame & msg) + { + ++counter_; + } }; TEST(DispatcherTest, testFilteredDispatcher) { - can::FilteredDispatcher dispatcher; - Counter counter1; - Counter counter2; - std::vector listeners; - const size_t max_id = (1<<11); - for(size_t i=0; i < max_id; i+=2) { - listeners.push_back(dispatcher.createListener(can::MsgHeader(i), can::CommInterface::FrameDelegate(&counter1, &Counter::count))); - listeners.push_back(dispatcher.createListener(can::MsgHeader(i+1), can::CommInterface::FrameDelegate(&counter2, &Counter::count))); - } - - boost::chrono::steady_clock::time_point start = boost::chrono::steady_clock::now(); - const size_t num = 1000 * max_id; - for(size_t i=0; i < num; ++i) { - dispatcher.dispatch(can::Frame(can::MsgHeader(i%max_id))); - } - boost::chrono::steady_clock::time_point now = boost::chrono::steady_clock::now(); - double diff = boost::chrono::duration_cast >(now-start).count(); - - EXPECT_EQ(num, counter1.counter_+ counter2.counter_); - EXPECT_EQ(counter1.counter_, counter2.counter_); - std::cout << std::fixed << diff << "\t" << num << "\t" << num / diff << std::endl; + can::FilteredDispatcher dispatcher; + Counter counter1; + Counter counter2; + std::vector listeners; + const size_t max_id = (1 << 11); + + for (size_t i = 0; i < max_id; i += 2) { + listeners.push_back( + dispatcher.createListener( + i, + std::bind(&Counter::count, &counter1, std::placeholders::_1))); + listeners.push_back( + dispatcher.createListener( + i + 1, + std::bind(&Counter::count, &counter2, std::placeholders::_1))); + } + boost::chrono::steady_clock::time_point start = boost::chrono::steady_clock::now(); + const size_t num = 1000 * max_id; + + for (size_t i = 0; i < num; ++i) { + dispatcher.dispatch(i % max_id, can::Frame(can::MsgHeader(i % max_id))); + } + boost::chrono::steady_clock::time_point now = boost::chrono::steady_clock::now(); + double diff = boost::chrono::duration_cast>(now - start).count(); + + EXPECT_EQ(num, counter1.counter_ + counter2.counter_); + EXPECT_EQ(counter1.counter_, counter2.counter_); + std::cout << std::fixed << diff << "\t" << num << "\t" << num / diff << std::endl; } TEST(DispatcherTest, testSimpleDispatcher) { - can::SimpleDispatcher dispatcher; - Counter counter; - can::CommInterface::FrameListenerConstSharedPtr listener = dispatcher.createListener(can::CommInterface::FrameDelegate(&counter, &Counter::count)); - - boost::chrono::steady_clock::time_point start = boost::chrono::steady_clock::now(); - const size_t max_id = (1<<11); - const size_t num = 1000*max_id; - for(size_t i=0; i < num; ++i) { - dispatcher.dispatch(can::Frame(can::MsgHeader(i%max_id))); - } - boost::chrono::steady_clock::time_point now = boost::chrono::steady_clock::now(); - double diff = boost::chrono::duration_cast >(now-start).count(); - - EXPECT_EQ(num, counter.counter_); - std::cout << std::fixed << diff << "\t" << num << "\t" << num / diff << std::endl; -} + can::SimpleDispatcher dispatcher; + Counter counter; -TEST(DispatcherTest, testDelegateOnly) -{ - Counter counter; - can::CommInterface::FrameDelegate delegate(&counter, &Counter::count); + can::CommInterface::FrameListenerConstSharedPtr listener = + dispatcher.createListener(std::bind(&Counter::count, &counter, std::placeholders::_1)); + + boost::chrono::steady_clock::time_point start = boost::chrono::steady_clock::now(); + const size_t max_id = (1 << 11); + const size_t num = 1000 * max_id; - boost::chrono::steady_clock::time_point start = boost::chrono::steady_clock::now(); - const size_t max_id = (1<<11); - const size_t num = 10000*max_id; - for(size_t i=0; i < num; ++i) { - delegate(can::Frame(can::MsgHeader(i%max_id))); - } - boost::chrono::steady_clock::time_point now = boost::chrono::steady_clock::now(); - double diff = boost::chrono::duration_cast >(now-start).count(); + for (size_t i = 0; i < num; ++i) { + dispatcher.dispatch(can::Frame(can::MsgHeader(i % max_id))); + } - EXPECT_EQ(num, counter.counter_); - std::cout << std::fixed << diff << "\t" << num << "\t" << num / diff << std::endl; + boost::chrono::steady_clock::time_point now = boost::chrono::steady_clock::now(); + double diff = boost::chrono::duration_cast>(now - start).count(); + EXPECT_EQ(num, counter.counter_); + std::cout << std::fixed << diff << "\t" << num << "\t" << num / diff << std::endl; } // Run all the tests that were declared with TEST() -int main(int argc, char **argv){ -testing::InitGoogleTest(&argc, argv); -return RUN_ALL_TESTS(); +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); } diff --git a/socketcan_interface/test/test_dummy_interface.cpp b/socketcan_interface/test/test_dummy_interface.cpp index c2c6bc9f2..428be7e73 100644 --- a/socketcan_interface/test/test_dummy_interface.cpp +++ b/socketcan_interface/test/test_dummy_interface.cpp @@ -1,39 +1,61 @@ -// Bring in my package's API, which is what I'm testing -#include -#include +// Copyright (c) 2016-2019, Mathias Lüdtke, AutonomouStuff +// +// This program is free software: you can redistribute it and/or modify +// it under the terms of the GNU Lesser General Public License as published by +// the Free Software Foundation, either version 3 of the License, or +// (at your option) any later version. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU Lesser General Public License for more details. +// +// You should have received a copy of the GNU Lesser General Public License +// along with this program. If not, see . -// Bring in gtest #include -class DummyInterfaceTest : public ::testing::Test{ +#include +#include + +#include "socketcan_interface/dispatcher.hpp" +#include "socketcan_interface/dummy.hpp" + +class DummyInterfaceTest + : public ::testing::Test +{ public: - std::list responses; - can::DummyInterface dummy; - DummyInterfaceTest() : dummy(true), listener(dummy.createMsgListenerM(this, &DummyInterfaceTest::handle)) { } - - void handle(const can::Frame &f){ - responses.push_back(can::tostring(f, true)); - } - can::FrameListenerConstSharedPtr listener; + std::list responses; + can::DummyInterface dummy; + DummyInterfaceTest() + : dummy(true), listener(dummy.createMsgListenerM(this, &DummyInterfaceTest::handle)) {} + + void handle(const can::Frame & f) + { + responses.push_back(can::tostring(f, true)); + } + + can::FrameListenerConstSharedPtr listener; }; // Declare a test TEST_F(DummyInterfaceTest, testCase1) { - dummy.add("0#8200", "701#00" ,false); + dummy.add("0#8200", "701#00", false); - std::list expected; + std::list expected; - dummy.send(can::toframe("0#8200")); - expected.push_back("0#8200"); - expected.push_back("701#00"); + dummy.send(can::toframe("0#8200")); + expected.push_back("0#8200"); + expected.push_back("701#00"); - EXPECT_EQ(expected, responses); + EXPECT_EQ(expected, responses); } // Run all the tests that were declared with TEST() -int main(int argc, char **argv){ -testing::InitGoogleTest(&argc, argv); -return RUN_ALL_TESTS(); +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); } diff --git a/socketcan_interface/test/test_filter.cpp b/socketcan_interface/test/test_filter.cpp index 28d7f87bf..4b6ccf361 100644 --- a/socketcan_interface/test/test_filter.cpp +++ b/socketcan_interface/test/test_filter.cpp @@ -1,13 +1,25 @@ -// Bring in my package's API, which is what I'm testing -#include +// Copyright (c) 2016-2019, Mathias Lüdtke, AutonomouStuff +// +// This program is free software: you can redistribute it and/or modify +// it under the terms of the GNU Lesser General Public License as published by +// the Free Software Foundation, either version 3 of the License, or +// (at your option) any later version. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU Lesser General Public License for more details. +// +// You should have received a copy of the GNU Lesser General Public License +// along with this program. If not, see . - -#include -#include - -// Bring in gtest #include +#include + +#include "socketcan_interface/filter.hpp" +#include "socketcan_interface/string.hpp" +#include "socketcan_interface/dummy.hpp" TEST(FilterTest, simpleMask) { @@ -42,7 +54,6 @@ TEST(FilterTest, maskTests) EXPECT_FALSE(f3->pass(can::toframe(msg1))); EXPECT_TRUE(f3->pass(can::toframe(msg2))); EXPECT_TRUE(f3->pass(can::toframe(msg3))); - } TEST(FilterTest, rangeTest) @@ -66,28 +77,32 @@ TEST(FilterTest, rangeTest) EXPECT_TRUE(f3->pass(can::toframe(msg1))); EXPECT_TRUE(f3->pass(can::toframe(msg2))); EXPECT_FALSE(f3->pass(can::toframe(msg3))); - } -class Counter { +class Counter +{ public: - size_t count_; - Counter(): count_(0) {} - void count(const can::Frame &frame) { - ++count_; - } + size_t count_; + Counter() + : count_(0) {} + + void count(const can::Frame & frame) + { + ++count_; + } }; TEST(FilterTest, listenerTest) { - Counter counter; can::CommInterfaceSharedPtr dummy(new can::DummyInterface(true)); can::FilteredFrameListener::FilterVector filters; filters.push_back(can::tofilter("123:FFE")); - can::FrameListenerConstSharedPtr listener(new can::FilteredFrameListener(dummy,std::bind(&Counter::count, std::ref(counter), std::placeholders::_1), filters)); + can::FrameListenerConstSharedPtr listener( + new can::FilteredFrameListener( + dummy, std::bind(&Counter::count, std::ref(counter), std::placeholders::_1), filters)); can::Frame f1 = can::toframe("123#"); can::Frame f2 = can::toframe("124#"); @@ -99,11 +114,11 @@ TEST(FilterTest, listenerTest) EXPECT_EQ(1, counter.count_); dummy->send(f3); EXPECT_EQ(2, counter.count_); - } // Run all the tests that were declared with TEST() -int main(int argc, char **argv){ -testing::InitGoogleTest(&argc, argv); -return RUN_ALL_TESTS(); +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); } diff --git a/socketcan_interface/test/test_string.cpp b/socketcan_interface/test/test_string.cpp index 4e3a339b2..24ac14c56 100644 --- a/socketcan_interface/test/test_string.cpp +++ b/socketcan_interface/test/test_string.cpp @@ -1,9 +1,23 @@ -// Bring in my package's API, which is what I'm testing -#include +// Copyright (c) 2016-2019, Mathias Lüdtke, AutonomouStuff +// +// This program is free software: you can redistribute it and/or modify +// it under the terms of the GNU Lesser General Public License as published by +// the Free Software Foundation, either version 3 of the License, or +// (at your option) any later version. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU Lesser General Public License for more details. +// +// You should have received a copy of the GNU Lesser General Public License +// along with this program. If not, see . -// Bring in gtest #include +#include + +#include "socketcan_interface/string.hpp" TEST(StringTest, stringconversion) { @@ -22,7 +36,7 @@ TEST(StringTest, stringconversion) EXPECT_EQ(f3.fullid(), 0x80001337); EXPECT_TRUE(f3.isValid()); EXPECT_TRUE(f3.is_extended); - EXPECT_EQ(s4, can::tostring(f3, true)); // 8000 is converted to 0000 + EXPECT_EQ(s4, can::tostring(f3, true)); // 8000 is converted to 0000 can::Frame f4 = can::toframe(s4); EXPECT_EQ(f4.fullid(), 0x80001337); @@ -43,11 +57,11 @@ TEST(StringTest, stringconversion) EXPECT_TRUE(f6.isValid()); EXPECT_TRUE(f6.is_rtr); EXPECT_EQ(s6, can::tostring(f6, true)); - } // Run all the tests that were declared with TEST() -int main(int argc, char **argv){ -testing::InitGoogleTest(&argc, argv); -return RUN_ALL_TESTS(); +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); }