pass settings from ROS node to SocketCANInterface

ros-industrial · Jul 7, 2020 · 7551f04 · 7551f04
1 parent 6393786
commit 7551f04
Show file tree

Hide file tree

Showing 6 changed files with 18 additions and 5 deletions.
diff --git a/canopen_chain_node/src/ros_chain.cpp b/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;
 }

diff --git a/canopen_chain_node/src/sync_node.cpp b/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);

diff --git a/socketcan_bridge/src/socketcan_bridge_node.cpp b/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;

diff --git a/socketcan_bridge/src/socketcan_to_topic_node.cpp b/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;

diff --git a/socketcan_bridge/src/topic_to_socketcan_node.cpp b/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;

diff --git 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<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_;