Option to ignore error frames

canopen_chain_node/src/ros_chain.cpp

@@ -1,5 +1,6 @@
 #include <ros/package.h>
 
+#include <socketcan_interface/xmlrpc_settings.h>
 #include <canopen_chain_node/ros_chain.h>
 
 #include <std_msgs/Int8.h>
@@ -325,7 +326,7 @@ bool RosChain::setup_bus(){
         return false;
     }
 
-    add(std::make_shared<CANLayer>(interface_, can_device, loopback));
+    add(std::make_shared<CANLayer>(interface_, can_device, loopback, XmlRpcSettings::create(bus_nh)));
 
     return true;
 }

canopen_chain_node/src/sync_node.cpp

@@ -1,5 +1,6 @@
 #include <canopen_master/bcm_sync.h>
 #include <socketcan_interface/string.h>
+#include <socketcan_interface/xmlrpc_settings.h>
 #include <canopen_master/can_layer.h>
 #include <ros/ros.h>
 #include <diagnostic_updater/diagnostic_updater.h>
@@ -86,7 +87,7 @@ int main(int argc, char** argv){
 
     canopen::LayerStack stack("SyncNodeLayer");
 
-    stack.add(std::make_shared<canopen::CANLayer>(driver, can_device, false));
+    stack.add(std::make_shared<canopen::CANLayer>(driver, can_device, false, XmlRpcSettings::create(nh_priv, "bus")));
     stack.add(sync);
 
     diagnostic_updater::Updater diag_updater(nh, nh_priv);

canopen_master/include/canopen_master/can_layer.h

@@ -12,6 +12,7 @@ class CANLayer: public Layer{
     can::DriverInterfaceSharedPtr driver_;
     const std::string device_;
     const bool loopback_;
+    can::SettingsConstSharedPtr settings_;
     can::Frame last_error_;
     can::FrameListenerConstSharedPtr error_listener_;
     void handleFrame(const can::Frame & msg){
@@ -22,8 +23,11 @@ class CANLayer: public Layer{
     std::shared_ptr<boost::thread> thread_;
 
 public:
-    CANLayer(const can::DriverInterfaceSharedPtr &driver, const std::string &device, bool loopback)
-    : Layer(device + " Layer"), driver_(driver), device_(device), loopback_(loopback) { assert(driver_); }
+    CANLayer(const can::DriverInterfaceSharedPtr &driver, const std::string &device, bool loopback, can::SettingsConstSharedPtr settings)
+    : Layer(device + " Layer"), driver_(driver), device_(device), loopback_(loopback), settings_(settings) { assert(driver_); }
+
+    [[deprecated("provide settings explicitly")]]  CANLayer(const can::DriverInterfaceSharedPtr &driver, const std::string &device, bool loopback)
+    : Layer(device + " Layer"), driver_(driver), device_(device), loopback_(loopback), settings_(can::NoSettings::create()) { assert(driver_); }
 
     virtual void handleRead(LayerStatus &status, const LayerState &current_state) {
         if(current_state > Init){
@@ -66,7 +70,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_, settings_)) {
             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::create())){
         std::cout << "Could not initialize CAN" << std::endl;
         return 1;
     }

canopen_motor_node/src/motor_chain.cpp

@@ -1,30 +1,9 @@
 
 #include <canopen_motor_node/motor_chain.h>
 #include <canopen_motor_node/handle_layer.h>
-
+#include <socketcan_interface/xmlrpc_settings.h>
 using namespace canopen;
 
-
-class XmlRpcSettings : public Settings{
-public:
-    XmlRpcSettings() {}
-    XmlRpcSettings(const XmlRpc::XmlRpcValue &v) : value_(v) {}
-    XmlRpcSettings& operator=(const XmlRpc::XmlRpcValue &v) { value_ = v; return *this; }
-private:
-    virtual bool getRepr(const std::string &n, std::string & repr) const {
-        if(value_.hasMember(n)){
-            std::stringstream sstr;
-            sstr << const_cast< XmlRpc::XmlRpcValue &>(value_)[n]; // does not write since already existing
-            repr = sstr.str();
-            return true;
-        }
-        return false;
-    }
-    XmlRpc::XmlRpcValue value_;
-
-};
-
-
 MotorChain::MotorChain(const ros::NodeHandle &nh, const ros::NodeHandle &nh_priv) :
         RosChain(nh, nh_priv), motor_allocator_("canopen_402", "canopen::MotorBase::Allocator") {}
 

socketcan_bridge/src/socketcan_bridge_node.cpp

@@ -29,6 +29,7 @@
 #include <socketcan_bridge/topic_to_socketcan.h>
 #include <socketcan_bridge/socketcan_to_topic.h>
 #include <socketcan_interface/threading.h>
+#include <socketcan_interface/xmlrpc_settings.h>
 #include <memory>
 #include <string>
 
@@ -44,7 +45,8 @@ 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.
+  // initialize device at can_device, 0 for no loopback.
+  if (!driver->init(can_device, 0, XmlRpcSettings::create(nh_param)))
   {
     ROS_FATAL("Failed to initialize can_device at %s", can_device.c_str());
     return 1;

socketcan_bridge/src/socketcan_to_topic_node.cpp

@@ -29,6 +29,7 @@
 #include <socketcan_bridge/socketcan_to_topic.h>
 #include <socketcan_interface/threading.h>
 #include <socketcan_interface/string.h>
+#include <socketcan_interface/xmlrpc_settings.h>
 #include <memory>
 #include <string>
 
@@ -45,7 +46,8 @@ 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.
+  // initialize device at can_device, 0 for no loopback.
+  if (!driver->init(can_device, 0, XmlRpcSettings::create(nh_param)))
   {
     ROS_FATAL("Failed to initialize can_device at %s", can_device.c_str());
     return 1;

socketcan_bridge/src/topic_to_socketcan_node.cpp

@@ -29,6 +29,7 @@
 #include <socketcan_bridge/topic_to_socketcan.h>
 #include <socketcan_interface/threading.h>
 #include <socketcan_interface/string.h>
+#include <socketcan_interface/xmlrpc_settings.h>
 #include <memory>
 #include <string>
 
@@ -45,7 +46,8 @@ 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.
+  // initialize device at can_device, 0 for no loopback.
+  if (!driver->init(can_device, 0, XmlRpcSettings::create(nh_param)))
   {
     ROS_FATAL("Failed to initialize can_device at %s", can_device.c_str());
     return 1;

socketcan_interface/CMakeLists.txt

@@ -104,6 +104,7 @@ install(
   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
@@ -122,6 +123,18 @@ if(CATKIN_ENABLE_TESTING)
     ${catkin_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
   )

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, 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
@@ -217,6 +227,4 @@ using DriverInterfaceSharedPtr = std::shared_ptr<DriverInterface>;
 
 } // namespace can
 
-#include "logging.h"
-
 #endif

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