git commit -m "first commit for v2"

Devices/Libraries/Ros/delta_modbus/CMakeLists.txt

@@ -0,0 +1,85 @@
+cmake_minimum_required(VERSION 3.0.2)
+project(delta_modbus)
+
+find_package(catkin REQUIRED COMPONENTS
+  roscpp
+  std_msgs
+  modbus_tcp
+  diagnostic_msgs
+)
+
+find_package(Boost REQUIRED COMPONENTS system)
+
+catkin_package(
+  INCLUDE_DIRS include
+  LIBRARIES ${PROJECT_NAME}
+  CATKIN_DEPENDS roscpp std_msgs modbus_tcp diagnostic_msgs
+  DEPENDS Boost 
+)
+
+include_directories(
+  include
+  ${catkin_INCLUDE_DIRS}
+)
+
+add_library(${PROJECT_NAME} src/delta_modbus_tcp.cpp)
+add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} ${Boost_LIBRARIES} ${GSTREAMER_LIBRARIES})
+
+add_executable(test_delta test/test_delta.cpp)
+add_dependencies(test_delta ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+target_link_libraries(test_delta ${PROJECT_NAME} ${catkin_LIBRARIES})
+
+#############
+## Install ##
+#############
+
+# all install targets should use catkin DESTINATION variables
+# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
+
+## Mark executable scripts (Python etc.) for installation
+## in contrast to setup.py, you can choose the destination
+# catkin_install_python(PROGRAMS
+#   scripts/my_python_script
+#   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark executables for installation
+## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html
+# install(TARGETS test_delta
+#   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark libraries for installation
+## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html
+install(TARGETS ${PROJECT_NAME}
+  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
+)
+
+## Mark cpp header files for installation
+install(DIRECTORY include/${PROJECT_NAME}/
+  DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+  FILES_MATCHING PATTERN "*.h"
+  PATTERN ".svn" EXCLUDE
+)
+
+## Mark other files for installation (e.g. launch and bag files, etc.)
+install(FILES
+  launch/test_delta.launch
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+)
+
+#############
+## Testing ##
+#############
+
+## Add gtest based cpp test target and link libraries
+# catkin_add_gtest(${PROJECT_NAME}-test test/test_delta_modbus_tcp.cpp)
+# if(TARGET ${PROJECT_NAME}-test)
+#   target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
+# endif()
+
+## Add folders to be run by python nosetests
+# catkin_add_nosetests(test)

Devices/Libraries/Ros/delta_modbus/include/delta_modbus/delta_exception.h

@@ -0,0 +1,53 @@
+#ifndef __DELTA_EXCEPTION_H_INCLUDE
+#define __DELTA_EXCEPTION_H_INCLUDE
+#include <iostream>
+#include <exception>
+using namespace std;
+
+class CompareException : public std::exception {
+public:
+  const char * what () const throw () {
+    return "The first Address value must be less than the second Address value";
+  }
+};
+
+class AddressIsNotExistException : public std::exception {
+  int Address;
+public:
+  AddressIsNotExistException(int Address) {
+    this->Address = Address;
+  }
+
+  const char * what () const throw () {
+    std::stringstream msg;
+    msg << "The Addtess 0x" << Address << " is not Exist";
+    return msg.str().c_str();
+  }
+};
+
+class OutRangeException : public std::exception {
+  int Maximum;
+public:
+  OutRangeException(int Maximum) {
+    this->Maximum = Maximum;
+  }
+
+  const char * what () const throw () {
+    std::stringstream msg;
+    msg << "Starting address must be 0 - 65535; quantity must be 0 - " << Maximum;
+    return msg.str().c_str();
+  }
+};
+
+class Exception : public std::exception {
+  int Maximum;
+public:
+  std::string msg;
+  Exception(std::string msg) {
+    this->msg = msg;
+  }
+  const char * what () const throw () {
+    return msg.c_str();
+  }
+};
+#endif

Devices/Libraries/Ros/delta_modbus/include/delta_modbus/delta_infomation.h

@@ -0,0 +1,89 @@
+#ifndef __DELTA_INFOMATION_H_INCLUDE_
+#define __DELTA_INFOMATION_H_INCLUDE_
+
+#define PLC_RUN_OK                      0X00
+#define NOT_CONNECT_TO_PLC              0X01
+#define PLC_NOT_RUN                     0X02
+
+#define DELTA_NAMESPACE_BEGIN namespace delta {
+#define DELTA_NAMESPACE delta
+#define DELTA_NAMESPACE_END }
+
+#define ON true
+#define OFF false
+#define TIME_ERROR  5
+
+#define S0_HEX      0x0001
+#define S1023_HEX   0x03FF
+#define S0_NUM      0
+#define S1023       1023
+
+#define X0_HEX      0x0400
+#define X377_HEX    0x04FF
+#define X0_NUM      0
+#define X377_NUM    377
+
+#define Y0_HEX      0x0500       
+#define Y377_HEX    0x05FF
+#define Y0_NUM      0      
+#define Y377_NUM    377
+
+#define M0_HEX      0x0800   
+#define M1535_HEX   0x0DFF
+#define M0_NUM      0  
+#define M1535_NUM   1535
+
+#define M1536_HEX   0xB000
+#define M4095_HEX   0xB9FF
+#define M1536_NUM   1536
+#define M4095_NUM   4095
+
+#define C0_HEX      0x0E00
+#define C199_HEX    0x0EC7
+#define C0_NUM      0
+#define C199_NUM    199
+
+#define D0_HEX      0x1000
+#define D4095_HEX   0x1FFF
+#define D0_NUM      0
+#define D4095_NUM   4095
+
+#define READ_M_BITS     0x01
+#define READ_D_REGS     0x02
+#define READ_Y_BITS     0x03
+#define READ_X_BITS     0x04
+#define WRITE_M_BITS    0x05
+#define WRITE_D_REGS    0x06
+#define WRITE_Y_BITS    0x0F
+
+enum DO_ {
+    Y0 	    =	0	,Y1	    =	1	,Y2	    =	2	,Y3 	=	3	,Y4	    =	4	,Y5	    =	5	,Y6	    =	6	,Y7	    =	7	,
+    Y10	    =	8	,Y11	=	9	,Y12	=	10	,Y13	=	11	,Y14	=	12	,Y15	=	13	,Y16	=	14	,Y17	=	15	,
+    Y20	    =	16	,Y21	=	17	,Y22	=	18	,Y23	=	19	,Y24	=	20	,Y25	=	21	,Y26	=	22	,Y27	=	23	,
+    Y30	    =	24	,Y31	=	25	,Y32	=	26	,Y33	=	27	,Y34	=	28	,Y35	=	29	,Y36	=	30	,Y37	=	31	,
+    Y40 	=	32	,Y41	=	33	,Y42	=	34	,Y43	=	35	,Y44	=	36	,Y45	=	37	,Y46	=	38	,Y47	=	39	,
+    Y50	    =	40	,Y51	=	41	,Y52	=	42	,Y53	=	43	,Y54	=	44	,Y55	=	45	,Y56	=	46	,Y57	=	47	,
+    Y60	    =	48	,Y61	=	49	,Y62	=	50	,Y63	=	51	,Y64	=	52	,Y65	=	53	,Y66	=	54	,Y67	=	55	,
+    Y70	    =	56	,Y71	=	57	,Y72	=	58	,Y73	=	59	,Y74	=	60	,Y75	=	61	,Y76	=	62	,Y77	=	63	,
+    Y100	=	64	,Y101	=	65	,Y102	=	66	,Y103	=	67	,Y104	=	68	,Y105	=	69	,Y106	=	70	,Y107	=	71	,
+    Y110	=	72	,Y111	=	73	,Y112	=	74	,Y113	=	75	,Y114	=	76	,Y115	=	77	,Y116	=	78	,Y117	=	79	,
+    Y120	=	80	,Y121	=	81	,Y122	=	82	,Y123	=	83	,Y124	=	84	,Y125	=	85	,Y126	=	86	,Y127	=	87	,
+    Y130	=	88	,Y131	=	89	,Y132	=	90	,Y133	=	91	,Y134	=	92	,Y135	=	93	,Y136	=	94	,Y137	=	95	
+};
+
+enum DI_ {
+    X0 	    =	0	,X1	    =	1	,X2	    =	2	,X3 	=	3	,X4	    =	4	,X5	    =	5	,X6	    =	6	,X7	    =	7	,
+    X10	    =	8	,X11	=	9	,X12	=	10	,X13	=	11	,X14	=	12	,X15	=	13	,X16	=	14	,X17	=	15	,
+    X20	    =	16	,X21	=	17	,X22	=	18	,X23	=	19	,X24	=	20	,X25	=	21	,X26	=	22	,X27	=	23	,
+    X30	    =	24	,X31	=	25	,X32	=	26	,X33	=	27	,X34	=	28	,X35	=	29	,X36	=	30	,X37	=	31	,
+    X40 	=	32	,X41	=	33	,X42	=	34	,X43	=	35	,X44	=	36	,X45	=	37	,X46	=	38	,X47	=	39	,
+    X50	    =	40	,X51	=	41	,X52	=	42	,X53	=	43	,X54	=	44	,X55	=	45	,X56	=	46	,X57	=	47	,
+    X60	    =	48	,X61	=	49	,X62	=	50	,X63	=	51	,X64	=	52	,X65	=	53	,X66	=	54	,X67	=	55	,
+    X70	    =	56	,X71	=	57	,X72	=	58	,X73	=	59	,X74	=	60	,X75	=	61	,X76	=	62	,X77	=	63	,
+    X100	=	64	,X101	=	65	,X102	=	66	,X103	=	67	,X104	=	68	,X105	=	69	,X106	=	70	,X107	=	71	,
+    X110	=	72	,X111	=	73	,X112	=	74	,X113	=	75	,X114	=	76	,X115	=	77	,X116	=	78	,X117	=	79	,
+    X120	=	80	,X121	=	81	,X122	=	82	,X123	=	83	,X124	=	84	,X125	=	85	,X126	=	86	,X127	=	87	,
+    X130	=	88	,X131	=	89	,X132	=	90	,X133	=	91	,X134	=	92	,X135	=	93	,X136	=	94	,X137	=	95	
+};
+
+#endif

Devices/Libraries/Ros/delta_modbus/include/delta_modbus/delta_modbus_tcp.h

@@ -0,0 +1,216 @@
+#ifndef __DELTA_LIBRARY_H_INCLUDED
+#define __DELTA_LIBRARY_H_INCLUDED 
+#include <ros/ros.h>
+#include <iostream>
+#include "modbus_tcp/modbus.h"
+#include <boost/shared_ptr.hpp>
+#include "delta_modbus/delta_infomation.h"
+#include "delta_modbus/delta_exception.h"
+
+
+DELTA_NAMESPACE_BEGIN
+class PLC
+{  
+public:
+    PLC(std::string ip_address, int port, int id = 1);
+
+    virtual ~PLC();
+    
+    /**
+     * @brief Connect to PLC
+     */
+    void connect(void);
+
+     /**
+     * @brief Close to PLC
+     */
+    void close(void);
+
+     /**
+     * @brief Checking PLC is connected
+     */
+    bool checkConnected(void);
+
+    /**
+     * @brief Write ON/OFF M Bit
+     * @param M_number  M bit
+     * @return 0/-1
+     */
+    int writeM(int M_number, const bool status);
+
+
+    /** 
+     * @brief Set ON/OFF multiple M bit
+     * @param start_number  begin M bit
+     * @param end_number    end M bit
+     * @return 0/-1
+     */
+    int mulWriteM(int start_number, int end_number, bool *status);
+
+
+    /** 
+     * @brief SET ON M Bit
+     * @param M_number  M bit
+     * @return 0/-1
+     */
+    int setM(int M_number);
+
+    /** 
+     * @brief  SET OFF M Bit
+     * @param M_number Node handle to get param for PLC
+     * @return 0/-1
+     */
+    int resetM(int M_number);
+
+    /** 
+     * @brief Set ON multiple M bit
+     * @param start_number  begin M bit
+     * @param end_number    end M bit
+     * @return 0/-1
+     */
+    int mulSetM(int start_number, int end_number);
+
+    /** 
+     * @brief Set OFF multiple M bit
+     * @param start_number  begin M bit
+     * @param end_number    end M bit
+     * @return 0/-1
+     */
+    int mulResetM(int start_number, int end_number);
+
+    /** 
+     * @brief Get load M Bit
+     * @param M_number  M bit 
+     * @param buffer    Status of M bit
+     * @return 0/-1
+     */
+    int getM(int M_number, bool &buffer);
+
+    /** 
+     * @brief Get load mul M bit 
+     * @param start_number  Begin M bit
+     * @param end_number    End M bit
+     * @param buffer        Status of M bit  
+     * @return 0/-1
+     */
+    int mulGetM(int start_number, int end_number, bool* buffer);
+
+    /**
+     * @brief Set On output Y
+     * @param Y_number  
+     * @return 0/-1
+     */
+    int setY(int Y_number);
+
+    /** 
+     * @brief Set Off output Y
+     * @param Y_number  
+     * @return 0/-1
+     */
+    int resetY(int Y_number);
+
+    /** 
+     * @brief Set multiple output Y is ON/OFF
+     * @param start_number  begin M bit
+     * @param end_number    end M bit
+     * @return 0/-1
+     */
+    int mulWriteY(int start_number, int end_number, bool *buffer);
+
+    /** 
+     * @brief Set multiple output Y is ON
+     * @param start_number  begin M bit
+     * @param end_number    end M bit
+     * @return 0/-1
+     */
+    int mulSetY(int start_number, int end_number);
+
+    /** 
+     * Set multiple output Y is OFF
+     * @param start_number  begin M bit
+     * @param end_number    end M bit
+     * @return 0/-1
+     */
+    int mulResetY(int start_number, int end_number);
+    
+    /** 
+     * @brief Get status Y coils from start_number to end_number.
+     * @param start_number  begin M bit
+     * @param end_number    end M bit
+     * @param buffer        The status bit coils add to buffer
+     */
+    int mulGetY(int start_number, int end_number, bool *buffer);
+
+    /** 
+     * @brief Read D register
+     * @param D_number Number D
+     * @param buffer   Value of D_number
+     */
+    int getD(int D_number, uint16_t &buffer);
+
+    /** 
+     * @brief Send data to D register
+     * @param D_number Number D
+     * @param buffer   Value of D_number
+     */
+    int mulGetD(int start_number, int end_number, uint16_t *buffer);
+
+    /** 
+     * @brief Send data to D register ()
+     * @param start_number  Begin D register
+     * @param end_number    End D register
+     * @param buffer        Buffer to store Data Read from Input Bits
+     */
+    int setD(int D_number, int16_t &buffer);
+
+    /** 
+     * @brief Send data to D register ()
+     * @param start_number  Begin D register
+     * @param end_number    End D register
+     * @param buffer        Buffer to store Data Read from Input Bits
+     */
+    int mulSetD(int start_number, int end_number, uint16_t *buffer);
+    
+    /** 
+     * @brief Read X bit
+     * @param X_number  Number X
+     * @param buffer    Status of X
+     */
+    int getX(int X_number, bool &buffer);
+
+    /** 
+     * @brief Read X bit
+     * @param start_number  Begin number X
+     * @param end_number    End number X
+     * @param buffer        Status of X
+     */
+    int mulGetX(int start_number, int end_number, bool* buffer);
+
+    /** 
+     * @brief Read C register
+     * @param C_number Number C
+     * @param buffer   Value of C_number
+     */
+    int getC(int C_number, uint16_t &buffer);
+
+    /** 
+     * @brief Send data to C register ()
+     * @param start_number  Begin C register
+     * @param end_number    End C register
+     * @param buffer        Buffer to store Data Read from Input Bits
+     */
+    int mulGetC(int start_number, int end_number, uint16_t *buffer);
+
+    /**
+     * @brief get error information
+     */
+    inline uint8_t getError();
+
+private:
+    /* Properties */
+    uint8_t error_num;
+    boost::shared_ptr<modbus> mb_;      
+};
+DELTA_NAMESPACE_END
+
+#endif

Devices/Libraries/Ros/delta_modbus/launch/test_delta.launch

@@ -0,0 +1,9 @@
+<?xml version="1.0"?>
+<launch>
+  <arg name="output_screen"       default="screen"/>
+  <arg name="path_yaml_file"  default="$(find delta_modbus_tcp)/plc_config.yaml"/>
+
+  <node pkg="delta_modbus_tcp" type="test_delta" name="test_delta" output="$(arg output_screen)"> 
+    <rosparam command="load"    file="$(arg path_yaml_file)" />
+  </node>
+</launch>

Devices/Libraries/Ros/delta_modbus/package.xml

@@ -0,0 +1,73 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>delta_modbus</name>
+  <version>0.0.0</version>
+  <description>The delta_modbus_tcp package</description>
+
+  <!-- One maintainer tag required, multiple allowed, one person per tag -->
+  <!-- Example:  -->
+  <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+  <maintainer email="robotics@todo.todo">robotics</maintainer>
+
+
+  <!-- One license tag required, multiple allowed, one license per tag -->
+  <!-- Commonly used license strings: -->
+  <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
+  <license>TODO</license>
+
+
+  <!-- Url tags are optional, but multiple are allowed, one per tag -->
+  <!-- Optional attribute type can be: website, bugtracker, or repository -->
+  <!-- Example: -->
+  <!-- <url type="website">http://wiki.ros.org/delta_modbus_tcp</url> -->
+
+
+  <!-- Author tags are optional, multiple are allowed, one per tag -->
+  <!-- Authors do not have to be maintainers, but could be -->
+  <!-- Example: -->
+  <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
+
+
+  <!-- The *depend tags are used to specify dependencies -->
+  <!-- Dependencies can be catkin packages or system dependencies -->
+  <!-- Examples: -->
+  <!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
+  <!--   <depend>roscpp</depend> -->
+  <!--   Note that this is equivalent to the following: -->
+  <!--   <build_depend>roscpp</build_depend> -->
+  <!--   <exec_depend>roscpp</exec_depend> -->
+  <!-- Use build_depend for packages you need at compile time: -->
+  <!--   <build_depend>message_generation</build_depend> -->
+  <!-- Use build_export_depend for packages you need in order to build against this package: -->
+  <!--   <build_export_depend>message_generation</build_export_depend> -->
+  <!-- Use buildtool_depend for build tool packages: -->
+  <!--   <buildtool_depend>catkin</buildtool_depend> -->
+  <!-- Use exec_depend for packages you need at runtime: -->
+  <!--   <exec_depend>message_runtime</exec_depend> -->
+  <!-- Use test_depend for packages you need only for testing: -->
+  <!--   <test_depend>gtest</test_depend> -->
+  <!-- Use doc_depend for packages you need only for building documentation: -->
+  <!--   <doc_depend>doxygen</doc_depend> -->
+  <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>roscpp</build_depend>
+  <build_export_depend>roscpp</build_export_depend>
+  <exec_depend>roscpp</exec_depend>
+
+  <build_depend>std_msgs</build_depend>
+  <build_depend>modbus_tcp</build_depend>
+  <build_depend>diagnostic_msgs</build_depend>
+
+  <build_export_depend>std_msgs</build_export_depend>
+  <build_export_depend>modbus_tcp</build_export_depend>  
+  <build_export_depend>diagnostic_msgs</build_export_depend>
+
+  <exec_depend>std_msgs</exec_depend>
+  <exec_depend>modbus_tcp</exec_depend>
+  <exec_depend>diagnostic_msgs</exec_depend>
+
+  <!-- The export tag contains other, unspecified, tags -->
+  <export>
+    <!-- Other tools can request additional information be placed here -->
+
+  </export>
+</package>

Devices/Libraries/Ros/delta_modbus/plc_config.yaml

@@ -0,0 +1,3 @@
+IP: 192.168.2.100 # Địa chỉ HOST của thiết bị cua PLC
+PORT: 502         # Cổng PORT kết nối vớt PLC
+ID: 1             # Số ID của thiết bị mặc định là 1

Devices/Libraries/Ros/delta_modbus/src/delta_modbus_tcp.cpp

@@ -0,0 +1,866 @@
+#include "delta_modbus/delta_modbus_tcp.h"
+#include <boost/exception/all.hpp>
+
+DELTA_NAMESPACE::PLC::PLC(std::string ip_address, int port, int id)
+ : mb_(nullptr) 
+{
+    mb_ = boost::make_shared<modbus>(ip_address, port);
+    mb_->modbus_set_slave_id(id);
+}
+
+DELTA_NAMESPACE::PLC::~PLC()
+{
+    mb_ = nullptr;
+}
+
+void DELTA_NAMESPACE::PLC::connect(void) {
+    ROS_INFO("Delta PLC is connecting.......");
+    try
+    {
+        if(!mb_->modbus_connect()) {
+            ROS_ERROR("Delta PLC connection is error");
+            error_num = NOT_CONNECT_TO_PLC;
+        } else {
+            ros::Duration(0.5).sleep();
+            error_num = PLC_RUN_OK;
+            ROS_INFO("Delta PLC is Connected");
+        } 
+    }
+    catch(const boost::exception& e)
+    {
+        ROS_ERROR_STREAM(boost::diagnostic_information(e));
+    }
+    catch (const std::exception& e) 
+    {
+        ROS_ERROR_STREAM(e.what());
+    } 
+    catch (...) 
+    {
+        ROS_ERROR_STREAM("Caught unknown exception");
+    }   
+}
+
+void DELTA_NAMESPACE::PLC::close()
+{
+    try
+    {
+        mb_->modbus_close();
+    }
+    catch(const boost::exception& e)
+    {
+        ROS_ERROR_STREAM(boost::diagnostic_information(e));
+    }
+    catch (const std::exception& e) 
+    {
+        ROS_ERROR_STREAM(e.what());
+    } 
+    catch (...) 
+    {
+        ROS_ERROR_STREAM("Caught unknown exception");
+    }   
+}
+
+bool DELTA_NAMESPACE::PLC::checkConnected()
+{
+    try
+    {
+        return mb_->_connected;
+    }
+    catch (const std::exception& e) 
+    {
+        ROS_ERROR_STREAM(e.what());
+    } 
+    catch(const boost::exception& e)
+    {
+        ROS_ERROR_STREAM(boost::diagnostic_information(e));
+    }
+    catch (...) 
+    {
+        ROS_ERROR_STREAM("Caught unknown exception");
+    }   
+}
+
+int DELTA_NAMESPACE::PLC::writeM(int M_number, const bool status) {
+    try {
+        if(M_number < M0_NUM || M_number > M4095_NUM) { throw AddressIsNotExistException(M_number); }
+        if(M_number >= M0_NUM && M_number <= M1535_NUM) {
+            return mb_->modbus_write_coil(M0_HEX + M_number, status);
+        }
+        else if(M_number >= M1536_NUM && M_number <= M4095_NUM) {
+            return mb_->modbus_write_coil(M1536_HEX + M_number, status);
+        }
+    }    
+    catch(AddressIsNotExistException & e) {
+        ROS_ERROR_STREAM("delta::writeM: " << e.what());
+        ros::Duration(TIME_ERROR).sleep();
+        return BAD_CON; 
+    }
+    catch(std::exception& e) {
+        ROS_ERROR_STREAM("delta::writeM: " << e.what());
+        ros::Duration(TIME_ERROR).sleep();
+        return BAD_CON; 
+    }
+    catch(const boost::exception& e)
+    {
+        ROS_ERROR_STREAM(boost::diagnostic_information(e));
+    }
+    catch (...) 
+    {
+        ROS_ERROR_STREAM("Caught unknown exception");
+    }   
+    return 0;
+}
+
+int DELTA_NAMESPACE::PLC::mulWriteM(int start_number, int end_number, bool *status) {
+    try {
+        if(start_number >= end_number)                          { throw CompareException(); }
+        if(start_number < M0_NUM || start_number > M4095_NUM)   { throw AddressIsNotExistException(start_number); }
+        if(end_number < M0_NUM || end_number > M4095_NUM)       { throw AddressIsNotExistException(end_number); }
+       
+        if(start_number >= M0_NUM && end_number <= M1535_NUM) {
+            int amount = end_number - start_number + 1;
+            bool value[amount];
+            for(int i = 0; i < amount; i++) value[i] = status[i];
+            return mb_->modbus_write_coils(M0_HEX + start_number, amount,value);
+        }
+        else if(start_number >= M1536_NUM && end_number <= M4095_NUM) {
+            int amount = end_number - start_number + 1;
+            bool value[amount];
+            for(int i = 0; i < amount; i++) value[i] = status[start_number + i];
+            return mb_->modbus_write_coils(M1536_HEX + start_number - M1535_NUM, amount, value);
+        }
+        else {
+            int amount1,amount2;
+            amount1 = M1535_NUM - start_number + 1;
+            amount2 = end_number - M1536_NUM + 1;
+            bool value1[amount1], value2[amount2];
+            for(int i = 0; i < amount1; i++) value1[i] = status[i];
+            for(int i = 0; i < amount2; i++) value2[i] = status[amount1 + i];
+            int n1,n2;
+            n1 = mb_->modbus_write_coils(M0_HEX + start_number,amount1,value1);
+            n2 = mb_->modbus_write_coils(M1536_HEX, amount2,value2);
+            return n1*n2;
+        }
+    }
+    catch(CompareException & e) {
+        ROS_ERROR_STREAM("delta::mulWriteM: " << e.what());
+        ros::Duration(TIME_ERROR).sleep();
+        return BAD_CON;
+    } 
+    catch(AddressIsNotExistException & e) {
+        ROS_ERROR_STREAM("delta::mulWriteM: " << e.what());
+        ros::Duration(TIME_ERROR).sleep();
+        return BAD_CON; 
+    }
+    catch(std::exception & e) {
+        ROS_ERROR_STREAM("delta::mulWriteM: " << e.what());
+        ros::Duration(TIME_ERROR).sleep();
+        return BAD_CON; 
+    }
+    catch(const boost::exception& e)
+    {
+        ROS_ERROR_STREAM(boost::diagnostic_information(e));
+    }
+    catch (...) 
+    {
+        ROS_ERROR_STREAM("Caught unknown exception");
+    } 
+    return 0;
+}
+
+int DELTA_NAMESPACE::PLC::setM(int M_number) {
+    try {
+        if(M_number < M0_NUM || M_number > M4095_NUM) { throw AddressIsNotExistException(M_number); }
+        if(M_number >= M0_NUM && M_number <= M1535_NUM){
+            return mb_->modbus_write_coil(M0_HEX + M_number, ON);
+        }
+        else if(M_number >= M1536_NUM && M_number <= M4095_NUM) {
+            return mb_->modbus_write_coil(M1536_HEX + M_number, ON);
+        }
+    }
+    catch(AddressIsNotExistException & e) {
+        ROS_ERROR_STREAM("PLC::setM: " << e.what());
+        ros::Duration(TIME_ERROR).sleep();
+        return BAD_CON; 
+    }
+    catch(std::exception& e) {
+        ROS_ERROR_STREAM("PLC::setM: " << e.what());
+        ros::Duration(TIME_ERROR).sleep();
+        return BAD_CON; 
+    }
+    catch(const boost::exception& e)
+    {
+        ROS_ERROR_STREAM(boost::diagnostic_information(e));
+    }
+    catch (...) 
+    {
+        ROS_ERROR_STREAM("Caught unknown exception");
+    } 
+    return 0;
+}
+
+int DELTA_NAMESPACE::PLC::resetM(int M_number) {
+    try {
+        if(M_number < M0_NUM || M_number > M4095_NUM) { throw AddressIsNotExistException(M_number); }
+        if(M_number >= M0_NUM && M_number <= M1535_NUM){
+            return mb_->modbus_write_coil(M0_HEX + M_number, OFF);
+        }
+        else if(M_number >= M1536_NUM && M_number <= M4095_NUM) {
+            return mb_->modbus_write_coil(M1536_HEX + M_number, OFF);
+        }
+    }
+    catch(AddressIsNotExistException & e) {
+        ROS_ERROR_STREAM("PLC::setM: " << e.what());
+        ros::Duration(TIME_ERROR).sleep();
+        return BAD_CON; 
+    }
+    catch(std::exception& e) {
+        ROS_ERROR_STREAM("PLC::setM: " << e.what());
+        ros::Duration(TIME_ERROR).sleep();
+        return BAD_CON; 
+    }
+    catch(const boost::exception& e)
+    {
+        ROS_ERROR_STREAM(boost::diagnostic_information(e));
+    }
+    catch (...) 
+    {
+        ROS_ERROR_STREAM("Caught unknown exception");
+    } 
+    return 0;
+}
+
+int DELTA_NAMESPACE::PLC::mulSetM(int start_number, int end_number) {
+    try {
+        if(start_number >= end_number)                         { throw CompareException(); }
+        if(start_number < M0_NUM || start_number > M4095_NUM)  { throw AddressIsNotExistException(start_number); }
+        if(end_number < M0_NUM || end_number > M4095_NUM)      { throw AddressIsNotExistException(end_number); }
+        
+        if(start_number >= M0_NUM && end_number <= M1535_NUM) {
+            int amount = end_number - start_number + 1;
+            bool value[amount];
+            for(int i = 0; i < amount; i++) value[i] = ON;
+            return mb_->modbus_write_coils(M0_HEX + start_number, amount,value);
+        }
+        else if(start_number >= M1536_NUM && end_number <= M4095_NUM) {
+            int amount = end_number - start_number + 1;
+            bool value[amount];
+            for(int i = 0; i < amount; i++) value[i] = ON;
+            return mb_->modbus_write_coils(M1536_HEX + start_number - M1535_NUM, amount, value);
+        }
+        else {
+            int amount1,amount2;
+            amount1 = M1535_NUM - start_number + 1;
+            amount2 = end_number - M1536_NUM + 1;
+            bool value1[amount1], value2[amount2];
+            for(int i = 0; i < amount1; i++) value1[i] = ON;
+            for(int i = 0; i < amount2; i++) value2[i] = ON;
+            int n1,n2;
+            n1 = mb_->modbus_write_coils(M0_HEX + start_number,amount1,value1);
+            n2 = mb_->modbus_write_coils(M1536_HEX, amount2,value2);
+            return n1*n2;
+        }
+    }
+    catch(CompareException & e) {
+        ROS_ERROR_STREAM("PLC::mulSetM: " << e.what());
+        ros::Duration(TIME_ERROR).sleep();
+        return BAD_CON;
+    } 
+    catch(AddressIsNotExistException & e) {
+        ROS_ERROR_STREAM("PLC::mulSetM: " << e.what());
+        ros::Duration(TIME_ERROR).sleep();
+        return BAD_CON; 
+    }
+    catch(std::exception& e) {
+        ROS_ERROR_STREAM("PLC::mulSetM: " << e.what());
+        ros::Duration(TIME_ERROR).sleep();
+        return BAD_CON; 
+    }
+    catch(const boost::exception& e)
+    {
+        ROS_ERROR_STREAM(boost::diagnostic_information(e));
+    }
+    catch (...) 
+    {
+        ROS_ERROR_STREAM("Caught unknown exception");
+    } 
+    return 0;
+}
+
+int DELTA_NAMESPACE::PLC::mulResetM(int start_number, int end_number) {
+    try {
+        if(start_number >= end_number)                         { throw CompareException(); }
+        if(start_number < M0_NUM || start_number > M4095_NUM)  { throw AddressIsNotExistException(start_number); }
+        if(end_number < M0_NUM || end_number > M4095_NUM)      { throw AddressIsNotExistException(end_number); }
+        
+        if(start_number >= M0_NUM && end_number <= M1535_NUM) {
+            int amount = end_number - start_number + 1;
+            bool value[amount];
+            for(int i = 0; i < amount; i++) value[i] = OFF;
+            return mb_->modbus_write_coils(M0_HEX + start_number, amount,value);
+        }
+        else if(start_number >= M1536_NUM && end_number <= M4095_NUM) {
+            int amount = end_number - start_number + 1;
+            bool value[amount];
+            for(int i = 0; i < amount; i++) value[i] = OFF;
+            return mb_->modbus_write_coils(M1536_HEX + start_number - M1535_NUM, amount, value);
+        }
+        else {
+            int amount1,amount2;
+            amount1 = M1535_NUM - start_number + 1;
+            amount2 = end_number - M1536_NUM + 1;
+            bool value1[amount1], value2[amount2];
+            for(int i = 0; i < amount1; i++) value1[i] = OFF;
+            for(int i = 0; i < amount2; i++) value2[i] = OFF;
+            int n1,n2;
+            n1 = mb_->modbus_write_coils(M0_HEX + start_number,amount1,value1);
+            n2 = mb_->modbus_write_coils(M1536_HEX, amount2,value2);
+            return n1*n2;
+        }
+    }
+    catch(CompareException & e) {
+        ROS_ERROR_STREAM("PLC::mulSetM: " << e.what());
+        ros::Duration(TIME_ERROR).sleep();
+        return BAD_CON;
+    } 
+    catch(AddressIsNotExistException & e) {
+        ROS_ERROR_STREAM("PLC::mulSetM: " << e.what());
+        ros::Duration(TIME_ERROR).sleep();
+        return BAD_CON; 
+    }
+    catch(std::exception& e) {
+        ROS_ERROR_STREAM("PLC::mulSetM: " << e.what());
+        ros::Duration(TIME_ERROR).sleep();
+        return BAD_CON; 
+    }
+    catch(const boost::exception& e)
+    {
+        ROS_ERROR_STREAM(boost::diagnostic_information(e));
+    }
+    catch (...) 
+    {
+        ROS_ERROR_STREAM("Caught unknown exception");
+    } 
+    return 0;
+}
+
+int DELTA_NAMESPACE::PLC::getM(int M_number, bool &buffer) {
+    try {
+        if(M_number < M0_NUM || M_number > M4095_NUM) { throw AddressIsNotExistException(M_number); }
+        
+        if(M_number >= M0_NUM && M_number <= M1535_NUM)
+        {
+            return mb_->modbus_read_coil(M0_HEX + M_number, buffer);
+        }
+        else if(M_number >= M1536_NUM && M_number <= M4095_NUM){
+            return mb_->modbus_read_coil(M1536_HEX + M_number, buffer);
+        }
+    }
+    catch(AddressIsNotExistException & e) {
+        ROS_ERROR_STREAM("PLC::getM: " << e.what());
+        ros::Duration(TIME_ERROR).sleep();
+        return BAD_CON; 
+    }
+    catch(std::exception& e) {
+        ROS_ERROR_STREAM("PLC::getM: " << e.what());
+        ros::Duration(TIME_ERROR).sleep();
+        return BAD_CON; 
+    } 
+    catch(const boost::exception& e)
+    {
+        ROS_ERROR_STREAM(boost::diagnostic_information(e));
+    }
+    catch (...) 
+    {
+        ROS_ERROR_STREAM("Caught unknown exception");
+    } 
+    return 0;
+}
+
+int DELTA_NAMESPACE::PLC::mulGetM(int start_number, int end_number, bool* buffer) {
+    try {
+        if(start_number >= end_number)                          { throw CompareException(); }
+        if(start_number < M0_NUM || start_number > M4095_NUM)  { throw AddressIsNotExistException(start_number); }
+        if(end_number < M0_NUM || end_number > M4095_NUM)      { throw AddressIsNotExistException(end_number); }
+        
+        if(start_number >= M0_NUM && end_number <= M1535_NUM) {
+            int amount = end_number - start_number + 1;
+            return mb_->modbus_read_coils(M0_HEX + start_number, amount,buffer);
+        }
+        else if(start_number >= M1536_NUM && end_number <= M4095_NUM) {
+            int amount = end_number - start_number + 1;
+            return mb_->modbus_read_coils(M1536_HEX + start_number - M1535_NUM, amount,buffer);
+        }
+        else {
+            int amount1,amount2;
+            amount1 = M1535_NUM - start_number + 1;
+            amount2 = end_number - M1536_NUM + 1;
+            bool value1[amount1], value2[amount2];
+            int n1,n2;
+            n1 = mb_->modbus_read_coils(M0_HEX + start_number,amount1,value1);
+            n2 = mb_->modbus_read_coils(M1536_HEX, amount2,value2);
+            for(int i = 0; i < amount1; i++) buffer[i] = value1[i];
+            for(int i = 0; i < amount2; i++) buffer[amount1 + i] = value2[i];
+            return n1*n2;
+        }
+    }
+    catch(CompareException & e) {
+        ROS_ERROR_STREAM("PLC::mulGetM: " << e.what());
+        ros::Duration(TIME_ERROR).sleep();
+        return BAD_CON;
+    } 
+    catch(AddressIsNotExistException & e) {
+        ROS_ERROR_STREAM("PLC::mulGetM: " << e.what());
+        ros::Duration(TIME_ERROR).sleep();
+        return BAD_CON; 
+    }
+    catch(std::exception& e) {
+        ROS_ERROR_STREAM("PLC::mulGetM: " << e.what());
+        ros::Duration(TIME_ERROR).sleep();
+        return BAD_CON; 
+    }
+    catch(const boost::exception& e)
+    {
+        ROS_ERROR_STREAM(boost::diagnostic_information(e));
+    }
+    catch (...) 
+    {
+        ROS_ERROR_STREAM("Caught unknown exception");
+    } 
+    return 0;
+}
+
+int DELTA_NAMESPACE::PLC::setY(int Y_number) {
+    try {
+        if(Y_number < Y0_NUM || Y_number > Y377_NUM) { throw AddressIsNotExistException(Y_number); }
+        return mb_->modbus_write_coil(Y0_HEX + Y_number, ON);
+    }
+    catch(AddressIsNotExistException & e) {
+        ROS_ERROR_STREAM("PLC::setY: " << e.what());
+        ros::Duration(TIME_ERROR).sleep();
+        return BAD_CON; 
+    }
+    catch(std::exception& e) {
+        ROS_ERROR_STREAM("PLC::setY: " << e.what());
+        ros::Duration(TIME_ERROR).sleep();
+        return BAD_CON; 
+    } 
+    catch(const boost::exception& e)
+    {
+        ROS_ERROR_STREAM(boost::diagnostic_information(e));
+    }
+    catch (...) 
+    {
+        ROS_ERROR_STREAM("Caught unknown exception");
+    } 
+    return 0; 
+}
+
+int DELTA_NAMESPACE::PLC::resetY(int Y_number) {
+    try {
+        if(Y_number < Y0_NUM || Y_number > Y377_NUM) { throw AddressIsNotExistException(Y_number); }
+        return mb_->modbus_write_coil(Y0_HEX + Y_number, OFF);
+    }
+    catch(AddressIsNotExistException & e) {
+        ROS_ERROR_STREAM("PLC::setY: " << e.what());
+        ros::Duration(TIME_ERROR).sleep();
+        return BAD_CON; 
+    }
+    catch(std::exception& e) {
+        ROS_ERROR_STREAM("PLC::setY: " << e.what());
+        ros::Duration(TIME_ERROR).sleep();
+        return BAD_CON; 
+    }  
+    catch(const boost::exception& e)
+    {
+        ROS_ERROR_STREAM(boost::diagnostic_information(e));
+    }
+    catch (...) 
+    {
+        ROS_ERROR_STREAM("Caught unknown exception");
+    } 
+    return 0;
+}
+
+int DELTA_NAMESPACE::PLC::mulWriteY(int start_number, int end_number, bool *buffer) {
+
+    try {
+        if(start_number >= end_number)                        { throw CompareException(); }
+        if(start_number < Y0_NUM || start_number > Y377_NUM)  { throw AddressIsNotExistException(start_number); }
+        if(end_number < Y0_NUM || end_number > Y377_NUM)      { throw AddressIsNotExistException(end_number); }
+        int amount = end_number - start_number + 1;
+        return mb_->modbus_write_coils(Y0_HEX + start_number, amount, buffer);
+    }
+    catch(CompareException & e) {
+        ROS_ERROR_STREAM("PLC::mulSetY: " << e.what());
+        ros::Duration(TIME_ERROR).sleep();
+        return BAD_CON;
+    } 
+    catch(AddressIsNotExistException & e) {
+        ROS_ERROR_STREAM("PLC::mulSetY: " << e.what());
+        ros::Duration(TIME_ERROR).sleep();
+        return BAD_CON; 
+    }
+    catch(std::exception& e) {
+        ROS_ERROR_STREAM("PLC::mulSetY: " << e.what());
+        ros::Duration(TIME_ERROR).sleep();
+        return BAD_CON; 
+    }
+    catch(const boost::exception& e)
+    {
+        ROS_ERROR_STREAM(boost::diagnostic_information(e));
+    }
+    catch (...) 
+    {
+        ROS_ERROR_STREAM("Caught unknown exception");
+    } 
+    return 0;
+}
+
+int DELTA_NAMESPACE::PLC::mulSetY(int start_number, int end_number) {
+    try {
+        if(start_number >= end_number)                        { throw CompareException(); }
+        if(start_number < Y0_NUM || start_number > Y377_NUM)  { throw AddressIsNotExistException(start_number); }
+        if(end_number < Y0_NUM || end_number > Y377_NUM)      { throw AddressIsNotExistException(end_number); }
+        int amount = end_number - start_number + 1;
+        bool value[amount];
+        for(int i = 0; i < amount; i++) value[i] = ON;
+        return mb_->modbus_write_coils(Y0_HEX + start_number, amount, value);
+    }
+    catch(CompareException & e) {
+        ROS_ERROR_STREAM("PLC::mulSetY: " << e.what());
+        ros::Duration(TIME_ERROR).sleep();
+        return BAD_CON;
+    } 
+    catch(AddressIsNotExistException & e) {
+        ROS_ERROR_STREAM("PLC::mulSetY: " << e.what());
+        ros::Duration(TIME_ERROR).sleep();
+        return BAD_CON; 
+    }
+    catch(std::exception& e) {
+        ROS_ERROR_STREAM("PLC::mulSetY: " << e.what());
+        ros::Duration(TIME_ERROR).sleep();
+        return BAD_CON; 
+    }
+    catch(const boost::exception& e)
+    {
+        ROS_ERROR_STREAM(boost::diagnostic_information(e));
+    }
+    catch (...) 
+    {
+        ROS_ERROR_STREAM("Caught unknown exception");
+    } 
+    return 0;
+}
+
+int DELTA_NAMESPACE::PLC::mulResetY(int start_number, int end_number) {
+    try {
+        if(start_number >= end_number)                        { throw CompareException(); }
+        if(start_number < Y0_NUM || start_number > Y377_NUM)  { throw AddressIsNotExistException(start_number); }
+        if(end_number < Y0_NUM || end_number > Y377_NUM)      { throw AddressIsNotExistException(end_number); }
+        int amount = end_number - start_number + 1;
+        bool value[amount];
+        for(int i = 0; i < amount; i++) value[i] = OFF;
+        return mb_->modbus_write_coils(Y0_HEX + start_number, amount, value);
+    }
+    catch(CompareException & e) {
+        ROS_ERROR_STREAM("PLC::mulSetY: " << e.what());
+        ros::Duration(TIME_ERROR).sleep();
+        return BAD_CON;
+    } 
+    catch(AddressIsNotExistException & e) {
+        ROS_ERROR_STREAM("PLC::mulSetY: " << e.what());
+        ros::Duration(TIME_ERROR).sleep();
+        return BAD_CON; 
+    }
+    catch(std::exception& e) {
+        ROS_ERROR_STREAM("PLC::mulSetY: " << e.what());
+        ros::Duration(TIME_ERROR).sleep();
+        return BAD_CON; 
+    }
+    catch(const boost::exception& e)
+    {
+        ROS_ERROR_STREAM(boost::diagnostic_information(e));
+    }
+    catch (...) 
+    {
+        ROS_ERROR_STREAM("Caught unknown exception");
+    } 
+    return 0;
+}
+
+int DELTA_NAMESPACE::PLC::mulGetY(int start_number, int end_number, bool *buffer) {
+    try {
+        if(start_number >= end_number)                        { throw CompareException(); }
+        if(start_number < Y0_NUM || start_number > Y377_NUM)  { throw AddressIsNotExistException(start_number); }
+        if(end_number < Y0_NUM || end_number > Y377_NUM)      { throw AddressIsNotExistException(end_number); }
+        int amount = end_number - start_number + 1;
+        return mb_->modbus_read_coils(Y0_HEX + start_number, amount, buffer);
+    }
+    catch(CompareException & e) {
+        ROS_ERROR_STREAM("PLC::mulGetY: " << e.what());
+        ros::Duration(TIME_ERROR).sleep();
+        return BAD_CON;
+    } 
+    catch(AddressIsNotExistException & e) {
+        ROS_ERROR_STREAM("PLC::mulGetY: " << e.what());
+        ros::Duration(TIME_ERROR).sleep();
+        return BAD_CON; 
+    }
+    catch(std::exception& e) {
+        ROS_ERROR_STREAM("PLC::mulGetY: " << e.what());
+        ros::Duration(TIME_ERROR).sleep();
+        return BAD_CON; 
+    }  
+    catch(const boost::exception& e)
+    {
+        ROS_ERROR_STREAM(boost::diagnostic_information(e));
+    }
+    catch (...) 
+    {
+        ROS_ERROR_STREAM("Caught unknown exception");
+    } 
+    return 0;
+}
+
+int DELTA_NAMESPACE::PLC::getD(int D_number, uint16_t &buffer) {
+    try 
+    {
+        if(D_number < D0_NUM || D_number > D4095_NUM) 
+            throw AddressIsNotExistException(D_number); 
+        return mb_->modbus_read_holding_register(D0_HEX + D_number, buffer);
+    }
+    catch(AddressIsNotExistException & e) {
+        ROS_ERROR_STREAM("PLC::getD: " << e.what());
+        ros::Duration(TIME_ERROR).sleep();
+        return BAD_CON; 
+    }
+    catch(std::exception& e) {
+        ROS_ERROR_STREAM("PLC::getD: " << e.what());
+        ros::Duration(TIME_ERROR).sleep();
+        return BAD_CON; 
+    }
+    catch(const boost::exception& e)
+    {
+        ROS_ERROR_STREAM(boost::diagnostic_information(e));
+    }
+    catch (...) 
+    {
+        ROS_ERROR_STREAM("Caught unknown exception");
+    } 
+    return 0;
+}
+
+int DELTA_NAMESPACE::PLC::mulGetD(int start_number, int end_number, uint16_t *buffer) {
+    try {
+        if(start_number >= end_number)                         { throw CompareException(); }
+        if(start_number < D0_NUM || start_number > D4095_NUM)  { throw AddressIsNotExistException(start_number); }
+        if(end_number < D0_NUM || end_number > D4095_NUM)      { throw AddressIsNotExistException(end_number); }
+        int amount = end_number - start_number + 1;
+        return mb_->modbus_read_holding_registers(D0_HEX + start_number, amount, buffer);
+    }
+    catch(CompareException & e) {
+        ROS_ERROR_STREAM("PLC::mulGetD: " << e.what());
+        ros::Duration(TIME_ERROR).sleep();
+        return BAD_CON;
+    } 
+    catch(AddressIsNotExistException & e) {
+        ROS_ERROR_STREAM("PLC::mulGetD: " << e.what());
+        ros::Duration(TIME_ERROR).sleep();
+        return BAD_CON; 
+    }
+    catch(std::exception& e) {
+        ROS_ERROR_STREAM("PLC::mulGetD: " << e.what());
+        ros::Duration(TIME_ERROR).sleep();
+        return BAD_CON; 
+    }
+    catch(const boost::exception& e)
+    {
+        ROS_ERROR_STREAM(boost::diagnostic_information(e));
+    }
+    catch (...) 
+    {
+        ROS_ERROR_STREAM("Caught unknown exception");
+    } 
+    return 0;
+}
+
+int DELTA_NAMESPACE::PLC::setD(int D_number, int16_t &buffer) {
+    try {
+        if(D_number < D0_NUM || D_number > D4095_NUM) { throw AddressIsNotExistException(D_number); }
+        return mb_->modbus_write_register(D0_HEX + D_number, buffer);
+    }
+    catch(AddressIsNotExistException & e) {
+        ROS_ERROR_STREAM("PLC::setD: " << e.what());
+        ros::Duration(TIME_ERROR).sleep();
+        return BAD_CON; 
+    }
+    catch(std::exception& e) {
+        ROS_ERROR_STREAM("PLC::setD: " << e.what());
+        ros::Duration(TIME_ERROR).sleep();
+        return BAD_CON; 
+    }
+    catch(const boost::exception& e)
+    {
+        ROS_ERROR_STREAM(boost::diagnostic_information(e));
+    }
+    catch (...) 
+    {
+        ROS_ERROR_STREAM("Caught unknown exception");
+    } 
+    return 0;
+}
+
+int DELTA_NAMESPACE::PLC::mulSetD(int start_number, int end_number, uint16_t *buffer) {
+    try {
+        if(start_number >= end_number)                         { throw CompareException(); }
+        if(start_number < D0_NUM || start_number > D4095_NUM)  { throw AddressIsNotExistException(start_number); }
+        if(end_number < D0_NUM || end_number > D4095_NUM)      { throw AddressIsNotExistException(end_number); }
+        int amount = end_number - start_number + 1;
+        return mb_->modbus_write_registers(D0_HEX + start_number, amount, buffer);
+    }
+    catch(CompareException & e) {
+        ROS_ERROR_STREAM("PLC::mulSetD: " << e.what());
+        ros::Duration(TIME_ERROR).sleep();
+        return BAD_CON;
+    } 
+    catch(AddressIsNotExistException & e) {
+        ROS_ERROR_STREAM("PLC::mulSetD: " << e.what());
+        ros::Duration(TIME_ERROR).sleep();
+        return BAD_CON; 
+    }
+    catch(std::exception& e) {
+        ROS_ERROR_STREAM("PLC::mulSetD: " << e.what());
+        ros::Duration(TIME_ERROR).sleep();
+        return BAD_CON; 
+    }
+    catch(const boost::exception& e)
+    {
+        ROS_ERROR_STREAM(boost::diagnostic_information(e));
+    }
+    catch (...) 
+    {
+        ROS_ERROR_STREAM("Caught unknown exception");
+    } 
+    return 0;
+}
+
+int DELTA_NAMESPACE::PLC::getX(int X_number, bool &buffer) {
+    try {
+        if(X_number < X0_NUM || X_number > X377_NUM) { throw AddressIsNotExistException(X_number); }
+        mb_->modbus_read_input_bit(X0_HEX + X_number, buffer);
+    }
+    catch(AddressIsNotExistException & e) {
+        ROS_ERROR_STREAM("PLC::getX: " << e.what());
+        ros::Duration(TIME_ERROR).sleep();
+        return BAD_CON; 
+    }
+    catch(std::exception& e) {
+        ROS_ERROR_STREAM("PLC::getX: " << e.what());
+        ros::Duration(TIME_ERROR).sleep();
+        return BAD_CON; 
+    }
+    return 0;
+}
+
+/*
+ * Read X bit
+ * @param start_number  Begin number X
+ * @param end_number    End number X
+ * @param buffer        Status of X
+ */
+int DELTA_NAMESPACE::PLC::mulGetX(int start_number, int end_number, bool* buffer) {
+    try {
+        if(start_number >= end_number)                          { throw CompareException(); }
+        if(start_number < X0_NUM || start_number > X377_NUM)    { throw AddressIsNotExistException(start_number); }
+        if(end_number < X0_NUM || end_number > X377_NUM)        { throw AddressIsNotExistException(end_number); }
+        int amount = end_number - start_number + 1;
+        mb_->modbus_read_input_bits(X0_HEX + start_number, amount, buffer);
+    }
+    catch(CompareException & e) {
+        ROS_ERROR_STREAM("PLC::mulGetX: " << e.what());
+        ros::Duration(TIME_ERROR).sleep();
+        return BAD_CON;
+    } 
+    catch(AddressIsNotExistException & e) {
+        ROS_ERROR_STREAM("PLC::mulGetX: " << e.what());
+        ros::Duration(TIME_ERROR).sleep();
+        return BAD_CON; 
+    }
+    catch(std::exception& e) {
+        ROS_ERROR_STREAM("PLC::mulGetX: " << e.what());
+        ros::Duration(TIME_ERROR).sleep();
+        return BAD_CON; 
+    }
+    catch(const boost::exception& e)
+    {
+        ROS_ERROR_STREAM(boost::diagnostic_information(e));
+    }
+    catch (...) 
+    {
+        ROS_ERROR_STREAM("Caught unknown exception");
+    } 
+    return 0;
+}
+
+int DELTA_NAMESPACE::PLC::getC(int C_number, uint16_t &buffer) {
+    try {
+        if(C_number < C0_NUM || C_number > C199_NUM) { throw AddressIsNotExistException(C_number); }
+        mb_->modbus_read_holding_register(C0_HEX + C_number, buffer);
+    }
+    catch(AddressIsNotExistException & e) {
+        ROS_ERROR_STREAM("PLC::getC: " << e.what());
+        ros::Duration(TIME_ERROR).sleep();
+        return BAD_CON; 
+    }
+    catch(std::exception& e) {
+        ROS_ERROR_STREAM("PLC::getC: " << e.what());
+        ros::Duration(TIME_ERROR).sleep();
+        return BAD_CON; 
+    }
+    catch(const boost::exception& e)
+    {
+        ROS_ERROR_STREAM(boost::diagnostic_information(e));
+    }
+    catch (...) 
+    {
+        ROS_ERROR_STREAM("Caught unknown exception");
+    } 
+    return 0;
+}
+   
+int DELTA_NAMESPACE::PLC::mulGetC(int start_number, int end_number, uint16_t *buffer) {
+    try {
+        if(start_number >= end_number)                          { throw CompareException(); }
+        if(start_number < C0_NUM || start_number > C199_NUM)    { throw AddressIsNotExistException(start_number); }
+        if(end_number < C0_NUM || end_number > C199_NUM)        { throw AddressIsNotExistException(end_number); }
+        int amount = end_number - start_number + 1;
+        mb_->modbus_read_holding_registers(C0_HEX + start_number, amount, buffer);
+    }
+    catch(CompareException & e) {
+        ROS_ERROR_STREAM("PLC::mulGetC: " << e.what());
+        ros::Duration(TIME_ERROR).sleep();
+        return BAD_CON;
+    } 
+    catch(AddressIsNotExistException & e) {
+        ROS_ERROR_STREAM("PLC::mulGetC: " << e.what());
+        ros::Duration(TIME_ERROR).sleep();
+        return BAD_CON; 
+    }
+    catch(std::exception& e) {
+        ROS_ERROR_STREAM("PLC::mulGetC: " << e.what());
+        ros::Duration(TIME_ERROR).sleep();
+        return BAD_CON; 
+    } 
+    catch(const boost::exception& e)
+    {
+        ROS_ERROR_STREAM(boost::diagnostic_information(e));
+    }
+    catch (...) 
+    {
+        ROS_ERROR_STREAM("Caught unknown exception");
+    } 
+    return 0;
+}
+
+

Devices/Libraries/Ros/delta_modbus/test/test_delta.cpp

@@ -0,0 +1,34 @@
+#include <ros/ros.h>
+#include <boost/thread.hpp>
+#include "delta_modbus/delta_modbus_tcp.h"
+#include <diagnostic_msgs/DiagnosticArray.h>
+
+
+/**********************************************************************
+ *                   		MAIN 
+***********************************************************************/
+int main(int argc, char** argv) {
+    ros::init(argc, argv, "test_delta_modbus_tcp");
+    ros::NodeHandle nh("~");
+
+    std::string node_name = ros::this_node::getName();
+    ROS_INFO("node_name: %s", node_name.c_str());
+
+    DELTA_NAMESPACE::PLC *plc = NULL;
+
+    std::string IP = "192.168.1.5";
+    int PORT = 502;
+
+    plc = new DELTA_NAMESPACE::PLC(IP, PORT);
+    plc->connect();
+    
+    uint16_t D101;
+    bool M[2];
+    plc->getD(1, D101); plc->mulGetM(1, 2,M);
+    ROS_INFO("D1 %d M1 %x %x", D101, M[0], M[1]);
+
+    ros::spin();  
+    plc = NULL;  
+    delete(plc);
+    return 0;
+}

Devices/Libraries/Ros/libserial/.gitignore

@@ -0,0 +1,14 @@
+# ---> VisualStudioCode
+.vscode/*
+!.vscode/settings.json
+!.vscode/tasks.json
+!.vscode/launch.json
+!.vscode/extensions.json
+!.vscode/*.code-snippets
+
+# Local History for Visual Studio Code
+.history/
+
+# Built Visual Studio Code Extensions
+*.vsix
+

Devices/Libraries/Ros/libserial/CMakeLists.txt

@@ -0,0 +1,45 @@
+cmake_minimum_required(VERSION 3.0.2)
+project(libserial)
+
+find_package(catkin REQUIRED COMPONENTS
+  roscpp
+)
+
+catkin_package(
+  INCLUDE_DIRS include
+  LIBRARIES ${PROJECT_NAME}
+  CATKIN_DEPENDS roscpp
+#  DEPENDS other non-ROS libs
+)
+include_directories(
+  include
+  ${Boost_INCLUDE_DIRS}
+  ${catkin_INCLUDE_DIRS}
+  ${console_bridge_INCLUDE_DIRS}
+)
+
+#modbustcp
+set(YOUR_LIB_SOURCES
+    src/serial.cpp
+    src/rs485.cpp
+)
+add_library(${PROJECT_NAME} ${YOUR_LIB_SOURCES})
+SET(-ludev udev)
+target_link_libraries(${PROJECT_NAME} 
+                      ${catkin_LIBRARIES}
+                      ${Boost_LIBRARIES}
+                      ${GSTREAMER_LIBRARIES}
+                      ${-ludev}
+)
+
+install(TARGETS ${PROJECT_NAME}
+  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
+)
+
+install(DIRECTORY include/${PROJECT_NAME}/
+  DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+  FILES_MATCHING PATTERN "*.h"
+  PATTERN ".svn" EXCLUDE
+)

Devices/Libraries/Ros/libserial/README.md

@@ -0,0 +1,2 @@
+# libserial
+

Devices/Libraries/Ros/libserial/include/libserial/define.h

@@ -0,0 +1,43 @@
+#pragma once
+
+#define OFF 0
+#define ON 1
+#define MAX_MSG_LENGTH 2048
+
+typedef enum parity {
+	PARITY_NONE,
+	PARITY_EVEN,
+	PARITY_ODD
+} parity_t;
+
+typedef enum stop_bits {
+	STOPBIT_1,
+	STOPBIT_2
+} stop_bits_t;
+
+typedef enum data_bits {
+    DATABIT_5,
+    DATABIT_6,
+	DATABIT_7,
+    DATABIT_8
+} data_bits_t;
+
+struct _rs485 {
+    /* Socket or file descriptor */
+    int s;
+    /* Device: "/dev/ttyS0", "/dev/ttyUSB0" or "/dev/tty.USA19*" on Mac OS X. */
+    const char* _port;
+    /* Bauds: 9600, 19200, 57600, 115200, etc */
+    unsigned int _baud;
+    /* Data bit */
+    data_bits_t _data_bit;
+    /* Stop bit */
+    stop_bits_t _stop_bit;
+    /* Parity:*/
+    parity_t _parity;
+    /* Save old termios settings */
+    struct termios _old_tios;
+};
+
+
+

Devices/Libraries/Ros/libserial/include/libserial/rs485.h

@@ -0,0 +1,88 @@
+//
+// Created by Hiep  on 5/12/2020.
+//
+#pragma once
+#include <ros/ros.h>
+#include "serial.h"
+
+typedef struct _rs485 rs485_t;
+
+class rs485: public Serial
+{
+private:  
+    /**
+     * Configure
+     * @param devfile   Path name of dev  
+     * @param baud      Baudrate 
+     * @param parity    Parity
+     * @param data_bit
+     * @param stop_bit 
+     */
+    virtual int _rs485_connect(rs485_t &ctx); 
+
+    /**
+     * create new port
+     * @param devfile   Path name of dev  
+     * @param baud      Baudrate 
+     * @param parity    Parity
+     * @param data_bit
+     * @param stop_bit 
+     */
+    virtual void new_port(const char* devfile, unsigned int baud, parity_t parity, 
+                                        data_bits_t data_bit,stop_bits_t stop_bit);
+    
+public:
+    /**
+     * Data Sender
+     * @param to_send Request to Be Sent to Server
+     * @param length  Length of the Request
+     * @return        Size of the request
+     */
+    virtual ssize_t sendMsgs(uint8_t *to_send, uint16_t length);
+
+    /**
+     * Data Receiver
+     * @param buffer Buffer to Store the Data` Retrieved
+     * @return       Size of Incoming Data
+     */
+    virtual ssize_t receiveMsgs(uint8_t *buffer) const;
+
+    /*
+     *  Reconnect to device if rs485 modules was disconected.
+     */
+    virtual void reconnect(void);
+
+    /*
+     * Set read character miximum
+     * @param[in] num The character minximum to receive.
+     */
+    virtual int setCharacterNumToRead(int num);
+
+    /**
+     * Close serial port
+     */
+    virtual void close_port();
+
+    /**
+     * @brief Constructor 
+     * @param[in] devfile   Example /dev/tty* 
+     * @param[in] baud      Number of transmitted bits  per a second
+     * @param[in] parity    The parity check bit {Even, None , Old }
+     * @param[in] data_bit  Number of bits in a transmission frame 
+     * @param[in] stop_bit  End bit
+     */
+    rs485(const char* devfile, unsigned int baud, parity_t parity, 
+                                        data_bits_t data_bit,stop_bits_t stop_bit);
+    /**
+     * Destructor.
+     */
+    virtual ~rs485();
+/* Properties */
+protected:
+    rs485_t ctx;
+    uint8_t size_pkg;
+    float param_safety;
+public:
+    bool _connected{};      /* Write status connection of devices */
+    std::string error_msg;  /* To saved error message */
+};

Devices/Libraries/Ros/libserial/include/libserial/serial.h

@@ -0,0 +1,44 @@
+//
+// Created by Hiep  on 4/12/2020.
+//
+#pragma once 
+// #include <bits/stdc++.h>
+#include <iostream>  /* std::cout */
+#include <stdio.h>   /* Standard input/output definitions */
+#include <string.h>  /* String function definitions */
+#include <unistd.h>  /* UNIX standard function definitions */
+#include <fcntl.h>   /* File control definitions */
+#include <errno.h>   /* Error number definitions */
+#include <termios.h> /* POSIX terminal control definitions */
+#include <cstdint>	/* uin8_t */
+#include <stdlib.h>  
+#include <sys/stat.h>
+#include "define.h"
+
+class Serial
+{
+public:
+   /*
+    * @brief Setting baudrate
+    * @param[in] b baud speed input
+    */
+   virtual speed_t setBaudRate(unsigned int b);
+
+   /*
+    * @brief Setting parity bits type
+    * @param[in] b baud speed input
+    */
+   virtual void setParity(termios &tios, parity p);
+
+   /*
+    * @brief Setting stop bits type
+    * @param[in] b baud speed input
+    */
+   virtual void setStopBits(termios &tios, stop_bits s);
+
+   /*
+    * @brief Setting data bits type
+    * @param[in] b baud speed input
+    */
+   virtual void setDataBits(termios &tios, data_bits d);
+};

Devices/Libraries/Ros/libserial/include/libserial/udevadm_info.h

@@ -0,0 +1,119 @@
+#ifndef    __UDEVADM_INFO_H_INCLUDE__
+#define    __UDEVADM_INFO_H_INCLUDE__
+
+#include <ros/ros.h>
+#include <unistd.h>
+#include <poll.h>
+#include <cstdio>
+#include <cstdlib>
+#include <cerrno>
+#include <cstring>
+#include <sys/signalfd.h>
+#include <csignal>
+#include <libudev.h>
+#include <string>
+
+class udevadmInfo {
+public:
+    udevadmInfo(const char *product_name);
+    virtual ~udevadmInfo(void);
+    virtual int init();
+    virtual bool scanDevices(void);
+    const char *port;
+protected:
+    struct udev *udev;
+    char *product_name_;
+};
+
+udevadmInfo::udevadmInfo(const char *product_name)
+ : product_name_((char *)product_name)
+{
+    
+}
+
+udevadmInfo::~udevadmInfo(void){
+    udev_unref(udev);
+}
+
+int udevadmInfo::init() {
+    udev = udev_new();
+    if (!udev) {
+        printf("Error while initialization!\n");
+        return EXIT_FAILURE;
+    }
+    sigset_t mask;
+
+    // Set signals we want to catch
+    sigemptyset(&mask);
+    sigaddset(&mask, SIGTERM);
+    sigaddset(&mask, SIGINT);
+
+    // Change the signal mask and check
+    if (sigprocmask(SIG_BLOCK, &mask, nullptr) < 0) {
+        ROS_ERROR("UDEV-Error while sigprocmask(): %s\n", std::strerror(errno));
+        return EXIT_FAILURE;
+    }
+    // Get a signal file descriptor
+    int signal_fd = signalfd(-1, &mask, 0);
+    // Check the signal file descriptor
+    if (signal_fd < 0) {
+        ROS_ERROR("UDEV-Error while signalfd(): %s\n", std::strerror(errno));
+        return EXIT_FAILURE;
+    }
+    return EXIT_SUCCESS;
+}
+
+bool udevadmInfo::scanDevices(void) {
+    struct udev_device *device;
+    struct udev_enumerate *enumerate;
+    struct udev_list_entry *devices, *dev_list_entry;
+
+    // Create enumerate object
+    enumerate = udev_enumerate_new(udev);
+    if (!enumerate) {
+        ROS_ERROR("Error while creating udev enumerate\n");
+        return false;
+    }
+
+    // Scan devices
+    udev_enumerate_scan_devices(enumerate);
+
+    // Fill up device list
+    devices = udev_enumerate_get_list_entry(enumerate);
+    if (!devices) {
+        ROS_ERROR("Error while getting device list\n");
+        return false;
+    }
+
+    udev_list_entry_foreach(dev_list_entry, devices) {
+        const char* path = udev_list_entry_get_name(dev_list_entry);
+        // Get the device
+        device = udev_device_new_from_syspath(udev, udev_list_entry_get_name(dev_list_entry));
+        // Print device information
+        if(udev_device_get_devnode(device) != NULL && strstr(udev_device_get_sysname(device),"ttyUSB") != NULL) {
+            // ROS_INFO("DEVNODE=%s", udev_device_get_devnode(device));
+            struct udev_device* usb = udev_device_get_parent_with_subsystem_devtype(device,"usb","usb_device");
+                // ROS_INFO("usb = %s:%s, manufacturer = %s, product = %s, serial = %s, speed = %s, bMaxPower = %s",
+                //         udev_device_get_sysattr_value(usb, "idVendor"),
+                //         udev_device_get_sysattr_value(usb, "idProduct"),
+                //         udev_device_get_sysattr_value(usb, "manufacturer"),
+                //         udev_device_get_sysattr_value(usb, "product"),
+                //         udev_device_get_sysattr_value(usb, "serial"),
+                //         udev_device_get_sysattr_value(usb, "speed"),
+                //         udev_device_get_sysattr_value(usb, "bMaxPower"));
+                if(strstr(udev_device_get_sysattr_value(usb, "product"), (const char *)product_name_) != NULL) {
+                    port = udev_device_get_devnode(device);
+                    return true;
+                }
+        }
+        else {
+            continue;
+        }
+        // Free the device
+        udev_device_unref(device);
+    }
+    // Free enumerate
+    udev_enumerate_unref(enumerate);
+    return false;
+}
+#endif

Devices/Libraries/Ros/libserial/package.xml

@@ -0,0 +1,62 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>libserial</name>
+  <version>0.0.0</version>
+  <description>The libserial package</description>
+
+  <!-- One maintainer tag required, multiple allowed, one person per tag -->
+  <!-- Example:  -->
+  <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+  <maintainer email="robotics@todo.todo">robotics</maintainer>
+
+
+  <!-- One license tag required, multiple allowed, one license per tag -->
+  <!-- Commonly used license strings: -->
+  <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
+  <license>TODO</license>
+
+
+  <!-- Url tags are optional, but multiple are allowed, one per tag -->
+  <!-- Optional attribute type can be: website, bugtracker, or repository -->
+  <!-- Example: -->
+  <!-- <url type="website">http://wiki.ros.org/libserial</url> -->
+
+
+  <!-- Author tags are optional, multiple are allowed, one per tag -->
+  <!-- Authors do not have to be maintainers, but could be -->
+  <!-- Example: -->
+  <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
+
+
+  <!-- The *depend tags are used to specify dependencies -->
+  <!-- Dependencies can be catkin packages or system dependencies -->
+  <!-- Examples: -->
+  <!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
+  <!--   <depend>roscpp</depend> -->
+  <!--   Note that this is equivalent to the following: -->
+  <!--   <build_depend>roscpp</build_depend> -->
+  <!--   <exec_depend>roscpp</exec_depend> -->
+  <!-- Use build_depend for packages you need at compile time: -->
+  <!--   <build_depend>message_generation</build_depend> -->
+  <!-- Use build_export_depend for packages you need in order to build against this package: -->
+  <!--   <build_export_depend>message_generation</build_export_depend> -->
+  <!-- Use buildtool_depend for build tool packages: -->
+  <!--   <buildtool_depend>catkin</buildtool_depend> -->
+  <!-- Use exec_depend for packages you need at runtime: -->
+  <!--   <exec_depend>message_runtime</exec_depend> -->
+  <!-- Use test_depend for packages you need only for testing: -->
+  <!--   <test_depend>gtest</test_depend> -->
+  <!-- Use doc_depend for packages you need only for building documentation: -->
+  <!--   <doc_depend>doxygen</doc_depend> -->
+  <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>roscpp</build_depend>
+  <build_export_depend>roscpp</build_export_depend>
+  <exec_depend>roscpp</exec_depend>
+
+
+  <!-- The export tag contains other, unspecified, tags -->
+  <export>
+    <!-- Other tools can request additional information be placed here -->
+
+  </export>
+</package>

Devices/Libraries/Ros/libserial/src/rs485.cpp

@@ -0,0 +1,309 @@
+#include "libserial/rs485.h"
+
+/*
+ * Configure
+ * @param devfile   Path name of dev  
+ * @param baud      Baudrate 
+ * @param parity    Parity
+ * @param data_bit
+ * @param stop_bit 
+ */
+int rs485::_rs485_connect(rs485_t &ctx)
+{
+    struct termios tios;
+    int flags;
+    /* The O_NOCTTY flag tells UNIX that this program doesn't want
+       to be the "controlling terminal" for that port. If you
+       don't specify this then any input (such as keyboard abort
+       signals and so forth) will affect your process
+       Timeouts are ignored in canonical input mode or when the
+       NDELAY option is set on the file via open or fcntl 
+	*/
+    flags = O_RDWR | O_NOCTTY | O_NONBLOCK | O_NDELAY;
+    ctx.s = open(ctx._port,flags);       
+    if (ctx.s < 0) 
+    {
+        std::stringstream e;
+        e << ctx._port <<" is failed: " << strerror(errno);
+        error_msg = e.str();
+        _connected = false;
+        return -1;
+    }
+    else
+    {
+        fcntl(ctx.s, F_SETFL, 0);
+    }
+    /* Save */
+    if(tcgetattr(ctx.s, &ctx._old_tios) <0)
+    {
+        ROS_ERROR("Can't get terminal parameters");
+    }
+
+    /*
+    * Enable the receiver and set local mode...
+    */
+    bzero(&tios, sizeof(tios));
+    
+    /*  C_ISPEED     Input baud (new interface)
+     *  C_OSPEED     Output baud (new interface)
+     */
+    speed_t speed = setBaudRate(ctx._baud);
+    /* Set the baud rate */
+    if ((cfsetispeed(&tios, speed) < 0) ||
+        (cfsetospeed(&tios, speed) < 0)) 
+    {
+        close(ctx.s);
+        ctx.s = -1;
+        _connected = false;
+        return -1;
+    }
+    setDataBits(tios,ctx._data_bit);
+    setStopBits(tios,ctx._stop_bit);
+    setParity(tios, ctx._parity);
+    
+    /* Read the man page of termios if you need more information.
+     * This field isn't used on POSIX systems
+     * tios.c_line = 0;
+     */
+
+    /* C_LFLAG      Line options
+       ISIG Enable SIGINTR, SIGSUSP, SIGDSUSP, and SIGQUIT signals
+       ICANON       Enable canonical input (else raw)
+       XCASE        Map uppercase \lowercase (obsolete)
+       ECHO Enable echoing of input characters
+       ECHOE        Echo erase character as BS-SP-BS
+       ECHOK        Echo NL after kill character
+       ECHONL       Echo NL
+       NOFLSH       Disable flushing of input buffers after
+       interrupt or quit characters
+       IEXTEN       Enable extended functions
+       ECHOCTL      Echo control characters as ^char and delete as ~?
+       ECHOPRT      Echo erased character as character erased
+       ECHOKE       BS-SP-BS entire line on line kill
+       FLUSHO       Output being flushed
+       PENDIN       Retype pending input at next read or input char
+       TOSTOP       Send SIGTTOU for background output
+       Canonical input is line-oriented. Input characters are put
+       into a buffer which can be edited interactively by the user
+       until a CR (carriage return) or LF (line feed) character is
+       received.
+       Raw input is unprocessed. Input characters are passed
+       through exactly as they are received, when they are
+       received. Generally you'll deselect the ICANON, ECHO,
+       ECHOE, and ISIG options when using raw input
+    */
+
+    /* Raw input */
+    tios.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG);;
+
+    /* Software flow control is disabled */
+    tios.c_iflag &= ~(IXON | IXOFF | IXANY);
+
+    /* C_OFLAG      Output options
+       OPOST        Postprocess output (not set = raw output)
+       ONLCR        Map NL to CR-NL
+       ONCLR ant others needs OPOST to be enabled
+    */
+
+    /* Raw ouput */
+    tios.c_oflag = 0;
+
+    /* C_CC         Control characters
+       VMIN         Minimum number of characters to read
+       VTIME        Time to wait for data (tenths of seconds)
+       UNIX serial interface drivers provide the ability to
+       specify character and packet timeouts. Two elements of the
+       c_cc array are used for timeouts: VMIN and VTIME. Timeouts
+       are ignored in canonical input mode or when the NDELAY
+       option is set on the file via open or fcntl.
+       VMIN specifies the minimum number of characters to read. If
+       it is set to 0, then the VTIME value specifies the time to
+       wait for every character read. Note that this does not mean
+       that a read call for N bytes will wait for N characters to
+       come in. Rather, the timeout will apply to the first
+       character and the read call will return the number of
+       characters immediately available (up to the number you
+       request).
+       If VMIN is non-zero, VTIME specifies the time to wait for
+       the first character read. If a character is read within the
+       time given, any read will block (wait) until all VMIN
+       characters are read. That is, once the first character is
+       read, the serial interface driver expects to receive an
+       entire packet of characters (VMIN bytes total). If no
+       character is read within the time allowed, then the call to
+       read returns 0. This method allows you to tell the serial
+       driver you need exactly N bytes and any read call will
+       return 0 or N bytes. However, the timeout only applies to
+       the first character read, so if for some reason the driver
+       misses one character inside the N byte packet then the read
+       call could block forever waiting for additional input
+       characters.
+       VTIME specifies the amount of time to wait for incoming
+       characters in tenths of seconds. If VTIME is set to 0 (the
+       default), reads will block (wait) indefinitely unless the
+       NDELAY option is set on the port with open or fcntl.
+    */
+    /* Unused because we use open with the NDELAY option */
+    tios.c_cc[VMIN] = 5;        /* The character minximum to read*/
+    tios.c_cc[VTIME] = 20;      /* The wait time maximum to read*/
+    /* clean port */
+  	tcflush(ctx.s, TCIFLUSH);
+    /* activate the settings port */
+    if (tcsetattr(ctx.s, TCSANOW, &tios) < 0) {
+        ROS_ERROR("Can't get terminal parameters");
+        close(ctx.s);
+        ctx.s = -1;
+        _connected = false;
+        return -1;
+    }
+    ctx._old_tios = tios;
+    _connected = true;
+    return 0;
+}
+
+/*
+ *  Reconnect to device if rs485 modules was disconected.
+ */
+void rs485::reconnect(void)
+{
+    _rs485_connect(ctx);
+}
+
+/*
+ * create new port
+ * @param devfile   Path name of dev  
+ * @param baud      Baudrate 
+ * @param parity    Parity
+ * @param data_bit
+ * @param stop_bit 
+ */
+void rs485::new_port(const char* devfile,unsigned int baud, parity_t parity, 
+                                        data_bits_t data_bit,stop_bits_t stop_bit)
+{
+    ctx._port = devfile;
+    ctx._baud = baud;
+    ctx._parity = parity;
+    ctx._data_bit = data_bit;
+    ctx._stop_bit = stop_bit;
+    _connected = false;
+    if(parity == PARITY_EVEN || parity == PARITY_ODD)
+        size_pkg++;
+    switch(stop_bit)
+    {
+        case STOPBIT_1:
+            size_pkg++;
+            break;
+        case STOPBIT_2:
+            size_pkg +=2;
+            break;
+        default:
+            break;
+    }
+
+    switch(data_bit)
+    {
+        case DATABIT_5:
+            size_pkg += 5;
+            break;
+        case DATABIT_6:
+            size_pkg += 6;
+            break;
+        case DATABIT_7:
+            size_pkg += 7;
+            break;
+        case DATABIT_8:
+            size_pkg += 8;
+            break;
+        default:
+            break;
+    }
+    size_pkg +=2;  // bit start and stop
+
+    switch(baud)
+    {
+    case 19200:
+        param_safety = 1.5;
+        break;
+    case 115200:
+        param_safety = 3.5;
+        break;
+    default:
+        param_safety = 1.5;
+        break;
+    }
+}
+
+/*
+ * Close serial port
+ */
+void rs485::close_port()
+{
+    close(this->ctx.s);
+}
+
+/*
+ * Data Sender
+ * @param to_send Request to Be Sent to Server
+ * @param length  Length of the Request
+ * @return        Size of the request
+ */
+ssize_t rs485::sendMsgs(uint8_t *to_send, uint16_t length)
+{
+    float time = param_safety*1000000/(ctx._baud / (size_pkg * length));
+    usleep(time);
+    memset((to_send + length),'\0', 1);
+    ssize_t num = write(ctx.s, to_send, (size_t)length);
+    usleep(time);
+    return num;
+}
+
+/*
+ * Data Receiver
+ * @param buffer Buffer to Store the Data` Retrieved
+ * @return       Size of Incoming Data
+ */
+ssize_t rs485::receiveMsgs(uint8_t *buffer) const 
+{
+    memset(buffer,'\0',sizeof(buffer));
+    return read(ctx.s, (char *) buffer,MAX_MSG_LENGTH);
+}
+
+/*
+ * @brief Constructor 
+ * @param[in] devfile   Example /dev/tty* 
+ * @param[in] baud      Number of transmitted bits  per a second
+ * @param[in] parity    The parity check bit {Even, None , Old }
+ * @param[in] data_bit  Number of bits in a transmission frame 
+ * @param[in] stop_bit  End bit
+ */
+rs485::rs485(const char* devfile, unsigned int baud, parity_t parity, data_bits_t data_bit,stop_bits_t stop_bit)
+{
+    new_port(devfile, baud, parity, data_bit, stop_bit);
+    _rs485_connect(ctx);
+}
+
+/*
+ * Destructor.
+ */
+rs485::~rs485()
+{
+   
+}
+
+/*
+ * Set read character miximum
+ * @param[in] num The character minximum to receive.
+ */
+int rs485::setCharacterNumToRead(int num)
+{
+    ctx._old_tios.c_cc[VMIN] = num;
+    /* clean port */
+    tcflush(ctx.s, TCIFLUSH);
+    /* activate the settings port */
+    if (tcsetattr(ctx.s, TCSANOW, &ctx._old_tios) < 0) {
+        ROS_ERROR("Can't get terminal parameters");
+        close(ctx.s);
+        ctx.s = -1;
+    }
+    return 0;
+}

Devices/Libraries/Ros/libserial/src/serial.cpp

@@ -0,0 +1,181 @@
+#include "libserial/serial.h"
+
+/*
+ * @brief Setting baudrate
+ * @param[in] b baud speed input
+ */
+speed_t Serial::setBaudRate(unsigned int b) {
+  speed_t speed;
+  switch (b) {
+  case 110:
+    speed = B110;
+    break;
+  case 300:
+    speed = B300;
+    break;
+  case 600:
+    speed = B600;
+    break;
+  case 1200:
+    speed = B1200;
+    break;
+  case 2400:
+    speed = B2400;
+    break;
+  case 4800:
+    speed = B4800;
+    break;
+  case 9600:
+    speed = B9600;
+    break;
+  case 19200:
+    speed = B19200;
+    break;
+  case 8400:
+    speed = B38400;
+    break;
+  case 57600:
+    speed = B57600;
+    break;
+  case 115200:
+    speed = B115200;
+    break;
+  case 230400:
+    speed = B230400;
+    break;
+  case 460800:
+    speed = B460800;
+    break;
+  case 500000:
+    speed = B500000;
+    break;
+  case 576000:
+    speed = B576000;
+    break;
+  case 921600:
+    speed = B921600;
+    break;
+  case 1000000:
+    speed = B1000000;
+    break;
+  case 1152000:
+    speed = B1152000;
+    break;
+  case 1500000:
+    speed = B1500000;
+    break;
+  case 2500000:
+    speed = B2500000;
+    break;
+  case 3000000:
+    speed = B3000000;
+    break;
+  case 3500000:
+    speed = B3500000;
+    break;
+  case 4000000:
+    speed = B4000000;
+    break;
+  default:
+    speed = B9600;
+    break;
+  }
+  return speed;
+}
+
+/*
+ * @brief Setting parity bits type
+ * @param[in] b baud speed input
+ */
+void Serial::setParity(termios& tios, parity p) {
+  /* PARENB       Enable parity bit
+  PARODD       Use odd parity instead of even */
+  switch (p)
+  {
+    /* None */
+  case PARITY_NONE:
+    tios.c_cflag &= ~PARENB;
+    tios.c_iflag &= ~INPCK;
+    break;
+    /* Even */
+  case PARITY_EVEN:
+    tios.c_cflag |= PARENB;
+    tios.c_cflag &= ~PARODD;
+    tios.c_iflag |= INPCK;
+    break;
+    /* Odd */
+  case PARITY_ODD:
+    tios.c_cflag |= PARENB;
+    tios.c_cflag |= PARODD;
+    break;
+    /* Default None */
+  default:
+    tios.c_cflag &= ~PARENB;
+    tios.c_iflag &= ~INPCK;
+    break;
+  }
+}
+
+/*
+ * @brief Setting stop bits type
+ * @param[in] b baud speed input
+ */
+void Serial::setStopBits(termios& tios, stop_bits s) {
+  /* Stop bit (1 or 2) */
+  switch (s)
+  {
+  case STOPBIT_1:
+    tios.c_cflag &= ~CSTOPB;
+    break;
+
+  case STOPBIT_2:
+    tios.c_cflag |= CSTOPB;
+    break;
+
+  default:
+    tios.c_cflag &= ~CSTOPB;
+    break;
+  }
+}
+
+/*
+ * @brief Setting data bits type
+ * @param[in] b baud speed input
+ */
+void Serial::setDataBits(termios& tios, data_bits d)
+{
+  /* C_CFLAG   Control options
+   * CLOCAL    Local line - do not change "owner" of port
+   * CREAD     Enable receiver
+   */
+  tios.c_cflag |= (CREAD | CLOCAL);
+  /* CSIZE, HUPCL, CRTSCTS (hardware flow control) */
+
+  /* Set data bits (5, 6, 7, 8 bits)
+   * CSIZE         Bit mask for data bits
+   */
+  tios.c_cflag &= ~CSIZE;
+  switch (d)
+  {
+  case DATABIT_8:
+    tios.c_cflag |= CS8;
+    break;
+
+  case DATABIT_7:
+    tios.c_cflag |= CS7;
+    break;
+
+  case DATABIT_6:
+    tios.c_cflag |= CS6;
+    break;
+
+  case DATABIT_5:
+    tios.c_cflag |= CS5;
+    break;
+
+  default:
+    tios.c_cflag |= CS8;
+    break;
+  }
+}
+

Devices/Libraries/Ros/md_controller/CMakeLists.txt

@@ -0,0 +1,229 @@
+cmake_minimum_required(VERSION 3.0.2)
+project(md_controller)
+
+## Compile as C++17, supported in ROS Kinetic and newer
+add_compile_options(-std=c++17)
+
+## Find catkin macros and libraries
+## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
+## is used, also find other catkin packages
+find_package(catkin REQUIRED COMPONENTS
+  roscpp
+  rospy
+  std_msgs
+  libserial
+)
+
+## System dependencies are found with CMake's conventions
+find_package(Boost REQUIRED COMPONENTS system)
+
+
+## Uncomment this if the package has a setup.py. This macro ensures
+## modules and global scripts declared therein get installed
+## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
+# catkin_python_setup()
+
+################################################
+## Declare ROS messages, services and actions ##
+################################################
+
+## To declare and build messages, services or actions from within this
+## package, follow these steps:
+## * Let MSG_DEP_SET be the set of packages whose message types you use in
+##   your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
+## * In the file package.xml:
+##   * add a build_depend tag for "message_generation"
+##   * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET
+##   * If MSG_DEP_SET isn't empty the following dependency has been pulled in
+##     but can be declared for certainty nonetheless:
+##     * add a exec_depend tag for "message_runtime"
+## * In this file (CMakeLists.txt):
+##   * add "message_generation" and every package in MSG_DEP_SET to
+##     find_package(catkin REQUIRED COMPONENTS ...)
+##   * add "message_runtime" and every package in MSG_DEP_SET to
+##     catkin_package(CATKIN_DEPENDS ...)
+##   * uncomment the add_*_files sections below as needed
+##     and list every .msg/.srv/.action file to be processed
+##   * uncomment the generate_messages entry below
+##   * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
+
+## Generate messages in the 'msg' folder
+# add_message_files(
+#   FILES
+#   Message1.msg
+#   Message2.msg
+# )
+
+## Generate services in the 'srv' folder
+# add_service_files(
+#   FILES
+#   Service1.srv
+#   Service2.srv
+# )
+
+## Generate actions in the 'action' folder
+# add_action_files(
+#   FILES
+#   Action1.action
+#   Action2.action
+# )
+
+## Generate added messages and services with any dependencies listed here
+# generate_messages(
+#   DEPENDENCIES
+#   std_msgs
+# )
+
+################################################
+## Declare ROS dynamic reconfigure parameters ##
+################################################
+
+## To declare and build dynamic reconfigure parameters within this
+## package, follow these steps:
+## * In the file package.xml:
+##   * add a build_depend and a exec_depend tag for "dynamic_reconfigure"
+## * In this file (CMakeLists.txt):
+##   * add "dynamic_reconfigure" to
+##     find_package(catkin REQUIRED COMPONENTS ...)
+##   * uncomment the "generate_dynamic_reconfigure_options" section below
+##     and list every .cfg file to be processed
+
+## Generate dynamic reconfigure parameters in the 'cfg' folder
+# generate_dynamic_reconfigure_options(
+#   cfg/DynReconf1.cfg
+#   cfg/DynReconf2.cfg
+# )
+
+###################################
+## catkin specific configuration ##
+###################################
+## The catkin_package macro generates cmake config files for your package
+## Declare things to be passed to dependent projects
+## INCLUDE_DIRS: uncomment this if your package contains header files
+## LIBRARIES: libraries you create in this project that dependent projects also need
+## CATKIN_DEPENDS: catkin_packages dependent projects also need
+## DEPENDS: system dependencies of this project that dependent projects also need
+catkin_package(
+  INCLUDE_DIRS include
+  LIBRARIES ${PROJECT_NAME}_lib
+  CATKIN_DEPENDS roscpp rospy std_msgs libserial
+  DEPENDS Boost
+)
+
+###########
+## Build ##
+###########
+
+## Specify additional locations of header files
+## Your package locations should be listed before other locations
+include_directories(
+  include
+  ${catkin_INCLUDE_DIRS}
+  ${Boost_INCLUDE_DIRS}
+)
+
+## Declare a C++ library
+add_library(${PROJECT_NAME}_lib
+  src/md_controller.cpp
+  src/diagnostic.cpp
+  src/md_controller_subscriber.cpp
+)
+
+## Add cmake target dependencies of the library
+## as an example, code may need to be generated before libraries
+## either from message generation or dynamic reconfigure
+# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Declare a C++ executable
+## With catkin_make all packages are built within a single CMake context
+## The recommended prefix ensures that target names across packages don't collide
+add_executable(${PROJECT_NAME}_node src/md_controller_node.cpp)
+
+## Rename C++ executable without prefix
+## The above recommended prefix causes long target names, the following renames the
+## target back to the shorter version for ease of user use
+## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
+# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")
+
+## Add cmake target dependencies of the executable
+## same as for the library above
+add_dependencies(${PROJECT_NAME}_node
+  ${PROJECT_NAME}_lib
+  ${${PROJECT_NAME}_EXPORTED_TARGETS}
+  ${catkin_EXPORTED_TARGETS}
+)
+
+add_dependencies(${PROJECT_NAME}_lib
+  ${${PROJECT_NAME}_EXPORTED_TARGETS}
+  ${catkin_EXPORTED_TARGETS}
+)
+
+## Specify libraries to link a library or executable target against
+target_link_libraries(${PROJECT_NAME}_node
+  ${PROJECT_NAME}_lib
+  ${Boost_LIBRARIES}
+  ${catkin_LIBRARIES}
+)
+
+target_link_libraries(${PROJECT_NAME}_lib
+  ${Boost_LIBRARIES}
+  ${catkin_LIBRARIES}
+)
+#############
+## Install ##
+#############
+
+# all install targets should use catkin DESTINATION variables
+# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
+
+## Mark executable scripts (Python etc.) for installation
+## in contrast to setup.py, you can choose the destination
+# catkin_install_python(PROGRAMS
+#   scripts/my_python_script
+#   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark executables for installation
+## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html
+install(TARGETS ${PROJECT_NAME}_node
+  RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
+
+## Mark libraries for installation
+## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html
+install(TARGETS ${PROJECT_NAME}_lib 
+   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+   RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
+ )
+
+## Mark cpp header files for installation
+install(DIRECTORY include/${PROJECT_NAME}/
+  DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+  FILES_MATCHING PATTERN "*.h"
+  PATTERN ".svn" EXCLUDE
+ )
+
+install(DIRECTORY 
+  launch
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+)
+
+## Mark other files for installation (e.g. launch and bag files, etc.)
+install(FILES
+  motorInfomation.yaml
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+)
+
+#############
+## Testing ##
+#############
+
+## Add gtest based cpp test target and link libraries
+# catkin_add_gtest(${PROJECT_NAME}-test test/test_md_controller.cpp)
+# if(TARGET ${PROJECT_NAME}-test)
+#   target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
+# endif()
+
+## Add folders to be run by python nosetests
+# catkin_add_nosetests(test)

Devices/Libraries/Ros/md_controller/README.md

@@ -0,0 +1,15 @@
+#KHởi tạo USB->rs485:
+
+Bước 1: Cấu hình USB->rs485:
+	- Mở cổng terminal, Gõ lệnh "sudo touch /etc/udev/rules.d/99-usb-serial.rules" 
+	- Mở file rules "sudo nano /etc/udev/rules.d/99-usb-serial.rules"
+	- Copy 2 dòng lệnh sau: 
+		- KERNEL=="ttyUSB[0-9]*", OWNER="robotics"
+		- SUBSYSTEM=="tty", ATTRS{idVendor}=="0403", ATTRS{idProduct}=="6001", ATTRS{manufacturer}=="PRATI", ATTRS{product}=="USB_MD200", SYMLINK+="USB_MD200"
+	- Save và exit
+	- kiểm tra thuộc tính "udevadm info --name=/dev/ttyUSB0 --attribute-walk"
+- Nếu đã cấu hình rồi thì bỏ qua bước 1, IPC sẽ tự động thực hiện và cấp quyền cho cổng serial mỗi khi khởi động. Đồng thời, Không cần dùng lệnh  "sudo chmod 666 /dev/ttyUSB*".
+
+Bước 2:
+	- Kiểm tra USB->rs485 đã kết nối ok chưa. Gõ lệnh "ls /dev". Bạn sẽ thấy thiết bị tên "AGV-BLDV20KM". Nếu không có thì chưa kết nối được
+	- KHởi chạy chương trình "roslaunch driver_blvd_controller agvrun.launch"

Devices/Libraries/Ros/md_controller/include/md_controller/define.h

@@ -0,0 +1,112 @@
+#ifndef __MD_CTRL_DEFINE_H_INCLUDED
+#define __MD_CTRL_DEFINE_H_INCLUDED
+ 
+#define HEADER_RMID_SEND 0xB7
+#define HEADER_TMID_SEND 0XAC
+
+#define MAX_BUFFER_LENGHT 2084
+#define PACKAGE_MSG_MAIN_DATA 22
+#define MAX_ID_LENGHT 253
+#define TIME_PUBLISH_TO_SEND 7500 //us 
+
+
+/* Frame of messgae package*/
+typedef struct _Header
+{
+   uint8_t RMID = HEADER_RMID_SEND;
+   uint8_t TMID;
+}header_t;
+
+typedef struct _ID_Number
+{
+   uint8_t adr[MAX_ID_LENGHT];
+   uint8_t size;
+}id_number_t;
+
+typedef struct _Parameter_ID
+{
+   /* data */
+   uint8_t PID;
+}param_id_t;
+
+typedef struct Data_number
+{
+   uint8_t NUM;
+}dt_num_t;
+
+/***********************************************************************************************************/
+
+typedef struct _filter
+{
+   uint8_t _buffer[MAX_BUFFER_LENGHT];
+   uint8_t result[PACKAGE_MSG_MAIN_DATA];
+   unsigned long r_index;
+   unsigned long w_index;
+   unsigned long c_flag;
+}filter_t;
+
+enum PID_COMMAND
+{
+   CMD_TQ_OFF = 2,               // Tq-off, motor free state
+   CMD_BRAKE  = 4,               // Erectric brake
+   CMD_MAIN_BC_ON = 5,           // PID_MAIN_DATA broadcasting ON
+   CMD_MAIN_BC_OFF = 6,          // broadcasing OFF
+   CMD_ALARM_RESET = 8,          //  Reset alarm
+   CMD_POSI_RESET = 10,          // Position reset(set position to zero)
+   CMD_MONITOR_BC_ON = 11,       // PID_MONITOR broadcasting ON
+   CMD_MONITOR_BC_OFF = 12,      // Broadcasting off
+   CMD_IO_MONITOR_ON = 13,       // PID_IO_MONITOR BC ON
+   CMD_IO_MONITOR_OFF = 14,      // PID_IO_MONITOR BC OFF
+   CMD_FAN_ON = 15,              // Fan ON(motor cooling fan)
+   CMD_FAN_OFF = 16,             // Fan OFF
+   CMD_CLUTCH_ON = 17,           // Mechanical brake(clutch) ON
+   CMD_CLUTCH_OFF = 18,          // Mechanical breka OFF
+   CMD_TAR_VEL_OFF = 20,         // Erase target vel, set by PID_TAR_VEL
+   CMD_SLOW_START_OFF = 21,      // Erase target slow/start value
+   CMD_SLOW_DOWN_OFF = 22,       // Erase target slow/down vaule
+   CMD_CAN_RESEND_ON = 23,       // Send CAN data to RS485 serial port.
+   CMD_CAN_RESEND_OFF = 24,      // Turn off resending of CAN data
+   CMD_MAX_TQ_OFF = 25,          // Erase target limit load(max. current)
+   CMD_ENC_OFF = 26,             // Cancel the use of encoder sensor.
+   CMD_LOW_SPEED_LIMIT_OF = 27,  // Cancel the set of low speed limit
+   F_HIGH = 28,                  // Cancel the set of high speed limit.
+   CMD_SPEED_LI_OFF =29,         // Cancel the set of low/high speed limits
+   CMD_CURVE_OFF = 31,           // Cancel set of curve fitting func.
+   CMD_STEP_OFF = 32,            // Cancel step input mode
+   CMD_UICOM_OFF = 44,           // I/O control(ctrl 11pin cnt) available
+   CMD_UICOM_ON = 45,            // I/O control disable(when comm. is used)
+   CMD_MAX_RP_OFF = 46,          // Cancel max. speed set by DIP SW
+   CMD_HALL_TY_OFF = 47,         // Cancel set of motor hall type
+   CMD_LOW_POT_OFF = 48,         // Cancel set of low limit of POT input
+   CMD_HIGH_POT_OFF = 49,        // Cancel set of high limit of POT input
+   CMD_MAIN_BC_ON2 = 50,         // PID_MAIN_DATA, BC ON for 2nd motor
+   CMD_MAIN_BC_OFF2 = 51,        // PID_MAIN_DATA, BC OFF for 2nd motor
+   CMD_MONIT_BC_ON2 = 52,        // PID_MONITOR, BC ON for 2nd motor
+   CMD_MONIT_BC_OFF2 = 53,       // PID_MONITOR, BC OFF for 2nd motor
+   CMD_IO_MONIT_BC_ON2 = 54,     // PID_IO_MONITOR, BC ON for 2nd motor
+   CMD_IO_MONIT_BC_OFF2 = 55     // PID_IO_MONITOR, BC OFF for 2nd motor
+};
+
+enum _BIT
+{
+   _OFF = 0x00u,
+   _ON = 0x01u 
+};
+
+enum _BAUD_OF_MD200
+{
+   _9600bps = 1,
+   _19200bps = 2,
+   _38400bps = 3,
+   _57600bps = 4,
+   _115200bps = 5
+};
+
+typedef enum EXIT_CODES_ENUM
+{
+  EXIT_OK = 0,
+  EXIT_ERROR = 1,
+  EXIT_CONFIG_ERROR = -1
+} EXIT_CODES;
+
+#endif // __MD_CTRL_DEFINE_H_INCLUDED

Devices/Libraries/Ros/md_controller/include/md_controller/diagnostic.h

@@ -0,0 +1,94 @@
+#ifndef __MD_CTRL_DIAGNOTICS_H_INCLUDED
+#define __MD_CTRL_DIAGNOTICS_H_INCLUDED
+
+#include <ros/ros.h>
+
+namespace MD 
+{
+    typedef enum DIAGNOSTIC_STATUS_ENUM
+    {
+        OK,                       // status okay, no errors
+        EXIT,                     // delta exiting
+        ERROR_STATUS,             // device signaled an error 
+        CONFIGURATION_ERROR,      // invalid configuration, check configuration files
+        INITIALIZATION_ERROR,     // initialization of driver failed
+        INTERNAL_ERROR            // internal error, should never happen
+        
+    } DIAGNOSTIC_STATUS;
+    /*
+    * class Diagnostic publishes diagnostic messages for sick_line_guidance
+    */
+    class Diagnostic
+    {
+    public:
+        /*
+        * Initializes the global diagnostic handler.
+        *
+        * @param[in] nh ros::NodeHandle
+        * @param[in] publish_topic ros topic to publish diagnostic messages
+        * @param[in] component description of the component reporting
+        */
+        static void init(ros::NodeHandle &nh, const std::string & publish_topic, const std::string & component)
+        {
+            g_diagnostic_handler.init(nh, publish_topic, component);
+        }
+        
+        /*
+        * Updates and reports the current status.
+        *
+        * @param[in] status current status (OK, ERROR, ...)
+        * @param[in] message optional diagnostic message
+        */
+        static void update(DIAGNOSTIC_STATUS status, const std::string & message = "")
+        {
+            g_diagnostic_handler.update(status, message);
+        }
+    
+    protected:
+        /*
+        * class DiagnosticImpl implements diagnostic for sick_line_guidance
+        */
+        class DiagnosticImpl
+        {
+        public:
+    
+            /*
+            * Constructor.
+            */
+            DiagnosticImpl();
+            
+            /*
+            * Initialization.
+            *
+            * @param[in] nh ros::NodeHandle
+            * @param[in] publish_topic ros topic to publish diagnostic messages
+            * @param[in] component description of the component reporting
+            */
+            void init(ros::NodeHandle & nh, const std::string & publish_topic, const std::string & component);
+            
+            /*
+            * Updates and reports the current status.
+            *
+            * @param[in] status current status (OK, ERROR, ...)
+            * @param[in] message optional diagnostic message
+            */
+            void update(DIAGNOSTIC_STATUS status, const std::string & message = "");
+            
+        protected:
+            
+            /*
+            * member data.
+            */
+            
+            bool m_diagnostic_initialized;         // flag indicating proper initialization of diagnostic
+            ros::Publisher m_diagnostic_publisher; // publishes diagnostic messages
+            std::string m_diagnostic_component;    // name of the component publishing diagnostic messages
+        
+        }; // class DiagnosticImpl
+        
+        
+        static DiagnosticImpl g_diagnostic_handler; // singleton, implements the diagnostic for sick_line_guidance
+        
+  }; // class Diagnostic
+}
+#endif // __MD_CTRL_DIAGNOTICS_H_INCLUDED

Devices/Libraries/Ros/md_controller/include/md_controller/md_controller.h

@@ -0,0 +1,78 @@
+#ifndef __MD_CTRL_H_INCLUDED
+#define __MD_CTRL_H_INCLUDED
+
+#include <ros/ros.h>
+#include <vector>
+#include "libserial/rs485.h"
+#include "md_controller/define.h"
+#include "md_controller/diagnostic.h"
+#include <boost/thread/thread.hpp>
+
+namespace MD
+{
+
+// shortcut to read a member from a XmlRpcValue, or to return a defaultvalue, it the member does not exist
+template<class T> static T readMember(XmlRpc::XmlRpcValue & value, const std::string & member, const T & defaultvalue)
+{
+  if(value.hasMember(member))
+    return value[member];
+  return defaultvalue;
+}
+
+class MdController: public rs485
+{
+public:
+   MdController(ros::NodeHandlePtr nh, const char *devfile, unsigned int baud, parity_t parity, data_bits_t data_bit,stop_bits_t stop_bit);
+   virtual ~MdController(); 
+
+  // virtual void set_slave_id(int *ids, uint8_t lenght);
+  // virtual void publish_speed(int16_t *speed);
+  // virtual void main_BC_ON();
+  // virtual void main_BC_OFF();
+
+  /* COMMAND */
+  virtual void pid_defaut_set(uint8_t address);
+  virtual void pid_tq_off(uint8_t address); 
+  virtual void pid_break(uint8_t address);
+  virtual void pid_stop(uint8_t address);
+  virtual void pid_command(uint8_t address, PID_COMMAND cmd);
+  virtual void pid_alarm_reset(uint8_t address);
+  virtual void pid_posi_reset(uint8_t address);
+  virtual void pid_main_bc_state(uint8_t address, _BIT data);
+  virtual void pid_monitor_bc_state(uint8_t address, _BIT data);
+  virtual void pid_preset_save(uint8_t address, uint8_t data);
+  virtual void pid_preset_recall(uint8_t address, uint8_t data);
+  virtual void pid_vel_cmd(uint8_t address, int16_t speed);
+  virtual void pid_vel_cmd2(uint8_t address, int16_t high_speed);
+  virtual void pid_open_vel_cmd(uint8_t address, int16_t data);
+
+  bool err{};
+  int err_no{};
+
+private:
+  id_number_t slave_id;
+  uint8_t rate = 100;
+  boost::thread ThreadOne,ThreadTwo;
+  bool theadEnable;
+  filter_t cs; 
+  ros::NodeHandlePtr nh_;
+  XmlRpc::XmlRpcValue drivers;
+
+  virtual uint8_t GetcheckSum(uint length, uint8_t *to_send);
+  virtual void buidRequest(uint8_t *to_send,header_t hd ,uint8_t add, param_id_t pid, dt_num_t num) const;
+  virtual int Read(uint8_t *read_buf, ssize_t &num);
+  virtual int Write(header_t hd ,uint8_t address, param_id_t pid, dt_num_t num, uint8_t *value);
+  virtual bool check_baudrate(unsigned int baud);
+  virtual void set_bad_con(void);
+  // virtual void loadParam(void);
+
+  /*Write*/
+  virtual void pid_id_setting(uint8_t id_setting);
+  virtual void pid_baudrate_setting(uint8_t address, _BAUD_OF_MD200 baudrate_setting);
+  /* READ */
+  // static void cmdMainBCdoStuff(void *payload, uint8_t *publish_rate);
+};
+
+} // namespace MD
+
+#endif // __MD_CTRL_H_INCLUDED

Devices/Libraries/Ros/md_controller/include/md_controller/md_controller_subscriber.h

@@ -0,0 +1,70 @@
+#ifndef __MD_CTRL_SUBSCRIBER_H_INCLUDED
+#define __MD_CTRL_SUBSCRIBER_H_INCLUDED
+
+#include <ros/ros.h>
+#include <boost/thread.hpp>
+#include <std_msgs/Float32.h>
+
+namespace MD
+{
+   class Subscriber
+   {
+   public:
+      /*
+       *  @brief Constructor.
+       *  @param[in] nh ros::NodeHandle
+       *  @param[in] ros_topic topic for ros messages
+       *  @param[in] subscribe_queue_size buffer size for ros messages
+       *  @param[in] wait_time Time for checking to Wheel stop (1 second)
+       */
+      Subscriber(ros::NodeHandle & nh, const std::string & ros_topic, int subscribe_queue_size = 1, double schedule_delay = 0.05, double schedule_delay_lastest = 0.5);
+
+      Subscriber(ros::NodeHandle & nh, XmlRpc::XmlRpcValue::iterator object);
+
+      /**
+      * @brief returns true, if publishing of a wheel topic is publishing
+      * 
+      */
+      bool isWheelTriggered(void);
+
+      /**
+      *  @brief returns true, if not subcribe wheel
+      */
+      bool isWheelLastestTriggered(void);
+
+      /**
+      * @brief schedules WheelController function 
+      * @param[in] schedule if true, publishing is scheduled, otherwise a possibly pending schedule is removed.
+      */
+      void scheduleWheelController(bool schedule);
+
+      /*
+       * @brief Destructor.
+       */
+      virtual ~Subscriber();
+
+      boost::mutex m_measurement_mutex;
+      int16_t speed;
+      uint8_t slave_id;
+      double ratio;
+
+   protected:
+      /*
+       * @brief Callbacks for ros messages from wheel topic. 
+       */
+      virtual void wheelCallback(const std_msgs::Float32::Ptr & msg);
+
+      ros::NodeHandle nh_;
+      std::string topic_name_;
+      int m_subscribe_queue_size_;
+      ros::Subscriber control_wheel_sub_;  
+
+      ros::Time publish_time_;
+      ros::Time publish_lastest_time_;
+      ros::Duration schedule_delay_;
+      ros::Duration schedule_lastest_delay_;
+      boost::mutex publish_mutex_;                  
+   };
+}
+
+#endif // __MD_CTRL_SUBSCRIBER_H_INCLUDED

Devices/Libraries/Ros/md_controller/launch/run.launch

@@ -0,0 +1,13 @@
+<?xml version="1.0"?>
+<launch>
+  <rosparam command="load" file="$(find md_controller)/motorInfomation.yaml"/> 
+  <arg name="node_name"     default="md_controller"/>
+  <arg name="product"         default=""/>
+  <arg name="port_name"       default=""/>
+  <arg name="baudrate"        default="19200"/>
+  <node pkg="md_controller" type="md_controller_node" name="$(arg node_name)" output="screen">
+    <param name="product" type="str" value="$(arg product)"/> 
+    <param name="port_name" type="str" value="$(arg port_name)"/> 
+    <param name="baudrate" type="int" value="$(arg baudrate)"/>
+  </node>
+</launch>

Devices/Libraries/Ros/md_controller/motorInfomation.yaml

@@ -0,0 +1,15 @@
+
+port_name: /dev/ttyTHS0
+baudrate: 19200
+producte : no_name
+drivers:
+  lelf_driver:
+    id: 0x02
+    ratio: -30.0
+    toppic: "/left_wheel"
+    subscribe_queue_size: 1
+  right_driver:
+    id: 0x01
+    ratio: 30.0
+    toppic: "/right_wheel"
+    subscribe_queue_size: 1

Devices/Libraries/Ros/md_controller/package.xml

@@ -0,0 +1,74 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>md_controller</name>
+  <version>0.0.0</version>
+  <description>The md_controller package</description>
+
+  <!-- One maintainer tag required, multiple allowed, one person per tag -->
+  <!-- Example:  -->
+  <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+  <maintainer email="robotics@todo.todo">robotics</maintainer>
+
+
+  <!-- One license tag required, multiple allowed, one license per tag -->
+  <!-- Commonly used license strings: -->
+  <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
+  <license>TODO</license>
+
+
+  <!-- Url tags are optional, but multiple are allowed, one per tag -->
+  <!-- Optional attribute type can be: website, bugtracker, or repository -->
+  <!-- Example: -->
+  <!-- <url type="website">http://wiki.ros.org/md_controller</url> -->
+
+
+  <!-- Author tags are optional, multiple are allowed, one per tag -->
+  <!-- Authors do not have to be maintainers, but could be -->
+  <!-- Example: -->
+  <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
+
+
+  <!-- The *depend tags are used to specify dependencies -->
+  <!-- Dependencies can be catkin packages or system dependencies -->
+  <!-- Examples: -->
+  <!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
+  <!--   <depend>roscpp</depend> -->
+  <!--   Note that this is equivalent to the following: -->
+  <!--   <build_depend>roscpp</build_depend> -->
+  <!--   <exec_depend>roscpp</exec_depend> -->
+  <!-- Use build_depend for packages you need at compile time: -->
+  <!--   <build_depend>message_generation</build_depend> -->
+  <!-- Use build_export_depend for packages you need in order to build against this package: -->
+  <!--   <build_export_depend>message_generation</build_export_depend> -->
+  <!-- Use buildtool_depend for build tool packages: -->
+  <!--   <buildtool_depend>catkin</buildtool_depend> -->
+  <!-- Use exec_depend for packages you need at runtime: -->
+  <!--   <exec_depend>message_runtime</exec_depend> -->
+  <!-- Use test_depend for packages you need only for testing: -->
+  <!--   <test_depend>gtest</test_depend> -->
+  <!-- Use doc_depend for packages you need only for building documentation: -->
+  <!--   <doc_depend>doxygen</doc_depend> -->
+  <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>roscpp</build_depend>
+  <build_depend>rospy</build_depend>
+  <build_depend>std_msgs</build_depend>
+  <build_depend>libserial</build_depend>
+
+  <build_export_depend>roscpp</build_export_depend>
+  <build_export_depend>rospy</build_export_depend>
+  <build_export_depend>std_msgs</build_export_depend>
+  <build_export_depend>libserial</build_export_depend>
+
+  <exec_depend>roscpp</exec_depend>
+  <exec_depend>rospy</exec_depend>
+  <exec_depend>std_msgs</exec_depend>
+  <exec_depend>libserial</exec_depend>
+
+  <!-- <depend>agv_define</depend> -->
+
+  <!-- The export tag contains other, unspecified, tags -->
+  <export>
+    <!-- Other tools can request additional information be placed here -->
+
+  </export>
+</package>

Devices/Libraries/Ros/md_controller/src/diagnostic.cpp

@@ -0,0 +1,76 @@
+#include <diagnostic_msgs/DiagnosticArray.h>
+#include <diagnostic_msgs/DiagnosticStatus.h>
+#include "md_controller/diagnostic.h"
+
+
+/**
+ * g_diagnostic_handler: singleton, implements the diagnostic for sick_line_guidance
+ */
+MD::Diagnostic::DiagnosticImpl MD::Diagnostic::g_diagnostic_handler;
+
+
+/*
+ * Constructor.
+ */
+MD::Diagnostic::DiagnosticImpl::DiagnosticImpl() : m_diagnostic_initialized(false), m_diagnostic_component("")
+{
+}
+
+/*
+ * Initialization.
+ *
+ * @param[in] nh ros::NodeHandle
+ * @param[in] publish_topic ros topic to publish diagnostic messages
+ * @param[in] component description of the component reporting
+ */
+void MD::Diagnostic::DiagnosticImpl::init(ros::NodeHandle &nh, const std::string & publish_topic, const std::string & component)
+{
+  m_diagnostic_publisher = nh.advertise<diagnostic_msgs::DiagnosticArray>(publish_topic, 1);
+  m_diagnostic_component = component;
+  m_diagnostic_initialized = true;
+}
+
+/*
+ * Updates and reports the current status.
+ *
+ * @param[in] status current status (OK, ERROR, ...)
+ * @param[in] message optional diagnostic message
+ */
+void MD::Diagnostic::DiagnosticImpl::update(DIAGNOSTIC_STATUS status, const std::string &message)
+{
+  if (m_diagnostic_initialized)
+  {
+    static std::map<DIAGNOSTIC_STATUS, std::string> status_description = {
+      {DIAGNOSTIC_STATUS::OK, "OK"},
+      {DIAGNOSTIC_STATUS::EXIT, "EXIT"},
+      {DIAGNOSTIC_STATUS::ERROR_STATUS, "ERROR_STATUS"},
+      {DIAGNOSTIC_STATUS::CONFIGURATION_ERROR, "CONFIGURATION_ERROR"},
+      {DIAGNOSTIC_STATUS::INITIALIZATION_ERROR, "INITIALIZATION_ERROR"},
+      {DIAGNOSTIC_STATUS::INTERNAL_ERROR, "INTERNAL_ERROR"}
+    };
+  
+    // create DiagnosticStatus
+    diagnostic_msgs::DiagnosticStatus msg;
+    msg.level = (status == DIAGNOSTIC_STATUS::OK) ? (diagnostic_msgs::DiagnosticStatus::OK) : (diagnostic_msgs::DiagnosticStatus::ERROR); // Level of operation
+    msg.name = m_diagnostic_component; // description of the test/component reporting
+    msg.hardware_id = "";  // hardware unique string (tbd)
+    msg.values.clear();    // array of values associated with the status
+    // description of the status
+    msg.message = status_description[status];
+    if(msg.message.empty())
+    {
+      msg.message = "ERROR";
+    }
+    if (!message.empty())
+    {
+      msg.message = msg.message + ": " + message;
+    }
+    // publish DiagnosticStatus in DiagnosticArray
+    diagnostic_msgs::DiagnosticArray msg_array;
+    msg_array.header.stamp = ros::Time::now();
+    msg_array.header.frame_id = "";
+    msg_array.status.clear();
+    msg_array.status.push_back(msg);
+    m_diagnostic_publisher.publish(msg_array);
+  }
+}

Devices/Libraries/Ros/md_controller/src/md_controller.cpp

@@ -0,0 +1,666 @@
+#include "md_controller/md_controller.h"
+         
+
+MD::MdController::~MdController(){}
+/*
+ * Object
+ * @param devfile   Path name of dev  
+ * @param baud      Baudrate 
+ * @param parity    Parity
+ * @param data_bit  Data bit
+ * @param stop_bit  Stop bit 
+ */
+MD::MdController::MdController(ros::NodeHandlePtr nh, const char *devfile, unsigned int baud, parity_t parity, data_bits_t data_bit,stop_bits_t stop_bit)
+    : nh_(nh), rs485(devfile,baud, parity, data_bit, stop_bit)
+{
+    if(this->_connected == true)
+    {
+        if(!this->check_baudrate(baud))
+        {   
+            std::stringstream msg;
+            msg << "DRIVER not support baudrate" << baud << "bps";
+            MD::Diagnostic::update(MD::DIAGNOSTIC_STATUS::CONFIGURATION_ERROR, msg.str());
+            ROS_ERROR_STREAM("md_controller_node: " << msg.str());
+            this->close_port();
+            exit(-1);
+        }
+        else
+        {
+            std::stringstream msg;
+            msg << " Connected "<<devfile<<" with "<<baud<<" bps";
+            this->err = false;
+            this->err_no = 0;
+            ROS_INFO_STREAM("md_controller_node: " << msg.str());
+            ros::Duration(0.7).sleep(); // sleep for 0.5 second
+            MD::Diagnostic::update(MD::DIAGNOSTIC_STATUS::OK, msg.str());
+        }
+    }
+    else
+    {
+        ros::Duration(2).sleep();
+        std::stringstream err;
+        err << error_msg;
+        ROS_ERROR_STREAM(error_msg);
+        MD::Diagnostic::update(MD::DIAGNOSTIC_STATUS::INITIALIZATION_ERROR, error_msg);
+    }
+} 
+
+// /**
+//  * Get parameter from yaml file
+//  */
+// void MD::MdController::loadParam(void)
+// {
+//     int node_id[this->drivers.size()];
+//     int count = 0;
+
+//     for(XmlRpc::XmlRpcValue::iterator _iter = this->drivers.begin(); _iter != this->drivers.end(); ++_iter)
+//     {   
+//         std::string driver_name = readMember<std::string>(_iter->second, "name", _iter->first);
+//         if(_iter->second.hasMember("id"))
+//         {
+//             node_id[count] = _iter->second["id"];
+//             ROS_INFO("md_controller: initializing driver_name \"%s\", id \"0x0%d\"...", driver_name.c_str(),node_id[count]);
+//             count++;
+//         } else
+//         {
+//             ROS_ERROR_STREAM("Node '" << _iter->first  << "' has no id");
+//             MD::Diagnostic::update(MD::DIAGNOSTIC_STATUS::CONFIGURATION_ERROR, " Node name has no id");
+//         } 
+//     }
+//     this->set_slave_id(node_id,count);
+// }
+
+/*
+ * Check baudrate has supported for device
+ * @param baud      Package Array size byte
+ * @param return    true/false 
+ */
+bool MD::MdController::check_baudrate(unsigned int baud)
+{
+    bool status = false;
+    unsigned int data_defaut[] = {9600, 19200, 38400, 57600, 115200};
+    for(uint8_t i = 0; i <  (uint8_t)sizeof(data_defaut)/sizeof(unsigned int); i++)
+    {
+        if(baud == data_defaut[i])
+        {
+            status = true;
+            break;
+        } 
+    }
+    return status;
+}
+
+/*
+ * Checksum
+ * @param length    Package Array size byte
+ * @param to_send   Package Array
+ * @param return    byte 
+ */
+uint8_t MD::MdController::GetcheckSum(uint length, uint8_t *to_send)
+{
+   uint8_t byTmp = 0;
+   for(short i = 0; i < length; i++ ) byTmp += *(to_send + i);
+   return (~byTmp +1);
+
+} //GetcheckSum
+
+/**
+ * Set connection is bad
+ */
+void MD::MdController::set_bad_con(void)
+{
+    this->err = true;
+    ROS_ERROR("BAD CONNECTION");
+}
+
+/*
+ * Modbus Request Builder
+ * @param to_send   Message Buffer input
+ * @param hd        Header 
+ * @param add       ID Number 
+ * @param pid       Parameter ID
+ * @param num       Data number
+ */
+void MD::MdController::buidRequest(uint8_t *to_send,header_t hd ,
+                                    uint8_t address, param_id_t pid, dt_num_t num) const
+{
+    to_send[0] = hd.RMID;
+    to_send[1] = hd.TMID;
+    to_send[2] = address;
+    to_send[3] = pid.PID;
+    to_send[4] = num.NUM;
+} // buidRequest 
+
+/*
+ * Write Request Builder and Sender
+ * @param address   Reference Address
+ * @param lenght    Amount of data to be Written
+ * @param value     Data to Be Written
+ */
+int MD::MdController::Write(header_t hd ,uint8_t address, param_id_t pid,
+                                         dt_num_t num, uint8_t *value)
+{
+    int status = 0;
+    uint length = num.NUM + 5;
+    uint8_t to_send[length+1];
+    this->buidRequest(to_send,hd,address,pid,num);
+    for(uint8_t i = 0; i < num.NUM; i++) to_send[length -num.NUM +i] = value[i];
+    to_send[length] = this->GetcheckSum(length,to_send);
+    struct stat sb;
+    if(stat(this->ctx._port, &sb) < 0) this->reconnect();
+    if(this->_connected)
+    {
+        status = this->sendMsgs(to_send,length+1);
+    }
+    return status;
+}
+
+/*
+ * Read Request Builder and Sender
+ * @param address   Reference Address
+ * @param lenght    Amount of Data to Read
+ * @param return    EXIT_CODES Type
+ */
+int MD::MdController::Read(uint8_t *read_buf, ssize_t &num)
+{
+    ssize_t k =  this->receiveMsgs(read_buf);
+    num = k;
+    if(k < 0) 
+    {
+        this->set_bad_con();
+        return EXIT_CONFIG_ERROR;
+    }
+    else{
+        return EXIT_OK;
+    }
+    
+}
+
+/*
+ * Default setting
+ * @param address   ID  
+ *                  Data : 0x55(CHECK)
+ *                  183, 172, ID, 3, 1, 0x55, CHK
+ */
+void MD::MdController::pid_defaut_set(uint8_t address)
+{
+    header_t hd;        hd.TMID = 184; 
+    param_id_t pid;     pid.PID = 3;
+    dt_num_t num;       num.NUM = 1;
+    uint8_t value[num.NUM];
+    value[num.NUM - 1] = 0x55u; //Data : 0x55(CHECK)
+    this->Write(hd,address,pid,num,value);
+}
+
+/*
+ * Stop naturally
+ * @param address   ID  
+ *                  Stop motor naturally, data don’t care(x).
+ *                  183, 172, ID, 5, 1, x, CHK
+ */
+void MD::MdController::pid_tq_off(uint8_t address)
+{
+    header_t hd;        hd.TMID = 184; 
+    param_id_t pid;     pid.PID = 5;
+    dt_num_t num;       num.NUM = 1;
+    uint8_t value[num.NUM];
+    value[num.NUM - 1] = 0X01u; //data don’t care(x).
+    this->Write(hd,address,pid,num,value);
+}
+
+/*
+ * Erectric brake
+ * @param address   ID  
+ *                  Stop motor urgently(electric braking mode)
+ *                  183, 172, ID, 6, 1, x, CHK
+ */
+void MD::MdController::pid_break(uint8_t address)
+{
+    header_t hd;        hd.TMID = 184; 
+    param_id_t pid;     pid.PID = 6;
+    dt_num_t num;       num.NUM = 1;
+    uint8_t value[num.NUM];
+    value[num.NUM - 1] = 0x01u; //data don’t care(x).
+    this->Write(hd,address,pid,num,value);
+}
+
+/*
+ * stop
+ * @param address   ID  
+ *                  Stop motor urgently(electric braking mode)
+ *                  183, 172, ID, 6, 1, x, CHK
+ */
+void MD::MdController::pid_stop(uint8_t address)
+{
+    header_t hd;        hd.TMID = 184; 
+    param_id_t pid;     pid.PID = 5;
+    dt_num_t num;       num.NUM = 1;
+    uint8_t value[num.NUM];
+    value[num.NUM - 1] = 0x01u; //data don’t care(x).
+    this->Write(hd,address,pid,num,value);
+}
+
+/*
+ * Pid command
+ * @param address   ID  
+ * @param cmd       Contents on CMD number  (forllow page 6-CommunicationProtocolOnRS485_V44)
+ *                  183, 172, ID, 10, 1, CMD, CHK
+ */
+void MD::MdController::pid_command(uint8_t address, PID_COMMAND cmd)
+{
+    header_t hd;        hd.TMID = 184; 
+    param_id_t pid;     pid.PID = 10;
+    dt_num_t num;       num.NUM = 1;
+    uint8_t value[num.NUM];
+    value[num.NUM - 1] = cmd;
+    this->Write(hd,address,pid,num,value);
+}
+
+/*
+ * Reset alarm
+ * @param address   ID  
+ *                  Data don’t care(x)
+ *                  183, 172, ID, 12, 1, x, CHK
+ */
+void MD::MdController::pid_alarm_reset(uint8_t address)
+{
+    header_t hd;        hd.TMID = 184; 
+    param_id_t pid;     pid.PID = 12;
+    dt_num_t num;       num.NUM = 1;
+    uint8_t value[num.NUM];
+    value[num.NUM - 1] = 0X01u; //data don’t care(x).
+    this->Write(hd,address,pid,num,value);
+}
+
+/*
+ * Reset position, Motor position to zero
+ * @param address   ID  
+ *                  Data don’t care(x)
+ *                  183, 172, ID, 13, 1, x, CHK
+ */
+void MD::MdController::pid_posi_reset(uint8_t address)
+{
+    header_t hd;        hd.TMID = 184;
+    param_id_t pid;     pid.PID = 13;
+    dt_num_t num;       num.NUM = 1;
+    uint8_t value[num.NUM];
+    value[num.NUM - 1] = 0X01u; //data don’t care(x).
+    this->Write(hd,address,pid,num,value);
+}
+
+/*
+ * Request broadcasting of PID_MAIN_DATA
+ * @param address   ID  
+ * @param data      DATA  1 : PID 193 broadcasting on OR 0 : broasdcasting off
+ *                  183, 172, ID, 14, 1, DATA, CHK
+ */
+void MD::MdController::pid_main_bc_state(uint8_t address, _BIT data)
+{
+    header_t hd;        hd.TMID = 184; 
+    param_id_t pid;     pid.PID = 14;
+    dt_num_t num;       num.NUM = 1;
+    uint8_t value[num.NUM];
+    value[num.NUM - 1] = data; 
+    this->Write(hd,address,pid,num,value);
+}
+
+/*
+ * Request BC on/off of PID_MONITOR
+ * @param address   ID  
+ * @param data      DATA  1 : PID 196 broadcasting on OR 0 : broasdcasting off
+ *                  183, 172, ID, 15, 1, DATA, CHK
+ */
+void MD::MdController::pid_monitor_bc_state(uint8_t address, _BIT data)
+{
+    header_t hd;        hd.TMID = 184; 
+    param_id_t pid;     pid.PID = 15;
+    dt_num_t num;       num.NUM = 1;
+    uint8_t value[num.NUM];
+    value[num.NUM - 1] = data; 
+    this->Write(hd,address,pid,num,value);
+}
+
+/*
+ * Save preset position
+ * @param address   ID  
+ * @param data      DATA  Preset number(address, 1~20)
+ *                        Set current position to the preset address
+ *                  183, 172, ID, 31, 1, DATA, CHK
+ */
+void MD::MdController::pid_preset_save(uint8_t address, uint8_t data)
+{
+    header_t hd;        hd.TMID = 184; 
+    param_id_t pid;     pid.PID = 31;
+    dt_num_t num;       num.NUM = 1;
+    uint8_t value[num.NUM];
+    if(data > 0 && data <=20)
+    {   
+        value[num.NUM - 1] = data; 
+        this->Write(hd,address,pid,num,value);
+    }
+    else
+    {
+        ROS_ERROR("Command 'PID_PRESET_SAVE' element 'data' number must in ranges (1~20). Prefer page 10 CommunicationProtocolOnRS485_V44");
+        
+    }  
+}
+
+/*
+ * Go to the recalled preset position
+ * @param address   ID  
+ * @param data      DATA  Preset number(address, 1~20)
+ *                        Recall the saved preset data and move to thatposition.(position control)
+ *                  183, 172, ID, 32, 1, DATA, CHK
+ */
+void MD::MdController::pid_preset_recall(uint8_t address, uint8_t data)
+{
+    header_t hd;        hd.TMID = 184; 
+    param_id_t pid;     pid.PID = 32;
+    dt_num_t num;       num.NUM = 1;
+    uint8_t value[num.NUM];
+    if(data > 0 && data <=20)
+    {   
+        value[num.NUM - 1] = data; 
+        this->Write(hd,address,pid,num,value);
+    }
+    else
+    {
+        ROS_ERROR("Command 'PID_PRESET_RECALL' element 'data' number must in ranges (1~20). Prefer page 10 CommunicationProtocolOnRS485_V44");
+    }
+    
+}
+
+/*
+ * Velocity command
+ * @param address   ID 
+ * @param speed     Speed(rpm) = (D1 | D2<<8) 
+ *                  Speed>0, CCW direction
+ *                  Speed<0, CW direction
+ *                  183, 172, ID, 130, 2, D1, D2, CHK
+ */
+void MD::MdController::pid_vel_cmd(uint8_t address, int16_t speed)
+{
+    header_t hd;        hd.TMID = 184; 
+    param_id_t pid;     pid.PID = 130;
+    dt_num_t num;       num.NUM = 2;
+    uint8_t value[num.NUM];
+    value[num.NUM - 2] = speed & 0x00FFu;
+    value[num.NUM - 1] = speed >> 8u;
+    this->Write(hd,address,pid,num,value);
+}
+
+/*
+ * Velocity command used more than 25,000rpm
+ * @param address   ID  
+ * @param high_speed    Speed(rpm) = (D1 | D2<<8) x 10 
+ *                      Speed>0, CCW direction
+ *                      Speed<0, CW direction
+ *                      183, 172, ID, 131, 2, D1, D2, CHK
+ */
+void MD::MdController::pid_vel_cmd2(uint8_t address, int16_t high_speed)
+{
+    header_t hd;        hd.TMID = 184; 
+    param_id_t pid;     pid.PID = 131;
+    dt_num_t num;       num.NUM = 2;
+    uint8_t value[num.NUM];
+    value[num.NUM - 2] = high_speed*10 & 0x00FFu;
+    value[num.NUM - 1] = high_speed*10 >> 8u;
+    this->Write(hd,address,pid,num,value);
+}
+
+/*
+ * Velocity command used more than 25,000rpm
+ * @param address   ID  
+ * @param data      D1, D2: Open-loop velocity 
+ *                  Range : -1023~1023
+ *                  183, 172, ID, 134, 2, D1, D2, CHK
+ */
+void MD::MdController::pid_open_vel_cmd(uint8_t address, int16_t data)
+{
+    header_t hd;        hd.TMID = 184; 
+    param_id_t pid;     pid.PID = 134;
+    dt_num_t num;       num.NUM = 2;
+    uint8_t value[num.NUM];
+    if(data >= -1023 && data <= 1023)
+    {
+        value[num.NUM - 2] = data & 0x00FFu;
+        value[num.NUM - 1] = data >> 8u;
+        this->Write(hd,address,pid,num,value);
+    }
+    else
+    {
+        ROS_ERROR("Command 'PID_PRESET_RECALL' element 'data' number must in ranges (-1023 ~ 1023). Prefer page 10 CommunicationProtocolOnRS485_V44");
+    }
+}
+
+/*
+ * ID setting
+ * @param id_setting    ID : 1~253 : setting ID
+ *                      Write command(0xaa)
+ *                      183, 172, 254, 133, 2, 0xaa, ID, CHK
+ */
+void MD::MdController::pid_id_setting(uint8_t id_setting)
+{
+    header_t hd;        hd.TMID = 184; 
+    param_id_t pid;     pid.PID = 133;
+    dt_num_t num;       num.NUM = 2;
+    uint8_t value[num.NUM];
+    if(id_setting > 0 && id_setting <=253)
+    {
+        value[num.NUM - 2] = 0xAAu;
+        value[num.NUM - 1] = id_setting;
+        this->Write(hd,254,pid,num,value);
+    }
+    else
+    {
+        ROS_ERROR("Command 'PID_ID' element 'id_setting' number must in ranges (1 ~ 253). Prefer page 12 CommunicationProtocolOnRS485_V44");
+    }
+    
+}
+
+/*
+ * Set the baudrate, BAUD
+ * @param address           ID 
+ * @param baudrate_setting  Baudrate setting(RS485)
+ *                          Write command(0xaa)
+ *                          183, 172, ID, 135, 2, 0xaa, BAUD, CHK
+ */
+void MD::MdController::pid_baudrate_setting(uint8_t address, _BAUD_OF_MD200 baudrate_setting)
+{
+    header_t hd;        hd.TMID = 184; 
+    param_id_t pid;     pid.PID = 133;
+    dt_num_t num;       num.NUM = 2;
+    uint8_t value[num.NUM];
+    value[num.NUM - 2] = 0xAAu;
+    value[num.NUM - 1] = (uint8_t)baudrate_setting;
+    this->Write(hd,address,pid,num,value);
+}
+
+// /*
+//  * Set slave id 
+//  * @param ids       Pointer save array slave id  
+//  * @param lenght    Lenght of ids array (sizeof(ids)/sizeof(uint8_t))
+//  */
+// void MD::MdController::set_slave_id(int *ids, uint8_t lenght)
+// {
+//    uint8_t n = lenght; 
+//    for(uint8_t i = 0; i < n; i++)
+//    {
+//        for(int j = 0; j < n; j++)
+//        {
+//            if(ids[j] == ids[i] && j != i)
+//            {
+//                ids[j] = ids[j+1];
+//                n--;
+//            }
+//        }
+//    }
+//    if(n != lenght)  ROS_WARN("There are %d ID input to be same!!", lenght - n);
+//    this->slave_id.size = n;
+//    for(uint8_t k = 0; k < n; k++) this->slave_id.adr[k] = ids[k];
+// }
+
+// /*
+//  * Set slave id 
+//  * @param speed     Speed list input 
+//  */
+// void MD::MdController::publish_speed(int16_t *speed)
+// {
+//     if(this->slave_id.size > 0)
+//     {
+//         for(uint8_t i = 0; i < this->slave_id.size; i++)
+//         {
+//             if(abs(speed[i]) > 0)
+//                 this->pid_vel_cmd(this->slave_id.adr[i],speed[i]);
+//             else 
+//                 this->pid_break(this->slave_id.adr[i]);
+//         }
+//     }else
+//     {
+//         ros::Duration(3).sleep(); // sleep for 3 second
+//         ROS_ERROR("There are not slave ID of devices MD Motor");
+//     }
+    
+// }
+
+// /*
+//  * PID_MAIN_DATA broadcasting ON
+//  * @param CMD_MAIN_BC_ON 5
+//  */
+// void MD::MdController::main_BC_ON()
+// {
+//     if(this->slave_id.size > 0)
+//     {
+//         for(uint8_t i = 0; i < this->slave_id.size; i++)
+//         {
+//             this->pid_command(this->slave_id.adr[i],CMD_MAIN_BC_ON);
+//             usleep(30000);
+//         }
+//         this->theadEnable = true;
+//         if(this->ThreadOne.joinable() == false)
+//         {
+//             this->ThreadOne = boost::thread(this->cmdMainBCdoStuff,this,&this->rate);
+//         }
+//     }else
+//     {
+//         ros::Duration(3).sleep(); // sleep for 3 second
+//         ROS_ERROR("There are not slave ID of devices MD Motor");
+//     }
+// }
+
+// /*
+//  * PID_MAIN_DATA broadcasting OFF
+//  * @param CMD_MAIN_BC_OFF 6
+//  */
+// void MD::MdController::main_BC_OFF()
+// {
+//     if(this->slave_id.size > 0)
+//     {
+//         for(uint8_t i = 0; i < this->slave_id.size; i++)
+//         {
+//             this->pid_command(this->slave_id.adr[i],CMD_MAIN_BC_OFF);
+//             usleep(30000);
+//         }
+//         this->theadEnable = false;
+//         if(this->ThreadOne.joinable() == true)
+//             this->ThreadOne.join();
+//     }else
+//     {
+//         ros::Duration(3).sleep(); // sleep for 3 second
+//         ROS_ERROR("There are not slave ID of devices MD Motor");
+//     }
+// }
+
+// /*
+//  * Read data 
+//  * @param payload       This 
+//  * @param publish_rate  Loop rate
+//  */
+// void MD::MdController::cmdMainBCdoStuff(void *payload, uint8_t *publish_rate)
+// {
+//     MD::MdController *as = (MD::MdController *)payload;
+//     ros::Rate loop_rate(*publish_rate);
+//     ROS_INFO("TheadOne to read msg devices motor is ready !!");
+//     while(ros::ok() && as->theadEnable)
+//     {
+//         ssize_t num;
+//         uint8_t msg[256];
+//         as->Read(msg,num);
+        
+//         as->cs.w_index += num;
+//         if(as->cs.w_index >= MAX_BUFFER_LENGHT) {
+//             uint8_t l_part = MAX_BUFFER_LENGHT - (as->cs.w_index - num);
+//             for(uint8_t i = 0; i < l_part; i++) as->cs._buffer[as->cs.w_index - num + i] = msg[i]; 
+//             as->cs.w_index = num - l_part;
+//             for(uint8_t i = l_part; i < num; i++) as->cs._buffer[as->cs.w_index - num + l_part + i] = msg[i]; 
+//             as->cs.c_flag++; 
+//         } 
+//         else {
+//             for(uint8_t i = 0; i < num; i++) as->cs._buffer[as->cs.w_index - num + i] = msg[i];
+//         }
+        
+//         if(as->cs.w_index - as->cs.r_index > PACKAGE_MSG_MAIN_DATA || as->cs.c_flag > 0)
+//         {
+//             if(as->cs._buffer[as->cs.r_index] == 172 && as->cs._buffer[as->cs.r_index + 1] == 183)
+//             {    
+//                 int num_data = as->cs._buffer[as->cs.r_index + 4];
+//                 if(as->cs.w_index - as->cs.r_index >= 6 + num_data)
+//                 {
+//                     as->cs.r_index += 6 + num_data;
+
+//                     if(as->cs.r_index >= MAX_BUFFER_LENGHT) 
+//                     {
+//                         as->cs.c_flag--;
+//                         uint8_t l_part = MAX_BUFFER_LENGHT - (as->cs.r_index - PACKAGE_MSG_MAIN_DATA);
+//                         for(uint8_t i = 0; i < l_part; i++) 
+//                             as->cs.result[i] = as->cs._buffer[as->cs.r_index - PACKAGE_MSG_MAIN_DATA + i];
+//                         as->cs.r_index = PACKAGE_MSG_MAIN_DATA - l_part;
+//                         for(uint8_t i = l_part; i < PACKAGE_MSG_MAIN_DATA; i++) 
+//                             as->cs.result[i] = as->cs._buffer[as->cs.r_index - PACKAGE_MSG_MAIN_DATA + l_part + i ];
+//                     }   
+//                     else 
+//                     {
+//                         for(uint8_t i = 0; i < PACKAGE_MSG_MAIN_DATA; i++) 
+//                             as->cs.result[i] = as->cs._buffer[as->cs.r_index - PACKAGE_MSG_MAIN_DATA + i];
+//                     }
+//                     std::cout << "num_data " << std::dec << num_data << " ";
+//                     // std::cout << "w_index " << std::dec << (int)(as->cs.w_index) << " r_index = " << std::dec << (int)(as->cs.r_index) << " ";
+//                     // std::cout << " c_flags = " <<  std::dec << (int)(as->cs.c_flag); 
+//                     //std::cout << " result = "; 
+//                     // for(int i = 0; i < 22; i++) 
+//                     // {
+//                     //     std::cout  << std::hex << (int)as->cs.result[i] << " ";
+//                     // }
+//                     std::cout << std:: endl;
+//                 }
+//             }
+//             else 
+//             {
+//                 std::cout << " buffer = "; 
+//                 for(int i = as->cs.r_index; i < as->cs.r_index + 22; i++) 
+//                 {
+//                     std::cout  << std::hex << (int)as->cs._buffer[i] << " ";
+//                 }
+//                 std::cout << std:: endl;
+//                 as->cs.r_index ++;   
+//             }
+//             if(as->cs.c_flag > 1)
+//             {
+//                 ROS_WARN("ERROR DATA");
+//                 exit(-1);
+//             }
+//         }
+//         std::cout << "w_index = " << std::dec << (int)(as->cs.w_index) << " r_index = " << std::dec << (int)(as->cs.r_index) << " ";
+//         std::cout << " c_flags = " <<  std::dec << (int)(as->cs.c_flag); 
+//         // std::cout << " buffer = "; 
+//         // for(int i = 0; i < 300; i++) 
+//         // {
+//         //     std::cout  << std::hex << (int)as->cs._buffer[i] << " ";
+//         // }
+//         std::cout << std:: endl;
+
+//         loop_rate.sleep();
+//         ros::spinOnce();
+//     }
+
+// }

Devices/Libraries/Ros/md_controller/src/md_controller_node.cpp

@@ -0,0 +1,146 @@
+#include <ros/ros.h>
+#include <XmlRpcValue.h>
+#include <boost/thread/thread.hpp>
+#include "md_controller/md_controller.h"
+#include "md_controller/diagnostic.h"
+#include "md_controller/md_controller_subscriber.h"
+#include "libserial/udevadm_info.h"
+#include <XmlRpcException.h>
+
+/* define struct */
+typedef struct device{
+  std::string _port;
+  int _baud;
+}device_t;
+
+
+bool findDevice(ros::NodeHandlePtr nh, std::string node_name, device_t *dv) {  
+	ROS_INFO("loadParam() - node_name: %s",  node_name.c_str());
+	if(!nh->param(node_name + "/baudrate", dv->_baud, dv->_baud)) 
+	{
+    if(!nh->getParam("baudrate", dv->_baud)) 
+      return false;
+  }
+
+  ROS_INFO("product");
+	std::string product;    //port name
+  if(!nh->param(node_name + "/product", product, product)) {}
+  else if(!nh->getParam("product", product)) {}
+  else
+  {
+    udevadmInfo *ludev = new udevadmInfo(product.c_str());
+    if(ludev->init() == EXIT_FAILURE) {}
+    if(ludev->scanDevices())
+    {
+      strcpy(dv->_port.data(), ludev->port);
+      return true;
+    }
+  }
+  
+  ROS_INFO("port_name");
+  if(!nh->param(node_name + "/port_name", dv->_port, dv->_port))
+  {
+    if(!nh->getParam("port_name", dv->_port))
+      return false;
+  }
+
+  ROS_WARN("Can not scan device from /product. So try to use /port_name is %s", dv->_port.c_str());
+  return true;
+
+}
+
+/**
+ * @brief shortcut to read a member from a XmlRpcValue, or to return a defaultvalue, it the member does not exist
+ */
+template<class T> static T readMember(XmlRpc::XmlRpcValue & value, const std::string & member, const T & defaultvalue)
+{
+  try
+  {
+    if(value.hasMember(member))
+      return value[member];
+    return defaultvalue;
+  }
+  catch(const XmlRpc::XmlRpcException& e)
+  {
+    ROS_WARN_STREAM( "'"<<member << "' "<< e.getMessage() << '\n');
+    return defaultvalue;
+  }
+}
+/**********************************************************************
+* MAIN 
+***********************************************************************/
+int main(int argc,char **argv)
+{
+  /* create ros ndoe */
+  ros::init(argc, argv, "MD");
+  ros::NodeHandlePtr nh = boost::make_shared<ros::NodeHandle>();
+  std::string node_name = ros::this_node::getName();
+  ROS_INFO("%s.cpp-node_name: %s", node_name.c_str(), node_name.c_str());
+
+  /* connect with the server */
+  std::string diagnostic_topic = "MD_diagnostic";
+  MD::Diagnostic::init(*nh, diagnostic_topic, "MD md_controller");
+
+
+  XmlRpc::XmlRpcValue drivers;
+  if(!nh->getParam("drivers", drivers) || drivers.size() < 1)
+  {
+    ROS_ERROR("%s: no driver found in yaml-file, please check configuration. Aborting...", node_name.c_str());
+    MD::Diagnostic::update(MD::DIAGNOSTIC_STATUS::CONFIGURATION_ERROR, " No driver found in yaml-file");
+    return 2;
+  }
+
+  std::vector<boost::shared_ptr<MD::Subscriber>> p_md_controller_subscriber;
+  for(XmlRpc::XmlRpcValue::iterator object_iter = drivers.begin(); object_iter != drivers.end(); ++object_iter)
+  {
+    std::string topic_name    = readMember<std::string>(object_iter->second, "toppic", "");
+    int subscribe_queue_size  = readMember<int>(object_iter->second, "subscribe_queue_size", 1);
+    ROS_INFO("%s: initializing toppic \"%s\" queue_size \"%d\"...", node_name.c_str(), topic_name.c_str(), subscribe_queue_size);
+    p_md_controller_subscriber.push_back(boost::make_shared<MD::Subscriber>(*nh, object_iter));
+  }
+
+  device_t dv;
+	if(findDevice(nh, node_name, &dv))
+  {
+		ROS_INFO("%s: found device successfull", node_name.c_str());
+	}
+  else 
+  {
+    ROS_ERROR("%s: Error when find device", node_name.c_str());
+    return 2;
+  }
+  
+  uint8_t rate = 20; // Hz
+  ros::Rate loop_rate(rate); 
+  while (ros::ok())
+  {
+    ROS_INFO("%s: Conecting port name %s with baudrate %d ...", node_name.c_str(), dv._port.c_str(), dv._baud);
+    boost::shared_ptr<MD::MdController> md_controller = boost::make_shared<MD::MdController>(nh, dv._port.c_str(), dv._baud, PARITY_NONE, DATABIT_8, STOPBIT_1);
+    while(ros::ok() && md_controller->_connected)
+    {
+      for(auto &p_sub : p_md_controller_subscriber)
+      {
+        if(p_sub->isWheelTriggered()) 
+        {
+          double rotate_speed_wheel;
+          {
+            boost::lock_guard<boost::mutex> publish_lockguard(p_sub->m_measurement_mutex);
+            rotate_speed_wheel = p_sub->speed;
+          }
+          if(abs(rotate_speed_wheel) > 0)
+            md_controller->pid_vel_cmd(p_sub->slave_id, rotate_speed_wheel);
+          else 
+            md_controller->pid_break(p_sub->slave_id);
+          p_sub->scheduleWheelController(false);
+        }
+      }
+      loop_rate.sleep();
+      ros::spinOnce();
+    }
+    ros::spinOnce();
+    md_controller->close_port();
+  }
+  ros::spin();
+  return 0;
+}
+

Devices/Libraries/Ros/md_controller/src/md_controller_subscriber.cpp

@@ -0,0 +1,102 @@
+#include "md_controller/md_controller_subscriber.h"
+#include <XmlRpcException.h>
+
+/**
+ * @brief shortcut to read a member from a XmlRpcValue, or to return a defaultvalue, it the member does not exist
+ */
+template<class T> static T readMember(XmlRpc::XmlRpcValue & value, const std::string & member, const T & defaultvalue)
+{
+  try
+  {
+    if(value.hasMember(member))
+      return value[member];
+    return defaultvalue;
+  }
+  catch(const XmlRpc::XmlRpcException& e)
+  {
+    return defaultvalue;
+  }
+  
+}
+
+MD::Subscriber::Subscriber(ros::NodeHandle & nh, const std::string & ros_topic, int subscribe_queue_size, double schedule_delay, double schedule_delay_lastest)
+ :  nh_(nh), slave_id(0), 
+    topic_name_(ros_topic), 
+    m_subscribe_queue_size_(subscribe_queue_size),
+    schedule_delay_(ros::Duration(schedule_delay)),
+    schedule_lastest_delay_(ros::Duration(schedule_delay_lastest)),
+    publish_time_(ros::Time(0)),
+    publish_lastest_time_(ros::Time(0))
+{
+  control_wheel_sub_ = nh_.subscribe(topic_name_, m_subscribe_queue_size_, &MD::Subscriber::wheelCallback, this);
+}
+
+MD::Subscriber::Subscriber(ros::NodeHandle & nh, XmlRpc::XmlRpcValue::iterator object)
+ : nh_(nh), slave_id(0),
+  publish_time_(ros::Time(0)),
+  publish_lastest_time_(ros::Time(0))
+{
+  topic_name_               = readMember<std::string>(object->second, "toppic", "");
+  m_subscribe_queue_size_   = readMember<int>(object->second, "subscribe_queue_size", 1);
+  slave_id                  = (uint8_t)readMember<int>(object->second, "id", 1);
+  ratio                     = (double)readMember<int>(object->second, "ratio", 0.0);
+  ratio                     = readMember<double>(object->second, "ratio", ratio);
+  double schedule_delay     = readMember<double>(object->second, "schedule_delay", 0.05);
+  double schedule_delay_lastest = readMember<double>(object->second, "schedule_delay_lastest", 0.5);
+
+  this->schedule_delay_ = ros::Duration(schedule_delay);
+  this->schedule_lastest_delay_ = ros::Duration(schedule_delay_lastest);
+  control_wheel_sub_ = nh_.subscribe(topic_name_, m_subscribe_queue_size_, &MD::Subscriber::wheelCallback, this);
+}
+
+MD::Subscriber::~Subscriber() = default;
+
+
+void MD::Subscriber::wheelCallback(const std_msgs::Float32::Ptr & msg)
+{
+  boost::lock_guard<boost::mutex> schedule_lockguard(this->m_measurement_mutex);
+  // check that we don't have multiple publishers on the command topic
+  if (control_wheel_sub_.getNumPublishers() > 1)
+  {
+    ROS_ERROR_STREAM_THROTTLE(1.0, "Detected " << control_wheel_sub_.getNumPublishers()
+      << " publishers " << topic_name_ << ". Only 1 publisher is allowed. Going to brake.");
+    speed = 0; 
+    return;
+  }
+
+  if (!std::isfinite(msg->data))
+  {
+    ROS_WARN_THROTTLE(1.0, "Received NaN in %s command. Ignoring.", topic_name_.c_str());
+    speed = 0;
+    return;
+  }
+  speed = (int16_t)(msg->data * ratio * 60 / 2 / M_PI);
+  this->scheduleWheelController(true);
+}
+
+bool MD::Subscriber::isWheelTriggered(void)
+{
+  boost::lock_guard<boost::mutex> schedule_lockguard(publish_mutex_);
+  return !publish_time_.isZero() && ros::Time::now() > publish_time_;
+}
+
+bool MD::Subscriber::isWheelLastestTriggered(void)
+{
+  boost::lock_guard<boost::mutex> schedule_lockguard(publish_mutex_);
+  return !publish_lastest_time_.isZero() && ros::Time::now() > publish_lastest_time_;
+}
+
+void MD::Subscriber::scheduleWheelController(bool schedule)
+{
+  boost::lock_guard<boost::mutex> schedule_lockguard(publish_mutex_);
+  if(schedule && publish_time_.isZero())
+  {
+    publish_time_ = ros::Time::now() + schedule_delay_;
+    publish_lastest_time_ = ros::Time::now() + schedule_lastest_delay_;
+  }
+  if(!schedule && !publish_time_.isZero())
+  {
+    publish_time_ = ros::Time(0);
+    publish_lastest_time_ = ros::Time(0);
+  }
+}

Devices/Libraries/Ros/modbus/README.md

@@ -0,0 +1 @@
+# modbus

Devices/Libraries/Ros/modbus/modbus_rtu/CMakeLists.txt

@@ -0,0 +1,38 @@
+cmake_minimum_required(VERSION 3.0.2)
+project(modbus_rtu)
+
+find_package(catkin REQUIRED COMPONENTS
+  roscpp
+  std_msgs
+  libserial
+)
+
+catkin_package(
+  INCLUDE_DIRS include
+  LIBRARIES modbus_rtu
+  CATKIN_DEPENDS roscpp std_msgs libserial
+#  DEPENDS system_lib
+)
+
+include_directories(
+  include
+  ${catkin_INCLUDE_DIRS}
+)
+
+## Declare a C++ library
+add_library(${PROJECT_NAME} src/modbus_rtu.cpp)
+add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+target_link_libraries(${PROJECT_NAME} ${Boost_INCLUDE_DIRS} ${catkin_LIBRARIES})
+
+install(TARGETS ${PROJECT_NAME}
+  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
+)
+
+## Mark cpp header files for installation
+install(DIRECTORY include/${PROJECT_NAME}/
+  DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+  FILES_MATCHING PATTERN "*.h"
+  PATTERN ".svn" EXCLUDE
+)

Devices/Libraries/Ros/modbus/modbus_rtu/include/modbus_rtu/define.h

@@ -0,0 +1,40 @@
+#pragma once
+
+/*
+ * Exception Codes
+ */
+#define    EX_PARAM_NOT_EXIT       0x00 /* The parameter number does not exits */
+#define    EX_ACCESS_TO_PARAM      0x01 /* There is no write access to the parameter */
+#define    EX_PARAM_LIMITS         0x02 /* The data value exceeds the parameter limits */
+#define    EX_SUB_INDEX_NO_EXIT    0x03 /* The sub-index in use does not exits */
+#define    EX_PARAM_NOT_TYPE       0x04 /* The parameter is not of the array type */
+#define    EX_TYPE_NO_MATCH        0x05 /* The data type does not match the parameter called */
+#define    EX_ONLY_RESET           0x06 /* Only reset */
+#define    EX_NOT_CHANGEABLE       0x07 /* Not changeable */
+#define    EX_NO_WRITE_ACCESS      0x0B /* No write address */
+#define    EX_NO_POSSIBLE_PRE_MODE 0x11 /* Data change in the parameter called is not possible in the present mode */
+#define    EX_OTHER_ERROR          0x12 /* Other error */
+#define    EX_INVALID_DATA_ADDR    0x40 /* Invalid data address */
+#define    EX_INVALID_MSGS_LENGTH  0x41 /* Invalid message length */
+#define    EX_INVALID_LENGTH_VALUE 0x42 /* Invalid data length or value */
+#define    EX_INVALID_FUNC_CODE    0x43 /* Invalid function code */
+#define    EX_NO_BUS_ACCESS        0x82 /* There is no bus access */
+#define    EX_DATA_NOT_POSSIBLE    0x83 /* Data change is not possible because factory set-up is selected */
+#define    EX_BAD_DATA             0XFF /* Bad Data lenght or Address */
+
+#define    BAD_CON                 -1
+#define    BAD_CONFIG              -2
+#define    WORD_BITS               65535
+#define    DATA_BITS               16
+/*
+ * Commonly use function codes
+ */
+
+#define    READ_COIL_BITS              0x01 /* Read coils status */
+#define    READ_INPUT_BITS             0x02 /* Read input status */
+#define    READ_HOLDING_REGS           0x03 /* Read holding registers */
+#define    READ_INPUT_REGS             0x04 /* Read input register */
+#define    WRITE_SINGLE_COIL           0x05 /* Write single coil status */
+#define    WRITE_SINGLE_HOLDING_REG    0x06 /* Write single register */
+#define    WRITE_MULTIPLE_COILS        0X0F /* Multiple coil write */ 
+#define    WRITE_MULTIPLE_HOLDING_REGS 0X10 /* Multiple register write*/

Devices/Libraries/Ros/modbus/modbus_rtu/include/modbus_rtu/modbus_rtu.h

@@ -0,0 +1,152 @@
+#ifndef __MOSBUS_RTU_H_INCLUDE_
+#define __MOSBUS_RTU_H_INCLUDE_
+#include <iostream>
+#include <stdio.h>
+#include <cstdint>	/* uin8_t */
+#include "modbus_rtu/define.h"
+#include "libserial/rs485.h"
+
+class modbus_rtu : public rs485{
+    public:
+
+        /*
+        * @brief Constructor 
+        * @param[in] poly      The poly number to caculartion CRC16
+        * @param[in] devfile   Example /dev/tty* 
+        * @param[in] baud      Number of transmitted bits  per a second
+        * @param[in] parity    The parity check bit {Even, None , Old }
+        * @param[in] data_bit  Number of bits in a transmission frame 
+        * @param[in] stop_bit  End bit
+        */
+        modbus_rtu(uint16_t poly, const char* devfile, unsigned int baud, parity_t parity, data_bits_t data_bit,stop_bits_t stop_bit);
+        /*
+        * Destructor.
+        */
+        virtual ~modbus_rtu();
+
+        /*
+        * Read Holding Registers
+        * @brief MODBUS     FUNCTION 0x03
+        * @param[in] slave_id   The id on modbus device 
+        * @param[in] address    Reference Address
+        * @param[in] amount     Amount of Registers to Read
+        * @param[in] buffer     Buffer to Store Data Read from Registers
+        */
+        virtual int modbus_read_holding_registers(uint8_t slave_id, int address, int amount, uint16_t *buffer);
+        
+        /*
+        * Read Input Registers
+        * @brief MODBUS FUNCTION 0x04
+        * @param[in] slave_id    The id on modbus device 
+        * @param[in] address     Reference Address
+        * @param[in] amount      Amount of Registers to Read
+        * @param[in] buffer      Buffer to Store Data Read from Registers
+        */
+        virtual int modbus_read_input_registers(uint8_t slave_id, int address, int amount, uint16_t *buffer);
+
+        /*
+        * Write Single Register
+        * @brief FUCTION 0x06
+        * @param[in] slave_id    The id on modbus device 
+        * @param[in] address   Reference Address
+        * @param[in] value     Value to Be Written to Register
+        */
+        virtual int modbus_write_register(uint8_t slave_id, int address, const uint16_t& value);
+    
+        /*
+        * Write Multiple Registers
+        * @brief MODBUS FUNCION 0x10
+        * @param[in] slave_id    The id on modbus device 
+        * @param[in] address Reference Address
+        * @param[in] amount  Amount of Value to Write
+        * @param[in] value   Values to Be Written to the Registers
+        */
+        int modbus_write_registers(uint8_t slave_id, int address, int amount, const uint16_t *value);
+
+        /*
+        * @brief Mb_calcul_crc : compute the crc of a packet and put it at the end
+        * @param[in] msg         Message to send 
+        * @param[in] length      The length of message to send
+        * @param[in] poly        The poly number to caculartion CRC16
+        * @return CRC            Result is CRC value 16 bit
+        */
+        virtual uint16_t getCRC16(uint8_t *to_send, uint16_t length);
+        /*
+        * @brief Mb_calcul_crc : compute the crc of a packet and put it at the end
+        * @param[in] msg         Message to send 
+        * @param[in] length      The length of message to send
+        * @param[in] poly        The poly number to caculartion CRC16
+        * @return bool           Result is CRC value true/false
+        */
+        virtual bool checkCRC16(uint8_t *to_send, uint16_t length);
+        /*
+        * Modbus Request Builder
+        * @param[in] slave_id   The id on modbus device
+        * @param[in] to_send   Message Buffer to Be Sent
+        * @param[in] address   Reference Address
+        * @param[im] func      Modbus Functional Code
+        */
+        inline void modbus_build_request(uint8_t slave_id, uint8_t *to_send, int address, uint8_t func) const;
+        
+        /*
+        * Read Request Builder and Sender
+        * @param[in] slave_id   The id on modbus device
+        * @param[in] address   Reference Address
+        * @param[in] amount    Amount of Data to Read
+        * @param[in] func      Modbus Functional Code
+        */
+        ssize_t modbus_read(uint8_t slave_id, int address, uint amount, int func);
+
+        /*
+        * Write Request Builder and Sender
+        * @param[in] slave_id   The id on modbus device
+        * @param[in] address   Reference Address
+        * @param[in] amount    Amount of data to be Written
+        * @param[in] func      Modbus Functional Code
+        * @param[in] value     Data to Be Written
+        */
+        ssize_t modbus_write(uint8_t slave_id, int address, uint amount, int func, const uint16_t *value);
+        
+        /*
+        * Data Sender
+        * @param[in] to_send Request to Be Sent to Server
+        * @param[in] length  Length of the Request
+        * @param[in]        Size of the request
+        */
+        inline ssize_t modbus_send(uint8_t *to_send, uint16_t length);
+
+        /*
+        * Data Receiver
+        * @param[in] buffer Buffer to Store the Data Retrieved
+        * @return       Size of Incoming Data
+        */
+        inline ssize_t modbus_receive(uint8_t *buffer);
+
+        /*
+        * Error Code Handler
+        * @param[in] msg   Message Received from the Server
+        * @param[in] func  Modbus Functional Code
+        */
+        virtual void modbuserror_handle(const uint8_t *msg, int func);
+
+        /*
+        * Set Bad Data lenght or Address
+        */
+        inline void set_bad_input(void);
+
+        /*
+        * Set Bad connection
+        */
+        virtual void set_bad_con(void);
+        /**
+         * Set config faild
+         */
+        virtual void setConfigFaild(void);
+        
+    private:
+        /* Properties */
+        bool err{};
+        int err_no{};
+        uint16_t _poly;          /* POLY is const number to cacurla CRC16 */
+    };
+#endif

Devices/Libraries/Ros/modbus/modbus_rtu/package.xml

@@ -0,0 +1,67 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>modbus_rtu</name>
+  <version>0.0.0</version>
+  <description>The modbus_rtu package</description>
+
+  <!-- One maintainer tag required, multiple allowed, one person per tag -->
+  <!-- Example:  -->
+  <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+  <maintainer email="robotics@todo.todo">robotics</maintainer>
+
+
+  <!-- One license tag required, multiple allowed, one license per tag -->
+  <!-- Commonly used license strings: -->
+  <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
+  <license>TODO</license>
+
+
+  <!-- Url tags are optional, but multiple are allowed, one per tag -->
+  <!-- Optional attribute type can be: website, bugtracker, or repository -->
+  <!-- Example: -->
+  <!-- <url type="website">http://wiki.ros.org/modbus_rtu</url> -->
+
+
+  <!-- Author tags are optional, multiple are allowed, one per tag -->
+  <!-- Authors do not have to be maintainers, but could be -->
+  <!-- Example: -->
+  <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
+
+
+  <!-- The *depend tags are used to specify dependencies -->
+  <!-- Dependencies can be catkin packages or system dependencies -->
+  <!-- Examples: -->
+  <!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
+  <!--   <depend>roscpp</depend> -->
+  <!--   Note that this is equivalent to the following: -->
+  <!--   <build_depend>roscpp</build_depend> -->
+  <!--   <exec_depend>roscpp</exec_depend> -->
+  <!-- Use build_depend for packages you need at compile time: -->
+  <!--   <build_depend>message_generation</build_depend> -->
+  <!-- Use build_export_depend for packages you need in order to build against this package: -->
+  <!--   <build_export_depend>message_generation</build_export_depend> -->
+  <!-- Use buildtool_depend for build tool packages: -->
+  <!--   <buildtool_depend>catkin</buildtool_depend> -->
+  <!-- Use exec_depend for packages you need at runtime: -->
+  <!--   <exec_depend>message_runtime</exec_depend> -->
+  <!-- Use test_depend for packages you need only for testing: -->
+  <!--   <test_depend>gtest</test_depend> -->
+  <!-- Use doc_depend for packages you need only for building documentation: -->
+  <!--   <doc_depend>doxygen</doc_depend> -->
+  <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>roscpp</build_depend>
+  <build_depend>std_msgs</build_depend>
+  <build_export_depend>roscpp</build_export_depend>
+  <build_export_depend>std_msgs</build_export_depend>
+  <exec_depend>roscpp</exec_depend>
+  <exec_depend>std_msgs</exec_depend>
+
+  <build_depend>libserial</build_depend>
+  <exec_depend>libserial</exec_depend>
+
+  <!-- The export tag contains other, unspecified, tags -->
+  <export>
+    <!-- Other tools can request additional information be placed here -->
+
+  </export>
+</package>

Devices/Libraries/Ros/modbus/modbus_rtu/src/modbus_rtu.cpp

@@ -0,0 +1,450 @@
+#include "modbus_rtu/modbus_rtu.h"
+
+/** 
+ * @brief Constructor 
+ * @param[in] poly      The poly number to caculartion CRC16
+ * @param[in] devfile   Example /dev/tty* 
+ * @param[in] baud      Number of transmitted bits  per a second
+ * @param[in] parity    The parity check bit {Even, None , Old }
+ * @param[in] data_bit  Number of bits in a transmission frame 
+ * @param[in] stop_bit  End bit
+ */
+modbus_rtu::modbus_rtu(uint16_t poly, const char* devfile, unsigned int baud, parity_t parity, data_bits_t data_bit,stop_bits_t stop_bit) : _poly(poly), rs485(devfile, baud, parity, data_bit, stop_bit){
+    err = false;
+    err_no = 0;
+    error_msg = "";
+    ROS_INFO("Set POLY value is 0x%x", _poly);
+    if(setCharacterNumToRead(5) < 0) {setConfigFaild();}
+}      
+
+/** 
+ * Destructor.
+ */                       
+modbus_rtu::~modbus_rtu(){}
+
+/**
+ * Read Holding Registers
+ * MODBUS FUNCTION 0x03
+ * @param[in] slave_id   The id on modbus device
+ * @param[in] address    Reference Address
+ * @param[in] amount     Amount of Registers to Read
+ * @param[in] buffer     Buffer to Store Data Read from Registers
+ */
+int modbus_rtu::modbus_read_holding_registers(uint8_t slave_id, int address, int amount, uint16_t *buffer){
+    if(_connected){
+        if(address > WORD_BITS || amount > DATA_BITS){
+            set_bad_input();
+            return EX_BAD_DATA;
+        }
+        modbus_read(slave_id, address, amount, READ_HOLDING_REGS);
+        //if(setCharacterNumToRead(5 + amount * 2) < 0) {setConfigFaild(); return BAD_CONFIG;}
+        uint8_t to_rec[MAX_MSG_LENGTH];
+        memset(to_rec,'\0',MAX_MSG_LENGTH);
+        ssize_t k = 0;
+        k = modbus_receive(to_rec);
+        if (k == -1 || k == 65535) {return BAD_CON;}
+        if(k == 5 + amount * 2) {
+            if(!checkCRC16(to_rec,k)){
+                ROS_ERROR("modbus_read_holding_registers");
+                set_bad_input();
+                return EX_BAD_DATA;
+            }
+            modbuserror_handle(to_rec, READ_HOLDING_REGS);
+            if(err) return err_no;
+            for(int i = 0; i < to_rec[2u]/2; i++){
+                buffer[i] = ((uint16_t)to_rec[3u + 2u * i]) << 8u;
+                buffer[i] |= ((uint16_t)to_rec[4u + 2u * i] & 0x00FFu);
+            }
+            return 0;
+        } else {
+                ROS_ERROR("receive message is not correct %d ", (int)k);
+                for(int i = 0; i < k; i++) printf("0x%x ",to_rec[i]);
+                printf("\n");
+                set_bad_input();
+                return EX_BAD_DATA;
+            }
+    } else {
+        return BAD_CON;
+    }
+}
+
+/**
+ * Read Input Registers
+ * MODBUS FUNCTION 0x04
+ * @param[in] slave_id    The id on modbus device 
+ * @param[in] address     Reference Address
+ * @param[in] amount      Amount of Registers to Read
+ * @param[in] buffer      Buffer to Store Data Read from Registers
+ */
+int modbus_rtu::modbus_read_input_registers(uint8_t slave_id, int address, int amount, uint16_t *buffer){
+    if(_connected){
+        if(amount > DATA_BITS || address > WORD_BITS){
+            set_bad_input();
+            return EX_BAD_DATA;           
+        }
+        modbus_read(slave_id, address, amount, READ_INPUT_REGS);
+        //if(setCharacterNumToRead(5 + amount * 2) < 0) {setConfigFaild(); return BAD_CONFIG;}
+        uint8_t to_rec[MAX_MSG_LENGTH];
+        memset(to_rec,'\0',MAX_MSG_LENGTH);
+        ssize_t k = 0;
+        k = modbus_receive(to_rec);
+        if (k == -1 || k == 65535) {return BAD_CON;}
+        if(k > 0){
+            if(!checkCRC16(to_rec,k)) {
+                ROS_ERROR("modbus_read_input_registers");
+                set_bad_input();
+                return EX_BAD_DATA;
+            }
+            modbuserror_handle(to_rec, READ_INPUT_REGS);
+            if(err) return err_no; 
+            for(int i = 0; i < to_rec[2u]/2; i++){
+                buffer[i] = ((uint16_t)to_rec[3u + 2u * i]) << 8u;
+                buffer[i] |= ((uint16_t)to_rec[4u + 2u * i] & 0x00FFu);
+            }
+            return 0;
+        } else {
+                ROS_ERROR("Slave no response !!");
+                set_bad_input();
+                return EX_BAD_DATA;
+            }
+    } else {
+        return BAD_CON;
+    }
+}
+
+/**
+ * Write Single Register
+ * @brief FUCTION 0x06
+ * @param[in] slave_id      The id on modbus device 
+ * @param[in] address       Reference Address
+ * @param[in] value         Value to Be Written to Register
+ */
+int modbus_rtu::modbus_write_register(uint8_t slave_id, int address, const uint16_t& value) {
+    if(_connected){
+        if(address > WORD_BITS){
+            set_bad_input();
+            return EX_BAD_DATA;
+        }
+        modbus_write(slave_id, address, 1, WRITE_SINGLE_HOLDING_REG, &value);
+        //if(setCharacterNumToRead(8) < 0 ) {setConfigFaild(); return BAD_CONFIG;}
+        uint8_t to_rec[MAX_MSG_LENGTH];
+        memset(to_rec,'\0',MAX_MSG_LENGTH);
+        ssize_t k = 0;
+        k = modbus_receive(to_rec);
+        if (k == -1 || k == 65535) {return BAD_CON;}
+        if(k == 8){ 
+            // for(int i = 0; i < k; i++) ROS_INFO("0x%x",to_rec[i]);
+            if(!checkCRC16(to_rec, k)) {
+                ROS_ERROR("modbus_write_register");
+                set_bad_input();
+                return EX_BAD_DATA;
+            }
+            modbuserror_handle(to_rec, WRITE_SINGLE_HOLDING_REG);
+            if(err) return err_no;
+            return 0;
+        } else {
+                ROS_ERROR("Slave no response !!");
+                set_bad_input();
+                return EX_BAD_DATA;
+            }
+    } else {
+        return BAD_CON;
+    }
+}
+
+/**
+ * Write Multiple Registers
+ * @brief MODBUS FUNCION 0x10
+ * @param[in] slave_id      The id on modbus device
+ * @param[in] address   Reference Address
+ * @param[in] amount    Amount of Value to Write
+ * @param[in] value     Values to Be Written to the Registers
+ */
+int modbus_rtu::modbus_write_registers(uint8_t slave_id, int address, int amount, const uint16_t *value){
+    if(_connected){
+        if(amount > DATA_BITS || address > WORD_BITS){
+            set_bad_input();
+            return EX_BAD_DATA;
+        }
+        modbus_write(slave_id, address, amount, WRITE_MULTIPLE_HOLDING_REGS, value);
+        //if(setCharacterNumToRead(8) < 0) {setConfigFaild(); return BAD_CONFIG;}
+        uint8_t to_rec[MAX_MSG_LENGTH];
+        memset(to_rec,'\0',MAX_MSG_LENGTH);
+        ssize_t k = 0;
+        k = modbus_receive(to_rec);
+        if (k == -1 || k == 65535) {return BAD_CON;}
+        if(k == 8) { 
+            if(!checkCRC16(to_rec, k)){
+                ROS_ERROR("modbus_write_registers");
+                set_bad_input();
+                return EX_BAD_DATA;
+            }
+            modbuserror_handle(to_rec, WRITE_MULTIPLE_HOLDING_REGS);
+            if(err) return err_no;
+            return 0;
+        } else {
+                ROS_ERROR("Slave no response !!");
+                set_bad_input();
+                return EX_BAD_DATA;
+            }
+    } else {
+        return BAD_CON;
+    }
+}
+
+/**
+ * @brief Mb_calcul_crc : compute the crc of a packet and put it at the end
+ * @param[in] msg         Message to send 
+ * @param[in] length      The length of message to send
+ * @param[in] poly        The poly number to caculartion CRC16
+ * @return CRC            Result is CRC value 16 bit
+ */
+uint16_t modbus_rtu::getCRC16(uint8_t *to_send, uint16_t length){
+    /*
+        EX: Ban đầu CRC = 1111 1111 1111 1111 chuyển sang Hex là FFFF
+        Chọn data_p là 54 hay 0101 0100(1 byte) là số cần tính. 
+        Chọn số poly =  A001h hay 1010 000 000 0001 
+        (Poly là một số mà bạn sử dụng làm phép tính số CRC cơ sở của mình.)
+
+        + Bước 1: Dịch CRC và data_p sang phải 1 bit 
+        data_p = 54, là 0101 0100 trở thành 0010 1010 
+        CRC = 1111 1111 1111 1111 trở thành 0111 1111 1111 1111
+        
+        + Bước 2: Kiểm tra BIT ngoài cùng bên phải của Dữ liệu và so sánh nó với một trong các CRC
+        NẾU chúng bằng nhau, dịch chuyển CRC sang phải 1 bit 
+        NẾU chúng không phải, dịch chuyển CRC sang phải 1 bit VÀ cộng thêm số Poly một lần nữa.
+        Thực hiện bước 2 đúng 8 lần vì 1 byte có 8 bit.
+
+        +Bước 3: Bước 1 và 2 sẽ được lăp lại theo số lượng data_p.
+    */
+    unsigned char i;
+	unsigned int data;
+	unsigned int crc = 0xffff;
+    do {
+	    for (i=0, data=(unsigned int)0xff & *to_send++; i < 8; i++, data >>= 1) {
+	        if ((crc & 0x0001) ^ (data & 0x0001))
+	            crc = (crc >> 1) ^ _poly;
+	        else  crc >>= 1;
+	    }
+	} while (--length);
+	return (crc);
+}
+
+/**
+ * @brief Mb_calcul_crc : compute the crc of a packet and put it at the end
+ * @param[in] msg         Message to send 
+ * @param[in] length      The length of message to send
+ * @param[in] poly        The poly number to caculartion CRC16
+ * @return bool           Result is CRC value true/false
+ */
+bool modbus_rtu::checkCRC16(uint8_t *to_send, uint16_t length){
+    uint16_t to_check;
+    to_check = getCRC16(to_send,length - 2);
+    for(int i = 0; i < length; i++) printf("0x%x ",to_send[i]);
+    printf("length(%d) crc16(0x%x 0x%x) 0x%x 0x%x ", (int)length, to_check & 0x00FFu, to_check >> 8u, to_send[length - 2], to_send[length - 1]);
+    printf("\n");
+    return to_send[length - 2] == (uint8_t)(to_check & 0x00FFu) && to_send[length - 1] == (uint8_t)(to_check >> 8u);
+}
+
+/*
+ * Modbus Request Builder
+ * @param[in] slave_id   The id on modbus device
+ * @param[in] to_send   Message Buffer to Be Sent
+ * @param[in] address   Reference Address
+ * @param[in] func      Modbus Functional Code
+ */
+void modbus_rtu::modbus_build_request(uint8_t slave_id, uint8_t *to_send, int address, uint8_t func) const {
+    to_send[0] = slave_id;
+    to_send[1] = func;
+    to_send[2] = (uint8_t) (address >> 8u);
+    to_send[3] = (uint8_t) (address & 0x00FFu);
+}
+
+/*
+ * Write Request Builder and Sender
+ * @param[in] slave_id   The id on modbus device
+ * @param[in] address   Reference Address
+ * @param[in] amount    Amount of data to be Written
+ * @param[in] func      Modbus Functional Code
+ * @param[in] value     Data to Be Written
+ */
+ssize_t modbus_rtu::modbus_write(uint8_t slave_id, int address, uint amount, int func, const uint16_t *value) {
+    ssize_t result = 0;
+    if(func == WRITE_SINGLE_COIL || func == WRITE_SINGLE_HOLDING_REG){
+        uint8_t to_send[8];
+        modbus_build_request(slave_id, to_send, address, func);
+        to_send[4] = (uint8_t) (value[0] >> 8u);
+        to_send[5] = (uint8_t) (value[0] & 0x00FFu);
+        uint16_t crc = getCRC16(to_send,6);
+        to_send[6] = (uint8_t) (crc & 0x00FFu);
+        to_send[7] = (uint8_t) (crc >> 8u);
+        // for(int i = 0; i < 8; i++) ROS_INFO("0x%x", to_send[i]);
+        result = modbus_send(to_send,8);
+    } else if(func == WRITE_MULTIPLE_HOLDING_REGS){
+        uint8_t to_send[9 + 2 * amount];
+        modbus_build_request(slave_id,to_send, address, func);
+        to_send[4] = (uint8_t)(amount >> 8u);
+        to_send[5] = (uint8_t)(amount & 0x00FFu);
+        to_send[6] = (uint8_t)(amount * 2);
+        for(int i = 0; i < amount; i++){
+            to_send[7 + 2 * i] = (uint8_t)(value[i] >> 8u);
+            to_send[8 + 2 * i] = (uint8_t)(value[i] & 0x00FFu);
+        }
+        uint16_t crc = getCRC16(to_send, 9 + 2 * amount - 2);
+        to_send[9 + 2 * amount - 2] = (uint8_t)(crc & 0x00FFu);
+        to_send[9 + 2 * amount - 1] = (uint8_t)(crc >> 8u);
+        //for(int i = 0; i < 9 + 2 * amount; i++) ROS_INFO("0x%x", to_send[i]);
+        result = modbus_send(to_send, 9 + 2 * amount);
+    }
+    return result;
+}
+/*
+ * Read Request Builder and Sender
+ * @param[in] slave_id   The id on modbus device
+ * @param[in] address   Reference Address
+ * @param[in] amount    Amount of Data to Read
+ * @param[in] func      Modbus Functional Code
+ */
+ssize_t modbus_rtu::modbus_read(uint8_t slave_id, int address, uint amount, int func){
+    uint8_t to_send[8];
+    modbus_build_request(slave_id, to_send, address, func);
+    to_send[4] = (uint8_t) (amount >> 8u);
+    to_send[5] = (uint8_t) (amount & 0x00FFu);
+    uint16_t crc = getCRC16(to_send,6);
+    to_send[6] = (uint8_t) (crc & 0x00FFu);
+    to_send[7] = (uint8_t) (crc >> 8u);
+    return modbus_send(to_send, 8);
+}
+
+/*
+ * Data Sender
+ * @param[in] to_send Request to Be Sent to Server
+ * @param[in] length  Length of the Request
+ * @return        Size of the request
+ */
+ssize_t modbus_rtu::modbus_send(uint8_t *to_send, uint16_t length){
+    struct stat sb;
+    if(stat(ctx._port, &sb) < 0) reconnect();
+    std::cout << std::endl; 
+    return sendMsgs(to_send, length);
+}
+
+/*
+ * Data Receiver
+ * @param[in] buffer Buffer to Store the Data Retrieved
+ * @return       Size of Incoming Data
+ */
+ssize_t modbus_rtu::modbus_receive(uint8_t *buffer) {
+    struct timeval timeout;
+    timeout.tv_sec = 2;
+	timeout.tv_usec = 0;
+    fd_set set;
+    FD_ZERO(&set); /* clear the set */
+	FD_SET(ctx.s, &set); /* add our file descriptor to the set */
+    int rv = select(ctx.s + 1, &set, NULL, NULL, &timeout);
+    if(rv == -1){
+        err = true;
+        _connected = false;
+        error_msg = "Bad connection select";
+        return BAD_CON;
+    } else if(rv == 0) {
+        err = true;
+        _connected = false;
+     	error_msg = "Bad connection timeout";  /* an timeout occured */
+        return BAD_CON;
+    } else
+        return receiveMsgs(buffer);
+}
+
+/*
+ * Error Code Handler
+ * @param[in] msg   Message Received from the Server
+ * @param[in] func  Modbus Functional Code
+ */
+void modbus_rtu::modbuserror_handle(const uint8_t *msg, int func){
+    if(msg[1] == func + 0x80){
+        err = true;
+        err_no = 1;
+        switch(msg[2]){
+        case EX_PARAM_NOT_EXIT:
+            error_msg = "The parameter number does not exits";
+            break;
+        case EX_ACCESS_TO_PARAM:
+            error_msg = "There is no write access to the parameter";
+            break;
+        case EX_PARAM_LIMITS:
+            error_msg = "The data value exceeds the parameter limits";
+            break;
+        case EX_SUB_INDEX_NO_EXIT:
+            error_msg = "The sub-index in use does not exits";
+            break;
+        case EX_PARAM_NOT_TYPE:
+            error_msg = "The parameter is not of the array type";
+            break;
+        case EX_TYPE_NO_MATCH:
+            error_msg = "The data type does not match the parameter called";
+            break;
+        case EX_ONLY_RESET:
+            error_msg = "Only reset";
+            break;
+        case EX_NOT_CHANGEABLE:
+            error_msg = "Not changeable";
+            break;
+        case EX_NO_WRITE_ACCESS:
+            error_msg = "No write address";
+            break;
+        case EX_NO_POSSIBLE_PRE_MODE:
+            error_msg = "Data change in the parameter called is not possible in the present mode";
+            break;
+        case EX_OTHER_ERROR:
+            error_msg = "Other error";
+            break;      
+        case EX_INVALID_DATA_ADDR:
+            error_msg = "Invalid data address";
+            break; 
+        case EX_INVALID_MSGS_LENGTH:
+            error_msg = "Invalid message length";
+            break; 
+        case EX_INVALID_LENGTH_VALUE:
+            error_msg = "Invalid data length or value";
+            break; 
+        case EX_INVALID_FUNC_CODE:
+            error_msg = "Invalid function code";
+            break; 
+        case EX_NO_BUS_ACCESS:
+            error_msg = "There is no bus access";
+            break; 
+        case EX_DATA_NOT_POSSIBLE:
+            error_msg = "Data change is not possible because factory set-up is selected";
+            break;  
+        }
+    }
+    err = false;
+    error_msg = "NO ERR";
+}
+
+/*
+ * Set Bad Data lenght or Address
+ */
+void modbus_rtu::set_bad_input(void) {
+    err = true;
+    error_msg = "Bad Data lenght or Address";
+}
+
+/*
+ * Set Bad connection
+ */
+void modbus_rtu::set_bad_con(void) {
+    err = true;
+    _connected = false;
+    error_msg = "Bad connection";
+}
+
+/**
+ * Set config faild
+ */
+void modbus_rtu::setConfigFaild(void){
+    err = true;
+    _connected = false;
+    error_msg = "Driver config failded";
+}

Devices/Libraries/Ros/modbus/modbus_tcp/CMakeLists.txt

@@ -0,0 +1,78 @@
+cmake_minimum_required(VERSION 3.0.2)
+project(modbus_tcp)
+
+find_package(catkin REQUIRED COMPONENTS
+  roscpp
+  std_msgs
+)
+
+catkin_package(
+  INCLUDE_DIRS include
+  LIBRARIES modbus_tcp
+  CATKIN_DEPENDS roscpp std_msgs
+#  DEPENDS system_lib
+)
+
+include_directories(
+  include
+  ${catkin_INCLUDE_DIRS}
+)
+
+add_library(${PROJECT_NAME} src/modbus.cpp)
+add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_TARGETS})
+target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} ${Boost_LIBRARIES} ${GSTREAMER_LIBRARIES})
+
+#############
+## Install ##
+#############
+
+# all install targets should use catkin DESTINATION variables
+# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
+
+## Mark executable scripts (Python etc.) for installation
+## in contrast to setup.py, you can choose the destination
+# catkin_install_python(PROGRAMS
+#   scripts/my_python_script
+#   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark executables for installation
+## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html
+# install(TARGETS ${PROJECT_NAME}_node
+#   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark libraries for installation
+## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html
+install(TARGETS ${PROJECT_NAME}
+  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
+)
+
+## Mark cpp header files for installation
+install(DIRECTORY include/${PROJECT_NAME}/
+  DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+  FILES_MATCHING PATTERN "*.h"
+  PATTERN ".svn" EXCLUDE
+)
+
+## Mark other files for installation (e.g. launch and bag files, etc.)
+# install(FILES
+#   # myfile1
+#   # myfile2
+#   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+# )
+
+#############
+## Testing ##
+#############
+
+## Add gtest based cpp test target and link libraries
+# catkin_add_gtest(${PROJECT_NAME}-test test/test_modbus_tcp.cpp)
+# if(TARGET ${PROJECT_NAME}-test)
+#   target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
+# endif()
+
+## Add folders to be run by python nosetests
+# catkin_add_nosetests(test)

Devices/Libraries/Ros/modbus/modbus_tcp/include/modbus_tcp/modbus.h

@@ -0,0 +1,240 @@
+#ifndef MODBUS_H
+#define MODBUS_H
+#pragma once 
+#include <ros/ros.h>
+#include <cstring>
+#include <stdint.h>
+#include <iostream>
+#include <sys/socket.h>
+#include <netinet/in.h>
+#include <arpa/inet.h>
+#include <unistd.h>
+
+
+#define MAX_MSG_LENGTH 260
+
+///Function Code
+#define     READ_COILS        0x01
+#define     READ_INPUT_BITS   0x02
+#define     READ_REGS         0x03
+#define     READ_INPUT_REGS   0x04
+#define     WRITE_COIL        0x05
+#define     WRITE_REG         0x06
+#define     WRITE_COILS       0x0F
+#define     WRITE_REGS        0x10
+
+///Exception Codes
+
+#define    EX_ILLEGAL_FUNCTION  0x01 // Function Code not Supported
+#define    EX_ILLEGAL_ADDRESS   0x02 // Output Address not exists
+#define    EX_ILLEGAL_VALUE     0x03 // Output Value not in Range
+#define    EX_SERVER_FAILURE    0x04 // Slave Deive Fails to process request
+#define    EX_ACKNOWLEDGE       0x05 // Service Need Long Time to Execute
+#define    EX_SERVER_BUSY       0x06 // Server Was Unable to Accept MB Request PDU
+#define    EX_NEGATIVE_ACK      0x07
+#define    EX_MEM_PARITY_PROB   0x08
+#define    EX_GATEWAY_PROBLEMP  0x0A // Gateway Path not Available
+#define    EX_GATEWYA_PROBLEMF  0x0B // Target Device Failed to Response
+#define    EX_BAD_DATA          0XFF // Bad Data lenght or Address
+
+#define    BAD_CON              -1
+
+/**
+ * @class Modbus Operator Class
+ * @brief Providing networking support and mobus operation support.
+ */
+class modbus {
+public:
+  /**
+  * Main Constructor of Modbus Connector Object
+  * @param host IP Address of Host
+  * @param port Port for the TCP Connection
+  * @return     A Modbus Connector Object
+  */
+  modbus(std::string ip_address, int port);
+
+  /**
+   * Destructor of Modbus Connector Object
+   */
+  virtual ~modbus();
+
+  /**
+   * Modbus Slave ID Setter
+   * @param id  ID of the Modbus Server Slave
+   */
+  void modbus_set_slave_id(int id);
+
+  /**
+   * @brief Build up a Modbus/TCP Connection
+   * @return   If A Connection Is Successfully Built
+   */
+  bool modbus_connect();
+
+  /**
+   * @brief Close the Modbus/TCP Connection
+   */
+  void modbus_close() const;
+
+  /**
+   * @brief Read Coil
+   *        MODBUS FUNCTION 0x01
+   * @param address     Reference Address
+   * @param buffer      Buffer to Store Data Read from Coils
+   */
+  int  modbus_read_coil(int address, bool &buffer);
+
+  /**
+   * @brief Read Coils
+   *        MODBUS FUNCTION 0x01
+   * @param address     Reference Address
+   * @param amount      Amount of Coils to Read
+   * @param buffer      Buffer to Store Data Read from Coils
+   */
+  int  modbus_read_coils(int address, int amount, bool* buffer);
+
+  /**
+   * @brief Read Input Bits(Discrete Data)
+   *        MODBUS FUNCITON 0x02
+   * @param address   Reference Address
+   * @param buffer    Buffer to store Data Read from Input Bits
+   */
+  int  modbus_read_input_bit(int address, bool &buffer);
+
+  /**
+   * @brief Read Input Bits(Discrete Data)
+   *        MODBUS FUNCITON 0x02
+   * @param address   Reference Address
+   * @param amount    Amount of Bits to Read
+   * @param buffer    Buffer to store Data Read from Input Bits
+   */
+  int  modbus_read_input_bits(int address, int amount, bool* buffer);
+
+  /**
+   * @brief Read Holding Register 
+   *        MODBUS FUNCTION 0x03
+   * @param address    Reference Address
+   * @param buffer     Buffer to Store Data Read from Registers
+   */
+  int  modbus_read_holding_register(int address, uint16_t &buffer);
+
+  /**
+   * @brief Read Holding Registers
+   *        MODBUS FUNCTION 0x03
+   * @param address    Reference Address
+   * @param amount     Amount of Registers to Read
+   * @param buffer     Buffer to Store Data Read from Registers
+   */
+  int  modbus_read_holding_registers(int address, int amount, uint16_t *buffer);
+
+  /**
+   * @brief Read Input Registers
+   *        MODBUS FUNCTION 0x04
+   * @param address     Reference Address
+   * @param amount      Amount of Registers to Read
+   * @param buffer      Buffer to Store Data Read from Registers
+   */
+  int  modbus_read_input_registers(int address, int amount, uint16_t *buffer);
+
+
+  /**
+   * @brief  Write Single Coils
+   *         MODBUS FUNCTION 0x05
+   * @param address    Reference Address
+   * @param to_write   Value to be Written to Coil
+   */
+  int  modbus_write_coil(int address, const bool& to_write);
+
+  /**
+   * @brief Write Single Register
+   *        FUCTION 0x06
+   * @param address   Reference Address
+   * @param value     Value to Be Written to Register
+   */
+  int  modbus_write_register(int address, const uint16_t& value);
+
+  /**
+   * @brief Write Multiple Coils
+   *        MODBUS FUNCTION 0x0F
+   * @param address  Reference Address
+   * @param amount   Amount of Coils to Write
+   * @param value    Values to Be Written to Coils
+   */
+  int  modbus_write_coils(int address, int amount, const bool *value);
+
+  /**
+   * @brief Write Multiple Registers
+   *        MODBUS FUNCION 0x10
+   * @param address Reference Address
+   * @param amount  Amount of Value to Write
+   * @param value   Values to Be Written to the Registers
+   */
+  int  modbus_write_registers(int address, int amount, const uint16_t *value); 
+
+  bool _connected{};
+  bool err{};
+  int err_no{};
+  std::string error_msg;
+  struct timeval timeout{};
+
+private:
+
+  /**
+   * @brief Modbus Request Builder
+   * @param to_send   Message Buffer to Be Sent
+   * @param address   Reference Address
+   * @param func      Modbus Functional Code
+   */
+  void modbus_build_request(uint8_t *to_send, uint address, int func) const;
+
+  /**
+   * @brief Read Request Builder and Sender
+   * @param address   Reference Address
+   * @param amount    Amount of Data to Read
+   * @param func      Modbus Functional Code
+   */
+  int modbus_read(int address, uint amount, int func);
+
+  /**
+   * @brief Write Request Builder and Sender
+   * @param address   Reference Address
+   * @param amount    Amount of data to be Written
+   * @param func      Modbus Functional Code
+   * @param value     Data to Be Written
+   */
+  int modbus_write(int address, uint amount, int func, const uint16_t *value);
+
+  /**
+   * @brief Data Sender
+   * @param to_send Request to Be Sent to Server
+   * @param length  Length of the Request
+   * @return        Size of the request
+   */
+  ssize_t modbus_send(uint8_t *to_send, int length);
+
+  /**
+   * @brief Data Receiver
+   * @param buffer Buffer to Store the Data Retrieved
+   * @return       Size of Incoming Data
+   */
+  ssize_t modbus_receive(uint8_t *buffer) const;
+
+  /**
+   * @brief Error Code Handler
+   * @param msg   Message Received from the Server
+   * @param func  Modbus Functional Code
+   */
+  void modbuserror_handle(const uint8_t *msg, int func);
+
+  void set_bad_con();
+  void set_bad_input();
+
+// Properties
+  int _socket{};
+  uint _msg_id{};
+  int _slaveid{};
+  uint16_t PORT{};
+  std::string HOST;
+  struct sockaddr_in _server{};
+};
+
+#endif //MODBUS_H

Devices/Libraries/Ros/modbus/modbus_tcp/package.xml

@@ -0,0 +1,65 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>modbus_tcp</name>
+  <version>0.0.0</version>
+  <description>The modbus_tcp package</description>
+
+  <!-- One maintainer tag required, multiple allowed, one person per tag -->
+  <!-- Example:  -->
+  <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+  <maintainer email="robotics@todo.todo">robotics</maintainer>
+
+
+  <!-- One license tag required, multiple allowed, one license per tag -->
+  <!-- Commonly used license strings: -->
+  <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
+  <license>TODO</license>
+
+
+  <!-- Url tags are optional, but multiple are allowed, one per tag -->
+  <!-- Optional attribute type can be: website, bugtracker, or repository -->
+  <!-- Example: -->
+  <!-- <url type="website">http://wiki.ros.org/modbus_tcp</url> -->
+
+
+  <!-- Author tags are optional, multiple are allowed, one per tag -->
+  <!-- Authors do not have to be maintainers, but could be -->
+  <!-- Example: -->
+  <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
+
+
+  <!-- The *depend tags are used to specify dependencies -->
+  <!-- Dependencies can be catkin packages or system dependencies -->
+  <!-- Examples: -->
+  <!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
+  <!--   <depend>roscpp</depend> -->
+  <!--   Note that this is equivalent to the following: -->
+  <!--   <build_depend>roscpp</build_depend> -->
+  <!--   <exec_depend>roscpp</exec_depend> -->
+  <!-- Use build_depend for packages you need at compile time: -->
+  <!--   <build_depend>message_generation</build_depend> -->
+  <!-- Use build_export_depend for packages you need in order to build against this package: -->
+  <!--   <build_export_depend>message_generation</build_export_depend> -->
+  <!-- Use buildtool_depend for build tool packages: -->
+  <!--   <buildtool_depend>catkin</buildtool_depend> -->
+  <!-- Use exec_depend for packages you need at runtime: -->
+  <!--   <exec_depend>message_runtime</exec_depend> -->
+  <!-- Use test_depend for packages you need only for testing: -->
+  <!--   <test_depend>gtest</test_depend> -->
+  <!-- Use doc_depend for packages you need only for building documentation: -->
+  <!--   <doc_depend>doxygen</doc_depend> -->
+  <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>roscpp</build_depend>
+  <build_depend>std_msgs</build_depend>
+  <build_export_depend>roscpp</build_export_depend>
+  <build_export_depend>std_msgs</build_export_depend>
+  <exec_depend>roscpp</exec_depend>
+  <exec_depend>std_msgs</exec_depend>
+
+
+  <!-- The export tag contains other, unspecified, tags -->
+  <export>
+    <!-- Other tools can request additional information be placed here -->
+
+  </export>
+</package>

Devices/Libraries/Ros/modbus/modbus_tcp/src/modbus.cpp

@@ -0,0 +1,501 @@
+#include "modbus_tcp/modbus.h"
+
+
+
+modbus::modbus(std::string ip_address, int port) 
+{
+  this->HOST = ip_address;
+  ROS_INFO("modbus - HOST = %s", this->HOST.c_str());
+  this->PORT = (uint16_t)port;
+  ROS_INFO("modbus - PORT = %d", this->PORT);
+  this->_slaveid = 1;
+  this->_msg_id = 1;
+  this->_connected = false;
+  this->err = false;
+  this->err_no = 0;
+  this->error_msg = "";
+  this->timeout.tv_sec  = 10;  // after 10 seconds connect() will timeout
+  this->timeout.tv_usec = 0;
+}
+
+modbus::~modbus(void)  
+{
+  modbus_close();
+}
+
+void modbus::modbus_set_slave_id(int id)
+{
+  _slaveid = id;
+}
+
+bool modbus::modbus_connect() 
+{
+  if(this->HOST.empty() || this->PORT == 0) {
+      ROS_ERROR_STREAM("Modbus TCP: Missing Host and Port");
+      return false;
+  } else {
+      ROS_INFO_STREAM("Modbus TCP: Found Proper Host "<< this->HOST << " and Port " <<this->PORT);
+  }
+
+  this->_socket = socket(AF_INET, SOCK_STREAM, 0);
+  if(this->_socket == -1) {
+      ROS_ERROR_STREAM("Modbus TCP: Error Opening Socket");
+      return false;
+  } else {
+      ROS_INFO_STREAM("Modbus TCP: Socket Opened Successfully");
+      //ROS_INFO_STREAM("Device is connecting......");
+  }
+
+  setsockopt(_socket, SOL_SOCKET, SO_SNDTIMEO, &timeout, sizeof(timeout));
+  setsockopt(_socket, SOL_SOCKET, SO_RCVTIMEO, &timeout, sizeof(timeout));
+
+  _server.sin_family = AF_INET;
+  _server.sin_addr.s_addr = inet_addr(HOST.c_str());
+  _server.sin_port = htons(PORT);
+
+  if (connect(_socket, (struct sockaddr*)&_server, sizeof(_server)) < 0) {
+      ROS_ERROR_STREAM("Modbus TCP: Host "<< this->HOST << " and Port " <<this->PORT << " Connection Error");
+      return false;
+  }
+
+//   ROS_INFO_STREAM("Modbus TCP: Host "<< this->HOST << " and Port " <<this->PORT << "is Connected");
+  _connected = true;
+  err = false;
+  return true;
+}
+
+void modbus::modbus_close() const {
+    close(_socket);
+    ROS_INFO_STREAM("Modbus TCP: Socket Closed");
+}
+
+void modbus::modbus_build_request(uint8_t *to_send, uint address, int func) const {
+    to_send[0] = (uint8_t) _msg_id >> 8u;
+    to_send[1] = (uint8_t) (_msg_id & 0x00FFu);
+    to_send[2] = 0;
+    to_send[3] = 0;
+    to_send[4] = 0;
+    to_send[6] = (uint8_t) _slaveid;
+    to_send[7] = (uint8_t) func;
+    to_send[8] = (uint8_t) (address >> 8u);
+    to_send[9] = (uint8_t) (address & 0x00FFu);
+}
+
+int modbus::modbus_write(int address, uint amount, int func, const uint16_t *value) {
+    int status = 0;
+    if(func == WRITE_COIL || func == WRITE_REG) {
+        uint8_t to_send[12];
+        modbus_build_request(to_send, address, func);
+        to_send[5] = 6;
+        to_send[10] = (uint8_t) (value[0] >> 8u);
+        to_send[11] = (uint8_t) (value[0] & 0x00FFu);
+        status = modbus_send(to_send, 12);
+    } else if(func == WRITE_REGS){
+        uint8_t to_send[13 + 2 * amount];
+        modbus_build_request(to_send, address, func);
+        to_send[5] = (uint8_t) (7 + 2 * amount);
+        to_send[10] = (uint8_t) (amount >> 8u);
+        to_send[11] = (uint8_t) (amount & 0x00FFu);
+        to_send[12] = (uint8_t) (2 * amount);
+        for(int i = 0; i < amount; i++) {
+            to_send[13 + 2 * i] = (uint8_t) (value[i] >> 8u);
+            to_send[14 + 2 * i] = (uint8_t) (value[i] & 0x00FFu);
+        }
+        status = modbus_send(to_send, 13 + 2 * amount);
+    } else if(func == WRITE_COILS) {
+        uint8_t to_send[14 + (amount -1) / 8 ];
+        modbus_build_request(to_send, address, func);
+        to_send[5] = (uint8_t) (7 + (amount + 7) / 8);
+        to_send[10] = (uint8_t) (amount >> 8u);
+        to_send[11] = (uint8_t) (amount & 0x00FFu);
+        to_send[12] = (uint8_t) ((amount + 7) / 8);
+        for(int i = 0; i < (amount+7)/8; i++)
+            to_send[13 + i] = 0; // init needed before summing!
+        for(int i = 0; i < amount; i++) {
+            to_send[13 + i/8] += (uint8_t) (value[i] << (i % 8u));
+        }
+        status = modbus_send(to_send, 14 + (amount - 1) / 8);
+    }
+    return status;
+
+}
+
+int modbus::modbus_read(int address, uint amount, int func){
+    uint8_t to_send[12];
+    modbus_build_request(to_send, address, func);
+    to_send[5] = 6;
+    to_send[10] = (uint8_t) (amount >> 8u);
+    to_send[11] = (uint8_t) (amount & 0x00FFu);
+    return modbus_send(to_send, 12);
+}
+
+int modbus::modbus_read_holding_register(int address, uint16_t &buffer) {
+    if(_connected) {
+        if(address > 65535) {
+            set_bad_input();
+            return EX_BAD_DATA;
+        }
+        modbus_read(address, 1, READ_REGS);
+        uint8_t to_rec[MAX_MSG_LENGTH];
+        ssize_t k = modbus_receive(to_rec);
+        if (k == -1) {
+            set_bad_con();
+            return BAD_CON;
+        }
+        if(k < 11) return EX_BAD_DATA;
+        modbuserror_handle(to_rec, READ_REGS);
+        if(err) return err_no;
+        buffer = ((uint16_t)to_rec[9u]) << 8u;
+        buffer += (uint16_t) to_rec[10u];
+        return 0;
+    } else {
+        set_bad_con();
+        return BAD_CON;
+    }
+}
+
+int modbus::modbus_read_holding_registers(int address, int amount, uint16_t *buffer) {
+    if(_connected) {
+        if(amount > 65535 || address > 65535) {
+            set_bad_input();
+            return EX_BAD_DATA;
+        }
+        modbus_read(address, amount, READ_REGS);
+        uint8_t to_rec[MAX_MSG_LENGTH];
+        ssize_t k = modbus_receive(to_rec);
+        if (k == -1) {
+            set_bad_con();
+            return BAD_CON;
+        }
+        if(k < 2 * amount + 9) return EX_BAD_DATA;
+        modbuserror_handle(to_rec, READ_REGS);
+        if(err) return err_no;
+        for(uint i = 0; i < amount; i++) {
+            buffer[i] = ((uint16_t)to_rec[9u + 2u * i]) << 8u;
+            buffer[i] += (uint16_t) to_rec[10u + 2u * i];
+        }
+        return 0;
+    } else {
+        set_bad_con();
+        return BAD_CON;
+    }
+}
+
+int modbus::modbus_read_input_registers(int address, int amount, uint16_t *buffer) {
+    if(_connected){
+        if(amount > 65535 || address > 65535) {
+            set_bad_input();
+            return EX_BAD_DATA;
+        }
+        modbus_read(address, amount, READ_INPUT_REGS);
+        uint8_t to_rec[MAX_MSG_LENGTH];
+        ssize_t k = modbus_receive(to_rec);
+        // ROS_INFO("modbus_read_input_registers: k = %d, amount = %d", (int)k , amount);
+        if (k == -1) {
+            set_bad_con();
+            return BAD_CON;
+        }
+        modbuserror_handle(to_rec, READ_INPUT_REGS);
+        if(err) return err_no;
+        for(uint i = 0; i < amount; i++) {
+            buffer[i] = ((uint16_t)to_rec[9u + 2u * i]) << 8u;
+            buffer[i] += (uint16_t) to_rec[10u + 2u * i];
+        }
+        return 0;
+    } else {
+        set_bad_con();
+        return BAD_CON;
+    }
+}
+
+int modbus::modbus_read_coil(int address, bool &buffer) {
+    if(_connected) {
+        if(address > 65535) {
+            set_bad_input();
+            return EX_BAD_DATA;
+        }
+        modbus_read(address, 1, READ_COILS);
+        uint8_t to_rec[MAX_MSG_LENGTH];
+        ssize_t k = modbus_receive(to_rec);
+        // ROS_INFO("modbus_read_coil: k = %d, amount = %d", (int)k , 1);
+        if (k == -1) {
+            set_bad_con();
+            return BAD_CON;
+        }
+        modbuserror_handle(to_rec, READ_COILS);
+        if(err) return err_no;
+        buffer = (bool) ((to_rec[9u]) & 1u);
+        return 0;
+    } else {
+        set_bad_con();
+        return BAD_CON;
+    }
+}
+
+int modbus::modbus_read_coils(int address, int amount, bool *buffer) {
+    if(_connected) {
+        if(amount > 2040 || address > 65535) {
+            set_bad_input();
+            return EX_BAD_DATA;
+        }
+        modbus_read(address, amount, READ_COILS);
+        uint8_t to_rec[MAX_MSG_LENGTH];
+        ssize_t k = modbus_receive(to_rec);
+        if (k == -1) {
+            set_bad_con();
+            return BAD_CON;
+        }
+        modbuserror_handle(to_rec, READ_COILS);
+        // ROS_INFO("modbus_read_coils: k = %d, amount = %d", (int)k , amount);
+        if(err) return err_no;
+        for(uint i = 0; i < amount; i++) {
+            buffer[i] = (bool) ((to_rec[9u + i / 8u] >> (i % 8u)) & 1u);
+        }
+        return 0;
+    } else {
+        set_bad_con();
+        return BAD_CON;
+    }
+}
+
+int modbus::modbus_read_input_bit(int address, bool &buffer) {
+    if(_connected) {
+        if(address > 65535) {
+            set_bad_input();
+            return EX_BAD_DATA;
+        }
+        modbus_read(address, 1, READ_INPUT_BITS);
+        uint8_t to_rec[MAX_MSG_LENGTH];
+        ssize_t k = modbus_receive(to_rec);
+        // ROS_INFO("modbus_read_input_bit: k = %d, amount = %d", (int)k , 1);
+        if (k == -1) {
+            set_bad_con();
+            return BAD_CON;
+        }
+        if(err) return err_no;
+        buffer = (bool)(to_rec[9u] & 1u);
+        modbuserror_handle(to_rec, READ_INPUT_BITS);
+        return 0;
+    } else {
+        return BAD_CON;
+    }
+}
+
+int modbus::modbus_read_input_bits(int address, int amount, bool* buffer) {
+    if(_connected) {
+        if(amount > 2040 || address > 65535) {
+            set_bad_input();
+            return EX_BAD_DATA;
+        }
+        modbus_read(address, amount, READ_INPUT_BITS);
+        uint8_t to_rec[MAX_MSG_LENGTH];
+        ssize_t k = modbus_receive(to_rec);
+        // ROS_INFO("modbus_read_input_bits: k = %d, amount = %d", (int)k , amount);
+        if (k == -1) {
+            set_bad_con();
+            return BAD_CON;
+        }
+        if(err) return err_no;
+        for(uint i = 0; i < amount; i++) {
+            buffer[i] = (bool) ((to_rec[9u + i / 8u] >> (i % 8u)) & 1u);
+        }
+        modbuserror_handle(to_rec, READ_INPUT_BITS);
+        return 0;
+    } else {
+        return BAD_CON;
+    }
+}
+
+int modbus::modbus_write_coil(int address, const bool& to_write) {
+    if(_connected) {
+        if(address > 65535) {
+            set_bad_input();
+            return EX_BAD_DATA;
+        }
+        int value = to_write * 0xFF00;
+        modbus_write(address, 1, WRITE_COIL, (uint16_t *)&value);
+        uint8_t to_rec[MAX_MSG_LENGTH];
+        ssize_t k = modbus_receive(to_rec);
+        if (k == -1) {
+            set_bad_con();
+            return BAD_CON;
+        }
+        modbuserror_handle(to_rec, WRITE_COIL);
+        if(err) return err_no;
+        return 0;
+    } else {
+        set_bad_con();
+        return BAD_CON;
+    }
+}
+
+int modbus::modbus_write_register(int address, const uint16_t& value) {
+    if(_connected) {
+        if(address > 65535) {
+            set_bad_input();
+            return EX_BAD_DATA;
+        }
+        modbus_write(address, 1, WRITE_REG, &value);
+        uint8_t to_rec[MAX_MSG_LENGTH];
+        ssize_t k = modbus_receive(to_rec);
+        if (k == -1) {
+            set_bad_con();
+            return BAD_CON;
+        }
+        modbuserror_handle(to_rec, WRITE_REG);
+        if(err) return err_no;
+        return 0;
+    } else {
+        set_bad_con();
+        return BAD_CON;
+    }
+}
+
+int modbus::modbus_write_coils(int address, int amount, const bool *value) {
+    if(_connected) {
+        if(address > 65535 || amount > 65535) {
+            set_bad_input();
+            return EX_BAD_DATA;
+        }
+        uint16_t temp[amount];
+        for(int i = 0; i < amount; i++) {
+            temp[i] = (uint16_t)value[i];
+        }
+        modbus_write(address, amount, WRITE_COILS, temp);
+        uint8_t to_rec[MAX_MSG_LENGTH];
+        ssize_t k = modbus_receive(to_rec);
+        // ROS_INFO("modbus_write_coils: k = %d, amount = %d", (int)k , amount);
+        if (k == -1) {
+            set_bad_con();
+            return BAD_CON;
+        }
+        modbuserror_handle(to_rec, WRITE_COILS);
+        if(err) return err_no;
+        return 0;
+    } else {
+        set_bad_con();
+        return BAD_CON;
+    }
+}
+
+int modbus::modbus_write_registers(int address, int amount, const uint16_t *value) {
+    if(_connected) {
+        if(address > 65535 || amount > 65535) {
+            set_bad_input();
+            return EX_BAD_DATA;
+        }
+        modbus_write(address, amount, WRITE_REGS, value);
+        uint8_t to_rec[MAX_MSG_LENGTH];
+        ssize_t k = modbus_receive(to_rec);
+        if (k == -1) {
+            set_bad_con();
+            return BAD_CON;
+        }
+        modbuserror_handle(to_rec, WRITE_REGS);
+        if(err) return err_no;
+        return 0;
+    } else {
+        set_bad_con();
+        return BAD_CON;
+    }
+}
+
+ssize_t modbus::modbus_send(uint8_t *to_send, int length) {
+    _msg_id++;
+    return send(_socket, to_send, (size_t)length, 0);
+}
+
+
+ssize_t modbus::modbus_receive(uint8_t *buffer) const {
+    
+    timeval t_out;
+    fd_set set;
+    t_out.tv_sec = 1;
+	t_out.tv_usec = 0;
+    FD_ZERO(&set); /* clear the set */
+	FD_SET(_socket, &set); /* add our file descriptor to the set */
+    int rv = select(_socket +1, &set, NULL, NULL, &t_out);
+    if(rv == -1)
+    {
+        ROS_ERROR_STREAM("socket error accured");
+        return -1;
+    }
+    else if(rv == 0)
+    {
+        ROS_ERROR_STREAM("socket timeout occured");
+        return -1;
+    }
+    else
+        return recv(_socket, (char *) buffer, 1024, 0);
+}
+
+void modbus::set_bad_con() {
+    err = true;
+    _connected = false;
+    error_msg = "BAD CONNECTION";
+}
+
+
+void modbus::set_bad_input() {
+    err = true;
+    error_msg = "BAD FUNCTION INPUT";
+}
+
+/**
+ * Error Code Handler
+ * @param msg   Message Received from the Server
+ * @param func  Modbus Functional Code
+ */
+void modbus::modbuserror_handle(const uint8_t *msg, int func) {
+    if(msg[7] == func + 0x80) {
+        err = true;
+        switch(msg[8]){
+            case EX_ILLEGAL_FUNCTION:
+                error_msg = "1 Illegal Function";
+                err_no = EX_ILLEGAL_FUNCTION;
+                break;
+            case EX_ILLEGAL_ADDRESS:
+                error_msg = "2 Illegal Address";
+                err_no = EX_ILLEGAL_ADDRESS;
+                break;
+            case EX_ILLEGAL_VALUE:
+                error_msg = "3 Illegal Value";
+                err_no = EX_ILLEGAL_VALUE;
+                break;
+            case EX_SERVER_FAILURE:
+                error_msg = "4 Server Failure";
+                err_no = EX_SERVER_FAILURE;
+                break;
+            case EX_ACKNOWLEDGE:
+                error_msg = "5 Acknowledge";
+                err_no = EX_ACKNOWLEDGE;
+                break;
+            case EX_SERVER_BUSY:
+                error_msg = "6 Server Busy";
+                err_no = EX_SERVER_BUSY;
+                break;
+            case EX_NEGATIVE_ACK:
+                error_msg = "7 Negative Acknowledge";
+                err_no = EX_NEGATIVE_ACK;
+                break;
+            case EX_MEM_PARITY_PROB:
+                error_msg = "8 Memory Parity Problem";
+                err_no = EX_MEM_PARITY_PROB;
+                break;
+            case EX_GATEWAY_PROBLEMP:
+                error_msg = "10 Gateway Path Unavailable";
+                err_no = EX_GATEWAY_PROBLEMP;
+                break;
+            case EX_GATEWYA_PROBLEMF:
+                error_msg = "11 Gateway Target Device Failed to Respond";
+                err_no = EX_GATEWYA_PROBLEMF;
+                break;
+            default:
+                error_msg = "UNK";
+                err_no = 0;
+                break;
+        }
+    }
+    err = false;
+    error_msg = "NO ERR";
+}

Devices/Libraries/Ros/random_numbers/.clang-format

@@ -0,0 +1,64 @@
+---
+BasedOnStyle:  Google
+AccessModifierOffset: -2
+ConstructorInitializerIndentWidth: 2
+AlignEscapedNewlinesLeft: false
+AlignTrailingComments: true
+AllowAllParametersOfDeclarationOnNextLine: false
+AllowShortIfStatementsOnASingleLine: false
+AllowShortLoopsOnASingleLine: false
+AllowShortFunctionsOnASingleLine: None
+AlwaysBreakTemplateDeclarations: true
+AlwaysBreakBeforeMultilineStrings: false
+BreakBeforeBinaryOperators: false
+BreakBeforeTernaryOperators: false
+BreakConstructorInitializersBeforeComma: true
+BinPackParameters: true
+ColumnLimit:    120
+ConstructorInitializerAllOnOneLineOrOnePerLine: true
+DerivePointerBinding: false
+PointerBindsToType: true
+ExperimentalAutoDetectBinPacking: false
+IndentCaseLabels: true
+MaxEmptyLinesToKeep: 1
+NamespaceIndentation: None
+ObjCSpaceBeforeProtocolList: true
+PenaltyBreakBeforeFirstCallParameter: 19
+PenaltyBreakComment: 60
+PenaltyBreakString: 100
+PenaltyBreakFirstLessLess: 1000
+PenaltyExcessCharacter: 1000
+PenaltyReturnTypeOnItsOwnLine: 70
+SpacesBeforeTrailingComments: 2
+Cpp11BracedListStyle: false
+Standard:        Auto
+IndentWidth:     2
+TabWidth:        2
+UseTab:          Never
+IndentFunctionDeclarationAfterType: false
+SpacesInParentheses: false
+SpacesInAngles:  false
+SpaceInEmptyParentheses: false
+SpacesInCStyleCastParentheses: false
+SpaceAfterControlStatementKeyword: true
+SpaceBeforeAssignmentOperators: true
+ContinuationIndentWidth: 4
+SortIncludes: false
+SpaceAfterCStyleCast: false
+
+# Configure each individual brace in BraceWrapping
+BreakBeforeBraces: Custom
+
+# Control of individual brace wrapping cases
+BraceWrapping:
+    AfterClass: 'true'
+    AfterControlStatement: 'true'
+    AfterEnum : 'true'
+    AfterFunction : 'true'
+    AfterNamespace : 'true'
+    AfterStruct : 'true'
+    AfterUnion : 'true'
+    BeforeCatch : 'true'
+    BeforeElse : 'true'
+    IndentBraces : 'false'
+...

Devices/Libraries/Ros/random_numbers/.github/workflows/format.yaml

@@ -0,0 +1,17 @@
+# This is a format job. Pre-commit has a first-party GitHub action, so we use
+# that: https://github.com/pre-commit/action
+
+name: Format
+
+on: [push, pull_request]
+
+jobs:
+  pre-commit:
+    name: Format
+    runs-on: ubuntu-latest
+    steps:
+    - uses: actions/checkout@v2
+    - uses: actions/setup-python@v2
+    - name: Install clang-format-10
+      run: sudo apt-get install clang-format-10
+    - uses: pre-commit/action@v2.0.0

Devices/Libraries/Ros/random_numbers/.github/workflows/industrial_ci_action.yaml

@@ -0,0 +1,68 @@
+# This config uses industrial_ci (https://github.com/ros-industrial/industrial_ci.git).
+# For troubleshooting, see readme (https://github.com/ros-industrial/industrial_ci/blob/master/README.rst)
+
+name: BuildAndTest
+
+on:
+  workflow_dispatch:
+  pull_request:
+  push:
+    branches:
+      - master
+
+jobs:
+  industrial_ci:
+    strategy:
+      matrix:
+        env:
+          - ROS_DISTRO: noetic
+            ROS_REPO: main
+            CATKIN_LINT: true
+          - ROS_DISTRO: melodic
+            ROS_REPO: main
+            CATKIN_LINT: true
+          - ROS_DISTRO: kinetic
+            ROS_REPO: main
+    env:
+      CCACHE_DIR: "${{ github.workspace }}/.ccache"
+      BASEDIR: ${{ github.workspace }}/.work
+      CACHE_PREFIX: "${{ matrix.env.ROS_DISTRO }}-${{ matrix.env.ROS_REPO }}"
+
+    name: "${{ matrix.env.ROS_DISTRO }}-${{ matrix.env.ROS_REPO }}${{ matrix.env.CATKIN_LINT && ' + catkin_lint' || ''}}"
+    runs-on: ubuntu-latest
+    steps:
+      - uses: actions/checkout@v2
+      # The target directory cache doesn't include the source directory because
+      # that comes from the checkout.  See "prepare target_ws for cache" task below
+      - name: cache target_ws
+        if: ${{ ! matrix.env.CCOV }}
+        uses: pat-s/always-upload-cache@v2.1.5
+        with:
+          path: ${{ env.BASEDIR }}/target_ws
+          key: target_ws-${{ env.CACHE_PREFIX }}-${{ hashFiles('**/CMakeLists.txt', '**/package.xml') }}-${{ github.run_id }}
+          restore-keys: |
+            target_ws-${{ env.CACHE_PREFIX }}-${{ hashFiles('**/CMakeLists.txt', '**/package.xml') }}
+      - name: cache ccache
+        uses: pat-s/always-upload-cache@v2.1.5
+        with:
+          path: ${{ env.CCACHE_DIR }}
+          key: ccache-${{ env.CACHE_PREFIX }}-${{ github.sha }}-${{ github.run_id }}
+          restore-keys: |
+            ccache-${{ env.CACHE_PREFIX }}-${{ github.sha }}
+            ccache-${{ env.CACHE_PREFIX }}
+      - name: industrial_ci
+        uses: 'ros-industrial/industrial_ci@master'
+        env: ${{ matrix.env }}
+      - name: upload test artifacts (on failure)
+        uses: actions/upload-artifact@v2
+        if: failure()
+        with:
+          name: test-results
+          path: ${{ env.BASEDIR }}/target_ws/**/test_results/**/*.xml
+      - name: prepare target_ws for cache
+        if: ${{ always() && ! matrix.env.CCOV }}
+        run: |
+          du -sh ${{ env.BASEDIR }}/target_ws
+          sudo find ${{ env.BASEDIR }}/target_ws -wholename '*/test_results/*' -delete
+          sudo rm -rf ${{ env.BASEDIR }}/target_ws/src
+          du -sh ${{ env.BASEDIR }}/target_ws

Devices/Libraries/Ros/random_numbers/.github/workflows/prerelease.yaml

@@ -0,0 +1,26 @@
+# This config uses industrial_ci (https://github.com/ros-industrial/industrial_ci.git).
+# For troubleshooting, see readme (https://github.com/ros-industrial/industrial_ci/blob/master/README.rst)
+
+name: pre-release
+
+on:
+  workflow_dispatch:
+
+jobs:
+  default:
+    strategy:
+      fail-fast: false
+      matrix:
+        distro: [melodic, noetic]
+
+    env:
+      ROS_DISTRO: ${{ matrix.distro }}
+      PRERELEASE: true
+      BASEDIR: ${{ github.workspace }}/.work
+
+    name: "${{ matrix.distro }}"
+    runs-on: ubuntu-latest
+    steps:
+      - uses: actions/checkout@v2
+      - name: industrial_ci
+        uses: ros-industrial/industrial_ci@master

Devices/Libraries/Ros/random_numbers/.gitignore

@@ -0,0 +1 @@
+*~

Devices/Libraries/Ros/random_numbers/.pre-commit-config.yaml

@@ -0,0 +1,43 @@
+# To use:
+#
+#     pre-commit run -a
+#
+# Or:
+#
+#     pre-commit install  # (runs every time you commit in git)
+#
+# To update this file:
+#
+#     pre-commit autoupdate
+#
+# See https://github.com/pre-commit/pre-commit
+
+repos:
+  # Standard hooks
+  - repo: https://github.com/pre-commit/pre-commit-hooks
+    rev: v3.4.0
+    hooks:
+      - id: check-added-large-files
+      - id: check-case-conflict
+      - id: check-merge-conflict
+      - id: check-symlinks
+      - id: check-yaml
+      - id: debug-statements
+      - id: end-of-file-fixer
+      - id: mixed-line-ending
+      - id: trailing-whitespace
+
+  - repo: https://github.com/psf/black
+    rev: 22.3.0
+    hooks:
+      - id: black
+
+  - repo: local
+    hooks:
+      - id: clang-format
+        name: clang-format
+        description: Format files with ClangFormat.
+        entry: clang-format-10
+        language: system
+        files: \.(c|cc|cxx|cpp|frag|glsl|h|hpp|hxx|ih|ispc|ipp|java|js|m|proto|vert)$
+        args: ['-fallback-style=none', '-i']

Devices/Libraries/Ros/random_numbers/CHANGELOG.rst

@@ -0,0 +1,130 @@
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for package random_numbers
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+0.3.2 (2018-02-27)
+------------------
+* Update maintainership. (`#11 <https://github.com/ros-planning/random_numbers/issues/11>`_)
+* Contributors: Steven! Ragnarök
+
+0.3.1 (2016-04-04)
+------------------
+* Merge pull request `#10 <https://github.com/ros-planning/random_numbers/issues/10>`_ from jspricke/cmake_lib
+  Use catkin variables for install dirs
+* Contributors: Dave Coleman, Jochen Sprickerhof
+
+0.3.0 (2014-09-05)
+------------------
+* Update README.md with Documentation
+* Allow the randomly generated seed to be saved so that experiments / benc...
+* Initialize static int to 0
+* Save the first_seed even when passed in manually.
+* Allow the randomly generated seed to be saved so that experiments / benchmarks can be recreated in the future
+* Added ability to specify random number generator seed for stochastic behavior
+* Added travis build status indicator in README.md
+* Contributors: Dave Coleman, Dave Hershberger, Ioan A Sucan
+
+0.2.0 (2013-07-16)
+------------------
+* Merge pull request `#2 <https://github.com/ros-planning/random_numbers/issues/2>`_ from wjwwood/patch-1
+  Fix linkedit error on OS X with newer versions of Boost
+* Fix linkedit error on OS X with newer versions of Boost
+  When building `random_numbers` on OS X with Boost 1.53.0 I get:
+  ```
+  ==> Processing catkin package: 'random_numbers'
+  ==> Creating build directory: 'build_isolated/random_numbers'
+  ==> Building with env: '/Users/william/moveit_ws/install_isolated/env.sh'
+  ==> cmake /Users/william/moveit_ws/src/random_numbers -DCATKIN_DEVEL_PREFIX=/Users/william/moveit_ws/devel_isolated/random_numbers -DCMAKE_INSTALL_PREFIX=/Users/william/moveit_ws/install_isolated
+  -- The C compiler identification is Clang 4.2.0
+  -- The CXX compiler identification is Clang 4.2.0
+  -- Check for working C compiler: /usr/bin/cc
+  -- Check for working C compiler: /usr/bin/cc -- works
+  -- Detecting C compiler ABI info
+  -- Detecting C compiler ABI info - done
+  -- Check for working CXX compiler: /usr/bin/c++
+  -- Check for working CXX compiler: /usr/bin/c++ -- works
+  -- Detecting CXX compiler ABI info
+  -- Detecting CXX compiler ABI info - done
+  -- Using CATKIN_DEVEL_PREFIX: /Users/william/moveit_ws/devel_isolated/random_numbers
+  -- Using CMAKE_PREFIX_PATH: /Users/william/moveit_ws/install_isolated
+  -- This workspace overlays: /Users/william/moveit_ws/install_isolated
+  -- Found PythonInterp: /usr/bin/python (found version "2.7.2")
+  -- Found PY_em: /Library/Python/2.7/site-packages/em.pyc
+  -- Found gtest: gtests will be built
+  -- Using CATKIN_TEST_RESULTS_DIR: /Users/william/moveit_ws/build_isolated/random_numbers/test_results
+  -- catkin 0.5.65
+  WARNING: 'catkin' should be listed as a buildtool dependency in the package.xml (instead of build dependency)
+  -- Boost version: 1.53.0
+  -- Found the following Boost libraries:
+  --   date_time
+  --   thread
+  -- Configuring done
+  -- Generating done
+  -- Build files have been written to: /Users/william/moveit_ws/build_isolated/random_numbers
+  ==> make -j4 -l4 in '/Users/william/moveit_ws/build_isolated/random_numbers'
+  Scanning dependencies of target random_numbers
+  [100%] Building CXX object CMakeFiles/random_numbers.dir/src/random_numbers.cpp.o
+  Linking CXX shared library /Users/william/moveit_ws/devel_isolated/random_numbers/lib/librandom_numbers.dylib
+  Undefined symbols for architecture x86_64:
+  "boost::system::system_category()", referenced from:
+  ___cxx_global_var_init3 in random_numbers.cpp.o
+  boost::thread_exception::thread_exception(int, char const*) in random_numbers.cpp.o
+  "boost::system::generic_category()", referenced from:
+  ___cxx_global_var_init1 in random_numbers.cpp.o
+  ___cxx_global_var_init2 in random_numbers.cpp.o
+  ld: symbol(s) not found for architecture x86_64
+  clang: error: linker command failed with exit code 1 (use -v to see invocation)
+  make[2]: *** [/Users/william/moveit_ws/devel_isolated/random_numbers/lib/librandom_numbers.dylib] Error 1
+  make[1]: *** [CMakeFiles/random_numbers.dir/all] Error 2
+  make: *** [all] Error 2
+  Traceback (most recent call last):
+  File "./src/catkin/bin/../python/catkin/builder.py", line 658, in build_workspace_isolated
+  number=index + 1, of=len(ordered_packages)
+  File "./src/catkin/bin/../python/catkin/builder.py", line 443, in build_package
+  install, jobs, force_cmake, quiet, last_env, cmake_args, make_args
+  File "./src/catkin/bin/../python/catkin/builder.py", line 297, in build_catkin_package
+  run_command(make_cmd, build_dir, quiet)
+  File "./src/catkin/bin/../python/catkin/builder.py", line 186, in run_command
+  raise subprocess.CalledProcessError(proc.returncode, ' '.join(cmd))
+  CalledProcessError: Command '/Users/william/moveit_ws/install_isolated/env.sh make -j4 -l4' returned non-zero exit status 2
+  <== Failed to process package 'random_numbers':
+  Command '/Users/william/moveit_ws/install_isolated/env.sh make -j4 -l4' returned non-zero exit status 2
+  Reproduce this error by running:
+  ==> /Users/william/moveit_ws/install_isolated/env.sh make -j4 -l4
+  Command failed, exiting.
+  ```
+  Adding the `system` element to the `Boost` components being found fixes this.
+* fix typo
+* Create README.md
+* update description
+* Merge pull request `#1 <https://github.com/ros-planning/random_numbers/issues/1>`_ from ablasdel/patch-1
+  Update package.xml to buildtool_depend
+* Update package.xml to buildtool_depend
+* Added tag 0.1.3 for changeset 78f37b23c724
+* Contributors: Aaron Blasdel, Tully Foote, William Woodall, isucan
+
+0.1.3 (2012-10-12 20:13)
+------------------------
+* removing outdated install rule
+* fixing install rule
+* Added tag 0.1.2 for changeset 42db44939f5e
+* Contributors: Tully Foote
+
+0.1.2 (2012-10-12 19:50)
+------------------------
+* forgot rename
+* Added tag 0.1.2 for changeset 79869d337273
+* updating catkinization and 0.1.2
+* Added tag 0.1.1 for changeset 2e564507c3d1
+* Contributors: Ioan Sucan, Tully Foote
+
+0.1.1 (2012-06-18 13:21)
+------------------------
+* fix manifest
+* Added tag 0.1.0 for changeset a1286e23910e
+* Contributors: Ioan Sucan
+
+0.1.0 (2012-06-18 13:17)
+------------------------
+* add initial version
+* Contributors: Ioan Sucan

Devices/Libraries/Ros/random_numbers/CMakeLists.txt

@@ -0,0 +1,29 @@
+cmake_minimum_required(VERSION 3.0.2)
+project(random_numbers)
+
+find_package(catkin REQUIRED)
+
+catkin_package(
+  LIBRARIES ${PROJECT_NAME}
+  INCLUDE_DIRS include
+  )
+
+find_package(Boost REQUIRED date_time system thread)
+include_directories(${Boost_INCLUDE_DIR})
+
+include_directories(include)
+
+add_library(${PROJECT_NAME}
+  src/random_numbers.cpp)
+
+target_link_libraries(${PROJECT_NAME} ${Boost_LIBRARIES})
+
+set_target_properties(${PROJECT_NAME} PROPERTIES WINDOWS_EXPORT_ALL_SYMBOLS TRUE)
+
+install(TARGETS ${PROJECT_NAME}
+        ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+        LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+        RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION})
+
+install(DIRECTORY include/${PROJECT_NAME}/
+        DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION})

Devices/Libraries/Ros/random_numbers/README.md

@@ -0,0 +1,12 @@
+# random_numbers
+
+This library contains wrappers for generating floating point values, integers, quaternions using boost libraries.
+
+## Build Status
+
+[![BuildAndTest](https://github.com/ros-planning/random_numbers/actions/workflows/industrial_ci_action.yaml/badge.svg?branch=master)](https://github.com/ros-planning/random_numbers/actions/workflows/industrial_ci_action.yaml?branch=master)
+[![Format](https://github.com/ros-planning/random_numbers/actions/workflows/format.yaml/badge.svg?branch=master)](https://github.com/ros-planning/random_numbers/actions/workflows/format.yaml?branch=master)
+
+## Features
+
+New: you can pass in a custom random number generator seed to allow optional deterministic behavior during debugging, testing, etc. using the secondary constructor.

Devices/Libraries/Ros/random_numbers/include/random_numbers/random_numbers.h

@@ -0,0 +1,118 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2008, Willow Garage, Inc.
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of the Willow Garage nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *********************************************************************/
+
+/* Author: Ioan Sucan */
+
+#ifndef RANDOM_NUMBERS_RANDOM_NUMBERS_
+#define RANDOM_NUMBERS_RANDOM_NUMBERS_
+
+#include <boost/random/mersenne_twister.hpp>
+#include <boost/random/uniform_real.hpp>
+#include <boost/random/uniform_int.hpp>
+#include <boost/random/variate_generator.hpp>
+#include <boost/random/normal_distribution.hpp>
+
+namespace random_numbers
+{
+/** \brief Random number generation (wrapper for boost). An instance of this class
+    cannot be used by multiple threads at once (member functions
+    are not const). However, the constructor is thread safe and
+    different instances can be used safely in any number of
+    threads. It is also guaranteed that all created instances will
+    have a "random" random seed. */
+class RandomNumberGenerator
+{
+public:
+  /** \brief Constructor. Always sets a "random" random seed */
+  RandomNumberGenerator(void);
+
+  /** \brief Constructor. Allow a seed to be specified for deterministic behaviour */
+  RandomNumberGenerator(boost::uint32_t seed);
+
+  /** \brief Generate a random real between 0 and 1 */
+  double uniform01(void)
+  {
+    return uni_();
+  }
+
+  /**
+   * @brief Generate a random real within given bounds: [\e lower_bound, \e upper_bound)
+   * @param lower_bound The lower bound
+   * @param upper_bound The upper bound
+   */
+  double uniformReal(double lower_bound, double upper_bound)
+  {
+    return (upper_bound - lower_bound) * uni_() + lower_bound;
+  }
+
+  /** \brief Generate a random real using a normal distribution with mean 0 and variance 1 */
+  double gaussian01(void)
+  {
+    return normal_();
+  }
+
+  /** \brief Generate a random real using a normal distribution with given mean and variance */
+  double gaussian(double mean, double stddev)
+  {
+    return normal_() * stddev + mean;
+  }
+
+  /** \brief Uniform random unit quaternion sampling. The computed value has the order (x,y,z,w)
+   * @param value[4] A four dimensional array in which the computed quaternion will be returned
+   */
+  void quaternion(double value[4]);
+
+  /** \brief Generate an integer uniformly at random within a specified range (inclusive) */
+  int uniformInteger(int min, int max)
+  {
+    boost::uniform_int<> dis(min, max);
+    return dis(generator_);
+  }
+
+  /**
+   * \brief Allow the randomly generated seed to be saved so that experiments / benchmarks can be recreated in the
+   * future
+   */
+  boost::uint32_t getFirstSeed();
+
+private:
+  boost::mt19937 generator_;
+  boost::uniform_real<> uniDist_;
+  boost::normal_distribution<> normalDist_;
+  boost::variate_generator<boost::mt19937&, boost::uniform_real<> > uni_;
+  boost::variate_generator<boost::mt19937&, boost::normal_distribution<> > normal_;
+};
+}  // namespace random_numbers
+
+#endif

Devices/Libraries/Ros/random_numbers/package.xml

@@ -0,0 +1,20 @@
+<package>
+  <name>random_numbers</name>
+  <version>0.3.2</version>
+  <description>
+  This  library contains wrappers for generating floating point values, integers, quaternions using boost libraries.
+
+  The constructor of the wrapper is guaranteed to be thread safe and initialize its random number generator to a random seed.
+  Seeds are obtained using a separate and different random number generator.
+  </description>
+  <author email="isucan@willowgarage.edu">Ioan Sucan</author>
+  <maintainer email="stevenragnarok@osrfoundation.org">Steven! Ragnarök</maintainer>
+  <license>BSD</license>
+  <url type="website">http://ros.org/wiki/random_numbers</url>
+
+  <buildtool_depend>catkin</buildtool_depend>
+
+  <build_depend>boost</build_depend>
+
+  <run_depend>boost</run_depend>
+</package>

Devices/Libraries/Ros/random_numbers/src/random_numbers.cpp

@@ -0,0 +1,108 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2008, Willow Garage, Inc.
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of the Willow Garage nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *********************************************************************/
+
+/* Author: Ioan Sucan */
+
+#include "random_numbers/random_numbers.h"
+
+#include <boost/random/lagged_fibonacci.hpp>
+#include <boost/random/uniform_int.hpp>
+#include <boost/thread/mutex.hpp>
+#include <boost/date_time/posix_time/posix_time.hpp>
+#include <boost/math/constants/constants.hpp>
+#include <boost/scoped_ptr.hpp>
+
+static boost::uint32_t first_seed_ = 0;
+
+/// Compute the first seed to be used; this function should be called only once
+static boost::uint32_t firstSeed(void)
+{
+  boost::scoped_ptr<int> mem(new int());
+  first_seed_ = (boost::uint32_t)(
+      (boost::posix_time::microsec_clock::universal_time() - boost::posix_time::ptime(boost::date_time::min_date_time))
+          .total_microseconds() +
+      (unsigned long long)(mem.get()));
+  return first_seed_;
+}
+
+/// We use a different random number generator for the seeds of the
+/// Other random generators. The root seed is from the number of
+/// nano-seconds in the current time.
+static boost::uint32_t nextSeed(void)
+{
+  static boost::mutex rngMutex;
+  boost::mutex::scoped_lock slock(rngMutex);
+  static boost::lagged_fibonacci607 sGen(firstSeed());
+  static boost::uniform_int<> sDist(1, 1000000000);
+  static boost::variate_generator<boost::lagged_fibonacci607&, boost::uniform_int<> > s(sGen, sDist);
+  boost::uint32_t v = s();
+  return v;
+}
+
+random_numbers::RandomNumberGenerator::RandomNumberGenerator(void)
+  : generator_(nextSeed())
+  , uniDist_(0, 1)
+  , normalDist_(0, 1)
+  , uni_(generator_, uniDist_)
+  , normal_(generator_, normalDist_)
+{
+}
+
+random_numbers::RandomNumberGenerator::RandomNumberGenerator(boost::uint32_t seed)
+  : generator_(seed), uniDist_(0, 1), normalDist_(0, 1), uni_(generator_, uniDist_), normal_(generator_, normalDist_)
+{
+  // Because we manually specified a seed, we need to save it ourselves
+  first_seed_ = seed;
+}
+
+// From: "Uniform Random Rotations", Ken Shoemake, Graphics Gems III,
+//       pg. 124-132
+void random_numbers::RandomNumberGenerator::quaternion(double value[4])
+{
+  double x0 = uni_();
+  double r1 = sqrt(1.0 - x0), r2 = sqrt(x0);
+  double t1 = 2.0 * boost::math::constants::pi<double>() * uni_(),
+         t2 = 2.0 * boost::math::constants::pi<double>() * uni_();
+  double c1 = cos(t1), s1 = sin(t1);
+  double c2 = cos(t2), s2 = sin(t2);
+  value[0] = s1 * r1;
+  value[1] = c1 * r1;
+  value[2] = s2 * r2;
+  value[3] = c2 * r2;
+}
+
+boost::uint32_t random_numbers::RandomNumberGenerator::getFirstSeed()
+{
+  return first_seed_;
+}

Devices/Libraries/Ros/ros_canopen/.github/workflows/main.yml

@@ -0,0 +1,34 @@
+# This config uses industrial_ci (https://github.com/ros-industrial/industrial_ci.git).
+# For troubleshooting, see readme (https://github.com/ros-industrial/industrial_ci/blob/master/README.rst)
+
+name: CI
+
+on:
+  push:
+  pull_request:
+  schedule:
+   - cron: '0 4 * * *' # every day at 4 AM (UTC)
+  workflow_dispatch: # allow manually starting this workflow
+
+jobs:
+  industrial_ci:
+    name: ROS ${{ matrix.ROS_DISTRO }} (${{ matrix.ROS_REPO }})
+    runs-on: ubuntu-latest
+    strategy:
+      matrix: # matrix is the product of entries
+        ROS_DISTRO: [melodic, noetic]
+        ROS_REPO: [testing, main]
+    env:
+      CCACHE_DIR: "${{ github.workspace }}/.ccache" # directory for ccache (and how we enable ccache in industrial_ci)
+    steps:
+      - uses: actions/checkout@v2 # clone target repository
+      - uses: actions/cache@v2 # fetch/store the directory used by ccache before/after the ci run
+        with:
+          path: ${{ env.CCACHE_DIR }}
+          key: ccache-${{ matrix.ROS_DISTRO }}-${{ matrix.ROS_REPO }}-${{github.run_id}}
+          restore-keys: |
+            ccache-${{ matrix.ROS_DISTRO }}-${{ matrix.ROS_REPO }}-
+      - uses: 'ros-industrial/industrial_ci@master' # run industrial_ci
+        env:
+          ROS_DISTRO: ${{ matrix.ROS_DISTRO }}
+          ROS_REPO: ${{ matrix.ROS_REPO }}

Devices/Libraries/Ros/ros_canopen/.github/workflows/prerelease.yml

@@ -0,0 +1,20 @@
+name: Pre-release
+
+on: [workflow_dispatch]
+
+jobs:
+  default:
+    strategy:
+      matrix:
+        distro: [melodic, noetic]
+
+    env:
+      BUILDER: catkin_make_isolated
+      ROS_DISTRO: ${{ matrix.distro }}
+      PRERELEASE: true
+
+    name: "${{ matrix.distro }}"
+    runs-on: ubuntu-latest
+    steps:
+      - uses: actions/checkout@v2
+      - uses: ros-industrial/industrial_ci@master

Devices/Libraries/Ros/ros_canopen/.gitignore

@@ -0,0 +1,440 @@
+# ---> VisualStudio
+## Ignore Visual Studio temporary files, build results, and
+## files generated by popular Visual Studio add-ons.
+##
+## Get latest from https://github.com/github/gitignore/blob/main/VisualStudio.gitignore
+
+# User-specific files
+*.rsuser
+*.suo
+*.user
+*.userosscache
+*.sln.docstates
+
+# User-specific files (MonoDevelop/Xamarin Studio)
+*.userprefs
+
+# Mono auto generated files
+mono_crash.*
+
+# Build results
+[Dd]ebug/
+[Dd]ebugPublic/
+[Rr]elease/
+[Rr]eleases/
+x64/
+x86/
+[Ww][Ii][Nn]32/
+[Aa][Rr][Mm]/
+[Aa][Rr][Mm]64/
+bld/
+[Bb]in/
+[Oo]bj/
+[Ll]og/
+[Ll]ogs/
+
+# Visual Studio 2015/2017 cache/options directory
+.vs/
+# Uncomment if you have tasks that create the project's static files in wwwroot
+#wwwroot/
+
+# Visual Studio 2017 auto generated files
+Generated\ Files/
+
+# MSTest test Results
+[Tt]est[Rr]esult*/
+[Bb]uild[Ll]og.*
+
+# NUnit
+*.VisualState.xml
+TestResult.xml
+nunit-*.xml
+
+# Build Results of an ATL Project
+[Dd]ebugPS/
+[Rr]eleasePS/
+dlldata.c
+
+# Benchmark Results
+BenchmarkDotNet.Artifacts/
+
+# .NET Core
+project.lock.json
+project.fragment.lock.json
+artifacts/
+
+# ASP.NET Scaffolding
+ScaffoldingReadMe.txt
+
+# StyleCop
+StyleCopReport.xml
+
+# Files built by Visual Studio
+*_i.c
+*_p.c
+*_h.h
+*.ilk
+*.meta
+*.obj
+*.iobj
+*.pch
+*.pdb
+*.ipdb
+*.pgc
+*.pgd
+*.rsp
+*.sbr
+*.tlb
+*.tli
+*.tlh
+*.tmp
+*.tmp_proj
+*_wpftmp.csproj
+*.log
+*.tlog
+*.vspscc
+*.vssscc
+.builds
+*.pidb
+*.svclog
+*.scc
+
+# Chutzpah Test files
+_Chutzpah*
+
+# Visual C++ cache files
+ipch/
+*.aps
+*.ncb
+*.opendb
+*.opensdf
+*.sdf
+*.cachefile
+*.VC.db
+*.VC.VC.opendb
+
+# Visual Studio profiler
+*.psess
+*.vsp
+*.vspx
+*.sap
+
+# Visual Studio Trace Files
+*.e2e
+
+# TFS 2012 Local Workspace
+$tf/
+
+# Guidance Automation Toolkit
+*.gpState
+
+# ReSharper is a .NET coding add-in
+_ReSharper*/
+*.[Rr]e[Ss]harper
+*.DotSettings.user
+
+# TeamCity is a build add-in
+_TeamCity*
+
+# DotCover is a Code Coverage Tool
+*.dotCover
+
+# AxoCover is a Code Coverage Tool
+.axoCover/*
+!.axoCover/settings.json
+
+# Coverlet is a free, cross platform Code Coverage Tool
+coverage*.json
+coverage*.xml
+coverage*.info
+
+# Visual Studio code coverage results
+*.coverage
+*.coveragexml
+
+# NCrunch
+_NCrunch_*
+.*crunch*.local.xml
+nCrunchTemp_*
+
+# MightyMoose
+*.mm.*
+AutoTest.Net/
+
+# Web workbench (sass)
+.sass-cache/
+
+# Installshield output folder
+[Ee]xpress/
+
+# DocProject is a documentation generator add-in
+DocProject/buildhelp/
+DocProject/Help/*.HxT
+DocProject/Help/*.HxC
+DocProject/Help/*.hhc
+DocProject/Help/*.hhk
+DocProject/Help/*.hhp
+DocProject/Help/Html2
+DocProject/Help/html
+
+# Click-Once directory
+publish/
+
+# Publish Web Output
+*.[Pp]ublish.xml
+*.azurePubxml
+# Note: Comment the next line if you want to checkin your web deploy settings,
+# but database connection strings (with potential passwords) will be unencrypted
+*.pubxml
+*.publishproj
+
+# Microsoft Azure Web App publish settings. Comment the next line if you want to
+# checkin your Azure Web App publish settings, but sensitive information contained
+# in these scripts will be unencrypted
+PublishScripts/
+
+# NuGet Packages
+*.nupkg
+# NuGet Symbol Packages
+*.snupkg
+# The packages folder can be ignored because of Package Restore
+**/[Pp]ackages/*
+# except build/, which is used as an MSBuild target.
+!**/[Pp]ackages/build/
+# Uncomment if necessary however generally it will be regenerated when needed
+#!**/[Pp]ackages/repositories.config
+# NuGet v3's project.json files produces more ignorable files
+*.nuget.props
+*.nuget.targets
+
+# Microsoft Azure Build Output
+csx/
+*.build.csdef
+
+# Microsoft Azure Emulator
+ecf/
+rcf/
+
+# Windows Store app package directories and files
+AppPackages/
+BundleArtifacts/
+Package.StoreAssociation.xml
+_pkginfo.txt
+*.appx
+*.appxbundle
+*.appxupload
+
+# Visual Studio cache files
+# files ending in .cache can be ignored
+*.[Cc]ache
+# but keep track of directories ending in .cache
+!?*.[Cc]ache/
+
+# Others
+ClientBin/
+~$*
+*~
+*.dbmdl
+*.dbproj.schemaview
+*.jfm
+*.pfx
+*.publishsettings
+orleans.codegen.cs
+
+# Including strong name files can present a security risk
+# (https://github.com/github/gitignore/pull/2483#issue-259490424)
+#*.snk
+
+# Since there are multiple workflows, uncomment next line to ignore bower_components
+# (https://github.com/github/gitignore/pull/1529#issuecomment-104372622)
+#bower_components/
+
+# RIA/Silverlight projects
+Generated_Code/
+
+# Backup & report files from converting an old project file
+# to a newer Visual Studio version. Backup files are not needed,
+# because we have git ;-)
+_UpgradeReport_Files/
+Backup*/
+UpgradeLog*.XML
+UpgradeLog*.htm
+ServiceFabricBackup/
+*.rptproj.bak
+
+# SQL Server files
+*.mdf
+*.ldf
+*.ndf
+
+# Business Intelligence projects
+*.rdl.data
+*.bim.layout
+*.bim_*.settings
+*.rptproj.rsuser
+*- [Bb]ackup.rdl
+*- [Bb]ackup ([0-9]).rdl
+*- [Bb]ackup ([0-9][0-9]).rdl
+
+# Microsoft Fakes
+FakesAssemblies/
+
+# GhostDoc plugin setting file
+*.GhostDoc.xml
+
+# Node.js Tools for Visual Studio
+.ntvs_analysis.dat
+node_modules/
+
+# Visual Studio 6 build log
+*.plg
+
+# Visual Studio 6 workspace options file
+*.opt
+
+# Visual Studio 6 auto-generated workspace file (contains which files were open etc.)
+*.vbw
+
+# Visual Studio 6 auto-generated project file (contains which files were open etc.)
+*.vbp
+
+# Visual Studio 6 workspace and project file (working project files containing files to include in project)
+*.dsw
+*.dsp
+
+# Visual Studio 6 technical files
+*.ncb
+*.aps
+
+# Visual Studio LightSwitch build output
+**/*.HTMLClient/GeneratedArtifacts
+**/*.DesktopClient/GeneratedArtifacts
+**/*.DesktopClient/ModelManifest.xml
+**/*.Server/GeneratedArtifacts
+**/*.Server/ModelManifest.xml
+_Pvt_Extensions
+
+# Paket dependency manager
+.paket/paket.exe
+paket-files/
+
+# FAKE - F# Make
+.fake/
+
+# CodeRush personal settings
+.cr/personal
+
+# Python Tools for Visual Studio (PTVS)
+__pycache__/
+*.pyc
+
+# Cake - Uncomment if you are using it
+# tools/**
+# !tools/packages.config
+
+# Tabs Studio
+*.tss
+
+# Telerik's JustMock configuration file
+*.jmconfig
+
+# BizTalk build output
+*.btp.cs
+*.btm.cs
+*.odx.cs
+*.xsd.cs
+
+# OpenCover UI analysis results
+OpenCover/
+
+# Azure Stream Analytics local run output
+ASALocalRun/
+
+# MSBuild Binary and Structured Log
+*.binlog
+
+# NVidia Nsight GPU debugger configuration file
+*.nvuser
+
+# MFractors (Xamarin productivity tool) working folder
+.mfractor/
+
+# Local History for Visual Studio
+.localhistory/
+
+# Visual Studio History (VSHistory) files
+.vshistory/
+
+# BeatPulse healthcheck temp database
+healthchecksdb
+
+# Backup folder for Package Reference Convert tool in Visual Studio 2017
+MigrationBackup/
+
+# Ionide (cross platform F# VS Code tools) working folder
+.ionide/
+
+# Fody - auto-generated XML schema
+FodyWeavers.xsd
+
+# VS Code files for those working on multiple tools
+.vscode/*
+!.vscode/settings.json
+!.vscode/tasks.json
+!.vscode/launch.json
+!.vscode/extensions.json
+*.code-workspace
+
+# Local History for Visual Studio Code
+.history/
+
+# Windows Installer files from build outputs
+*.cab
+*.msi
+*.msix
+*.msm
+*.msp
+
+# JetBrains Rider
+*.sln.iml
+
+# ---> VisualStudioCode
+.vscode/*
+!.vscode/settings.json
+!.vscode/tasks.json
+!.vscode/launch.json
+!.vscode/extensions.json
+!.vscode/*.code-snippets
+
+# Local History for Visual Studio Code
+.history/
+
+# Built Visual Studio Code Extensions
+*.vsix
+
+# temporary files
+*~
+
+# autogenerated folders
+bin
+build
+lib
+__pycache__
+msg_gen
+srv_gen
+
+# autogenerated msgs
+*Action.msg
+*Action.py
+*ActionFeedback.msg
+*ActionFeedback.py
+*ActionGoal.msg
+*ActionGoal.py
+*Feedback.msg
+*Feedback.py
+*Goal.msg
+*Goal.py
+*Result.msg
+*Result.py
+*.pyc
+

Devices/Libraries/Ros/ros_canopen/.travis.yml

@@ -0,0 +1,27 @@
+language: generic
+services:
+  - docker
+
+cache:
+  directories:
+    - $HOME/.ccache
+
+env:
+  global:
+    - CCACHE_DIR=$HOME/.ccache
+  matrix:
+    - ROS_DISTRO="melodic" ROS_REPO=ros-shadow-fixed
+    - ROS_DISTRO="melodic" ROS_REPO=ros CATKIN_LINT=pedantic
+    - ROS_DISTRO="melodic" ROSDEP_SKIP_KEYS=muparser EXPECT_EXIT_CODE=1
+    - ROS_DISTRO="melodic" ABICHECK_URL='github:ros-industrial/ros_canopen#melodic'
+    - ROS_DISTRO="noetic"
+    - ROS_DISTRO="noetic" ROS_REPO=ros OS_NAME=debian OS_CODE_NAME=buster
+
+matrix:
+  allow_failures:
+    - env: ROS_DISTRO="melodic" ABICHECK_URL='github:ros-industrial/ros_canopen#melodic'
+
+install:
+  - git clone --quiet --depth=1 https://github.com/ros-industrial/industrial_ci.git .industrial_ci -b master
+script:
+  - .industrial_ci/travis.sh

Devices/Libraries/Ros/ros_canopen/CONTRIBUTING.md

@@ -0,0 +1,21 @@
+ROS-Industrial is a community project. We welcome contributions from any source, from those who are extremely active to casual users. The following sections outline the steps on how to contribute to ROS-Industrial. It assumes there is an existing repository to which one would like to contribute (item 1 in the figure above) and one is familiar with the Git "Fork and Branch" workflow, detailed [here](http://blog.scottlowe.org/2015/01/27/using-fork-branch-git-workflow/).
+
+1. Before any development is undertaken, a contributor would communicate a need and/or issue to the ROS-Industrial community. This can be done by submitting an issue on the appropriate GitHub repo, the [issues repo](https://github.com/ros-industrial/ros_industrial_issues), or by posting a message in the [ROS-Industrial category on ROS Discourse](//swri-ros-pkg-dev@googlegroups.com). . Doing so may save you time if similar development is underway and ensure that whatever approach you take is acceptable to the community of reviewers once it is submitted.
+2. The second step (item 2) is to implement your change. If you are working on a code contribution, we highly recommend you utilize the [ROS Qt-Creator Plug-in](http://rosindustrial.org/news/2016/6/9/ros-qt-ide-plugin). Verify that your change successfully builds and passes all tests.
+3. Next, push your changes to a "feature" branch in your personal fork of the repo and issue a pull request (PR)(item 3). The PR allows maintainers to review the submitted code. Before the PR can be accepted, the maintainer and contributor must agree that the contribution is implemented appropriately. This process can take several back-and-forth steps (see [example](https://github.com/ros-industrial/motoman/pull/89)). Contributors should expect to spend as much time reviewing/changing the code as on the initial implementation. This time can be minimized by communicating with the ROS-Industrial community before any contribution is made.
+4. Issuing a Pull Request (PR) triggers the [Travis Continuous Integrations (CI)](https://github.com/ros-industrial/industrial_ci) step (item 4) which happens automatically in the background. The Travis CI performs several operations, and if any of the steps below fail, then the PR is marked accordingly for the maintainer.
+  * Travis Workflow:
+    * Installs a barebones ROS distribution on a fresh Ubuntu virtual machine.
+    * Creates a catkin workspace and puts the repository in it.
+    * Uses wstool to check out any from-source dependencies (i.e. other repositories).
+    * Resolves package dependencies using rosdep (i.e. install packages using apt-get).
+    * Compiles the catkin workspace.
+    * Runs all available unit tests.
+5. If the PR passes Travis CI and one of the maintainers is satisfied with the changes, they post a +1 as a comment on the PR (item 5). The +1 signifies that the PR is ready to be merged. All PRs require at least one +1 and pass Travis CI before it can be merged.
+6. The next step (item 6) is for the PR to be merged into the main branch. This is done through the GitHub web interface by selecting the “Merge pull request” button. After the PR is merged, all status badges are updated automatically.
+7. Periodically, the maintainer will release the package (item 7), which then gets sent to the [ROS Build Farm](http://wiki.ros.org/build.ros.org) for Debian creation.
+8. The publishing of the released packages (item 8) is managed by OSRF and is not on a set schedule. This usually happens when all packages for a given distro are built successfully and stable. The current status for the distro kinetic can be found [here](http://repositories.ros.org/status_page/ros_kinetic_default.html) . Navigating to other distros can be done by changing the distro name in the link.
+9. Once the package has been published, it is available to be installed by the developer (item 9).
+10. After the install of a new version, the developer may have questions, experience issues or it may not have the necessary functionality which should all be reported on the packages GitHub repository as an issue (item 10). If an issue is identified or there is missing functionality that the developer requires, the cycle starts back at (item 2).
+
+For more details, please refer to the [ROS-I wiki](http://wiki.ros.org/Industrial/DevProcess).

Devices/Libraries/Ros/ros_canopen/LICENSE

@@ -0,0 +1,165 @@
+GNU LESSER GENERAL PUBLIC LICENSE
+                       Version 3, 29 June 2007
+
+ Copyright (C) 2007 Free Software Foundation, Inc. <http://fsf.org/>
+ Everyone is permitted to copy and distribute verbatim copies
+ of this license document, but changing it is not allowed.
+
+
+  This version of the GNU Lesser General Public License incorporates
+the terms and conditions of version 3 of the GNU General Public
+License, supplemented by the additional permissions listed below.
+
+  0. Additional Definitions.
+
+  As used herein, "this License" refers to version 3 of the GNU Lesser
+General Public License, and the "GNU GPL" refers to version 3 of the GNU
+General Public License.
+
+  "The Library" refers to a covered work governed by this License,
+other than an Application or a Combined Work as defined below.
+
+  An "Application" is any work that makes use of an interface provided
+by the Library, but which is not otherwise based on the Library.
+Defining a subclass of a class defined by the Library is deemed a mode
+of using an interface provided by the Library.
+
+  A "Combined Work" is a work produced by combining or linking an
+Application with the Library.  The particular version of the Library
+with which the Combined Work was made is also called the "Linked
+Version".
+
+  The "Minimal Corresponding Source" for a Combined Work means the
+Corresponding Source for the Combined Work, excluding any source code
+for portions of the Combined Work that, considered in isolation, are
+based on the Application, and not on the Linked Version.
+
+  The "Corresponding Application Code" for a Combined Work means the
+object code and/or source code for the Application, including any data
+and utility programs needed for reproducing the Combined Work from the
+Application, but excluding the System Libraries of the Combined Work.
+
+  1. Exception to Section 3 of the GNU GPL.
+
+  You may convey a covered work under sections 3 and 4 of this License
+without being bound by section 3 of the GNU GPL.
+
+  2. Conveying Modified Versions.
+
+  If you modify a copy of the Library, and, in your modifications, a
+facility refers to a function or data to be supplied by an Application
+that uses the facility (other than as an argument passed when the
+facility is invoked), then you may convey a copy of the modified
+version:
+
+   a) under this License, provided that you make a good faith effort to
+   ensure that, in the event an Application does not supply the
+   function or data, the facility still operates, and performs
+   whatever part of its purpose remains meaningful, or
+
+   b) under the GNU GPL, with none of the additional permissions of
+   this License applicable to that copy.
+
+  3. Object Code Incorporating Material from Library Header Files.
+
+  The object code form of an Application may incorporate material from
+a header file that is part of the Library.  You may convey such object
+code under terms of your choice, provided that, if the incorporated
+material is not limited to numerical parameters, data structure
+layouts and accessors, or small macros, inline functions and templates
+(ten or fewer lines in length), you do both of the following:
+
+   a) Give prominent notice with each copy of the object code that the
+   Library is used in it and that the Library and its use are
+   covered by this License.
+
+   b) Accompany the object code with a copy of the GNU GPL and this license
+   document.
+
+  4. Combined Works.
+
+  You may convey a Combined Work under terms of your choice that,
+taken together, effectively do not restrict modification of the
+portions of the Library contained in the Combined Work and reverse
+engineering for debugging such modifications, if you also do each of
+the following:
+
+   a) Give prominent notice with each copy of the Combined Work that
+   the Library is used in it and that the Library and its use are
+   covered by this License.
+
+   b) Accompany the Combined Work with a copy of the GNU GPL and this license
+   document.
+
+   c) For a Combined Work that displays copyright notices during
+   execution, include the copyright notice for the Library among
+   these notices, as well as a reference directing the user to the
+   copies of the GNU GPL and this license document.
+
+   d) Do one of the following:
+
+       0) Convey the Minimal Corresponding Source under the terms of this
+       License, and the Corresponding Application Code in a form
+       suitable for, and under terms that permit, the user to
+       recombine or relink the Application with a modified version of
+       the Linked Version to produce a modified Combined Work, in the
+       manner specified by section 6 of the GNU GPL for conveying
+       Corresponding Source.
+
+       1) Use a suitable shared library mechanism for linking with the
+       Library.  A suitable mechanism is one that (a) uses at run time
+       a copy of the Library already present on the user's computer
+       system, and (b) will operate properly with a modified version
+       of the Library that is interface-compatible with the Linked
+       Version.
+
+   e) Provide Installation Information, but only if you would otherwise
+   be required to provide such information under section 6 of the
+   GNU GPL, and only to the extent that such information is
+   necessary to install and execute a modified version of the
+   Combined Work produced by recombining or relinking the
+   Application with a modified version of the Linked Version. (If
+   you use option 4d0, the Installation Information must accompany
+   the Minimal Corresponding Source and Corresponding Application
+   Code. If you use option 4d1, you must provide the Installation
+   Information in the manner specified by section 6 of the GNU GPL
+   for conveying Corresponding Source.)
+
+  5. Combined Libraries.
+
+  You may place library facilities that are a work based on the
+Library side by side in a single library together with other library
+facilities that are not Applications and are not covered by this
+License, and convey such a combined library under terms of your
+choice, if you do both of the following:
+
+   a) Accompany the combined library with a copy of the same work based
+   on the Library, uncombined with any other library facilities,
+   conveyed under the terms of this License.
+
+   b) Give prominent notice with the combined library that part of it
+   is a work based on the Library, and explaining where to find the
+   accompanying uncombined form of the same work.
+
+  6. Revised Versions of the GNU Lesser General Public License.
+
+  The Free Software Foundation may publish revised and/or new versions
+of the GNU Lesser General Public License from time to time. Such new
+versions will be similar in spirit to the present version, but may
+differ in detail to address new problems or concerns.
+
+  Each version is given a distinguishing version number. If the
+Library as you received it specifies that a certain numbered version
+of the GNU Lesser General Public License "or any later version"
+applies to it, you have the option of following the terms and
+conditions either of that published version or of any later version
+published by the Free Software Foundation. If the Library as you
+received it does not specify a version number of the GNU Lesser
+General Public License, you may choose any version of the GNU Lesser
+General Public License ever published by the Free Software Foundation.
+
+  If the Library as you received it specifies that a proxy can decide
+whether future versions of the GNU Lesser General Public License shall
+apply, that proxy's public statement of acceptance of any version is
+permanent authorization for you to choose that version for the
+Library.

Devices/Libraries/Ros/ros_canopen/README.md

@@ -0,0 +1,11 @@
+ros_canopen
+===========
+
+Canopen implementation for ROS.
+
+[![Build Status](https://travis-ci.com/ros-industrial/ros_canopen.svg?branch=melodic-devel)](https://travis-ci.com/ros-industrial/ros_canopen)
+[![License: LGPL v3](https://img.shields.io/badge/License-LGPL%20v3-blue.svg)](https://www.gnu.org/licenses/lgpl-3.0)
+([![License: BSD 3-Clause](https://img.shields.io/badge/License-BSD%203--Clause-blue.svg)](https://opensource.org/licenses/BSD-3-Clause) for `can_msgs` and `socketcan_bridge`)
+
+The current develop branch is `melodic-devel`, it targets ROS `melodic`. Needs C++14 compiler.
+The released version gets synced over to the distro branch for each release.

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

@@ -0,0 +1,72 @@
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for package can_msgs
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+0.8.5 (2020-09-22)
+------------------
+
+0.8.4 (2020-08-22)
+------------------
+
+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)
+------------------
+
+0.8.1 (2019-07-14)
+------------------
+* Set C++ standard to c++14
+* Contributors: Harsh Deshpande
+
+0.8.0 (2018-07-11)
+------------------
+
+0.7.8 (2018-05-04)
+------------------
+
+0.7.7 (2018-05-04)
+------------------
+
+0.7.6 (2017-08-30)
+------------------
+
+0.7.5 (2017-05-29)
+------------------
+
+0.7.4 (2017-04-25)
+------------------
+
+0.7.3 (2017-04-25)
+------------------
+
+0.7.2 (2017-03-28)
+------------------
+
+0.7.1 (2017-03-20)
+------------------
+
+0.7.0 (2016-12-13)
+------------------
+
+0.6.5 (2016-12-10)
+------------------
+* hamonized versions
+* styled and sorted CMakeLists.txt
+  * removed boilerplate comments
+  * indention
+  * reviewed exported dependencies
+* styled and sorted package.xml
+* Adds message_runtime to can_msgs dependencies.
+  Added the missing dependency, also changes message_generation to a build_depend.
+* Finalizes work on the socketcan_bridge and can_msgs.
+  Readies the packages socketcan_bridge and can_msgs for the merge with ros_canopen.
+  Bumps the version for both packages to 0.1.0. Final cleanup in CMakeLists, added
+  comments to the shell script and launchfile used for testing.
+* Introduces the can_msgs package for message types.
+  Package to hold CAN message types, the Frame message type can contain the data
+  as returned by SocketCAN.
+* Contributors: Ivor Wanders, Mathias Lüdtke

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

@@ -0,0 +1,29 @@
+cmake_minimum_required(VERSION 3.0.2)
+project(can_msgs)
+
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD_REQUIRED ON)
+  set(CMAKE_CXX_STANDARD 14)
+endif()
+
+find_package(catkin REQUIRED
+  COMPONENTS
+    message_generation
+    std_msgs
+)
+
+add_message_files(DIRECTORY msg
+  FILES
+    Frame.msg
+)
+
+generate_messages(
+  DEPENDENCIES
+    std_msgs
+)
+
+catkin_package(
+  CATKIN_DEPENDS
+    message_runtime
+    std_msgs
+)

Devices/Libraries/Ros/ros_canopen/can_msgs/msg/Frame.msg

@@ -0,0 +1,7 @@
+Header header
+uint32 id
+bool is_rtr
+bool is_extended
+bool is_error
+uint8 dlc
+uint8[8] data

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

@@ -0,0 +1,24 @@
+<?xml version="1.0"?>
+<package format="2">.
+  <name>can_msgs</name>
+  <version>0.8.5</version>
+  <description>CAN related message types.</description>
+
+  <maintainer email="mathias.luedtke@ipa.fraunhofer.de">Mathias Lüdtke</maintainer>
+  <author email="ivor@iwanders.net">Ivor Wanders</author>
+
+  <license>BSD</license>
+
+  <url type="website">http://wiki.ros.org/can_msgs</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>
+
+  <depend>std_msgs</depend>
+
+  <build_depend>message_generation</build_depend>
+  <build_export_depend>message_runtime</build_export_depend>
+  <exec_depend>message_runtime</exec_depend>
+
+</package>

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

@@ -0,0 +1,233 @@
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for package canopen_402
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+0.8.5 (2020-09-22)
+------------------
+
+0.8.4 (2020-08-22)
+------------------
+* handle illegal states in nextStateForEnabling
+* tranistion -> transition
+* Contributors: Mathias Lüdtke, Mikael Arguedas
+
+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)
+------------------
+* 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
+* Contributors: Harsh Deshpande
+
+0.8.0 (2018-07-11)
+------------------
+* handle invalid supported drive modes object
+* made Mode402::registerMode a variadic template
+* use std::isnan
+* migrated to std::atomic
+* migrated to std::unordered_map and std::unordered_set
+* migrated to std pointers
+* fix initialization bug in ProfiledPositionMode
+* 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 state_switch_timeout parameter to motor.
+* 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
+* Contributors: Alexander Gutenkunst, Mathias Lüdtke
+
+0.7.6 (2017-08-30)
+------------------
+
+0.7.5 (2017-05-29)
+------------------
+
+0.7.4 (2017-04-25)
+------------------
+* use portable boost::math::isnan
+* Contributors: Mathias Lüdtke
+
+0.7.3 (2017-04-25)
+------------------
+
+0.7.2 (2017-03-28)
+------------------
+
+0.7.1 (2017-03-20)
+------------------
+* do quickstop for halt only if operation is enabled
+* Contributors: Mathias Lüdtke
+
+0.7.0 (2016-12-13)
+------------------
+
+0.6.5 (2016-12-10)
+------------------
+* stop on internal limit only if limit was not reached before
+* hardened code with the help of cppcheck
+* Do not send control if it was not changed
+  Otherwise invalid state machine transitions might get commanded
+* styled and sorted CMakeLists.txt
+  * removed boilerplate comments
+  * indention
+  * reviewed exported dependencies
+* styled and sorted package.xml
+* update package URLs
+* added option to turn off mode monitor
+* reset Fault_Reset bit in output only
+* enforce rising edge on fault reset bit on init and recover
+* Revert "Enforce rising edge on fault reset bit on init and recover"
+* enforce rising edge on fault reset bit on init and recover
+* Merge pull request `#117 <https://github.com/ipa-mdl/ros_canopen/issues/117>`_ from ipa-mdl/condvars
+  Reviewed condition variables
+* use boost::numeric_cast for limit checks since it handles 64bit values right
+* add clamping test
+* enforce target type limits
+* ModeTargetHelper is now template
+* readability fix
+* simplified State402::waitForNewState and Motor402::switchState
+* Set Halt bit unless current mode releases it
+* Contributors: Mathias Lüdtke
+
+0.6.4 (2015-07-03)
+------------------
+
+0.6.3 (2015-06-30)
+------------------
+* improved PP mode
+* do not quickstop in fault states
+* do not diable selected mode on recover, just restart it
+* initialize to No_Mode
+* removed some empty lines
+* implemented support for mode switching in a certain 402 state, defaults to Operation_Enable
+* pass LayerStatus to switchMode
+* remove enableMotor, introduced switchState
+* added motor_layer settings
+* fixed PP mode
+* explicit find for class_loader
+* fail-safe setting of op_mode to No_Mode
+* Improved behaviour of concurrent commands
+* added alternative Transitions 7 and 10 via Quickstop
+* added alternative success condition to waitForNewState
+* make motor init/recover interruptable
+* changed maintainer
+* removed SM-based 402 implementation
+* added Motor402 plugin
+* added new 402 implementation
+* added MotorBase
+* Added validity checks
+* Removed overloaded functions and makes the handle functions protected
+* Removes test executable
+* Removes unnecessary configure_drive and clears the set Targets
+* Some position fixes
+* Removed timeout
+  Reduced recover trials
+* Removes some logs
+* Homing integrated
+* handleRead, handleWrite fixes
+* Merge remote-tracking branch 'mdl/indigo_dev' into refactor_sm
+  Conflicts:
+  canopen_402/include/canopen_402/canopen_402.h
+  canopen_402/src/canopen_402/canopen_402.cpp
+  canopen_motor_node/src/control_node.cpp
+* Moved supported_drive_modes to ModeSpecificEntries
+* * Init, Recover, Halt for SCHUNK
+  * Removed sleeps from the state machine
+  * Now works as reentrant states
+* refactored Layer mechanisms
+* Recover failure
+* Merge remote-tracking branch 'mdl/indigo_dev' into refactor_sm
+  Conflicts:
+  canopen_402/include/canopen_402/canopen_402.h
+  canopen_402/src/canopen_402/canopen_402.cpp
+* Removing some unnecessary couts
+* First version with Recover
+  * Tested on SCHUNK LWA4D
+* Initializing all modules at once
+* Moving SCHUNK using the IP mode sub-state machine
+* Schunk does not set operation mode via synchronized RPDO
+* initialize homing_needed to false
+* Working with the guard handling and some scoped_locks to prevent unwanted access
+* Passing ``state_`` to motor machine
+* Fixes crash for unitialized boost pointer for ``target_vel_`` and ``target_pos_``
+* Thread running
+* Separates the hw with the SM test
+  Advance on the Mode Switching Machine
+* Organizing IPMode State Machine
+* Adds mode switch and a pre-version of the turnOn sequence
+* Event argument passed to the Motor state machine
+* Adds the internal actions
+* Control_word is set from motor state machine
+* Motor abstraction on higher level machine
+  Some pointers organization
+* * Begins with the Higher Level Machine
+  * Separates the status and control from the 402 node
+* Ip mode sub-machine
+* Organizing the status and control machine
+* do not read homing method if homing mode is not supported
+* inti ``enter_mode_failure_`` to false
+* require message strings for error indicators, added missing strings, added ROS logging in sync loop
+* Merge pull request `#75 <https://github.com/ros-industrial/ros_canopen/issues/75>`_ from mistoll/indigo_release_candidate
+  Move ip_mode_sub_mode to configureModeSpecificEntries
+* Fixed tabs/spaces
+* bind IP sub mode only if IP is supported
+* Move ip_mode_sub_mode to configureModeSpecificEntries
+* Update state_machine.h
+* Ongoing changes for the state machine
+* * Eliminates Internal State conflict
+  * Treats exceptions inside the state machine
+* Cleaning the 402.cpp file
+* Test file
+* Adds state machine definition
+* Adds state machine simple test
+* Some cleaning necessary to proceed
+* Header files for isolating the 402 state machine
+* Effort value
+* Merge pull request `#6 <https://github.com/ros-industrial/ros_canopen/issues/6>`_ from ipa-mdl/indigo_dev
+  Work-around for https://github.com/ipa320/ros_canopen/issues/62
+* Merge branch 'indigo_dev' of https://github.com/ipa-mdl/ros_canopen into indigo_dev
+* fixed unintialized members
+* Mode Error priority
+* Order issue
+* Merge branch 'indigo_dev' of https://github.com/ipa-mdl/ros_canopen into indigo_dev
+  Conflicts:
+  canopen_motor_node/CMakeLists.txt
+* Error status
+* 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
+* Contributors: Florian Weisshardt, Mathias Lüdtke, Michael Stoll, Thiago de Freitas Oliveira Araujo, 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_402 to canopen_402
+* Contributors: Florian Weisshardt, Mathias Lüdtke

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

@@ -0,0 +1,78 @@
+cmake_minimum_required(VERSION 3.0.2)
+project(canopen_402)
+
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD_REQUIRED ON)
+  set(CMAKE_CXX_STANDARD 14)
+endif()
+
+find_package(catkin REQUIRED
+  COMPONENTS
+    canopen_master
+    class_loader
+)
+
+find_package(Boost REQUIRED
+)
+
+catkin_package(
+  INCLUDE_DIRS
+    include
+  LIBRARIES
+    ${PROJECT_NAME}
+  CATKIN_DEPENDS
+    canopen_master
+  DEPENDS
+    Boost
+)
+
+include_directories(
+  include
+  ${Boost_INCLUDE_DIRS}
+  ${catkin_INCLUDE_DIRS}
+)
+
+# canopen_402
+add_library(${PROJECT_NAME}
+  src/motor.cpp
+)
+target_link_libraries(${PROJECT_NAME}
+  ${catkin_LIBRARIES}
+)
+
+# canopen_402_plugin
+add_library(${PROJECT_NAME}_plugin
+  src/plugin.cpp
+)
+target_link_libraries(${PROJECT_NAME}_plugin
+   ${catkin_LIBRARIES}
+   ${PROJECT_NAME}
+)
+
+install(
+  TARGETS
+    ${PROJECT_NAME}
+    ${PROJECT_NAME}_plugin
+  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(
+  FILES
+    ${PROJECT_NAME}_plugin.xml
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+)
+
+if(CATKIN_ENABLE_TESTING)
+  catkin_add_gtest(${PROJECT_NAME}-test_clamping
+    test/clamping.cpp
+  )
+  target_link_libraries(${PROJECT_NAME}-test_clamping
+    ${catkin_LIBRARIES}
+  )
+endif()

Devices/Libraries/Ros/ros_canopen/canopen_402/canopen_402_plugin.xml

@@ -0,0 +1,5 @@
+<library path="lib/libcanopen_402_plugin">
+  <class type="canopen::Motor402::Allocator" base_class_type="canopen::MotorBase::Allocator">
+    <description>Allocator for Motor402 instances</description>
+  </class>
+</library>

Devices/Libraries/Ros/ros_canopen/canopen_402/include/canopen_402/base.h

@@ -0,0 +1,45 @@
+#ifndef CANOPEN_402_BASE_H
+#define CANOPEN_402_BASE_H
+
+#include <canopen_master/canopen.h>
+
+namespace canopen
+{
+
+class MotorBase : public canopen::Layer {
+protected:
+    MotorBase(const std::string &name) : Layer(name) {}
+public:
+    enum OperationMode
+    {
+        No_Mode = 0,
+        Profiled_Position = 1,
+        Velocity = 2,
+        Profiled_Velocity = 3,
+        Profiled_Torque = 4,
+        Reserved = 5,
+        Homing = 6,
+        Interpolated_Position = 7,
+        Cyclic_Synchronous_Position = 8,
+        Cyclic_Synchronous_Velocity = 9,
+        Cyclic_Synchronous_Torque = 10,
+    };
+    virtual bool setTarget(double val) = 0;
+    virtual bool enterModeAndWait(uint16_t mode) = 0;
+    virtual bool isModeSupported(uint16_t mode) = 0;
+    virtual uint16_t getMode() = 0;
+    virtual void registerDefaultModes(ObjectStorageSharedPtr storage) {}
+
+    typedef std::shared_ptr<MotorBase> MotorBaseSharedPtr;
+
+    class Allocator {
+    public:
+        virtual MotorBaseSharedPtr allocate(const std::string &name, ObjectStorageSharedPtr storage, const canopen::Settings &settings) = 0;
+        virtual ~Allocator() {}
+    };
+};
+typedef MotorBase::MotorBaseSharedPtr MotorBaseSharedPtr;
+
+}
+
+#endif

Devices/Libraries/Ros/ros_canopen/canopen_402/include/canopen_402/motor.h

@@ -0,0 +1,390 @@
+#ifndef CANOPEN_402_MOTOR_H
+#define CANOPEN_402_MOTOR_H
+
+#include <canopen_402/base.h>
+#include <canopen_master/canopen.h>
+#include <functional>
+#include <boost/container/flat_map.hpp>
+
+#include <boost/numeric/conversion/cast.hpp>
+#include <limits>
+#include <algorithm>
+
+namespace canopen
+{
+
+class State402{
+public:
+    enum StatusWord
+    {
+        SW_Ready_To_Switch_On=0,
+        SW_Switched_On=1,
+        SW_Operation_enabled=2,
+        SW_Fault=3,
+        SW_Voltage_enabled=4,
+        SW_Quick_stop=5,
+        SW_Switch_on_disabled=6,
+        SW_Warning=7,
+        SW_Manufacturer_specific0=8,
+        SW_Remote=9,
+        SW_Target_reached=10,
+        SW_Internal_limit=11,
+        SW_Operation_mode_specific0=12,
+        SW_Operation_mode_specific1=13,
+        SW_Manufacturer_specific1=14,
+        SW_Manufacturer_specific2=15
+    };
+    enum InternalState
+    {
+        Unknown = 0,
+        Start = 0,
+        Not_Ready_To_Switch_On = 1,
+        Switch_On_Disabled = 2,
+        Ready_To_Switch_On = 3,
+        Switched_On = 4,
+        Operation_Enable = 5,
+        Quick_Stop_Active = 6,
+        Fault_Reaction_Active = 7,
+        Fault = 8,
+    };
+    InternalState getState();
+    InternalState read(uint16_t sw);
+    bool waitForNewState(const time_point &abstime, InternalState &state);
+    State402() : state_(Unknown) {}
+private:
+    boost::condition_variable cond_;
+    boost::mutex mutex_;
+    InternalState state_;
+};
+
+class Command402 {
+    struct Op {
+        uint16_t to_set_;
+        uint16_t to_reset_;
+        Op(uint16_t to_set ,uint16_t to_reset) : to_set_(to_set), to_reset_(to_reset) {}
+        void operator()(uint16_t &val) const {
+            val = (val & ~to_reset_) | to_set_;
+        }
+    };
+    class TransitionTable {
+        boost::container::flat_map< std::pair<State402::InternalState, State402::InternalState>, Op > transitions_;
+        void add(const State402::InternalState &from, const State402::InternalState &to, Op op){
+            transitions_.insert(std::make_pair(std::make_pair(from, to), op));
+        }
+    public:
+        TransitionTable();
+        const Op& get(const State402::InternalState &from, const State402::InternalState &to) const {
+            return transitions_.at(std::make_pair(from, to));
+        }
+    };
+    static const TransitionTable transitions_;
+    static State402::InternalState nextStateForEnabling(State402::InternalState state);
+    Command402();
+public:
+    enum ControlWord
+    {
+        CW_Switch_On=0,
+        CW_Enable_Voltage=1,
+        CW_Quick_Stop=2,
+        CW_Enable_Operation=3,
+        CW_Operation_mode_specific0=4,
+        CW_Operation_mode_specific1=5,
+        CW_Operation_mode_specific2=6,
+        CW_Fault_Reset=7,
+        CW_Halt=8,
+        CW_Operation_mode_specific3=9,
+        // CW_Reserved1=10,
+        CW_Manufacturer_specific0=11,
+        CW_Manufacturer_specific1=12,
+        CW_Manufacturer_specific2=13,
+        CW_Manufacturer_specific3=14,
+        CW_Manufacturer_specific4=15,
+    };
+    static bool setTransition(uint16_t &cw, const State402::InternalState &from, const State402::InternalState &to, State402::InternalState *next);
+};
+
+template<uint16_t MASK> class WordAccessor{
+    uint16_t &word_;
+public:
+    WordAccessor(uint16_t &word) : word_(word) {}
+    bool set(uint8_t bit){
+        uint16_t val = MASK & (1<<bit);
+        word_ |= val;
+        return val;
+   }
+    bool reset(uint8_t bit){
+        uint16_t val = MASK & (1<<bit);
+        word_ &= ~val;
+        return val;
+    }
+    bool get(uint8_t bit) const { return word_ & (1<<bit); }
+    uint16_t get() const { return word_ & MASK; }
+    WordAccessor & operator=(const uint16_t &val){
+        uint16_t was = word_;
+        word_ = (word_ & ~MASK) | (val & MASK);
+        return *this;
+    }
+};
+
+class Mode {
+public:
+    const uint16_t mode_id_;
+    Mode(uint16_t id) : mode_id_(id) {}
+    typedef WordAccessor<(1<<Command402::CW_Operation_mode_specific0)|(1<<Command402::CW_Operation_mode_specific1)|(1<<Command402::CW_Operation_mode_specific2)|(1<<Command402::CW_Operation_mode_specific3)> OpModeAccesser;
+    virtual bool start() = 0;
+    virtual bool read(const uint16_t &sw) = 0;
+    virtual bool write(OpModeAccesser& cw) = 0;
+    virtual bool setTarget(const double &val) { ROSCANOPEN_ERROR("canopen_402", "Mode::setTarget not implemented"); return false; }
+    virtual ~Mode() {}
+};
+typedef std::shared_ptr<Mode> ModeSharedPtr;
+
+template<typename T> class ModeTargetHelper : public Mode {
+    T target_;
+    std::atomic<bool> has_target_;
+
+public:
+    ModeTargetHelper(uint16_t mode) : Mode (mode) {}
+    bool hasTarget() { return has_target_; }
+    T getTarget() { return target_; }
+    virtual bool setTarget(const double &val) {
+        if(std::isnan(val)){
+            ROSCANOPEN_ERROR("canopen_402", "target command is not a number");
+            return false;
+        }
+
+        using boost::numeric_cast;
+        using boost::numeric::positive_overflow;
+        using boost::numeric::negative_overflow;
+
+        try
+        {
+            target_= numeric_cast<T>(val);
+        }
+        catch(negative_overflow&) {
+            ROSCANOPEN_WARN("canopen_402", "Command " << val << " does not fit into target, clamping to min limit");
+            target_= std::numeric_limits<T>::min();
+        }
+        catch(positive_overflow&) {
+            ROSCANOPEN_WARN("canopen_402", "Command " << val << " does not fit into target, clamping to max limit");
+            target_= std::numeric_limits<T>::max();
+        }
+        catch(...){
+            ROSCANOPEN_ERROR("canopen_402", "Was not able to cast command " << val);
+            return false;
+        }
+
+        has_target_ = true;
+        return true;
+    }
+    virtual bool start() { has_target_ = false; return true; }
+};
+
+template<uint16_t ID, typename TYPE, uint16_t OBJ, uint8_t SUB, uint16_t CW_MASK> class ModeForwardHelper : public ModeTargetHelper<TYPE> {
+    canopen::ObjectStorage::Entry<TYPE> target_entry_;
+public:
+    ModeForwardHelper(ObjectStorageSharedPtr storage) : ModeTargetHelper<TYPE>(ID) {
+        if(SUB) storage->entry(target_entry_, OBJ, SUB);
+        else storage->entry(target_entry_, OBJ);
+    }
+    virtual bool read(const uint16_t &sw) { return true;}
+    virtual bool write(Mode::OpModeAccesser& cw) {
+        if(this->hasTarget()){
+            cw = cw.get() | CW_MASK;
+            target_entry_.set(this->getTarget());
+            return true;
+        }else{
+            cw = cw.get() & ~CW_MASK;
+            return false;
+        }
+    }
+};
+
+typedef ModeForwardHelper<MotorBase::Profiled_Velocity, int32_t, 0x60FF, 0, 0> ProfiledVelocityMode;
+typedef ModeForwardHelper<MotorBase::Profiled_Torque, int16_t, 0x6071, 0, 0> ProfiledTorqueMode;
+typedef ModeForwardHelper<MotorBase::Cyclic_Synchronous_Position, int32_t, 0x607A, 0, 0> CyclicSynchronousPositionMode;
+typedef ModeForwardHelper<MotorBase::Cyclic_Synchronous_Velocity, int32_t, 0x60FF, 0, 0> CyclicSynchronousVelocityMode;
+typedef ModeForwardHelper<MotorBase::Cyclic_Synchronous_Torque, int16_t, 0x6071, 0, 0> CyclicSynchronousTorqueMode;
+typedef ModeForwardHelper<MotorBase::Velocity, int16_t, 0x6042, 0, (1<<Command402::CW_Operation_mode_specific0)|(1<<Command402::CW_Operation_mode_specific1)|(1<<Command402::CW_Operation_mode_specific2)> VelocityMode;
+typedef ModeForwardHelper<MotorBase::Interpolated_Position, int32_t, 0x60C1, 0x01, (1<<Command402::CW_Operation_mode_specific0)> InterpolatedPositionMode;
+
+class ProfiledPositionMode : public ModeTargetHelper<int32_t> {
+    canopen::ObjectStorage::Entry<int32_t> target_position_;
+    double last_target_;
+    uint16_t sw_;
+public:
+    enum SW_masks {
+        MASK_Reached = (1<<State402::SW_Target_reached),
+        MASK_Acknowledged = (1<<State402::SW_Operation_mode_specific0),
+        MASK_Error = (1<<State402::SW_Operation_mode_specific1),
+    };
+    enum CW_bits {
+        CW_NewPoint =  Command402::CW_Operation_mode_specific0,
+        CW_Immediate =  Command402::CW_Operation_mode_specific1,
+        CW_Blending =  Command402::CW_Operation_mode_specific3,
+    };
+    ProfiledPositionMode(ObjectStorageSharedPtr storage) : ModeTargetHelper(MotorBase::Profiled_Position) {
+        storage->entry(target_position_, 0x607A);
+    }
+    virtual bool start() { sw_ = 0; last_target_= std::numeric_limits<double>::quiet_NaN(); return ModeTargetHelper::start(); }
+    virtual bool read(const uint16_t &sw) { sw_ = sw; return (sw & MASK_Error) == 0; }
+    virtual bool write(OpModeAccesser& cw) {
+        cw.set(CW_Immediate);
+        if(hasTarget()){
+            int32_t target = getTarget();
+            if((sw_ & MASK_Acknowledged) == 0 && target != last_target_){
+                if(cw.get(CW_NewPoint)){
+                    cw.reset(CW_NewPoint); // reset if needed
+                }else{
+                    target_position_.set(target);
+                    cw.set(CW_NewPoint);
+                    last_target_ = target;
+                }
+            } else if (sw_ & MASK_Acknowledged){
+                cw.reset(CW_NewPoint);
+            }
+            return true;
+        }
+        return false;
+    }
+};
+
+class HomingMode: public Mode{
+protected:
+    enum SW_bits {
+        SW_Attained =  State402::SW_Operation_mode_specific0,
+        SW_Error =  State402::SW_Operation_mode_specific1,
+    };
+    enum CW_bits {
+        CW_StartHoming =  Command402::CW_Operation_mode_specific0,
+    };
+public:
+    HomingMode() : Mode(MotorBase::Homing) {}
+    virtual bool executeHoming(canopen::LayerStatus &status) = 0;
+};
+
+class DefaultHomingMode: public HomingMode{
+    canopen::ObjectStorage::Entry<int8_t> homing_method_;
+    std::atomic<bool> execute_;
+
+    boost::mutex mutex_;
+    boost::condition_variable cond_;
+    uint16_t status_;
+
+    enum SW_masks {
+        MASK_Reached = (1<<State402::SW_Target_reached),
+        MASK_Attained = (1<<SW_Attained),
+        MASK_Error = (1<<SW_Error),
+    };
+    bool error(canopen::LayerStatus &status, const std::string& msg) { execute_= false; status.error(msg); return false; }
+public:
+    DefaultHomingMode(ObjectStorageSharedPtr storage) {
+        storage->entry(homing_method_, 0x6098);
+    }
+    virtual bool start();
+    virtual bool read(const uint16_t &sw);
+    virtual bool write(OpModeAccesser& cw);
+
+    virtual bool executeHoming(canopen::LayerStatus &status);
+};
+
+class Motor402 : public MotorBase
+{
+public:
+
+    Motor402(const std::string &name, ObjectStorageSharedPtr storage, const canopen::Settings &settings)
+    : MotorBase(name), status_word_(0),control_word_(0),
+      switching_state_(State402::InternalState(settings.get_optional<unsigned int>("switching_state", static_cast<unsigned int>(State402::Operation_Enable)))),
+      monitor_mode_(settings.get_optional<bool>("monitor_mode", true)),
+      state_switch_timeout_(settings.get_optional<unsigned int>("state_switch_timeout", 5))
+    {
+        storage->entry(status_word_entry_, 0x6041);
+        storage->entry(control_word_entry_, 0x6040);
+        storage->entry(op_mode_display_, 0x6061);
+        storage->entry(op_mode_, 0x6060);
+        try{
+            storage->entry(supported_drive_modes_, 0x6502);
+        }
+        catch(...){
+        }
+    }
+
+    virtual bool setTarget(double val);
+    virtual bool enterModeAndWait(uint16_t mode);
+    virtual bool isModeSupported(uint16_t mode);
+    virtual uint16_t getMode();
+
+    template<typename T, typename ...Args>
+    bool registerMode(uint16_t mode, Args&&... args) {
+        return mode_allocators_.insert(std::make_pair(mode,  [args..., mode, this](){
+            if(isModeSupportedByDevice(mode)) registerMode(mode, ModeSharedPtr(new T(args...)));
+        })).second;
+    }
+
+    virtual void registerDefaultModes(ObjectStorageSharedPtr storage){
+        registerMode<ProfiledPositionMode> (MotorBase::Profiled_Position, storage);
+        registerMode<VelocityMode> (MotorBase::Velocity, storage);
+        registerMode<ProfiledVelocityMode> (MotorBase::Profiled_Velocity, storage);
+        registerMode<ProfiledTorqueMode> (MotorBase::Profiled_Torque, storage);
+        registerMode<DefaultHomingMode> (MotorBase::Homing, storage);
+        registerMode<InterpolatedPositionMode> (MotorBase::Interpolated_Position, storage);
+        registerMode<CyclicSynchronousPositionMode> (MotorBase::Cyclic_Synchronous_Position, storage);
+        registerMode<CyclicSynchronousVelocityMode> (MotorBase::Cyclic_Synchronous_Velocity, storage);
+        registerMode<CyclicSynchronousTorqueMode> (MotorBase::Cyclic_Synchronous_Torque, storage);
+    }
+
+    class Allocator : public MotorBase::Allocator{
+    public:
+        virtual MotorBaseSharedPtr allocate(const std::string &name, ObjectStorageSharedPtr storage, const canopen::Settings &settings);
+    };
+protected:
+    virtual void handleRead(LayerStatus &status, const LayerState &current_state);
+    virtual void handleWrite(LayerStatus &status, const LayerState &current_state);
+    virtual void handleDiag(LayerReport &report);
+    virtual void handleInit(LayerStatus &status);
+    virtual void handleShutdown(LayerStatus &status);
+    virtual void handleHalt(LayerStatus &status);
+    virtual void handleRecover(LayerStatus &status);
+
+private:
+    virtual bool isModeSupportedByDevice(uint16_t mode);
+    void registerMode(uint16_t id, const ModeSharedPtr &m);
+
+    ModeSharedPtr allocMode(uint16_t mode);
+
+    bool readState(LayerStatus &status, const LayerState &current_state);
+    bool switchMode(LayerStatus &status, uint16_t mode);
+    bool switchState(LayerStatus &status, const State402::InternalState &target);
+
+    std::atomic<uint16_t> status_word_;
+    uint16_t control_word_;
+    boost::mutex cw_mutex_;
+    std::atomic<bool> start_fault_reset_;
+    std::atomic<State402::InternalState> target_state_;
+
+
+    State402 state_handler_;
+
+    boost::mutex map_mutex_;
+    std::unordered_map<uint16_t, ModeSharedPtr > modes_;
+    typedef std::function<void()> AllocFuncType;
+    std::unordered_map<uint16_t, AllocFuncType> mode_allocators_;
+
+    ModeSharedPtr selected_mode_;
+    uint16_t mode_id_;
+    boost::condition_variable mode_cond_;
+    boost::mutex mode_mutex_;
+    const State402::InternalState switching_state_;
+    const bool monitor_mode_;
+    const boost::chrono::seconds state_switch_timeout_;
+
+    canopen::ObjectStorage::Entry<uint16_t>  status_word_entry_;
+    canopen::ObjectStorage::Entry<uint16_t >  control_word_entry_;
+    canopen::ObjectStorage::Entry<int8_t>  op_mode_display_;
+    canopen::ObjectStorage::Entry<int8_t>  op_mode_;
+    canopen::ObjectStorage::Entry<uint32_t>  supported_drive_modes_;
+};
+
+}
+
+#endif

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

@@ -0,0 +1,28 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>canopen_402</name>
+  <version>0.8.5</version>
+  <description>This implements the CANopen device profile for drives and motion control. CiA(r) 402</description>
+
+  <maintainer email="mathias.luedtke@ipa.fraunhofer.de">Mathias Lüdtke</maintainer>
+  <author email="tdf@ipa.fhg.de">Thiago de Freitas</author>
+  <author email="mathias.luedtke@ipa.fraunhofer.de">Mathias Lüdtke</author>
+
+  <license>LGPLv3</license>
+
+  <url type="website">http://wiki.ros.org/canopen_402</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>
+
+  <depend>libboost-dev</depend>
+  <depend>canopen_master</depend>
+  <depend>class_loader</depend>
+
+  <test_depend>rosunit</test_depend>
+
+  <export>
+     <canopen_402 plugin="${prefix}/canopen_402_plugin.xml" />
+  </export>
+</package>

Devices/Libraries/Ros/ros_canopen/canopen_402/src/motor.cpp

@@ -0,0 +1,549 @@
+#include <canopen_402/motor.h>
+#include <boost/thread/reverse_lock.hpp>
+
+namespace canopen
+{
+
+State402::InternalState State402::getState(){
+    boost::mutex::scoped_lock lock(mutex_);
+    return state_;
+}
+
+State402::InternalState State402::read(uint16_t sw) {
+    static const uint16_t r = (1 << SW_Ready_To_Switch_On);
+    static const uint16_t s = (1 << SW_Switched_On);
+    static const uint16_t o = (1 << SW_Operation_enabled);
+    static const uint16_t f = (1 << SW_Fault);
+    static const uint16_t q = (1 << SW_Quick_stop);
+    static const uint16_t d = (1 << SW_Switch_on_disabled);
+
+    InternalState new_state = Unknown;
+
+    uint16_t state = sw & ( d | q | f | o | s | r );
+    switch ( state )
+    {
+    //   ( d | q | f | o | s | r ):
+    case ( 0 | 0 | 0 | 0 | 0 | 0 ):
+    case ( 0 | q | 0 | 0 | 0 | 0 ):
+        new_state = Not_Ready_To_Switch_On;
+        break;
+
+    case ( d | 0 | 0 | 0 | 0 | 0 ):
+    case ( d | q | 0 | 0 | 0 | 0 ):
+        new_state =  Switch_On_Disabled;
+        break;
+
+    case ( 0 | q | 0 | 0 | 0 | r ):
+        new_state =  Ready_To_Switch_On;
+        break;
+
+    case ( 0 | q | 0 | 0 | s | r ):
+        new_state =  Switched_On;
+        break;
+
+    case ( 0 | q | 0 | o | s | r ):
+        new_state =  Operation_Enable;
+        break;
+
+    case ( 0 | 0 | 0 | o | s | r ):
+        new_state =  Quick_Stop_Active;
+        break;
+
+    case ( 0 | 0 | f | o | s | r ):
+    case ( 0 | q | f | o | s | r ):
+        new_state =  Fault_Reaction_Active;
+        break;
+
+    case ( 0 | 0 | f | 0 | 0 | 0 ):
+    case ( 0 | q | f | 0 | 0 | 0 ):
+        new_state =  Fault;
+        break;
+
+    default:
+        ROSCANOPEN_WARN("canopen_402", "Motor is currently in an unknown state: " << std::hex <<  state << std::dec);
+    }
+    boost::mutex::scoped_lock lock(mutex_);
+    if(new_state != state_){
+        state_ = new_state;
+        cond_.notify_all();
+    }
+    return state_;
+}
+bool State402::waitForNewState(const time_point &abstime, State402::InternalState &state){
+    boost::mutex::scoped_lock lock(mutex_);
+    while(state_ == state && cond_.wait_until(lock, abstime) == boost::cv_status::no_timeout) {}
+    bool res = state != state_;
+    state = state_;
+    return res;
+}
+
+const Command402::TransitionTable Command402::transitions_;
+
+Command402::TransitionTable::TransitionTable(){
+    typedef State402 s;
+
+    transitions_.reserve(32);
+
+    Op disable_voltage(0,(1<<CW_Fault_Reset) | (1<<CW_Enable_Voltage));
+    /* 7*/ add(s::Ready_To_Switch_On, s::Switch_On_Disabled, disable_voltage);
+    /* 9*/ add(s::Operation_Enable, s::Switch_On_Disabled, disable_voltage);
+    /*10*/ add(s::Switched_On, s::Switch_On_Disabled, disable_voltage);
+    /*12*/ add(s::Quick_Stop_Active, s::Switch_On_Disabled, disable_voltage);
+
+    Op automatic(0,0);
+    /* 0*/ add(s::Start, s::Not_Ready_To_Switch_On, automatic);
+    /* 1*/ add(s::Not_Ready_To_Switch_On, s::Switch_On_Disabled, automatic);
+    /*14*/ add(s::Fault_Reaction_Active, s::Fault, automatic);
+
+    Op shutdown((1<<CW_Quick_Stop) | (1<<CW_Enable_Voltage), (1<<CW_Fault_Reset) | (1<<CW_Switch_On));
+    /* 2*/ add(s::Switch_On_Disabled, s::Ready_To_Switch_On, shutdown);
+    /* 6*/ add(s::Switched_On, s::Ready_To_Switch_On, shutdown);
+    /* 8*/ add(s::Operation_Enable, s::Ready_To_Switch_On, shutdown);
+
+    Op switch_on((1<<CW_Quick_Stop) | (1<<CW_Enable_Voltage) | (1<<CW_Switch_On), (1<<CW_Fault_Reset) | (1<<CW_Enable_Operation));
+    /* 3*/ add(s::Ready_To_Switch_On, s::Switched_On, switch_on);
+    /* 5*/ add(s::Operation_Enable, s::Switched_On, switch_on);
+
+    Op enable_operation((1<<CW_Quick_Stop) | (1<<CW_Enable_Voltage) | (1<<CW_Switch_On) | (1<<CW_Enable_Operation), (1<<CW_Fault_Reset));
+    /* 4*/ add(s::Switched_On, s::Operation_Enable, enable_operation);
+    /*16*/ add(s::Quick_Stop_Active, s::Operation_Enable, enable_operation);
+
+    Op quickstop((1<<CW_Enable_Voltage), (1<<CW_Fault_Reset) | (1<<CW_Quick_Stop));
+    /* 7*/ add(s::Ready_To_Switch_On, s::Quick_Stop_Active, quickstop); // transit to Switch_On_Disabled
+    /*10*/ add(s::Switched_On, s::Quick_Stop_Active, quickstop); // transit to Switch_On_Disabled
+    /*11*/ add(s::Operation_Enable, s::Quick_Stop_Active, quickstop);
+
+    // fault reset
+    /*15*/ add(s::Fault, s::Switch_On_Disabled, Op((1<<CW_Fault_Reset), 0));
+}
+State402::InternalState Command402::nextStateForEnabling(State402::InternalState state){
+    switch(state){
+    case State402::Start:
+        return State402::Not_Ready_To_Switch_On;
+
+    case State402::Fault:
+    case State402::Not_Ready_To_Switch_On:
+        return State402::Switch_On_Disabled;
+
+    case State402::Switch_On_Disabled:
+        return State402::Ready_To_Switch_On;
+
+    case State402::Ready_To_Switch_On:
+        return State402::Switched_On;
+
+    case State402::Switched_On:
+    case State402::Quick_Stop_Active:
+    case State402::Operation_Enable:
+        return State402::Operation_Enable;
+
+    case State402::Fault_Reaction_Active:
+        return State402::Fault;
+    }
+    throw std::out_of_range ("state value is illegal");
+}
+
+bool Command402::setTransition(uint16_t &cw, const State402::InternalState &from, const State402::InternalState &to, State402::InternalState *next){
+    try{
+        if(from != to){
+            State402::InternalState hop = to;
+            if(next){
+                if(to == State402::Operation_Enable) hop = nextStateForEnabling(from);
+                *next = hop;
+            }
+            transitions_.get(from, hop)(cw);
+        }
+        return true;
+    }
+    catch(...){
+        ROSCANOPEN_WARN("canopen_402", "illegal transition " << from << " -> " << to);
+    }
+    return false;
+}
+
+template<uint16_t mask, uint16_t not_equal> struct masked_status_not_equal {
+    uint16_t &status_;
+    masked_status_not_equal(uint16_t &status) : status_(status) {}
+    bool operator()() const { return (status_ & mask) != not_equal; }
+};
+bool DefaultHomingMode::start() {
+    execute_ = false;
+    return read(0);
+}
+bool DefaultHomingMode::read(const uint16_t &sw) {
+    boost::mutex::scoped_lock lock(mutex_);
+    uint16_t old = status_;
+    status_ = sw & (MASK_Reached | MASK_Attained | MASK_Error);
+    if(old != status_){
+        cond_.notify_all();
+    }
+    return true;
+}
+bool DefaultHomingMode::write(Mode::OpModeAccesser& cw) {
+    cw = 0;
+    if(execute_){
+        cw.set(CW_StartHoming);
+        return true;
+    }
+    return false;
+}
+
+bool DefaultHomingMode::executeHoming(canopen::LayerStatus &status) {
+    if(!homing_method_.valid()){
+        return error(status, "homing method entry is not valid");
+    }
+
+    if(homing_method_.get_cached() == 0){
+        return true;
+    }
+
+    time_point prepare_time = get_abs_time(boost::chrono::seconds(1));
+    // ensure homing is not running
+    boost::mutex::scoped_lock lock(mutex_);
+    if(!cond_.wait_until(lock, prepare_time, masked_status_not_equal<MASK_Error | MASK_Reached, 0> (status_))){
+        return error(status, "could not prepare homing");
+    }
+    if(status_ & MASK_Error){
+        return error(status, "homing error before start");
+    }
+
+    execute_ = true;
+
+    // ensure start
+    if(!cond_.wait_until(lock, prepare_time, masked_status_not_equal<MASK_Error | MASK_Attained | MASK_Reached, MASK_Reached> (status_))){
+        return error(status, "homing did not start");
+    }
+    if(status_ & MASK_Error){
+        return error(status, "homing error at start");
+    }
+
+    time_point finish_time = get_abs_time(boost::chrono::seconds(10)); //
+
+    // wait for attained
+    if(!cond_.wait_until(lock, finish_time, masked_status_not_equal<MASK_Error | MASK_Attained, 0> (status_))){
+        return error(status, "homing not attained");
+    }
+    if(status_ & MASK_Error){
+        return error(status, "homing error during process");
+    }
+
+    // wait for motion stop
+    if(!cond_.wait_until(lock, finish_time, masked_status_not_equal<MASK_Error | MASK_Reached, 0> (status_))){
+        return error(status, "homing did not stop");
+    }
+    if(status_ & MASK_Error){
+        return error(status, "homing error during stop");
+    }
+
+    if((status_ & MASK_Reached) && (status_ & MASK_Attained)){
+        execute_ = false;
+        return true;
+    }
+
+    return error(status, "something went wrong while homing");
+}
+
+bool Motor402::setTarget(double val){
+    if(state_handler_.getState() == State402::Operation_Enable){
+        boost::mutex::scoped_lock lock(mode_mutex_);
+        return selected_mode_ && selected_mode_->setTarget(val);
+    }
+    return false;
+}
+bool Motor402::isModeSupported(uint16_t mode) { return mode != MotorBase::Homing && allocMode(mode); }
+
+bool Motor402::enterModeAndWait(uint16_t mode) {
+    LayerStatus s;
+    bool okay = mode != MotorBase::Homing && switchMode(s, mode);
+    if(!s.bounded<LayerStatus::Ok>()){
+        ROSCANOPEN_ERROR("canopen_402", "Could not switch to mode " << mode << ", reason: " << s.reason());
+    }
+    return okay;
+}
+
+uint16_t Motor402::getMode() {
+    boost::mutex::scoped_lock lock(mode_mutex_);
+    return selected_mode_ ? selected_mode_->mode_id_ :  MotorBase::No_Mode;
+}
+
+bool Motor402::isModeSupportedByDevice(uint16_t mode){
+    if(!supported_drive_modes_.valid()) {
+        BOOST_THROW_EXCEPTION( std::runtime_error("Supported drive modes (object 6502) is not valid"));
+    }
+    return mode > 0 && mode <= 32 && (supported_drive_modes_.get_cached() & (1<<(mode-1)));
+}
+void Motor402::registerMode(uint16_t id, const ModeSharedPtr &m){
+    boost::mutex::scoped_lock map_lock(map_mutex_);
+    if(m && m->mode_id_ == id) modes_.insert(std::make_pair(id, m));
+}
+
+ModeSharedPtr Motor402::allocMode(uint16_t mode){
+    ModeSharedPtr res;
+    if(isModeSupportedByDevice(mode)){
+        boost::mutex::scoped_lock map_lock(map_mutex_);
+        std::unordered_map<uint16_t, ModeSharedPtr >::iterator it = modes_.find(mode);
+        if(it != modes_.end()){
+            res = it->second;
+        }
+    }
+    return res;
+}
+
+bool Motor402::switchMode(LayerStatus &status, uint16_t mode) {
+
+    if(mode == MotorBase::No_Mode){
+        boost::mutex::scoped_lock lock(mode_mutex_);
+        selected_mode_.reset();
+        try{ // try to set mode
+            op_mode_.set(mode);
+        }catch(...){}
+        return true;
+    }
+
+    ModeSharedPtr next_mode = allocMode(mode);
+    if(!next_mode){
+        status.error("Mode is not supported.");
+        return false;
+    }
+
+    if(!next_mode->start()){
+        status.error("Could not start mode.");
+        return false;
+    }
+
+    { // disable mode handler
+        boost::mutex::scoped_lock lock(mode_mutex_);
+
+        if(mode_id_ == mode && selected_mode_ && selected_mode_->mode_id_ == mode){
+            // nothing to do
+            return true;
+        }
+
+        selected_mode_.reset();
+    }
+
+    if(!switchState(status, switching_state_)) return false;
+
+    op_mode_.set(mode);
+
+    bool okay = false;
+
+    {  // wait for switch
+        boost::mutex::scoped_lock lock(mode_mutex_);
+
+        time_point abstime = get_abs_time(boost::chrono::seconds(5));
+        if(monitor_mode_){
+            while(mode_id_ != mode && mode_cond_.wait_until(lock, abstime) == boost::cv_status::no_timeout) {}
+        }else{
+            while(mode_id_ != mode && get_abs_time() < abstime){
+                boost::reverse_lock<boost::mutex::scoped_lock> reverse(lock); // unlock inside loop
+                op_mode_display_.get(); // poll
+                boost::this_thread::sleep_for(boost::chrono::milliseconds(20)); // wait some time
+            }
+        }
+
+        if(mode_id_ == mode){
+            selected_mode_ = next_mode;
+            okay = true;
+        }else{
+            status.error("Mode switch timed out.");
+            op_mode_.set(mode_id_);
+        }
+    }
+
+    if(!switchState(status, State402::Operation_Enable)) return false;
+
+    return okay;
+
+}
+
+bool Motor402::switchState(LayerStatus &status, const State402::InternalState &target){
+    time_point abstime = get_abs_time(state_switch_timeout_);
+    State402::InternalState state = state_handler_.getState();
+    target_state_ = target;
+    while(state != target_state_){
+        boost::mutex::scoped_lock lock(cw_mutex_);
+        State402::InternalState next = State402::Unknown;
+        if(!Command402::setTransition(control_word_ ,state, target_state_ , &next)){
+            status.error("Could not set transition");
+            return false;
+        }
+        lock.unlock();
+        if(state != next && !state_handler_.waitForNewState(abstime, state)){
+            status.error("Transition timeout");
+            return false;
+        }
+    }
+    return state == target;
+}
+
+bool Motor402::readState(LayerStatus &status, const LayerState &current_state){
+    uint16_t old_sw, sw = status_word_entry_.get(); // TODO: added error handling
+    old_sw = status_word_.exchange(sw);
+
+    state_handler_.read(sw);
+
+    boost::mutex::scoped_lock lock(mode_mutex_);
+    uint16_t new_mode = monitor_mode_ ? op_mode_display_.get() : op_mode_display_.get_cached();
+    if(selected_mode_ && selected_mode_->mode_id_ == new_mode){
+        if(!selected_mode_->read(sw)){
+            status.error("Mode handler has error");
+        }
+    }
+    if(new_mode != mode_id_){
+        mode_id_ = new_mode;
+        mode_cond_.notify_all();
+    }
+    if(selected_mode_ && selected_mode_->mode_id_ != new_mode){
+        status.warn("mode does not match");
+    }
+    if(sw & (1<<State402::SW_Internal_limit)){
+        if(old_sw & (1<<State402::SW_Internal_limit) || current_state != Ready){
+            status.warn("Internal limit active");
+        }else{
+            status.error("Internal limit active");
+        }
+    }
+
+    return true;
+}
+void Motor402::handleRead(LayerStatus &status, const LayerState &current_state){
+    if(current_state > Off){
+        readState(status, current_state);
+    }
+}
+void Motor402::handleWrite(LayerStatus &status, const LayerState &current_state){
+    if(current_state > Off){
+        boost::mutex::scoped_lock lock(cw_mutex_);
+        control_word_ |= (1<<Command402::CW_Halt);
+        if(state_handler_.getState() == State402::Operation_Enable){
+            boost::mutex::scoped_lock lock(mode_mutex_);
+            Mode::OpModeAccesser cwa(control_word_);
+            bool okay = false;
+            if(selected_mode_ && selected_mode_->mode_id_ == mode_id_){
+                okay = selected_mode_->write(cwa);
+            } else {
+                cwa = 0;
+            }
+            if(okay) {
+                control_word_ &= ~(1<<Command402::CW_Halt);
+            }
+        }
+        if(start_fault_reset_.exchange(false)){
+            control_word_entry_.set_cached(control_word_ & ~(1<<Command402::CW_Fault_Reset));
+        }else{
+            control_word_entry_.set_cached(control_word_);
+        }
+    }
+}
+void Motor402::handleDiag(LayerReport &report){
+    uint16_t sw = status_word_;
+    State402::InternalState state = state_handler_.getState();
+
+    switch(state){
+    case State402::Not_Ready_To_Switch_On:
+    case State402::Switch_On_Disabled:
+    case State402::Ready_To_Switch_On:
+    case State402::Switched_On:
+        report.warn("Motor operation is not enabled");
+    case State402::Operation_Enable:
+        break;
+
+    case State402::Quick_Stop_Active:
+        report.error("Quick stop is active");
+        break;
+    case State402::Fault:
+    case State402::Fault_Reaction_Active:
+        report.error("Motor has fault");
+        break;
+    case State402::Unknown:
+        report.error("State is unknown");
+        report.add("status_word", sw);
+        break;
+    }
+
+    if(sw & (1<<State402::SW_Warning)){
+        report.warn("Warning bit is set");
+    }
+    if(sw & (1<<State402::SW_Internal_limit)){
+        report.error("Internal limit active");
+    }
+}
+void Motor402::handleInit(LayerStatus &status){
+    for(std::unordered_map<uint16_t, AllocFuncType>::iterator it = mode_allocators_.begin(); it != mode_allocators_.end(); ++it){
+        (it->second)();
+    }
+
+    if(!readState(status, Init)){
+        status.error("Could not read motor state");
+        return;
+    }
+    {
+        boost::mutex::scoped_lock lock(cw_mutex_);
+        control_word_ = 0;
+        start_fault_reset_ = true;
+    }
+    if(!switchState(status, State402::Operation_Enable)){
+        status.error("Could not enable motor");
+        return;
+    }
+
+    ModeSharedPtr m = allocMode(MotorBase::Homing);
+    if(!m){
+        return; // homing not supported
+    }
+
+    HomingMode *homing = dynamic_cast<HomingMode*>(m.get());
+
+    if(!homing){
+        status.error("Homing mode has incorrect handler");
+        return;
+    }
+
+    if(!switchMode(status, MotorBase::Homing)){
+        status.error("Could not enter homing mode");
+        return;
+    }
+
+    if(!homing->executeHoming(status)){
+        status.error("Homing failed");
+        return;
+    }
+
+    switchMode(status, No_Mode);
+}
+void Motor402::handleShutdown(LayerStatus &status){
+    switchMode(status, MotorBase::No_Mode);
+    switchState(status, State402::Switch_On_Disabled);
+}
+void Motor402::handleHalt(LayerStatus &status){
+    State402::InternalState state = state_handler_.getState();
+    boost::mutex::scoped_lock lock(cw_mutex_);
+
+    // do not demand quickstop in case of fault
+    if(state == State402::Fault_Reaction_Active || state == State402::Fault) return;
+
+    if(state != State402::Operation_Enable){
+        target_state_ = state;
+    }else{
+        target_state_ = State402::Quick_Stop_Active;
+        if(!Command402::setTransition(control_word_ ,state, State402::Quick_Stop_Active, 0)){
+            status.warn("Could not quick stop");
+        }
+    }
+}
+void Motor402::handleRecover(LayerStatus &status){
+    start_fault_reset_ = true;
+    {
+        boost::mutex::scoped_lock lock(mode_mutex_);
+        if(selected_mode_ && !selected_mode_->start()){
+            status.error("Could not restart mode.");
+            return;
+        }
+    }
+    if(!switchState(status, State402::Operation_Enable)){
+        status.error("Could not enable motor");
+        return;
+    }
+}
+
+} // namespace

Devices/Libraries/Ros/ros_canopen/canopen_402/src/plugin.cpp

@@ -0,0 +1,8 @@
+#include <class_loader/class_loader.hpp>
+#include <canopen_402/motor.h>
+
+canopen::MotorBaseSharedPtr canopen::Motor402::Allocator::allocate(const std::string &name, canopen::ObjectStorageSharedPtr storage, const canopen::Settings &settings) {
+    return std::make_shared<canopen::Motor402>(name, storage, settings);
+}
+
+CLASS_LOADER_REGISTER_CLASS(canopen::Motor402::Allocator, canopen::MotorBase::Allocator);

Devices/Libraries/Ros/ros_canopen/canopen_402/test/clamping.cpp

@@ -0,0 +1,57 @@
+#include <canopen_402/motor.h>
+#include <gtest/gtest.h>
+
+
+template<typename T> class ModeTargetHelperTest : public canopen::ModeTargetHelper<T>, public ::testing::Test{
+public:
+    ModeTargetHelperTest() : canopen::ModeTargetHelper<T>(0) {}
+    virtual bool read(const uint16_t &sw) { return false; }
+    virtual bool write(canopen::Mode::OpModeAccesser& cw) { return false; }
+};
+
+typedef ::testing::Types<uint8_t, int8_t, uint16_t, int16_t, uint32_t, int32_t, uint64_t, int64_t> MyTypes;
+
+TYPED_TEST_CASE(ModeTargetHelperTest, MyTypes);
+
+TYPED_TEST(ModeTargetHelperTest, CheckNaN){
+    ASSERT_FALSE(this->setTarget(std::numeric_limits<double>::quiet_NaN()));
+}
+
+TYPED_TEST(ModeTargetHelperTest, CheckZero){
+    ASSERT_TRUE(this->setTarget(0.0));
+}
+
+TYPED_TEST(ModeTargetHelperTest, CheckOne){
+    ASSERT_TRUE(this->setTarget(1.0));
+}
+
+TYPED_TEST(ModeTargetHelperTest, CheckMax){
+    double max = static_cast<double>(std::numeric_limits<TypeParam>::max());
+
+    ASSERT_TRUE(this->setTarget(max));
+    ASSERT_EQ(max, this->getTarget());
+
+    ASSERT_TRUE(this->setTarget(max-1));
+    ASSERT_EQ(max-1,this->getTarget());
+
+    ASSERT_TRUE(this->setTarget(max+1));
+    ASSERT_EQ(max, this->getTarget());
+}
+
+TYPED_TEST(ModeTargetHelperTest, CheckMin){
+    double min = static_cast<double>(std::numeric_limits<TypeParam>::min());
+
+    ASSERT_TRUE(this->setTarget(min));
+    ASSERT_EQ(min, this->getTarget());
+
+    ASSERT_TRUE(this->setTarget(min-1));
+    ASSERT_EQ(min, this->getTarget());
+
+    ASSERT_TRUE(this->setTarget(min+1));
+    ASSERT_EQ(min+1,this->getTarget());
+}
+
+int main(int argc, char **argv){
+  testing::InitGoogleTest(&argc, argv);
+  return RUN_ALL_TESTS();
+}

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
+

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

@@ -0,0 +1,263 @@
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for package canopen_master
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+0.8.5 (2020-09-22)
+------------------
+
+0.8.4 (2020-08-22)
+------------------
+* added settings parameter to DriverInterface::init
+* moved canopen::Settings into can namespace
+* 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)
+------------------
+* implemented LayerStatus::equals<>()
+* added support for SYNC counter in SimpleSyncLayer (`#349 <https://github.com/ipa-mdl/ros_canopen/issues/349>`_)
+* enable rosconsole_bridge bindings
+* switch to new logging macros
+* add logging based on console_bridge
+* removed implicit Header operator
+* Contributors: Mathias Lüdtke
+
+0.8.1 (2019-07-14)
+------------------
+* Set C++ standard to c++14
+* added Delegate helpers for backwards compatibility
+* implemented create\*ListenerM helpers
+* Replacing FastDelegate with std::function and std::bind.
+* Contributors: Harsh Deshpande, Joshua Whitley, Mathias Lüdtke
+
+0.8.0 (2018-07-11)
+------------------
+* migrated to std::function and std::bind
+* migrated to std::atomic
+* got rid of boost::noncopyable
+* replaced BOOST_FOREACH
+* migrated to std::unordered_map and std::unordered_set
+* migrated to std pointers
+* provided KeyHash
+  for use with unordered containers
+* added c_array access functons to can::Frame
+* 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
+* throw bad_cast if datatype is not supported
+* special handling of std::bad_cast
+* 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 EMCYHandler::resetErrors
+* added VectorHelper::callFunc
+  generalized call templates
+* Contributors: Mathias Lüdtke
+
+0.7.4 (2017-04-25)
+------------------
+
+0.7.3 (2017-04-25)
+------------------
+* enforce boost::chrono-based timer
+* Contributors: Mathias Lüdtke
+
+0.7.2 (2017-03-28)
+------------------
+* fix: handle EMCY as error, not as warning
+* Contributors: Mathias Lüdtke
+
+0.7.1 (2017-03-20)
+------------------
+* refactored EMCY handling into separate layer
+* print EMCY to stdout
+* send node start on recover
+  needed for external sync to work properly
+* pass halt on error unconditionally
+* added canopen_bcm_sync
+* implemented ExternalMaster
+* added object access services
+* implemented ObjectStorage::getStringReader
+* Contributors: Mathias Lüdtke
+
+0.7.0 (2016-12-13)
+------------------
+
+0.6.5 (2016-12-10)
+------------------
+* Merge pull request `#179 <https://github.com/ipa-mdl/ros_canopen/issues/179>`_ from ipa-mdl/mixed_case_access
+  support mixed-case access strings in EDS
+* decouple listener initialization from 1003 binding
+* introduced THROW_WITH_KEY and ObjectDict::key_info
+* added access type tests
+* convert access string to lowercase
+* Do not remove shared memory automatically
+* hardened code with the help of cppcheck
+* throw verbose exception if AccessType is missing (`#64 <https://github.com/ipa-mdl/ros_canopen/issues/64>`_)
+* styled and sorted CMakeLists.txt
+  * removed boilerplate comments
+  * indention
+  * reviewed exported dependencies
+* styled and sorted package.xml
+* canopen_master needs to depend on rosunit for gtest
+* update package URLs
+* fixed typo
+* do not reset PDO COB-ID if it is not writable
+* Do not recurse into sub-objects, handle them as simple data
+* strip string before searching for $NODEID
+* added NodeID/hex parser test
+* do full recover if if driver is not ready
+* wait for driver to be shutdown in run()
+* limit SDO reader to size of 1
+* do not send abort twice
+* removed unnecessary sleep (added for tests only)
+* catch all std exceptions in layer handlers
+* migrated SDOClient to BufferedReader
+* getter for LayerState
+* fixed lost wake-up condition, unified SDO accessors
+* minor NMT improvements
+* removed cond from PDOMapper, it does not wait on empty buffer anymore
+* Simple master counts nodes as well
+* throw exception on read from empty buffer
+* proper initialisation of PDO data from SDOs
+* change sync subscription only on change
+* shutdown and restart CAN layer on recover
+* canopen::Exception is now based on std::runtime_error
+* 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
+* stop after heartbeat was disabled, do not wait for state switch
+* added virtual destructor to SyncCounter
+* Use getHeartbeatInterval()
+* minor shutdown improvements
+* removed unstable StateWaiter::wait_for
+* Revert change to handleShutdown
+* Heartbeat interval is uint16, not double
+* Added validity check to heartbeat\_ (Some devices do not support heartbeat)
+* Contributors: Florian Weisshardt, Mathias Lüdtke, Michael Stoll
+
+0.6.4 (2015-07-03)
+------------------
+* added missing include, revised depends etc.
+
+0.6.3 (2015-06-30)
+------------------
+* added Settings class
+* added SimpleMaster
+* remove boost::posix_time::milliseconds from SyncProperties
+* removed support for silence_us since bus timing cannot be guaranteed
+* properly handle cases where def_val == init_val
+* implemented plugin-based Master allocators, defaults to LocalMaster
+* moved master/synclayer base classes to canopen.h
+* added support for non-continuous PDO ranges
+* added has() check to object dictionary interface
+* improved ObjectStorage entry interface
+* verbose out_of_range exception
+* improved timer: duration cast, autostart flag
+* reset sync waiter number after timeout
+* verbose timeout exception
+* little fix im EMCY diagnostics
+* string instead of mulit-char constant
+* Merge branch 'hwi_switch' into muparser
+* added std::string converters to ObjectDict::Key
+* do not warn on profile-only errors
+* added get_abs_time without parameter
+* link against boost_atomic for platforms with lock-based implementation
+* reset sent Reset and Reset_Com, c&p bug
+* stop heartbeat after node shutdown
+* protect reads of LayerState
+* protect layers in VectorHelper
+* protect buffer data
+* set error only if generic error bit is set, otherwise just warn about it
+* Fixes https://github.com/ipa320/ros_canopen/issues/81
+* Update emcy.cpp
+* removed debug outputs
+* refactored Layer mechanisms
+* simplified init
+* simplified EMCY handling
+* improved hearbeat handling
+* do not stop master on slave timeout
+* improved pending handling in complex layers
+* added set_cached for object entries
+* removed IPCLayer sync listener, loopback is disabled per default
+* Merge branch 'dummy_interface' into indigo_dev
+  Conflicts:
+  canopen_master/src/objdict.cpp
+* added sync silence feature
+* Merge remote-tracking branch 'origin/fix32bit' into indigo_dev
+* require message strings for error indicators, added missing strings, added ROS logging in sync loop
+* fix ambiguous buffer access with 32bit compilers
+* pad octet strings if necessary
+* reset pending to layers.begin()
+* enforce RPDO (device-side) transmimssion type to 1 if <=240
+* introduced LayerVector to unify pending support
+* introduced read_integer to enfoce hex parsing, closes `#74 <https://github.com/ros-industrial/ros_canopen/issues/74>`_
+* clear layer before plugin loader is deleted
+* Merge branch 'indigo_dev' of https://github.com/ipa320/ros_canopen into indigo_dev
+* Merge pull request `#70 <https://github.com/ros-industrial/ros_canopen/issues/70>`_ from ipa-mdl/pluginlib
+  added plugin feature to socketcan_interface
+* exception-aware get functions
+* removed RPDO sync timeout in favour of LayerStatus
+* added message string helper
+* EDS files are case-insensitive, so switching to iptree
+* handle errors entries that are not in the dictionary
+* sub entry number must be hex coded
+* do not send initilized-only PDO data
+* init entries if init value was given and default value was not
+* implemented threading in CANLayer
+* removed bitrate, added loopback to DriverInterface::init
+* removed SimpleLayer, migrated to Layer
+* Layer::pending and Layer::halt are now virtual pure as well
+* schunk version of reset
+* Merge branch 'elmo_console' of https://github.com/ipa-mdl/ros_canopen into dcf_overlay
+* remove debug prints
+* resize buffer if needed in expedited SDO upload
+* fix SDO segment download
+* only access EMCY errors if available
+* added ObjectStorage:Entry::valid()
+* added ObjectDict overlay feature
+* Fixes the bus controller problems for the Elmo chain
+* Work-around for Elmo SDO bug(?)
+* improved PDO buffer initialization, buffer if filled per SDO if needed
+* pass permission object
+* disable threading interrupts while waiting for SDO response
+* 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
+* Contributors: Mathias Lüdtke, Thiago de Freitas Oliveira Araujo, ipa-cob4-2, ipa-fmw, 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_master to canopen_master
+* Contributors: Florian Weisshardt, Mathias Lüdtke

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

@@ -0,0 +1,102 @@
+cmake_minimum_required(VERSION 3.0.2)
+project(canopen_master)
+
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD_REQUIRED ON)
+  set(CMAKE_CXX_STANDARD 14)
+endif()
+
+find_package(catkin REQUIRED
+  COMPONENTS
+    class_loader
+    socketcan_interface
+)
+
+find_package(Boost REQUIRED
+  COMPONENTS
+    atomic
+    chrono
+    thread
+)
+
+catkin_package(
+  INCLUDE_DIRS
+    include
+  LIBRARIES
+    ${PROJECT_NAME}
+  CATKIN_DEPENDS
+    socketcan_interface
+  DEPENDS
+    Boost
+)
+include_directories(include ${catkin_INCLUDE_DIRS})
+
+add_library(${PROJECT_NAME}
+  src/emcy.cpp
+  src/node.cpp
+  src/objdict.cpp
+  src/pdo.cpp
+  src/sdo.cpp
+)
+target_link_libraries(${PROJECT_NAME}
+  ${catkin_LIBRARIES}
+  ${Boost_LIBRARIES}
+)
+
+add_library(${PROJECT_NAME}_plugin
+  src/master_plugin.cpp
+)
+
+target_link_libraries(${PROJECT_NAME}_plugin
+  ${catkin_LIBRARIES}
+  ${Boost_LIBRARIES}
+  ${PROJECT_NAME}
+)
+
+# canopen_bcm_sync
+add_executable(canopen_bcm_sync
+  src/bcm_sync.cpp
+)
+target_link_libraries(canopen_bcm_sync
+  ${Boost_LIBRARIES}
+  ${catkin_LIBRARIES}
+)
+
+install(
+  TARGETS
+    canopen_bcm_sync
+    ${PROJECT_NAME}
+   ${PROJECT_NAME}_plugin
+  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(
+  FILES
+    master_plugin.xml
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+)
+
+if(CATKIN_ENABLE_TESTING)
+  catkin_add_gtest(${PROJECT_NAME}-test_parser
+    test/test_parser.cpp
+  )
+  target_link_libraries(${PROJECT_NAME}-test_parser
+    ${PROJECT_NAME}
+  )
+
+  catkin_add_gtest(${PROJECT_NAME}-test_node
+    test/test_node.cpp
+  )
+  target_link_libraries(${PROJECT_NAME}-test_node
+    ${PROJECT_NAME}
+  )
+endif()

Devices/Libraries/Ros/ros_canopen/canopen_master/include/canopen_master/bcm_sync.h

@@ -0,0 +1,167 @@
+#ifndef H_BCM_SYNC
+#define H_BCM_SYNC
+
+#include <socketcan_interface/bcm.h>
+#include <socketcan_interface/socketcan.h>
+#include <canopen_master/canopen.h>
+
+namespace canopen {
+
+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();
+}
+
+class BCMsync : public Layer {
+    boost::mutex mutex_;
+
+    std::string device_;
+
+    std::set<int> ignored_nodes_;
+    std::set<int> monitored_nodes_;
+    std::set<int> known_nodes_;
+    std::set<int> started_nodes_;
+
+    bool sync_running_;
+    can::BCMsocket bcm_;
+    can::SocketCANDriverSharedPtr  driver_;
+    uint16_t sync_ms_;
+    can::FrameListenerConstSharedPtr handler_;
+
+    std::vector<can::Frame> sync_frames_;
+
+    bool skipNode(uint8_t id){
+        if(ignored_nodes_.find(id) != ignored_nodes_.end()) return true;
+        if(!monitored_nodes_.empty() && monitored_nodes_.find(id) == monitored_nodes_.end()) return true;
+        known_nodes_.insert(id);
+        return false;
+    }
+
+    void handleFrame(const can::Frame &frame){
+        boost::mutex::scoped_lock lock(mutex_);
+
+        if(frame.id == NMT_ID){
+            if(frame.dlc > 1){
+                uint8_t cmd = frame.data[0];
+                uint8_t id = frame.data[1];
+                if(skipNode(id)) return;
+
+                if(cmd == 1){ // started
+                    if(id != 0) started_nodes_.insert(id);
+                    else started_nodes_.insert(known_nodes_.begin(), known_nodes_.end()); // start all
+                }else{
+                    if(id != 0) started_nodes_.erase(id);
+                    else started_nodes_.clear(); // stop all
+                }
+            }
+        }else if((frame.id & ~ALL_NODES_MASK) == HEARTBEAT_ID){
+            int id = frame.id & ALL_NODES_MASK;
+            if(skipNode(id)) return;
+
+            if(frame.dlc > 0 && frame.data[0] ==  canopen::Node::Stopped) started_nodes_.erase(id);
+        }
+
+        // toggle sync if needed
+        if(started_nodes_.empty() && sync_running_){
+            sync_running_ = !bcm_.stopTX(sync_frames_.front());
+        }else if(!started_nodes_.empty() && !sync_running_){
+            sync_running_ = bcm_.startTX(boost::chrono::milliseconds(sync_ms_),sync_frames_.front(), sync_frames_.size(), &sync_frames_[0]);
+        }
+    }
+protected:
+    virtual void handleRead(LayerStatus &status, const LayerState &current_state) {
+        if(current_state > Init){
+        }
+    }
+    virtual void handleWrite(LayerStatus &status, const LayerState &current_state) {
+        if(current_state > Init){
+        }
+    }
+    virtual void handleDiag(LayerReport &report){
+        boost::mutex::scoped_lock lock(mutex_);
+        if(!sync_running_) report.warn("sync is not running");
+
+        report.add("sync_running", sync_running_);
+        report.add("known_nodes", join(known_nodes_, ", "));
+        report.add("started_nodes", join(started_nodes_, ", "));
+    }
+
+    virtual void handleInit(LayerStatus &status){
+        boost::mutex::scoped_lock lock(mutex_);
+        started_nodes_.clear();
+
+        if(!bcm_.init(device_)){
+            status.error("BCM_init failed");
+            return;
+        }
+        int sc = driver_->getInternalSocket();
+
+        struct can_filter filter[2];
+
+        filter[0].can_id   = NMT_ID;
+        filter[0].can_mask = CAN_SFF_MASK;
+        filter[1].can_id   = HEARTBEAT_ID;
+        filter[1].can_mask =  ~ALL_NODES_MASK;
+
+        if(setsockopt(sc, SOL_CAN_RAW, CAN_RAW_FILTER, &filter, sizeof(filter)) < 0){
+            status.warn("could not set filter");
+            return;
+        }
+
+        handler_ = driver_->createMsgListenerM(this, &BCMsync::handleFrame);
+    }
+    virtual void handleShutdown(LayerStatus &status){
+        boost::mutex::scoped_lock lock(mutex_);
+        handler_.reset();
+        bcm_.shutdown();
+    }
+
+    virtual void handleHalt(LayerStatus &status) {
+        boost::mutex::scoped_lock lock(mutex_);
+        if(sync_running_){
+            bcm_.stopTX(sync_frames_.front());
+            sync_running_ = false;
+            started_nodes_.clear();
+        }
+    }
+
+    virtual void handleRecover(LayerStatus &status){
+        handleShutdown(status);
+        handleInit(status);
+    }
+public:
+    static const uint32_t ALL_NODES_MASK = 127;
+    static const uint32_t HEARTBEAT_ID = 0x700;
+    static const uint32_t NMT_ID = 0x000;
+
+    BCMsync(const std::string &device, can::SocketCANDriverSharedPtr  driver, const SyncProperties &sync_properties)
+    : Layer(device + " SyncLayer"), device_(device), sync_running_(false), sync_ms_(sync_properties.period_ms_), driver_(driver) {
+        if(sync_properties.overflow_ == 0){
+            sync_frames_.resize(1);
+            sync_frames_[0] = can::Frame(sync_properties.header_,0);
+        }else{
+            sync_frames_.resize(sync_properties.overflow_);
+            for(int i = 0; i < sync_properties.overflow_; ++i){
+                sync_frames_[i] = can::Frame(sync_properties.header_,1);
+                sync_frames_[i].data[0] = i+1;
+            }
+        }
+    }
+    template <class T> void setMonitored(const T &other){
+        monitored_nodes_.clear();
+        monitored_nodes_.insert(other.begin(), other.end());
+    }
+    template <class T> void setIgnored(const T &other){
+        ignored_nodes_.clear();
+        ignored_nodes_.insert(other.begin(), other.end());
+    }
+};
+
+}
+#endif

Devices/Libraries/Ros/ros_canopen/canopen_master/include/canopen_master/can_layer.h

@@ -0,0 +1,115 @@
+#ifndef H_CAN_LAYER
+#define H_CAN_LAYER
+
+#include <socketcan_interface/threading.h>
+#include "layer.h"
+#include <socketcan_interface/string.h>
+
+namespace canopen{
+
+class CANLayer: public Layer{
+    boost::mutex mutex_;
+    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){
+        boost::mutex::scoped_lock lock(mutex_);
+        last_error_ = msg;
+        ROSCANOPEN_ERROR("canopen_master", "Received error frame: " << msg);
+    }
+    std::shared_ptr<boost::thread> thread_;
+
+public:
+    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){
+            if(!driver_->getState().isReady()) status.error("CAN not ready");
+        }
+    }
+    virtual void handleWrite(LayerStatus &status, const LayerState &current_state) {
+        if(current_state > Init){
+            if(!driver_->getState().isReady()) status.error("CAN not ready");
+        }
+    }
+
+    virtual void handleDiag(LayerReport &report){
+        can::State s = driver_->getState();
+        if(!s.isReady()){
+            report.error("CAN layer not ready");
+            report.add("driver_state", int(s.driver_state));
+        }
+        if(s.error_code){
+            report.add("socket_error", s.error_code);
+        }
+        if(s.internal_error != 0){
+            report.add("internal_error", int(s.internal_error));
+            std::string desc;
+            if(driver_->translateError(s.internal_error, desc)) report.add("internal_error_desc", desc);
+            std::stringstream sstr;
+            sstr << std::hex;
+            {
+                boost::mutex::scoped_lock lock(mutex_);
+                for(size_t i=0; i < last_error_.dlc; ++i){
+                    sstr << (unsigned int) last_error_.data[i] << " ";
+                }
+            }
+            report.add("can_error_frame", sstr.str());
+
+        }
+
+    }
+
+    virtual void handleInit(LayerStatus &status){
+	if(thread_){
+            status.warn("CAN thread already running");
+        } else if(!driver_->init(device_, loopback_, settings_)) {
+            status.error("CAN init failed");
+        } else {
+            can::StateWaiter waiter(driver_.get());
+
+            thread_.reset(new boost::thread(&can::DriverInterface::run, driver_));
+            error_listener_ = driver_->createMsgListenerM(can::ErrorHeader(),this, &CANLayer::handleFrame);
+
+	    if(!waiter.wait(can::State::ready, boost::posix_time::seconds(1))){
+		status.error("CAN init timed out");
+	    }
+        }
+	if(!driver_->getState().isReady()){
+	  status.error("CAN is not ready");
+	}
+    }
+    virtual void handleShutdown(LayerStatus &status){
+        can::StateWaiter waiter(driver_.get());
+        error_listener_.reset();
+        driver_->shutdown();
+        if(!waiter.wait(can::State::closed, boost::posix_time::seconds(1))){
+             status.warn("CAN shutdown timed out");
+        }
+        if(thread_){
+            thread_->interrupt();
+            thread_->join();
+            thread_.reset();
+        }
+    }
+
+    virtual void handleHalt(LayerStatus &status) { /* nothing to do */ }
+
+    virtual void handleRecover(LayerStatus &status){
+        if(!driver_->getState().isReady()){
+            handleShutdown(status);
+            handleInit(status);
+        }
+    }
+
+};
+} // namespace canopen
+
+#endif

Devices/Libraries/Ros/ros_canopen/canopen_master/include/canopen_master/canopen.h

@@ -0,0 +1,331 @@
+#ifndef H_CANOPEN
+#define H_CANOPEN
+
+#include <socketcan_interface/interface.h>
+#include <socketcan_interface/dispatcher.h>
+#include <socketcan_interface/reader.h>
+#include "exceptions.h"
+#include "layer.h"
+#include "objdict.h"
+#include "timer.h"
+#include <stdexcept>
+#include <boost/thread/condition_variable.hpp>
+#include <boost/chrono/system_clocks.hpp>
+#include <boost/lexical_cast.hpp>
+
+namespace canopen{
+
+typedef boost::chrono::high_resolution_clock::time_point time_point;
+typedef boost::chrono::high_resolution_clock::duration time_duration;
+inline time_point get_abs_time(const time_duration& timeout) { return boost::chrono::high_resolution_clock::now() + timeout; }
+inline time_point get_abs_time() { return boost::chrono::high_resolution_clock::now(); }
+
+
+template<typename T> struct FrameOverlay: public can::Frame{
+    T &data;
+    FrameOverlay(const Header &h) : can::Frame(h,sizeof(T)), data(* (T*) can::Frame::c_array()) {
+        can::Frame::data.fill(0);
+    }
+    FrameOverlay(const can::Frame &f) : can::Frame(f), data(* (T*) can::Frame::c_array()) { }
+};
+
+class SDOClient{
+
+    can::Header client_id;
+
+    boost::timed_mutex mutex;
+
+    can::BufferedReader reader_;
+    bool processFrame(const can::Frame & msg);
+
+    String buffer;
+    size_t offset;
+    size_t total;
+    bool done;
+    can::Frame last_msg;
+    const canopen::ObjectDict::Entry * current_entry;
+
+    void transmitAndWait(const canopen::ObjectDict::Entry &entry, const String &data, String *result);
+    void abort(uint32_t reason);
+
+    const can::CommInterfaceSharedPtr interface_;
+protected:
+    void read(const canopen::ObjectDict::Entry &entry, String &data);
+    void write(const canopen::ObjectDict::Entry &entry, const String &data);
+public:
+    const ObjectStorageSharedPtr storage_;
+
+    void init();
+
+    SDOClient(const can::CommInterfaceSharedPtr interface, const ObjectDictSharedPtr dict, uint8_t node_id)
+    : interface_(interface),
+      storage_(std::make_shared<ObjectStorage>(dict, node_id,
+                                               std::bind(&SDOClient::read, this, std::placeholders::_1, std::placeholders::_2),
+                                               std::bind(&SDOClient::write, this, std::placeholders::_1, std::placeholders::_2))
+              ),
+      reader_(false, 1)
+    {
+    }
+};
+
+class PDOMapper{
+    boost::mutex mutex_;
+
+    class Buffer{
+    public:
+        bool read(uint8_t* b, const size_t len);
+        void write(const uint8_t* b, const size_t len);
+        void read(const canopen::ObjectDict::Entry &entry, String &data);
+        void write(const canopen::ObjectDict::Entry &, const String &data);
+        void clean() { dirty = false; }
+        const size_t size;
+        Buffer(const size_t sz) : size(sz), dirty(false), empty(true), buffer(sz) {}
+
+    private:
+        boost::mutex mutex;
+        bool dirty;
+        bool empty;
+        std::vector<char> buffer;
+    };
+    typedef std::shared_ptr<Buffer> BufferSharedPtr;
+
+    class PDO {
+    protected:
+        void parse_and_set_mapping(const ObjectStorageSharedPtr &storage, const uint16_t &com_index, const uint16_t &map_index, const bool &read, const bool &write);
+        can::Frame frame;
+        uint8_t transmission_type;
+        std::vector<BufferSharedPtr>buffers;
+    };
+
+    struct TPDO: public PDO{
+        typedef std::shared_ptr<TPDO> TPDOSharedPtr;
+        void sync();
+        static TPDOSharedPtr create(const can::CommInterfaceSharedPtr interface, const ObjectStorageSharedPtr &storage, const uint16_t &com_index, const uint16_t &map_index){
+            TPDOSharedPtr tpdo(new TPDO(interface));
+            if(!tpdo->init(storage, com_index, map_index))
+                tpdo.reset();
+            return tpdo;
+        }
+    private:
+        TPDO(const can::CommInterfaceSharedPtr interface) : interface_(interface){}
+        bool init(const ObjectStorageSharedPtr &storage, const uint16_t &com_index, const uint16_t &map_index);
+        const can::CommInterfaceSharedPtr interface_;
+        boost::mutex mutex;
+    };
+
+    struct RPDO : public PDO{
+        void sync(LayerStatus &status);
+        typedef std::shared_ptr<RPDO> RPDOSharedPtr;
+        static RPDOSharedPtr create(const can::CommInterfaceSharedPtr interface, const ObjectStorageSharedPtr &storage, const uint16_t &com_index, const uint16_t &map_index){
+            RPDOSharedPtr rpdo(new RPDO(interface));
+            if(!rpdo->init(storage, com_index, map_index))
+                rpdo.reset();
+            return rpdo;
+        }
+    private:
+        bool init(const ObjectStorageSharedPtr &storage, const uint16_t &com_index, const uint16_t &map_index);
+        RPDO(const can::CommInterfaceSharedPtr interface) : interface_(interface), timeout(-1) {}
+        boost::mutex mutex;
+        const can::CommInterfaceSharedPtr interface_;
+
+        can::FrameListenerConstSharedPtr listener_;
+        void handleFrame(const can::Frame & msg);
+        int timeout;
+    };
+
+    std::unordered_set<RPDO::RPDOSharedPtr> rpdos_;
+    std::unordered_set<TPDO::TPDOSharedPtr> tpdos_;
+
+    const can::CommInterfaceSharedPtr interface_;
+
+public:
+    PDOMapper(const can::CommInterfaceSharedPtr interface);
+    void read(LayerStatus &status);
+    bool write();
+    bool init(const ObjectStorageSharedPtr storage, LayerStatus &status);
+};
+
+class EMCYHandler : public Layer {
+    std::atomic<bool> has_error_;
+    ObjectStorage::Entry<uint8_t> error_register_;
+    ObjectStorage::Entry<uint8_t> num_errors_;
+    can::FrameListenerConstSharedPtr emcy_listener_;
+    void handleEMCY(const can::Frame & msg);
+    const ObjectStorageSharedPtr storage_;
+
+    virtual void handleDiag(LayerReport &report);
+
+    virtual void handleInit(LayerStatus &status);
+    virtual void handleRecover(LayerStatus &status);
+    virtual void handleRead(LayerStatus &status, const LayerState &current_state);
+    virtual void handleWrite(LayerStatus &status, const LayerState &current_state);
+    virtual void handleHalt(LayerStatus &status);
+    virtual void handleShutdown(LayerStatus &status);
+
+public:
+    EMCYHandler(const can::CommInterfaceSharedPtr interface, const ObjectStorageSharedPtr storage);
+    void resetErrors(LayerStatus &status);
+};
+
+struct SyncProperties{
+    const can::Header header_;
+    const uint16_t period_ms_;
+    const uint8_t overflow_;
+    SyncProperties(const can::Header &h, const uint16_t  &p, const uint8_t &o) : header_(h), period_ms_(p), overflow_(o) {}
+    bool operator==(const SyncProperties &p) const { return p.header_.key() == header_.key() && p.overflow_ == overflow_ && p.period_ms_ == period_ms_; }
+
+};
+
+class SyncCounter {
+public:
+    const SyncProperties properties;
+    SyncCounter(const SyncProperties &p) : properties(p) {}
+    virtual void addNode(void * const ptr)  = 0;
+    virtual  void removeNode(void * const ptr) = 0;
+    virtual ~SyncCounter() {}
+};
+typedef std::shared_ptr<SyncCounter> SyncCounterSharedPtr;
+
+class Node : public Layer{
+public:
+    enum State{
+        Unknown = 255, BootUp = 0, Stopped = 4, Operational = 5 , PreOperational = 127
+    };
+    const uint8_t node_id_;
+    Node(const can::CommInterfaceSharedPtr interface, const ObjectDictSharedPtr dict, uint8_t node_id, const SyncCounterSharedPtr sync = SyncCounterSharedPtr());
+
+    const State getState();
+    void enterState(const State &s);
+
+    const ObjectStorageSharedPtr getStorage() { return sdo_.storage_; }
+
+    bool start();
+    bool stop();
+    bool reset();
+    bool reset_com();
+    bool prepare();
+
+    using StateFunc = std::function<void(const State&)>;
+    using StateDelegate [[deprecated("use StateFunc instead")]] = can::DelegateHelper<StateFunc>;
+
+    typedef can::Listener<const StateFunc, const State&> StateListener;
+    typedef StateListener::ListenerConstSharedPtr StateListenerConstSharedPtr;
+
+    StateListenerConstSharedPtr addStateListener(const StateFunc & s){
+        return state_dispatcher_.createListener(s);
+    }
+
+    template<typename T> T get(const ObjectDict::Key& k){
+        return getStorage()->entry<T>(k).get();
+    }
+
+private:
+    virtual void handleDiag(LayerReport &report);
+
+    virtual void handleInit(LayerStatus &status);
+    virtual void handleRecover(LayerStatus &status);
+    virtual void handleRead(LayerStatus &status, const LayerState &current_state);
+    virtual void handleWrite(LayerStatus &status, const LayerState &current_state);
+    virtual void handleHalt(LayerStatus &status);
+    virtual void handleShutdown(LayerStatus &status);
+
+    template<typename T> int wait_for(const State &s, const T &timeout);
+
+    boost::timed_mutex mutex;
+    boost::mutex cond_mutex;
+    boost::condition_variable cond;
+
+    const can::CommInterfaceSharedPtr interface_;
+    const SyncCounterSharedPtr sync_;
+    can::FrameListenerConstSharedPtr nmt_listener_;
+
+    ObjectStorage::Entry<ObjectStorage::DataType<ObjectDict::DEFTYPE_UNSIGNED16>::type> heartbeat_;
+
+    can::SimpleDispatcher<StateListener> state_dispatcher_;
+
+    void handleNMT(const can::Frame & msg);
+    void switchState(const uint8_t &s);
+
+    State state_;
+    SDOClient sdo_;
+    PDOMapper pdo_;
+
+    boost::chrono::high_resolution_clock::time_point heartbeat_timeout_;
+    uint16_t getHeartbeatInterval() { return heartbeat_.valid()?heartbeat_.get_cached() : 0; }
+    void setHeartbeatInterval() { if(heartbeat_.valid()) heartbeat_.set(heartbeat_.desc().value().get<uint16_t>()); }
+    bool checkHeartbeat();
+};
+typedef std::shared_ptr<Node> NodeSharedPtr;
+
+template<typename T> class Chain{
+public:
+    typedef std::shared_ptr<T> MemberSharedPtr;
+    void call(void (T::*func)(void)){
+        typename std::vector<MemberSharedPtr>::iterator it = elements.begin();
+        while(it != elements.end()){
+            ((**it).*func)();
+            ++it;
+        }
+    }
+    template<typename V> void call(void (T::*func)(const V&), const std::vector<V> &vs){
+        typename std::vector<MemberSharedPtr>::iterator it = elements.begin();
+        typename std::vector<V>::const_iterator it_v = vs.begin();
+        while(it_v != vs.end() &&  it != elements.end()){
+            ((**it).*func)(*it_v);
+            ++it; ++it_v;
+        }
+    }
+    template<typename V> void call(void (T::*func)(V&), std::vector<V> &vs){
+        vs.resize(elements.size());
+
+        typename std::vector<MemberSharedPtr>::iterator it = elements.begin();
+        typename std::vector<V>::iterator it_v = vs.begin();
+        while(it_v != vs.end() &&  it != elements.end()){
+            ((**it).*func)(*it_v);
+            ++it; ++it_v;
+        }
+    }
+    void add(MemberSharedPtr t){
+        elements.push_back(t);
+    }
+protected:
+    std::vector<MemberSharedPtr> elements;
+};
+
+template<typename T> class NodeChain: public Chain<T>{
+public:
+    const std::vector<typename Chain<T>::MemberSharedPtr>& getElements() { return Chain<T>::elements; }
+    void start() { this->call(&T::start); }
+    void stop() { this->call(&T::stop); }
+    void reset() { this->call(&T::reset); }
+    void reset_com() { this->call(&T::reset_com); }
+    void prepare() { this->call(&T::prepare); }
+};
+
+class SyncLayer: public Layer, public SyncCounter{
+public:
+    SyncLayer(const SyncProperties &p) : Layer("Sync layer"), SyncCounter(p) {}
+};
+typedef std::shared_ptr<SyncLayer> SyncLayerSharedPtr;
+
+class Master{
+    Master(const Master&) = delete; // prevent copies
+    Master& operator=(const Master&) = delete;
+public:
+    Master() = default;
+    virtual SyncLayerSharedPtr getSync(const SyncProperties &properties) = 0;
+    virtual ~Master() {}
+
+    typedef std::shared_ptr<Master> MasterSharedPtr;
+    class Allocator {
+    public:
+        virtual MasterSharedPtr allocate(const std::string &name, can::CommInterfaceSharedPtr interface) = 0;
+        virtual ~Allocator() {}
+    };
+};
+typedef Master::MasterSharedPtr MasterSharedPtr;
+
+using can::Settings;
+
+} // canopen
+#endif // !H_CANOPEN

Devices/Libraries/Ros/ros_canopen/canopen_master/include/canopen_master/exceptions.h

@@ -0,0 +1,33 @@
+#ifndef H_EXCEPTIONS
+#define H_EXCEPTIONS
+
+#include <exception>
+#include <boost/exception/all.hpp>
+#include <boost/format.hpp>
+
+namespace canopen{
+
+class Exception : public std::runtime_error {
+public:
+    Exception(const std::string &w) : std::runtime_error(w) {}
+};
+
+class PointerInvalid : public Exception{
+public:
+    PointerInvalid(const std::string &w) : Exception("Pointer invalid") {}
+};
+
+class ParseException : public Exception{
+public:
+    ParseException(const std::string &w) : Exception(w) {}
+};
+
+class TimeoutException : public Exception{
+public:
+    TimeoutException(const std::string &w) : Exception(w) {}
+};
+
+
+} // canopen
+
+#endif // !H_EXCEPTIONS

Devices/Libraries/Ros/ros_canopen/canopen_master/include/canopen_master/layer.h

@@ -0,0 +1,260 @@
+#ifndef H_CANOPEN_LAYER
+#define H_CANOPEN_LAYER
+
+#include <vector>
+#include <memory>
+#include <boost/thread/shared_mutex.hpp>
+#include <atomic>
+#include <boost/exception/diagnostic_information.hpp>
+
+namespace canopen{
+
+class LayerStatus{
+    mutable boost::mutex write_mutex_;
+    enum State{
+        OK = 0, WARN = 1, ERROR= 2, STALE = 3, UNBOUNDED = 3
+    };
+    std::atomic<State> state;
+    std::string reason_;
+
+    virtual void set(const State &s, const std::string &r){
+        boost::mutex::scoped_lock lock(write_mutex_);
+        if(s > state) state = s;
+        if(!r.empty()){
+            if(reason_.empty())  reason_ = r;
+            else reason_ += "; " + r;
+        }
+    }
+public:
+    struct Ok { static const State state = OK; private: Ok();};
+    struct Warn { static const State state = WARN; private: Warn(); };
+    struct Error { static const State state = ERROR; private: Error(); };
+    struct Stale { static const State state = STALE; private: Stale(); };
+    struct Unbounded { static const State state = UNBOUNDED; private: Unbounded(); };
+
+    template<typename T> bool bounded() const{ return state <= T::state; }
+    template<typename T> bool equals() const{ return state == T::state; }
+
+    LayerStatus() : state(OK) {}
+
+    int get() const { return state; }
+
+    const std::string reason() const { boost::mutex::scoped_lock lock(write_mutex_); return reason_; }
+
+    const void warn(const std::string & r) { set(WARN, r); }
+    const void error(const std::string & r) { set(ERROR, r); }
+    const void stale(const std::string & r) { set(STALE, r); }
+};
+class LayerReport : public LayerStatus {
+    std::vector<std::pair<std::string, std::string> > values_;
+public:
+    const std::vector<std::pair<std::string, std::string> > &values() const { return values_; }
+    template<typename T> void add(const std::string &key, const T &value) {
+        std::stringstream str;
+        str << value;
+        values_.push_back(std::make_pair(key,str.str()));
+    }
+};
+
+#define CATCH_LAYER_HANDLER_EXCEPTIONS(command, status)                           \
+    try{ command; }                                                             \
+    catch(std::exception &e) {status.error(boost::diagnostic_information(e)); }
+
+class Layer{
+public:
+    enum LayerState{
+        Off,
+        Init,
+        Shutdown,
+        Error,
+        Halt,
+        Recover,
+        Ready
+    };
+
+    const std::string name;
+
+    void read(LayerStatus &status) {
+        if(state > Off) CATCH_LAYER_HANDLER_EXCEPTIONS(handleRead(status, state), status);
+    }
+    void write(LayerStatus &status) {
+        if(state > Off) CATCH_LAYER_HANDLER_EXCEPTIONS(handleWrite(status, state), status);
+    }
+    void diag(LayerReport &report) {
+        if(state > Shutdown) CATCH_LAYER_HANDLER_EXCEPTIONS(handleDiag(report), report);
+    }
+    void init(LayerStatus &status) {
+        if(state == Off){
+            if(status.bounded<LayerStatus::Warn>()){
+                state = Init;
+                CATCH_LAYER_HANDLER_EXCEPTIONS(handleInit(status), status);
+            }
+            if(!status.bounded<LayerStatus::Warn>()) shutdown(status);
+            else state = Ready;
+        }
+    }
+    void shutdown(LayerStatus &status) {
+        if(state != Off){
+            state = Shutdown;
+            CATCH_LAYER_HANDLER_EXCEPTIONS(handleShutdown(status), status);
+            state = Off;
+        }
+    }
+    void halt(LayerStatus &status) {
+        if(state > Halt){
+            state = Halt;
+            CATCH_LAYER_HANDLER_EXCEPTIONS(handleHalt(status), status);
+            state = Error;
+        }
+    }
+    void recover(LayerStatus &status) {
+        if(state == Error){
+            if(status.bounded<LayerStatus::Warn>()){
+                state = Recover;
+                CATCH_LAYER_HANDLER_EXCEPTIONS(handleRecover(status), status);
+            }
+            if(!status.bounded<LayerStatus::Warn>()){
+                halt(status);
+            }else{
+                state = Ready;
+            }
+        }
+
+    }
+
+    LayerState getLayerState() { return state; }
+
+    Layer(const std::string &n) : name(n), state(Off) {}
+
+    virtual ~Layer() {}
+
+protected:
+    virtual void handleRead(LayerStatus &status, const LayerState &current_state)  = 0;
+    virtual void handleWrite(LayerStatus &status, const LayerState &current_state)  = 0;
+
+    virtual void handleDiag(LayerReport &report)  = 0;
+
+    virtual void handleInit(LayerStatus &status)  = 0;
+    virtual void handleShutdown(LayerStatus &status)  = 0;
+
+    virtual void handleHalt(LayerStatus &status)  = 0;
+    virtual void handleRecover(LayerStatus &status)  = 0;
+
+private:
+    std::atomic<LayerState> state;
+
+};
+
+template<typename T> class VectorHelper{
+public:
+    typedef std::shared_ptr<T> VectorMemberSharedPtr;
+protected:
+    typedef std::vector<VectorMemberSharedPtr> vector_type ;
+    template<typename Bound, typename Data, typename FuncType> typename vector_type::iterator call(FuncType func, Data &status){
+        boost::shared_lock<boost::shared_mutex> lock(mutex);
+        return call<Bound>(func, status, layers.begin(), layers.end());
+    }
+    template<typename Data, typename FuncType> typename vector_type::iterator call(FuncType func, Data &status){
+        boost::shared_lock<boost::shared_mutex> lock(mutex);
+        return call<LayerStatus::Unbounded>(func, status, layers.begin(), layers.end());
+    }
+    template<typename Bound, typename Data, typename FuncType> typename vector_type::reverse_iterator call_rev(FuncType func, Data &status){
+        boost::shared_lock<boost::shared_mutex> lock(mutex);
+        return call<Bound>(func, status, layers.rbegin(), layers.rend());
+    }
+    template<typename Data, typename FuncType> typename vector_type::reverse_iterator call_rev(FuncType func, Data &status){
+        boost::shared_lock<boost::shared_mutex> lock(mutex);
+        return call<LayerStatus::Unbounded>(func, status, layers.rbegin(), layers.rend());
+    }
+    void destroy() { boost::unique_lock<boost::shared_mutex> lock(mutex); layers.clear(); }
+
+public:
+    virtual void add(const VectorMemberSharedPtr &l) { boost::unique_lock<boost::shared_mutex> lock(mutex); layers.push_back(l); }
+
+    template<typename Bound, typename Data, typename FuncType> bool callFunc(FuncType func, Data &status){
+        boost::shared_lock<boost::shared_mutex> lock(mutex);
+        return call<Bound>(func, status, layers.begin(), layers.end()) == layers.end();
+    }
+private:
+  vector_type layers;
+  boost::shared_mutex mutex;
+
+  template<typename Bound, typename Iterator, typename Data, typename FuncType> Iterator call(FuncType func, Data &status, const Iterator &begin, const Iterator &end){
+      bool okay_on_start = status.template bounded<Bound>();
+
+      for(Iterator it = begin; it != end; ++it){
+          ((**it).*func)(status);
+          if(okay_on_start && !status.template bounded<Bound>()){
+              return it;
+          }
+      }
+      return end;
+  }
+  template<typename Iterator, typename Data, typename FuncType> Iterator call(FuncType func, Data &status, const Iterator &begin, const Iterator &end){
+      return call<LayerStatus::Unbounded, Iterator, Data>(func, status, begin, end);
+  }
+};
+
+template<typename T=Layer> class LayerGroup : public Layer, public VectorHelper<T> {
+protected:
+    template<typename Data, typename FuncType, typename FailType> void call_or_fail(FuncType func, FailType fail, Data &status){
+        this->template call(func, status);
+        if(!status.template bounded<LayerStatus::Warn>()){
+            this->template call(fail, status);
+            (this->*fail)(status);
+        }
+    }
+    template<typename Data, typename FuncType, typename FailType> void call_or_fail_rev(FuncType func, FailType fail, Data &status){
+        this->template call_rev(func, status);
+        if(!status.template bounded<LayerStatus::Warn>()){
+            this->template call_rev(fail, status);
+            (this->*fail)(status);
+        }
+    }
+
+    virtual void handleRead(LayerStatus &status, const LayerState &current_state) {
+        this->template call_or_fail(&Layer::read, &Layer::halt, status);
+    }
+    virtual void handleWrite(LayerStatus &status, const LayerState &current_state) {
+        this->template call_or_fail(&Layer::write, &Layer::halt, status);
+    }
+
+    virtual void handleDiag(LayerReport &report) { this->template call(&Layer::diag, report); }
+
+    virtual void handleInit(LayerStatus &status) { this->template call<LayerStatus::Warn>(&Layer::init, status); }
+    virtual void handleShutdown(LayerStatus &status) { this->template call(&Layer::shutdown, status); }
+
+    virtual void handleHalt(LayerStatus &status) {  this->template call(&Layer::halt, status); }
+    virtual void handleRecover(LayerStatus &status) { this->template call<LayerStatus::Warn>(&Layer::recover, status); }
+public:
+    LayerGroup(const std::string &n) : Layer(n) {}
+};
+
+class LayerStack : public LayerGroup<>{
+
+protected:
+    virtual void handleWrite(LayerStatus &status, const LayerState &current_state) { call_or_fail_rev(&Layer::write, &Layer::halt, status);}
+    virtual void handleShutdown(LayerStatus &status) { call_rev(&Layer::shutdown, status); }
+public:
+    LayerStack(const std::string &n) : LayerGroup(n) {}
+};
+
+template<typename T> class LayerGroupNoDiag : public LayerGroup<T>{
+public:
+    LayerGroupNoDiag(const std::string &n) : LayerGroup<T>(n) {}
+    virtual void handleDiag(LayerReport &report) {}
+};
+
+template<typename T> class DiagGroup : public VectorHelper<T>{
+    typedef VectorHelper<T> V;
+public:
+    virtual void diag(LayerReport &report){
+        this->template call(&Layer::diag, report);
+    }
+};
+
+
+
+} // namespace canopen
+
+#endif

Devices/Libraries/Ros/ros_canopen/canopen_master/include/canopen_master/objdict.h

@@ -0,0 +1,587 @@
+#ifndef H_OBJDICT
+#define H_OBJDICT
+
+#include <list>
+#include <memory>
+#include <unordered_map>
+#include <unordered_set>
+
+#include <socketcan_interface/delegates.h>
+
+#include <boost/thread/mutex.hpp>
+#include <functional>
+#include <typeinfo>
+#include <vector>
+#include "exceptions.h"
+
+namespace canopen{
+
+class TypeGuard{
+    const std::type_info& (*get_type)();
+    size_t type_size;
+
+    template<typename T> class TypeInfo{
+    public:
+        static const std::type_info& id() { return typeid(T); }
+    };
+    TypeGuard(const std::type_info& (*ti)(), const size_t s): get_type(ti), type_size(s) {}
+public:
+
+    template<typename T> bool is_type() const {
+        return valid() && get_type() == typeid(T);
+    }
+
+    bool operator==(const TypeGuard &other) const {
+        return valid() && other.valid() && (get_type() == other.get_type());
+    }
+
+    TypeGuard(): get_type(0), type_size(0) {}
+    bool valid() const { return get_type != 0; }
+    size_t get_size() const { return type_size; }
+    template<typename T> static TypeGuard create() { return TypeGuard(TypeInfo<T>::id, sizeof(T)); }
+};
+
+class String: public std::vector<char>{
+public:
+    String() {}
+    String(const std::string &str) : std::vector<char>(str.begin(), str.end()) {}
+    operator const char * () const {
+        return &at(0);
+    }
+    operator const std::string () const {
+        return std::string(begin(), end());
+    }
+};
+
+class HoldAny{
+    String buffer;
+    TypeGuard type_guard;
+    bool empty;
+public:
+    HoldAny() : empty(true) {}
+
+    const TypeGuard& type() const{ return type_guard; }
+
+    template<typename T> HoldAny(const T &t) : type_guard(TypeGuard::create<T>()), empty(false){
+        buffer.resize(sizeof(T));
+        *(T*)&(buffer.front()) = t;
+    }
+    HoldAny(const std::string &t): type_guard(TypeGuard::create<std::string>()), empty(false){
+        if(!type_guard.is_type<std::string>()){
+            BOOST_THROW_EXCEPTION(std::bad_cast());
+        }
+        buffer = t;
+    }
+    HoldAny(const TypeGuard &t): type_guard(t), empty(true){ }
+
+    bool is_empty() const { return empty; }
+
+    const String& data() const {
+        if(empty){
+            BOOST_THROW_EXCEPTION(std::length_error("buffer empty"));
+        }
+        return buffer;
+    }
+
+    template<typename T> const T & get() const{
+        if(!type_guard.is_type<T>()){
+            BOOST_THROW_EXCEPTION(std::bad_cast());
+        }else if(empty){
+            BOOST_THROW_EXCEPTION(std::length_error("buffer empty"));
+        }
+        return *(T*)&(buffer.front());
+    }
+};
+
+template<> const String & HoldAny::get() const;
+
+struct DeviceInfo{
+    std::string vendor_name;
+    uint32_t vendor_number;
+    std::string product_name;
+    uint32_t product_number;
+    uint32_t revision_number;
+    std::string order_code;
+    std::unordered_set<uint32_t> baudrates;
+    bool simple_boot_up_master;
+    bool simple_boot_up_slave;
+    uint8_t granularity;
+    bool dynamic_channels_supported;
+    bool group_messaging;
+    uint16_t nr_of_rx_pdo;
+    uint16_t nr_of_tx_pdo;
+    bool lss_supported;
+    std::unordered_set<uint16_t> dummy_usage;
+};
+
+#define THROW_WITH_KEY(e,k) BOOST_THROW_EXCEPTION(boost::enable_error_info(e) << ObjectDict::key_info(k))
+
+class ObjectDict{
+protected:
+public:
+    class Key{
+        static size_t fromString(const std::string &str);
+    public:
+        const size_t hash;
+        Key(const uint16_t i) : hash((i<<16)| 0xFFFF) {}
+        Key(const uint16_t i, const uint8_t s): hash((i<<16)| s) {}
+        Key(const std::string &str): hash(fromString(str)) {}
+        bool hasSub() const { return (hash & 0xFFFF) != 0xFFFF; }
+        uint8_t sub_index() const { return hash & 0xFFFF; }
+        uint16_t index() const { return hash  >> 16;}
+        bool operator==(const Key &other) const { return hash == other.hash; }
+        operator std::string() const;
+    };
+    struct KeyHash {
+        std::size_t operator()(const Key& k) const { return k.hash; }
+    };
+
+    enum Code{
+        NULL_DATA = 0x00,
+        DOMAIN_DATA = 0x02,
+        DEFTYPE = 0x05,
+        DEFSTRUCT = 0x06,
+        VAR = 0x07,
+        ARRAY = 0x08,
+        RECORD = 0x09
+    };
+    enum DataTypes{
+        DEFTYPE_INTEGER8 = 0x0002,
+        DEFTYPE_INTEGER16 = 0x0003,
+        DEFTYPE_INTEGER32 = 0x0004,
+        DEFTYPE_UNSIGNED8 = 0x0005,
+        DEFTYPE_UNSIGNED16 = 0x0006,
+        DEFTYPE_UNSIGNED32 = 0x0007,
+        DEFTYPE_REAL32 = 0x0008,
+        DEFTYPE_VISIBLE_STRING = 0x0009,
+        DEFTYPE_OCTET_STRING = 0x000A,
+        DEFTYPE_UNICODE_STRING = 0x000B,
+        DEFTYPE_DOMAIN = 0x000F,
+        DEFTYPE_REAL64 = 0x0010,
+        DEFTYPE_INTEGER64 = 0x0015,
+        DEFTYPE_UNSIGNED64 = 0x001B
+    };
+    struct Entry{
+        Code obj_code;
+        uint16_t index;
+        uint8_t sub_index;
+        uint16_t data_type;
+        bool constant;
+        bool readable;
+        bool writable;
+        bool mappable;
+        std::string desc;
+        HoldAny def_val;
+        HoldAny init_val;
+
+        Entry() {}
+
+        Entry(const Code c, const uint16_t i,  const uint16_t t, const std::string & d, const bool r = true, const bool w = true, bool m = false, const HoldAny def = HoldAny(), const HoldAny init = HoldAny()):
+        obj_code(c), index(i), sub_index(0),data_type(t),readable(r), writable(w), mappable(m), desc(d), def_val(def), init_val(init) {}
+
+        Entry(const uint16_t i, const uint8_t s, const uint16_t t, const std::string & d, const bool r = true, const bool w = true, bool m = false, const HoldAny def = HoldAny(), const HoldAny init = HoldAny()):
+        obj_code(VAR), index(i), sub_index(s),data_type(t),readable(r), writable(w), mappable(m), desc(d), def_val(def), init_val(init) {}
+
+        operator Key() const { return Key(index, sub_index); }
+        const HoldAny & value() const { return !init_val.is_empty() ? init_val : def_val; }
+
+    };
+    typedef std::shared_ptr<const Entry> EntryConstSharedPtr;
+
+    const Entry& operator()(uint16_t i) const{
+        return *at(Key(i));
+    }
+    const Entry& operator()(uint16_t i, uint8_t s) const{
+        return *at(Key(i,s));
+    }
+    const EntryConstSharedPtr& get(const Key &k) const{
+        return at(k);
+    }
+    bool has(uint16_t i, uint8_t s) const{
+        return dict_.find(Key(i,s)) != dict_.end();
+    }
+    bool has(uint16_t i) const{
+        return dict_.find(Key(i)) != dict_.end();
+    }
+    bool has(const Key &k) const{
+        return dict_.find(k) != dict_.end();
+    }
+
+    bool insert(bool is_sub, EntryConstSharedPtr e){
+        return dict_.insert(std::make_pair(is_sub?Key(e->index,e->sub_index):Key(e->index),e)).second;
+    }
+
+    typedef std::unordered_map<Key, EntryConstSharedPtr, KeyHash>  ObjectDictMap;
+    bool iterate(ObjectDictMap::const_iterator &it) const;
+    typedef std::list<std::pair<std::string, std::string> > Overlay;
+    typedef std::shared_ptr<ObjectDict> ObjectDictSharedPtr;
+    static ObjectDictSharedPtr fromFile(const std::string &path, const Overlay &overlay = Overlay());
+    const DeviceInfo device_info;
+
+    ObjectDict(const DeviceInfo &info): device_info(info) {}
+    typedef boost::error_info<struct tag_objectdict_key, ObjectDict::Key> key_info;
+protected:
+    const EntryConstSharedPtr& at(const Key &key) const{
+        try{
+            return dict_.at(key);
+        }
+        catch(const std::out_of_range &e){
+            THROW_WITH_KEY(e, key);
+        }
+    }
+
+    ObjectDictMap dict_;
+};
+typedef ObjectDict::ObjectDictSharedPtr ObjectDictSharedPtr;
+typedef std::shared_ptr<const ObjectDict> ObjectDictConstSharedPtr;
+
+[[deprecated]]
+std::size_t hash_value(ObjectDict::Key const& k);
+
+template<typename T> class NodeIdOffset{
+    T offset;
+    T (*adder)(const uint8_t &, const T &);
+
+    static T add(const uint8_t &u, const T &t) {
+        return u+t;
+    }
+public:
+    NodeIdOffset(const T &t): offset(t), adder(add) {}
+
+    static const T apply(const HoldAny &val, const uint8_t &u){
+        if(!val.is_empty()){
+            if(TypeGuard::create<T>() ==  val.type() ){
+                return val.get<T>();
+            }else{
+                const NodeIdOffset<T> &no = val.get< NodeIdOffset<T> >();
+                return no.adder(u, no.offset);
+            }
+        }else{
+            BOOST_THROW_EXCEPTION(std::bad_cast());
+        }
+
+    }
+};
+
+template<typename T> std::ostream& operator<<(std::ostream& stream, const NodeIdOffset<T> &n) {
+    //stream << "Offset: " << n.apply(0);
+    return stream;
+ }
+std::ostream& operator<<(std::ostream& stream, const ObjectDict::Key &k);
+
+class AccessException : public Exception{
+public:
+    AccessException(const std::string &w) : Exception(w) {}
+};
+
+
+class ObjectStorage{
+public:
+    using ReadFunc = std::function<void(const ObjectDict::Entry&, String &)>;
+    using ReadDelegate [[deprecated("use ReadFunc instead")]] = can::DelegateHelper<ReadFunc>;
+
+    using WriteFunc = std::function<void(const ObjectDict::Entry&, const String &)>;
+    using WriteDelegate  [[deprecated("use WriteFunc instead")]] = can::DelegateHelper<WriteFunc>;
+
+    typedef std::shared_ptr<ObjectStorage> ObjectStorageSharedPtr;
+
+protected:
+    class Data {
+        Data(const Data&) = delete; // prevent copies
+        Data& operator=(const Data&) = delete;
+
+        boost::mutex mutex;
+        String buffer;
+        bool valid;
+
+        ReadFunc read_delegate;
+        WriteFunc write_delegate;
+
+        template <typename T> T & access(){
+            if(!valid){
+                THROW_WITH_KEY(std::length_error("buffer not valid"), key);
+            }
+            return *(T*)&(buffer.front());
+        }
+        template <typename T> T & allocate(){
+            if(!valid){
+                buffer.resize(sizeof(T));
+                valid = true;
+            }
+            return access<T>();
+        }
+    public:
+        const TypeGuard type_guard;
+        const ObjectDict::EntryConstSharedPtr entry;
+        const ObjectDict::Key key;
+        size_t size() { boost::mutex::scoped_lock lock(mutex); return buffer.size(); }
+
+        template<typename T> Data(const ObjectDict::Key &k, const ObjectDict::EntryConstSharedPtr &e, const T &val, const ReadFunc &r, const WriteFunc &w)
+        : valid(false), read_delegate(r), write_delegate(w), type_guard(TypeGuard::create<T>()), entry(e), key(k){
+            assert(r);
+            assert(w);
+            assert(e);
+            allocate<T>() = val;
+        }
+        Data(const ObjectDict::Key &k, const ObjectDict::EntryConstSharedPtr &e, const TypeGuard &t, const ReadFunc &r, const WriteFunc &w)
+        : valid(false), read_delegate(r), write_delegate(w), type_guard(t), entry(e), key(k){
+            assert(r);
+            assert(w);
+            assert(e);
+            assert(t.valid());
+            buffer.resize(t.get_size());
+        }
+        void set_delegates(const ReadFunc &r, const WriteFunc &w){
+            boost::mutex::scoped_lock lock(mutex);
+            if(r) read_delegate = r;
+            if(w) write_delegate = w;
+        }
+        template<typename T> const T get(bool cached) {
+            boost::mutex::scoped_lock lock(mutex);
+
+            if(!entry->readable){
+                THROW_WITH_KEY(AccessException("no read access"), key);
+
+            }
+
+            if(entry->constant) cached = true;
+
+            if(!valid || !cached){
+                allocate<T>();
+                read_delegate(*entry, buffer);
+            }
+            return access<T>();
+        }
+        template<typename T>  void set(const T &val) {
+            boost::mutex::scoped_lock lock(mutex);
+
+            if(!entry->writable){
+                if(access<T>() != val){
+                    THROW_WITH_KEY(AccessException("no write access"), key);
+                }
+            }else{
+                allocate<T>() = val;
+                write_delegate(*entry, buffer);
+            }
+        }
+        template<typename T>  void set_cached(const T &val) {
+            boost::mutex::scoped_lock lock(mutex);
+            if(!valid || val != access<T>() ){
+                if(!entry->writable){
+                    THROW_WITH_KEY(AccessException("no write access and not cached"), key);
+                }else{
+                    allocate<T>() = val;
+                    write_delegate(*entry, buffer);
+                }
+            }
+        }
+        void init();
+        void reset();
+        void force_write();
+
+    };
+    typedef std::shared_ptr<Data> DataSharedPtr;
+public:
+    template<const uint16_t dt> struct DataType{
+        typedef void type;
+    };
+
+    template<typename T> class Entry{
+        DataSharedPtr data;
+    public:
+        typedef T type;
+        bool valid() const { return data != 0; }
+        const T get() {
+            if(!data) BOOST_THROW_EXCEPTION( PointerInvalid("ObjectStorage::Entry::get()") );
+
+            return data->get<T>(false);
+        }
+        bool get(T & val){
+            try{
+                val = get();
+                return true;
+            }catch(...){
+                return false;
+            }
+        }
+        const T get_cached() {
+            if(!data) BOOST_THROW_EXCEPTION( PointerInvalid("ObjectStorage::Entry::get_cached()") );
+
+            return data->get<T>(true);
+        }
+        bool get_cached(T & val){
+            try{
+                val = get_cached();
+                return true;
+            }catch(...){
+                return false;
+            }
+        }
+        void set(const T &val) {
+            if(!data) BOOST_THROW_EXCEPTION( PointerInvalid("ObjectStorage::Entry::set(val)") );
+            data->set(val);
+        }
+        bool set_cached(const T &val) {
+            if(!data) return false;
+            try{
+	            data->set_cached(val);
+				return true;
+            }catch(...){
+                return false;
+            }
+        }
+
+        Entry() {}
+        Entry(DataSharedPtr &d)
+        : data(d){
+            assert(data);
+        }
+        Entry(ObjectStorageSharedPtr storage, uint16_t index)
+        : data(storage->entry<type>(index).data) {
+            assert(data);
+        }
+        Entry(ObjectStorageSharedPtr storage, uint16_t index, uint8_t sub_index)
+        : data(storage->entry<type>(index, sub_index).data) {
+            assert(data);
+        }
+        Entry(ObjectStorageSharedPtr storage, const ObjectDict::Key &k)
+        : data(storage->entry<type>(k).data) {
+            assert(data);
+        }
+        const ObjectDict::Entry & desc() const{
+            return *(data->entry);
+        }
+    };
+
+    void reset();
+
+protected:
+    typedef std::unordered_map<ObjectDict::Key, DataSharedPtr, ObjectDict::KeyHash> ObjectStorageMap;
+    ObjectStorageMap storage_;
+    boost::mutex mutex_;
+
+    void init_nolock(const ObjectDict::Key &key, const ObjectDict::EntryConstSharedPtr &entry);
+
+    ReadFunc read_delegate_;
+    WriteFunc write_delegate_;
+    size_t map(const ObjectDict::EntryConstSharedPtr &e, const ObjectDict::Key &key, const ReadFunc & read_delegate, const WriteFunc & write_delegate);
+public:
+    template<typename T> Entry<T> entry(const ObjectDict::Key &key){
+        boost::mutex::scoped_lock lock(mutex_);
+
+        ObjectStorageMap::iterator it = storage_.find(key);
+
+        if(it == storage_.end()){
+            const ObjectDict::EntryConstSharedPtr e = dict_->get(key);
+
+            DataSharedPtr data;
+            TypeGuard type = TypeGuard::create<T>();
+
+            if(!e->def_val.is_empty()){
+                T val = NodeIdOffset<T>::apply(e->def_val, node_id_);
+                data = std::make_shared<Data>(key, e,val, read_delegate_, write_delegate_);
+            }else{
+                if(!e->def_val.type().valid() ||  e->def_val.type() == type) {
+                    data = std::make_shared<Data>(key,e,type, read_delegate_, write_delegate_);
+                }else{
+                    THROW_WITH_KEY(std::bad_cast(), key);
+                }
+            }
+
+            std::pair<ObjectStorageMap::iterator, bool>  ok = storage_.insert(std::make_pair(key, data));
+            it = ok.first;
+        }
+
+        if(!it->second->type_guard.is_type<T>()){
+            THROW_WITH_KEY(std::bad_cast(), key);
+        }
+        return Entry<T>(it->second);
+    }
+
+    size_t map(uint16_t index, uint8_t sub_index, const ReadFunc & read_delegate, const WriteFunc & write_delegate);
+
+    template<typename T> Entry<T> entry(uint16_t index){
+        return entry<T>(ObjectDict::Key(index));
+    }
+    template<typename T> Entry<T> entry(uint16_t index, uint8_t sub_index){
+        return entry<T>(ObjectDict::Key(index,sub_index));
+    }
+
+    template<typename T> void entry(Entry<T> &e, uint16_t index){ // TODO: migrate to bool
+        e = entry<T>(ObjectDict::Key(index));
+    }
+    template<typename T> void entry(Entry<T> &e, uint16_t index, uint8_t sub_index){  // TODO: migrate to bool
+        e = entry<T>(ObjectDict::Key(index,sub_index));
+    }
+    template<typename T> bool entry(Entry<T> &e, const ObjectDict::Key &k){
+        try{
+            e = entry<T>(k);
+            return true;
+        }catch(...){
+            return false;
+        }
+    }
+    typedef std::function<std::string()> ReadStringFuncType;
+    ReadStringFuncType getStringReader(const ObjectDict::Key &key, bool cached = false);
+    typedef std::function<void(const std::string &)>  WriteStringFuncType;
+    WriteStringFuncType getStringWriter(const ObjectDict::Key &key, bool cached = false);
+
+    const ObjectDictConstSharedPtr dict_;
+    const uint8_t node_id_;
+
+    ObjectStorage(ObjectDictConstSharedPtr dict, uint8_t node_id, ReadFunc read_delegate, WriteFunc write_delegate);
+
+    void init(const ObjectDict::Key &key);
+    void init_all();
+};
+typedef ObjectStorage::ObjectStorageSharedPtr ObjectStorageSharedPtr;
+
+template<> String & ObjectStorage::Data::access();
+template<> String & ObjectStorage::Data::allocate();
+
+template<> struct ObjectStorage::DataType<ObjectDict::DEFTYPE_INTEGER8> { typedef int8_t type;};
+template<> struct ObjectStorage::DataType<ObjectDict::DEFTYPE_INTEGER16> { typedef int16_t type;};
+template<> struct ObjectStorage::DataType<ObjectDict::DEFTYPE_INTEGER32> { typedef int32_t type;};
+template<> struct ObjectStorage::DataType<ObjectDict::DEFTYPE_INTEGER64> { typedef int64_t type;};
+template<> struct ObjectStorage::DataType<ObjectDict::DEFTYPE_UNSIGNED8> { typedef uint8_t type;};
+
+template<> struct ObjectStorage::DataType<ObjectDict::DEFTYPE_UNSIGNED16> { typedef uint16_t type;};
+template<> struct ObjectStorage::DataType<ObjectDict::DEFTYPE_UNSIGNED32> { typedef uint32_t type;};
+template<> struct ObjectStorage::DataType<ObjectDict::DEFTYPE_UNSIGNED64> { typedef uint64_t type;};
+
+template<> struct ObjectStorage::DataType<ObjectDict::DEFTYPE_REAL32> { typedef float type;};
+template<> struct ObjectStorage::DataType<ObjectDict::DEFTYPE_REAL64> { typedef double type;};
+
+template<> struct ObjectStorage::DataType<ObjectDict::DEFTYPE_VISIBLE_STRING> { typedef String type;};
+template<> struct ObjectStorage::DataType<ObjectDict::DEFTYPE_OCTET_STRING> { typedef String type;};
+template<> struct ObjectStorage::DataType<ObjectDict::DEFTYPE_UNICODE_STRING> { typedef String type;};
+template<> struct ObjectStorage::DataType<ObjectDict::DEFTYPE_DOMAIN> { typedef String type;};
+
+template<typename T, typename R> static R *branch_type(const uint16_t data_type){
+    switch(ObjectDict::DataTypes(data_type)){
+        case ObjectDict::DEFTYPE_INTEGER8: return T::template func< ObjectDict::DEFTYPE_INTEGER8 >;
+        case ObjectDict::DEFTYPE_INTEGER16: return T::template func< ObjectDict::DEFTYPE_INTEGER16 >;
+        case ObjectDict::DEFTYPE_INTEGER32: return T::template func< ObjectDict::DEFTYPE_INTEGER32 >;
+        case ObjectDict::DEFTYPE_INTEGER64: return T::template func< ObjectDict::DEFTYPE_INTEGER64 >;
+
+        case ObjectDict::DEFTYPE_UNSIGNED8: return T::template func< ObjectDict::DEFTYPE_UNSIGNED8 >;
+        case ObjectDict::DEFTYPE_UNSIGNED16: return T::template func< ObjectDict::DEFTYPE_UNSIGNED16 >;
+        case ObjectDict::DEFTYPE_UNSIGNED32: return T::template func< ObjectDict::DEFTYPE_UNSIGNED32 >;
+        case ObjectDict::DEFTYPE_UNSIGNED64: return T::template func< ObjectDict::DEFTYPE_UNSIGNED64 >;
+
+        case ObjectDict::DEFTYPE_REAL32: return T::template func< ObjectDict::DEFTYPE_REAL32 >;
+        case ObjectDict::DEFTYPE_REAL64: return T::template func< ObjectDict::DEFTYPE_REAL64 >;
+
+        case ObjectDict::DEFTYPE_VISIBLE_STRING: return T::template func< ObjectDict::DEFTYPE_VISIBLE_STRING >;
+        case ObjectDict::DEFTYPE_OCTET_STRING: return T::template func< ObjectDict::DEFTYPE_OCTET_STRING >;
+        case ObjectDict::DEFTYPE_UNICODE_STRING: return T::template func< ObjectDict::DEFTYPE_UNICODE_STRING >;
+        case ObjectDict::DEFTYPE_DOMAIN: return T::template func< ObjectDict::DEFTYPE_DOMAIN >;
+
+        default:
+            throw std::bad_cast();
+    }
+}
+
+} // canopen
+
+#endif // !H_OBJDICT

Devices/Libraries/Ros/ros_canopen/canopen_master/include/canopen_master/timer.h

@@ -0,0 +1,75 @@
+#ifndef H_CANOPEN_TIMER
+#define H_CANOPEN_TIMER
+
+#include <functional>
+#include <memory>
+
+#include <boost/asio.hpp>
+#include <boost/thread/thread.hpp>
+#include <boost/asio/high_resolution_timer.hpp>
+
+#include <socketcan_interface/delegates.h>
+
+namespace canopen{
+
+class Timer{
+public:
+    using TimerFunc = std::function<bool(void)>;
+    using TimerDelegate [[deprecated("use TimerFunc instead")]] = can::DelegateHelper<TimerFunc>;
+
+    Timer():work(io), timer(io),thread(std::bind(
+        static_cast<size_t(boost::asio::io_service::*)(void)>(&boost::asio::io_service::run), &io))
+    {
+    }
+
+    void stop(){
+        boost::mutex::scoped_lock lock(mutex);
+        timer.cancel();
+    }
+    template<typename T> void start(const TimerFunc &del, const  T &dur, bool start_now = true){
+        boost::mutex::scoped_lock lock(mutex);
+        delegate = del;
+        period = boost::chrono::duration_cast<boost::chrono::high_resolution_clock::duration>(dur);
+        if(start_now){
+            timer.expires_from_now(period);
+            timer.async_wait(std::bind(&Timer::handler, this, std::placeholders::_1));
+        }
+    }
+    void restart(){
+        boost::mutex::scoped_lock lock(mutex);
+        timer.expires_from_now(period);
+        timer.async_wait(std::bind(&Timer::handler, this, std::placeholders::_1));
+    }
+    const  boost::chrono::high_resolution_clock::duration & getPeriod(){
+        boost::mutex::scoped_lock lock(mutex);
+        return period;
+    }
+    ~Timer(){
+        io.stop();
+        thread.join();
+    }
+
+private:
+    boost::asio::io_service io;
+    boost::asio::io_service::work work;
+    boost::asio::basic_waitable_timer<boost::chrono::high_resolution_clock> timer;
+    boost::chrono::high_resolution_clock::duration period;
+    boost::mutex mutex;
+    boost::thread thread;
+
+    TimerFunc delegate;
+    void handler(const boost::system::error_code& ec){
+        if(!ec){
+            boost::mutex::scoped_lock lock(mutex);
+            if(delegate && delegate()){
+                timer.expires_at(timer.expires_at() + period);
+                timer.async_wait(std::bind(&Timer::handler, this, std::placeholders::_1));
+            }
+
+        }
+    }
+};
+
+}
+
+#endif

Devices/Libraries/Ros/ros_canopen/canopen_master/master_plugin.xml

@@ -0,0 +1,8 @@
+<library path="lib/libcanopen_master_plugin">
+  <class type="canopen::SimpleMaster::Allocator" base_class_type="canopen::Master::Allocator">
+    <description>Allocator for SimpleMaster instances</description>
+  </class>
+  <class type="canopen::ExternalMaster::Allocator" base_class_type="canopen::Master::Allocator">
+    <description>Allocator for ExternalMaster instances</description>
+  </class>
+</library>

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

@@ -0,0 +1,30 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>canopen_master</name>
+  <version>0.8.5</version>
+  <description>CiA(r) CANopen 301 master implementation with support for interprocess master synchronisation.</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_master</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>
+
+  <depend>libboost-dev</depend>
+  <depend>libboost-atomic-dev</depend>
+  <depend>libboost-chrono-dev</depend>
+  <depend>libboost-thread-dev</depend>
+  <depend>class_loader</depend>
+  <depend>socketcan_interface</depend>
+
+  <test_depend>rosunit</test_depend>
+
+  <export>
+    <canopen_master plugin="${prefix}/master_plugin.xml" />
+  </export>
+</package>

Devices/Libraries/Ros/ros_canopen/canopen_master/src/bcm_sync.cpp

@@ -0,0 +1,68 @@
+#include <canopen_master/bcm_sync.h>
+#include <socketcan_interface/string.h>
+#include <iostream>
+
+int main(int argc, char** argv){
+
+    if(argc < 4){
+        std::cout << "Usage: " << argv[0] << " DEVICE PERIOD_MS HEADER [OVERFLOW] [+ID*] [-ID*] [--]" << std::endl;
+        return 1;
+    }
+
+    std::string can_device = argv[1];
+    int sync_ms = atoi(argv[2]);
+    can::Header header = can::toheader(argv[3]);
+
+    if(!header.isValid()){
+            std::cout << "header is invalid" << std::endl;
+            return 1;
+    }
+    int sync_overflow = 0;
+
+    int start = 4;
+    if(argc > start && argv[start][0] != '-' && argv[start][0] != '+'){
+        sync_overflow = atoi(argv[4]);
+        if(sync_overflow == 1 || sync_overflow < 0 || sync_overflow > 240){
+            std::cout << "sync overflow is invalid" << std::endl;
+            return 1;
+        }
+        ++start;
+    }
+
+    std::set<int> monitored, ignored;
+
+    for(; argc > start; ++start){
+        if(strncmp("--", argv[start], 2) == 0) break;
+        int id = atoi(argv[start]);
+
+        if(id > 0 && id < 128) monitored.insert(id);
+        else if (id < 0 && id > -128) ignored.insert(-id);
+        else{
+            std::cout << "ID is invalid: " << id  << std::endl;
+            return 1;
+        }
+    }
+
+    can::SocketCANDriverSharedPtr driver = std::make_shared<can::SocketCANDriver>();
+    if(!driver->init(can_device, false, can::NoSettings::create())){
+        std::cout << "Could not initialize CAN" << std::endl;
+        return 1;
+    }
+
+    canopen::SyncProperties sync_properties(header, sync_ms, sync_overflow);
+    canopen::BCMsync sync(can_device, driver, sync_properties);
+
+    sync.setMonitored(monitored);
+    sync.setIgnored(ignored);
+
+    canopen::LayerStatus status;
+    sync.init(status);
+    if(!status.bounded<canopen::LayerStatus::Warn>()){
+        std::cout << "Could not initialize sync" << std::endl;
+        return 1;
+    }
+
+    driver->run();
+
+    return 0;
+}

Devices/Libraries/Ros/ros_canopen/canopen_master/src/emcy.cpp

@@ -0,0 +1,148 @@
+#include <canopen_master/canopen.h>
+#include <socketcan_interface/string.h>
+
+using namespace canopen;
+
+#pragma pack(push) /* push current alignment to stack */
+#pragma pack(1) /* set alignment to 1 byte boundary */
+
+struct EMCYid{
+    uint32_t id:29;
+    uint32_t extended:1;
+    uint32_t :1;
+    uint32_t invalid:1;
+    EMCYid(uint32_t val){
+        *(uint32_t*) this = val;
+    }
+    can::Header header() {
+        return can::Header(id, extended, false, false);
+    }
+    const uint32_t get() const { return *(uint32_t*) this; }
+};
+
+struct EMCYfield{
+    uint32_t error_code:16;
+    uint32_t addition_info:16;
+    EMCYfield(uint32_t val){
+        *(uint32_t*) this = val;
+    }
+};
+
+struct EMCYmsg{
+    uint16_t error_code;
+    uint8_t error_register;
+    uint8_t manufacturer_specific_error_field[5];
+
+    struct Frame: public FrameOverlay<EMCYmsg>{
+        Frame(const can::Frame &f) : FrameOverlay(f){ }
+    };
+};
+
+#pragma pack(pop) /* pop previous alignment from stack */
+
+void EMCYHandler::handleEMCY(const can::Frame & msg){
+    EMCYmsg::Frame em(msg);
+    if (em.data.error_code == 0) {
+        ROSCANOPEN_INFO("canopen_master", "EMCY reset: " << msg);
+    } else {
+        ROSCANOPEN_ERROR("canopen_master", "EMCY received: " << msg);
+    }
+    has_error_ = (em.data.error_register & ~32) != 0;
+}
+
+EMCYHandler::EMCYHandler(const can::CommInterfaceSharedPtr interface, const ObjectStorageSharedPtr storage): Layer("EMCY handler"), storage_(storage), has_error_(true){
+    storage_->entry(error_register_, 0x1001);
+    try{
+        storage_->entry(num_errors_, 0x1003,0);
+    }
+    catch(...){
+       // pass, 1003 is optional
+    }
+    try{
+        EMCYid emcy_id(storage_->entry<uint32_t>(0x1014).get_cached());
+        emcy_listener_ = interface->createMsgListenerM(emcy_id.header(), this, &EMCYHandler::handleEMCY);
+
+
+    }
+    catch(...){
+       // pass, EMCY is optional
+    }
+}
+
+void EMCYHandler::handleRead(LayerStatus &status, const LayerState &current_state) {
+    if(current_state == Ready){
+        if(has_error_){
+            status.error("Node has emergency error");
+        }
+    }
+}
+void EMCYHandler::handleWrite(LayerStatus &status, const LayerState &current_state) {
+    // noithing to do
+}
+
+void EMCYHandler::handleDiag(LayerReport &report){
+    uint8_t error_register = 0;
+    if(!error_register_.get(error_register)){
+        report.error("Could not read error error_register");
+        return;
+    }
+
+    if(error_register){
+        if(error_register & 1){ // first bit should be set on all errors
+            report.error("Node has emergency error");
+        }else if(error_register & ~32){ // filter profile-specific bit
+            report.warn("Error register is not zero");
+        }
+        report.add("error_register", (uint32_t) error_register);
+
+        uint8_t num = num_errors_.valid() ? num_errors_.get() : 0;
+        std::stringstream buf;
+        for(size_t i = 0; i < num; ++i) {
+            if( i!= 0){
+                buf << ", ";
+            }
+            try{
+                ObjectStorage::Entry<uint32_t> error;
+                storage_->entry(error, 0x1003,i+1);
+                EMCYfield field(error.get());
+                buf << std::hex << field.error_code << "#" << field.addition_info;
+            }
+            catch (const std::out_of_range & e){
+                buf << "NOT_IN_DICT!";
+            }
+            catch (const TimeoutException & e){
+                buf << "LIST_UNDERFLOW!";
+                break;
+            }
+
+        }
+        report.add("errors", buf.str());
+
+    }
+}
+void EMCYHandler::handleInit(LayerStatus &status){
+    uint8_t error_register = 0;
+    if(!error_register_.get(error_register)){
+        status.error("Could not read error error_register");
+        return;
+    }else if(error_register & 1){
+        ROSCANOPEN_ERROR("canopen_master", "error register: " << int(error_register));
+        status.error("Node has emergency error");
+        return;
+    }
+
+    resetErrors(status);
+}
+void EMCYHandler::resetErrors(LayerStatus &status){
+    if(num_errors_.valid()) num_errors_.set(0);
+    has_error_ = false;
+}
+
+void EMCYHandler::handleRecover(LayerStatus &status){
+    handleInit(status);
+}
+void EMCYHandler::handleShutdown(LayerStatus &status){
+}
+void EMCYHandler::handleHalt(LayerStatus &status){
+    // do nothing
+}

Devices/Libraries/Ros/ros_canopen/canopen_master/src/master_plugin.cpp

@@ -0,0 +1,138 @@
+#include <class_loader/class_loader.hpp>
+#include <socketcan_interface/reader.h>
+#include <canopen_master/canopen.h>
+
+#include <set>
+
+namespace canopen {
+
+class ManagingSyncLayer: public SyncLayer {
+protected:
+    can::CommInterfaceSharedPtr interface_;
+    boost::chrono::milliseconds step_, half_step_;
+
+    std::set<void *> nodes_;
+    boost::mutex nodes_mutex_;
+    std::atomic<size_t> nodes_size_;
+
+    virtual void handleShutdown(LayerStatus &status) {
+    }
+
+    virtual void handleHalt(LayerStatus &status)  { /* nothing to do */ }
+    virtual void handleDiag(LayerReport &report)  { /* TODO */ }
+    virtual void handleRecover(LayerStatus &status)  { /* TODO */ }
+
+public:
+    ManagingSyncLayer(const SyncProperties &p, can::CommInterfaceSharedPtr interface)
+    : SyncLayer(p), interface_(interface), step_(p.period_ms_), half_step_(p.period_ms_/2), nodes_size_(0)
+    {
+    }
+
+    virtual void addNode(void * const ptr) {
+        boost::mutex::scoped_lock lock(nodes_mutex_);
+        nodes_.insert(ptr);
+        nodes_size_ = nodes_.size();
+    }
+    virtual void removeNode(void * const ptr)  {
+        boost::mutex::scoped_lock lock(nodes_mutex_);
+        nodes_.erase(ptr);
+        nodes_size_ = nodes_.size();
+    }
+};
+
+
+class SimpleSyncLayer: public ManagingSyncLayer {
+    time_point read_time_, write_time_;
+    can::Frame frame_;
+    uint8_t overflow_;
+
+    void resetCounter(){
+        frame_.data[0] = 1; // SYNC counter starts at 1
+    }
+    void tryUpdateCounter(){
+        if (frame_.dlc > 0) { // sync counter is used
+            if (frame_.data[0] >= overflow_) {
+                resetCounter();
+            }else{
+                ++frame_.data[0];
+            }
+        }
+    }
+protected:
+    virtual void handleRead(LayerStatus &status, const LayerState &current_state) {
+        if(current_state > Init){
+            boost::this_thread::sleep_until(read_time_);
+            write_time_ += step_;
+        }
+    }
+    virtual void handleWrite(LayerStatus &status, const LayerState &current_state) {
+        if(current_state > Init){
+            boost::this_thread::sleep_until(write_time_);
+            tryUpdateCounter();
+            if(nodes_size_){ //)
+                interface_->send(frame_);
+            }
+            read_time_ = get_abs_time(half_step_);
+        }
+    }
+
+    virtual void handleInit(LayerStatus &status){
+        write_time_ = get_abs_time(step_);
+        read_time_ = get_abs_time(half_step_);
+    }
+public:
+    SimpleSyncLayer(const SyncProperties &p, can::CommInterfaceSharedPtr interface)
+    : ManagingSyncLayer(p, interface), frame_(p.header_, 0), overflow_(p.overflow_) {
+        if(overflow_ == 1 || overflow_ > 240){
+            BOOST_THROW_EXCEPTION(Exception("SYNC counter overflow is invalid"));
+        }else if(overflow_ > 1){
+            frame_.dlc = 1;
+            resetCounter();
+        }
+    }
+};
+
+class ExternalSyncLayer: public ManagingSyncLayer {
+    can::BufferedReader reader_;
+protected:
+    virtual void handleRead(LayerStatus &status, const LayerState &current_state) {
+        can::Frame msg;
+        if(current_state > Init){
+            if(reader_.readUntil(&msg, get_abs_time(step_))){ // wait for sync
+                boost::this_thread::sleep_until(get_abs_time(half_step_)); // shift readout to middle of period
+            }
+        }
+    }
+    virtual void handleWrite(LayerStatus &status, const LayerState &current_state) {
+        // nothing to do here
+    }
+    virtual void handleInit(LayerStatus &status){
+        reader_.listen(interface_, properties.header_);
+    }
+public:
+    ExternalSyncLayer(const SyncProperties &p, can::CommInterfaceSharedPtr interface)
+    : ManagingSyncLayer(p, interface), reader_(true,1) {}
+};
+
+
+template<typename SyncType> class WrapMaster: public Master{
+    can::CommInterfaceSharedPtr interface_;
+public:
+    virtual SyncLayerSharedPtr getSync(const SyncProperties &properties){
+        return std::make_shared<SyncType>(properties, interface_);
+    }
+    WrapMaster(can::CommInterfaceSharedPtr interface) : interface_(interface)  {}
+
+    class Allocator : public Master::Allocator{
+    public:
+        virtual MasterSharedPtr allocate(const std::string &name,  can::CommInterfaceSharedPtr interface){
+            return std::make_shared<WrapMaster>(interface);
+        }
+    };
+};
+
+typedef WrapMaster<SimpleSyncLayer> SimpleMaster;
+typedef WrapMaster<ExternalSyncLayer> ExternalMaster;
+}
+CLASS_LOADER_REGISTER_CLASS(canopen::SimpleMaster::Allocator, canopen::Master::Allocator);
+CLASS_LOADER_REGISTER_CLASS(canopen::ExternalMaster::Allocator, canopen::Master::Allocator);

Devices/Libraries/Ros/ros_canopen/canopen_master/src/node.cpp

@@ -0,0 +1,218 @@
+#include <canopen_master/canopen.h>
+
+using namespace canopen;
+
+#pragma pack(push) /* push current alignment to stack */
+#pragma pack(1) /* set alignment to 1 byte boundary */
+
+
+struct NMTcommand{
+    enum Command{
+        Start = 1,
+        Stop = 2,
+        Prepare = 128,
+        Reset = 129,
+        Reset_Com = 130
+    };
+    uint8_t command;
+    uint8_t node_id;
+
+    struct Frame: public FrameOverlay<NMTcommand>{
+        Frame(uint8_t node_id, const Command &c) : FrameOverlay(can::Header()) {
+            data.command = c;
+            data.node_id = node_id;
+        }
+    };
+};
+
+#pragma pack(pop) /* pop previous alignment from stack */
+
+Node::Node(const can::CommInterfaceSharedPtr interface, const ObjectDictSharedPtr dict, uint8_t node_id, const SyncCounterSharedPtr sync)
+: Layer("Node 301"), node_id_(node_id), interface_(interface), sync_(sync) , state_(Unknown), sdo_(interface, dict, node_id), pdo_(interface){
+    try{
+        getStorage()->entry(heartbeat_, 0x1017);
+    }
+    catch(const std::out_of_range){
+    }
+}
+
+const Node::State Node::getState(){
+    boost::timed_mutex::scoped_lock lock(mutex); // TODO: timed lock?
+    return state_;
+}
+
+bool Node::reset_com(){
+    boost::timed_mutex::scoped_lock lock(mutex); // TODO: timed lock?
+    getStorage()->reset();
+    interface_->send(NMTcommand::Frame(node_id_, NMTcommand::Reset_Com));
+    if(wait_for(BootUp, boost::chrono::seconds(10)) != 1){
+        return false;
+    }
+    state_ = PreOperational;
+    setHeartbeatInterval();
+    return true;
+}
+bool Node::reset(){
+    boost::timed_mutex::scoped_lock lock(mutex); // TODO: timed lock?
+    getStorage()->reset();
+
+    interface_->send(NMTcommand::Frame(node_id_, NMTcommand::Reset));
+    if(wait_for(BootUp, boost::chrono::seconds(10)) != 1){
+        return false;
+    }
+    state_ = PreOperational;
+    setHeartbeatInterval();
+    return true;
+}
+
+bool Node::prepare(){
+    boost::timed_mutex::scoped_lock lock(mutex); // TODO: timed lock?
+    if(state_ == BootUp){
+        // ERROR
+    }
+    interface_->send(NMTcommand::Frame(node_id_, NMTcommand::Prepare));
+    return 0 != wait_for(PreOperational, boost::chrono::seconds(2));
+}
+bool Node::start(){
+    boost::timed_mutex::scoped_lock lock(mutex); // TODO: timed lock?
+    if(state_ == BootUp){
+        // ERROR
+    }
+    interface_->send(NMTcommand::Frame(node_id_, NMTcommand::Start));
+    return 0 != wait_for(Operational, boost::chrono::seconds(2));
+}
+bool Node::stop(){
+    boost::timed_mutex::scoped_lock lock(mutex); // TODO: timed lock?
+    if(sync_) sync_->removeNode(this);
+    if(state_ == BootUp){
+        // ERROR
+    }
+    interface_->send(NMTcommand::Frame(node_id_, NMTcommand::Stop));
+    return true;
+}
+
+void Node::switchState(const uint8_t &s){
+    bool changed = state_ != s;
+    switch(s){
+        case Operational:
+            if(changed && sync_) sync_->addNode(this);
+            break;
+        case BootUp:
+        case PreOperational:
+        case Stopped:
+            if(changed && sync_) sync_->removeNode(this);
+            break;
+        default:
+            //error
+            ;
+    }
+    if(changed){
+        state_ = (State) s;
+        state_dispatcher_.dispatch(state_);
+        cond.notify_one();
+    }
+}
+void Node::handleNMT(const can::Frame & msg){
+    boost::mutex::scoped_lock cond_lock(cond_mutex);
+    uint16_t interval = getHeartbeatInterval();
+    if(interval) heartbeat_timeout_ = get_abs_time(boost::chrono::milliseconds(3*interval));
+    assert(msg.dlc == 1);
+    switchState(msg.data[0]);
+}
+template<typename T> int Node::wait_for(const State &s, const T &timeout){
+    boost::mutex::scoped_lock cond_lock(cond_mutex);
+    time_point abs_time = get_abs_time(timeout);
+
+    while(s != state_) {
+        if(cond.wait_until(cond_lock,abs_time) == boost::cv_status::timeout)
+        {
+            break;
+        }
+    }
+    if( s!= state_){
+        if(getHeartbeatInterval() == 0){
+            switchState(s);
+            return -1;
+        }
+        return 0;
+    }
+    return 1;
+}
+bool Node::checkHeartbeat(){
+    if(getHeartbeatInterval() == 0) return true; //disabled
+    boost::mutex::scoped_lock cond_lock(cond_mutex);
+    return heartbeat_timeout_ >= boost::chrono::high_resolution_clock::now();
+}
+
+
+void Node::handleRead(LayerStatus &status, const LayerState &current_state) {
+    if(current_state > Init){
+        if(!checkHeartbeat()){
+            status.error("heartbeat problem");
+        } else if(getState() != Operational){
+            status.error("not operational");
+        } else{
+            pdo_.read(status);
+        }
+    }
+}
+void Node::handleWrite(LayerStatus &status, const LayerState &current_state) {
+    if(current_state > Init){
+        if(getState() != Operational)  status.error("not operational");
+        else if(! pdo_.write())  status.error("PDO write problem");
+    }
+}
+
+
+void Node::handleDiag(LayerReport &report){
+    State state = getState();
+    if(state != Operational){
+        report.error("Mode not operational");
+        report.add("Node state", (int)state);
+    }else if(!checkHeartbeat()){
+        report.error("Heartbeat timeout");
+    }
+}
+void Node::handleInit(LayerStatus &status){
+    nmt_listener_ = interface_->createMsgListenerM(can::MsgHeader(0x700 + node_id_), this, &Node::handleNMT);
+
+    sdo_.init();
+    try{
+        if(!reset_com()) BOOST_THROW_EXCEPTION( TimeoutException("reset_timeout") );
+    }
+    catch(const TimeoutException&){
+        status.error(boost::str(boost::format("could not reset node '%1%'") % (int)node_id_));
+        return;
+    }
+
+    if(!pdo_.init(getStorage(), status)){
+        return;
+    }
+    getStorage()->init_all();
+    sdo_.init(); // reread SDO paramters;
+    // TODO: set SYNC data
+
+    try{
+        if(!start()) BOOST_THROW_EXCEPTION( TimeoutException("start timeout") );
+    }
+    catch(const TimeoutException&){
+        status.error(boost::str(boost::format("could not start node '%1%'") %  (int)node_id_));
+    }
+}
+void Node::handleRecover(LayerStatus &status){
+    try{
+        start();
+    }
+    catch(const TimeoutException&){
+        status.error(boost::str(boost::format("could not start node '%1%'") %  (int)node_id_));
+    }
+}
+void Node::handleShutdown(LayerStatus &status){
+    if(getHeartbeatInterval()> 0) heartbeat_.set(0);
+    stop();
+    nmt_listener_.reset();
+    switchState(Unknown);
+}
+void Node::handleHalt(LayerStatus &status){
+    // do nothing
+}