git commit -m "first commit for v2"
This commit is contained in:
189
Devices/Libraries/Ros/ros_canopen/socketcan_interface/CHANGELOG.rst
Executable file
189
Devices/Libraries/Ros/ros_canopen/socketcan_interface/CHANGELOG.rst
Executable file
@@ -0,0 +1,189 @@
|
||||
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
|
||||
Changelog for package socketcan_interface
|
||||
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
|
||||
|
||||
0.8.5 (2020-09-22)
|
||||
------------------
|
||||
* check settings pointer and print error if null
|
||||
* initalize settings properly in deprecated SocketCANInterface::init
|
||||
* Contributors: Mathias Lüdtke
|
||||
|
||||
0.8.4 (2020-08-22)
|
||||
------------------
|
||||
* make parse_error_mask a static member function
|
||||
* pass settings from ROS node to SocketCANInterface
|
||||
* add support for recursive XmlRpcSettings lookups
|
||||
* implemented report-only and fatal errors for SocketCANInterface
|
||||
* added settings parameter to DriverInterface::init
|
||||
* moved XmlRpcSettings to socketcan_interface
|
||||
* moved canopen::Settings into can namespace
|
||||
* Contributors: Mathias Lüdtke
|
||||
|
||||
0.8.3 (2020-05-07)
|
||||
------------------
|
||||
* Fixed Boost link in test-dispacher
|
||||
Signed-off-by: ahcorde <ahcorde@gmail.com>
|
||||
* Bump CMake version to avoid CMP0048 warning
|
||||
Signed-off-by: ahcorde <ahcorde@gmail.com>
|
||||
* do not print ERROR in candump
|
||||
* Contributors: Mathias Lüdtke, ahcorde
|
||||
|
||||
0.8.2 (2019-11-04)
|
||||
------------------
|
||||
* enable rosconsole_bridge bindings
|
||||
* switch to new logging macros
|
||||
* add logging based on console_bridge
|
||||
* handle extended frame strings like candump
|
||||
* implement Frame::fullid()
|
||||
* removed implicit Header operator
|
||||
* move stream operators into can namespace
|
||||
* Contributors: Mathias Lüdtke
|
||||
|
||||
0.8.1 (2019-07-14)
|
||||
------------------
|
||||
* Set C++ standard to c++14
|
||||
* implemented test for dispatcher
|
||||
* Replacing typedefs in socketcan_interface with using aliases.
|
||||
* added Delegate helpers for backwards compatibility
|
||||
* implemented create\*ListenerM helpers
|
||||
* Replacing FastDelegate with std::function and std::bind.
|
||||
* Contributors: Harsh Deshpande, Joshua Whitley, Mathias Lüdtke, pzzlr
|
||||
|
||||
0.8.0 (2018-07-11)
|
||||
------------------
|
||||
* migrated to std::function and std::bind
|
||||
* got rid of boost::noncopyable
|
||||
* replaced BOOST_FOREACH
|
||||
* migrated to std::unordered_map and std::unordered_set
|
||||
* migrated to std:array
|
||||
* migrated to std pointers
|
||||
* removed deprecated types
|
||||
* introduced ROSCANOPEN_MAKE_SHARED
|
||||
* added c_array access functons to can::Frame
|
||||
* Contributors: Mathias Lüdtke
|
||||
|
||||
0.7.8 (2018-05-04)
|
||||
------------------
|
||||
* Revert "pull make_shared into namespaces"
|
||||
This reverts commit 9b2cd05df76d223647ca81917d289ca6330cdee6.
|
||||
* Contributors: Mathias Lüdtke
|
||||
|
||||
0.7.7 (2018-05-04)
|
||||
------------------
|
||||
* pull make_shared into namespaces
|
||||
* added types for all shared_ptrs
|
||||
* fix catkin_lint warnings in filter tests
|
||||
* migrate to new classloader headers
|
||||
* find and link the thread library properly
|
||||
* compile also with boost >= 1.66.0
|
||||
* explicitly include iostream to compile with boost >= 1.65.0
|
||||
* address catkin_lint errors/warnings
|
||||
* added test for FilteredFrameListener
|
||||
* fix string parsers
|
||||
* default to relaxed filter handling
|
||||
works for standard and extended frames
|
||||
* fix string handling of extended frames
|
||||
* added filter parsers
|
||||
should work for vector<unsigned int>, vector<string> and custom vector-like classes
|
||||
* implemented mask and range filters for can::Frame
|
||||
* Contributors: Lukas Bulwahn, Mathias Lüdtke
|
||||
|
||||
0.7.6 (2017-08-30)
|
||||
------------------
|
||||
* make can::Header/Frame::isValid() const
|
||||
* Contributors: Mathias Lüdtke
|
||||
|
||||
0.7.5 (2017-05-29)
|
||||
------------------
|
||||
* fix rosdep dependency on kernel headers
|
||||
* Contributors: Mathias Lüdtke
|
||||
|
||||
0.7.4 (2017-04-25)
|
||||
------------------
|
||||
|
||||
0.7.3 (2017-04-25)
|
||||
------------------
|
||||
|
||||
0.7.2 (2017-03-28)
|
||||
------------------
|
||||
|
||||
0.7.1 (2017-03-20)
|
||||
------------------
|
||||
* stop CAN driver on read errors as well
|
||||
* expose socketcan handle
|
||||
* implemented BCMsocket
|
||||
* introduced BufferedReader::readUntil
|
||||
* Contributors: Mathias Lüdtke
|
||||
|
||||
0.7.0 (2016-12-13)
|
||||
------------------
|
||||
|
||||
0.6.5 (2016-12-10)
|
||||
------------------
|
||||
* removed Baseclass typedef since its use prevented virtual functions calls
|
||||
* add missing chrono dependency
|
||||
* Added catch-all features in BufferedReader
|
||||
* hardened code with the help of cppcheck
|
||||
* styled and sorted CMakeLists.txt
|
||||
* removed boilerplate comments
|
||||
* indention
|
||||
* reviewed exported dependencies
|
||||
* styled and sorted package.xml
|
||||
* update package URLs
|
||||
* Improves StateInterface implementation of the DummyInterface.
|
||||
The doesLoopBack() method now returns the correct value. A state change is
|
||||
correctly dispatched when the init() method is called.
|
||||
* Changes inheritance of DummyInterface to DriverInterface.
|
||||
Such that this interface can also be used for tests requiring a DriverInterface
|
||||
class.
|
||||
Test results of the socketcan_interface tests are unchanged by this
|
||||
modification as it only uses the CommInterface methods.
|
||||
* added socketcan_interface_string to test
|
||||
* moved string functions into separate lib
|
||||
* Introduced setNotReady, prevent enqueue() to switch from closed to open
|
||||
* Reading state\_ should be protected by lock
|
||||
* improved BufferedReader interface and ScopedEnabler
|
||||
* added flush() and max length support to BufferedReader
|
||||
* added BufferedReader
|
||||
* wake multiple waiting threads if needed
|
||||
* pad hex buffer strings in all cases
|
||||
* removed unstable StateWaiter::wait_for
|
||||
* Contributors: Ivor Wanders, Mathias Lüdtke, Michael Stoll
|
||||
|
||||
0.6.4 (2015-07-03)
|
||||
------------------
|
||||
* added missing include, revised depends etc.
|
||||
|
||||
|
||||
0.6.3 (2015-06-30)
|
||||
------------------
|
||||
* dependencies revised
|
||||
* reordering fix for `#87 <https://github.com/ros-industrial/ros_canopen/issues/87>`_
|
||||
* intialize structs
|
||||
* tostring fixed for headers
|
||||
* removed empty test
|
||||
* added DummyInterface with first test
|
||||
* added message string helper
|
||||
* added missing include
|
||||
* install socketcan_interface_plugin.xml
|
||||
* migrated to class_loader for non-ROS parts
|
||||
* moved ThreadedInterface to dedicated header
|
||||
* removed bitrate, added loopback to DriverInterface::init
|
||||
* added socketcan plugin
|
||||
* CommInterstate and StateInterface are now bases of DriverInterface.
|
||||
Therefore DispatchedInterface was moved into AsioBase.
|
||||
* remove debug prints
|
||||
* shutdown asio driver in destructor
|
||||
* proper mask shifts
|
||||
* Contributors: Mathias Lüdtke
|
||||
|
||||
0.6.2 (2014-12-18)
|
||||
------------------
|
||||
|
||||
0.6.1 (2014-12-15)
|
||||
------------------
|
||||
* remove ipa_* and IPA_* prefixes
|
||||
* fixed catkin_lint errors
|
||||
* added descriptions and authors
|
||||
* renamed ipa_can_interface to socketcaninterface
|
||||
* Contributors: Florian Weisshardt, Mathias Lüdtke
|
||||
168
Devices/Libraries/Ros/ros_canopen/socketcan_interface/CMakeLists.txt
Executable file
168
Devices/Libraries/Ros/ros_canopen/socketcan_interface/CMakeLists.txt
Executable file
@@ -0,0 +1,168 @@
|
||||
cmake_minimum_required(VERSION 3.0.2)
|
||||
project(socketcan_interface)
|
||||
|
||||
if(NOT CMAKE_CXX_STANDARD)
|
||||
set(CMAKE_CXX_STANDARD_REQUIRED ON)
|
||||
set(CMAKE_CXX_STANDARD 14)
|
||||
endif()
|
||||
|
||||
find_package(catkin REQUIRED
|
||||
COMPONENTS
|
||||
class_loader
|
||||
)
|
||||
|
||||
find_package(console_bridge 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
|
||||
${Boost_INCLUDE_DIRS}
|
||||
${catkin_INCLUDE_DIRS}
|
||||
${console_bridge_INCLUDE_DIRS}
|
||||
)
|
||||
|
||||
# ${PROJECT_NAME}_string
|
||||
add_library(${PROJECT_NAME}_string
|
||||
src/string.cpp
|
||||
)
|
||||
|
||||
# socketcan_dump
|
||||
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}
|
||||
)
|
||||
|
||||
# socketcan_bcm
|
||||
add_executable(socketcan_bcm
|
||||
src/canbcm.cpp
|
||||
)
|
||||
|
||||
target_link_libraries(socketcan_bcm
|
||||
${PROJECT_NAME}_string
|
||||
${console_bridge_LIBRARIES}
|
||||
${catkin_LIBRARIES}
|
||||
${Boost_LIBRARIES}
|
||||
)
|
||||
|
||||
# ${PROJECT_NAME}_plugin
|
||||
add_library(${PROJECT_NAME}_plugin
|
||||
src/${PROJECT_NAME}_plugin.cpp
|
||||
)
|
||||
target_link_libraries(${PROJECT_NAME}_plugin
|
||||
${console_bridge_LIBRARIES}
|
||||
${catkin_LIBRARIES}
|
||||
${Boost_LIBRARIES}
|
||||
)
|
||||
|
||||
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}
|
||||
)
|
||||
|
||||
install(
|
||||
DIRECTORY
|
||||
include/${PROJECT_NAME}/
|
||||
DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
|
||||
FILES_MATCHING PATTERN "*.h"
|
||||
)
|
||||
|
||||
install(
|
||||
FILES
|
||||
${PROJECT_NAME}_plugin.xml
|
||||
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
|
||||
)
|
||||
if(CATKIN_ENABLE_TESTING)
|
||||
find_package(xmlrpcpp REQUIRED)
|
||||
|
||||
catkin_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}
|
||||
${Boost_LIBRARIES}
|
||||
)
|
||||
|
||||
catkin_add_gtest(${PROJECT_NAME}-test_settings
|
||||
test/test_settings.cpp
|
||||
)
|
||||
target_include_directories(${PROJECT_NAME}-test_settings PRIVATE
|
||||
${xmlrpcpp_INCLUDE_DIRS}
|
||||
)
|
||||
target_link_libraries(${PROJECT_NAME}-test_settings
|
||||
${PROJECT_NAME}_string
|
||||
${catkin_LIBRARIES}
|
||||
${xmlrpcpp_LIBRARIES}
|
||||
)
|
||||
|
||||
catkin_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}
|
||||
)
|
||||
|
||||
catkin_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
|
||||
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()
|
||||
@@ -0,0 +1,136 @@
|
||||
#ifndef H_ASIO_BASE
|
||||
#define H_ASIO_BASE
|
||||
|
||||
#include <socketcan_interface/interface.h>
|
||||
#include <socketcan_interface/dispatcher.h>
|
||||
#include <boost/asio.hpp>
|
||||
#include <boost/thread/mutex.hpp>
|
||||
#include <boost/thread/thread.hpp>
|
||||
#include <functional>
|
||||
|
||||
namespace can{
|
||||
|
||||
|
||||
template<typename Socket> class AsioDriver : public DriverInterface{
|
||||
using FrameDispatcher = FilteredDispatcher<unsigned int, CommInterface::FrameListener>;
|
||||
using StateDispatcher = SimpleDispatcher<StateInterface::StateListener>;
|
||||
FrameDispatcher frame_dispatcher_;
|
||||
StateDispatcher state_dispatcher_;
|
||||
|
||||
State state_;
|
||||
boost::mutex state_mutex_;
|
||||
boost::mutex socket_mutex_;
|
||||
|
||||
void shutdown_internal(){
|
||||
if(socket_.is_open()){
|
||||
socket_.cancel();
|
||||
socket_.close();
|
||||
}
|
||||
io_service_.stop();
|
||||
}
|
||||
|
||||
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_internal(); }
|
||||
|
||||
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(){
|
||||
shutdown_internal();
|
||||
}
|
||||
|
||||
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
|
||||
@@ -0,0 +1,120 @@
|
||||
#ifndef H_CAN_BCM
|
||||
#define H_CAN_BCM
|
||||
|
||||
#include <socketcan_interface/interface.h>
|
||||
|
||||
#include <sys/types.h>
|
||||
#include <sys/socket.h>
|
||||
#include <sys/ioctl.h>
|
||||
#include <net/if.h>
|
||||
|
||||
#include <linux/can.h>
|
||||
#include <linux/can/bcm.h>
|
||||
#include <linux/can/error.h>
|
||||
|
||||
#include <cstring>
|
||||
|
||||
#include <boost/chrono.hpp>
|
||||
|
||||
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<typename T> void setIVal2(T period){
|
||||
long long usec = boost::chrono::duration_cast<boost::chrono::microseconds>(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<typename DurationType> 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
|
||||
@@ -0,0 +1,24 @@
|
||||
#ifndef SOCKETCAN_INTERFACE_DELEGATES_H_
|
||||
#define SOCKETCAN_INTERFACE_DELEGATES_H_
|
||||
|
||||
#include <functional>
|
||||
|
||||
namespace can
|
||||
{
|
||||
|
||||
template <typename T> class DelegateHelper : public T {
|
||||
public:
|
||||
template <typename Object, typename Instance, typename ...Args>
|
||||
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 <typename Callable>
|
||||
DelegateHelper(Callable &&c) : T(c)
|
||||
{
|
||||
}
|
||||
};
|
||||
|
||||
} // namespace can
|
||||
|
||||
#endif // SOCKETCAN_INTERFACE_DELEGATES_H_
|
||||
@@ -0,0 +1,120 @@
|
||||
#ifndef H_CAN_DISPATCHER
|
||||
#define H_CAN_DISPATCHER
|
||||
|
||||
#include <functional>
|
||||
#include <memory>
|
||||
#include <list>
|
||||
#include <unordered_map>
|
||||
|
||||
#include <socketcan_interface/interface.h>
|
||||
#include <boost/thread/mutex.hpp>
|
||||
|
||||
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<DispatcherBase>;
|
||||
class DispatcherBase {
|
||||
DispatcherBase(const DispatcherBase&) = delete; // prevent copies
|
||||
DispatcherBase& operator=(const DispatcherBase&) = delete;
|
||||
|
||||
class GuardedListener: public Listener{
|
||||
std::weak_ptr<DispatcherBase> 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<const Listener* > listeners_;
|
||||
public:
|
||||
DispatcherBase(boost::mutex &mutex) : mutex_(mutex) {}
|
||||
void dispatch_nolock(const Type &obj, const Listener* loopback=nullptr) const{
|
||||
for(typename std::list<const Listener* >::const_iterator it=listeners_.begin(); it != listeners_.end(); ++it){
|
||||
if (loopback != *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);
|
||||
}
|
||||
void dispatch_filtered(const Type &obj, ListenerConstSharedPtr without){
|
||||
boost::mutex::scoped_lock lock(mutex_);
|
||||
dispatcher_->dispatch_nolock(obj, without.get());
|
||||
}
|
||||
size_t numListeners(){
|
||||
return dispatcher_->numListeners();
|
||||
}
|
||||
operator Callable() { return Callable(this,&SimpleDispatcher::dispatch); }
|
||||
};
|
||||
|
||||
template<typename K, typename Listener, typename Hash = std::hash<K> > class FilteredDispatcher: public SimpleDispatcher<Listener>{
|
||||
using BaseClass = SimpleDispatcher<Listener>;
|
||||
std::unordered_map<K, typename BaseClass::DispatcherBaseSharedPtr, Hash> 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 <typename T>
|
||||
[[deprecated("provide key explicitly")]]
|
||||
typename BaseClass::ListenerConstSharedPtr createListener(const T &key, const typename BaseClass::Callable &callable){
|
||||
return createListener(static_cast<K>(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<K>(obj), obj);
|
||||
}
|
||||
|
||||
operator typename BaseClass::Callable() { return typename BaseClass::Callable(this,&FilteredDispatcher::dispatch); }
|
||||
};
|
||||
|
||||
} // namespace can
|
||||
#endif
|
||||
@@ -0,0 +1,245 @@
|
||||
#ifndef SOCKETCAN_INTERFACE_DUMMY_H
|
||||
#define SOCKETCAN_INTERFACE_DUMMY_H
|
||||
|
||||
#include <deque>
|
||||
#include <unordered_map>
|
||||
|
||||
#include "interface.h"
|
||||
#include "dispatcher.h"
|
||||
#include "string.h"
|
||||
#include "logging.h"
|
||||
#include "threading.h"
|
||||
#include <boost/algorithm/string.hpp>
|
||||
|
||||
|
||||
namespace can {
|
||||
|
||||
class DummyBus {
|
||||
public:
|
||||
using FrameDispatcher = SimpleDispatcher<CommInterface::FrameListener>;
|
||||
using FrameDispatcherSharedPtr = std::shared_ptr<FrameDispatcher>;
|
||||
using Buses = std::unordered_map<std::string, FrameDispatcherSharedPtr>;
|
||||
private:
|
||||
static Buses& get_buses() {
|
||||
static Buses buses;
|
||||
return buses;
|
||||
}
|
||||
FrameDispatcherSharedPtr bus_;
|
||||
public:
|
||||
const std::string name;
|
||||
DummyBus(const std::string& name) : name(name), bus_(get_buses().emplace(name, std::make_shared<FrameDispatcher>()).first->second) {
|
||||
}
|
||||
~DummyBus() {
|
||||
get_buses().erase(name);
|
||||
}
|
||||
class Connection {
|
||||
public:
|
||||
inline Connection(FrameDispatcherSharedPtr bus, FrameListenerConstSharedPtr listener)
|
||||
: bus_(bus), listener_(listener)
|
||||
{}
|
||||
void dispatch(const Frame & msg){
|
||||
bus_->dispatch_filtered(msg, listener_);
|
||||
}
|
||||
private:
|
||||
FrameDispatcherSharedPtr bus_;
|
||||
FrameListenerConstSharedPtr listener_;
|
||||
};
|
||||
using ConnectionSharedPtr = std::shared_ptr<Connection>;
|
||||
template <typename Instance, typename Callable> static inline ConnectionSharedPtr connect(const std::string & name, Instance inst, Callable callable) {
|
||||
FrameDispatcherSharedPtr bus = get_buses().at(name);
|
||||
return std::make_shared<Connection>(bus, bus->createListener(std::bind(callable, inst, std::placeholders::_1)));
|
||||
}
|
||||
};
|
||||
|
||||
class DummyInterface : public DriverInterface{
|
||||
using FrameDispatcher = FilteredDispatcher<unsigned int, CommInterface::FrameListener>;
|
||||
using StateDispatcher = SimpleDispatcher<StateInterface::StateListener>;
|
||||
FrameDispatcher frame_dispatcher_;
|
||||
StateDispatcher state_dispatcher_;
|
||||
DummyBus::ConnectionSharedPtr bus_;
|
||||
State state_;
|
||||
std::deque<can::Frame> in_;
|
||||
bool loopback_;
|
||||
bool trace_;
|
||||
boost::mutex mutex_;
|
||||
boost::condition_variable cond_;
|
||||
|
||||
void setDriverState(State::DriverState state){
|
||||
boost::mutex::scoped_lock lock(mutex_);
|
||||
if(state_.driver_state != state){
|
||||
state_.driver_state = state;
|
||||
state_dispatcher_.dispatch(state_);
|
||||
}
|
||||
cond_.notify_all();
|
||||
}
|
||||
void enqueue(const Frame & msg){
|
||||
boost::mutex::scoped_lock cond_lock(mutex_);
|
||||
in_.push_back(msg);
|
||||
cond_lock.unlock();
|
||||
cond_.notify_all();
|
||||
}
|
||||
|
||||
void shutdown_internal(){
|
||||
setDriverState(State::closed);
|
||||
bus_.reset();
|
||||
};
|
||||
public:
|
||||
DummyInterface() : loopback_(false), trace_(false) {}
|
||||
DummyInterface(bool loopback) : loopback_(loopback), trace_(false) {}
|
||||
virtual ~DummyInterface() { shutdown_internal(); }
|
||||
|
||||
|
||||
virtual bool send(const Frame & msg){
|
||||
if (trace_) {
|
||||
ROSCANOPEN_DEBUG("socketcan_interface", "send: " << msg);
|
||||
}
|
||||
if (loopback_) {
|
||||
enqueue(msg);
|
||||
}
|
||||
bus_->dispatch(msg);
|
||||
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(){
|
||||
boost::mutex::scoped_lock cond_lock(mutex_);
|
||||
return state_;
|
||||
}
|
||||
|
||||
virtual void shutdown(){
|
||||
flush();
|
||||
shutdown_internal();
|
||||
};
|
||||
|
||||
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_;
|
||||
};
|
||||
|
||||
void flush(){
|
||||
while (true) {
|
||||
{
|
||||
boost::mutex::scoped_lock cond_lock(mutex_);
|
||||
if (in_.empty() || state_.driver_state == State::closed) {
|
||||
return;
|
||||
}
|
||||
}
|
||||
boost::this_thread::sleep_for(boost::chrono::milliseconds(100));
|
||||
}
|
||||
}
|
||||
|
||||
virtual void run(){
|
||||
boost::mutex::scoped_lock cond_lock(mutex_);
|
||||
while (true) {
|
||||
|
||||
state_.driver_state = State::ready;
|
||||
state_dispatcher_.dispatch(state_);
|
||||
|
||||
cond_.wait_for(cond_lock, boost::chrono::seconds(1));
|
||||
while(!in_.empty()){
|
||||
const can::Frame msg = in_.front();
|
||||
in_.pop_front();
|
||||
if (trace_) {
|
||||
ROSCANOPEN_DEBUG("socketcan_interface", "receive: " << msg);
|
||||
}
|
||||
frame_dispatcher_.dispatch(msg.key(), msg);
|
||||
}
|
||||
if (state_.driver_state == State::closed) {
|
||||
return;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
bool init(const std::string &device, bool loopback){
|
||||
loopback_ = loopback;
|
||||
bus_ = DummyBus::connect(device, this, &DummyInterface::enqueue);
|
||||
setDriverState(State::open);
|
||||
return true;
|
||||
}
|
||||
|
||||
virtual bool init(const std::string &device, bool loopback, SettingsConstSharedPtr settings) {
|
||||
if(DummyInterface::init(device, loopback)) {
|
||||
trace_ = settings->get_optional("trace", false);
|
||||
return true;
|
||||
} else {
|
||||
return false;
|
||||
}
|
||||
}
|
||||
virtual StateListenerConstSharedPtr createStateListener(const StateFunc &delegate){
|
||||
return state_dispatcher_.createListener(delegate);
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
using DummyInterfaceSharedPtr = std::shared_ptr<DummyInterface>;
|
||||
using ThreadedDummyInterface = ThreadedInterface<DummyInterface>;
|
||||
using ThreadedDummyInterfaceSharedPtr = std::shared_ptr<ThreadedDummyInterface>;
|
||||
|
||||
class DummyResponder {
|
||||
public:
|
||||
DummyResponder() : dummy_(), listener_(dummy_.createMsgListenerM(this, &DummyResponder::respond)) {
|
||||
}
|
||||
bool init(const DummyBus &bus) {
|
||||
return dummy_.init(bus.name, false, NoSettings::create());
|
||||
}
|
||||
void flush() {
|
||||
dummy_.flush();
|
||||
}
|
||||
virtual ~DummyResponder() {}
|
||||
protected:
|
||||
void send(const Frame & msg) {
|
||||
dummy_.send(msg);
|
||||
}
|
||||
private:
|
||||
ThreadedDummyInterface dummy_;
|
||||
FrameListenerConstSharedPtr listener_;
|
||||
virtual void respond(const Frame & msg) = 0;
|
||||
};
|
||||
|
||||
class DummyReplay : public DummyResponder {
|
||||
private:
|
||||
virtual void respond(const Frame & msg) {
|
||||
const auto &front = replay_.front();
|
||||
if (tostring(msg, true) == front.first) {
|
||||
for(auto &f: front.second) {
|
||||
send(f);
|
||||
}
|
||||
replay_.pop_front();
|
||||
}
|
||||
}
|
||||
std::list<std::pair<std::string, std::vector<Frame> > > replay_;
|
||||
bool error_;
|
||||
public:
|
||||
void add(const std::string &read, const std::initializer_list<std::string> &write){
|
||||
std::vector<Frame> frames;
|
||||
frames.reserve(write.size());
|
||||
for(auto &w : write) {
|
||||
frames.push_back(toframe(w));
|
||||
}
|
||||
replay_.push_back(std::make_pair(boost::to_lower_copy(read), frames));
|
||||
}
|
||||
void add(const std::string &read, const std::string &write){
|
||||
add(read, {write});
|
||||
}
|
||||
bool done() { return replay_.empty(); }
|
||||
};
|
||||
|
||||
}
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,70 @@
|
||||
#ifndef SOCKETCAN_INTERFACE_FILTER_H
|
||||
#define SOCKETCAN_INTERFACE_FILTER_H
|
||||
|
||||
#include <vector>
|
||||
|
||||
#include "interface.h"
|
||||
|
||||
namespace can {
|
||||
|
||||
class FrameFilter {
|
||||
public:
|
||||
virtual bool pass(const can::Frame &frame) const = 0;
|
||||
virtual ~FrameFilter() {}
|
||||
};
|
||||
using FrameFilterSharedPtr = std::shared_ptr<FrameFilter>;
|
||||
|
||||
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<FrameFilterSharedPtr>;
|
||||
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<FrameFilterSharedPtr> filters_;
|
||||
CommInterface::FrameListenerConstSharedPtr listener_;
|
||||
};
|
||||
|
||||
} // namespace can
|
||||
|
||||
#endif /*SOCKETCAN_INTERFACE_FILTER_H*/
|
||||
@@ -0,0 +1,230 @@
|
||||
#ifndef H_CAN_INTERFACE
|
||||
#define H_CAN_INTERFACE
|
||||
|
||||
#include <array>
|
||||
#include <memory>
|
||||
#include <functional>
|
||||
|
||||
#include <boost/system/error_code.hpp>
|
||||
|
||||
#include "socketcan_interface/delegates.h"
|
||||
#include "socketcan_interface/logging.h"
|
||||
#include "socketcan_interface/settings.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<value_type, 8> 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 <typename T,typename U> class Listener{
|
||||
const T callable_;
|
||||
public:
|
||||
using Type = U;
|
||||
using Callable = T;
|
||||
using ListenerConstSharedPtr = std::shared_ptr<const Listener>;
|
||||
|
||||
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<void(const State&)>;
|
||||
using StateDelegate [[deprecated("use StateFunc instead")]] = DelegateHelper<StateFunc>;
|
||||
using StateListener = Listener<const StateFunc, const State&>;
|
||||
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 <typename Instance, typename Callable> inline StateListenerConstSharedPtr createStateListenerM(Instance inst, Callable callable) {
|
||||
return this->createStateListener(std::bind(callable, inst, std::placeholders::_1));
|
||||
}
|
||||
|
||||
virtual ~StateInterface() {}
|
||||
};
|
||||
using StateInterfaceSharedPtr = std::shared_ptr<StateInterface>;
|
||||
using StateListenerConstSharedPtr = StateInterface::StateListenerConstSharedPtr;
|
||||
|
||||
class CommInterface{
|
||||
public:
|
||||
using FrameFunc = std::function<void(const Frame&)>;
|
||||
using FrameDelegate [[deprecated("use FrameFunc instead")]] = DelegateHelper<FrameFunc>;
|
||||
using FrameListener = Listener<const FrameFunc, const Frame&>;
|
||||
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 <typename Instance, typename Callable> 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 <typename Instance, typename Callable> 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<CommInterface>;
|
||||
using FrameListenerConstSharedPtr = CommInterface::FrameListenerConstSharedPtr;
|
||||
|
||||
class DriverInterface : public CommInterface, public StateInterface {
|
||||
public:
|
||||
[[deprecated("provide settings explicitly")]] virtual bool init(const std::string &device, bool loopback) = 0;
|
||||
|
||||
/**
|
||||
* initialize interface
|
||||
*
|
||||
* @param[in] device: driver-specific device name/path
|
||||
* @param[in] loopback: loop-back own messages
|
||||
* @param[in] settings: driver-specific settings
|
||||
* @return true if device was initialized succesfully, false otherwise
|
||||
*/
|
||||
virtual bool init(const std::string &device, bool loopback, SettingsConstSharedPtr settings) {
|
||||
ROSCANOPEN_ERROR("socketcan_interface", "Driver does not support custom settings");
|
||||
#pragma GCC diagnostic push
|
||||
#pragma GCC diagnostic ignored "-Wdeprecated-declarations"
|
||||
return init(device, loopback);
|
||||
#pragma GCC diagnostic pop
|
||||
}
|
||||
|
||||
/**
|
||||
* 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<DriverInterface>;
|
||||
|
||||
|
||||
} // namespace can
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,17 @@
|
||||
#ifndef SOCKETCAN_INTERFACE_LOGGING_H
|
||||
#define SOCKETCAN_INTERFACE_LOGGING_H
|
||||
|
||||
#include <console_bridge/console.h>
|
||||
#include <sstream>
|
||||
|
||||
#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
|
||||
@@ -0,0 +1,7 @@
|
||||
#ifndef SOCKETCAN_INTERFACE_MAKE_SHARED_H
|
||||
#define SOCKETCAN_INTERFACE_MAKE_SHARED_H
|
||||
|
||||
#include <memory>
|
||||
#define ROSCANOPEN_MAKE_SHARED std::make_shared
|
||||
|
||||
#endif // ! SOCKETCAN_INTERFACE_MAKE_SHARED_H
|
||||
@@ -0,0 +1,114 @@
|
||||
#ifndef H_CAN_BUFFERED_READER
|
||||
#define H_CAN_BUFFERED_READER
|
||||
|
||||
#include <socketcan_interface/interface.h>
|
||||
#include <deque>
|
||||
|
||||
#include <boost/thread/mutex.hpp>
|
||||
#include <boost/thread/condition_variable.hpp>
|
||||
#include <boost/chrono.hpp>
|
||||
|
||||
namespace can{
|
||||
|
||||
class BufferedReader {
|
||||
std::deque<can::Frame> 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<typename DurationType> 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
|
||||
@@ -0,0 +1,59 @@
|
||||
#ifndef SOCKETCAN_INTERFACE_SETTINGS_H
|
||||
#define SOCKETCAN_INTERFACE_SETTINGS_H
|
||||
|
||||
#include <map>
|
||||
#include <string>
|
||||
#include <memory>
|
||||
|
||||
#include <boost/lexical_cast.hpp>
|
||||
|
||||
namespace can {
|
||||
class Settings
|
||||
{
|
||||
public:
|
||||
template <typename T> T get_optional(const std::string &n, const T& def) const {
|
||||
std::string repr;
|
||||
if(!getRepr(n, repr)){
|
||||
return def;
|
||||
}
|
||||
return boost::lexical_cast<T>(repr);
|
||||
}
|
||||
template <typename T> bool get(const std::string &n, T& val) const {
|
||||
std::string repr;
|
||||
if(!getRepr(n, repr)) return false;
|
||||
val = boost::lexical_cast<T>(repr);
|
||||
return true;
|
||||
}
|
||||
virtual ~Settings() {}
|
||||
private:
|
||||
virtual bool getRepr(const std::string &n, std::string & repr) const = 0;
|
||||
};
|
||||
using SettingsConstSharedPtr = std::shared_ptr<const Settings>;
|
||||
using SettingsSharedPtr = std::shared_ptr<Settings>;
|
||||
|
||||
class NoSettings : public Settings {
|
||||
public:
|
||||
static SettingsConstSharedPtr create() { return SettingsConstSharedPtr(new NoSettings); }
|
||||
private:
|
||||
virtual bool getRepr(const std::string &n, std::string & repr) const { return false; }
|
||||
};
|
||||
|
||||
class SettingsMap : public Settings {
|
||||
std::map<std::string, std::string> settings_;
|
||||
virtual bool getRepr(const std::string &n, std::string & repr) const {
|
||||
std::map<std::string, std::string>::const_iterator it = settings_.find(n);
|
||||
if (it == settings_.cend()) return false;
|
||||
repr = it->second;
|
||||
return true;
|
||||
}
|
||||
public:
|
||||
template <typename T> void set(const std::string &n, const T& val) {
|
||||
settings_[n] = boost::lexical_cast<std::string>(val);
|
||||
}
|
||||
static std::shared_ptr<SettingsMap> create() { return std::shared_ptr<SettingsMap>(new SettingsMap); }
|
||||
};
|
||||
|
||||
|
||||
} // can
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,276 @@
|
||||
#ifndef H_SOCKETCAN_DRIVER
|
||||
#define H_SOCKETCAN_DRIVER
|
||||
|
||||
#include <socketcan_interface/asio_base.h>
|
||||
#include <boost/bind.hpp>
|
||||
|
||||
#include <sys/types.h>
|
||||
#include <sys/socket.h>
|
||||
#include <sys/ioctl.h>
|
||||
#include <net/if.h>
|
||||
|
||||
#include <linux/can.h>
|
||||
#include <linux/can/raw.h>
|
||||
#include <linux/can/error.h>
|
||||
|
||||
#include <cstring>
|
||||
|
||||
#include <socketcan_interface/dispatcher.h>
|
||||
#include <socketcan_interface/string.h>
|
||||
|
||||
namespace can {
|
||||
|
||||
class SocketCANInterface : public AsioDriver<boost::asio::posix::stream_descriptor> {
|
||||
bool loopback_;
|
||||
int sc_;
|
||||
can_err_mask_t error_mask_, fatal_error_mask_;
|
||||
|
||||
static can_err_mask_t parse_error_mask(SettingsConstSharedPtr settings, const std::string &entry, can_err_mask_t defaults) {
|
||||
can_err_mask_t mask = 0;
|
||||
|
||||
#define add_bit(e) mask |= (settings->get_optional(entry + "/" + #e, (defaults & e) != 0) ? e : 0)
|
||||
add_bit(CAN_ERR_LOSTARB);
|
||||
add_bit(CAN_ERR_CRTL);
|
||||
add_bit(CAN_ERR_PROT);
|
||||
add_bit(CAN_ERR_TRX);
|
||||
add_bit(CAN_ERR_ACK);
|
||||
add_bit(CAN_ERR_TX_TIMEOUT);
|
||||
add_bit(CAN_ERR_BUSOFF);
|
||||
add_bit(CAN_ERR_BUSERROR);
|
||||
add_bit(CAN_ERR_RESTARTED);
|
||||
#undef add_bit
|
||||
|
||||
return mask;
|
||||
}
|
||||
public:
|
||||
SocketCANInterface()
|
||||
: loopback_(false), sc_(-1), error_mask_(0), fatal_error_mask_(0)
|
||||
{}
|
||||
|
||||
bool doesLoopBack() const override{
|
||||
return loopback_;
|
||||
}
|
||||
|
||||
can_err_mask_t getErrorMask() const {
|
||||
return error_mask_;
|
||||
}
|
||||
|
||||
can_err_mask_t getFatalErrorMask() const {
|
||||
return fatal_error_mask_;
|
||||
}
|
||||
[[deprecated("provide settings explicitly")]] virtual bool init(const std::string &device, bool loopback) override {
|
||||
return SocketCANInterface::init(device, loopback, NoSettings::create());
|
||||
}
|
||||
virtual bool init(const std::string &device, bool loopback, SettingsConstSharedPtr settings) override {
|
||||
if (!settings) {
|
||||
ROSCANOPEN_ERROR("socketcan_interface", "settings must not be a null pointer");
|
||||
return false;
|
||||
}
|
||||
const can_err_mask_t fatal_errors = ( CAN_ERR_TX_TIMEOUT /* TX timeout (by netdevice driver) */
|
||||
| CAN_ERR_BUSOFF /* bus off */
|
||||
| CAN_ERR_BUSERROR /* bus error (may flood!) */
|
||||
| CAN_ERR_RESTARTED /* controller restarted */
|
||||
);
|
||||
const can_err_mask_t report_errors = ( 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_mask_t fatal_error_mask = parse_error_mask(settings, "fatal_error_mask", fatal_errors) | CAN_ERR_BUSOFF;
|
||||
can_err_mask_t error_mask = parse_error_mask(settings, "error_mask", report_errors | fatal_error_mask) | fatal_error_mask;
|
||||
return init(device, loopback, error_mask, fatal_error_mask);
|
||||
}
|
||||
|
||||
bool recover() override{
|
||||
if(!getState().isReady()){
|
||||
shutdown();
|
||||
return init(device_, loopback_, error_mask_, fatal_error_mask_);
|
||||
}
|
||||
return getState().isReady();
|
||||
}
|
||||
bool translateError(unsigned int internal_error, std::string & str) override{
|
||||
|
||||
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_;
|
||||
|
||||
bool init(const std::string &device, bool loopback, can_err_mask_t error_mask, can_err_mask_t fatal_error_mask) {
|
||||
State s = getState();
|
||||
if(s.driver_state == State::closed){
|
||||
sc_ = 0;
|
||||
device_ = device;
|
||||
loopback_ = loopback;
|
||||
error_mask_ = error_mask;
|
||||
fatal_error_mask_ = fatal_error_mask;
|
||||
|
||||
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;
|
||||
}
|
||||
ret = setsockopt(sc, SOL_CAN_RAW, CAN_RAW_ERR_FILTER,
|
||||
&error_mask, sizeof(error_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();
|
||||
}
|
||||
|
||||
void triggerReadSome() override{
|
||||
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));
|
||||
}
|
||||
|
||||
bool enqueue(const Frame & msg) override{
|
||||
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<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;
|
||||
|
||||
if (frame_.can_id & fatal_error_mask_) {
|
||||
ROSCANOPEN_ERROR("socketcan_interface", "internal 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<SocketCANDriver>;
|
||||
using SocketCANInterfaceSharedPtr = std::shared_ptr<SocketCANInterface>;
|
||||
|
||||
template <typename T> class ThreadedInterface;
|
||||
using ThreadedSocketCANInterface = ThreadedInterface<SocketCANInterface>;
|
||||
using ThreadedSocketCANInterfaceSharedPtr = std::shared_ptr<ThreadedSocketCANInterface>;
|
||||
|
||||
|
||||
} // namespace can
|
||||
#endif
|
||||
@@ -0,0 +1,47 @@
|
||||
#ifndef SOCKETCAN_INTERFACE_STRING_H
|
||||
#define SOCKETCAN_INTERFACE_STRING_H
|
||||
|
||||
#include "interface.h"
|
||||
#include "filter.h"
|
||||
#include <sstream>
|
||||
|
||||
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<class T> 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 <typename T> FilteredFrameListener::FilterVector tofilters(const T& v) {
|
||||
FilteredFrameListener::FilterVector filters;
|
||||
for(size_t i = 0; i < static_cast<size_t>(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
|
||||
@@ -0,0 +1,92 @@
|
||||
#ifndef H_CAN_THREADING_BASE
|
||||
#define H_CAN_THREADING_BASE
|
||||
|
||||
#include <socketcan_interface/interface.h>
|
||||
#include <boost/thread/thread.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<typename InterfaceType> StateWaiter(InterfaceType *interface){
|
||||
state_ = interface->getState();
|
||||
state_listener_ = interface->createStateListener(std::bind(&StateWaiter::updateState, this, std::placeholders::_1));
|
||||
}
|
||||
template<typename DurationType> 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<typename WrappedInterface> class ThreadedInterface : public WrappedInterface{
|
||||
std::shared_ptr<boost::thread> thread_;
|
||||
void run_thread(){
|
||||
WrappedInterface::run();
|
||||
}
|
||||
void shutdown_internal(){
|
||||
if(thread_){
|
||||
thread_->interrupt();
|
||||
thread_->join();
|
||||
thread_.reset();
|
||||
}
|
||||
}
|
||||
public:
|
||||
[[deprecated("provide settings explicitly")]] bool init(const std::string &device, bool loopback) override {
|
||||
#pragma GCC diagnostic push
|
||||
#pragma GCC diagnostic ignored "-Wdeprecated-declarations"
|
||||
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();
|
||||
#pragma GCC diagnostic pop
|
||||
}
|
||||
bool init(const std::string &device, bool loopback, SettingsConstSharedPtr settings) override {
|
||||
if(!thread_ && WrappedInterface::init(device, loopback, settings)){
|
||||
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();
|
||||
}
|
||||
|
||||
void shutdown() override{
|
||||
WrappedInterface::shutdown();
|
||||
shutdown_internal();
|
||||
}
|
||||
void join(){
|
||||
if(thread_){
|
||||
thread_->join();
|
||||
}
|
||||
}
|
||||
virtual ~ThreadedInterface() {
|
||||
shutdown_internal();
|
||||
}
|
||||
ThreadedInterface(): WrappedInterface() {}
|
||||
template<typename T1> ThreadedInterface(const T1 &t1): WrappedInterface(t1) {}
|
||||
template<typename T1, typename T2> ThreadedInterface(const T1 &t1, const T2 &t2): WrappedInterface(t1, t2) {}
|
||||
|
||||
};
|
||||
|
||||
|
||||
} // namespace can
|
||||
#endif
|
||||
@@ -0,0 +1,44 @@
|
||||
#ifndef SOCKETCAN_INTERFACE_XMLRPC_SETTINGS_H
|
||||
#define SOCKETCAN_INTERFACE_XMLRPC_SETTINGS_H
|
||||
#include <socketcan_interface/logging.h>
|
||||
|
||||
#include <socketcan_interface/settings.h>
|
||||
#include "xmlrpcpp/XmlRpcValue.h"
|
||||
#include <sstream>
|
||||
#include <string>
|
||||
|
||||
class XmlRpcSettings : public can::Settings {
|
||||
public:
|
||||
XmlRpcSettings() {}
|
||||
XmlRpcSettings(const XmlRpc::XmlRpcValue &v) : value_(v) {}
|
||||
XmlRpcSettings& operator=(const XmlRpc::XmlRpcValue &v) { value_ = v; return *this; }
|
||||
template<typename T> static can::SettingsConstSharedPtr create(T nh, const std::string &ns="/") {
|
||||
std::shared_ptr<XmlRpcSettings> settings(new XmlRpcSettings);
|
||||
nh.getParam(ns, settings->value_);
|
||||
return settings;
|
||||
}
|
||||
private:
|
||||
virtual bool getRepr(const std::string &name, std::string & repr) const {
|
||||
const XmlRpc::XmlRpcValue *value = &value_;
|
||||
|
||||
std::string n = name;
|
||||
size_t delim_pos;
|
||||
while (value->getType() == XmlRpc::XmlRpcValue::TypeStruct && (delim_pos = n.find('/')) != std::string::npos){
|
||||
std::string segment = n.substr(0, delim_pos);
|
||||
if (!value->hasMember(segment)) return false;
|
||||
value = &((*value)[segment]);
|
||||
n.erase(0, delim_pos+1);
|
||||
}
|
||||
if(value->hasMember(n)){
|
||||
std::stringstream sstr;
|
||||
sstr << (*value)[n];
|
||||
repr = sstr.str();
|
||||
return true;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
XmlRpc::XmlRpcValue value_;
|
||||
|
||||
};
|
||||
|
||||
#endif
|
||||
32
Devices/Libraries/Ros/ros_canopen/socketcan_interface/package.xml
Executable file
32
Devices/Libraries/Ros/ros_canopen/socketcan_interface/package.xml
Executable file
@@ -0,0 +1,32 @@
|
||||
<?xml version="1.0"?>
|
||||
<package format="2">
|
||||
<name>socketcan_interface</name>
|
||||
<version>0.8.5</version>
|
||||
<description>Generic CAN interface description with helpers for filtering and driver implementation. Further a socketcan implementation based on boost::asio is included.</description>
|
||||
|
||||
<maintainer email="mathias.luedtke@ipa.fraunhofer.de">Mathias Lüdtke</maintainer>
|
||||
<author email="mathias.luedtke@ipa.fraunhofer.de">Mathias Lüdtke</author>
|
||||
|
||||
<license>LGPLv3</license>
|
||||
|
||||
<url type="website">http://wiki.ros.org/socketcan_interface</url>
|
||||
<url type="repository">https://github.com/ros-industrial/ros_canopen</url>
|
||||
<url type="bugtracker">https://github.com/ros-industrial/ros_canopen/issues</url>
|
||||
|
||||
<buildtool_depend>catkin</buildtool_depend>
|
||||
|
||||
<depend>libboost-dev</depend>
|
||||
<depend>libboost-chrono-dev</depend>
|
||||
<depend>libboost-system-dev</depend>
|
||||
<depend>libboost-thread-dev</depend>
|
||||
<depend>class_loader</depend>
|
||||
<depend>libconsole-bridge-dev</depend>
|
||||
<depend>linux-kernel-headers</depend>
|
||||
|
||||
<test_depend>rosunit</test_depend>
|
||||
<test_depend>xmlrpcpp</test_depend>
|
||||
|
||||
<export>
|
||||
<socketcan_interface plugin="${prefix}/socketcan_interface_plugin.xml" />
|
||||
</export>
|
||||
</package>
|
||||
@@ -0,0 +1,5 @@
|
||||
<library path="lib/libsocketcan_interface_plugin">
|
||||
<class type="can::SocketCANInterface" base_class_type="can::DriverInterface">
|
||||
<description>SocketCAN inteface plugin.</description>
|
||||
</class>
|
||||
</library>
|
||||
39
Devices/Libraries/Ros/ros_canopen/socketcan_interface/src/canbcm.cpp
Executable file
39
Devices/Libraries/Ros/ros_canopen/socketcan_interface/src/canbcm.cpp
Executable file
@@ -0,0 +1,39 @@
|
||||
#include <socketcan_interface/bcm.h>
|
||||
#include <socketcan_interface/string.h>
|
||||
|
||||
using namespace can;
|
||||
|
||||
#include <iostream>
|
||||
|
||||
int main(int argc, char *argv[]){
|
||||
BCMsocket bcm;
|
||||
|
||||
int extra_frames = argc - 4;
|
||||
|
||||
if(extra_frames < 0){
|
||||
std::cout << "usage: "<< argv[0] << " DEVICE PERIOD HEADER#DATA [DATA*]" << std::endl;
|
||||
return 1;
|
||||
}
|
||||
|
||||
if(!bcm.init(argv[1])){
|
||||
return 2;
|
||||
}
|
||||
|
||||
int num_frames = extra_frames+1;
|
||||
Frame *frames = new Frame[num_frames];
|
||||
Header header = frames[0] = toframe(argv[3]);
|
||||
|
||||
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<double>(atof(argv[2])), header, num_frames, frames)){
|
||||
pause();
|
||||
return 0;
|
||||
}
|
||||
return 4;
|
||||
}
|
||||
90
Devices/Libraries/Ros/ros_canopen/socketcan_interface/src/candump.cpp
Executable file
90
Devices/Libraries/Ros/ros_canopen/socketcan_interface/src/candump.cpp
Executable file
@@ -0,0 +1,90 @@
|
||||
#include <iostream>
|
||||
#include <memory>
|
||||
#include <unordered_set>
|
||||
|
||||
#include <boost/exception/diagnostic_information.hpp>
|
||||
#include <class_loader/class_loader.hpp>
|
||||
#include <socketcan_interface/socketcan.h>
|
||||
|
||||
using namespace can;
|
||||
|
||||
#include <iostream>
|
||||
|
||||
void print_frame(const 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";
|
||||
|
||||
if(f.is_rtr){
|
||||
std::cout << "r";
|
||||
}else{
|
||||
std::cout << (int) f.dlc << std::hex;
|
||||
|
||||
for(int i=0; i < f.dlc; ++i){
|
||||
std::cout << std::hex << " " << (int) f.data[i];
|
||||
}
|
||||
}
|
||||
|
||||
std::cout << std::dec << std::endl;
|
||||
}
|
||||
|
||||
|
||||
std::shared_ptr<class_loader::ClassLoader> g_loader;
|
||||
DriverInterfaceSharedPtr g_driver;
|
||||
|
||||
void print_state(const State & s){
|
||||
std::string err;
|
||||
g_driver->translateError(s.internal_error,err);
|
||||
std::cout << "STATE: driver_state=" << s.driver_state << " internal_error=" << s.internal_error << "('" << 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<class_loader::ClassLoader>(argv[2]);
|
||||
g_driver = g_loader->createUniqueInstance<DriverInterface>(argv[3]);
|
||||
}
|
||||
|
||||
catch(std::exception& ex)
|
||||
{
|
||||
std::cerr << boost::diagnostic_information(ex) << std::endl;;
|
||||
return 1;
|
||||
}
|
||||
}else{
|
||||
g_driver = std::make_shared<SocketCANInterface>();
|
||||
}
|
||||
|
||||
|
||||
|
||||
FrameListenerConstSharedPtr frame_printer = g_driver->createMsgListener(print_frame);
|
||||
StateListenerConstSharedPtr error_printer = g_driver->createStateListener(print_state);
|
||||
|
||||
if(!g_driver->init(argv[1], false, can::NoSettings::create())){
|
||||
print_state(g_driver->getState());
|
||||
return 1;
|
||||
}
|
||||
|
||||
g_driver->run();
|
||||
|
||||
g_driver->shutdown();
|
||||
g_driver.reset();
|
||||
g_loader.reset();
|
||||
|
||||
return 0;
|
||||
|
||||
}
|
||||
@@ -0,0 +1,4 @@
|
||||
#include <class_loader/class_loader.hpp>
|
||||
#include <socketcan_interface/socketcan.h>
|
||||
|
||||
CLASS_LOADER_REGISTER_CLASS(can::SocketCANInterface, can::DriverInterface);
|
||||
182
Devices/Libraries/Ros/ros_canopen/socketcan_interface/src/string.cpp
Executable file
182
Devices/Libraries/Ros/ros_canopen/socketcan_interface/src/string.cpp
Executable file
@@ -0,0 +1,182 @@
|
||||
#include <socketcan_interface/string.h>
|
||||
#include <iomanip>
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
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 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;
|
||||
}
|
||||
|
||||
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<std::string>(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);
|
||||
}
|
||||
|
||||
}
|
||||
102
Devices/Libraries/Ros/ros_canopen/socketcan_interface/test/test_delegates.cpp
Executable file
102
Devices/Libraries/Ros/ros_canopen/socketcan_interface/test/test_delegates.cpp
Executable file
@@ -0,0 +1,102 @@
|
||||
// Bring in my package's API, which is what I'm testing
|
||||
#include <socketcan_interface/delegates.h>
|
||||
#include <socketcan_interface/dummy.h>
|
||||
#include <boost/bind.hpp>
|
||||
|
||||
// Bring in gtest
|
||||
#include <gtest/gtest.h>
|
||||
|
||||
class Receiver {
|
||||
public:
|
||||
std::list<std::string> 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::DummyBus bus("testFrameDelegate");
|
||||
can::ThreadedDummyInterface dummy;
|
||||
dummy.init(bus.name, true, can::NoSettings::create());
|
||||
Receiver r1, r3, r4, r5;
|
||||
boost::shared_ptr<Receiver> r6(new Receiver());
|
||||
std::shared_ptr<Receiver> 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<std::string> expected;
|
||||
dummy.send(can::toframe("0#8200"));
|
||||
dummy.flush();
|
||||
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<bool(void)>;
|
||||
using BoolDelegate = can::DelegateHelper<BoolFunc>;
|
||||
|
||||
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();
|
||||
}
|
||||
@@ -0,0 +1,84 @@
|
||||
// Bring in my package's API, which is what I'm testing
|
||||
#include <socketcan_interface/dispatcher.h>
|
||||
|
||||
// Bring in gtest
|
||||
#include <gtest/gtest.h>
|
||||
|
||||
class Counter {
|
||||
public:
|
||||
size_t counter_;
|
||||
Counter() : counter_(0) {}
|
||||
void count(const can::Frame &msg) {
|
||||
++counter_;
|
||||
}
|
||||
};
|
||||
|
||||
TEST(DispatcherTest, testFilteredDispatcher)
|
||||
{
|
||||
can::FilteredDispatcher<unsigned int, can::CommInterface::FrameListener> dispatcher;
|
||||
Counter counter1;
|
||||
Counter counter2;
|
||||
std::vector<can::CommInterface::FrameListenerConstSharedPtr> 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<boost::chrono::duration<double> >(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<can::CommInterface::FrameListener> 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<boost::chrono::duration<double> >(now-start).count();
|
||||
|
||||
EXPECT_EQ(num, counter.counter_);
|
||||
std::cout << std::fixed << diff << "\t" << num << "\t" << num / diff << std::endl;
|
||||
}
|
||||
|
||||
TEST(DispatcherTest, testDelegateOnly)
|
||||
{
|
||||
Counter counter;
|
||||
can::CommInterface::FrameDelegate delegate(&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 = 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<boost::chrono::duration<double> >(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();
|
||||
}
|
||||
@@ -0,0 +1,42 @@
|
||||
// Bring in my package's API, which is what I'm testing
|
||||
#include <socketcan_interface/dispatcher.h>
|
||||
#include <socketcan_interface/dummy.h>
|
||||
|
||||
// Bring in gtest
|
||||
#include <gtest/gtest.h>
|
||||
|
||||
// Declare a test
|
||||
TEST(DummyInterfaceTest, testCase1)
|
||||
{
|
||||
can::DummyBus bus("testCase1");
|
||||
can::ThreadedDummyInterface dummy;
|
||||
dummy.init(bus.name, true, can::NoSettings::create());
|
||||
|
||||
can::DummyReplay replay;
|
||||
replay.add("0#8200", {"701#00", "701#04"});
|
||||
replay.init(bus);
|
||||
|
||||
std::list<std::string> expected{"0#8200", "701#00", "701#04"};
|
||||
std::list<std::string> responses;
|
||||
|
||||
auto listener = dummy.createMsgListener([&responses](auto& f) {
|
||||
responses.push_back(can::tostring(f, true));
|
||||
});
|
||||
|
||||
EXPECT_FALSE(replay.done());
|
||||
|
||||
dummy.send(can::toframe("0#8200"));
|
||||
|
||||
replay.flush();
|
||||
dummy.flush();
|
||||
|
||||
EXPECT_EQ(expected, responses);
|
||||
EXPECT_TRUE(replay.done());
|
||||
}
|
||||
|
||||
|
||||
// Run all the tests that were declared with TEST()
|
||||
int main(int argc, char **argv){
|
||||
testing::InitGoogleTest(&argc, argv);
|
||||
return RUN_ALL_TESTS();
|
||||
}
|
||||
115
Devices/Libraries/Ros/ros_canopen/socketcan_interface/test/test_filter.cpp
Executable file
115
Devices/Libraries/Ros/ros_canopen/socketcan_interface/test/test_filter.cpp
Executable file
@@ -0,0 +1,115 @@
|
||||
// Bring in my package's API, which is what I'm testing
|
||||
#include <socketcan_interface/filter.h>
|
||||
|
||||
|
||||
#include <socketcan_interface/string.h>
|
||||
#include <socketcan_interface/dummy.h>
|
||||
|
||||
// Bring in gtest
|
||||
#include <gtest/gtest.h>
|
||||
|
||||
|
||||
TEST(FilterTest, simpleMask)
|
||||
{
|
||||
const std::string msg1("123#");
|
||||
const std::string msg2("124#");
|
||||
|
||||
can::FrameFilterSharedPtr f1 = can::tofilter("123");
|
||||
|
||||
EXPECT_TRUE(f1->pass(can::toframe(msg1)));
|
||||
EXPECT_FALSE(f1->pass(can::toframe(msg2)));
|
||||
}
|
||||
|
||||
TEST(FilterTest, maskTests)
|
||||
{
|
||||
const std::string msg1("123#");
|
||||
const std::string msg2("124#");
|
||||
const std::string msg3("122#");
|
||||
|
||||
can::FrameFilterSharedPtr f1 = can::tofilter("123:123");
|
||||
can::FrameFilterSharedPtr f2 = can::tofilter("123:ffe");
|
||||
can::FrameFilterSharedPtr f3 = can::tofilter("123~123");
|
||||
|
||||
EXPECT_TRUE(f1->pass(can::toframe(msg1)));
|
||||
EXPECT_FALSE(f1->pass(can::toframe(msg2)));
|
||||
EXPECT_FALSE(f1->pass(can::toframe(msg3)));
|
||||
|
||||
|
||||
EXPECT_TRUE(f2->pass(can::toframe(msg1)));
|
||||
EXPECT_FALSE(f2->pass(can::toframe(msg2)));
|
||||
EXPECT_TRUE(f2->pass(can::toframe(msg3)));
|
||||
|
||||
EXPECT_FALSE(f3->pass(can::toframe(msg1)));
|
||||
EXPECT_TRUE(f3->pass(can::toframe(msg2)));
|
||||
EXPECT_TRUE(f3->pass(can::toframe(msg3)));
|
||||
|
||||
}
|
||||
|
||||
TEST(FilterTest, rangeTest)
|
||||
{
|
||||
const std::string msg1("120#");
|
||||
const std::string msg2("125#");
|
||||
const std::string msg3("130#");
|
||||
|
||||
can::FrameFilterSharedPtr f1 = can::tofilter("120-120");
|
||||
can::FrameFilterSharedPtr f2 = can::tofilter("120_120");
|
||||
can::FrameFilterSharedPtr f3 = can::tofilter("120-125");
|
||||
|
||||
EXPECT_TRUE(f1->pass(can::toframe(msg1)));
|
||||
EXPECT_FALSE(f1->pass(can::toframe(msg2)));
|
||||
EXPECT_FALSE(f1->pass(can::toframe(msg3)));
|
||||
|
||||
EXPECT_FALSE(f2->pass(can::toframe(msg1)));
|
||||
EXPECT_TRUE(f2->pass(can::toframe(msg2)));
|
||||
EXPECT_TRUE(f2->pass(can::toframe(msg3)));
|
||||
|
||||
EXPECT_TRUE(f3->pass(can::toframe(msg1)));
|
||||
EXPECT_TRUE(f3->pass(can::toframe(msg2)));
|
||||
EXPECT_FALSE(f3->pass(can::toframe(msg3)));
|
||||
|
||||
}
|
||||
|
||||
class Counter {
|
||||
public:
|
||||
size_t count_;
|
||||
Counter(): count_(0) {}
|
||||
void count(const can::Frame &frame) {
|
||||
++count_;
|
||||
}
|
||||
};
|
||||
|
||||
TEST(FilterTest, listenerTest)
|
||||
{
|
||||
|
||||
Counter counter;
|
||||
can::DummyBus bus("listenerTest");
|
||||
can::ThreadedDummyInterfaceSharedPtr dummy = std::make_shared<can::ThreadedDummyInterface>();
|
||||
dummy->init(bus.name, true, can::NoSettings::create());
|
||||
|
||||
|
||||
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::Frame f1 = can::toframe("123#");
|
||||
can::Frame f2 = can::toframe("124#");
|
||||
can::Frame f3 = can::toframe("122#");
|
||||
|
||||
dummy->send(f1);
|
||||
dummy->flush();
|
||||
EXPECT_EQ(1, counter.count_);
|
||||
dummy->send(f2);
|
||||
dummy->flush();
|
||||
EXPECT_EQ(1, counter.count_);
|
||||
dummy->send(f3);
|
||||
dummy->flush();
|
||||
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();
|
||||
}
|
||||
91
Devices/Libraries/Ros/ros_canopen/socketcan_interface/test/test_settings.cpp
Executable file
91
Devices/Libraries/Ros/ros_canopen/socketcan_interface/test/test_settings.cpp
Executable file
@@ -0,0 +1,91 @@
|
||||
// Bring in my package's API, which is what I'm testing
|
||||
#include <socketcan_interface/settings.h>
|
||||
#include <socketcan_interface/xmlrpc_settings.h>
|
||||
#include <socketcan_interface/socketcan.h>
|
||||
|
||||
// Bring in gtest
|
||||
#include <gtest/gtest.h>
|
||||
|
||||
TEST(SettingTest, socketcan_masks)
|
||||
{
|
||||
const can_err_mask_t fatal_errors = ( CAN_ERR_TX_TIMEOUT /* TX timeout (by netdevice driver) */
|
||||
| CAN_ERR_BUSOFF /* bus off */
|
||||
| CAN_ERR_BUSERROR /* bus error (may flood!) */
|
||||
| CAN_ERR_RESTARTED /* controller restarted */
|
||||
);
|
||||
const can_err_mask_t report_errors = ( 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::SocketCANInterface sci;
|
||||
|
||||
// defaults
|
||||
sci.init("None", false, can::NoSettings::create());
|
||||
EXPECT_EQ(sci.getErrorMask(), fatal_errors | report_errors);
|
||||
EXPECT_EQ(sci.getFatalErrorMask(), fatal_errors);
|
||||
|
||||
// remove report-only flag
|
||||
auto m1 = can::SettingsMap::create();
|
||||
m1->set("error_mask/CAN_ERR_LOSTARB", false);
|
||||
sci.init("None", false, m1);
|
||||
EXPECT_EQ(sci.getErrorMask(), (fatal_errors | report_errors) & (~CAN_ERR_LOSTARB));
|
||||
EXPECT_EQ(sci.getFatalErrorMask(), fatal_errors);
|
||||
|
||||
// cannot remove fatal flag from report only
|
||||
auto m2 = can::SettingsMap::create();
|
||||
m2->set("error_mask/CAN_ERR_TX_TIMEOUT", false);
|
||||
sci.init("None", false, m2);
|
||||
EXPECT_EQ(sci.getErrorMask(), (fatal_errors | report_errors));
|
||||
EXPECT_EQ(sci.getFatalErrorMask(), fatal_errors);
|
||||
|
||||
// remove fatal flag
|
||||
auto m3 = can::SettingsMap::create();
|
||||
m3->set("fatal_error_mask/CAN_ERR_TX_TIMEOUT", false);
|
||||
sci.init("None", false, m3);
|
||||
EXPECT_EQ(sci.getErrorMask(), (fatal_errors | report_errors) & (~CAN_ERR_TX_TIMEOUT));
|
||||
EXPECT_EQ(sci.getFatalErrorMask(), fatal_errors & (~CAN_ERR_TX_TIMEOUT));
|
||||
|
||||
// remove fatal and report flag
|
||||
auto m4 = can::SettingsMap::create();
|
||||
m4->set("fatal_error_mask/CAN_ERR_TX_TIMEOUT", false);
|
||||
m4->set("error_mask/CAN_ERR_LOSTARB", false);
|
||||
sci.init("None", false, m4);
|
||||
EXPECT_EQ(sci.getErrorMask(), (fatal_errors | report_errors) & ~(CAN_ERR_TX_TIMEOUT|CAN_ERR_LOSTARB));
|
||||
EXPECT_EQ(sci.getFatalErrorMask(), fatal_errors & (~CAN_ERR_TX_TIMEOUT));
|
||||
}
|
||||
|
||||
TEST(SettingTest, xmlrpc)
|
||||
{
|
||||
XmlRpc::XmlRpcValue value;
|
||||
value["param"] = 1;
|
||||
XmlRpc::XmlRpcValue segment;
|
||||
segment["param"] = 2;
|
||||
value["segment"] = segment;
|
||||
XmlRpcSettings settings(value);
|
||||
|
||||
ASSERT_TRUE(value["segment"].hasMember(std::string("param")));
|
||||
|
||||
int res;
|
||||
EXPECT_TRUE(settings.get<int>("param", res));
|
||||
EXPECT_EQ(res, 1);
|
||||
EXPECT_EQ(settings.get_optional("param", 0), 1);
|
||||
EXPECT_FALSE(settings.get<int>("param2", res));
|
||||
EXPECT_EQ(settings.get_optional("param2", 0), 0);
|
||||
|
||||
EXPECT_TRUE(settings.get<int>("segment/param", res));
|
||||
EXPECT_EQ(res, 2);
|
||||
EXPECT_EQ(settings.get_optional("segment/param", 0), 2);
|
||||
|
||||
EXPECT_FALSE(settings.get<int>("segment/param2", res));
|
||||
EXPECT_EQ(settings.get_optional("segment/param2", 0), 0);
|
||||
EXPECT_FALSE(settings.get<int>("segment2/param", res));
|
||||
EXPECT_EQ(settings.get_optional("segment2/param", 0), 0);
|
||||
}
|
||||
|
||||
// Run all the tests that were declared with TEST()
|
||||
int main(int argc, char **argv){
|
||||
testing::InitGoogleTest(&argc, argv);
|
||||
return RUN_ALL_TESTS();
|
||||
}
|
||||
53
Devices/Libraries/Ros/ros_canopen/socketcan_interface/test/test_string.cpp
Executable file
53
Devices/Libraries/Ros/ros_canopen/socketcan_interface/test/test_string.cpp
Executable file
@@ -0,0 +1,53 @@
|
||||
// Bring in my package's API, which is what I'm testing
|
||||
#include <socketcan_interface/string.h>
|
||||
|
||||
// Bring in gtest
|
||||
#include <gtest/gtest.h>
|
||||
|
||||
|
||||
TEST(StringTest, stringconversion)
|
||||
{
|
||||
const std::string s1("123#1234567812345678");
|
||||
can::Frame f1 = can::toframe(s1);
|
||||
EXPECT_EQ(s1, can::tostring(f1, true));
|
||||
|
||||
const std::string s2("1337#1234567812345678");
|
||||
can::Frame f2 = can::toframe(s2);
|
||||
EXPECT_FALSE(f2.isValid());
|
||||
|
||||
const std::string s3("80001337#1234567812345678");
|
||||
const std::string s4("00001337#1234567812345678");
|
||||
|
||||
can::Frame f3 = can::toframe(s3);
|
||||
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
|
||||
|
||||
can::Frame f4 = can::toframe(s4);
|
||||
EXPECT_EQ(f4.fullid(), 0x80001337);
|
||||
EXPECT_TRUE(f4.isValid());
|
||||
EXPECT_TRUE(f4.is_extended);
|
||||
EXPECT_EQ(s4, can::tostring(f4, true));
|
||||
|
||||
const std::string s5("20001337#1234567812345678");
|
||||
can::Frame f5 = can::toframe(s5);
|
||||
EXPECT_EQ(f5.fullid(), 0xA0001337);
|
||||
EXPECT_TRUE(f5.isValid());
|
||||
EXPECT_TRUE(f5.is_error);
|
||||
EXPECT_EQ(s5, can::tostring(f5, true));
|
||||
|
||||
const std::string s6("40001337#1234567812345678");
|
||||
can::Frame f6 = can::toframe(s6);
|
||||
EXPECT_EQ(f6.fullid(), 0xC0001337);
|
||||
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();
|
||||
}
|
||||
Reference in New Issue
Block a user