Initial commit

sensors/ros_canopen-melodic/socketcan_interface/include/socketcan_interface/asio_base.h

@@ -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

sensors/ros_canopen-melodic/socketcan_interface/include/socketcan_interface/bcm.h

@@ -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

sensors/ros_canopen-melodic/socketcan_interface/include/socketcan_interface/delegates.h

@@ -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_

sensors/ros_canopen-melodic/socketcan_interface/include/socketcan_interface/dispatcher.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

sensors/ros_canopen-melodic/socketcan_interface/include/socketcan_interface/dummy.h

@@ -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

sensors/ros_canopen-melodic/socketcan_interface/include/socketcan_interface/filter.h

@@ -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*/

sensors/ros_canopen-melodic/socketcan_interface/include/socketcan_interface/interface.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

sensors/ros_canopen-melodic/socketcan_interface/include/socketcan_interface/logging.h

@@ -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

sensors/ros_canopen-melodic/socketcan_interface/include/socketcan_interface/make_shared.h

@@ -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

sensors/ros_canopen-melodic/socketcan_interface/include/socketcan_interface/reader.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

sensors/ros_canopen-melodic/socketcan_interface/include/socketcan_interface/settings.h

@@ -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

sensors/ros_canopen-melodic/socketcan_interface/include/socketcan_interface/socketcan.h

@@ -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

sensors/ros_canopen-melodic/socketcan_interface/include/socketcan_interface/string.h

@@ -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

sensors/ros_canopen-melodic/socketcan_interface/include/socketcan_interface/threading.h

@@ -0,0 +1,102 @@
+#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_;
+    SettingsConstSharedPtr settings_;
+    std::string device_;
+    bool loopback_;
+    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 {
+        device_ =  device;
+        loopback_ = loopback;
+        settings_ = settings;
+        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 bool recover(){
+        shutdown();
+        return init(device_,loopback_,settings_);
+    }
+    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

sensors/ros_canopen-melodic/socketcan_interface/include/socketcan_interface/xmlrpc_settings.h

@@ -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