git commit -m "first commit for v2"

Devices/Libraries/Ros/ros_canopen/canopen_chain_node/CHANGELOG.rst

@@ -0,0 +1,199 @@
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for package canopen_chain_node
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+0.8.5 (2020-09-22)
+------------------
+
+0.8.4 (2020-08-22)
+------------------
+* pass settings from ROS node to SocketCANInterface
+* Contributors: Mathias Lüdtke
+
+0.8.3 (2020-05-07)
+------------------
+* Bump CMake version to avoid CMP0048 warning
+  Signed-off-by: ahcorde <ahcorde@gmail.com>
+* Contributors: ahcorde
+
+0.8.2 (2019-11-04)
+------------------
+* rename to logWarning to fix build on Debian stretch
+* log the result of all services in RosChain
+* enable rosconsole_bridge bindings
+* switch to new logging macros
+* Contributors: Mathias Lüdtke
+
+0.8.1 (2019-07-14)
+------------------
+* Set C++ standard to c++14
+* implemented create\*ListenerM helpers
+* Replacing FastDelegate with std::function and std::bind.
+* Contributors: Alexander Gutenkunst, Harsh Deshpande, Joshua Whitley, Mathias Lüdtke
+
+0.8.0 (2018-07-11)
+------------------
+* migrated to std::function and std::bind
+* make sync_node return proper error codes
+* refactored PublishFunc
+* migrated to std::atomic
+* migrated to std pointers
+* removed deprecated types
+* Contributors: Mathias Lüdtke
+
+0.7.8 (2018-05-04)
+------------------
+* Revert "pull make_shared into namespaces"
+  This reverts commit 9b2cd05df76d223647ca81917d289ca6330cdee6.
+* Contributors: Mathias Lüdtke
+
+0.7.7 (2018-05-04)
+------------------
+* added types for all function objects
+* pull make_shared into namespaces
+* added types for all shared_ptrs
+* migrate to new classloader headers
+* address catkin_lint errors/warnings
+* removed IPC/SHM based sync masters
+* Contributors: Mathias Lüdtke
+
+0.7.6 (2017-08-30)
+------------------
+
+0.7.5 (2017-05-29)
+------------------
+* added reset_errors_before_recover option
+* Contributors: Mathias Lüdtke
+
+0.7.4 (2017-04-25)
+------------------
+
+0.7.3 (2017-04-25)
+------------------
+
+0.7.2 (2017-03-28)
+------------------
+
+0.7.1 (2017-03-20)
+------------------
+* refactored EMCY handling into separate layer
+* do not reset thread for recover
+* properly stop run thread if init failed
+* deprecation warning for SHM-based master implementations
+* implemented canopen_sync_node
+* wait only if sync is disabled
+* added object access services
+* implement level-based object logging
+* added node name lookup
+* Contributors: Mathias Lüdtke
+
+0.7.0 (2016-12-13)
+------------------
+
+0.6.5 (2016-12-10)
+------------------
+* protect MotorChain setup with RosChain lock
+* added include to <boost/scoped_ptr.hpp>; solving `#177 <https://github.com/ipa-mdl/ros_canopen/issues/177>`_
+* default to canopen::SimpleMaster::Allocator (`#71 <https://github.com/ipa-mdl/ros_canopen/issues/71>`_)
+* exit code for generic error should be 1, not -1
+* styled and sorted CMakeLists.txt
+  * removed boilerplate comments
+  * indention
+  * reviewed exported dependencies
+* styled and sorted package.xml
+* update package URLs
+* moved roslib include into source file
+* renamed chain_ros.h to ros_chain.h, fixes `#126 <https://github.com/ipa-mdl/ros_canopen/issues/126>`_
+* Use of catkin_EXPORTED_TARGETS
+  Install target for canopen_ros_chain
+* Splitted charn_ros.h into chain_ros.h and ros_chain.cpp
+* Revert "stop heartbeat after stack was shutdown"
+  This reverts commit de985b5e9664edbbcc4f743fff3e2a2391e1bf8f.
+* improve failure handling in init service callback
+* improved excetion handling in init and recover callback
+* Merge pull request `#109 <https://github.com/ipa-mdl/ros_canopen/issues/109>`_ from ipa-mdl/shutdown-crashes
+  Fix for pluginlib-related crashes on shutdown
+* catch std::exception instead of canopen::Exception (`#110 <https://github.com/ipa-mdl/ros_canopen/issues/110>`_)
+* call to detroy is not needed anymore
+* added GuardedClassLoader implementation
+* minor shutdown improvements
+* Contributors: Mathias Lüdtke, Michael Stoll, xaedes
+
+0.6.4 (2015-07-03)
+------------------
+
+0.6.3 (2015-06-30)
+------------------
+* added motor_layer settings
+* remove boost::posix_time::milliseconds from SyncProperties
+* removed support for silence_us since bus timing cannot be guaranteed
+* implemented plugin-based Master allocators, defaults to LocalMaster
+* set initialized to false explicitly if init failed
+* include for std_msgs::String was missing
+* Merge remote-tracking branch 'origin/std_trigger' into new_402
+  Conflicts:
+  canopen_chain_node/CMakeLists.txt
+  canopen_chain_node/include/canopen_chain_node/chain_ros.h
+* halt explicitly on shutdown
+* stop heartbeat after stack was shutdown
+* migrated to Timer instead of ros::Timer to send heartbeat even after ros was shutdown
+* run loop even if ros is shutdown
+* improved chain shutdown behaviour
+* fix for g++: proper message generation
+* Merge branch 'publisher' into muparser
+  Conflicts:
+  canopen_motor_node/src/control_node.cpp
+* added generic object publishers
+* migrated to std_srvs/Trigger
+* use atomic flag instead of thread pointer for synchronization
+* do not run diagnostics if chain was not initalized, output warning instead
+* Changes Layer Status to Warning during the service calls
+* refactored Layer mechanisms
+* heartbeat works now
+* check XmlRpcValue types in dcf_overlay
+* removed IPCLayer sync listener, loopback is disabled per default
+* added simple heartbeat timer
+* added sync silence feature
+* parse sync properties only if sync_ms is valid
+* require message strings for error indicators, added missing strings, added ROS logging in sync loop
+* skip "eds_pkg" if not provided
+* clear layer before plugin loader is deleted
+* implemented node list as struct
+* 'modules' was renamed to 'nodes'
+* removed chain name
+* added driver_plugin parameter for pluginlib look-up
+* implemented threading in CANLayer
+* removed bitrate, added loopback to DriverInterface::init
+* allow dcf_overlay in defaults as well
+* recursive merge of MergedXmlRpcStruct
+* added dcf_overlay parameter
+* Merge branch 'auto_scale' into indigo_dev
+  Conflicts:
+  canopen_chain_node/include/canopen_chain_node/chain_ros.h
+* Merge remote-tracking branch 'ipa320/indigo_dev' into indigo_dev
+  Conflicts:
+  canopen_chain_node/include/canopen_chain_node/chain_ros.h
+  canopen_motor_node/src/control_node.cpp
+* catch exceptions during master creation
+* removed MasterType form template
+* added master_type parameter
+* Merge branch 'indigo_dev' into merge
+  Conflicts:
+  canopen_chain_node/include/canopen_chain_node/chain_ros.h
+  canopen_master/include/canopen_master/canopen.h
+  canopen_master/include/canopen_master/layer.h
+  canopen_master/src/node.cpp
+  canopen_motor_node/CMakeLists.txt
+  canopen_motor_node/src/control_node.cpp
+* added MergedXmlRpcStruct as replacement for read_xmlrpc_or_praram
+* Contributors: Mathias Lüdtke, thiagodefreitas
+
+0.6.2 (2014-12-18)
+------------------
+
+0.6.1 (2014-12-15)
+------------------
+* remove ipa_* and IPA_* prefixes
+* added descriptions and authors
+* renamed ipa_canopen_chain_ros to canopen_chain_node
+* Contributors: Florian Weisshardt, Mathias Lüdtke

Devices/Libraries/Ros/ros_canopen/canopen_chain_node/CMakeLists.txt

@@ -0,0 +1,114 @@
+cmake_minimum_required(VERSION 3.0.2)
+project(canopen_chain_node)
+
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD_REQUIRED ON)
+  set(CMAKE_CXX_STANDARD 14)
+endif()
+
+find_package(catkin REQUIRED
+  COMPONENTS
+    canopen_master
+    diagnostic_updater
+    message_generation
+    pluginlib
+    rosconsole_bridge
+    roscpp
+    roslib
+    socketcan_interface
+    std_msgs
+    std_srvs
+)
+
+find_package(Boost REQUIRED
+  COMPONENTS
+    filesystem
+)
+
+add_service_files(DIRECTORY srv
+                  FILES
+                  GetObject.srv
+                  SetObject.srv)
+
+generate_messages(DEPENDENCIES)
+
+catkin_package(
+  INCLUDE_DIRS
+    include
+  LIBRARIES
+    canopen_ros_chain
+  CATKIN_DEPENDS
+    canopen_master
+    diagnostic_updater
+    message_runtime
+    pluginlib
+    rosconsole_bridge
+    roscpp
+    socketcan_interface
+    std_srvs
+  DEPENDS
+    Boost
+)
+
+include_directories(
+  include
+  ${Boost_INCLUDE_DIRS}
+  ${catkin_INCLUDE_DIRS}
+)
+
+# canopen_ros_chain
+add_library(canopen_ros_chain
+  src/ros_chain.cpp
+  src/rosconsole_bridge.cpp
+)
+target_link_libraries(canopen_ros_chain
+  ${Boost_LIBRARIES}
+  ${catkin_LIBRARIES}
+)
+add_dependencies(canopen_ros_chain
+  ${catkin_EXPORTED_TARGETS}
+  ${${PROJECT_NAME}_EXPORTED_TARGETS}
+)
+
+# canopen_chain_node
+add_executable(${PROJECT_NAME}
+  src/chain_node.cpp
+)
+target_link_libraries(${PROJECT_NAME}
+  canopen_ros_chain
+  ${Boost_LIBRARIES}
+  ${catkin_LIBRARIES}
+)
+
+# canopen_sync_node
+add_executable(canopen_sync_node
+  src/rosconsole_bridge.cpp
+  src/sync_node.cpp
+)
+target_link_libraries(canopen_sync_node
+  ${Boost_LIBRARIES}
+  ${catkin_LIBRARIES}
+)
+
+install(
+  TARGETS
+    ${PROJECT_NAME}
+    canopen_ros_chain
+    canopen_sync_node
+  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
+
+install(
+  DIRECTORY
+    include/${PROJECT_NAME}/
+  DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+  FILES_MATCHING PATTERN "*.h"
+)
+
+install(
+  DIRECTORY
+    launch
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+)

Devices/Libraries/Ros/ros_canopen/canopen_chain_node/include/canopen_chain_node/ros_chain.h

@@ -0,0 +1,215 @@
+#ifndef H_CANOPEN_ROS_CHAIN
+#define H_CANOPEN_ROS_CHAIN
+
+#include <memory>
+#include <canopen_master/canopen.h>
+#include <canopen_master/can_layer.h>
+#include <canopen_chain_node/GetObject.h>
+#include <canopen_chain_node/SetObject.h>
+#include <socketcan_interface/string.h>
+#include <ros/ros.h>
+#include <std_srvs/Trigger.h>
+#include <diagnostic_updater/diagnostic_updater.h>
+#include <boost/filesystem/path.hpp>
+#include <pluginlib/class_loader.hpp>
+
+namespace canopen{
+
+typedef std::function<void()> PublishFuncType;
+PublishFuncType createPublishFunc(ros::NodeHandle &nh,  const std::string &name, canopen::NodeSharedPtr node, const std::string &key, bool force);
+
+class MergedXmlRpcStruct : public XmlRpc::XmlRpcValue{
+    MergedXmlRpcStruct(const XmlRpc::XmlRpcValue& a) :XmlRpc::XmlRpcValue(a){ assertStruct(); }
+public:
+    MergedXmlRpcStruct(){ assertStruct(); }
+    MergedXmlRpcStruct(const XmlRpc::XmlRpcValue& a, const MergedXmlRpcStruct &b, bool recursive= true) :XmlRpc::XmlRpcValue(a){
+        assertStruct();
+
+        for(ValueStruct::const_iterator it = b._value.asStruct->begin(); it != b._value.asStruct->end(); ++it){
+            std::pair<XmlRpc::XmlRpcValue::iterator,bool> res =  _value.asStruct->insert(*it);
+
+            if(recursive && !res.second && res.first->second.getType() == XmlRpc::XmlRpcValue::TypeStruct && it->second.getType() == XmlRpc::XmlRpcValue::TypeStruct){
+                res.first->second = MergedXmlRpcStruct(res.first->second, it->second); // recursive struct merge with implicit cast
+            }
+        }
+
+
+    }
+};
+
+class Logger: public DiagGroup<canopen::Layer>{
+    const canopen::NodeSharedPtr node_;
+
+    std::vector<std::function< void (diagnostic_updater::DiagnosticStatusWrapper &)> > entries_;
+
+    static void log_entry(diagnostic_updater::DiagnosticStatusWrapper &stat, uint8_t level, const std::string &name, std::function<std::string()> getter){
+        if(stat.level >= level){
+            try{
+                stat.add(name, getter());
+            }catch(...){
+                stat.add(name, "<ERROR>");
+            }
+        }
+    }
+
+public:
+    Logger(canopen::NodeSharedPtr node):  node_(node) { add(node_); }
+
+    bool add(uint8_t level, const std::string &key, bool forced){
+        try{
+            ObjectDict::Key k(key);
+            const ObjectDict::EntryConstSharedPtr entry = node_->getStorage()->dict_->get(k);
+            std::string name = entry->desc.empty() ? key : entry->desc;
+            entries_.push_back(std::bind(log_entry, std::placeholders::_1, level, name, node_->getStorage()->getStringReader(k, !forced)));
+            return true;
+        }
+        catch(std::exception& e){
+            ROS_ERROR_STREAM(boost::diagnostic_information(e));
+            return false;
+        }
+    }
+
+    template<typename T> void add(const std::shared_ptr<T> &n){
+        DiagGroup::add(std::static_pointer_cast<canopen::Layer>(n));
+    }
+
+    virtual void log(diagnostic_updater::DiagnosticStatusWrapper &stat){
+        if(node_->getState() == canopen::Node::Unknown){
+            stat.summary(stat.WARN,"Not initialized");
+        }else{
+            LayerReport r;
+            diag(r);
+            if(r.bounded<LayerStatus::Unbounded>()){ // valid
+                stat.summary(r.get(), r.reason());
+                for(std::vector<std::pair<std::string, std::string> >::const_iterator it = r.values().begin(); it != r.values().end(); ++it){
+                    stat.add(it->first, it->second);
+                }
+                for(size_t i=0; i < entries_.size(); ++i) entries_[i](stat);
+            }
+        }
+    }
+    virtual ~Logger() {}
+};
+typedef std::shared_ptr<Logger> LoggerSharedPtr;
+
+class GuardedClassLoaderList {
+public:
+    typedef std::shared_ptr<pluginlib::ClassLoaderBase> ClassLoaderBaseSharedPtr;
+    static void addLoader(ClassLoaderBaseSharedPtr b){
+        guarded_loaders().push_back(b);
+    }
+   ~GuardedClassLoaderList(){
+       guarded_loaders().clear();
+   }
+private:
+   static std::vector< ClassLoaderBaseSharedPtr>& guarded_loaders(){
+       static std::vector<ClassLoaderBaseSharedPtr> loaders;
+       return loaders;
+   }
+};
+
+template<typename T> class GuardedClassLoader {
+    typedef pluginlib::ClassLoader<T> Loader;
+    std::shared_ptr<Loader> loader_;
+public:
+    typedef std::shared_ptr<T> ClassSharedPtr;
+    GuardedClassLoader(const std::string& package, const std::string& allocator_base_class)
+    : loader_(new Loader(package, allocator_base_class)) {
+        GuardedClassLoaderList::addLoader(loader_);
+    }
+    ClassSharedPtr createInstance(const std::string& lookup_name){
+        return loader_->createUniqueInstance(lookup_name);
+    }
+};
+
+template<typename T> class ClassAllocator : public GuardedClassLoader<typename T::Allocator> {
+public:
+    typedef std::shared_ptr<T> ClassSharedPtr;
+    ClassAllocator (const std::string& package, const std::string& allocator_base_class) : GuardedClassLoader<typename T::Allocator>(package, allocator_base_class) {}
+    template<typename T1> ClassSharedPtr allocateInstance(const std::string& lookup_name, const T1 & t1){
+        return this->createInstance(lookup_name)->allocate(t1);
+    }
+    template<typename T1, typename T2> ClassSharedPtr allocateInstance(const std::string& lookup_name, const T1 & t1, const T2 & t2){
+        return this->createInstance(lookup_name)->allocate(t1, t2);
+    }
+    template<typename T1, typename T2, typename T3> ClassSharedPtr allocateInstance(const std::string& lookup_name, const T1 & t1, const T2 & t2, const T3 & t3){
+        return this->createInstance(lookup_name)->allocate(t1, t2, t3);
+    }
+};
+class RosChain : GuardedClassLoaderList, public canopen::LayerStack {
+private:
+    GuardedClassLoader<can::DriverInterface> driver_loader_;
+    ClassAllocator<canopen::Master> master_allocator_;
+    bool setup_node(const XmlRpc::XmlRpcValue &params, const std::string& name, const MergedXmlRpcStruct &defaults);
+protected:
+    can::DriverInterfaceSharedPtr interface_;
+    MasterSharedPtr master_;
+    std::shared_ptr<canopen::LayerGroupNoDiag<canopen::Node> > nodes_;
+    std::shared_ptr<canopen::LayerGroupNoDiag<canopen::EMCYHandler> > emcy_handlers_;
+    std::map<std::string, canopen::NodeSharedPtr > nodes_lookup_;
+    canopen::SyncLayerSharedPtr sync_;
+    std::vector<LoggerSharedPtr > loggers_;
+    std::vector<PublishFuncType> publishers_;
+
+    can::StateListenerConstSharedPtr state_listener_;
+
+    std::unique_ptr<boost::thread> thread_;
+
+    ros::NodeHandle nh_;
+    ros::NodeHandle nh_priv_;
+
+    diagnostic_updater::Updater diag_updater_;
+    ros::Timer diag_timer_;
+
+    boost::mutex mutex_;
+    ros::ServiceServer srv_init_;
+    ros::ServiceServer srv_recover_;
+    ros::ServiceServer srv_halt_;
+    ros::ServiceServer srv_shutdown_;
+    ros::ServiceServer srv_get_object_;
+    ros::ServiceServer srv_set_object_;
+
+    time_duration update_duration_;
+
+    struct HeartbeatSender{
+      can::Frame frame;
+      can::DriverInterfaceSharedPtr interface;
+      bool send(){
+          return interface && interface->send(frame);
+      }
+    } hb_sender_;
+    Timer heartbeat_timer_;
+
+    std::atomic<bool> running_;
+    boost::mutex diag_mutex_;
+
+    bool reset_errors_before_recover_;
+
+    void logState(const can::State &s);
+    void run();
+    virtual bool handle_init(std_srvs::Trigger::Request  &req, std_srvs::Trigger::Response &res);
+    virtual bool handle_recover(std_srvs::Trigger::Request  &req, std_srvs::Trigger::Response &res);
+    virtual void handleWrite(LayerStatus &status, const LayerState &current_state);
+    virtual void handleShutdown(LayerStatus &status);
+    virtual bool handle_shutdown(std_srvs::Trigger::Request  &req, std_srvs::Trigger::Response &res);
+    virtual bool handle_halt(std_srvs::Trigger::Request  &req, std_srvs::Trigger::Response &res);
+
+    bool handle_get_object(canopen_chain_node::GetObject::Request  &req, canopen_chain_node::GetObject::Response &res);
+    bool handle_set_object(canopen_chain_node::SetObject::Request  &req, canopen_chain_node::SetObject::Response &res);
+
+    bool setup_bus();
+    bool setup_sync();
+    bool setup_heartbeat();
+    bool setup_nodes();
+    virtual bool nodeAdded(XmlRpc::XmlRpcValue &params, const canopen::NodeSharedPtr &node, const LoggerSharedPtr &logger);
+    void report_diagnostics(diagnostic_updater::DiagnosticStatusWrapper &stat);
+    virtual bool setup_chain();
+public:
+    RosChain(const ros::NodeHandle &nh, const ros::NodeHandle &nh_priv);
+    bool setup();
+    virtual ~RosChain();
+};
+
+} //namespace canopen
+
+#endif

Devices/Libraries/Ros/ros_canopen/canopen_chain_node/launch/chain.launch

@@ -0,0 +1,6 @@
+<launch>
+  <arg name="yaml"/>
+  <node name="canopen_chain" pkg="canopen_chain_node" type="canopen_chain_node" output="screen" clear_params="true">
+    <rosparam command="load" file="$(arg yaml)" />
+  </node>
+</launch>

Devices/Libraries/Ros/ros_canopen/canopen_chain_node/package.xml

@@ -0,0 +1,36 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>canopen_chain_node</name>
+  <version>0.8.5</version>
+  <description>Base implementation for CANopen chains node with support for management services and diagnostics</description>
+
+  <maintainer email="mathias.luedtke@ipa.fraunhofer.de">Mathias Lüdtke</maintainer>
+  <author email="mathias.luedtke@ipa.fraunhofer.de">Mathias Lüdtke</author>
+
+  <license>LGPLv3</license>
+
+  <url type="website">http://wiki.ros.org/canopen_chain_node</url>
+  <url type="repository">https://github.com/ros-industrial/ros_canopen</url>
+  <url type="bugtracker">https://github.com/ros-industrial/ros_canopen/issues</url>
+
+  <buildtool_depend>catkin</buildtool_depend>
+
+  <build_depend>message_generation</build_depend>
+  <build_export_depend>message_runtime</build_export_depend>
+  <exec_depend>message_runtime</exec_depend>
+
+  <depend>libboost-dev</depend>
+  <depend>libboost-filesystem-dev</depend>
+  <depend>canopen_master</depend>
+  <depend>diagnostic_updater</depend>
+  <depend>pluginlib</depend>
+  <depend>rosconsole_bridge</depend>
+  <depend>roscpp</depend>
+  <depend>roslib</depend>
+  <depend>socketcan_interface</depend> <!-- direct dependency needed for pluginlib look-up -->
+  <depend>std_srvs</depend>
+
+  <build_depend>std_msgs</build_depend>
+  <exec_depend>std_msgs</exec_depend>
+
+</package>

Devices/Libraries/Ros/ros_canopen/canopen_chain_node/src/chain_node.cpp

@@ -0,0 +1,21 @@
+#include <socketcan_interface/dispatcher.h>
+#include <socketcan_interface/socketcan.h>
+#include <canopen_chain_node/ros_chain.h>
+
+using namespace can;
+using namespace canopen;
+
+int main(int argc, char** argv){
+  ros::init(argc, argv, "canopen_chain_node_node");
+  ros::NodeHandle nh;
+  ros::NodeHandle nh_priv("~");
+  
+  RosChain chain(nh, nh_priv);
+  
+  if(!chain.setup()){
+      return 1;
+  }
+  
+  ros::spin();
+  return 0;
+}

Devices/Libraries/Ros/ros_canopen/canopen_chain_node/src/ros_chain.cpp

@@ -0,0 +1,644 @@
+#include <ros/package.h>
+
+#include <socketcan_interface/xmlrpc_settings.h>
+#include <canopen_chain_node/ros_chain.h>
+
+#include <std_msgs/Int8.h>
+#include <std_msgs/Int16.h>
+#include <std_msgs/Int32.h>
+#include <std_msgs/Int64.h>
+#include <std_msgs/UInt8.h>
+#include <std_msgs/UInt16.h>
+#include <std_msgs/UInt32.h>
+#include <std_msgs/UInt64.h>
+#include <std_msgs/Float32.h>
+#include <std_msgs/Float64.h>
+#include <std_msgs/String.h>
+
+namespace canopen {
+
+template<typename Tpub, int dt>
+static PublishFuncType create(ros::NodeHandle &nh,  const std::string &name, ObjectStorageSharedPtr storage, const std::string &key, const bool force){
+    using data_type = typename ObjectStorage::DataType<dt>::type;
+    using entry_type = ObjectStorage::Entry<data_type>;
+
+    entry_type entry = storage->entry<data_type>(key);
+    if(!entry.valid()) return 0;
+
+    const ros::Publisher pub = nh.advertise<Tpub>(name, 1);
+
+    typedef const data_type(entry_type::*getter_type)(void);
+    const getter_type getter = force ? static_cast<getter_type>(&entry_type::get) : static_cast<getter_type>(&entry_type::get_cached);
+
+    return [force, pub, entry, getter] () mutable {
+        Tpub msg;
+        msg.data = (const typename Tpub::_data_type &) (entry.*getter)();
+        pub.publish(msg);
+    };
+}
+
+PublishFuncType createPublishFunc(ros::NodeHandle &nh,  const std::string &name, canopen::NodeSharedPtr node, const std::string &key, bool force){
+    ObjectStorageSharedPtr s = node->getStorage();
+
+    switch(ObjectDict::DataTypes(s->dict_->get(key)->data_type)){
+        case ObjectDict::DEFTYPE_INTEGER8:       return create< std_msgs::Int8,    ObjectDict::DEFTYPE_INTEGER8       >(nh, name, s, key, force);
+        case ObjectDict::DEFTYPE_INTEGER16:      return create< std_msgs::Int16,   ObjectDict::DEFTYPE_INTEGER16      >(nh, name, s, key, force);
+        case ObjectDict::DEFTYPE_INTEGER32:      return create< std_msgs::Int32,   ObjectDict::DEFTYPE_INTEGER32      >(nh, name, s, key, force);
+        case ObjectDict::DEFTYPE_INTEGER64:      return create< std_msgs::Int64,   ObjectDict::DEFTYPE_INTEGER64      >(nh, name, s, key, force);
+
+        case ObjectDict::DEFTYPE_UNSIGNED8:      return create< std_msgs::UInt8,   ObjectDict::DEFTYPE_UNSIGNED8      >(nh, name, s, key, force);
+        case ObjectDict::DEFTYPE_UNSIGNED16:     return create< std_msgs::UInt16,  ObjectDict::DEFTYPE_UNSIGNED16     >(nh, name, s, key, force);
+        case ObjectDict::DEFTYPE_UNSIGNED32:     return create< std_msgs::UInt32,  ObjectDict::DEFTYPE_UNSIGNED32     >(nh, name, s, key, force);
+        case ObjectDict::DEFTYPE_UNSIGNED64:     return create< std_msgs::UInt64,  ObjectDict::DEFTYPE_UNSIGNED64     >(nh, name, s, key, force);
+
+        case ObjectDict::DEFTYPE_REAL32:         return create< std_msgs::Float32, ObjectDict::DEFTYPE_REAL32         >(nh, name, s, key, force);
+        case ObjectDict::DEFTYPE_REAL64:         return create< std_msgs::Float64, ObjectDict::DEFTYPE_REAL64         >(nh, name, s, key, force);
+
+        case ObjectDict::DEFTYPE_VISIBLE_STRING: return create< std_msgs::String,  ObjectDict::DEFTYPE_VISIBLE_STRING >(nh, name, s, key, force);
+        case ObjectDict::DEFTYPE_OCTET_STRING:   return create< std_msgs::String,  ObjectDict::DEFTYPE_DOMAIN         >(nh, name, s, key, force);
+        case ObjectDict::DEFTYPE_UNICODE_STRING: return create< std_msgs::String,  ObjectDict::DEFTYPE_UNICODE_STRING >(nh, name, s, key, force);
+        case ObjectDict::DEFTYPE_DOMAIN:         return create< std_msgs::String,  ObjectDict::DEFTYPE_DOMAIN         >(nh, name, s, key, force);
+
+        default: return 0;
+    }
+}
+
+void RosChain::logState(const can::State &s){
+    can::DriverInterfaceSharedPtr interface = interface_;
+    std::string msg;
+    if(interface && !interface->translateError(s.internal_error, msg)) msg  =  "Undefined"; ;
+    ROS_INFO_STREAM("Current state: " << s.driver_state << " device error: " << s.error_code << " internal_error: " << s.internal_error << " (" << msg << ")");
+}
+
+void RosChain::run(){
+    running_ = true;
+    time_point abs_time = boost::chrono::high_resolution_clock::now();
+    while(running_){
+        LayerStatus s;
+        try{
+            read(s);
+            write(s);
+            if(!s.bounded<LayerStatus::Warn>()) ROS_ERROR_STREAM_THROTTLE(10, s.reason());
+            else if(!s.bounded<LayerStatus::Ok>()) ROS_WARN_STREAM_THROTTLE(10, s.reason());
+        }
+        catch(const canopen::Exception& e){
+            ROS_ERROR_STREAM_THROTTLE(1, boost::diagnostic_information(e));
+        }
+        if(!sync_){
+            abs_time += update_duration_;
+            boost::this_thread::sleep_until(abs_time);
+        }
+    }
+}
+
+template <typename T> class ResponseLogger {
+protected:
+    bool logged;
+    const T& res;
+    const std::string command;
+public:
+    ResponseLogger(const T& res, const std::string &command) : logged(false), res(res), command(command){}
+    ~ResponseLogger() {
+        if(!logged && !res.success){
+            if (res.message.empty()){
+                ROS_ERROR_STREAM(command << " failed");
+            }else{
+                ROS_ERROR_STREAM(command << " failed: " << res.message);
+            }
+            logged = true;
+        }
+    }
+};
+
+class TriggerResponseLogger: public ResponseLogger<std_srvs::Trigger::Response> {
+public:
+    TriggerResponseLogger(const std_srvs::Trigger::Response& res, const std::string &command) : ResponseLogger(res, command){
+        ROS_INFO_STREAM(command << "...");
+    }
+    void logWarning() {
+        ROS_WARN_STREAM(command << " successful with warning(s): " << res.message);
+        logged = true;
+    }
+    ~TriggerResponseLogger() {
+        if(!logged && res.success){
+            if (res.message.empty()){
+                ROS_INFO_STREAM(command << " successful");
+            }else{
+                ROS_INFO_STREAM(command << " successful: " << res.message);
+            }
+            logged = true;
+        }
+    }
+};
+
+bool RosChain::handle_init(std_srvs::Trigger::Request  &req, std_srvs::Trigger::Response &res){
+    TriggerResponseLogger rl(res, "Initializing");
+    boost::mutex::scoped_lock lock(mutex_);
+    if(getLayerState() > Off){
+        res.success = true;
+        res.message = "already initialized";
+        return true;
+    }
+    thread_.reset(new boost::thread(&RosChain::run, this));
+    LayerReport status;
+    try{
+        init(status);
+        res.success = status.bounded<LayerStatus::Ok>();
+        res.message = status.reason();
+        if(!status.bounded<LayerStatus::Warn>()){
+            diag(status);
+            res.message = status.reason();
+        }else{
+            heartbeat_timer_.restart();
+            return true;
+        }
+    }
+    catch( const std::exception &e){
+        std::string info = boost::diagnostic_information(e);
+        ROS_ERROR_STREAM(info);
+        res.message = info;
+        status.error(res.message);
+    }
+    catch(...){
+        res.message = "Unknown exception";
+        status.error(res.message);
+    }
+
+    res.success = false;
+    shutdown(status);
+    return true;
+}
+bool RosChain::handle_recover(std_srvs::Trigger::Request  &req, std_srvs::Trigger::Response &res){
+    TriggerResponseLogger rl(res, "Recovering");
+    boost::mutex::scoped_lock lock(mutex_);
+    res.success = false;
+
+    if(getLayerState() > Init){
+        LayerReport status;
+        try{
+            if(!reset_errors_before_recover_ || emcy_handlers_->callFunc<LayerStatus::Warn>(&EMCYHandler::resetErrors, status)){
+                recover(status);
+            }
+            if(!status.bounded<LayerStatus::Warn>()){
+                diag(status);
+            }
+            res.success = status.bounded<LayerStatus::Warn>();
+            res.message = status.reason();
+            if(status.equals<LayerStatus::Warn>()) rl.logWarning();
+        }
+        catch( const std::exception &e){
+            std::string info = boost::diagnostic_information(e);
+            ROS_ERROR_STREAM(info);
+            res.message = info;
+        }
+        catch(...){
+            res.message = "Unknown exception";
+        }
+    }else{
+        res.message = "not running";
+    }
+    return true;
+}
+
+void RosChain::handleWrite(LayerStatus &status, const LayerState &current_state) {
+    LayerStack::handleWrite(status, current_state);
+    if(current_state > Shutdown){
+        for(const PublishFuncType& func: publishers_) func();
+    }
+}
+
+void RosChain::handleShutdown(LayerStatus &status){
+    boost::mutex::scoped_lock lock(diag_mutex_);
+    heartbeat_timer_.stop();
+    LayerStack::handleShutdown(status);
+    if(running_){
+        running_ = false;
+        thread_->interrupt();
+        thread_->join();
+        thread_.reset();
+    }
+}
+
+bool RosChain::handle_shutdown(std_srvs::Trigger::Request  &req, std_srvs::Trigger::Response &res){
+    TriggerResponseLogger rl(res, "Shutting down");
+    boost::mutex::scoped_lock lock(mutex_);
+    res.success = true;
+    if(getLayerState() > Init){
+        LayerStatus s;
+        halt(s);
+        shutdown(s);
+    }else{
+        res.message = "not running";
+    }
+    return true;
+}
+
+bool RosChain::handle_halt(std_srvs::Trigger::Request  &req, std_srvs::Trigger::Response &res){
+    TriggerResponseLogger rl(res, "Halting down");
+    boost::mutex::scoped_lock lock(mutex_);
+     res.success = true;
+     if(getLayerState() > Init){
+        LayerStatus s;
+        halt(s);
+    }else{
+        res.message = "not running";
+    }
+    return true;
+}
+
+bool RosChain::handle_get_object(canopen_chain_node::GetObject::Request  &req, canopen_chain_node::GetObject::Response &res){
+    ResponseLogger<canopen_chain_node::GetObject::Response> rl(res, "Getting object " + req.node);
+    std::map<std::string, canopen::NodeSharedPtr >::iterator it = nodes_lookup_.find(req.node);
+    if(it == nodes_lookup_.end()){
+        res.message = "node not found";
+    }else{
+        try {
+            res.value = it->second->getStorage()->getStringReader(canopen::ObjectDict::Key(req.object), req.cached)();
+            res.success = true;
+        } catch(std::exception& e) {
+            res.message = boost::diagnostic_information(e);
+        }
+    }
+    return true;
+}
+
+bool RosChain::handle_set_object(canopen_chain_node::SetObject::Request  &req, canopen_chain_node::SetObject::Response &res){
+    ResponseLogger<canopen_chain_node::SetObject::Response> rl(res, "Setting object " + req.node);
+    std::map<std::string, canopen::NodeSharedPtr >::iterator it = nodes_lookup_.find(req.node);
+    if(it == nodes_lookup_.end()){
+        res.message = "node not found";
+    }else{
+        try {
+            it->second->getStorage()->getStringWriter(canopen::ObjectDict::Key(req.object), req.cached)(req.value);
+            res.success = true;
+        } catch(std::exception& e) {
+            res.message = boost::diagnostic_information(e);
+        }
+    }
+    return true;
+}
+
+bool RosChain::setup_bus(){
+    ros::NodeHandle bus_nh(nh_priv_,"bus");
+    std::string can_device;
+    std::string driver_plugin;
+    std::string master_alloc;
+    bool loopback;
+
+    if(!bus_nh.getParam("device",can_device)){
+        ROS_ERROR("Device not set");
+        return false;
+    }
+
+    bus_nh.param("loopback",loopback, false);
+
+    bus_nh.param("driver_plugin",driver_plugin, std::string("can::SocketCANInterface"));
+
+    try{
+        interface_ =  driver_loader_.createInstance(driver_plugin);
+    }
+
+    catch(pluginlib::PluginlibException& ex){
+        ROS_ERROR_STREAM(ex.what());
+        return false;
+    }
+
+    state_listener_ = interface_->createStateListenerM(this, &RosChain::logState);
+
+    if(bus_nh.getParam("master_type",master_alloc)){
+        ROS_ERROR("please migrate to master allocators");
+        return false;
+    }
+
+    bus_nh.param("master_allocator",master_alloc, std::string("canopen::SimpleMaster::Allocator"));
+
+    try{
+        master_= master_allocator_.allocateInstance(master_alloc, can_device, interface_);
+    }
+    catch( const std::exception &e){
+        std::string info = boost::diagnostic_information(e);
+        ROS_ERROR_STREAM(info);
+        return false;
+    }
+
+    if(!master_){
+        ROS_ERROR_STREAM("Could not allocate master.");
+        return false;
+    }
+
+    add(std::make_shared<CANLayer>(interface_, can_device, loopback, XmlRpcSettings::create(bus_nh)));
+
+    return true;
+}
+
+bool RosChain::setup_sync(){
+    ros::NodeHandle sync_nh(nh_priv_,"sync");
+
+    int sync_ms = 0;
+    int sync_overflow = 0;
+
+    if(!sync_nh.getParam("interval_ms", sync_ms)){
+        ROS_WARN("Sync interval was not specified, so sync is disabled per default");
+    }
+
+    if(sync_ms < 0){
+        ROS_ERROR_STREAM("Sync interval  "<< sync_ms << " is invalid");
+        return false;
+    }
+
+    int update_ms = sync_ms;
+    if(sync_ms == 0) nh_priv_.getParam("update_ms", update_ms);
+    if(update_ms == 0){
+        ROS_ERROR_STREAM("Update interval  "<< sync_ms << " is invalid");
+        return false;
+    }else{
+        update_duration_ = boost::chrono::milliseconds(update_ms);
+    }
+
+    if(sync_ms){
+        if(!sync_nh.getParam("overflow", sync_overflow)){
+            ROS_WARN("Sync overflow was not specified, so overflow is disabled per default");
+        }
+        if(sync_overflow == 1 || sync_overflow > 240){
+            ROS_ERROR_STREAM("Sync overflow  "<< sync_overflow << " is invalid");
+            return false;
+        }
+        if(sync_nh.param("silence_us", 0) != 0){
+            ROS_WARN("silence_us is not supported anymore");
+        }
+
+        // TODO: parse header
+        sync_ = master_->getSync(SyncProperties(can::MsgHeader(0x80), sync_ms, sync_overflow));
+
+        if(!sync_ && sync_ms){
+            ROS_ERROR_STREAM("Initializing sync master failed");
+            return false;
+        }
+        add(sync_);
+    }
+    return true;
+}
+
+bool RosChain::setup_heartbeat(){
+        ros::NodeHandle hb_nh(nh_priv_,"heartbeat");
+        std::string msg;
+        double rate = 0;
+
+        bool got_any = hb_nh.getParam("msg", msg);
+        got_any = hb_nh.getParam("rate", rate) || got_any;
+
+        if( !got_any) return true; // nothing todo
+
+        if(rate <=0 ){
+            ROS_ERROR_STREAM("Rate '"<< rate << "' is invalid");
+            return false;
+        }
+
+        hb_sender_.frame = can::toframe(msg);
+
+
+        if(!hb_sender_.frame.isValid()){
+            ROS_ERROR_STREAM("Message '"<< msg << "' is invalid");
+            return false;
+        }
+
+        hb_sender_.interface = interface_;
+
+        heartbeat_timer_.start(std::bind(&HeartbeatSender::send, &hb_sender_), boost::chrono::duration<double>(1.0/rate), false);
+
+        return true;
+
+
+}
+std::pair<std::string, bool> parseObjectName(std::string obj_name){
+    size_t pos = obj_name.find('!');
+    bool force = pos != std::string::npos;
+    if(force) obj_name.erase(pos);
+    return std::make_pair(obj_name, force);
+}
+
+bool addLoggerEntries(XmlRpc::XmlRpcValue merged, const std::string param, uint8_t level, Logger &logger){
+    if(merged.hasMember(param)){
+        try{
+            XmlRpc::XmlRpcValue objs = merged[param];
+            for(int i = 0; i < objs.size(); ++i){
+                std::pair<std::string, bool> obj_name = parseObjectName(objs[i]);
+
+                if(!logger.add(level, obj_name.first, obj_name.second)){
+                    ROS_ERROR_STREAM("Could not create logger for '" << obj_name.first << "'");
+                    return false;
+                }
+            }
+        }
+        catch(...){
+            ROS_ERROR_STREAM("Could not parse " << param << " parameter");
+            return false;
+        }
+    }
+    return true;
+}
+
+bool RosChain::setup_nodes(){
+    nodes_.reset(new canopen::LayerGroupNoDiag<canopen::Node>("301 layer"));
+    add(nodes_);
+
+    emcy_handlers_.reset(new canopen::LayerGroupNoDiag<canopen::EMCYHandler>("EMCY layer"));
+
+    XmlRpc::XmlRpcValue nodes;
+    if(!nh_priv_.getParam("nodes", nodes)){
+        ROS_WARN("falling back to 'modules', please switch to 'nodes'");
+        nh_priv_.getParam("modules", nodes);
+    }
+    MergedXmlRpcStruct defaults;
+    nh_priv_.getParam("defaults", defaults);
+
+    if(nodes.getType() ==  XmlRpc::XmlRpcValue::TypeArray){
+        for(size_t i = 0; i < nodes.size(); ++i){
+            if(nodes[i].hasMember("name")){
+                if(!setup_node(nodes[i], nodes[i]["name"], defaults)) return false;
+            }else{
+                ROS_ERROR_STREAM("Node at list index " << i << " has no name");
+                return false;
+            }
+        }
+    }else{
+        for(XmlRpc::XmlRpcValue::iterator it = nodes.begin(); it != nodes.end(); ++it){
+            if(!setup_node(it->second, it->first, defaults)) return false;
+        }
+    }
+    return true;
+}
+
+bool RosChain::setup_node(const XmlRpc::XmlRpcValue& params, const std::string &name, const MergedXmlRpcStruct &defaults){
+    int node_id;
+    try{
+        node_id = params["id"];
+    }
+    catch(...){
+        ROS_ERROR_STREAM("Node '" << name  << "' has no id");
+        return false;
+    }
+    MergedXmlRpcStruct merged(params, defaults);
+
+    if(!merged.hasMember("name")){
+        merged["name"]=name;
+    }
+
+    ObjectDict::Overlay overlay;
+    if(merged.hasMember("dcf_overlay")){
+        XmlRpc::XmlRpcValue dcf_overlay = merged["dcf_overlay"];
+        if(dcf_overlay.getType() != XmlRpc::XmlRpcValue::TypeStruct){
+            ROS_ERROR_STREAM("dcf_overlay is no struct");
+            return false;
+        }
+        for(XmlRpc::XmlRpcValue::iterator ito = dcf_overlay.begin(); ito!= dcf_overlay.end(); ++ito){
+            if(ito->second.getType() != XmlRpc::XmlRpcValue::TypeString){
+                ROS_ERROR_STREAM("dcf_overlay '" << ito->first << "' must be string");
+                return false;
+            }
+            overlay.push_back(ObjectDict::Overlay::value_type(ito->first, ito->second));
+        }
+    }
+
+    std::string eds;
+
+    try{
+        eds = (std::string) merged["eds_file"];
+    }
+    catch(...){
+        ROS_ERROR_STREAM("EDS path '" << eds << "' invalid");
+        return false;
+    }
+
+    try{
+        if(merged.hasMember("eds_pkg")){
+            std::string pkg = merged["eds_pkg"];
+            std::string p = ros::package::getPath(pkg);
+            if(p.empty()){
+                    ROS_WARN_STREAM("Package '" << pkg << "' was not found");
+            }else{
+                eds = (boost::filesystem::path(p)/eds).make_preferred().native();;
+            }
+        }
+    }
+    catch(...){
+    }
+
+    ObjectDictSharedPtr  dict = ObjectDict::fromFile(eds, overlay);
+    if(!dict){
+        ROS_ERROR_STREAM("EDS '" << eds << "' could not be parsed");
+        return false;
+    }
+    canopen::NodeSharedPtr node = std::make_shared<canopen::Node>(interface_, dict, node_id, sync_);
+
+    LoggerSharedPtr logger = std::make_shared<Logger>(node);
+
+    if(!nodeAdded(merged, node, logger)) return false;
+
+    if(!addLoggerEntries(merged, "log", diagnostic_updater::DiagnosticStatusWrapper::OK, *logger)) return false;
+    if(!addLoggerEntries(merged, "log_warn", diagnostic_updater::DiagnosticStatusWrapper::WARN, *logger)) return false;
+    if(!addLoggerEntries(merged, "log_error", diagnostic_updater::DiagnosticStatusWrapper::ERROR, *logger)) return false;
+
+    loggers_.push_back(logger);
+    diag_updater_.add(name, std::bind(&Logger::log, logger, std::placeholders::_1));
+
+    std::string node_name = std::string(merged["name"]);
+
+    if(merged.hasMember("publish")){
+        try{
+            XmlRpc::XmlRpcValue objs = merged["publish"];
+            for(int i = 0; i < objs.size(); ++i){
+                std::pair<std::string, bool> obj_name = parseObjectName(objs[i]);
+
+                PublishFuncType pub = createPublishFunc(nh_, node_name +"_"+obj_name.first, node, obj_name.first, obj_name.second);
+                if(!pub){
+                    ROS_ERROR_STREAM("Could not create publisher for '" << obj_name.first << "'");
+                    return false;
+                }
+                publishers_.push_back(pub);
+            }
+        }
+        catch(...){
+            ROS_ERROR("Could not parse publish parameter");
+            return false;
+        }
+    }
+    nodes_->add(node);
+    nodes_lookup_.insert(std::make_pair(node_name, node));
+
+    std::shared_ptr<canopen::EMCYHandler> emcy = std::make_shared<canopen::EMCYHandler>(interface_, node->getStorage());
+    emcy_handlers_->add(emcy);
+    logger->add(emcy);
+
+    return true;
+}
+
+bool RosChain::nodeAdded(XmlRpc::XmlRpcValue &params, const canopen::NodeSharedPtr &node, const LoggerSharedPtr &logger){
+    return true;
+}
+
+void RosChain::report_diagnostics(diagnostic_updater::DiagnosticStatusWrapper &stat){
+    boost::mutex::scoped_lock lock(diag_mutex_);
+    LayerReport r;
+    if(getLayerState() == Off){
+        stat.summary(stat.WARN,"Not initialized");
+    }else if(!running_){
+        stat.summary(stat.ERROR,"Thread is not running");
+    }else{
+        diag(r);
+        if(r.bounded<LayerStatus::Unbounded>()){ // valid
+            stat.summary(r.get(), r.reason());
+            for(std::vector<std::pair<std::string, std::string> >::const_iterator it = r.values().begin(); it != r.values().end(); ++it){
+                stat.add(it->first, it->second);
+            }
+        }
+    }
+}
+
+RosChain::RosChain(const ros::NodeHandle &nh, const ros::NodeHandle &nh_priv)
+: LayerStack("ROS stack"),driver_loader_("socketcan_interface", "can::DriverInterface"),
+  master_allocator_("canopen_master", "canopen::Master::Allocator"),
+  nh_(nh), nh_priv_(nh_priv),
+  diag_updater_(nh_,nh_priv_),
+  running_(false),
+  reset_errors_before_recover_(false){}
+
+bool RosChain::setup(){
+    boost::mutex::scoped_lock lock(mutex_);
+    bool okay = setup_chain();
+    if(okay) add(emcy_handlers_);
+    return okay;
+}
+
+bool RosChain::setup_chain(){
+    std::string hw_id;
+    nh_priv_.param("hardware_id", hw_id, std::string("none"));
+    nh_priv_.param("reset_errors_before_recover", reset_errors_before_recover_, false);
+
+    diag_updater_.setHardwareID(hw_id);
+    diag_updater_.add("chain", this, &RosChain::report_diagnostics);
+
+    diag_timer_ = nh_.createTimer(ros::Duration(diag_updater_.getPeriod()/2.0),std::bind(&diagnostic_updater::Updater::update, &diag_updater_));
+
+    ros::NodeHandle nh_driver(nh_, "driver");
+
+    srv_init_ = nh_driver.advertiseService("init",&RosChain::handle_init, this);
+    srv_recover_ = nh_driver.advertiseService("recover",&RosChain::handle_recover, this);
+    srv_halt_ = nh_driver.advertiseService("halt",&RosChain::handle_halt, this);
+    srv_shutdown_ = nh_driver.advertiseService("shutdown",&RosChain::handle_shutdown, this);
+
+    srv_get_object_ = nh_driver.advertiseService("get_object",&RosChain::handle_get_object, this);
+    srv_set_object_ = nh_driver.advertiseService("set_object",&RosChain::handle_set_object, this);
+
+    return setup_bus() && setup_sync() && setup_heartbeat() && setup_nodes();
+}
+
+RosChain::~RosChain(){
+    try{
+        LayerStatus s;
+        halt(s);
+        shutdown(s);
+    }catch(...){ ROS_ERROR("CATCH"); }
+}
+
+}

Devices/Libraries/Ros/ros_canopen/canopen_chain_node/src/rosconsole_bridge.cpp

@@ -0,0 +1,2 @@
+#include <rosconsole_bridge/bridge.h>
+REGISTER_ROSCONSOLE_BRIDGE;

Devices/Libraries/Ros/ros_canopen/canopen_chain_node/src/sync_node.cpp

@@ -0,0 +1,101 @@
+#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>
+
+template<typename T > std::string join(const T &container, const std::string &delim){
+    if(container.empty()) return std::string();
+    std::stringstream sstr;
+    typename T::const_iterator it = container.begin();
+    sstr << *it;
+    for(++it; it != container.end(); ++it){
+        sstr << delim << *it;
+    }
+    return sstr.str();
+}
+void report_diagnostics(canopen::LayerStack &sync, diagnostic_updater::DiagnosticStatusWrapper &stat){
+    canopen::LayerReport r;
+    sync.read(r);
+    sync.diag(r);
+    if(sync.getLayerState() !=canopen::Layer::Off  &&  r.bounded<canopen::LayerStatus::Unbounded>()){ // valid
+        stat.summary(r.get(), r.reason());
+        for(std::vector<std::pair<std::string, std::string> >::const_iterator it = r.values().begin(); it != r.values().end(); ++it){
+            stat.add(it->first, it->second);
+        }
+        if(!r.bounded<canopen::LayerStatus::Warn>()){
+            canopen::LayerStatus s;
+            sync.recover(s);
+        }
+    }else{
+        stat.summary(stat.WARN, "sync not initilized");
+        canopen::LayerStatus s;
+        sync.init(s);
+    }
+}
+
+int main(int argc, char** argv){
+    ros::init(argc, argv, "canopen_sync_node");
+
+    ros::NodeHandle nh;
+    ros::NodeHandle nh_priv("~");
+
+    ros::NodeHandle sync_nh(nh_priv, "sync");
+    int sync_ms;
+    if(!sync_nh.getParam("interval_ms", sync_ms) || sync_ms <=0){
+        ROS_ERROR_STREAM("Sync interval  "<< sync_ms << " is invalid");
+        return 1;
+    }
+
+    int sync_overflow = 0;
+    if(!sync_nh.getParam("overflow", sync_overflow)){
+        ROS_WARN("Sync overflow was not specified, so overflow is disabled per default");
+    }
+    if(sync_overflow == 1 || sync_overflow > 240){
+        ROS_ERROR_STREAM("Sync overflow  "<< sync_overflow << " is invalid");
+        return 1;
+    }
+
+
+    std::string can_device;
+    if(!nh_priv.getParam("bus/device",can_device)){
+        ROS_ERROR("Device not set");
+        return 1;
+    }
+
+    can::SocketCANDriverSharedPtr driver = std::make_shared<can::SocketCANDriver>();
+    canopen::SyncProperties sync_properties(can::MsgHeader(sync_nh.param("sync_id", 0x080)), sync_ms, sync_overflow);
+
+    std::shared_ptr<canopen::BCMsync> sync = std::make_shared<canopen::BCMsync>(can_device, driver, sync_properties);
+
+    std::vector<int> nodes;
+
+    if(sync_nh.getParam("monitored_nodes",nodes)){
+        sync->setMonitored(nodes);
+    }else{
+        std::string msg;
+        if(nh_priv.getParam("heartbeat/msg", msg)){
+            can::Frame f = can::toframe(msg);
+            if(f.isValid() && (f.id & ~canopen::BCMsync::ALL_NODES_MASK) == canopen::BCMsync::HEARTBEAT_ID){
+                nodes.push_back(f.id & canopen::BCMsync::ALL_NODES_MASK);
+            }
+        }
+        sync_nh.getParam("ignored_nodes",nodes);
+        sync->setIgnored(nodes);
+    }
+
+    canopen::LayerStack stack("SyncNodeLayer");
+
+    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);
+    diag_updater.setHardwareID(nh_priv.param("hardware_id", std::string("none")));
+    diag_updater.add("sync", std::bind(report_diagnostics,std::ref(stack), std::placeholders::_1));
+
+    ros::Timer diag_timer = nh.createTimer(ros::Duration(diag_updater.getPeriod()/2.0),std::bind(&diagnostic_updater::Updater::update, &diag_updater));
+
+    ros::spin();
+    return 0;
+}

Devices/Libraries/Ros/ros_canopen/canopen_chain_node/srv/GetObject.srv

@@ -0,0 +1,8 @@
+string node
+string object
+bool cached
+---
+bool success
+string message
+string value
+

Devices/Libraries/Ros/ros_canopen/canopen_chain_node/srv/SetObject.srv

@@ -0,0 +1,8 @@
+string node
+string object
+string value
+bool cached
+---
+bool success
+string message
+