Option to ignore error frames

canopen_master/include/canopen_master/can_layer.h

@@ -66,7 +66,7 @@ class CANLayer: public Layer{
     virtual void handleInit(LayerStatus &status){
 	if(thread_){
             status.warn("CAN thread already running");
-        } else if(!driver_->init(device_, loopback_)) {
+        } else if(!driver_->init(device_, loopback_, can::NoSettings())) {
             status.error("CAN init failed");
         } else {
             can::StateWaiter waiter(driver_.get());

canopen_master/include/canopen_master/canopen.h

@@ -325,26 +325,7 @@ class Master{
 };
 typedef Master::MasterSharedPtr MasterSharedPtr;
 
-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 can::Settings;
 
 } // canopen
 #endif // !H_CANOPEN

canopen_master/src/bcm_sync.cpp

@@ -44,7 +44,7 @@ int main(int argc, char** argv){
     }
 
     can::SocketCANDriverSharedPtr driver = std::make_shared<can::SocketCANDriver>();
-    if(!driver->init(can_device, false)){
+    if(!driver->init(can_device, false, can::NoSettings())){
         std::cout << "Could not initialize CAN" << std::endl;
         return 1;
     }

socketcan_bridge/src/socketcan_bridge_node.cpp

@@ -43,7 +43,7 @@ int main(int argc, char *argv[])
 
   can::ThreadedSocketCANInterfaceSharedPtr driver = std::make_shared<can::ThreadedSocketCANInterface> ();
 
-  if (!driver->init(can_device, 0))  // initialize device at can_device, 0 for no loopback.
+  if (!driver->init(can_device, 0, can::NoSettings()))  // initialize device at can_device, 0 for no loopback.
   {
     ROS_FATAL("Failed to initialize can_device at %s", can_device.c_str());
     return 1;

socketcan_bridge/src/socketcan_to_topic_node.cpp

@@ -32,6 +32,19 @@
 #include <string>
 
 
+int get_ignore_error_param(ros::NodeHandle nh, std::string param_name)
+{
+  bool param;
+  if (nh.getParam("ignore_errors/"+param_name, param))
+  {
+    ROS_INFO("Ignoring %s", param_name.c_str());
+    return static_cast<int>(param);
+  }
+  else
+  {
+    return -1;
+  }
+}
 
 int main(int argc, char *argv[])
 {
@@ -42,9 +55,26 @@ int main(int argc, char *argv[])
   std::string can_device;
   nh_param.param<std::string>("can_device", can_device, "can0");
 
+  // get ignored errors from ros parameters
+  can::ignored_errors_t ignored_errors;
+  ignored_errors["can_err_tx_timeout"] = get_ignore_error_param(nh, "can_err_tx_timeout");
+  ignored_errors["can_err_lostarb"] = get_ignore_error_param(nh, "can_err_lostarb");
+  ignored_errors["can_err_crtl"] = get_ignore_error_param(nh, "can_err_crtl");
+  ignored_errors["can_err_prot"] = get_ignore_error_param(nh, "can_err_prot");
+  ignored_errors["can_err_trx"] = get_ignore_error_param(nh, "can_err_trx");
+  ignored_errors["can_err_ack"] = get_ignore_error_param(nh, "can_err_ack");
+  ignored_errors["can_err_busoff"] = get_ignore_error_param(nh, "can_err_busoff");
+  ignored_errors["can_err_buserror"] = get_ignore_error_param(nh, "can_err_buserror");
+  ignored_errors["can_err_restarted"] = get_ignore_error_param(nh, "can_err_restarted");
+
+  auto settings = can::SettingsMap();
+
+  settings.set("ignored_errors", ignored_errors);
+
+
   can::ThreadedSocketCANInterfaceSharedPtr driver = std::make_shared<can::ThreadedSocketCANInterface> ();
 
-  if (!driver->init(can_device, 0))  // initialize device at can_device, 0 for no loopback.
+  if (!driver->init(can_device, 0, settings))  // initialize device at can_device, 0 for no loopback.
   {
     ROS_FATAL("Failed to initialize can_device at %s", can_device.c_str());
     return 1;

socketcan_bridge/src/topic_to_socketcan_node.cpp

@@ -32,6 +32,19 @@
 #include <string>
 
 
+int get_ignore_error_param(ros::NodeHandle nh, std::string param_name)
+{
+  bool param;
+  if (nh.getParam("ignore_errors/"+param_name, param))
+  {
+    ROS_INFO("Ignoring %s", param_name.c_str());
+    return static_cast<int>(param);
+  }
+  else
+  {
+    return -1;
+  }
+}
 
 int main(int argc, char *argv[])
 {
@@ -42,9 +55,26 @@ int main(int argc, char *argv[])
   std::string can_device;
   nh_param.param<std::string>("can_device", can_device, "can0");
 
+  // get ignored errors from ros parameters
+  can::ignored_errors_t ignored_errors;
+  ignored_errors["can_err_tx_timeout"] = get_ignore_error_param(nh, "can_err_tx_timeout");
+  ignored_errors["can_err_lostarb"] = get_ignore_error_param(nh, "can_err_lostarb");
+  ignored_errors["can_err_crtl"] = get_ignore_error_param(nh, "can_err_crtl");
+  ignored_errors["can_err_prot"] = get_ignore_error_param(nh, "can_err_prot");
+  ignored_errors["can_err_trx"] = get_ignore_error_param(nh, "can_err_trx");
+  ignored_errors["can_err_ack"] = get_ignore_error_param(nh, "can_err_ack");
+  ignored_errors["can_err_busoff"] = get_ignore_error_param(nh, "can_err_busoff");
+  ignored_errors["can_err_buserror"] = get_ignore_error_param(nh, "can_err_buserror");
+  ignored_errors["can_err_restarted"] = get_ignore_error_param(nh, "can_err_restarted");
+
+  auto settings = can::SettingsMap();
+
+  settings.set("ignored_errors", ignored_errors);
+
+
   can::ThreadedSocketCANInterfaceSharedPtr driver = std::make_shared<can::ThreadedSocketCANInterface> ();
 
-  if (!driver->init(can_device, 0))  // initialize device at can_device, 0 for no loopback.
+  if (!driver->init(can_device, 0, settings))  // initialize device at can_device, 0 for no loopback.
   {
     ROS_FATAL("Failed to initialize can_device at %s", can_device.c_str());
     return 1;

socketcan_interface/include/socketcan_interface/interface.h

@@ -8,6 +8,8 @@
 #include <boost/system/error_code.hpp>
 
 #include "socketcan_interface/delegates.h"
+#include "socketcan_interface/logging.h"
+#include "socketcan_interface/settings.h"
 
 namespace can{
 
@@ -173,17 +175,25 @@ class 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) = 0;
+    virtual bool init(const std::string &device, bool loopback, const Settings &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
@@ -217,6 +227,4 @@ using DriverInterfaceSharedPtr = std::shared_ptr<DriverInterface>;
 
 } // namespace can
 
-#include "logging.h"
-
 #endif

socketcan_interface/include/socketcan_interface/serializable_map.h

@@ -0,0 +1,59 @@
+#ifndef SOCKETCAN_INTERFACE_SERIALIZABLE_MAP_H
+#define SOCKETCAN_INTERFACE_SERIALIZABLE_MAP_H
+
+#include <map>
+#include <sstream>
+
+template<class Key, class Value>
+class serializable_map : public std::map<Key, Value> {
+private:
+  size_t offset;
+
+  template<class T>
+  void write(std::stringstream &ss, T &t) const {
+    ss.write((char*)(&t), sizeof(t));
+  }
+
+  void write(std::stringstream &ss, std::string &str) const {
+    size_t size = str.size();
+    ss.write((char*)(&size), sizeof(size));
+    ss.write((char*)(str.data()), str.length());
+  }
+
+  template<class T>
+  void read(const std::string &s, T &t) {
+    t = (T)(*(s.data() + offset));
+    offset += sizeof(T);
+  }
+
+  void read(const std::string &s, std::string &str) {
+    size_t size = (int)(*(s.data() + offset));
+    offset += sizeof(size_t);
+    std::string str2(s.data() + offset, s.data() + offset + size);
+    str = str2;
+    offset += size;
+  }
+public:
+  std::string serialize() const {
+    std::stringstream ss;
+    for (auto &i : (*this)) {
+      Key str = i.first;
+      Value value = i.second;
+      write(ss, str);
+      write(ss, value);
+    }
+    return ss.str();
+  }
+  void deserialize(const std::string &s) {
+    offset = 0;
+    while (offset < s.size()) {
+      Key key;
+      Value value;
+      read(s, key);
+      read(s, value);
+      (*this)[key] = value;
+    }
+  }
+};
+
+#endif /* SOCKETCAN_INTERFACE_SERIALIZABLE_MAP_H */

socketcan_interface/include/socketcan_interface/settings.h

@@ -0,0 +1,51 @@
+#ifndef SOCKETCAN_INTERFACE_SETTINGS_H
+#define SOCKETCAN_INTERFACE_SETTINGS_H
+
+#include <map>
+#include <string>
+#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;
+  };
+
+  class NoSettings : public Settings {
+  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);
+    }
+  };
+
+} // can
+
+#endif