From 18a2ef41c41c24078757dbdf9b2c05de54398c6c Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Mathias=20L=C3=BCdtke?= Date: Wed, 23 Oct 2019 23:25:49 +0200 Subject: [PATCH 1/6] moved canopen::Settings into can namespace --- .../include/canopen_master/canopen.h | 21 +----------- .../include/socketcan_interface/interface.h | 1 + .../include/socketcan_interface/settings.h | 32 +++++++++++++++++++ 3 files changed, 34 insertions(+), 20 deletions(-) create mode 100644 socketcan_interface/include/socketcan_interface/settings.h diff --git a/canopen_master/include/canopen_master/canopen.h b/canopen_master/include/canopen_master/canopen.h index a6f397620..dd589c9b7 100644 --- a/canopen_master/include/canopen_master/canopen.h +++ b/canopen_master/include/canopen_master/canopen.h @@ -325,26 +325,7 @@ class Master{ }; typedef Master::MasterSharedPtr MasterSharedPtr; -class Settings -{ -public: - template T get_optional(const std::string &n, const T& def) const { - std::string repr; - if(!getRepr(n, repr)){ - return def; - } - return boost::lexical_cast(repr); - } - template bool get(const std::string &n, T& val) const { - std::string repr; - if(!getRepr(n, repr)) return false; - val = boost::lexical_cast(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 diff --git a/socketcan_interface/include/socketcan_interface/interface.h b/socketcan_interface/include/socketcan_interface/interface.h index 9fd747348..f774c9572 100644 --- a/socketcan_interface/include/socketcan_interface/interface.h +++ b/socketcan_interface/include/socketcan_interface/interface.h @@ -8,6 +8,7 @@ #include #include "socketcan_interface/delegates.h" +#include "socketcan_interface/settings.h" namespace can{ diff --git a/socketcan_interface/include/socketcan_interface/settings.h b/socketcan_interface/include/socketcan_interface/settings.h new file mode 100644 index 000000000..b2840e545 --- /dev/null +++ b/socketcan_interface/include/socketcan_interface/settings.h @@ -0,0 +1,32 @@ +#ifndef SOCKETCAN_INTERFACE_SETTINGS_H +#define SOCKETCAN_INTERFACE_SETTINGS_H + +#include +#include +#include + +namespace can { + class Settings + { + public: + template T get_optional(const std::string &n, const T& def) const { + std::string repr; + if(!getRepr(n, repr)){ + return def; + } + return boost::lexical_cast(repr); + } + template bool get(const std::string &n, T& val) const { + std::string repr; + if(!getRepr(n, repr)) return false; + val = boost::lexical_cast(repr); + return true; + } + virtual ~Settings() {} + private: + virtual bool getRepr(const std::string &n, std::string & repr) const = 0; + }; + +} // can + +#endif From 180ba7c1045f81da18e7d1596ed369a41598c34f Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Mathias=20L=C3=BCdtke?= Date: Tue, 7 Jul 2020 13:14:37 +0200 Subject: [PATCH 2/6] moved XmlRpcSettings to socketcan_interface --- canopen_motor_node/src/motor_chain.cpp | 23 +-------------- .../socketcan_interface/xmlrpc_settings.h | 28 +++++++++++++++++++ 2 files changed, 29 insertions(+), 22 deletions(-) create mode 100644 socketcan_interface/include/socketcan_interface/xmlrpc_settings.h diff --git a/canopen_motor_node/src/motor_chain.cpp b/canopen_motor_node/src/motor_chain.cpp index f1188f921..cbd37f9e1 100644 --- a/canopen_motor_node/src/motor_chain.cpp +++ b/canopen_motor_node/src/motor_chain.cpp @@ -1,30 +1,9 @@ #include #include - +#include 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") {} diff --git a/socketcan_interface/include/socketcan_interface/xmlrpc_settings.h b/socketcan_interface/include/socketcan_interface/xmlrpc_settings.h new file mode 100644 index 000000000..da7a196a9 --- /dev/null +++ b/socketcan_interface/include/socketcan_interface/xmlrpc_settings.h @@ -0,0 +1,28 @@ +#ifndef SOCKETCAN_INTERFACE_XMLRPC_SETTINGS_H +#define SOCKETCAN_INTERFACE_XMLRPC_SETTINGS_H + +#include +#include "xmlrpcpp/XmlRpcValue.h" +#include +#include + +class XmlRpcSettings : public can::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_; + +}; + +#endif From aeb2858b3b4f44156fa08388fb991fac7fdd2ff5 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Mathias=20L=C3=BCdtke?= Date: Tue, 7 Jul 2020 18:34:45 +0200 Subject: [PATCH 3/6] added settings parameter to DriverInterface::init --- .../include/canopen_master/can_layer.h | 10 ++++--- canopen_master/src/bcm_sync.cpp | 2 +- .../include/socketcan_interface/interface.h | 15 ++++++++--- .../include/socketcan_interface/settings.h | 27 +++++++++++++++++++ .../include/socketcan_interface/socketcan.h | 6 +++-- .../include/socketcan_interface/threading.h | 13 ++++++++- socketcan_interface/src/candump.cpp | 2 +- 7 files changed, 63 insertions(+), 12 deletions(-) diff --git a/canopen_master/include/canopen_master/can_layer.h b/canopen_master/include/canopen_master/can_layer.h index 9fd258ecb..9b94c0c44 100644 --- a/canopen_master/include/canopen_master/can_layer.h +++ b/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 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 ¤t_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()); diff --git a/canopen_master/src/bcm_sync.cpp b/canopen_master/src/bcm_sync.cpp index df533b9fd..db915e7d1 100644 --- a/canopen_master/src/bcm_sync.cpp +++ b/canopen_master/src/bcm_sync.cpp @@ -44,7 +44,7 @@ int main(int argc, char** argv){ } can::SocketCANDriverSharedPtr driver = std::make_shared(); - 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; } diff --git a/socketcan_interface/include/socketcan_interface/interface.h b/socketcan_interface/include/socketcan_interface/interface.h index f774c9572..6ac79f1d0 100644 --- a/socketcan_interface/include/socketcan_interface/interface.h +++ b/socketcan_interface/include/socketcan_interface/interface.h @@ -8,6 +8,7 @@ #include #include "socketcan_interface/delegates.h" +#include "socketcan_interface/logging.h" #include "socketcan_interface/settings.h" namespace can{ @@ -174,17 +175,25 @@ class CommInterface{ using CommInterfaceSharedPtr = std::shared_ptr; 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 @@ -218,6 +227,4 @@ using DriverInterfaceSharedPtr = std::shared_ptr; } // namespace can -#include "logging.h" - #endif diff --git a/socketcan_interface/include/socketcan_interface/settings.h b/socketcan_interface/include/socketcan_interface/settings.h index b2840e545..10d674d4a 100644 --- a/socketcan_interface/include/socketcan_interface/settings.h +++ b/socketcan_interface/include/socketcan_interface/settings.h @@ -3,6 +3,8 @@ #include #include +#include + #include namespace can { @@ -26,6 +28,31 @@ namespace can { private: virtual bool getRepr(const std::string &n, std::string & repr) const = 0; }; + using SettingsConstSharedPtr = std::shared_ptr; + using SettingsSharedPtr = std::shared_ptr; + + 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 settings_; + virtual bool getRepr(const std::string &n, std::string & repr) const { + std::map::const_iterator it = settings_.find(n); + if (it == settings_.cend()) return false; + repr = it->second; + return true; + } + public: + template void set(const std::string &n, const T& val) { + settings_[n] = boost::lexical_cast(val); + } + static std::shared_ptr create() { return std::shared_ptr(new SettingsMap); } + }; + } // can diff --git a/socketcan_interface/include/socketcan_interface/socketcan.h b/socketcan_interface/include/socketcan_interface/socketcan.h index 10ae56db3..5e5e02310 100644 --- a/socketcan_interface/include/socketcan_interface/socketcan.h +++ b/socketcan_interface/include/socketcan_interface/socketcan.h @@ -31,8 +31,7 @@ class SocketCANInterface : public AsioDriver class ThreadedInterface : public WrappedInte WrappedInterface::run(); } public: - virtual bool init(const std::string &device, bool loopback) { + [[deprecated("provide settings explicitly")]] virtual 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 + } + virtual 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(); } virtual void shutdown(){ WrappedInterface::shutdown(); diff --git a/socketcan_interface/src/candump.cpp b/socketcan_interface/src/candump.cpp index 551510c39..3df34cd0d 100644 --- a/socketcan_interface/src/candump.cpp +++ b/socketcan_interface/src/candump.cpp @@ -74,7 +74,7 @@ int main(int argc, char *argv[]){ FrameListenerConstSharedPtr frame_printer = g_driver->createMsgListener(print_frame); StateListenerConstSharedPtr error_printer = g_driver->createStateListener(print_state); - if(!g_driver->init(argv[1], false)){ + if(!g_driver->init(argv[1], false, can::NoSettings::create())){ print_state(g_driver->getState()); return 1; } From e13655297ba4092e737cbab3320eb6da6aab632b Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Mathias=20L=C3=BCdtke?= Date: Tue, 7 Jul 2020 19:16:51 +0200 Subject: [PATCH 4/6] implemented report-only and fatal errors for SocketCANInterface --- socketcan_interface/CMakeLists.txt | 8 + .../include/socketcan_interface/socketcan.h | 181 +++++++++++------- socketcan_interface/test/test_settings.cpp | 62 ++++++ 3 files changed, 179 insertions(+), 72 deletions(-) create mode 100644 socketcan_interface/test/test_settings.cpp diff --git a/socketcan_interface/CMakeLists.txt b/socketcan_interface/CMakeLists.txt index 02ab751bf..d891a2346 100644 --- a/socketcan_interface/CMakeLists.txt +++ b/socketcan_interface/CMakeLists.txt @@ -122,6 +122,14 @@ if(CATKIN_ENABLE_TESTING) ${catkin_LIBRARIES} ) + catkin_add_gtest(${PROJECT_NAME}-test_settings + test/test_settings.cpp + ) + target_link_libraries(${PROJECT_NAME}-test_settings + ${PROJECT_NAME}_string + ${catkin_LIBRARIES} + ) + catkin_add_gtest(${PROJECT_NAME}-test_string test/test_string.cpp ) diff --git a/socketcan_interface/include/socketcan_interface/socketcan.h b/socketcan_interface/include/socketcan_interface/socketcan.h index 5e5e02310..a0e82cb33 100644 --- a/socketcan_interface/include/socketcan_interface/socketcan.h +++ b/socketcan_interface/include/socketcan_interface/socketcan.h @@ -20,23 +20,124 @@ namespace can { + +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; +} + class SocketCANInterface : public AsioDriver { bool loopback_; int sc_; + can_err_mask_t error_mask_, fatal_error_mask_; public: SocketCANInterface() - : loopback_(false), sc_(-1) + : loopback_(false), sc_(-1), error_mask_(0), fatal_error_mask_(0) {} virtual bool doesLoopBack() const{ return loopback_; } - virtual bool init(const std::string &device, bool loopback) override { + + 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 init(device, loopback, SettingsConstSharedPtr()); + } + virtual bool init(const std::string &device, bool loopback, SettingsConstSharedPtr settings) override { + 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); + } + + virtual bool recover(){ + if(!getState().isReady()){ + shutdown(); + return init(device_, loopback_, error_mask_, fatal_error_mask_); + } + return getState().isReady(); + } + virtual bool translateError(unsigned int internal_error, std::string & str){ + + bool ret = false; + if(!internal_error){ + str = "OK"; + ret = true; + } + if( internal_error & CAN_ERR_TX_TIMEOUT){ + str += "TX timeout (by netdevice driver);"; + ret = true; + } + if( internal_error & CAN_ERR_LOSTARB){ + str += "lost arbitration;"; + ret = true; + } + if( internal_error & CAN_ERR_CRTL){ + str += "controller problems;"; + ret = true; + } + if( internal_error & CAN_ERR_PROT){ + str += "protocol violations;"; + ret = true; + } + if( internal_error & CAN_ERR_TRX){ + str += "transceiver status;"; + ret = true; + } + if( internal_error & CAN_ERR_BUSOFF){ + str += "bus off;"; + ret = true; + } + if( internal_error & CAN_ERR_RESTARTED){ + str += "controller restarted;"; + ret = true; + } + return ret; + } + int getInternalSocket() { + return sc_; + } +protected: + std::string device_; + can_frame frame_; + + 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){ @@ -53,20 +154,8 @@ class SocketCANInterface : public AsioDriver +#include + +// Bring in gtest +#include + +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)); +} + +// Run all the tests that were declared with TEST() +int main(int argc, char **argv){ +testing::InitGoogleTest(&argc, argv); +return RUN_ALL_TESTS(); +} From 6393786c33c6037dedbbfd812e799f61530fc7db Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Mathias=20L=C3=BCdtke?= Date: Tue, 7 Jul 2020 13:19:54 +0200 Subject: [PATCH 5/6] add support for recursive XmlRpcSettings lookups --- socketcan_interface/CMakeLists.txt | 5 ++++ .../socketcan_interface/xmlrpc_settings.h | 17 +++++++++-- socketcan_interface/package.xml | 1 + socketcan_interface/test/test_settings.cpp | 29 +++++++++++++++++++ 4 files changed, 49 insertions(+), 3 deletions(-) diff --git a/socketcan_interface/CMakeLists.txt b/socketcan_interface/CMakeLists.txt index d891a2346..a859dd32e 100644 --- a/socketcan_interface/CMakeLists.txt +++ b/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 @@ -125,9 +126,13 @@ if(CATKIN_ENABLE_TESTING) 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 diff --git a/socketcan_interface/include/socketcan_interface/xmlrpc_settings.h b/socketcan_interface/include/socketcan_interface/xmlrpc_settings.h index da7a196a9..ec4a2462c 100644 --- a/socketcan_interface/include/socketcan_interface/xmlrpc_settings.h +++ b/socketcan_interface/include/socketcan_interface/xmlrpc_settings.h @@ -1,5 +1,6 @@ #ifndef SOCKETCAN_INTERFACE_XMLRPC_SETTINGS_H #define SOCKETCAN_INTERFACE_XMLRPC_SETTINGS_H +#include #include #include "xmlrpcpp/XmlRpcValue.h" @@ -12,10 +13,20 @@ class XmlRpcSettings : public can::Settings { 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)){ + 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 << const_cast< XmlRpc::XmlRpcValue &>(value_)[n]; // does not write since already existing + sstr << (*value)[n]; repr = sstr.str(); return true; } diff --git a/socketcan_interface/package.xml b/socketcan_interface/package.xml index 7b066f9e6..970fb567e 100644 --- a/socketcan_interface/package.xml +++ b/socketcan_interface/package.xml @@ -21,6 +21,7 @@ linux-kernel-headers rosunit + xmlrpcpp diff --git a/socketcan_interface/test/test_settings.cpp b/socketcan_interface/test/test_settings.cpp index b1a7fc70a..1ebae0947 100644 --- a/socketcan_interface/test/test_settings.cpp +++ b/socketcan_interface/test/test_settings.cpp @@ -1,5 +1,6 @@ // Bring in my package's API, which is what I'm testing #include +#include #include // Bring in gtest @@ -55,6 +56,34 @@ TEST(SettingTest, socketcan_masks) 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("param", res)); + EXPECT_EQ(res, 1); + EXPECT_EQ(settings.get_optional("param", 0), 1); + EXPECT_FALSE(settings.get("param2", res)); + EXPECT_EQ(settings.get_optional("param2", 0), 0); + + EXPECT_TRUE(settings.get("segment/param", res)); + EXPECT_EQ(res, 2); + EXPECT_EQ(settings.get_optional("segment/param", 0), 2); + + EXPECT_FALSE(settings.get("segment/param2", res)); + EXPECT_EQ(settings.get_optional("segment/param2", 0), 0); + EXPECT_FALSE(settings.get("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); From 7551f043a70a9dadf8f59bc9e246211147ab0b82 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Mathias=20L=C3=BCdtke?= Date: Tue, 7 Jul 2020 19:22:18 +0200 Subject: [PATCH 6/6] pass settings from ROS node to SocketCANInterface --- canopen_chain_node/src/ros_chain.cpp | 3 ++- canopen_chain_node/src/sync_node.cpp | 3 ++- socketcan_bridge/src/socketcan_bridge_node.cpp | 4 +++- socketcan_bridge/src/socketcan_to_topic_node.cpp | 4 +++- socketcan_bridge/src/topic_to_socketcan_node.cpp | 4 +++- .../include/socketcan_interface/xmlrpc_settings.h | 5 +++++ 6 files changed, 18 insertions(+), 5 deletions(-) diff --git a/canopen_chain_node/src/ros_chain.cpp b/canopen_chain_node/src/ros_chain.cpp index 5414a6740..4ef18511b 100644 --- a/canopen_chain_node/src/ros_chain.cpp +++ b/canopen_chain_node/src/ros_chain.cpp @@ -1,5 +1,6 @@ #include +#include #include #include @@ -325,7 +326,7 @@ bool RosChain::setup_bus(){ return false; } - add(std::make_shared(interface_, can_device, loopback)); + add(std::make_shared(interface_, can_device, loopback, XmlRpcSettings::create(bus_nh))); return true; } diff --git a/canopen_chain_node/src/sync_node.cpp b/canopen_chain_node/src/sync_node.cpp index f6dbc158d..4aec70134 100644 --- a/canopen_chain_node/src/sync_node.cpp +++ b/canopen_chain_node/src/sync_node.cpp @@ -1,5 +1,6 @@ #include #include +#include #include #include #include @@ -86,7 +87,7 @@ int main(int argc, char** argv){ canopen::LayerStack stack("SyncNodeLayer"); - stack.add(std::make_shared(driver, can_device, false)); + stack.add(std::make_shared(driver, can_device, false, XmlRpcSettings::create(nh_priv, "bus"))); stack.add(sync); diagnostic_updater::Updater diag_updater(nh, nh_priv); diff --git a/socketcan_bridge/src/socketcan_bridge_node.cpp b/socketcan_bridge/src/socketcan_bridge_node.cpp index 71aedc805..c3709b4c3 100644 --- a/socketcan_bridge/src/socketcan_bridge_node.cpp +++ b/socketcan_bridge/src/socketcan_bridge_node.cpp @@ -29,6 +29,7 @@ #include #include #include +#include #include #include @@ -44,7 +45,8 @@ int main(int argc, char *argv[]) can::ThreadedSocketCANInterfaceSharedPtr driver = std::make_shared (); - 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; diff --git a/socketcan_bridge/src/socketcan_to_topic_node.cpp b/socketcan_bridge/src/socketcan_to_topic_node.cpp index 988c07080..2cfac50ac 100644 --- a/socketcan_bridge/src/socketcan_to_topic_node.cpp +++ b/socketcan_bridge/src/socketcan_to_topic_node.cpp @@ -29,6 +29,7 @@ #include #include #include +#include #include #include @@ -45,7 +46,8 @@ int main(int argc, char *argv[]) can::ThreadedSocketCANInterfaceSharedPtr driver = std::make_shared (); - 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; diff --git a/socketcan_bridge/src/topic_to_socketcan_node.cpp b/socketcan_bridge/src/topic_to_socketcan_node.cpp index ac87c53f2..3849f5605 100644 --- a/socketcan_bridge/src/topic_to_socketcan_node.cpp +++ b/socketcan_bridge/src/topic_to_socketcan_node.cpp @@ -29,6 +29,7 @@ #include #include #include +#include #include #include @@ -45,7 +46,8 @@ int main(int argc, char *argv[]) can::ThreadedSocketCANInterfaceSharedPtr driver = std::make_shared (); - 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; diff --git a/socketcan_interface/include/socketcan_interface/xmlrpc_settings.h b/socketcan_interface/include/socketcan_interface/xmlrpc_settings.h index ec4a2462c..f76e112c6 100644 --- a/socketcan_interface/include/socketcan_interface/xmlrpc_settings.h +++ b/socketcan_interface/include/socketcan_interface/xmlrpc_settings.h @@ -12,6 +12,11 @@ class XmlRpcSettings : public can::Settings { XmlRpcSettings() {} XmlRpcSettings(const XmlRpc::XmlRpcValue &v) : value_(v) {} XmlRpcSettings& operator=(const XmlRpc::XmlRpcValue &v) { value_ = v; return *this; } + template static can::SettingsConstSharedPtr create(T nh, const std::string &ns="/") { + std::shared_ptr 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_;