git commit -m "first commit for v2"

Devices/Packages/diff_wheel_plugin/CMakeLists.txt

@@ -0,0 +1,233 @@
+cmake_minimum_required(VERSION 3.0.2)
+project(diff_wheel_plugin)
+
+## 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
+  std_msgs
+  models
+  pluginlib
+  realtime_tools
+  geometry_msgs
+  delta_modbus
+  md_controller
+  nav_2d_utils
+)
+
+## 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  # Or other packages containing 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 diff_wheel_plugin
+  CATKIN_DEPENDS roscpp geometry_msgs std_msgs realtime_tools
+  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}
+  src/wheel_plugin.cpp
+  # src/encoder_plugin.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(diff_wheel_feedback src/diff_wheel_feedback.cpp)
+add_executable(diff_wheel_controller src/diff_wheel_controller.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(diff_wheel_feedback ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+add_dependencies(diff_wheel_controller ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Specify libraries to link a library or executable target against
+target_link_libraries(${PROJECT_NAME}
+  ${Boost_LIBRARIES}
+  ${catkin_LIBRARIES}
+)
+
+target_link_libraries(diff_wheel_feedback
+  ${PROJECT_NAME}
+  ${Boost_LIBRARIES}
+  ${catkin_LIBRARIES}
+)
+
+target_link_libraries(diff_wheel_controller
+  ${PROJECT_NAME}
+  ${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 diff_wheel_feedback diff_wheel_controller
+  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
+)
+
+install(DIRECTORY
+  launch
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+)
+
+## 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_diff_wheel_plugin.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/Packages/diff_wheel_plugin/include/diff_wheel_plugin/command.h

@@ -0,0 +1,17 @@
+#ifndef __DIFF_WHEEL_PLUGIN_COMMAND_H_INCLUDED_
+#define __DIFF_WHEEL_PLUGIN_COMMAND_H_INCLUDED_
+
+#include <ros/ros.h>
+
+namespace diff_wheel_plugin
+{
+    class WheelCommand
+    {
+    public:
+      double vel_wheel;
+      ros::Time stamp;
+
+      WheelCommand() : vel_wheel(0.0), stamp(0.0) {}
+    };
+} // namespace diff_wheel_plugin
+#endif

Devices/Packages/diff_wheel_plugin/include/diff_wheel_plugin/wheel_plugin.h

@@ -0,0 +1,159 @@
+#ifndef __ROS_DIFF_WHEEL_PLUGIN_H_INCLUDED_
+#define __ROS_DIFF_WHEEL_PLUGIN_H_INCLUDED_
+
+#include <ros/ros.h>
+#include <std_msgs/Float32.h>
+#include "diff_wheel_plugin/command.h"
+#include <realtime_tools/realtime_buffer.h>
+#include <realtime_tools/realtime_publisher.h>
+// Boost
+#include <boost/thread.hpp>
+#include <boost/make_shared.hpp>
+//plugin
+#include <models/BaseSteerDrive.h>
+
+
+namespace diff_wheel_plugin
+{
+/**
+ * @class WheelPlugin
+ */
+class WheelPlugin : public models::BaseSteerDrive
+{
+public:
+
+  /**
+   * @brief Contructor
+  */
+  WheelPlugin();
+
+  /**
+   * @brief Contructor
+   * @param[in] nh    NodeHandle 
+   * @param[in] name  The name to give this instance of the model base
+  */
+  WheelPlugin(ros::NodeHandle & nh, const std::string &name);
+
+
+  /**
+   * @brief Decontructor 
+  */
+  virtual ~WheelPlugin();
+
+  /**
+   * @brief  Init 
+   * @param[in] nh  NodeHandle 
+   * @param name    The name to give this instance of the model base
+  */
+  virtual void initialize(ros::NodeHandle & nh, const std::string &name) override;
+
+  /**
+   * @brief  Constructs 
+   * @param name Check model is deady
+   * @return true, if model is ready, false otherwise
+  */
+  virtual bool Ready() override;
+
+  /**
+   * @brief Get the velocity limit on axis(index).
+   * @return Velocity limit specified in SDF
+  */ 
+  virtual double GetVelocityLimit(unsigned int _index) override;
+
+  /**
+   * @brief Set the velocity limit on a joint axis.
+   * @param[in] _velocity Velocity limit for the axis.
+  */ 
+  virtual void SetVelocityLimit(double _velocity) override;
+
+  /**
+   * @brief Get the rotation rate of an axis(index)
+   * @return The rotaional velocity of the joint axis. (rad/s)
+  */ 
+  virtual double GetVelocity() override;
+
+  /**
+   * @brief Set the velocity of an axis(index).
+   * In ODE and Bullet, the SetVelocityMaximal function is used to
+   * set the velocity of the child link relative to the parent.
+   * In Simbody and DART, this function updates the velocity state,
+   * which has a recursive effect on the rest of the chain.
+   * @param[in] _vel Velocity.
+  */
+  virtual void SetVelocity(double _vel) override;
+
+  /**
+   * @brief Get the position of an axis according to its index.
+   *
+   * For rotational axes, the value is in radians. For prismatic axes,
+   * it is in meters.
+   *
+   * For static models, it returns the static joint position.
+   *
+   * It returns ignition::math::NAN_D in case the position can't be
+   * obtained. For instance, if the index is invalid, if the joint is
+   * fixed, etc.
+   *
+   * Subclasses can't override this method. See PositionImpl instead.
+   *
+   * @return Current position of the axis.
+   * @sa PositionImpl
+  */ 
+  virtual double Position() override;
+
+  /**
+   * @brief The child links of this joint are updated based on desired
+   * position.  And all the links connected to the child link of this joint
+   * except through the parent link of this joint moves with the child
+   * link.
+   * @param[in] _position Position to set the joint to.
+   * unspecified, pure kinematic teleportation.
+   * link with respect to the world frame will remain the same after
+   * setting the position. By default this is false, which means there are
+   * no guarantees about what the child link's world velocity will be after
+   * the position is changed (the behavior is determined by the underlying
+   * physics engine).
+   *
+   * @note{Only ODE and Bullet support _preserveWorldVelocity being true.}
+  */ 
+  virtual void SetPosition(const double _position) override;
+
+private:
+
+  /**
+   * @brief Brake by away SetVelocity(0.0)
+   */
+  void brake() { this->SetVelocity(0.0); }
+
+  /**
+   * @brief call back from a subscriber  
+  */
+  void DLS_Callback(const std_msgs::Float32::ConstPtr & msg);
+
+  /**
+   * @brief Do stuff when create boost::Thread
+  */
+  void DoStuff(void);
+
+  ros::NodeHandle nh_;
+  ros::NodeHandle nh_priv_;
+  std::string name_;
+  bool initialized_;
+  bool is_ready_;
+  int m_subscribe_queue_size_;
+  double update_rate_;
+  boost::thread * m_publish_thread_;
+  boost::mutex callback_mutex_;
+  std_msgs::Float32::ConstPtr m_measurement_;
+
+  bool allow_multiple_command_publishers_;
+  realtime_tools::RealtimeBuffer<diff_wheel_plugin::WheelCommand> command_;
+  diff_wheel_plugin::WheelCommand command_struct_;
+  double command_timeout_;
+  ros::Subscriber mesurement_sub_;
+  boost::shared_ptr<realtime_tools::RealtimePublisher<std_msgs::Float32>> wheel_pub_;
+};
+  
+} // namespace diff_wheel_plugin
+
+#endif // __ROS_DIFF_WHEEL_PLUGIN_H_INCLUDED_

Devices/Packages/diff_wheel_plugin/launch/run.launch

@@ -0,0 +1,16 @@
+<?xml version="1.0"?>
+<launch>
+    <arg name="output_screen"       default="screen"/>
+    <node pkg="diff_wheel_plugin"   type="diff_wheel_feedback" name="diff_wheel_feedback" output="$(arg output_screen)"> 
+        <param name="ip_address"    value="192.168.1.5"/>
+        <param name="port"          value="502"/>
+        <param name="publish_rate"  value="50"/>
+        <param name="start_bit"     value="111"/>
+        <param name="end_bit"       value="114"/>
+    </node>
+    
+    <node pkg="diff_wheel_plugin" type="diff_wheel_controller" name="diff_wheel_controller" output="screen">
+        <rosparam command="load" file="$(find md_controller)/motorInfomation.yaml"/> 
+        <param name="port_name" type="str" value="/dev/USB_MD200"/> 
+    </node>
+</launch>

Devices/Packages/diff_wheel_plugin/package.xml

@@ -0,0 +1,89 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>diff_wheel_plugin</name>
+  <version>0.0.0</version>
+  <description>The diff_wheel_plugin 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/diff_wheel_plugin</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>
+  <exec_depend>roscpp</exec_depend>
+
+  <build_depend>std_msgs</build_depend>
+  <build_depend>geometry_msgs</build_depend>
+  <build_depend>delta_modbus_tcp</build_depend>
+  <build_depend>md_controller</build_depend>
+  <build_depend>models</build_depend>
+  <build_depend>pluginlib</build_depend>
+  <build_depend>realtime_tools</build_depend>
+  <build_depend>nav_2d_utils</build_depend>
+
+  <build_export_depend>std_msgs</build_export_depend>
+  <build_export_depend>geometry_msgs</build_export_depend>
+  <build_export_depend>delta_modbus_tcp</build_export_depend>
+  <build_export_depend>md_controller</build_export_depend>
+  <build_export_depend>models</build_export_depend>
+  <build_export_depend>pluginlib</build_export_depend>
+  <build_export_depend>realtime_tools</build_export_depend>
+  <build_export_depend>nav_2d_utils</build_export_depend>
+
+  <exec_depend>std_msgs</exec_depend>
+  <exec_depend>geometry_msgs</exec_depend>
+  <exec_depend>delta_modbus_tcp</exec_depend>
+  <exec_depend>md_controller</exec_depend>
+  <exec_depend>models</exec_depend>
+  <exec_depend>pluginlib</exec_depend>
+  <exec_depend>realtime_tools</exec_depend>
+  <exec_depend>nav_2d_utils</exec_depend>
+
+  <!-- The export tag contains other, unspecified, tags -->
+  <export>
+    <!-- Other tools can request additional information be placed here -->
+    <models plugin="${prefix}/plugins.xml" />
+  </export>
+</package>

Devices/Packages/diff_wheel_plugin/plugins.xml

@@ -0,0 +1,9 @@
+<library path="lib/libdiff_wheel_plugin">
+  <class name="WheelPlugin" type="diff_wheel_plugin::WheelPlugin" base_class_type="models::BaseSteerDrive">
+    <description></description>
+  </class>
+
+  <class name="EncoderPlugin" type="diff_wheel_plugin::EncoderPlugin" base_class_type="models::BaseAbsoluteEncoder">
+    <description></description>
+  </class>
+</library>

Devices/Packages/diff_wheel_plugin/src/diff_wheel_controller.cpp

@@ -0,0 +1,201 @@
+#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 <cstdlib>
+#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(node_name + "/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 = 50; // Hz
+  ros::Rate loop_rate(rate); 
+
+  if (p_md_controller_subscriber.empty()) {
+      ROS_WARN("No subscribers available!");
+      return 1;
+  }
+  
+  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);
+    ros::Duration(1.0).sleep();
+    const std::string password = "robotics";  // Replace with the correct password
+    // First, set to RS232
+    std::string command = "sudo -S -k echo rs232 > /sys/class/sp339_mode_ctl/uartMode";
+    std::string full_command = "echo '" +password + "' | " + command;
+    system(full_command.c_str());
+
+    // Sleep for 0.5 seconds
+    ros::Duration(1.0).sleep();
+
+    // Now, set to RS485
+    command = "sudo -S -k echo rs485 > /sys/class/sp339_mode_ctl/uartMode";
+    full_command = "echo '" + password + "' | " + command;
+    system(full_command.c_str());
+
+    for(auto &p_sub : p_md_controller_subscriber)
+    {
+      md_controller->pid_break(p_sub->slave_id);
+      md_controller->pid_break(p_sub->slave_id);
+    }
+    int v_min = 60;
+    while(ros::ok() && md_controller->_connected)
+    {
+      for(auto it = p_md_controller_subscriber.begin(); it != p_md_controller_subscriber.end(); ++it)
+      {
+        auto &p_sub = *it;
+        if(p_sub->isWheelTriggered()) 
+        {
+          int16_t rotate_speed_wheel;
+          {
+            boost::lock_guard<boost::mutex> publish_lockguard(p_sub->m_measurement_mutex);
+            rotate_speed_wheel = p_sub->speed;
+          }
+
+          if(std::abs(rotate_speed_wheel) > 0)  
+          {
+            int sign = (rotate_speed_wheel / abs(rotate_speed_wheel)) > 0 ? 1 : -1;
+            int16_t speed = abs(rotate_speed_wheel) < v_min? (int)(sign * v_min) : rotate_speed_wheel;
+            md_controller->pid_vel_cmd(p_sub->slave_id, speed);
+            // ROS_INFO("%x %d", p_sub->slave_id, speed);
+          }
+          else
+          { 
+            md_controller->pid_break(p_sub->slave_id);
+            if(abs(rotate_speed_wheel) > 0) ROS_INFO("%x Brake %d", p_sub->slave_id, rotate_speed_wheel);
+          }
+          p_sub->scheduleWheelController(false);
+        }
+
+        if(p_sub->isWheelLastestTriggered())
+        { 
+          int16_t rotate_speed_wheel;
+          {
+            boost::lock_guard<boost::mutex> publish_lockguard(p_sub->m_measurement_mutex);
+            rotate_speed_wheel = p_sub->speed;
+          }
+          md_controller->pid_stop(p_sub->slave_id);
+          if(std::abs(rotate_speed_wheel) > 0) ROS_WARN("%x stop %d", p_sub->slave_id, rotate_speed_wheel);
+          p_sub->scheduleWheelController(false);
+        }
+      }
+      loop_rate.sleep();
+      ros::spinOnce();
+    }
+    ros::spinOnce();
+    for(auto &p_sub : p_md_controller_subscriber)
+    {
+      md_controller->pid_break(p_sub->slave_id);
+      md_controller->pid_break(p_sub->slave_id);
+    }
+    md_controller->close_port();
+  }
+  ros::spin();
+  return 0;
+}
+

Devices/Packages/diff_wheel_plugin/src/diff_wheel_feedback.cpp

@@ -0,0 +1,81 @@
+#include <ros/ros.h>
+#include <boost/thread.hpp>
+#include "delta_modbus/delta_modbus_tcp.h"
+#include <std_msgs/Float32.h>
+
+double combine_uint16_to_double(uint16_t high, uint16_t low) {
+  // Chuyển đổi high thành double và dịch nó sang trái 16 bit
+  double result = (double)high * 65536.0;  // 65536 = 2^16
+  
+  // Thêm low vào kết quả
+  result += (double)low;
+  
+  return result;
+}
+
+/**********************************************************************
+ *                   		MAIN 
+***********************************************************************/
+int main(int argc, char** argv) {
+  ros::init(argc, argv, "diff_encoder_node");
+  ros::NodeHandle nh;
+  ros::NodeHandle nh_node("~");
+  std::string node_name = ros::this_node::getName();
+  ROS_INFO("node_name: %s", node_name.c_str());
+  
+  std::string ip_address; 
+  int port;
+
+  if(!nh_node.getParam("ip_address", ip_address)) 
+  {
+    ROS_ERROR("Could not find 'ip_address' in node handle");
+    return 1;
+  }
+  if(!nh_node.getParam("port", port)) 
+  {
+    ROS_ERROR("Could not find 'port' in node handle");
+    return 1;
+  }
+
+  int id;
+  nh_node.param("id", id, 1);
+  double rate;
+  nh_node.param("publish_rate", rate, 20.0);
+  
+  int start, end;
+  nh_node.param("start_bit", start, 101);
+  nh_node.param("end_bit", end, 103);
+
+  ROS_INFO("%s: start_bit: %d end_bit: %d ", node_name.c_str(), start, end);
+
+  ros::Publisher left_encoder_pub   = nh.advertise<std_msgs::Float32>("left_encoder", 1);
+  ros::Publisher right_encoder_pub  = nh.advertise<std_msgs::Float32>("right_encoder", 1);
+  DELTA_NAMESPACE::PLC *plc = NULL;
+  plc = new DELTA_NAMESPACE::PLC(ip_address, port, id);
+  ros::Rate r(rate);
+  uint16_t dataD[4];
+  float left_encoder, righ_encoder;
+  while (ros::ok())
+  {   
+    plc->connect();
+    while (ros::ok() && plc->checkConnected())
+    {
+      plc->mulGetD(start, end, dataD);
+      std::memcpy(reinterpret_cast<void*>(&righ_encoder), reinterpret_cast<void const*>(&dataD[0]), sizeof left_encoder);
+      std::memcpy(reinterpret_cast<void*>(&left_encoder), reinterpret_cast<void const*>(&dataD[2]), sizeof righ_encoder);
+      std_msgs::Float32 left_encoder_msg, righ_encoder_msg;
+      left_encoder_msg.data = left_encoder * 2 * M_PI;
+      righ_encoder_msg.data = righ_encoder * 2 * M_PI;
+      left_encoder_pub.publish(left_encoder_msg);
+      right_encoder_pub.publish(righ_encoder_msg);
+      r.sleep();
+      ros::spinOnce();
+    }
+    ros::spinOnce();
+  }
+  
+  ros::spin();  
+  plc = NULL;  
+  delete(plc);
+  return 0;
+}

Devices/Packages/diff_wheel_plugin/src/wheel_plugin.cpp

@@ -0,0 +1,192 @@
+#include "diff_wheel_plugin/wheel_plugin.h"
+#include <pluginlib/class_list_macros.hpp>
+#include <nav_2d_utils/parameters.h>
+#include <nav_2d_utils/conversions.h>
+#include <nav_2d_utils/tf_help.h>
+#include <nav_2d_utils/path_ops.h>
+
+
+namespace diff_wheel_plugin
+{
+
+WheelPlugin::WheelPlugin() 
+ : command_timeout_(0.5)
+{
+
+}
+
+WheelPlugin::WheelPlugin(ros::NodeHandle & nh, const std::string &name)
+ : command_timeout_(0.5) 
+{
+  initialize(nh, name);
+}
+
+
+WheelPlugin::~WheelPlugin() 
+{
+  if (m_publish_thread_)
+  {
+    m_publish_thread_->join();
+    delete (m_publish_thread_);
+    m_publish_thread_ = 0;
+  }
+}
+
+void WheelPlugin::initialize(ros::NodeHandle & nh, const std::string &name) 
+{
+  if(!initialized_)
+  {
+    nh_ = nh;
+    name_ = name;
+    nh_priv_ = ros::NodeHandle(nh, name); 
+    nh_priv_.param("command_timeout", command_timeout_, 0.5);
+    nh_priv_.param("subscribe_queue_size", m_subscribe_queue_size_, 1); 
+    nh_priv_.param("max_publish_rate", update_rate_, 100.0);
+
+    ROS_INFO_NAMED("WheelPlugin","Initializing on %s with command_timeout %f subscribe_queue_size %d max_publish_rate %f", 
+                    name_.c_str(),
+                    command_timeout_,
+                    m_subscribe_queue_size_,
+                    update_rate_
+                  );
+    std::string mesurement_topic;
+    nh_priv_.param("mesurement_topic", mesurement_topic, std::string(""));
+    std::string wheel_topic;
+    nh_priv_.param("wheel_topic", wheel_topic, name_);
+
+    mesurement_sub_ = nh_.subscribe(mesurement_topic, m_subscribe_queue_size_, &diff_wheel_plugin::WheelPlugin::DLS_Callback, this);
+    
+    wheel_pub_.reset(new realtime_tools::RealtimePublisher<std_msgs::Float32>(nh_, wheel_topic , 100));
+    
+    m_publish_thread_ = new boost::thread(&diff_wheel_plugin::WheelPlugin::DoStuff,this);
+    command_struct_.stamp = ros::Time::now();
+    ROS_INFO_NAMED("WheelPlugin","Initializing on %s is successed", name_.c_str());
+    
+    initialized_ = true;
+  }
+}
+
+bool WheelPlugin::Ready()
+{
+  return is_ready_;
+}
+ 
+double WheelPlugin::GetVelocityLimit(unsigned int _index) 
+{
+  throw std::bad_function_call();
+}
+
+void WheelPlugin::SetVelocityLimit(double _velocity) 
+{
+  throw std::bad_function_call();
+}
+
+double WheelPlugin::GetVelocity() 
+{
+  boost::lock_guard<boost::mutex> publish_lockguard(callback_mutex_);
+  double result = 0.0;
+  if(m_measurement_)
+  {
+    result = m_measurement_->data;
+  }
+  return result;
+}
+
+void WheelPlugin::SetVelocity(double _vel) 
+{
+  if (!std::isfinite(_vel))
+  {
+    ROS_WARN_THROTTLE(1.0, "Received NaN in velocity %s command. Ignoring.", name_.c_str());
+    return;
+  }
+  command_struct_.vel_wheel = _vel;
+  command_struct_.stamp = ros::Time::now();
+  command_.writeFromNonRT(command_struct_);
+  ROS_DEBUG_STREAM_NAMED(name_,
+    "Added values to command."
+    << "vel_wheel: " << command_struct_.vel_wheel << ", "
+    << "Stamp: " << command_struct_.stamp);
+
+  // ROS_INFO_STREAM_NAMED(name_,
+  //   "Added values to command."
+  //   << "vel_wheel: " << command_struct_.vel_wheel << ", "
+  //   << "Stamp: " << command_struct_.stamp);
+}
+
+double WheelPlugin::Position()
+{
+  throw std::bad_function_call();
+}
+
+void WheelPlugin::SetPosition(const double _position)
+{
+  throw std::bad_function_call();
+}
+
+void WheelPlugin::DLS_Callback(const std_msgs::Float32::ConstPtr & msg)
+{
+  boost::lock_guard<boost::mutex> publish_lockguard(callback_mutex_);
+  // check that we don't have multiple publishers on the command topic
+  if (!allow_multiple_command_publishers_ && mesurement_sub_.getNumPublishers() > 1)
+  {
+    ROS_ERROR_STREAM_THROTTLE_NAMED(1.0, name_, "Detected " << mesurement_sub_.getNumPublishers()
+      << " publishers. Only 1 publisher is allowed. Going to brake.");
+    this->brake();
+    return;
+  }
+
+  if (!std::isfinite(msg->data))
+  {
+    ROS_WARN_THROTTLE(1.0, "Received NaN in velocity %s encoder. Ignoring.", name_.c_str());
+    return;
+  }
+
+  m_measurement_ = msg;
+}
+
+void WheelPlugin::DoStuff()
+{
+  ros::Rate rate(update_rate_);
+  is_ready_ = true;
+  ROS_INFO_NAMED("WheelPlugin","...Threading on %s is ready", name_.c_str());
+  bool stoped;
+  while (ros::ok())
+  {
+    WheelCommand curr_cmd =  *(command_.readFromRT());
+    ros::Time current_time = ros::Time::now();
+    double dt = ros::Duration(current_time - command_struct_.stamp).toSec();
+    
+    if(dt <= command_timeout_ && fabs(curr_cmd.vel_wheel) > 0.001)
+    {
+      if (wheel_pub_ && wheel_pub_->trylock())
+      {
+        wheel_pub_->msg_.data = curr_cmd.vel_wheel;
+        wheel_pub_->unlockAndPublish();
+      }
+      stoped = false;
+    }
+    else
+    {
+      if(!stoped)
+      {
+        for(int i = 0; i < 3; i++)
+        {
+          if (wheel_pub_ && wheel_pub_->trylock())
+          {
+            wheel_pub_->msg_.data = 0;
+            wheel_pub_->unlockAndPublish();
+          }
+          ros::Duration(0.1).sleep();
+        }
+        stoped = true;
+      }
+    }
+
+    rate.sleep();
+    ros::spinOnce();
+  }
+}
+
+}; //namespace diff_wheel_plugin
+
+PLUGINLIB_EXPORT_CLASS(diff_wheel_plugin::WheelPlugin, models::BaseSteerDrive)

Devices/Packages/msle_tf_base_link/CMakeLists.txt

@@ -0,0 +1,209 @@
+cmake_minimum_required(VERSION 3.0.2)
+project(msle_tf_base_link)
+
+## Compile as C++11, supported in ROS Kinetic and newer
+# add_compile_options(-std=c++11)
+
+## 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
+  std_msgs
+  sick_line_guidance
+  tf2
+  tf2_ros
+)
+
+## 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 msle_tf_base_link
+#  CATKIN_DEPENDS roscpp std_msgs
+#  DEPENDS system_lib
+)
+
+###########
+## Build ##
+###########
+
+## Specify additional locations of header files
+## Your package locations should be listed before other locations
+include_directories(
+#  include
+  ${catkin_INCLUDE_DIRS}
+)
+
+## Declare a C++ library
+#add_library(${PROJECT_NAME}
+#  src/mlse_tf_base_link_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(mlse_base_link_broadcaster src/mlse_tf_base_link_broadcaster.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(mlse_base_link_broadcaster ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Specify libraries to link a library or executable target against
+target_link_libraries(mlse_base_link_broadcaster
+  ${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 mlse_base_link_broadcaster
+  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 
+  launch
+  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_msle_tf_base_link.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/Packages/msle_tf_base_link/launch/msle_tf_base_link.launch

@@ -0,0 +1,8 @@
+<?xml version="1.0" ?>
+
+<launch>
+    <arg name="yaml" default="$(find sick_line_guidance)/sick_line_guidance_mls.yaml"/>
+    <node pkg="msle_tf_base_link" type="mlse_base_link_broadcaster" name="mlse_base_link_broadcaster" output="screen" >
+        <rosparam command="load" file="$(arg yaml)" />  
+    </node>
+</launch>

Devices/Packages/msle_tf_base_link/package.xml

@@ -0,0 +1,75 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>msle_tf_base_link</name>
+  <version>0.0.0</version>
+  <description>The msle_tf_base_link 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/msle_tf_base_link</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_depend>sick_line_guidance</build_depend>
+  <build_depend>tf2</build_depend>
+  <build_depend>tf2_ros</build_depend>
+
+  <build_export_depend>roscpp</build_export_depend>
+  <build_export_depend>std_msgs</build_export_depend>
+  <build_export_depend>sick_line_guidance</build_export_depend>
+  <build_export_depend>tf2</build_export_depend>
+  <build_export_depend>tf2_ros</build_export_depend>
+
+  <exec_depend>roscpp</exec_depend>
+  <exec_depend>std_msgs</exec_depend>
+  <exec_depend>sick_line_guidance</exec_depend>
+  <exec_depend>tf2</exec_depend>
+  <exec_depend>tf2_ros</exec_depend>
+
+  <!-- The export tag contains other, unspecified, tags -->
+  <export>
+    <!-- Other tools can request additional information be placed here -->
+
+  </export>
+</package>

Devices/Packages/msle_tf_base_link/src/mlse_tf_base_link_broadcaster.cpp

@@ -0,0 +1,66 @@
+#include <ros/ros.h>
+#include <tf2/LinearMath/Quaternion.h>
+#include <tf2_ros/transform_broadcaster.h>
+#include <geometry_msgs/TransformStamped.h>
+#include <sick_line_guidance/MLS_Measurement.h>
+#include <std_msgs/UInt8MultiArray.h>
+
+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;
+}
+
+
+std_msgs::UInt8MultiArray measurement_array;
+
+void msleCallback(const sick_line_guidance::MLS_MeasurementConstPtr& msg)
+{
+  if(((msg->lcp & 0x02) >> 1) == 0x01)
+  {
+    static tf2_ros::TransformBroadcaster br;
+    geometry_msgs::TransformStamped transformStamped;
+    int sign = msg->header.frame_id[0] == 'f' ? 1 : -1;
+
+    transformStamped.header.stamp = ros::Time::now();
+    transformStamped.header.frame_id = "base_link";
+    transformStamped.child_frame_id = msg->header.frame_id;
+    transformStamped.transform.translation.x = sign * 0.476;
+    transformStamped.transform.translation.y = msg->position[1];
+    transformStamped.transform.translation.z = 0.0;
+    tf2::Quaternion q;
+    q.setRPY(0, 0, 0);
+    transformStamped.transform.rotation.x = q.x();
+    transformStamped.transform.rotation.y = q.y();
+    transformStamped.transform.rotation.z = q.z();
+    transformStamped.transform.rotation.w = q.w();
+    br.sendTransform(transformStamped);
+  }
+}
+
+int main(int argc, char **argv)
+{
+  /* Khoi tao Node */
+  ros::init(argc, argv, "mlse_base_link_broadcaster");
+  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());
+  XmlRpc::XmlRpcValue nodes;
+  if(!nh->getParam(node_name + "/nodes", nodes) || nodes.size() < 1)
+  {
+    ROS_ERROR("%s: no driver found in yaml-file, please check configuration. Aborting...", node_name.c_str());
+    return 1;
+  }
+  std::vector<ros::Subscriber> mlse_subscribers;
+  for(XmlRpc::XmlRpcValue::iterator object_iter = nodes.begin(); object_iter != nodes.end(); ++object_iter)
+  {
+    std::string topic_name    = readMember<std::string>(object_iter->second, "sick_topic", "");
+    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);
+    ros::Subscriber sub = nh->subscribe(topic_name, subscribe_queue_size, &msleCallback);
+    mlse_subscribers.push_back(sub);
+  }  
+  ros::spin();  
+  return 0;
+}

Devices/Packages/olei_lidar_driver_ros/olei_msgs/CMakeLists.txt

@@ -0,0 +1,16 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(olei_msgs)
+
+find_package(catkin REQUIRED COMPONENTS message_generation std_msgs)
+
+add_message_files(
+  DIRECTORY msg
+  FILES
+  oleiPacket.msg
+  oleiScan.msg
+)
+generate_messages(DEPENDENCIES std_msgs)
+
+catkin_package(
+  CATKIN_DEPENDS message_runtime std_msgs
+)

Devices/Packages/olei_lidar_driver_ros/olei_msgs/msg/oleiPacket.msg

@@ -0,0 +1,5 @@
+# Raw olei LIDAR packet.
+
+time stamp              # packet timestamp
+uint8[1240] data        # packet contents
+

Devices/Packages/olei_lidar_driver_ros/olei_msgs/msg/oleiScan.msg

@@ -0,0 +1,4 @@
+# olei LIDAR scan packets.
+
+Header           header         # standard ROS message header
+oleiPacket[] packets        # vector of raw packets

Devices/Packages/olei_lidar_driver_ros/olei_msgs/package.xml

@@ -0,0 +1,23 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>olei_msgs</name>
+  <version>1.5.2</version>
+  <description>
+    ROS message definitions for olei 2D LIDARs.
+  </description>
+  <maintainer email="emanuele.montemurro@olei.com">Emanuele</maintainer>
+  <author>Emanuele</author>
+  <license>BSD</license>
+
+  <url type="website">TODO</url>
+  <url type="repository">TODO</url>
+  <url type="bugtracker">TODO</url>
+
+  <buildtool_depend>catkin</buildtool_depend>
+
+  <build_depend>message_generation</build_depend>
+
+  <depend>std_msgs</depend>
+
+  <exec_depend>message_runtime</exec_depend>
+</package>

Devices/Packages/olei_lidar_driver_ros/olelidar/CMakeLists.txt

@@ -0,0 +1,54 @@
+cmake_minimum_required(VERSION 3.5)
+project(olelidar)
+
+set(CMAKE_CXX_STANDARD 11)
+set(CMAKE_CXX_FLAGS "-std=c++11 -g ${CMAKE_CXX_FLAGS}")
+
+find_package(catkin REQUIRED
+  COMPONENTS 
+  roscpp
+  diagnostic_updater
+  dynamic_reconfigure
+  sensor_msgs
+  olei_msgs)
+
+generate_dynamic_reconfigure_options(cfg/oleiPuck.cfg)
+
+catkin_package()
+
+#add_executable(${PROJECT_NAME}_driver src/driver.cpp)
+add_library(${PROJECT_NAME}_driver src/driver.cpp)
+target_include_directories(${PROJECT_NAME}_driver PUBLIC ${catkin_INCLUDE_DIRS} src)
+target_link_libraries(${PROJECT_NAME}_driver PUBLIC ${catkin_LIBRARIES})
+
+add_executable(${PROJECT_NAME}_decoder src/decoder.cpp)
+target_include_directories(${PROJECT_NAME}_decoder PUBLIC ${catkin_INCLUDE_DIRS} src)
+target_link_libraries(${PROJECT_NAME}_decoder PUBLIC ${catkin_LIBRARIES} ${PROJECT_NAME}_driver)
+
+add_dependencies(${PROJECT_NAME}_driver ${PROJECT_NAME}_gencfg)
+add_dependencies(${PROJECT_NAME}_decoder ${PROJECT_NAME}_gencfg)
+
+## Mark executables and/or libraries for installation
+install(TARGETS ${PROJECT_NAME}_driver ${PROJECT_NAME}_decoder
+  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
+
+## Mark cpp header files for installation
+install(DIRECTORY include/${PROJECT_NAME}/include
+  DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+  FILES_MATCHING PATTERN "*.h"
+  PATTERN ".git" EXCLUDE
+)
+
+install(FILES
+  cfg/oleiPuck.cfg
+  launch/decoder.launch
+  launch/driver.launch
+  launch/scan.launch
+  # launch/olelidar.launch
+  launch/debug.conf
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch
+)
+

Devices/Packages/olei_lidar_driver_ros/olelidar/cfg/oleiPuck.cfg

@@ -0,0 +1,39 @@
+#!/usr/bin/env python
+PACKAGE = "olelidar"
+
+from dynamic_reconfigure.parameter_generator_catkin import *
+
+gen = ParameterGenerator()
+
+
+# device
+gen.add("device_ip", str_t, 0, "IP addr to receive packet from.", "192.168.2.122")
+gen.add("device_port", int_t, 0, "UDP port to receive packet from.", 2368)
+gen.add("local_ip", str_t, 0, "loacl IP addr.", "192.168.2.198")
+gen.add("multiaddr", str_t, 0, "multiaddr.", "239.255.0.100")
+gen.add("freq", double_t, 0, "rotate degree per second", 25.0)
+gen.add("route", int_t, 0, "rotate degree per second", 2000)
+gen.add("step", double_t, 0, "rotate degree per second", 0.225)
+# laserscan
+'''
+std_msgs/Header header
+      uint32 seq
+      time stamp
+      string frame_id
+float32 angle_min
+float32 angle_max
+float32 angle_increment
+float32 time_increment
+float32 scan_time
+float32 range_min
+float32 range_max
+float32[] ranges
+float32[] intensities
+'''
+# gen.add(name, type, level, description, default, min, max)
+gen.add("frame_id", str_t, 0, "olelidar")
+gen.add("angle_min", double_t, 0, "angle_min",0,0,360)
+gen.add("angle_max", double_t, 360, "angle_max",360,0,360)
+gen.add("range_min", double_t, 0.1, "min range", 0.1, 0.1, 20)
+gen.add("range_max", double_t, 0.1, "max range", 50.0, 0.1, 50)
+exit(gen.generate(PACKAGE, PACKAGE, "oleiPuck"))

Devices/Packages/olei_lidar_driver_ros/olelidar/launch/debug.conf

@@ -0,0 +1 @@
+log4j.logger.ros.olelidar=DEBUG

Devices/Packages/olei_lidar_driver_ros/olelidar/launch/decoder.launch

@@ -0,0 +1,22 @@
+<launch>
+  <arg name="pkg" value="olelidar"/>
+
+  <arg name="frame_id" default="olelidar"/>
+  <arg name="r_max" default="50"/>
+  <arg name="ang_start" default="0"/>
+  <arg name="ang_end" default="360"/>
+  <arg name="inverted" default="false"/>
+  <arg name="poly" default="1"/>
+  <node pkg="$(arg pkg)" type="$(arg pkg)_decoder" name="$(arg pkg)_decoder" output="screen">
+    <param name="frame_id" type="string" value="$(arg frame_id)"/>
+    <param name="r_max" type="int" value="$(arg r_max)"/>
+    <param name="ang_start" type="int" value="$(arg ang_start)"/>
+    <param name="ang_end" type="int" value="$(arg ang_end)"/>
+    <param name="inverted" type="bool" value="$(arg inverted)"/>
+    <param name="poly" type="int" value="$(arg poly)"/>
+    <remap from="~packet" to="packet"/>
+    <remap from="~scan" to="scan"/>
+
+  </node>
+
+</launch>

Devices/Packages/olei_lidar_driver_ros/olelidar/launch/driver.launch

@@ -0,0 +1,15 @@
+<launch>
+  <arg name="pkg" value="olelidar"/>
+  <arg name="device_ip" default="192.168.1.100"/>
+  <arg name="device_port" default="2368"/>
+  <arg name="local_ip" default="192.168.2.198"/>
+  <arg name="multiaddr" default="239.255.0.100"/>
+  <node pkg="olelidar" type="$(arg pkg)_driver" name="$(arg pkg)_driver" output="screen">
+    <param name="device_ip" type="string" value="$(arg device_ip)"/>
+    <param name="device_port" type="int" value="$(arg device_port)"/>
+    <param name="local_ip" type="string" value="$(arg local_ip)"/>
+    <param name="multiaddr" type="string" value="$(arg multiaddr)"/>
+    <remap from="~packet" to="packet"/>
+  </node>
+
+</launch>

Devices/Packages/olei_lidar_driver_ros/olelidar/launch/scan.launch

@@ -0,0 +1,57 @@
+<launch>
+  <arg name="pkg" value="olelidar"/>
+
+  <!-- driver -->
+  <arg name="driver" default="true"/>
+  <arg name="device_ip_1" default="192.168.1.124"/>
+  <arg name="device_ip_2" default="192.168.2.13"/>
+  <arg name="device_port_1" default="2368"/>
+  <arg name="device_port_2" default="2369"/>
+  <arg name="local_ip" default="192.168.1.121"/>
+  <arg name="multiaddr" default=""/>
+  
+  <!-- decoder -->
+  <arg name="frame_id_1" default="front_scan"/>
+  <arg name="frame_id_2" default="rear_scan"/>
+  <arg name="r_max_1" default="10"/>
+  <arg name="r_max_2" default="25"/>
+  <arg name="ang_start" default="-180"/>
+  <arg name="ang_end" default="180"/>
+  <arg name="decoder" default="true"/>
+  <arg name="inverted" default="false"/>
+  <arg name="debug" default="false"/>
+  <env if="$(arg debug)" name="ROSCONSOLE_CONFIG_FILE" value="$(find olelidar)/launch/debug.conf"/>
+
+  <node pkg="$(arg pkg)" type="$(arg pkg)_decoder" name="$(arg pkg)_decoder" output="screen" ns="front_scan">
+    <param name="frame_id" type="string" value="$(arg frame_id_1)"/>
+    <param name="r_max" type="int" value="$(arg r_max_1)"/>
+    <param name="ang_start" type="int" value="$(arg ang_start)"/>
+    <param name="ang_end" type="int" value="$(arg ang_end)"/>
+    <param name="inverted" type="bool" value="$(arg inverted)"/>
+    <remap from="~packet" to="packet"/>
+    <remap from="~scan" to="/front_scan"/>
+
+    <param name="device_ip" type="string" value="$(arg device_ip_1)"/>
+    <param name="device_port" type="int" value="$(arg device_port_1)"/>
+    <param name="local_ip" type="string" value="$(arg local_ip)"/>
+    <param name="multiaddr" type="string" value="$(arg multiaddr)"/>
+
+  </node>
+
+  <!-- <node pkg="$(arg pkg)" type="$(arg pkg)_decoder" name="$(arg pkg)_decoder" output="screen" ns="rear_scan">
+    <param name="frame_id" type="string" value="$(arg frame_id_2)"/>
+    <param name="r_max" type="int" value="$(arg r_max_2)"/>
+    <param name="ang_start" type="int" value="$(arg ang_start)"/>
+    <param name="ang_end" type="int" value="$(arg ang_end)"/>
+    <param name="inverted" type="bool" value="$(arg inverted)"/>
+    <remap from="~packet" to="packet"/>
+    <remap from="~scan" to="/rear_scan"/>
+
+    <param name="device_ip" type="string" value="$(arg device_ip_2)"/>
+    <param name="device_port" type="int" value="$(arg device_port_2)"/>
+    <param name="local_ip" type="string" value="$(arg local_ip)"/>
+    <param name="multiaddr" type="string" value="$(arg multiaddr)"/>
+
+  </node> -->
+
+</launch>

Devices/Packages/olei_lidar_driver_ros/olelidar/olei_msgs/CMakeLists.txt

@@ -0,0 +1,16 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(olei_msgs)
+
+find_package(catkin REQUIRED COMPONENTS message_generation std_msgs)
+
+add_message_files(
+  DIRECTORY msg
+  FILES
+  oleiPacket.msg
+  oleiScan.msg
+)
+generate_messages(DEPENDENCIES std_msgs)
+
+catkin_package(
+  CATKIN_DEPENDS message_runtime std_msgs
+)

Devices/Packages/olei_lidar_driver_ros/olelidar/olei_msgs/msg/oleiPacket.msg

@@ -0,0 +1,5 @@
+# Raw olei LIDAR packet.
+
+time stamp              # packet timestamp
+uint8[1240] data        # packet contents
+

Devices/Packages/olei_lidar_driver_ros/olelidar/olei_msgs/msg/oleiScan.msg

@@ -0,0 +1,4 @@
+# olei LIDAR scan packets.
+
+Header           header         # standard ROS message header
+oleiPacket[] packets        # vector of raw packets

Devices/Packages/olei_lidar_driver_ros/olelidar/olei_msgs/package.xml

@@ -0,0 +1,23 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>olei_msgs</name>
+  <version>1.5.2</version>
+  <description>
+    ROS message definitions for olei 2D LIDARs.
+  </description>
+  <maintainer email="emanuele.montemurro@olei.com">Emanuele</maintainer>
+  <author>Emanuele</author>
+  <license>BSD</license>
+
+  <url type="website">TODO</url>
+  <url type="repository">TODO</url>
+  <url type="bugtracker">TODO</url>
+
+  <buildtool_depend>catkin</buildtool_depend>
+
+  <build_depend>message_generation</build_depend>
+
+  <depend>std_msgs</depend>
+
+  <exec_depend>message_runtime</exec_depend>
+</package>

Devices/Packages/olei_lidar_driver_ros/olelidar/package.xml

@@ -0,0 +1,26 @@
+<package format="2">
+
+  <name>olelidar</name>
+  <version>3.0.0</version>
+  <description>
+    Basic ROS support for the olei 2D LIDARs.
+  </description>
+  <maintainer email="emanuele.montemurro@olei.com">Emanuele</maintainer>
+  <author>Emanuele</author>
+  <license>GNU General Public License V3.0</license>
+
+  <buildtool_depend>catkin</buildtool_depend>
+
+  <depend>sensor_msgs</depend>
+  <depend>olei_msgs</depend>
+
+  <depend>roscpp</depend>
+  <depend>diagnostic_updater</depend>
+  <depend>dynamic_reconfigure</depend>
+
+
+  <export>
+    <nodelet plugin="${prefix}/nodelet_plugins.xml"/>
+  </export>
+
+</package>

Devices/Packages/olei_lidar_driver_ros/olelidar/src/constants.h

@@ -0,0 +1,75 @@
+#pragma once
+
+#include <cmath>
+#include <cstdint>
+#include <limits>
+
+namespace olelidar {
+
+static constexpr auto kNaNF = std::numeric_limits<float>::quiet_NaN();
+static constexpr auto kNaND = std::numeric_limits<double>::quiet_NaN();
+static constexpr float kTau = M_PI * 2;
+static constexpr float deg2rad(float deg) { return deg * M_PI / 180.0; }
+static constexpr float rad2deg(float rad) { return rad * 180.0 / M_PI; }
+
+// Raw olei packet constants and structures.
+// 1 packet = 150 blocks
+// 1 block = 1 sequence
+// 1 sequence = 1 firing = 1 point
+// 1 point = 8 bytes
+static constexpr int kPointBytes = 8;
+static constexpr int kPointsPerBlock = 1;
+
+// According to Bruce Hall DISTANCE_MAX is 65.0, but we noticed
+// valid packets with readings up to 130.0.
+static constexpr float kDistanceMax = 20.0;         // [m]
+static constexpr float kDistanceResolution = 0.001;  // [m]  2d lidar,
+
+/** @todo make this work for both big and little-endian machines */
+static const uint16_t UPPER_BANK = 0xeeff;  //rsv
+static const uint16_t LOWER_BANK = 0xddff;  //rsv
+
+/** Special Defines for olei support **/
+static constexpr double kSingleFiringNs = 2304;  // [ns] rsv,1s/10hz/1600points = 62500
+static constexpr double kFiringCycleNs = 55296;  // [ns] rsv,                   = 62500
+static constexpr double kSingleFiringRatio = kSingleFiringNs / kFiringCycleNs;  // 1
+
+// The information from two firing sequences of 16 lasers is contained in each
+// data block. Each packet contains the data from 24 firing sequences in 12 data
+// blocks.
+static constexpr int kFiringsPerSequence = 1;
+static constexpr int kSequencesPerBlock = 1;
+static constexpr int kBlocksPerPacket = 150;
+static constexpr int kSequencesPerPacket =
+    kSequencesPerBlock * kBlocksPerPacket;  // 150
+
+inline int LaserId2Row(int id) {
+  const auto index = (id % 2 == 0) ? id / 2 : id / 2 + kFiringsPerSequence / 2;
+  return kFiringsPerSequence - index - 1;
+}
+
+static constexpr uint16_t kMaxRawAzimuth = 35999;
+static constexpr float kAzimuthResolution = 0.01f;
+
+// static constexpr float kMinElevation = deg2rad(-15.0f);
+// static constexpr float kMaxElevation = deg2rad(15.0f);
+// static constexpr float kDeltaElevation =
+//    (kMaxElevation - kMinElevation) / (kFiringsPerSequence - 1);
+
+inline constexpr float Raw2Azimuth(uint16_t raw) {
+  // According to the user manual,
+  return deg2rad(static_cast<float>(raw) * kAzimuthResolution);
+}
+
+/// p55 9.3.1.2
+inline constexpr float Raw2Distance(uint16_t raw) {
+  return static_cast<float>(raw) * kDistanceResolution;
+}
+
+/// p51 8.3.1
+inline constexpr float AzimuthResolutionDegree(int rpm) {
+  // rpm % 60 == 0
+  return rpm / 60.0 * 360.0 * kFiringCycleNs / 1e9;
+}
+
+}  // namespace olelidar

Devices/Packages/olei_lidar_driver_ros/olelidar/src/decoder.cpp

@@ -0,0 +1,446 @@
+#include "constants.h"
+
+#include <dynamic_reconfigure/server.h>
+
+#include <sensor_msgs/LaserScan.h>
+
+#include <olei_msgs/oleiPacket.h>
+#include <olei_msgs/oleiScan.h>
+#include <olelidar/oleiPuckConfig.h>
+
+#include "driver.cpp"
+// here maskoff waring of macro 'ROS_LOG'
+#pragma clang diagnostic ignored "-Wzero-as-null-pointer-constant"
+
+namespace olelidar
+{
+
+  using namespace sensor_msgs;
+  using namespace olei_msgs;
+
+  class Decoder
+  {
+  public:
+    explicit Decoder(const ros::NodeHandle &pnh);
+
+    Decoder(const Decoder &) = delete;
+    Decoder operator=(const Decoder &) = delete;
+
+    void PacketCb(const oleiPacketConstPtr &packet_msg);
+    void ConfigCb(oleiPuckConfig &config, int level);
+    void PublishMsg(ros::Publisher *pub, std::vector<uint16_t> &packet_r, std::vector<uint16_t> &packet_i, ros::Time t);
+
+  private:
+    /// 9.3.1.3 Data Point
+    /// A data point is a measurement by one laser channel of a relection of a
+    /// laser pulse
+    struct DataPoint
+    {
+      uint16_t azimuth;      //unit: 0.01 degree
+      uint16_t distance;     //unit: 1mm
+      uint16_t reflectivity; //unit: 1 percent,100 is the reflet of white target
+      uint16_t distance2;    //rsv for dual return
+    } __attribute__((packed));
+    static_assert(sizeof(DataPoint) == 8, "sizeof(DataPoint) != 8");
+
+    struct FiringSequence
+    {
+      DataPoint points[kFiringsPerSequence]; // 8
+    } __attribute__((packed));
+    static_assert(sizeof(FiringSequence) == 8, "sizeof(FiringSequence) != 8");
+
+    struct DataHead
+    {
+      uint8_t magic[4];
+      uint8_t version[2];
+      uint8_t scale;
+      uint8_t oem[3];
+      uint8_t model[12];
+      uint8_t code[2];
+      uint8_t hw[2];
+      uint8_t sw[2];
+      uint32_t timestamp;
+      uint16_t rpm;
+      uint8_t flag[2];
+      uint32_t rsv;
+    } __attribute__((packed));
+    static_assert(sizeof(DataHead) == 40, "sizeof(DataBlock) != 40");
+
+    struct DataBlock
+    {
+      FiringSequence sequences[kSequencesPerBlock]; // 8 * 1
+    } __attribute__((packed));
+    static_assert(sizeof(DataBlock) == 8, "sizeof(DataBlock) != 8");
+
+    /// 9.3.1.5 Packet format for 2d
+    struct Packet
+    {
+      DataHead head;
+      DataBlock blocks[kBlocksPerPacket]; // 8 * 150
+
+    } __attribute__((packed));
+    static_assert(sizeof(Packet) == 1240, "sizeof(DataBlock) != 1240");
+    static_assert(sizeof(Packet) == sizeof(olei_msgs::oleiPacket().data), "sizeof(Packet) != 1240");
+
+    void DecodeAndFill(const Packet *const packet_buf, uint64_t);
+
+  private:
+    bool CheckFactoryBytes(const Packet *const packet);
+
+    // ROS related parameters
+    uint32_t laststamp;
+    uint32_t scantime;
+    ros::Time start;
+    bool is_time_base_{false};
+    ros::Time local_timestamp_base_;
+    uint32_t inner_timestamp_base_;
+
+    std::string frame_id_;
+    int range_min_=200;
+    int range_max_;
+    int ange_start_;
+    int ange_end_;
+    bool inverted_;
+    int poly_=1;
+    ros::NodeHandle pnh_;
+    // sub driver topic(msg)
+    ros::Subscriber packet_sub_;
+    // pub laserscan message
+    ros::Publisher scan_pub_;
+
+    // dynamic param server
+    dynamic_reconfigure::Server<oleiPuckConfig> cfg_server_;
+    oleiPuckConfig config_;
+
+    // add vector for laserscan
+    std::vector<uint16_t> scanAngleVec_;
+    std::vector<uint16_t> scanRangeVec_;
+    std::vector<uint16_t> scanIntensityVec_;
+
+    std::vector<uint16_t> scanAngleInVec_;
+    std::vector<uint16_t> scanRangeInVec_;
+    std::vector<uint16_t> scanIntensityInVec_;
+
+    std::vector<float> scanRangeBuffer;
+    std::vector<float> scanintensitiesBuffer;
+    uint16_t azimuthLast_;
+    uint16_t azimuthNow_;
+    uint16_t azimuthFirst_;
+
+    // laserscan msg
+    uint32_t scanMsgSeq_;
+    //电机频率
+    float frequency;
+    //雷达型号类型
+    unsigned char lidarType=0x01; 
+    //电机方向定义
+    int direction;
+
+    ros::Time log_time_ = ros::Time::now();
+
+    std::shared_ptr<Driver> drv_;
+  };
+
+  Decoder::Decoder(const ros::NodeHandle &pnh)
+      : pnh_(pnh), cfg_server_(pnh)
+  {
+    // get param from cfg file at node start
+    pnh_.param<std::string>("frame_id", frame_id_, "olelidar");
+    pnh_.param<int>("r_max", range_max_, 30);
+    ROS_INFO("===========================");
+    ROS_INFO("Frame_id: %s", frame_id_.c_str());
+    ROS_INFO("Topic: /%s/scan", frame_id_.c_str());
+    ROS_INFO("Range: [%d ~ %d] mm",range_min_, range_max_*1000);
+    ROS_INFO("===========================");
+    start = ros::Time::now();
+    // dynamic callback
+    cfg_server_.setCallback(boost::bind(&Decoder::ConfigCb, this, _1, _2));
+    // packet receive
+    azimuthLast_ = 0;
+    azimuthNow_ = 0;
+    azimuthFirst_ = 0xFFFF;
+    // laserscan msg init
+    scanMsgSeq_ = 0;
+    direction = 0;
+
+    drv_ = std::make_shared<Driver>(pnh);
+    
+    scan_pub_ = pnh_.advertise<LaserScan>("scan", 100);
+#ifdef DRIVER_MODULE
+    packet_sub_ = pnh_.subscribe<oleiPacket>("packet", 100, &Decoder::PacketCb, this, ros::TransportHints().tcpNoDelay(true));
+#endif
+    drv_->setCallback(std::bind(&Decoder::PacketCb, this, std::placeholders::_1));
+
+    ROS_INFO("Drive Ver:2.0.11");
+    ROS_INFO("Decoder initialized");
+
+  }
+
+  void Decoder::PublishMsg(ros::Publisher *pub, std::vector<uint16_t> &packet_r, std::vector<uint16_t> &packet_i, ros::Time lidar_time)
+  {
+    sensor_msgs::LaserScan scanMsg;
+    /*
+  std_msgs/Header header
+        uint32 seq
+        time stamp
+        string frame_id
+  float32 angle_min
+  float32 angle_max
+  float32 angle_increment
+  float32 time_increment
+  float32 scan_time
+  float32 range_min
+  float32 range_max
+  float32[] ranges
+  float32[] intensities
+  */
+
+    int min = config_.angle_min * 100 + 18000;
+    int max = config_.angle_max * 100 + 18000;
+
+    scanRangeBuffer.clear();
+    scanintensitiesBuffer.clear();
+
+    for (uint16_t i = 0; i < scanAngleInVec_.size(); i++)
+    {
+      // 过滤出指定角度范围内点云
+      int angle = scanAngleInVec_[i];
+      if (angle >= min && angle <= max && i % poly_ == 0)
+      {
+        float range = scanRangeInVec_[i] * 0.001f;
+        float intensities = scanIntensityInVec_[i] * 1.0f;
+
+        scanRangeBuffer.push_back(range);
+        scanintensitiesBuffer.push_back(intensities);
+      }
+    }
+
+    float bufferlen = scanRangeBuffer.size();
+    scanMsg.ranges.resize(bufferlen);
+    scanMsg.intensities.resize(bufferlen);
+
+	if(direction ==0)		//电机旋转顺时针时，转换右手坐标系法则
+	{
+		reverse(scanRangeBuffer.begin(),scanRangeBuffer.end());
+		reverse(scanintensitiesBuffer.begin(),scanintensitiesBuffer.end());
+	}
+	
+    for (uint16_t i = 0; i < bufferlen; i++)
+    {
+        scanMsg.ranges[i] = scanRangeBuffer[i];
+        scanMsg.intensities[i] = scanintensitiesBuffer[i];
+    }
+
+    float len = scanMsg.ranges.size();
+    //扫描顺序自增ID序列
+    scanMsg.header.seq = scanMsgSeq_;
+    scanMsgSeq_++;
+    //激光数据时间戳
+    //scanMsg.header.stamp = lidar_time;
+    scanMsg.header.stamp = ros::Time::now();
+    //FrameId
+    scanMsg.header.frame_id = frame_id_.c_str();
+    //定义开始角度和结束角度
+    scanMsg.angle_min = deg2rad(config_.angle_min);
+    scanMsg.angle_max = deg2rad(config_.angle_max);
+    //定义测距的最小值和最大值单位:m
+    scanMsg.range_min = config_.range_min;
+    scanMsg.range_max = config_.range_max;
+    
+    float step=(config_.angle_max - config_.angle_min)/len;
+    //ROS_INFO("len:%f  step:%f",len,step);
+    //角度分辨率
+    scanMsg.angle_increment = deg2rad(step);
+    //扫描的时间间隔
+    scanMsg.scan_time = 1/frequency;
+    //时间分辨率（相邻两个角度之间耗费时间）
+    scanMsg.time_increment = scanMsg.scan_time/float(len);
+ 	//修正为雷达内部时钟间隔
+ 	//scanMsg.scan_time = scantime*0.001f;		//雷达时间戳为毫秒计数，故而转为秒单位应乘以0.001
+	//scanMsg.time_increment = scanMsg.scan_time/float(len);
+	
+    //ROS_INFO("scan_time:%f   time_increment:%f",scanMsg.scan_time,scanMsg.time_increment);
+	
+	
+    uint16_t size=scanAngleInVec_.size();
+    uint16_t fb=360/(config_.step);
+    if (fb==size){
+      pub->publish(scanMsg); //校验当符合点数完整的一帧数据才向外发布话题
+    }
+    else{
+      if(scanMsgSeq_==1) return;
+      ROS_INFO("pointCloud frame:%d  size:%d  scanMsgSeq_:%d",fb,size,scanMsgSeq_);
+    }
+  }
+
+  void Decoder::DecodeAndFill(const Packet *const packet_buf, uint64_t time)
+  {
+
+    // For each data block, 150 total
+    uint16_t azimuth;
+    uint16_t range;
+    uint16_t intensity;
+    for (int iblk = 0; iblk < kBlocksPerPacket; ++iblk)
+    {
+      const auto &block = packet_buf->blocks[iblk];
+
+      // simple loop
+      azimuth = block.sequences[0].points[0].azimuth;
+      range = block.sequences[0].points[0].distance;
+      intensity = block.sequences[0].points[0].reflectivity;
+      //排除异常点云数据
+      if (range > range_max_*1000 || range < range_min_)
+      {
+        range = 0;
+        intensity = 0;
+      }
+      // azimuth ge 36000 is not valid
+      if (azimuth < 0xFF00)
+      {
+        scanAngleVec_.push_back(azimuth);
+        scanRangeVec_.push_back(range);
+        scanIntensityVec_.push_back(intensity);
+      }
+    }
+  }
+
+  void Decoder::PacketCb(const oleiPacketConstPtr &packet_msg)
+  {
+    ros::Time now = ros::Time::now();
+    ros::Duration delta_t = now - log_time_;
+    log_time_ = now;
+    
+    const auto *packet_buf = reinterpret_cast<const Packet *>(&(packet_msg->data[0]));
+    azimuthNow_ = packet_buf->blocks[0].sequences[0].points[0].azimuth;
+    //取得第一个数据包
+    if (azimuthFirst_ == 0xFFFF)
+    {
+      //雷达型号类型
+      lidarType = packet_buf->head.code[1];
+      azimuthFirst_ = azimuthNow_;
+      //取得转速
+      int rpm = (packet_buf->head.rpm) & 0x7FFF;
+      //取得电机旋转方向
+      direction = (packet_buf->head.rpm) >> 15;
+      ROS_INFO("rpm:%d  direction:%d  lidarType:%d", rpm, direction,lidarType);
+      //是否启用倒装设定
+      if (inverted_) direction = !direction;
+    }
+
+    if (azimuthLast_ < azimuthNow_)
+    {
+      DecodeAndFill(packet_buf, packet_msg->stamp.toNSec());
+      azimuthLast_ = azimuthNow_;
+      return;
+    }
+    else
+    {
+      azimuthLast_ = azimuthNow_;
+    }
+    // scan first half route
+    if (azimuthFirst_ >= 200)
+    {
+      azimuthFirst_ = azimuthNow_;
+      return;
+    }
+    //雷达时间戳
+    uint32_t nowstamp = packet_buf->head.timestamp;
+    ros::Time lidar_time = packet_msg->stamp;
+    
+#ifdef TIMESTAMP_DEBUG
+    if(!is_time_base_) {
+      local_timestamp_base_ = ros::Time::now();
+      inner_timestamp_base_ = nowstamp;
+      lidar_time = local_timestamp_base_;
+      is_time_base_ = true;
+    } else {
+      uint32_t delta_time = nowstamp - inner_timestamp_base_;
+      ros::Duration dur_time;
+      ros::Duration delta_t = dur_time.fromNSec(delta_time*1000000);
+      lidar_time = local_timestamp_base_ + delta_t;
+      //ROS_INFO_STREAM("ros timestamp:" << delta_time << "," << delta_t << "," << lidar_time);
+    }
+#endif
+    scantime = nowstamp - laststamp;
+    laststamp = nowstamp;
+    //ROS_INFO("inner timestamp: %u, %d", nowstamp, scantime);
+
+    //雷达工作频率
+    if(frequency<0.001){
+       lidarType = packet_buf->head.code[1]; //雷达型号类型
+		if(lidarType==0x01){
+		  int rpm = (packet_buf->head.rpm) & 0x7FFF;
+		  frequency = rpm / 60.0;
+		  config_.step=0.225;    //当雷达型号为0x01类型时，角分辨率为固定值
+		}
+		else{    
+		      if(scanAngleVec_.size()>2) {
+					//角度分辨率
+					config_.step = (scanAngleVec_[1]-scanAngleVec_[0])/100.0;
+					frequency = config_.step * 10000.0/60.0; 
+				}
+				else{
+					return;
+				}
+		}
+		ROS_INFO("frequency: %.0f hz  config_.step:%f",frequency,config_.step);
+    }
+
+	//当启用NTP服务时，时间戳重定向为NTP服务时间
+	if(packet_buf->head.rsv>0){
+		ros::Time ntp(packet_buf->head.rsv, packet_buf->head.timestamp);
+		lidar_time=ntp;
+	}
+    scanAngleInVec_.clear();
+    scanRangeInVec_.clear();
+    scanIntensityInVec_.clear();
+
+    scanAngleInVec_.assign(scanAngleVec_.begin(), scanAngleVec_.end());
+    scanRangeInVec_.assign(scanRangeVec_.begin(), scanRangeVec_.end());
+    scanIntensityInVec_.assign(scanIntensityVec_.begin(), scanIntensityVec_.end());
+
+    scanAngleVec_.clear();
+    scanRangeVec_.clear();
+    scanIntensityVec_.clear();
+
+    //抛出点云数据
+    PublishMsg(&scan_pub_, scanRangeInVec_, scanIntensityInVec_, lidar_time);
+
+    //解码原始数据包
+    DecodeAndFill(packet_buf, packet_msg->stamp.toNSec());
+
+    //ROS_INFO("Time: %f", (ros::Time::now() - start).toSec());
+  }
+
+  void Decoder::ConfigCb(oleiPuckConfig &config, int level)
+  {
+    // config.min_range = std::min(config.min_range, config.max_range);
+    //config.route =4000;
+    pnh_.param<int>("ang_start", ange_start_, 0);
+    pnh_.param<int>("ang_end", ange_end_, 360);
+    pnh_.param<int>("r_max", range_max_, 50);
+    pnh_.param<bool>("inverted", inverted_, false);
+    config.angle_min = ange_start_;
+    config.angle_max = ange_end_;
+    config.range_min = range_min_*0.001;
+    config.range_max = range_max_;
+    config_ = config;
+
+
+    if (level < 0){
+      // topic = scan, msg = LaserScan, queuesize = 1000
+      scan_pub_ = pnh_.advertise<LaserScan>("scan", 100);
+#ifdef DRIVER_MODULE
+      packet_sub_ = pnh_.subscribe<oleiPacket>("packet", 100, &Decoder::PacketCb, this);
+#endif
+    }
+  }
+} // namespace olelidar
+
+int main(int argc, char **argv)
+{
+  ros::init(argc, argv, "olelidar_decoder");
+  ros::NodeHandle pnh("~");
+  olelidar::Decoder node(pnh);
+  ros::spin();
+}

Devices/Packages/olei_lidar_driver_ros/olelidar/src/driver.cpp

@@ -0,0 +1,285 @@
+#include <arpa/inet.h>
+#include <errno.h>
+#include <fcntl.h>
+#include <netinet/in.h>
+#include <poll.h>
+#include <sys/file.h>
+#include <sys/socket.h>
+#include <unistd.h>
+#include <cmath>
+#include <functional>
+#include <thread>
+
+#include "constants.h"
+
+#include <diagnostic_updater/diagnostic_updater.h>
+#include <diagnostic_updater/publisher.h>
+#include <olei_msgs/oleiPacket.h>
+#include <olei_msgs/oleiScan.h>
+
+// here maskoff waring of macro 'ROS_LOG'
+#pragma clang diagnostic ignored "-Wzero-as-null-pointer-constant"
+
+namespace olelidar
+{
+
+  using namespace olei_msgs;
+  using namespace diagnostic_updater;
+
+  /// Constants
+  //static constexpr uint16_t kUdpPort = 2368;
+  static constexpr size_t kPacketSize = sizeof(oleiPacket().data);
+  static constexpr int kError = -1;
+
+  class Driver
+  {
+    using data_cb_t =  std::function<void(const oleiPacketConstPtr&)>;
+  public:
+    explicit Driver(const ros::NodeHandle &pnh);
+    ~Driver();
+
+    bool Poll();
+
+    void setCallback(const data_cb_t &cb) { data_cb_ = cb; }
+
+  private:
+    bool OpenUdpPort();
+    int ReadPacket(oleiPacket &packet);
+
+    // Ethernet relate variables
+    std::string device_ip_str_;
+    std::string local_ip_str_;
+    std::string multiaddr_ip_str_;
+    int device_port_;
+    in_addr device_ip_;
+    int socket_id_{-1};
+    ros::Time last_time_;
+
+    // ROS related variables
+    ros::NodeHandle pnh_;
+    ros::Publisher pub_packet_;
+
+    // Diagnostics updater
+    diagnostic_updater::Updater updater_;
+    boost::shared_ptr<diagnostic_updater::TopicDiagnostic> topic_diag_;
+    std::vector<oleiPacket> buffer_;
+    // double freq_;
+    
+    // raw data callback
+    data_cb_t data_cb_{nullptr};
+    std::thread data_thr_;
+    bool is_loop_{false};
+  };
+
+  Driver::Driver(const ros::NodeHandle &pnh) 
+    : pnh_(pnh)
+    , last_time_(ros::Time::now())
+  {
+    ROS_INFO("packet size: %zu", kPacketSize);
+    pnh_.param("device_ip", device_ip_str_, std::string("192.168.1.122"));
+    pnh_.param("device_port", device_port_, 2369);
+    pnh_.param("local_ip", local_ip_str_, std::string("192.168.1.99"));
+    pnh_.param("multiaddr", multiaddr_ip_str_, std::string(""));
+    ROS_INFO("device_ip: %s:%d", device_ip_str_.c_str(), device_port_);
+
+    if (inet_aton(device_ip_str_.c_str(), &device_ip_) == 0)
+    {
+      // inet_aton() returns nonzero if the address is valid, zero if not.
+      ROS_FATAL("Invalid device ip: %s:%d", device_ip_str_.c_str(), device_port_);
+      ros::shutdown();
+    }
+
+    // Output
+    pub_packet_ = pnh_.advertise<oleiPacket>("packet", 10);
+
+    if (!OpenUdpPort()) ROS_ERROR("Failed to open UDP Port"); 
+     
+    data_thr_ = std::move(std::thread( [&] {
+      is_loop_ = true;
+      while (is_loop_) {
+          if(!is_loop_) break; 
+          Poll();
+      }
+    })); 
+    ROS_INFO("Successfully init olelidar driver");
+  }
+
+  Driver::~Driver()
+  {
+    is_loop_ = false;
+    if(data_thr_.joinable()) {
+      data_thr_.join();
+    }
+    if (close(socket_id_)) {
+      ROS_INFO("Close socket %d at %s", socket_id_, device_ip_str_.c_str());
+    }
+    else {
+      ROS_ERROR("Failed to close socket %d at %s", socket_id_,device_ip_str_.c_str());
+    }
+  }
+
+  bool Driver::OpenUdpPort()
+  {
+    socket_id_ = socket(AF_INET, SOCK_DGRAM, 0);
+    if (socket_id_ == -1) {
+      perror("socket");
+      ROS_ERROR("Failed to create socket");
+      return false;
+    }
+
+    sockaddr_in my_addr;                              // my address information
+    memset(&my_addr, 0, sizeof(my_addr));             // initialize to zeros
+    my_addr.sin_family = AF_INET;                     // host byte order
+    my_addr.sin_port = htons(uint16_t(device_port_)); // short, in network byte order
+    my_addr.sin_addr.s_addr = INADDR_ANY;            // automatically fill in my IP
+
+
+    if (bind(socket_id_, (sockaddr *)&my_addr, sizeof(sockaddr)) == -1)
+    {
+      perror("bind"); // TODO: ROS_ERROR errno
+      ROS_ERROR("Failed to bind to socket %d", socket_id_);
+      return false;
+    }
+    if(multiaddr_ip_str_!="")
+    {
+    //加入组播才能接受到组播信息
+    struct ip_mreq mreq;
+    mreq.imr_multiaddr.s_addr = inet_addr(multiaddr_ip_str_.c_str());
+    mreq.imr_interface.s_addr = inet_addr(local_ip_str_.c_str());
+    int err=setsockopt(socket_id_,IPPROTO_IP,IP_ADD_MEMBERSHIP,&mreq,sizeof(mreq));
+    ROS_INFO("AddMultiaddr:%s  local:%s   =>%d ",multiaddr_ip_str_.c_str(),local_ip_str_.c_str(),err);
+    }   
+    if (fcntl(socket_id_, F_SETFL, O_NONBLOCK | FASYNC) < 0)
+    {
+      perror("non-block");
+      ROS_ERROR("Failed to set socket to non-blocking");
+      return false;
+    }
+    return true;
+  }
+
+ 
+
+  int Driver::ReadPacket(oleiPacket &packet)
+  {
+    ros::Time time_before = ros::Time::now();
+
+    struct pollfd fds[1];
+    fds[0].fd = socket_id_;
+    fds[0].events = POLLIN;
+    const int timeout_ms = 500; // one second (in msec)
+
+    sockaddr_in sender_address;
+    socklen_t sender_address_len = sizeof(sender_address);
+
+    while (true)
+    {
+      do
+      {
+        const int retval = poll(fds, 1, timeout_ms);
+        if (retval < 0) {
+          if (errno != EINTR) ROS_ERROR("poll() error: %s", strerror(errno));
+          return kError;
+        }
+        else if (retval == 0)
+        {
+          ROS_WARN("olamlidar poll() timeout");
+          return kError;
+        }
+
+        if ((fds[0].revents & POLLERR) || (fds[0].revents & POLLHUP) || (fds[0].revents & POLLNVAL))
+        {
+          ROS_ERROR("poll() reports olamlidar error");
+          return kError;
+        }
+      } while ((fds[0].revents & POLLIN) == 0);
+
+      // Receive packets that should now be available from the
+      // socket using a blocking read.
+      const ssize_t nbytes = recvfrom(socket_id_, &packet.data[0], kPacketSize, 0, (sockaddr *)&sender_address, &sender_address_len);
+      
+      if (nbytes < 0)
+      {
+        if (errno != EWOULDBLOCK)
+        {
+          perror("recvfail");
+          ROS_ERROR("Failed to read from socket");
+          return kError;
+        }
+      }
+      else if (static_cast<size_t>(nbytes) == kPacketSize)
+      {
+        //else if ((size_t)nbytes == kPacketSize) {
+        // read successful,
+        // if packet is not from the lidar scanner we selected by IP,
+        // continue otherwise we are done
+        //ROS_ERROR("Failed to read from socket");
+
+        //ROS_INFO("sender:%d   device:%d",sender_address.sin_addr.s_addr,device_ip_.s_addr); 
+        if (device_ip_str_ != "" && sender_address.sin_addr.s_addr != device_ip_.s_addr)
+          continue;
+        else
+          break; // done
+      }
+
+      ROS_DEBUG_STREAM("incomplete olei packet read: " << nbytes << " bytes");
+    }
+
+    packet.stamp = time_before;
+
+#ifdef TIMESTAMP_DEBUG
+    ros::Duration delta = time_before - last_time_;
+    ROS_INFO_STREAM("raw data delta time: " << time_before << "," << delta.toSec()*1000);
+    last_time_ = time_before;
+#endif
+
+    return 0;
+  }
+
+  bool Driver::Poll()
+  {
+    oleiPacket::Ptr packet(new oleiPacket);
+
+    while (true)
+    {
+      // keep reading until full packet received
+      const int rc = ReadPacket(*packet);
+      if (rc == 0)
+        break; // got a full packet?
+      if (rc < 0)
+        return false; // end of file reached?
+    }
+
+    // publish message using time of last packet read
+#ifdef DRIVER_MODULE
+    pub_packet_.publish(packet);
+#else
+    data_cb_(packet);
+#endif
+
+    return true;
+  }
+
+} // namespace olelidar
+
+#ifdef DRIVER_MODULE
+int main(int argc, char **argv)
+{
+  ros::init(argc, argv, "olelidar_driver");
+  ros::NodeHandle pnh("~");
+
+  olelidar::Driver node(pnh);
+
+  while (ros::ok())
+  {
+    // poll device until end of file
+    if (!node.Poll())
+    {
+      ROS_WARN("Failed to poll device");
+      //break;
+    }
+    ros::spinOnce();
+  }
+  return 0;
+}
+#endif

Devices/Packages/ros_kinematics/.gitignore

@@ -0,0 +1,414 @@
+# ---> 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
+

Devices/Packages/ros_kinematics/CMakeLists.txt

@@ -0,0 +1,239 @@
+cmake_minimum_required(VERSION 3.0.2)
+project(ros_kinematics)
+
+## Compile as C++11, 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
+  pluginlib
+  models
+  geometry_msgs
+  std_msgs
+  dynamic_reconfigure
+  control_msgs
+  realtime_tools
+  urdf
+  tf
+  tf2
+  tf2_ros
+)
+
+## 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):ros_plugins
+##   * 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
+#   geometry_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/SteerDriveParameters.cfg 
+  cfg/DiffDriveParameters.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 ros_kinematics
+  CATKIN_DEPENDS 
+    geometry_msgs 
+    roscpp 
+    rospy 
+    models 
+    pluginlib 
+    tf 
+    tf2 
+    tf2_ros
+    dynamic_reconfigure 
+    control_msgs
+    realtime_tools
+  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}
+  src/ros_steer_drive.cpp
+  src/ros_steer_drive_parameters.cpp
+  src/ros_diff_drive_parameters.cpp
+  src/odometry.cpp
+  src/speed_limiter.cpp
+  src/ros_diff_drive.cpp
+  ##src/ros_diff_drive_controller.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/ros_kinematics_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}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Specify libraries to link a library or executable target against
+
+target_link_libraries(${PROJECT_NAME}
+  ${Boost_LIBRARIES}
+  ${catkin_LIBRARIES}
+)
+target_link_libraries(${PROJECT_NAME}_node
+  ${PROJECT_NAME}
+  ${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}
+  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(DIRECTORY
+  launch
+  config
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+)
+
+#############
+## Testing ##
+#############
+
+## Add gtest based cpp test target and link libraries
+# catkin_add_gtest(${PROJECT_NAME}-test test/test_ros_kinematics.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/Packages/ros_kinematics/README.md

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

Devices/Packages/ros_kinematics/cfg/DiffDriveParameters.cfg

@@ -0,0 +1,18 @@
+#!/usr/bin/env python
+
+PACKAGE = 'ros_kinematics'
+
+from dynamic_reconfigure.parameter_generator_catkin import ParameterGenerator, bool_t, double_t
+
+gen = ParameterGenerator()
+
+# Kinematic parameters related
+gen.add("left_wheel_radius_multiplier", double_t, 0, "Left wheel radius multiplier.", 1.0, 0.5, 1.5)
+gen.add("right_wheel_radius_multiplier", double_t, 0, "Right wheel radius multiplier.", 1.0, 0.5, 1.5)
+gen.add("wheel_separation_multiplier", double_t, 0, "Wheel separation multiplier.", 1.0, 0.5, 1.5)
+
+# Publication related
+gen.add("publish_rate", double_t, 0, "Publish rate of odom.", 50.0, 0.0, 2000.0)
+gen.add("enable_odom_tf", bool_t, 0, "Publish odom frame to tf.", False)
+
+exit(gen.generate(PACKAGE, "ros_kinematics", "DiffDriveParameters"))

Devices/Packages/ros_kinematics/cfg/SteerDriveParameters.cfg

@@ -0,0 +1,11 @@
+#!/usr/bin/env python
+from dynamic_reconfigure.parameter_generator_catkin import ParameterGenerator, double_t
+
+gen = ParameterGenerator()
+
+gen.add('odomEncSteeringAngleOffset', double_t, 0, 'Góc bẻ lái động cơ lái được offset so với gốc origin ', 1.757546557, 0.0, 6.283185)
+gen.add('steering_fix_wheel_distance_x', double_t, 0, 'Khoảng cách tâm O quay quanh trục Z động cơ Steer theo trục X',  1.3268, 0.0, 5.0)
+gen.add('steering_fix_wheel_distance_y', double_t, 0, 'Khoảng cách tâm O quay quanh trục Z động cơ Steer theo trục X', 0.0, 0.0, 5.0)
+gen.add('wheelAcceleration', double_t, 0, 'Gia tốc bánh kéo', 0.0, 0.0, 3.0)
+
+exit(gen.generate('ros_kinematics', 'ros_kinematics', 'SteerDriveParameters'))

Devices/Packages/ros_kinematics/config/ros_diff_drive_controller.yaml

@@ -0,0 +1,64 @@
+# -----------------------------------
+left_wheel  : 'left_wheel_joint'
+right_wheel : 'right_wheel_joint'
+publish_rate: 60              # this is what the real MiR platform publishes (default: 50)
+# These covariances are exactly what the real MiR platform publishes
+pose_covariance_diagonal : [0.00001, 0.00001, 1000000.0, 1000000.0, 1000000.0, 1000.0]
+twist_covariance_diagonal: [0.1, 0.1, 1000000.0, 1000000.0, 1000000.0, 1000.0]
+enable_odom_tf: false
+# open_loop: false
+# Wheel separation and diameter. These are both optional.
+# diff_drive_controller will attempt to read either one or both from the
+# URDF if not specified as a parameter.
+# We don't set the value here because it's different for each MiR type (100, 250, ...), and
+# the plugin figures out the correct values.
+wheel_separation : 0.445208
+wheel_radius : 0.0625
+
+# Wheel separation and radius multipliers
+wheel_separation_multiplier: 1.0 # default: 1.0
+wheel_radius_multiplier    : 1.0 # default: 1.0
+
+# Velocity commands timeout [s], default 0.5
+cmd_vel_timeout: 0.5
+
+# frame_ids (same as real MiR platform)
+base_frame_id: base_footprint # default: base_link base_footprint
+odom_frame_id: odom           # default: odom 
+
+# Velocity and acceleration limits
+# Whenever a min_* is unspecified, default to -max_*
+linear:
+  x:
+    has_velocity_limits    : true
+    max_velocity           : 1.5  # m/s; move_base max_vel_x: 0.8
+    min_velocity           : -1.5 # m/s
+    has_acceleration_limits: true
+    max_acceleration       : 2.0  # m/s^2; move_base acc_lim_x: 1.5
+    min_acceleration       : -2.0 # m/s^2
+    has_jerk_limits        : true
+    max_jerk               : 3.0  # m/s^3
+angular:
+  z:
+    has_velocity_limits    : true
+    max_velocity           : 2.0  # rad/s; move_base max_rot_vel: 1.0
+    has_acceleration_limits: true
+    max_acceleration       : 1.5 # rad/s^2; move_base acc_lim_th: 2.0
+    has_jerk_limits        : true
+    max_jerk               : 2.5  # rad/s^3
+
+left_wheel_joint: 
+  lookup_name: WheelPlugin
+  max_publish_rate: 50
+  mesurement_topic: left_encoder
+  wheel_topic: left_wheel
+  subscribe_queue_size: 1
+  command_timeout: 5.0
+
+right_wheel_joint:  
+  lookup_name: WheelPlugin
+  max_publish_rate: 50
+  mesurement_topic: right_encoder
+  wheel_topic: right_wheel
+  subscribe_queue_size: 1
+  command_timeout: 5.0

Devices/Packages/ros_kinematics/config/ros_steer_drive.yaml

@@ -0,0 +1,29 @@
+commandTopic: $(arg prefix)cmd_vel
+odometryTopic: odom
+odometryFrame: $(arg prefix)odom
+odometryEncTopic: odom_enc
+odometryEncChildFrame: $(arg prefix)odom_enc
+robotBaseFrame: $(arg prefix)base_link
+steerChildFrame: $(arg prefix)steer_link
+subscribe_queue_size: 1
+max_update_rate: 60
+odomEncSteeringAngleOffset: 0.0
+steering_fix_wheel_distance_x: 0.0
+steering_fix_wheel_distance_y: 0.0
+wheelAcceleration: 0.0
+use_abs_encoder: false
+
+steer_drive: tzbot_motor_wheel::SteerMotor
+SteerMotor:
+  ratio: 171
+  node_id: node1
+  max_publish_rate: 100
+  subscribe_queue_size: 1
+
+traction_drive: tzbot_motor_wheel::TractionMotor
+TractionMotor:
+  wheel_diameter: 0.210
+  ratio: 24.68
+  node_id: node2
+  max_publish_rate: 100
+  subscribe_queue_size: 1

Devices/Packages/ros_kinematics/include/ros_kinematics/base_drive.h

@@ -0,0 +1,41 @@
+#ifndef _ROS_KINEMATICS_BASE_DRIVE_H_INCLUDED_
+#define _ROS_KINEMATICS_BASE_DRIVE_H_INCLUDED_
+
+#include <realtime_tools/realtime_buffer.h>
+
+namespace ros_kinematics
+{
+  class BaseDrive
+  {
+  protected:
+    class DriveCmd
+    {
+    struct Linear_st
+    {
+      double x; 
+      double y;
+      double z;
+      Linear_st() : x(0.0), y(0.0), z(0.0) {}
+    };
+
+    struct Angular_st
+    {
+      double x; 
+      double y;
+      double z;
+      Angular_st() : x(0.0), y(0.0), z(0.0) {}
+    };
+    
+    public:
+      Linear_st linear;
+      Angular_st angular;
+      ros::Time stamp;
+      DriveCmd() : stamp(0.0) {}
+    };
+
+    realtime_tools::RealtimeBuffer<DriveCmd> command_;
+    DriveCmd command_struct_;
+  };
+}
+
+#endif

Devices/Packages/ros_kinematics/include/ros_kinematics/odometry.h

@@ -0,0 +1,212 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2013, PAL Robotics, S.L.
+ *  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 PAL Robotics 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: Luca Marchionni
+ * Author: Bence Magyar
+ * Author: Enrique Fernández
+ * Author: Paul Mathieu
+ */
+
+#ifndef __ROS_KINEMATICS_ODOMETRY_H_INCLUDED_ 
+#define __ROS_KINEMATICS_ODOMETRY_H_INCLUDED_ 
+
+#include <ros/time.h>
+#include <boost/accumulators/accumulators.hpp>
+#include <boost/accumulators/statistics/stats.hpp>
+#include <boost/accumulators/statistics/rolling_mean.hpp>
+#include <boost/function.hpp>
+
+namespace ros_kinematics
+{
+  namespace bacc = boost::accumulators;
+
+  /**
+   * \brief The Odometry class handles odometry readings
+   * (2D pose and velocity with related timestamp)
+   */
+  class Odometry
+  {
+  public:
+
+    /// Integration function, used to integrate the odometry:
+    typedef boost::function<void(double, double)> IntegrationFunction;
+
+    /**
+     * \brief Constructor
+     * Timestamp will get the current time value
+     * Value will be set to zero
+     * \param velocity_rolling_window_size Rolling window size used to compute the velocity mean
+     */
+    Odometry(size_t velocity_rolling_window_size = 10);
+
+    /**
+     * \brief Initialize the odometry
+     * \param time Current time
+     */
+    void init(const ros::Time &time);
+
+    /**
+     * \brief Updates the odometry class with latest wheels position
+     * \param left_pos  Left  wheel position [rad]
+     * \param right_pos Right wheel position [rad]
+     * \param time      Current time
+     * \return true if the odometry is actually updated
+     */
+    bool update(double left_pos, double right_pos, const ros::Time &time);
+
+    /**
+     * \brief Updates the odometry class with latest velocity command
+     * \param linear  Linear velocity [m/s]
+     * \param angular Angular velocity [rad/s]
+     * \param time    Current time
+     */
+    void updateOpenLoop(double linear, double angular, const ros::Time &time);
+
+    /**
+     * \brief heading getter
+     * \return heading [rad]
+     */
+    double getHeading() const
+    {
+      return heading_;
+    }
+
+    /**
+     * \brief x position getter
+     * \return x position [m]
+     */
+    double getX() const
+    {
+      return x_;
+    }
+
+    /**
+     * \brief y position getter
+     * \return y position [m]
+     */
+    double getY() const
+    {
+      return y_;
+    }
+
+    /**
+     * \brief linear velocity getter
+     * \return linear velocity [m/s]
+     */
+    double getLinear() const
+    {
+      return fabs(linear_) > 1e-9? linear_ : 0.0;
+    }
+
+    /**
+     * \brief angular velocity getter
+     * \return angular velocity [rad/s]
+     */
+    double getAngular() const
+    {
+      return fabs(angular_) > 1e-9? angular_ : 0.0;;
+    }
+
+    /**
+     * \brief Sets the wheel parameters: radius and separation
+     * \param wheel_separation   Separation between left and right wheels [m]
+     * \param left_wheel_radius  Left wheel radius [m]
+     * \param right_wheel_radius Right wheel radius [m]
+     */
+    void setWheelParams(double wheel_separation, double left_wheel_radius, double right_wheel_radius);
+
+    /**
+     * \brief Velocity rolling window size setter
+     * \param velocity_rolling_window_size Velocity rolling window size
+     */
+    void setVelocityRollingWindowSize(size_t velocity_rolling_window_size);
+
+  private:
+
+    /// Rolling mean accumulator and window:
+    typedef bacc::accumulator_set<double, bacc::stats<bacc::tag::rolling_mean> > RollingMeanAcc;
+    typedef bacc::tag::rolling_window RollingWindow;
+
+    /**
+     * \brief Integrates the velocities (linear and angular) using 2nd order Runge-Kutta
+     * \param linear  Linear  velocity   [m] (linear  displacement, i.e. m/s * dt) computed by encoders
+     * \param angular Angular velocity [rad] (angular displacement, i.e. m/s * dt) computed by encoders
+     */
+    void integrateRungeKutta2(double linear, double angular);
+
+    /**
+     * \brief Integrates the velocities (linear and angular) using exact method
+     * \param linear  Linear  velocity   [m] (linear  displacement, i.e. m/s * dt) computed by encoders
+     * \param angular Angular velocity [rad] (angular displacement, i.e. m/s * dt) computed by encoders
+     */
+    void integrateExact(double linear, double angular);
+
+    /**
+     *  \brief Reset linear and angular accumulators
+     */
+    void resetAccumulators();
+
+    /// Current timestamp:
+    ros::Time timestamp_;
+
+    /// Current pose:
+    double x_;        //   [m]
+    double y_;        //   [m]
+    double heading_;  // [rad]
+
+    /// Current velocity:
+    double linear_;  //   [m/s]
+    double angular_; // [rad/s]
+
+    /// Wheel kinematic parameters [m]:
+    double wheel_separation_;
+    double left_wheel_radius_;
+    double right_wheel_radius_;
+
+    /// Previou wheel position/state [rad]:
+    double left_wheel_old_pos_;
+    double right_wheel_old_pos_;
+
+    /// Rolling mean accumulators for the linar and angular velocities:
+    size_t velocity_rolling_window_size_;
+    RollingMeanAcc linear_acc_;
+    RollingMeanAcc angular_acc_;
+
+    /// Integration funcion, used to integrate the odometry:
+    IntegrationFunction integrate_fun_;
+  };
+}
+
+#endif

Devices/Packages/ros_kinematics/include/ros_kinematics/ros_diff_drive.h

@@ -0,0 +1,194 @@
+#ifndef _ROS_KINEMATICS_DIFF_DRIVE_H_INCLUDED_
+#define _ROS_KINEMATICS_DIFF_DRIVE_H_INCLUDED_
+
+#include <ros/ros.h>
+#include <ros_kinematics/base_drive.h>
+#include <ros_kinematics/odometry.h>
+#include <ros_kinematics/speed_limiter.h>
+#include <ros_kinematics/ros_diff_drive_parameters.h>
+
+#include <memory>
+#include <realtime_tools/realtime_buffer.h>
+#include <realtime_tools/realtime_publisher.h>
+#include <control_msgs/JointTrajectoryControllerState.h>
+#include <geometry_msgs/Twist.h>
+#include <geometry_msgs/Pose2D.h>
+#include <nav_msgs/Odometry.h>
+#include <tf/transform_broadcaster.h>
+#include <tf/transform_listener.h>
+#include <tf2/LinearMath/Quaternion.h>
+#include <tf2_ros/transform_broadcaster.h>
+// Boost
+#include <boost/thread.hpp>
+// plugin
+#include <models/BaseSteerDrive.h>
+#include <models/BaseAbsoluteEncoder.h>
+#include <pluginlib/class_loader.h>
+
+namespace ros_kinematics
+{
+  class RosDiffDrive : public BaseDrive
+  {
+  public:
+    /**
+     * @brief Constructor
+     */
+    RosDiffDrive(ros::NodeHandle& nh, const std::string& name);
+
+    /**
+     * @brief Deconstructor
+     */
+    virtual ~RosDiffDrive();
+
+  private:
+
+    void CmdVelCallback(const geometry_msgs::Twist::ConstPtr& cmd_msg);
+    /**
+     * @brief returns true, if publishing of a cmd_vel topic is publishing
+     * 
+    */
+    bool isCmdVelTriggered(void);
+
+    /**
+     *  @brief returns true, if not subcribe cmd_vel
+    */
+    bool isCmdVelLastestTriggered(void);
+
+    /**
+     * @brief schedules MotorController function 
+     * @param[in] schedule if true, publishing is scheduled, otherwise a possibly pending schedule is removed.
+    */
+    void scheduleMotorController(bool schedule);
+    
+    void updateDynamicParams();
+    void updateChild(void);
+    void updateOdometryEncoder(const ros::Time& current_time);
+    void publishOdometry(const ros::Time& current_time);
+    void brake();
+    bool allReady();
+
+    /**
+     * @brief Sets the odometry publishing fields
+     * @param root_nh Root node handle
+     * @param controller_nh Node handle inside the controller namespace
+     */
+    void setOdomPubFields(ros::NodeHandle& root_nh, ros::NodeHandle& controller_nh);
+    
+    /**
+     * @brief Get the wheel names from a wheel param
+     * @param [in]  controller_nh Controller node handler
+     * @param [in]  wheel_param   Param name
+     * @param [out] wheel_names   Vector with the whel names
+     * @return true if the wheel_param is available and the wheel_names are
+     *        retrieved successfully from the param server; false otherwise
+     */
+    bool getWheelNames(ros::NodeHandle& controller_nh, const std::string& wheel_param, std::vector<std::string>& wheel_names);
+
+    bool getWheelParams(ros::NodeHandle& controller_nh, const std::vector<std::string>& wheel_names, 
+                          std::map<std::string, XmlRpc::XmlRpcValue>& drive_param_map);
+
+    // properties
+    bool initialized_;
+    std::string name_;
+    ros::NodeHandle nh_;
+    ros::NodeHandle nh_priv_;
+
+    pluginlib::ClassLoader<models::BaseSteerDrive> motor_loader_;
+    std::vector<boost::shared_ptr<models::BaseSteerDrive> > left_drive_;
+    std::vector<boost::shared_ptr<models::BaseSteerDrive> > right_drive_;
+    
+    std::map<std::string, XmlRpc::XmlRpcValue> left_drive_param_map_;
+    std::map<std::string, XmlRpc::XmlRpcValue> right_drive_param_map_;
+    boost::shared_ptr<realtime_tools::RealtimePublisher<tf::tfMessage> > left_wheel_tf_pub_;
+    boost::shared_ptr<realtime_tools::RealtimePublisher<tf::tfMessage> > right_wheel_tf_pub_;
+
+    bool use_abs_encoder;
+    pluginlib::ClassLoader<models::BaseAbsoluteEncoder> encoder_loader_;
+    std::vector<boost::shared_ptr<models::BaseAbsoluteEncoder> > left_encoder_;
+    std::vector<boost::shared_ptr<models::BaseAbsoluteEncoder> > right_encoder_;
+    double left_wheel_curr_pos_;
+    double right_wheel_curr_pos_;
+
+
+
+    ros::Subscriber cmd_vel_subscriber_;
+    int m_subscribe_queue_size_;
+    boost::shared_ptr<boost::thread> callback_thread_;
+    ros::Duration schedule_delay_;
+    ros::Duration schedule_lastest_delay_;
+    ros::Time publish_time_;
+    ros::Time publish_lastest_time_;
+    boost::mutex publish_mutex_;
+    /// Publish executed commands
+    boost::shared_ptr<realtime_tools::RealtimePublisher<geometry_msgs::TwistStamped> > cmd_vel_pub_;
+
+    ros_kinematics::Odometry odometry_;
+    ros::Duration publish_period_;
+    ros::Time last_odom_state_publish_time_;
+    bool open_loop_;
+    ros::Time last_odom_update_;
+    std::shared_ptr<realtime_tools::RealtimePublisher<nav_msgs::Odometry> > odom_pub_;
+    std::shared_ptr<realtime_tools::RealtimePublisher<tf::tfMessage> > tf_odom_pub_;
+
+    /// Wheel separation, wrt the midpoint of the wheel width:
+    double wheel_separation_;
+
+    /// Wheel radius (assuming it's the same for the left and right wheels):
+    double wheel_radius_;
+
+    /// Wheel separation and radius calibration multipliers:
+    double wheel_separation_multiplier_;
+    double left_wheel_radius_multiplier_;
+    double right_wheel_radius_multiplier_;
+
+    /// Timeout to consider cmd_vel commands old:
+    double cmd_vel_timeout_;
+
+    /// Whether to allow multiple publishers on cmd_vel topic or not:
+    bool allow_multiple_cmd_vel_publishers_;
+
+    /// Frame to use for the robot base:
+    std::string base_frame_id_;
+
+    /// Frame to use for odometry and odom tf:
+    std::string odom_frame_id_;
+
+    /// Whether to publish odometry to tf or not:
+    bool enable_odom_tf_;
+
+    /// Whether to publish wheel link to tf or not:
+    bool enable_wheel_tf_;
+
+    /// Number of wheel joints:
+    size_t wheel_joints_size_;
+    boost::shared_ptr<tf2_ros::TransformBroadcaster> transform_broadcaster_;
+
+    /// Speed limiters:
+    ros_kinematics::BaseDrive::DriveCmd last1_cmd_;
+    ros_kinematics::BaseDrive::DriveCmd last0_cmd_;
+    ros_kinematics::SpeedLimiter limiter_lin_;
+    ros_kinematics::SpeedLimiter limiter_ang_;
+
+    /// Publish limited velocity:
+    bool publish_cmd_;
+    // Update Rate
+    double update_rate_;
+
+    
+    /// Dynamic Reconfigure server
+    boost::shared_ptr<ros_kinematics::KinematicDiffDriveParameters> kinematics_param_ptr_;
+
+    std::string odometry_topic_;
+    std::string odometry_frame_;
+    std::string odometry_enc_topic_;
+    std::string odometry_enc_child_frame_;
+    std::string command_topic_;
+
+    // Flags
+    bool publishOdomTF_;
+
+    // bool publishWheelJointState_;
+    bool odom_enc_initialized_;
+  };
+}
+#endif // _ROS_KINEMATICS_DIFF_DRIVE_H_INCLUDED_

Devices/Packages/ros_kinematics/include/ros_kinematics/ros_diff_drive_parameters.h

@@ -0,0 +1,51 @@
+#ifndef __ROS_KINEMATICS_DIFF_DRIVE_PARAMETERS_H_INCLUDED_
+#define __ROS_KINEMATICS_DIFF_DRIVE_PARAMETERS_H_INCLUDED_
+
+#include <ros/ros.h>
+#include <dynamic_reconfigure/server.h>
+#include <ros_kinematics/DiffDriveParametersConfig.h>
+#include <memory>
+#include <boost/thread.hpp>
+#include <dynamic_reconfigure/Config.h>
+
+namespace ros_kinematics
+{
+/**
+ * @class KinematicDiffDriveParameters
+ * @brief A dynamically reconfigurable class containing one representation of the robot's kinematics
+ */
+class KinematicDiffDriveParameters
+{
+public:
+  KinematicDiffDriveParameters();
+  KinematicDiffDriveParameters(const ros::NodeHandle& nh);
+  void initialize(const ros::NodeHandle& nh);
+
+  inline double getLeftWheelRadiusMultiplier()    { return left_wheel_radius_multiplier_; }
+  inline double getRightWheelRadiusMultiplier()   { return right_wheel_radius_multiplier_; }
+  inline double getWheelSeparationMultiplier()    { return wheel_separation_multiplier_; }
+  inline double getPublishRate()                  { return publish_rate_; }
+  inline bool   getEnableOdomTf()                 { return enable_odom_tf_; }
+  inline double isSetup() {return setup_;}
+  using Ptr = std::shared_ptr<KinematicDiffDriveParameters>;
+protected:
+  void reconfigureCB(DiffDriveParametersConfig &config, uint32_t level);
+  void kinematicsCallback(const dynamic_reconfigure::Config::ConstPtr& param);
+  
+  bool initialized_;
+  bool setup_;
+  // For parameter descriptions, see cfg/KinematicParams.cfg
+  double left_wheel_radius_multiplier_;
+  double right_wheel_radius_multiplier_;
+  double wheel_separation_multiplier_;
+  double publish_rate_;
+  bool   enable_odom_tf_;
+
+  boost::mutex reconfig_mutex;
+  ros::NodeHandle nh_;
+  ros::Subscriber kinematics_sub_;
+  std::shared_ptr<dynamic_reconfigure::Server<DiffDriveParametersConfig> > dsrv_;
+};
+
+}  // namespace ros_kinematics
+#endif

Devices/Packages/ros_kinematics/include/ros_kinematics/ros_steer_drive.h

@@ -0,0 +1,118 @@
+#ifndef _ROS_KINEMATICS_STEER_DRIVE_H_INCLUDED_
+#define _ROS_KINEMATICS_STEER_DRIVE_H_INCLUDED_
+
+#include <ros/ros.h>
+#include <ros_kinematics/base_drive.h>
+#include <ros_kinematics/ros_steer_drive_parameters.h>
+
+#include <geometry_msgs/Twist.h>
+#include <geometry_msgs/Pose2D.h>
+#include <nav_msgs/Odometry.h>
+#include <tf/transform_broadcaster.h>
+#include <tf/transform_listener.h>
+
+// Boost
+#include <boost/thread.hpp>
+// plugin
+#include <models/BaseSteerDrive.h>
+#include <models/BaseAbsoluteEncoder.h>
+#include <pluginlib/class_loader.h>
+
+namespace ros_kinematics
+{
+  class RosSteerDrive : public BaseDrive
+  {
+  public:
+    RosSteerDrive(ros::NodeHandle & nh, const std::string &name);
+    virtual ~RosSteerDrive();
+  private:
+
+    void CmdVelCallback(const geometry_msgs::Twist::ConstPtr& cmd_msg);
+
+    /**
+     * @brief returns true, if publishing of a cmd_vel topic is publishing
+     * 
+    */
+    bool isCmdVelTriggered(void);
+
+    /**
+     *  @brief returns true, if not subcribe cmd_vel
+    */
+    bool isCmdVelLastestTriggered(void);
+
+    /**
+     * @brief schedules MotorController function 
+     * @param[in] schedule if true, publishing is scheduled, otherwise a possibly pending schedule is removed.
+    */
+    void scheduleMotorController(bool schedule);
+
+    void MotorController(double target_speed, double target_steering_speed, double dt);
+
+    void UpdateOdometryEncoder();
+    void UpdateChild(void);
+    void PublishOdometry(double step_time);
+    void publishSteerJoint(double odom_alpha);
+
+    pluginlib::ClassLoader<models::BaseSteerDrive> motor_loader_;
+    std::string steer_motor_mode_, traction_motor_mode_;
+    boost::shared_ptr<models::BaseSteerDrive> steer_motor_;
+    boost::shared_ptr<models::BaseSteerDrive> traction_motor_;
+
+    bool use_abs_encoder;
+    pluginlib::ClassLoader<models::BaseAbsoluteEncoder> encoder_loader_;
+    boost::shared_ptr<models::BaseAbsoluteEncoder> absolute_encoder_;    
+
+    ros::NodeHandle nh_;
+    ros::NodeHandle nh_priv_;
+
+    ros::Publisher odometry_publisher_;
+    ros::Publisher odometry_enc_publisher_;
+    ros::Publisher cmd_vel_feedback_;
+    ros::Publisher steer_angle_pub_;
+    ros::Publisher cmd_vel_rb;
+    ros::Subscriber cmd_vel_subscriber_;
+    boost::shared_ptr<tf::TransformBroadcaster> transform_broadcaster_;
+    int m_subscribe_queue_size_;
+    boost::mutex callback_mutex_;
+    boost::thread *callback_thread_;
+
+    nav_msgs::Odometry odom_;
+    nav_msgs::Odometry odom_enc_;
+    geometry_msgs::Pose2D pose_encoder_;
+    ros::Time last_odom_update_;
+
+    // Update Rate
+    double update_rate_;
+    double update_period_;
+    ros::Time last_actuator_update_;
+    ros::Time publish_time_;
+    ros::Time publish_lastest_time_;
+    ros::Duration schedule_delay_;
+    ros::Duration schedule_lastest_delay_;
+    boost::mutex publish_mutex_;
+
+    boost::shared_ptr<ros_kinematics::KinematicSteerDriveParameters> kinematics_param_ptr;
+    double steering_fix_wheel_distance_x_;
+    double steering_fix_wheel_distance_y_;
+    double odom_enc_steering_angle_offset_;
+
+    double wheel_diameter_;
+    double wheel_acceleration_;
+    double steering_ratio_;
+    double traction_ratio_;
+
+    std::string odometry_topic_;
+    std::string odometry_frame_;
+    std::string odometry_enc_topic_;
+    std::string odometry_enc_child_frame_;
+    std::string robot_base_frame_;
+    std::string command_topic_;
+    std::string steer_id_;
+
+    // Flags
+    bool publishOdomTF_;
+    // bool publishWheelJointState_;
+    bool odom_enc_initialized_;
+  };
+}
+#endif

Devices/Packages/ros_kinematics/include/ros_kinematics/ros_steer_drive_parameters.h

@@ -0,0 +1,48 @@
+#ifndef __ROS_KINEMATICS_STEER_DRIVE_PARAMETERS_H_INCLUDED_
+#define __ROS_KINEMATICS_STEER_DRIVE_PARAMETERS_H_INCLUDED_
+
+#include <ros/ros.h>
+#include <dynamic_reconfigure/server.h>
+#include <ros_kinematics/SteerDriveParametersConfig.h>
+#include <memory>
+#include <boost/thread.hpp>
+#include <dynamic_reconfigure/Config.h>
+
+namespace ros_kinematics
+{
+/**
+ * @class KinematicSteerDriveParameters
+ * @brief A dynamically reconfigurable class containing one representation of the robot's kinematics
+ */
+class KinematicSteerDriveParameters
+{
+public:
+  KinematicSteerDriveParameters();
+  KinematicSteerDriveParameters(const ros::NodeHandle& nh);
+  void initialize(const ros::NodeHandle& nh);
+
+  inline double getOdomEncSteeringAngleOffset() { return odom_enc_steering_angle_offset_; }
+  inline double getSteeringFixWheelDistanceX()  { return steering_fix_wheel_distance_x_; }
+  inline double getSteeringFixWheelDistanceY()  { return steering_fix_wheel_distance_y_; }
+  inline double getWheelAcceleration()          { return wheel_acceleration_; }
+  inline double isSetup() {return setup_;}
+  using Ptr = std::shared_ptr<KinematicSteerDriveParameters>;
+protected:
+  void reconfigureCB(SteerDriveParametersConfig &config, uint32_t level);
+  void kinematicsCallback(const dynamic_reconfigure::Config::ConstPtr& param);
+  
+  bool initialized_;
+  bool setup_;
+  // For parameter descriptions, see cfg/KinematicParams.cfg
+  double odom_enc_steering_angle_offset_;
+  double steering_fix_wheel_distance_x_;
+  double steering_fix_wheel_distance_y_;
+  double wheel_acceleration_;
+  boost::mutex reconfig_mutex;
+  ros::NodeHandle nh_;
+  ros::Subscriber kinematics_sub_;
+  std::shared_ptr<dynamic_reconfigure::Server<SteerDriveParametersConfig> > dsrv_;
+};
+
+}  // namespace ros_kinematics
+#endif

Devices/Packages/ros_kinematics/include/ros_kinematics/speed_limiter.h

@@ -0,0 +1,131 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2013, PAL Robotics, S.L.
+ *  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 PAL Robotics 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: Enrique Fernández
+ */
+
+#ifndef __ROS_KINEMATICS_SPEED_LIMITER_H_INCLUDED_
+#define __ROS_KINEMATICS_SPEED_LIMITER_H_INCLUDED_
+
+namespace ros_kinematics
+{
+
+  class SpeedLimiter
+  {
+  public:
+
+    /**
+     * \brief Constructor
+     * \param [in] has_velocity_limits     if true, applies velocity limits
+     * \param [in] has_acceleration_limits if true, applies acceleration limits
+     * \param [in] has_jerk_limits         if true, applies jerk limits
+     * \param [in] min_velocity Minimum velocity [m/s], usually <= 0
+     * \param [in] max_velocity Maximum velocity [m/s], usually >= 0
+     * \param [in] min_acceleration Minimum acceleration [m/s^2], usually <= 0
+     * \param [in] max_acceleration Maximum acceleration [m/s^2], usually >= 0
+     * \param [in] min_jerk Minimum jerk [m/s^3], usually <= 0
+     * \param [in] max_jerk Maximum jerk [m/s^3], usually >= 0
+     */
+    SpeedLimiter(
+      bool has_velocity_limits = false,
+      bool has_acceleration_limits = false,
+      bool has_jerk_limits = false,
+      double min_velocity = 0.0,
+      double max_velocity = 0.0,
+      double min_acceleration = 0.0,
+      double max_acceleration = 0.0,
+      double min_jerk = 0.0,
+      double max_jerk = 0.0
+    );
+
+    /**
+     * \brief Limit the velocity and acceleration
+     * \param [in, out] v  Velocity [m/s]
+     * \param [in]      v0 Previous velocity to v  [m/s]
+     * \param [in]      v1 Previous velocity to v0 [m/s]
+     * \param [in]      dt Time step [s]
+     * \return Limiting factor (1.0 if none)
+     */
+    double limit(double& v, double v0, double v1, double dt);
+
+    /**
+     * \brief Limit the velocity
+     * \param [in, out] v Velocity [m/s]
+     * \return Limiting factor (1.0 if none)
+     */
+    double limit_velocity(double& v);
+
+    /**
+     * \brief Limit the acceleration
+     * \param [in, out] v  Velocity [m/s]
+     * \param [in]      v0 Previous velocity [m/s]
+     * \param [in]      dt Time step [s]
+     * \return Limiting factor (1.0 if none)
+     */
+    double limit_acceleration(double& v, double v0, double dt);
+
+    /**
+     * \brief Limit the jerk
+     * \param [in, out] v  Velocity [m/s]
+     * \param [in]      v0 Previous velocity to v  [m/s]
+     * \param [in]      v1 Previous velocity to v0 [m/s]
+     * \param [in]      dt Time step [s]
+     * \return Limiting factor (1.0 if none)
+     * \see http://en.wikipedia.org/wiki/Jerk_%28physics%29#Motion_control
+     */
+    double limit_jerk(double& v, double v0, double v1, double dt);
+
+  public:
+    // Enable/Disable velocity/acceleration/jerk limits:
+    bool has_velocity_limits;
+    bool has_acceleration_limits;
+    bool has_jerk_limits;
+
+    // Velocity limits:
+    double min_velocity;
+    double max_velocity;
+
+    // Acceleration limits:
+    double min_acceleration;
+    double max_acceleration;
+
+    // Jerk limits:
+    double min_jerk;
+    double max_jerk;
+  };
+
+} // namespace ros_kinematics
+
+#endif

Devices/Packages/ros_kinematics/launch/diff_drive.launch

@@ -0,0 +1,14 @@
+<?xml version="1.0"?>
+<launch>
+
+  <arg name="ros_kinematics_yaml" default="$(find ros_kinematics)/config/ros_diff_drive_controller.yaml"/>
+  <arg name="use_encoder" default="false"/>
+  <arg name="type" default="2"/>
+  <node pkg="ros_kinematics" type="ros_kinematics_node" name="ros_kinematics" output="screen"> 
+    <rosparam file="$(arg ros_kinematics_yaml)" command="load" />
+    <param name="use_encoder" type="bool" value="$(arg use_encoder)"/>
+    <param name="type" type="int" value="$(arg type)"/>
+    <remap from="/ros_kinematics/odom" to="/odom" />
+  </node>
+   
+</launch>  

Devices/Packages/ros_kinematics/launch/steer_drive.launch

@@ -0,0 +1,13 @@
+<?xml version="1.0"?>
+<launch>
+
+  <arg name="ros_kinematics_yaml" default="$(find ros_kinematics)/config/ros_steer_drive.yaml"/>
+  <arg name="use_encoder" default="false"/>
+  <arg name="type" default="1"/>
+  <node pkg="ros_kinematics" type="ros_kinematics_node" name="ros_kinematics"> 
+    <rosparam file="$(arg ros_kinematics_yaml)" command="load" />
+    <param name="use_encoder" type="bool" value="$(arg use_encoder)"/>
+    <param name="type" type="int" value="$(arg type)"/>
+  </node>
+   
+</launch>  

Devices/Packages/ros_kinematics/package.xml

@@ -0,0 +1,96 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>ros_kinematics</name>
+  <version>0.0.0</version>
+  <description>The ros_kinematics 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/ros_kinematics</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>geometry_msgs</build_depend>
+  <build_depend>std_msgs</build_depend>
+  <build_depend>roscpp</build_depend>
+  <build_depend>rospy</build_depend>
+  <build_depend>models</build_depend>
+  <build_depend>pluginlib</build_depend>
+  <build_depend>tf</build_depend>
+  <build_depend>dynamic_reconfigure</build_depend>
+  <build_depend>tf2</build_depend>
+  <build_depend>tf2_ros</build_depend>
+  <build_depend>control_msgs</build_depend>
+  <build_depend>realtime_tools</build_depend>
+
+  <build_export_depend>geometry_msgs</build_export_depend>
+  <build_export_depend>std_msgs</build_export_depend>
+  <build_export_depend>roscpp</build_export_depend>
+  <build_export_depend>rospy</build_export_depend>
+  <build_export_depend>models</build_export_depend>
+  <build_export_depend>pluginlib</build_export_depend>
+  <build_export_depend>tf</build_export_depend>
+  <build_export_depend>dynamic_reconfigure</build_export_depend>
+  <build_export_depend>tf2</build_export_depend>
+  <build_export_depend>tf2_ros</build_export_depend>
+  <build_export_depend>control_msgs</build_export_depend>
+  <build_export_depend>realtime_tools</build_export_depend>
+
+  <exec_depend>geometry_msgs</exec_depend>
+  <exec_depend>std_msgs</exec_depend>
+  <exec_depend>roscpp</exec_depend>
+  <exec_depend>rospy</exec_depend>
+  <exec_depend>models</exec_depend>
+  <exec_depend>pluginlib</exec_depend>
+  <exec_depend>tf</exec_depend>
+  <exec_depend>dynamic_reconfigure</exec_depend>
+  <exec_depend>tf2</exec_depend>
+  <exec_depend>tf2_ros</exec_depend>
+  <exec_depend>control_msgs</exec_depend>
+  <exec_depend>realtime_tools</exec_depend>
+  
+  <!-- The export tag contains other, unspecified, tags -->
+  <export>
+    <!-- Other tools can request additional information be placed here -->
+  <!-- <controller_interface plugin="${prefix}/plugins.xml"/> -->
+  </export>
+</package>

Devices/Packages/ros_kinematics/src/odometry.cpp

@@ -0,0 +1,173 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2013, PAL Robotics, S.L.
+ *  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 PAL Robotics 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: Luca Marchionni
+ * Author: Bence Magyar
+ * Author: Enrique Fernández
+ * Author: Paul Mathieu
+ */
+#include <ros/ros.h>
+#include <ros_kinematics/odometry.h>
+
+namespace ros_kinematics
+{
+  namespace bacc = boost::accumulators;
+
+  Odometry::Odometry(size_t velocity_rolling_window_size)
+  : timestamp_(0.0)
+  , x_(0.0)
+  , y_(0.0)
+  , heading_(0.0)
+  , linear_(0.0)
+  , angular_(0.0)
+  , wheel_separation_(0.0)
+  , left_wheel_radius_(0.0)
+  , right_wheel_radius_(0.0)
+  , left_wheel_old_pos_(0.0)
+  , right_wheel_old_pos_(0.0)
+  , velocity_rolling_window_size_(velocity_rolling_window_size)
+  , linear_acc_(RollingWindow::window_size = velocity_rolling_window_size)
+  , angular_acc_(RollingWindow::window_size = velocity_rolling_window_size)
+  , integrate_fun_(std::bind(&Odometry::integrateExact, this, std::placeholders::_1, std::placeholders::_2))
+  {
+  }
+
+  void Odometry::init(const ros::Time& time)
+  {
+    // Reset accumulators and timestamp:
+    resetAccumulators();
+    timestamp_ = time;
+  }
+
+  bool Odometry::update(double left_pos, double right_pos, const ros::Time &time)
+  {
+    /// Get current wheel joint positions:
+    const double left_wheel_cur_pos  = left_pos  * left_wheel_radius_;
+    const double right_wheel_cur_pos = right_pos * right_wheel_radius_;
+    
+    /// Estimate velocity of wheels using old and current position:
+    const double left_wheel_est_vel  = left_wheel_cur_pos  - left_wheel_old_pos_;
+    const double right_wheel_est_vel = right_wheel_cur_pos - right_wheel_old_pos_;
+
+    /// Update old position with current:
+    left_wheel_old_pos_  = left_wheel_cur_pos;
+    right_wheel_old_pos_ = right_wheel_cur_pos;
+
+    /// Compute linear and angular diff:
+    const double linear  = (right_wheel_est_vel + left_wheel_est_vel) * 0.5 ;
+    const double angular = (right_wheel_est_vel - left_wheel_est_vel) / wheel_separation_;
+
+    /// Integrate odometry:
+    integrate_fun_(linear, angular);
+
+    /// We cannot estimate the speed with very small time intervals:
+    const double dt = (time - timestamp_).toSec();
+    if (dt < 0.0001)
+      return false; // Interval too small to integrate with
+
+    timestamp_ = time;
+
+    /// Estimate speeds using a rolling mean to filter them out:
+    linear_acc_(linear/dt);
+    angular_acc_(angular/dt);
+
+    linear_ = bacc::rolling_mean(linear_acc_);
+    angular_ = bacc::rolling_mean(angular_acc_);
+    return true;
+  }
+
+  void Odometry::updateOpenLoop(double linear, double angular, const ros::Time &time)
+  {
+    /// Save last linear and angular velocity:
+    linear_ = linear;
+    angular_ = angular;
+
+    /// Integrate odometry:
+    const double dt = (time - timestamp_).toSec();
+    timestamp_ = time;
+    integrate_fun_(linear * dt, angular * dt);
+  }
+
+  void Odometry::setWheelParams(double wheel_separation, double left_wheel_radius, double right_wheel_radius)
+  {
+    wheel_separation_   = wheel_separation;
+    left_wheel_radius_  = left_wheel_radius;
+    right_wheel_radius_ = right_wheel_radius;
+  }
+
+  void Odometry::setVelocityRollingWindowSize(size_t velocity_rolling_window_size)
+  {
+    velocity_rolling_window_size_ = velocity_rolling_window_size;
+
+    resetAccumulators();
+  }
+
+  void Odometry::integrateRungeKutta2(double linear, double angular)
+  {
+    const double direction = heading_ + angular * 0.5;
+
+    /// Runge-Kutta 2nd order integration:
+    x_       += linear * cos(direction);
+    y_       += linear * sin(direction);
+    heading_ += angular;
+  }
+
+  /**
+   * \brief Other possible integration method provided by the class
+   * \param linear
+   * \param angular
+   */
+  void Odometry::integrateExact(double linear, double angular)
+  {
+    if (fabs(angular) < 1e-6)
+      integrateRungeKutta2(linear, angular);
+    else
+    {
+      /// Exact integration (should solve problems when angular is zero):
+      const double heading_old = heading_;
+      const double r = linear/angular;
+      heading_ += angular;
+      x_       +=  r * (sin(heading_) - sin(heading_old));
+      y_       += -r * (cos(heading_) - cos(heading_old));
+    }
+  }
+
+  void Odometry::resetAccumulators()
+  {
+    linear_acc_ = RollingMeanAcc(RollingWindow::window_size = velocity_rolling_window_size_);
+    angular_acc_ = RollingMeanAcc(RollingWindow::window_size = velocity_rolling_window_size_);
+  }
+
+} // namespace ros_kinematics

Devices/Packages/ros_kinematics/src/ros_diff_drive.cpp

@@ -0,0 +1,824 @@
+#include "ros_kinematics/ros_diff_drive.h"
+#include <std_msgs/UInt16.h>
+#include <std_msgs/Int32.h>
+#include <std_msgs/Float32.h>
+
+
+ros_kinematics::RosDiffDrive::RosDiffDrive(ros::NodeHandle& nh, const std::string& name)
+  : nh_(nh),
+  name_(name),
+  motor_loader_("models", "models::BaseSteerDrive"),
+  encoder_loader_("models", "models::BaseAbsoluteEncoder"),
+  last_odom_state_publish_time_(ros::Time(0)),
+  left_wheel_curr_pos_(0),
+  right_wheel_curr_pos_(0),
+  publish_time_(ros::Time(0)),
+  initialized_(false)
+{
+  if(!initialized_)
+  {
+    nh_priv_ = ros::NodeHandle(nh_, name_);
+    ROS_INFO("RosDiffDrive get node name is %s", name_.c_str());
+
+    bool init_odom_enc;
+    nh_priv_.param("initialize_odom_enc", init_odom_enc, true);
+    odom_enc_initialized_ = !init_odom_enc;
+
+    nh_priv_.param("wheel_separation_multiplier", wheel_separation_multiplier_, wheel_separation_multiplier_);
+    ROS_INFO_STREAM_NAMED(name_, "Wheel separation will be multiplied by " << wheel_separation_multiplier_ << ".");
+
+    if (nh_priv_.hasParam("wheel_radius_multiplier"))
+    {
+      double wheel_radius_multiplier;
+      nh_priv_.getParam("wheel_radius_multiplier", wheel_radius_multiplier);
+
+      left_wheel_radius_multiplier_ = wheel_radius_multiplier;
+      right_wheel_radius_multiplier_ = wheel_radius_multiplier;
+    }
+    else
+    {
+      nh_priv_.param("left_wheel_radius_multiplier", left_wheel_radius_multiplier_, left_wheel_radius_multiplier_);
+      nh_priv_.param("right_wheel_radius_multiplier", right_wheel_radius_multiplier_, right_wheel_radius_multiplier_);
+    }
+
+    ROS_INFO_STREAM_NAMED(name_, "Left wheel radius will be multiplied by " << left_wheel_radius_multiplier_ << ".");
+    ROS_INFO_STREAM_NAMED(name_, "Right wheel radius will be multiplied by " << right_wheel_radius_multiplier_ << ".");
+
+    int velocity_rolling_window_size = 10;
+    nh_priv_.param("velocity_rolling_window_size", velocity_rolling_window_size, velocity_rolling_window_size);
+    ROS_INFO_STREAM_NAMED(name_, "Velocity rolling window size of " << velocity_rolling_window_size << ".");
+
+    odometry_.setVelocityRollingWindowSize(velocity_rolling_window_size);
+
+    // Twist command related:
+    nh_priv_.param("cmd_vel_timeout", cmd_vel_timeout_, cmd_vel_timeout_);
+    ROS_INFO_STREAM_NAMED(name_, "Velocity commands will be considered old if they are older than " << cmd_vel_timeout_ << "s.");
+
+    nh_priv_.param("allow_multiple_cmd_vel_publishers", allow_multiple_cmd_vel_publishers_, allow_multiple_cmd_vel_publishers_);
+    ROS_INFO_STREAM_NAMED(name_, "Allow mutiple cmd_vel publishers is " << (allow_multiple_cmd_vel_publishers_ ? "enabled" : "disabled"));
+
+    nh_priv_.param("base_frame_id", base_frame_id_, base_frame_id_);
+    ROS_INFO_STREAM_NAMED(name_, "Base frame_id set to " << base_frame_id_);
+
+    nh_priv_.param("odom_frame_id", odom_frame_id_, odom_frame_id_);
+    ROS_INFO_STREAM_NAMED(name_, "Odometry frame_id set to " << odom_frame_id_);
+
+    nh_priv_.param("enable_odom_tf", enable_odom_tf_, enable_odom_tf_);
+    ROS_INFO_STREAM_NAMED(name_, "Publishing base frame to tf is " << (enable_odom_tf_ ? "enabled" : "disabled"));
+
+    nh_priv_.param("enable_wheel_tf", enable_wheel_tf_, enable_wheel_tf_);
+    ROS_INFO_STREAM_NAMED(name_, "Publishing wheel frame to tf is " << (enable_wheel_tf_ ? "enabled" : "disabled"));
+
+    nh_priv_.param("commandTopic", command_topic_, std::string("cmd_vel"));
+    // nh_priv_.param("publishOdomTF", publishOdomTF_, false);
+    nh_priv_.param("odometryTopic", odometry_topic_, std::string("odom"));
+    // nh_priv_.param("odometryFrame", odometry_frame_, std::string("odom"));
+    nh_priv_.param("odometryEncTopic", odometry_enc_topic_, std::string("odom_enc"));
+    nh_priv_.param("odometryEncChildFrame", odometry_enc_child_frame_, std::string("odom_enc"));
+    // nh_priv_.param("robotBaseFrame", robot_base_frame_, std::string("base_link"));
+    nh_priv_.param("subscribe_queue_size", m_subscribe_queue_size_, 1);
+    nh_priv_.param("publish_rate", update_rate_, 100.0);
+    publish_period_ = ros::Duration(1.0 / update_rate_);
+    nh_priv_.param("open_loop", open_loop_, false);
+
+    // Velocity and acceleration limits:
+    nh_priv_.param("linear/x/has_velocity_limits", limiter_lin_.has_velocity_limits, limiter_lin_.has_velocity_limits);
+    nh_priv_.param("linear/x/has_acceleration_limits", limiter_lin_.has_acceleration_limits, limiter_lin_.has_acceleration_limits);
+    nh_priv_.param("linear/x/has_jerk_limits", limiter_lin_.has_jerk_limits, limiter_lin_.has_jerk_limits);
+    nh_priv_.param("linear/x/max_velocity", limiter_lin_.max_velocity, limiter_lin_.max_velocity);
+    nh_priv_.param("linear/x/min_velocity", limiter_lin_.min_velocity, -limiter_lin_.max_velocity);
+    nh_priv_.param("linear/x/max_acceleration", limiter_lin_.max_acceleration, limiter_lin_.max_acceleration);
+    nh_priv_.param("linear/x/min_acceleration", limiter_lin_.min_acceleration, -limiter_lin_.max_acceleration);
+    nh_priv_.param("linear/x/max_jerk", limiter_lin_.max_jerk, limiter_lin_.max_jerk);
+    nh_priv_.param("linear/x/min_jerk", limiter_lin_.min_jerk, -limiter_lin_.max_jerk);
+
+    nh_priv_.param("angular/z/has_velocity_limits", limiter_ang_.has_velocity_limits, limiter_ang_.has_velocity_limits);
+    nh_priv_.param("angular/z/has_acceleration_limits", limiter_ang_.has_acceleration_limits, limiter_ang_.has_acceleration_limits);
+    nh_priv_.param("angular/z/has_jerk_limits", limiter_ang_.has_jerk_limits, limiter_ang_.has_jerk_limits);
+    nh_priv_.param("angular/z/max_velocity", limiter_ang_.max_velocity, limiter_ang_.max_velocity);
+    nh_priv_.param("angular/z/min_velocity", limiter_ang_.min_velocity, -limiter_ang_.max_velocity);
+    nh_priv_.param("angular/z/max_acceleration", limiter_ang_.max_acceleration, limiter_ang_.max_acceleration);
+    nh_priv_.param("angular/z/min_acceleration", limiter_ang_.min_acceleration, -limiter_ang_.max_acceleration);
+    nh_priv_.param("angular/z/max_jerk", limiter_ang_.max_jerk, limiter_ang_.max_jerk);
+    nh_priv_.param("angular/z/min_jerk", limiter_ang_.min_jerk, -limiter_ang_.max_jerk);
+    // Publish limited velocity:
+    nh_priv_.param("publish_cmd", publish_cmd_, publish_cmd_);
+    // Use encoder
+    nh_.param(name_ + "/use_encoder", use_abs_encoder, false);
+    // If either parameter is not available, we need to look up the value in the URDF
+    bool lookup_wheel_separation = !nh_priv_.getParam("wheel_separation", wheel_separation_);
+    bool lookup_wheel_radius = !nh_priv_.getParam("wheel_radius", wheel_radius_);
+
+    if(lookup_wheel_separation || lookup_wheel_radius) 
+      ROS_INFO("URDF if not specified as a parameter. We don't set the value here \n wheel_separation: %f,  wheel_radius %f",
+              wheel_separation_, wheel_radius_);
+    
+    // Regardless of how we got the separation and radius, use them
+    // to set the odometry parameters
+    const double ws = wheel_separation_multiplier_ * wheel_separation_;
+    const double lwr = left_wheel_radius_multiplier_ * wheel_radius_;
+    const double rwr = right_wheel_radius_multiplier_ * wheel_radius_;
+
+    odometry_.setWheelParams(ws, lwr, rwr);
+    ROS_INFO_STREAM_NAMED(name_,
+      "Odometry params : wheel separation " << ws
+      << ", left wheel radius " << lwr
+      << ", right wheel radius " << rwr);
+
+    if (publish_cmd_)
+    {
+      cmd_vel_pub_.reset(new realtime_tools::RealtimePublisher<geometry_msgs::TwistStamped>(nh_priv_, "cmd_vel_out", 100));
+    }
+
+    // Get joint names from the parameter server
+    std::vector<std::string> left_wheel_names, right_wheel_names;
+    if (!getWheelNames(nh_priv_, "left_wheel", left_wheel_names) ||
+      !getWheelNames(nh_priv_, "right_wheel", right_wheel_names))
+    {
+      exit(1);
+    }
+
+    if (!getWheelParams(nh_priv_, left_wheel_names, left_drive_param_map_) ||
+        !getWheelParams(nh_priv_, right_wheel_names, right_drive_param_map_))
+    {
+      exit(1);
+    }
+
+    if (left_wheel_names.size() != right_wheel_names.size()     || 
+        left_drive_param_map_.size() != left_wheel_names.size() || 
+        right_drive_param_map_.size() != right_wheel_names.size()
+       )
+    {
+      ROS_ERROR_STREAM_NAMED(name_,
+        "#left wheels (" << left_wheel_names.size() << ") != " <<
+        "#right wheels (" << right_wheel_names.size() << ").");
+      exit(1);
+    }
+    else
+    {
+      wheel_joints_size_ = left_wheel_names.size();
+
+      left_drive_.resize(wheel_joints_size_);
+      right_drive_.resize(wheel_joints_size_);
+      if (use_abs_encoder)
+      {
+        left_encoder_.resize(wheel_joints_size_);
+        right_encoder_.resize(wheel_joints_size_);
+      }
+    }
+
+    // plugin models
+    ROS_INFO("RosDiffDrive is plugining models...");
+    for (size_t i = 0; i < wheel_joints_size_; ++i)
+    {
+      std::string left_drive;
+      nh_priv_.param(left_wheel_names[i] + "/lookup_name", left_drive, std::string("motor_wheel::LeftMotor"));
+      try
+      {
+        left_drive_[i] = motor_loader_.createInstance(left_drive);
+        // left_drive_[i]->initialize(nh_, name_ + "/" + motor_loader_.getName(left_drive));
+        left_drive_[i]->initialize(nh_, name_ + "/" + left_wheel_names[i]);
+        if (use_abs_encoder)
+        {
+          std::string right_encoder;
+          nh_priv_.param(right_wheel_names[i] + "/encoder/lookup_name", right_encoder, std::string("absolute_encoder::Measurements"));
+          try
+          {
+            right_encoder_[i] = encoder_loader_.createInstance(right_encoder);
+            right_encoder_[i]->initialize(nh_, name_ + "/" + right_wheel_names[i] + "/encoder");
+          }
+          catch (const pluginlib::PluginlibException& ex)
+          {
+            ROS_FATAL("Failed to create the %s model, are you sure it is properly registered and that the containing library is built? Exception: %s",
+              right_encoder.c_str(), ex.what());
+            exit(1);
+          }
+        }
+
+      }
+      catch (const pluginlib::PluginlibException& ex)
+      {
+        ROS_FATAL("Failed to create the %s model, are you sure it is properly registered and that the containing library is built? Exception: %s",
+          left_drive.c_str(), ex.what());
+        exit(1);
+      }
+
+
+      std::string right_drive;
+      nh_priv_.param(right_wheel_names[i] + "/lookup_name", right_drive, std::string("motor_wheel::RightMotor"));
+      try
+      {
+        right_drive_[i] = motor_loader_.createInstance(right_drive);
+        // right_drive_[i]->initialize(nh_, name_ + "/" + motor_loader_.getName(right_drive));
+        right_drive_[i]->initialize(nh_, name_ + "/" + right_wheel_names[i]);
+
+        if (use_abs_encoder)
+        {
+          std::string left_encoder;
+          nh_priv_.param(left_wheel_names[i] + "/encoder/lookup_name", left_encoder, std::string("absolute_encoder::Measurements"));
+          try
+          {
+            left_encoder_[i] = encoder_loader_.createInstance(left_encoder);
+            left_encoder_[i]->initialize(nh_, name_ + "/" + left_wheel_names[i] + "/encoder");
+          }
+          catch (const pluginlib::PluginlibException& ex)
+          {
+            ROS_FATAL("Failed to create the %s model, are you sure it is properly registered and that the containing library is built? Exception: %s",
+              left_encoder.c_str(), ex.what());
+            exit(1);
+          }
+        }
+      }
+      catch (const pluginlib::PluginlibException& ex)
+      {
+        ROS_FATAL("Failed to create the %s model, are you sure it is properly registered and that the containing library is built? Exception: %s",
+          right_drive.c_str(), ex.what());
+        exit(1);
+      }
+    }
+
+    if(enable_wheel_tf_) 
+    {
+      left_wheel_tf_pub_.reset(new realtime_tools::RealtimePublisher<tf::tfMessage>(nh_, "/tf", 100));
+      for (std::map<std::string, XmlRpc::XmlRpcValue>::iterator itr = left_drive_param_map_.begin(); itr != left_drive_param_map_.end(); ++itr)
+      {
+        geometry_msgs::TransformStamped tfs;
+        tfs.child_frame_id = static_cast<std::string>(itr->second["frame_id"]);
+        tfs.header.frame_id = "base_link";
+        tfs.transform.translation.x = static_cast<double>(itr->second["origin"][0]);
+        tfs.transform.translation.y = static_cast<double>(itr->second["origin"][1]);
+        tfs.transform.translation.z = static_cast<double>(itr->second["origin"][2]);
+        double roll = static_cast<double>(itr->second["origin"][3]);
+        double pitch = static_cast<double>(itr->second["origin"][4]);
+        double yaw = static_cast<double>(itr->second["origin"][5]);
+        geometry_msgs::Quaternion q = tf::createQuaternionMsgFromRollPitchYaw(roll, pitch, yaw);
+        tfs.transform.rotation = q;
+        left_wheel_tf_pub_->msg_.transforms.push_back(tfs);
+      }
+
+      right_wheel_tf_pub_.reset(new realtime_tools::RealtimePublisher<tf::tfMessage>(nh_, "/tf", 100));
+      for (std::map<std::string, XmlRpc::XmlRpcValue>::iterator itr = right_drive_param_map_.begin(); itr != right_drive_param_map_.end(); ++itr)
+      {
+        geometry_msgs::TransformStamped tfs;
+        tfs.child_frame_id = static_cast<std::string>(itr->second["frame_id"]);
+        tfs.header.frame_id = "base_link";
+        tfs.transform.translation.x = static_cast<double>(itr->second["origin"][0]);
+        tfs.transform.translation.y = static_cast<double>(itr->second["origin"][1]);
+        tfs.transform.translation.z = static_cast<double>(itr->second["origin"][2]);
+        double roll = static_cast<double>(itr->second["origin"][3]);
+        double pitch = static_cast<double>(itr->second["origin"][4]);
+        double yaw = static_cast<double>(itr->second["origin"][5]);
+        geometry_msgs::Quaternion q = tf::createQuaternionMsgFromRollPitchYaw(roll, pitch, yaw);
+        tfs.transform.rotation = q;
+        right_wheel_tf_pub_->msg_.transforms.push_back(tfs);
+      }
+    }
+
+    schedule_delay_ = ros::Duration(1/update_rate_);
+    schedule_lastest_delay_ = ros::Duration(0.5);
+
+    this->setOdomPubFields(nh_, nh_priv_);
+    transform_broadcaster_  = boost::shared_ptr<tf2_ros::TransformBroadcaster>(new tf2_ros::TransformBroadcaster());
+    cmd_vel_subscriber_     = nh_.subscribe(command_topic_, m_subscribe_queue_size_, &ros_kinematics::RosDiffDrive::CmdVelCallback, this);
+    
+    // Kinematics
+    kinematics_param_ptr_ = boost::make_shared<ros_kinematics::KinematicDiffDriveParameters>();
+    kinematics_param_ptr_->initialize(nh_);
+
+    // Thread
+    callback_thread_ =  boost::make_shared<boost::thread>(&ros_kinematics::RosDiffDrive::updateChild, this);
+    
+    ROS_INFO_NAMED("RosDiffDrive","Initializing on %s is successed", name_.c_str());
+    initialized_ = true;
+  }
+}
+
+ros_kinematics::RosDiffDrive::~RosDiffDrive()
+{
+  callback_thread_ = nullptr;
+  kinematics_param_ptr_ = nullptr;
+}
+
+void ros_kinematics::RosDiffDrive::CmdVelCallback(const geometry_msgs::Twist::ConstPtr& cmd_msg)
+{
+  // check that we don't have multiple publishers on the command topic
+  if (!allow_multiple_cmd_vel_publishers_ && cmd_vel_subscriber_.getNumPublishers() > 1)
+  {
+    ROS_ERROR_STREAM_THROTTLE_NAMED(1.0, name_, "Detected " << cmd_vel_subscriber_.getNumPublishers()
+      << " publishers " << command_topic_ << ". Only 1 publisher is allowed. Going to brake.");
+    brake();
+    return;
+  }
+
+  if (!std::isfinite(cmd_msg->angular.z) || !std::isfinite(cmd_msg->linear.x))
+  {
+    ROS_WARN_THROTTLE(1.0, "Received NaN in velocity command. Ignoring.");
+    return;
+  }
+
+  command_struct_.angular.z = cmd_msg->angular.z;
+  command_struct_.linear.x = cmd_msg->linear.x;
+  command_struct_.stamp = ros::Time::now();
+  command_.writeFromNonRT(command_struct_);
+  this->scheduleMotorController(true);
+  ROS_DEBUG_STREAM_NAMED(name_,
+    "Added values to command. "
+    << "Ang: " << command_struct_.angular.z << ", "
+    << "Lin: " << command_struct_.linear.x << ", "
+    << "Stamp: " << command_struct_.stamp);
+}
+
+bool ros_kinematics::RosDiffDrive::isCmdVelTriggered(void)
+{
+  boost::lock_guard<boost::mutex> schedule_lockguard(publish_mutex_);
+  return !publish_time_.isZero() && ros::Time::now() > publish_time_;
+}
+
+
+bool ros_kinematics::RosDiffDrive::isCmdVelLastestTriggered(void)
+{
+  boost::lock_guard<boost::mutex> schedule_lockguard(publish_mutex_);
+  return !publish_lastest_time_.isZero() && ros::Time::now() > publish_lastest_time_;
+}
+
+
+void ros_kinematics::RosDiffDrive::scheduleMotorController(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);
+  }
+}
+
+void ros_kinematics::RosDiffDrive::updateOdometryEncoder(const ros::Time& current_time)
+{
+  // ros::Time current_time = ros::Time::now();
+  double step_time = ros::Duration(current_time - last_odom_update_).toSec();
+  last_odom_update_ = current_time;
+
+  // Apply (possibly new) multipliers:
+  const double ws = wheel_separation_multiplier_ * wheel_separation_;
+  const double lwr = left_wheel_radius_multiplier_ * wheel_radius_;
+  const double rwr = right_wheel_radius_multiplier_ * wheel_radius_;
+  odometry_.setWheelParams(ws, lwr, rwr);
+  // COMPUTE AND PUBLISH ODOMETRY
+  if (open_loop_)
+  {
+    odometry_.updateOpenLoop(last0_cmd_.linear.x, last0_cmd_.angular.z, current_time);
+  }
+  else
+  {
+    double left_pos = 0.0;
+    double right_pos = 0.0;
+    for (size_t i = 0; i < wheel_joints_size_; ++i)
+    {
+      double left_speed, right_speed;
+      if (use_abs_encoder)
+      {
+        left_speed = left_encoder_[i]->Velocity();
+        right_speed = right_encoder_[i]->Velocity();
+      }
+      else
+      {
+        left_speed = left_drive_[i]->GetVelocity();
+        right_speed = right_drive_[i]->GetVelocity();
+      }
+
+      const double lp = left_speed  * step_time;  // rad/s -> rad
+      const double rp = right_speed * step_time;
+  
+      if (std::isnan(lp) || std::isnan(rp))
+        return;
+
+      left_pos += lp;
+      right_pos += rp;
+    }
+    left_pos /= wheel_joints_size_;
+    right_pos /= wheel_joints_size_;
+    left_wheel_curr_pos_ += left_pos;
+    right_wheel_curr_pos_ += right_pos;
+    // Estimate linear and angular velocity using joint information
+    odometry_.update(left_wheel_curr_pos_, right_wheel_curr_pos_, current_time);
+
+    if(enable_wheel_tf_)
+    {
+      if(left_wheel_tf_pub_->trylock())
+      {
+        geometry_msgs::Quaternion q = tf::createQuaternionMsgFromRollPitchYaw(0, left_wheel_curr_pos_, 0);
+        for(auto &msg_tf : left_wheel_tf_pub_->msg_.transforms)
+        {
+          msg_tf.header.stamp = current_time;
+          msg_tf.transform.rotation = q;
+        }
+        left_wheel_tf_pub_->unlockAndPublish();
+      }
+
+      if(right_wheel_tf_pub_->trylock())
+      {
+        geometry_msgs::Quaternion q = tf::createQuaternionMsgFromRollPitchYaw(0, right_wheel_curr_pos_, 0);
+        for(auto &msg_tf : right_wheel_tf_pub_->msg_.transforms)
+        {
+          msg_tf.header.stamp = current_time;
+          msg_tf.transform.rotation = q;
+        }
+        right_wheel_tf_pub_->unlockAndPublish();
+      }
+    }
+
+  }
+}
+
+void ros_kinematics::RosDiffDrive::publishOdometry(const ros::Time& current_time)
+{
+  // Publish odometry message
+  if (last_odom_state_publish_time_ + publish_period_ < current_time)
+  {
+    last_odom_state_publish_time_ += publish_period_;
+    // Compute and store orientation info
+    const geometry_msgs::Quaternion orientation(
+      tf::createQuaternionMsgFromYaw(odometry_.getHeading()));
+
+    // Populate odom message and publish
+    if (odom_pub_ && odom_pub_->trylock())
+    {
+      odom_pub_->msg_.header.stamp = current_time;
+      odom_pub_->msg_.pose.pose.position.x = odometry_.getX();
+      odom_pub_->msg_.pose.pose.position.y = odometry_.getY();
+      odom_pub_->msg_.pose.pose.orientation = orientation;
+      odom_pub_->msg_.twist.twist.linear.x = odometry_.getLinear();
+      odom_pub_->msg_.twist.twist.angular.z = odometry_.getAngular();
+      odom_pub_->unlockAndPublish();
+    }
+
+    // Publish tf /odom frame
+    if (enable_odom_tf_ && tf_odom_pub_->trylock())
+    {
+      geometry_msgs::TransformStamped& odom_frame = tf_odom_pub_->msg_.transforms[0];
+      odom_frame.header.stamp = current_time;
+      odom_frame.transform.translation.x = odometry_.getX();
+      odom_frame.transform.translation.y = odometry_.getY();
+      odom_frame.transform.rotation = orientation;
+      tf_odom_pub_->unlockAndPublish();
+    }
+  }
+}
+
+void ros_kinematics::RosDiffDrive::updateChild(void)
+{
+  ros::Rate rate(update_rate_);
+  last_odom_state_publish_time_ = ros::Time::now();
+  last_odom_update_ = ros::Time::now();
+  odometry_.init(ros::Time::now());
+
+  while (ros::ok())
+  {
+    ros::Time current_time = ros::Time::now();
+    // update parameter from dynamic reconf
+    this->updateDynamicParams();
+
+    const double ws  = wheel_separation_multiplier_ * wheel_separation_;
+    const double lwr = left_wheel_radius_multiplier_ * wheel_radius_;
+    const double rwr = right_wheel_radius_multiplier_ * wheel_radius_;
+    
+    if (this->allReady())
+    {
+      this->updateOdometryEncoder(current_time);
+      this->publishOdometry(current_time);
+    }
+
+    // MOVE ROBOT
+    DriveCmd curr_cmd = *(command_.readFromRT());
+    const double dt = (current_time - curr_cmd.stamp).toSec();
+
+    // Brake if cmd_vel has timeout:
+    if (dt > cmd_vel_timeout_)
+    {
+      curr_cmd.linear.x = 0.0;
+      curr_cmd.angular.z = 0.0;
+    }
+    
+    // Limit velocities and accelerations:
+    const double cmd_dt(dt);
+
+    limiter_lin_.limit(curr_cmd.linear.x, last0_cmd_.linear.x, last1_cmd_.linear.x, cmd_dt);
+    limiter_ang_.limit(curr_cmd.angular.z, last0_cmd_.angular.z, last1_cmd_.angular.z, cmd_dt);
+
+    last1_cmd_ = last0_cmd_;
+    last0_cmd_ = curr_cmd;
+
+    // Compute wheels velocities:
+    const double vel_left = (curr_cmd.linear.x - curr_cmd.angular.z * ws / 2.0) / lwr;
+    const double vel_right = (curr_cmd.linear.x + curr_cmd.angular.z * ws / 2.0) / rwr;
+    
+    if(this->isCmdVelTriggered())
+    {
+      // Publish limited velocity:
+      if (publish_cmd_ && cmd_vel_pub_ && cmd_vel_pub_->trylock())
+      {
+        cmd_vel_pub_->msg_.header.stamp = current_time;
+        cmd_vel_pub_->msg_.twist.linear.x = curr_cmd.linear.x;
+        cmd_vel_pub_->msg_.twist.angular.z = curr_cmd.angular.z;
+        cmd_vel_pub_->unlockAndPublish();
+      }
+
+      for (size_t i = 0; i < wheel_joints_size_; ++i)
+      {
+        left_drive_[i]->SetVelocity(vel_left);
+        right_drive_[i]->SetVelocity(vel_right);
+      }
+      scheduleMotorController(false);
+    }
+    
+    if(this->isCmdVelLastestTriggered())
+    {
+      int counter = 3;
+      do
+      {
+        for (size_t i = 0; i < wheel_joints_size_; ++i)
+        {
+          left_drive_[i]->SetVelocity(0.0);
+          right_drive_[i]->SetVelocity(0.0);
+        } 
+        counter--;
+        rate.sleep();
+        ros::spinOnce();
+      }while(ros::ok() && counter > 0);
+      scheduleMotorController(false);
+    }
+
+    rate.sleep();
+    ros::spinOnce();
+  }
+
+}
+
+void ros_kinematics::RosDiffDrive::brake()
+{
+  const double vel = 0.0;
+  for (size_t i = 0; i < wheel_joints_size_; ++i)
+  {
+    left_drive_[i]->SetVelocity(vel);
+    right_drive_[i]->SetVelocity(vel);
+  }
+}
+
+void ros_kinematics::RosDiffDrive::updateDynamicParams()
+{
+  // Retreive dynamic params:
+  if (kinematics_param_ptr_ != nullptr)
+  {
+    left_wheel_radius_multiplier_ = kinematics_param_ptr_->getLeftWheelRadiusMultiplier();
+    right_wheel_radius_multiplier_ = kinematics_param_ptr_->getRightWheelRadiusMultiplier();
+    wheel_separation_multiplier_ = kinematics_param_ptr_->getWheelSeparationMultiplier();
+    publish_period_ = ros::Duration(1.0 / kinematics_param_ptr_->getPublishRate());
+    // enable_odom_tf_ = kinematics_param_ptr_->getEnableOdomTf();
+  }
+}
+
+void ros_kinematics::RosDiffDrive::setOdomPubFields(ros::NodeHandle& root_nh, ros::NodeHandle& controller_nh)
+{
+  // Get and check params for covariances
+  XmlRpc::XmlRpcValue pose_cov_list;
+  controller_nh.getParam("pose_covariance_diagonal", pose_cov_list);
+  ROS_ASSERT(pose_cov_list.getType() == XmlRpc::XmlRpcValue::TypeArray);
+  ROS_ASSERT(pose_cov_list.size() == 6);
+  for (int i = 0; i < pose_cov_list.size(); ++i)
+    ROS_ASSERT(pose_cov_list[i].getType() == XmlRpc::XmlRpcValue::TypeDouble);
+
+  XmlRpc::XmlRpcValue twist_cov_list;
+  controller_nh.getParam("twist_covariance_diagonal", twist_cov_list);
+  ROS_ASSERT(twist_cov_list.getType() == XmlRpc::XmlRpcValue::TypeArray);
+  ROS_ASSERT(twist_cov_list.size() == 6);
+  for (int i = 0; i < twist_cov_list.size(); ++i)
+    ROS_ASSERT(twist_cov_list[i].getType() == XmlRpc::XmlRpcValue::TypeDouble);
+
+  // Setup odometry realtime publisher + odom message constant fields
+  odom_pub_.reset(new realtime_tools::RealtimePublisher<nav_msgs::Odometry>(controller_nh, "odom", 100));
+  odom_pub_->msg_.header.frame_id = odom_frame_id_;
+  odom_pub_->msg_.child_frame_id = base_frame_id_;
+  odom_pub_->msg_.pose.pose.position.z = 0;
+  odom_pub_->msg_.pose.covariance = {
+      static_cast<double>(pose_cov_list[0]), 0., 0., 0., 0., 0.,
+      0., static_cast<double>(pose_cov_list[1]), 0., 0., 0., 0.,
+      0., 0., static_cast<double>(pose_cov_list[2]), 0., 0., 0.,
+      0., 0., 0., static_cast<double>(pose_cov_list[3]), 0., 0.,
+      0., 0., 0., 0., static_cast<double>(pose_cov_list[4]), 0.,
+      0., 0., 0., 0., 0., static_cast<double>(pose_cov_list[5]) };
+  odom_pub_->msg_.twist.twist.linear.y  = 0;
+  odom_pub_->msg_.twist.twist.linear.z  = 0;
+  odom_pub_->msg_.twist.twist.angular.x = 0;
+  odom_pub_->msg_.twist.twist.angular.y = 0;
+  odom_pub_->msg_.twist.covariance = {
+      static_cast<double>(twist_cov_list[0]), 0., 0., 0., 0., 0.,
+      0., static_cast<double>(twist_cov_list[1]), 0., 0., 0., 0.,
+      0., 0., static_cast<double>(twist_cov_list[2]), 0., 0., 0.,
+      0., 0., 0., static_cast<double>(twist_cov_list[3]), 0., 0.,
+      0., 0., 0., 0., static_cast<double>(twist_cov_list[4]), 0.,
+      0., 0., 0., 0., 0., static_cast<double>(twist_cov_list[5]) };
+  tf_odom_pub_.reset(new realtime_tools::RealtimePublisher<tf::tfMessage>(root_nh, "/tf", 100));
+  tf_odom_pub_->msg_.transforms.resize(1);
+  tf_odom_pub_->msg_.transforms[0].transform.translation.z = 0.0;
+  tf_odom_pub_->msg_.transforms[0].child_frame_id = base_frame_id_;
+  tf_odom_pub_->msg_.transforms[0].header.frame_id = odom_frame_id_;
+}
+
+
+bool ros_kinematics::RosDiffDrive::getWheelNames(ros::NodeHandle& _nh,
+  const std::string& wheel_param,
+  std::vector<std::string>& wheel_names)
+{
+  XmlRpc::XmlRpcValue wheel_list;
+  if (!nh_priv_.getParam(wheel_param, wheel_list))
+  {
+    ROS_ERROR_STREAM_NAMED(name_,
+      "Couldn't retrieve wheel param '" << wheel_param << "'.");
+    return false;
+  }
+
+  if (wheel_list.getType() == XmlRpc::XmlRpcValue::TypeArray)
+  {
+    if (wheel_list.size() == 0)
+    {
+      ROS_ERROR_STREAM_NAMED(name_,
+        "Wheel param '" << wheel_param << "' is an empty list");
+      return false;
+    }
+
+    for (int i = 0; i < wheel_list.size(); ++i)
+    {
+      if (wheel_list[i].getType() != XmlRpc::XmlRpcValue::TypeString)
+      {
+        ROS_ERROR_STREAM_NAMED(name_,
+          "Wheel param '" << wheel_param << "' #" << i <<
+          " isn't a string.");
+        return false;
+      }
+    }
+
+    wheel_names.resize(wheel_list.size());
+    for (int i = 0; i < wheel_list.size(); ++i)
+    {
+      wheel_names[i] = static_cast<std::string>(wheel_list[i]);
+    }
+  }
+  else if (wheel_list.getType() == XmlRpc::XmlRpcValue::TypeString)
+  {
+    wheel_names.push_back(wheel_list);
+  }
+  else
+  {
+    ROS_ERROR_STREAM_NAMED(name_,
+      "Wheel param '" << wheel_param <<
+      "' is neither a list of strings nor a string.");
+    return false;
+  }
+
+  return true;
+}
+
+
+bool ros_kinematics::RosDiffDrive::getWheelParams(ros::NodeHandle& controller_nh, 
+  const std::vector<std::string>& wheel_names,
+  std::map<std::string, XmlRpc::XmlRpcValue>& drive_param_map)
+{
+  for (int i = 0; i < wheel_names.size(); ++i)
+  {
+    XmlRpc::XmlRpcValue element_param;
+    if (!nh_priv_.getParam(wheel_names[i], element_param))
+    {
+      ROS_ERROR_STREAM_NAMED(name_,
+        "Couldn't retrieve wheel param '" << wheel_names[i] << "'.");
+      return false;
+    }
+    // check type of member
+    if(element_param.hasMember("lookup_name"))
+    {
+      if(element_param["lookup_name"].getType() != XmlRpc::XmlRpcValue::TypeString)
+      {
+        ROS_ERROR_STREAM_NAMED(name_, "'" << wheel_names[i] << "'->lookup_name" << " isn't a string.");
+        return false;
+      }
+    }
+
+    if(element_param.hasMember("mesurement_topic"))
+    {
+      if(element_param["mesurement_topic"].getType() != XmlRpc::XmlRpcValue::TypeString)
+      {
+        ROS_ERROR_STREAM_NAMED(name_, "'" << wheel_names[i] << "'->mesurement_topic" << " isn't a string.");
+        return false;
+      }
+    }
+
+    if(element_param.hasMember("frame_id"))
+    {
+      if(element_param["frame_id"].getType() != XmlRpc::XmlRpcValue::TypeString)
+      {
+        ROS_ERROR_STREAM_NAMED(name_, "'" << wheel_names[i] << "'->frame_id" << " isn't a string.");
+        return false;
+      }
+    }
+
+    if(element_param.hasMember("wheel_topic"))
+    {
+      if(element_param["wheel_topic"].getType() != XmlRpc::XmlRpcValue::TypeString)
+      {
+        ROS_ERROR_STREAM_NAMED(name_, "'" << wheel_names[i] << "'->wheel_topic" << " isn't a string.");
+        return false;
+      }
+    }
+
+    if(element_param.hasMember("max_publish_rate"))
+    {
+      if( element_param["max_publish_rate"].getType() != XmlRpc::XmlRpcValue::TypeDouble && 
+          element_param["max_publish_rate"].getType() != XmlRpc::XmlRpcValue::TypeInt 
+        )
+      {
+        ROS_ERROR_STREAM_NAMED(name_, "'" << wheel_names[i] << "'->max_publish_rate" << " isn't a double/int.");
+        return false;
+      }
+    }
+
+    if(element_param.hasMember("subscribe_queue_size"))
+    {
+      if( element_param["subscribe_queue_size"].getType() != XmlRpc::XmlRpcValue::TypeDouble &&
+          element_param["subscribe_queue_size"].getType() != XmlRpc::XmlRpcValue::TypeInt
+        )
+      {
+        ROS_ERROR_STREAM_NAMED(name_, "'" << wheel_names[i] << "'->subscribe_queue_size" << " isn't a double/int.");
+        return false;
+      }
+    }
+
+    if(element_param.hasMember("command_timeout"))
+    {
+      if( element_param["command_timeout"].getType() != XmlRpc::XmlRpcValue::TypeDouble &&
+          element_param["command_timeout"].getType() != XmlRpc::XmlRpcValue::TypeInt
+        )
+      {
+        ROS_ERROR_STREAM_NAMED(name_, "'" << wheel_names[i] << "'->command_timeout" << " isn't a double/int.");
+        return false;
+      }
+    }
+
+    if(!element_param.hasMember("origin"))
+    {
+      ROS_ERROR_STREAM_NAMED(name_,
+        "Couldn't retrieve wheel param '" << wheel_names[i] << "'->origin.");
+      return false;
+    }
+    else
+    {
+      if(element_param["origin"].getType() != XmlRpc::XmlRpcValue::TypeArray)
+      {
+        ROS_ERROR_STREAM_NAMED(name_, "'" << wheel_names[i] << "'->origin" << " isn't a arrays.");
+        return false;
+      }
+      else
+      {
+        if(element_param["origin"].size() < 6) // x y z roll pich yaw
+        {
+          ROS_ERROR_STREAM_NAMED(name_,
+          "Wheel param '" << wheel_names[i] << "'->origin is an error array");
+          return false;
+        }
+        else
+        {
+          for (int j = 0; j < element_param["origin"].size(); ++j)
+          {
+            if( element_param["origin"][j].getType() != XmlRpc::XmlRpcValue::TypeDouble &&
+                element_param["origin"][j].getType() != XmlRpc::XmlRpcValue::TypeInt
+              )
+            {
+              ROS_ERROR_STREAM_NAMED(name_,
+                "Wheel param '" << wheel_names[i] << "'->origin #" << j <<
+                " isn't a double/int.");
+              return false;
+            }
+          }
+        }
+      }
+    }
+    // Done
+    drive_param_map[wheel_names[i]] = element_param;
+  }
+  return true;
+}
+
+
+bool ros_kinematics::RosDiffDrive::allReady()
+{
+  bool result = true;
+  for (size_t i = 0; i < wheel_joints_size_; ++i)
+  {
+    result = result && left_drive_[i]->Ready() && right_drive_[i]->Ready();
+  }
+  return result;
+}
+

Devices/Packages/ros_kinematics/src/ros_diff_drive_parameters.cpp

@@ -0,0 +1,117 @@
+#include <ros_kinematics/ros_diff_drive_parameters.h>
+#include <memory>
+#include <string>
+#include <map>
+
+namespace ros_kinematics
+{
+
+  KinematicDiffDriveParameters::KinematicDiffDriveParameters()
+    : left_wheel_radius_multiplier_(0.0),
+    right_wheel_radius_multiplier_(0.0),
+    wheel_separation_multiplier_(0.0),
+    publish_rate_(0.0),
+    enable_odom_tf_(false),
+    setup_(false),
+    dsrv_(nullptr)
+  {
+  }
+
+  KinematicDiffDriveParameters::KinematicDiffDriveParameters(const ros::NodeHandle& nh)
+    : left_wheel_radius_multiplier_(0.0),
+    right_wheel_radius_multiplier_(0.0),
+    wheel_separation_multiplier_(0.0),
+    publish_rate_(0.0),
+    enable_odom_tf_(false),
+    setup_(false),
+    dsrv_(nullptr)
+  {
+    initialize(nh);
+  }
+
+  void KinematicDiffDriveParameters::initialize(const ros::NodeHandle& nh)
+  {
+    if (!initialized_)
+    {
+      nh_ = ros::NodeHandle(nh);
+      // the rest of the initial values are loaded through the dynamic reconfigure mechanisms
+      dsrv_ = std::make_shared<dynamic_reconfigure::Server<DiffDriveParametersConfig> >(nh_);
+      dynamic_reconfigure::Server<DiffDriveParametersConfig>::CallbackType cb =
+        boost::bind(&KinematicDiffDriveParameters::reconfigureCB, this, _1, _2);
+      dsrv_->setCallback(cb);
+      int m_subscribe_queue_size = 1000;
+      kinematics_sub_ = nh_.subscribe<dynamic_reconfigure::Config>("/agv_dynparam/parameter_updates", m_subscribe_queue_size, &KinematicDiffDriveParameters::kinematicsCallback, this);
+      initialized_ = true;
+    }
+  }
+
+  void KinematicDiffDriveParameters::reconfigureCB(DiffDriveParametersConfig& config, uint32_t level)
+  {
+    boost::lock_guard<boost::mutex> schedule_lockguard(reconfig_mutex);
+    // ROS_WARN("reconfigureCB");
+    left_wheel_radius_multiplier_   = config.left_wheel_radius_multiplier;
+    right_wheel_radius_multiplier_  = config.right_wheel_radius_multiplier;
+    wheel_separation_multiplier_    = config.wheel_separation_multiplier;
+    publish_rate_                   = config.publish_rate;
+    enable_odom_tf_                 = config.enable_odom_tf;
+    // reconfig_mutex.unlock();
+  }
+
+  void KinematicDiffDriveParameters::kinematicsCallback(const dynamic_reconfigure::Config::ConstPtr& param)
+  {
+    boost::lock_guard<boost::mutex> schedule_lockguard(reconfig_mutex);
+    // ROS_WARN("dynamic_reconfigure");
+    if (param != nullptr)
+    {
+      std::map<std::string, double> param_m;
+      for (auto db : param->doubles) param_m[db.name] = db.value;
+
+      auto it = param_m.find("left_wheel_radius_multiplier");
+      if (it != param_m.end())
+      {
+        left_wheel_radius_multiplier_ = param_m["left_wheel_radius_multiplier"];
+        ROS_INFO("[ros_kinematics]-left_wheel_radius_multiplier %f", left_wheel_radius_multiplier_);
+      }
+      else ROS_WARN("[ros_kinematics]-left_wheel_radius_multiplier %f", left_wheel_radius_multiplier_);
+
+      it = param_m.find("right_wheel_radius_multiplier");
+      if (it != param_m.end())
+      {
+        right_wheel_radius_multiplier_ = param_m["right_wheel_radius_multiplier"];
+        ROS_INFO("[ros_kinematics]-right_wheel_radius_multiplier %f", right_wheel_radius_multiplier_);
+      }
+      else ROS_WARN("[ros_kinematics]-right_wheel_radius_multiplier %f", right_wheel_radius_multiplier_);
+
+      it = param_m.find("wheel_separation_multiplier");
+      if (it != param_m.end())
+      {
+        wheel_separation_multiplier_ = param_m["wheel_separation_multiplier"];
+        ROS_INFO("[ros_kinematics]-wheel_separation_multiplier %f", wheel_separation_multiplier_);
+      }
+      else ROS_WARN("[ros_kinematics]-wheel_separation_multiplier %f", wheel_separation_multiplier_);
+
+      it = param_m.find("publish_rate");
+      if (it != param_m.end())
+      {
+        publish_rate_ = param_m["publish_rate"];
+        ROS_INFO("[ros_kinematics]-publish_rate %f", publish_rate_);
+      }
+      else ROS_WARN("[ros_kinematics]-publish_rate %f", publish_rate_);
+
+      // it = param_m.find("enable_odom_tf");
+      // if (it != param_m.end())
+      // {
+      //   enable_odom_tf_ = param_m["enable_odom_tf"];
+      //   ROS_INFO("[ros_kinematics]-enable_odom_tf %x", enable_odom_tf_);
+      // }
+      // else ROS_WARN("[ros_kinematics]-enable_odom_tf %x", enable_odom_tf_);
+
+      param_m.clear();
+      setup_ = true;
+    }
+    else ROS_WARN("dynamic_reconfigure param is null");
+    // reconfig_mutex.unlock();
+  }
+
+
+} // namespace ros_kinematics

Devices/Packages/ros_kinematics/src/ros_kinematics_node.cpp

@@ -0,0 +1,33 @@
+#include <ros/ros.h>
+#include <boost/shared_ptr.hpp>
+#include "ros_kinematics/base_drive.h"
+#include "ros_kinematics/ros_steer_drive.h"
+#include "ros_kinematics/ros_diff_drive.h"
+
+int main(int argc, char **argv)
+{
+  /* Khoi tao Node */
+  ros::init(argc, argv, "ros_kinematics");
+  ros::NodeHandle nh;
+  int type;
+  nh.param(ros::this_node::getName()+"/type", type, 1);
+  /* Constructors */
+  boost::shared_ptr<ros_kinematics::BaseDrive> ros_drive = nullptr;
+  switch (type)
+  {
+    case 1:
+      ros_drive = boost::make_shared<ros_kinematics::RosSteerDrive>(nh, ros::this_node::getName());
+      break;
+
+    case 2:
+      ros_drive = boost::make_shared<ros_kinematics::RosDiffDrive>(nh, ros::this_node::getName());
+      break;
+
+    default:
+      ros_drive = boost::make_shared<ros_kinematics::RosSteerDrive>(nh, ros::this_node::getName());
+      break;
+  }
+  ros::spin();
+  ros_drive = nullptr;
+  return 0;
+}

Devices/Packages/ros_kinematics/src/ros_steer_drive.cpp

@@ -0,0 +1,447 @@
+#include "ros_kinematics/ros_steer_drive.h"
+#include <std_msgs/UInt16.h>
+#include <std_msgs/Int32.h>
+#include <std_msgs/Float32.h>
+
+ros_kinematics::RosSteerDrive::RosSteerDrive(ros::NodeHandle & nh, const std::string &name)
+ : nh_(nh),
+   motor_loader_("models", "models::BaseSteerDrive"), 
+   encoder_loader_("models", "models::BaseAbsoluteEncoder"),
+   last_odom_update_(ros::Time(0)),
+   last_actuator_update_(ros::Time(0)),
+   publish_time_(ros::Time(0)),
+   publish_lastest_time_(ros::Time(0))
+{
+
+  nh_priv_ = ros::NodeHandle(nh_, name);
+  ROS_INFO("RosSteerDrive get node name is %s", name.c_str());
+
+  bool init_odom_enc;
+  nh_priv_.param("initialize_odom_enc", init_odom_enc, true);
+  odom_enc_initialized_ = !init_odom_enc;
+  
+  double schedule_delay, schedule_lastest_delay;
+  nh_priv_.param("commandTopic", command_topic_, std::string("cmd_vel"));
+  nh_priv_.param("publishOdomTF", publishOdomTF_, false);
+  nh_priv_.param("odometryTopic", odometry_topic_, std::string("odom"));
+  nh_priv_.param("odometryFrame", odometry_frame_, std::string("odom"));
+  nh_priv_.param("odometryEncTopic", odometry_enc_topic_, std::string("odom_enc"));
+  nh_priv_.param("odometryEncChildFrame", odometry_enc_child_frame_, std::string("odom_enc"));
+  nh_priv_.param("robotBaseFrame", robot_base_frame_, std::string("base_link"));
+  nh_priv_.param("steerChildFrame", steer_id_, std::string("steer_link"));
+
+  nh_priv_.param("subscribe_queue_size", m_subscribe_queue_size_, 1);
+  nh_priv_.param("max_update_rate", update_rate_, 100.0);
+  nh_priv_.param("schedule_delay", schedule_delay, 0.005);
+  nh_priv_.param("schedule_lastest_delay", schedule_lastest_delay, 0.5);
+  nh_priv_.param("odomEncSteeringAngleOffset",odom_enc_steering_angle_offset_, 0.);
+  nh_priv_.param("wheelAcceleration", wheel_acceleration_, 0.);
+  nh_priv_.param("steering_fix_wheel_distance_x", steering_fix_wheel_distance_x_, 0.68);
+  nh_priv_.param("steering_fix_wheel_distance_y", steering_fix_wheel_distance_y_, 0.);
+  
+
+  std::string steer_drive;
+  nh_priv_.param("steer_drive", steer_drive, std::string("motor_wheel::SteerMotor"));
+  try
+  {
+    steer_motor_ =  motor_loader_.createInstance(steer_drive);
+    steer_motor_->initialize(nh_, name + "/" + motor_loader_.getName(steer_drive));
+    ros::NodeHandle nh(nh_priv_, motor_loader_.getName(steer_drive));
+    nh.param("method", steer_motor_mode_, std::string(""));
+  }
+  catch(const pluginlib::PluginlibException &ex)
+  {
+    ROS_FATAL("Failed to create the %s model, are you sure it is properly registered and that the containing library is built? Exception: %s", steer_drive.c_str(), ex.what());
+    exit(1);
+  }
+
+  std::string traction_drive;
+  nh_priv_.param("traction_drive", traction_drive, std::string("motor_wheel::TractionMotor"));
+  nh_priv_.param("wheel_diameter", wheel_diameter_, 0.210); 
+  try
+  {
+    traction_motor_ =  motor_loader_.createInstance(traction_drive);
+    traction_motor_->initialize(nh_, name + "/" + motor_loader_.getName(traction_drive));
+    ros::NodeHandle nh(nh_priv_, motor_loader_.getName(traction_drive));
+    nh.param("method", traction_motor_mode_, std::string(""));
+  }
+  catch(const pluginlib::PluginlibException &ex)
+  {
+    ROS_FATAL("Failed to create the %s model, are you sure it is properly registered and that the containing library is built? Exception: %s", traction_drive.c_str(), ex.what());
+    exit(1);
+  }
+
+  nh_.param(name + "/use_encoder", use_abs_encoder, false);
+  if(use_abs_encoder)
+  {
+    std::string abs_encoder;
+    nh_priv_.param("absolute_encoder", abs_encoder, std::string("absolute_encoder::Measurements"));
+    try
+    {
+      absolute_encoder_ =  encoder_loader_.createInstance(abs_encoder);
+      absolute_encoder_->initialize(nh, "AbsoluteEncoder");
+    }
+    catch(const pluginlib::PluginlibException &ex)
+    {
+      ROS_FATAL("Failed to create the %s model, are you sure it is properly registered and that the containing library is built? Exception: %s", abs_encoder.c_str(), ex.what());
+      exit(1);
+    }
+  }
+
+  transform_broadcaster_  = boost::shared_ptr<tf::TransformBroadcaster>(new tf::TransformBroadcaster());
+  odometry_publisher_     = nh_.advertise<nav_msgs::Odometry>(odometry_topic_, 1);
+  odometry_enc_publisher_ = nh_.advertise<nav_msgs::Odometry>(odometry_enc_topic_, 1);
+  cmd_vel_subscriber_     = nh_.subscribe(command_topic_, m_subscribe_queue_size_, &ros_kinematics::RosSteerDrive::CmdVelCallback, this);
+  cmd_vel_feedback_       = nh_.advertise<geometry_msgs::Twist>("cmd_vel_feedback", 1);
+  cmd_vel_rb              = nh_.advertise<geometry_msgs::Twist>("cmd_vel_rb", 1);
+  steer_angle_pub_        = nh_.advertise<std_msgs::Float32>("steer_angle", 1);
+  
+  // Initialize update rate stuff
+  if (this->update_rate_ > 0.0)
+    this->update_period_ = 1.0 / this->update_rate_;
+  else
+    this->update_period_ = 0.0;
+    
+  schedule_delay_ = ros::Duration(schedule_delay);
+  schedule_lastest_delay_ = ros::Duration(schedule_lastest_delay);
+
+  // Kinematics
+  kinematics_param_ptr = boost::make_shared<ros_kinematics::KinematicSteerDriveParameters>();
+  kinematics_param_ptr->initialize(nh_);
+
+  // Thread
+  callback_thread_ = new boost::thread(&ros_kinematics::RosSteerDrive::UpdateChild,this);
+}
+
+ros_kinematics::RosSteerDrive::~RosSteerDrive()
+{
+  if (callback_thread_)
+  {
+    callback_thread_->join();
+    delete (callback_thread_);
+    callback_thread_ = 0;
+  }
+}
+
+void ros_kinematics::RosSteerDrive::CmdVelCallback(const geometry_msgs::Twist::ConstPtr &cmd_msg)
+{
+  boost::lock_guard<boost::mutex> schedule_lockguard(callback_mutex_);
+  command_struct_.linear.x = cmd_msg->linear.x;
+  command_struct_.angular.z = cmd_msg->angular.z;
+  scheduleMotorController(true);
+}
+
+/**
+ * @brief returns true, if publishing of a cmd_vel topic is publishing
+*/ 
+bool ros_kinematics::RosSteerDrive::isCmdVelTriggered(void)
+{
+  boost::lock_guard<boost::mutex> schedule_lockguard(publish_mutex_);
+  return !publish_time_.isZero() && ros::Time::now() > publish_time_;
+}
+
+/**
+ * @brief returns true, if publishing of a cmd_vel topic is publishing
+*/ 
+bool ros_kinematics::RosSteerDrive::isCmdVelLastestTriggered(void)
+{
+  boost::lock_guard<boost::mutex> schedule_lockguard(publish_mutex_);
+  return !publish_lastest_time_.isZero() && ros::Time::now() > publish_lastest_time_;
+}
+
+/**
+ * @brief schedules MotorController function 
+ * @param[in] schedule if true, publishing is scheduled, otherwise a possibly pending schedule is removed.
+*/
+void ros_kinematics::RosSteerDrive::scheduleMotorController(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);
+  }
+}
+
+void ros_kinematics::RosSteerDrive::MotorController(double target_speed, double target_steering_speed, double dt)
+{
+  // TODO add the accelerations etc. properly
+  double applied_speed = target_speed;
+  double applied_steering_speed = target_steering_speed;
+
+  if(traction_motor_->Ready() && steer_motor_->Ready())
+  {
+    double current_speed = traction_motor_->GetVelocity();
+    double current_steering_speed = steer_motor_->GetVelocity();
+
+    if(kinematics_param_ptr->isSetup())
+    {
+      wheel_acceleration_ = kinematics_param_ptr->getWheelAcceleration();
+    }
+
+    if (wheel_acceleration_ > 0)
+    {
+      // TODO
+      // applied_speed = ...;
+      // applied_steering_speed = ...;
+    }
+    if(traction_motor_mode_ == std::string("speed_mode")) 
+      traction_motor_->SetVelocity(applied_speed);
+
+    if(steer_motor_mode_ == std::string("position_mode")) 
+      steer_motor_->SetPosition(applied_steering_speed);
+    else if(steer_motor_mode_ == std::string("speed_mode")) 
+      steer_motor_->SetVelocity(applied_steering_speed);
+  }
+  if(!traction_motor_->Ready())
+    ROS_WARN_THROTTLE_NAMED(2.0, "ros_kinematics", "traction_motor_ is not Ready.");
+  if(!steer_motor_->Ready())
+    ROS_WARN_THROTTLE_NAMED(2.0, "ros_kinematics", "steer_motor_ is not Ready.");
+}
+
+void ros_kinematics::RosSteerDrive::publishSteerJoint(double odom_alpha)
+{
+  ros::Time current_time = ros::Time::now();
+  tf::Quaternion qt;
+  tf::Vector3 vt;
+  qt.setRPY(0, 0, odom_alpha);
+  if(kinematics_param_ptr->isSetup())
+  {
+    steering_fix_wheel_distance_x_ = kinematics_param_ptr->getSteeringFixWheelDistanceX();
+    steering_fix_wheel_distance_y_ = kinematics_param_ptr->getSteeringFixWheelDistanceY();
+  }
+  vt = tf::Vector3(steering_fix_wheel_distance_x_, steering_fix_wheel_distance_y_, 0);
+  tf::Transform base_to_steer_link(qt, vt);
+  transform_broadcaster_->sendTransform( tf::StampedTransform(base_to_steer_link, current_time, 
+                                                              robot_base_frame_, steer_id_));
+}
+
+void ros_kinematics::RosSteerDrive::UpdateOdometryEncoder()
+{
+  ros::Time current_time = ros::Time::now();
+  double step_time = ros::Duration(current_time - last_odom_update_).toSec();
+  last_odom_update_ = current_time;
+
+  double odom_alpha;
+  if(use_abs_encoder)
+    odom_alpha = absolute_encoder_->Position();
+  else
+    odom_alpha = steer_motor_->Position();
+
+  if(kinematics_param_ptr->isSetup())
+  {
+    odom_enc_steering_angle_offset_= kinematics_param_ptr->getOdomEncSteeringAngleOffset();
+    // ROS_WARN("odom_enc_steering_angle_offset_ %f", odom_enc_steering_angle_offset_);
+  }
+  odom_alpha += odom_enc_steering_angle_offset_;
+  
+  std_msgs::Float32 msg;
+  msg.data = odom_alpha/M_PI*180;
+  steer_angle_pub_.publish(msg);
+  
+  publishSteerJoint(odom_alpha);
+  // ROS_WARN_STREAM("Odom alpha " << odom_alpha << " "<< odom_alpha / M_PI *180);
+  // Distance travelled drive wheel
+  double drive_dist = traction_motor_->GetVelocity() * (wheel_diameter_ / 2) * step_time; 
+
+  double dd = 0.;
+  double da = 0.;
+
+  if (fabs(odom_alpha) < 0.000001) // Avoid dividing with a very small number...
+  {
+    dd = drive_dist;
+    da = 0.;
+  }
+  else
+  {
+    if(kinematics_param_ptr->isSetup())
+    {
+      steering_fix_wheel_distance_x_ = kinematics_param_ptr->getSteeringFixWheelDistanceX();
+      steering_fix_wheel_distance_y_ = kinematics_param_ptr->getSteeringFixWheelDistanceY();
+    }
+    double r_stear = steering_fix_wheel_distance_x_ / sin(odom_alpha);
+    double r_fix = r_stear * cos(odom_alpha) - steering_fix_wheel_distance_y_;
+
+    dd = r_fix / r_stear * drive_dist; // Adjust the actual forward movement (located between the fixed front wheels) based on the radius of the drive wheel).
+    da = drive_dist / r_stear;
+  }
+  
+  // Update the current estimate
+  double dx = dd * cos(pose_encoder_.theta + da / 2.);
+  double dy = dd * sin(pose_encoder_.theta + da / 2.);
+
+  // Compute odometric pose
+  pose_encoder_.x += dx;
+  pose_encoder_.y += dy;
+  pose_encoder_.theta += da;
+
+  double w = da / step_time;
+  
+  double v = dd / step_time;
+  geometry_msgs::Twist fb;
+  fb.linear.x = v;
+  fb.angular.z = w;
+  cmd_vel_feedback_.publish(fb);
+
+  tf::Quaternion qt;
+  tf::Vector3 vt;
+  qt.setRPY(0, 0, pose_encoder_.theta);
+  vt = tf::Vector3(pose_encoder_.x, pose_encoder_.y, 0);
+
+  odom_enc_.pose.pose.position.x = vt.x();
+  odom_enc_.pose.pose.position.y = vt.y();
+  odom_enc_.pose.pose.position.z = vt.z();
+
+  odom_enc_.pose.pose.orientation.x = qt.x();
+  odom_enc_.pose.pose.orientation.y = qt.y();
+  odom_enc_.pose.pose.orientation.z = qt.z();
+  odom_enc_.pose.pose.orientation.w = qt.w();
+
+  odom_enc_.twist.twist.angular.z = w;
+  odom_enc_.twist.twist.linear.x = v;
+  odom_enc_.twist.twist.linear.y = 0.0;
+
+}
+
+void ros_kinematics::RosSteerDrive::PublishOdometry(double step_time)
+{
+  // getting data form encoder integration
+  odom_ = odom_enc_;
+  ros::Time current_time = ros::Time::now();
+  std::string odom_frame = odometry_frame_;
+  std::string base_footprint_frame = robot_base_frame_;
+  tf::Quaternion qt;
+  tf::Vector3 vt;
+  if(publishOdomTF_)
+  {
+    qt = tf::Quaternion(odom_.pose.pose.orientation.x, odom_.pose.pose.orientation.y, odom_.pose.pose.orientation.z, odom_.pose.pose.orientation.w);
+    vt = tf::Vector3(odom_.pose.pose.position.x, odom_.pose.pose.position.y, odom_.pose.pose.position.z);
+    tf::Transform base_footprint_to_odom(qt, vt);
+    transform_broadcaster_->sendTransform(
+        tf::StampedTransform(base_footprint_to_odom, current_time,
+                              odom_frame, base_footprint_frame));
+  }
+  // set covariance - TODO, fix this(!)
+  odom_.pose.covariance[0] = 0.00001;
+  odom_.pose.covariance[7] = 0.00001;
+  odom_.pose.covariance[14] = 1000000000000.0;
+  odom_.pose.covariance[21] = 1000000000000.0;
+  odom_.pose.covariance[28] = 1000000000000.0;
+  odom_.pose.covariance[35] = 0.001;
+
+  // set header
+  odom_.header.stamp = current_time;
+  odom_.header.frame_id = odom_frame;
+  odom_.child_frame_id = base_footprint_frame;
+
+  odometry_publisher_.publish(odom_);
+
+  // publish the encoder based odometry
+  {
+    if (!odom_enc_initialized_)
+    {
+      pose_encoder_.x = odom_.pose.pose.position.x;
+      pose_encoder_.y = odom_.pose.pose.position.y;
+      pose_encoder_.theta = tf::getYaw(odom_.pose.pose.orientation);
+      odom_enc_initialized_ = true;
+
+      odom_enc_ = odom_;
+    }
+    std::string odom_enc_child_frame = odometry_enc_child_frame_;
+    if(publishOdomTF_)
+    {
+      qt = tf::Quaternion(odom_enc_.pose.pose.orientation.x, odom_enc_.pose.pose.orientation.y, odom_enc_.pose.pose.orientation.z, odom_enc_.pose.pose.orientation.w);
+      vt = tf::Vector3(odom_enc_.pose.pose.position.x, odom_enc_.pose.pose.position.y, odom_enc_.pose.pose.position.z);
+      
+      tf::Transform odom_enc_child_to_odom(qt, vt);
+      transform_broadcaster_->sendTransform(
+          tf::StampedTransform(odom_enc_child_to_odom, current_time,
+                                odom_frame, odom_enc_child_frame));
+    }
+    odom_enc_.pose.covariance = odom_.pose.covariance; // TODO...
+    odom_enc_.header.stamp = current_time;
+    odom_enc_.header.frame_id = odom_frame; // Note - this is typically /world
+    odom_enc_.child_frame_id = odom_enc_child_frame;
+    odometry_enc_publisher_.publish(odom_enc_);
+  }
+}
+
+void ros_kinematics::RosSteerDrive::UpdateChild(void)
+{
+  ros::Rate rate(update_rate_);
+  bool stopped;
+  while (ros::ok())
+  {
+    if(traction_motor_->Ready() && steer_motor_->Ready())
+    {
+      UpdateOdometryEncoder();
+    }
+    ros::Time current_time = ros::Time::now();
+    double seconds_since_last_update = ros::Duration(current_time - last_actuator_update_).toSec();
+
+    if (seconds_since_last_update > update_period_)
+    {
+      if(traction_motor_->Ready() && steer_motor_->Ready())
+      {
+        PublishOdometry(seconds_since_last_update);
+      }
+      // if (publishWheelTF_)
+      //   publishWheelTF();
+      // if (publishWheelJointState_)
+      //   publishWheelJointState();
+      double target_steering_angle_speed_saved_;
+      if(isCmdVelTriggered())
+      {
+        double target_wheel_rotation_speed = command_struct_.linear.x / (wheel_diameter_ / 2.0);
+        double target_steering_angle_speed = command_struct_.angular.z;
+        double odom_alpha;
+        if(use_abs_encoder)
+          odom_alpha = absolute_encoder_->Position();
+        else
+          odom_alpha = steer_motor_->Position();
+
+        if(kinematics_param_ptr->isSetup())
+        {
+          odom_enc_steering_angle_offset_= kinematics_param_ptr->getOdomEncSteeringAngleOffset();
+        }
+        odom_alpha += odom_enc_steering_angle_offset_;
+
+        geometry_msgs::Twist fb;
+
+        fb.linear.x = command_struct_.linear.x * fabs(cos(odom_alpha));
+
+        cmd_vel_rb.publish(fb);
+        
+        MotorController(target_wheel_rotation_speed, target_steering_angle_speed, seconds_since_last_update);
+        target_steering_angle_speed_saved_ = target_steering_angle_speed;
+        scheduleMotorController(false);
+        stopped = false;
+      }
+      if(isCmdVelLastestTriggered() && !stopped)
+      {
+        if(steer_motor_mode_ == std::string("position_mode")) 
+        {
+          MotorController(0, target_steering_angle_speed_saved_, seconds_since_last_update);
+          MotorController(0, target_steering_angle_speed_saved_, seconds_since_last_update);
+        }
+        else
+        {
+          MotorController(0, 0, seconds_since_last_update);
+          MotorController(0, 0, seconds_since_last_update);
+        }
+        stopped = true;
+      }
+      last_actuator_update_ = current_time;
+    }
+    rate.sleep();
+    ros::spinOnce();
+  }
+}
+
+
+

Devices/Packages/ros_kinematics/src/ros_steer_drive_parameters.cpp

@@ -0,0 +1,106 @@
+#include <ros_kinematics/ros_steer_drive_parameters.h>
+#include <memory>
+#include <string>
+#include <map>
+
+namespace ros_kinematics
+{
+
+  KinematicSteerDriveParameters::KinematicSteerDriveParameters()
+    : odom_enc_steering_angle_offset_(0.0),
+    steering_fix_wheel_distance_x_(0.0),
+    steering_fix_wheel_distance_y_(0.0),
+    wheel_acceleration_(0.0),
+    setup_(false),
+    dsrv_(nullptr)
+  {
+  }
+
+  KinematicSteerDriveParameters::KinematicSteerDriveParameters(const ros::NodeHandle& nh)
+    : odom_enc_steering_angle_offset_(0.0),
+    steering_fix_wheel_distance_x_(0.0),
+    steering_fix_wheel_distance_y_(0.0),
+    wheel_acceleration_(0.0),
+    setup_(false),
+    dsrv_(nullptr)
+  {
+    initialize(nh);
+  }
+
+  void KinematicSteerDriveParameters::initialize(const ros::NodeHandle& nh)
+  {
+    if (!initialized_)
+    {
+      nh_ = ros::NodeHandle(nh);
+      // the rest of the initial values are loaded through the dynamic reconfigure mechanisms
+      dsrv_ = std::make_shared<dynamic_reconfigure::Server<SteerDriveParametersConfig> >(nh_);
+      dynamic_reconfigure::Server<SteerDriveParametersConfig>::CallbackType cb =
+        boost::bind(&KinematicSteerDriveParameters::reconfigureCB, this, _1, _2);
+      dsrv_->setCallback(cb);
+      int m_subscribe_queue_size = 1000;
+      kinematics_sub_ = nh_.subscribe<dynamic_reconfigure::Config>("/agv_dynparam/parameter_updates", m_subscribe_queue_size, &KinematicSteerDriveParameters::kinematicsCallback, this);
+      initialized_ = true;
+    }
+  }
+
+  void KinematicSteerDriveParameters::reconfigureCB(SteerDriveParametersConfig& config, uint32_t level)
+  {
+    boost::lock_guard<boost::mutex> schedule_lockguard(reconfig_mutex);
+    // ROS_WARN("reconfigureCB");
+    odom_enc_steering_angle_offset_ = config.odomEncSteeringAngleOffset;
+    steering_fix_wheel_distance_x_ = config.steering_fix_wheel_distance_x;
+    steering_fix_wheel_distance_y_ = config.steering_fix_wheel_distance_y;
+    wheel_acceleration_ = config.wheelAcceleration;
+    // reconfig_mutex.unlock();
+  }
+
+  void KinematicSteerDriveParameters::kinematicsCallback(const dynamic_reconfigure::Config::ConstPtr& param)
+  {
+    boost::lock_guard<boost::mutex> schedule_lockguard(reconfig_mutex);
+    // ROS_WARN("dynamic_reconfigure");
+    if (param != nullptr)
+    {
+      std::map<std::string, double> param_m;
+      for (auto db : param->doubles) param_m[db.name] = db.value;
+
+      auto it = param_m.find("odomEncSteeringAngleOffset");
+      if (it != param_m.end())
+      {
+        odom_enc_steering_angle_offset_ = param_m["odomEncSteeringAngleOffset"];
+        ROS_INFO("[ros_kinematics]-odomEncSteeringAngleOffset %f", odom_enc_steering_angle_offset_);
+      }
+      else ROS_WARN("[ros_kinematics]-odomEncSteeringAngleOffset %f", odom_enc_steering_angle_offset_);
+
+      it = param_m.find("steering_fix_wheel_distance_x");
+      if (it != param_m.end())
+      {
+        steering_fix_wheel_distance_x_ = param_m["steering_fix_wheel_distance_x"];
+        ROS_INFO("[ros_kinematics]-steering_fix_wheel_distance_x %f", steering_fix_wheel_distance_x_);
+      }
+      else ROS_WARN("[ros_kinematics]-steering_fix_wheel_distance_x %f", steering_fix_wheel_distance_x_);
+
+      it = param_m.find("steering_fix_wheel_distance_y");
+      if (it != param_m.end())
+      {
+        steering_fix_wheel_distance_y_ = param_m["steering_fix_wheel_distance_y"];
+        ROS_INFO("[ros_kinematics]-steering_fix_wheel_distance_y %f", steering_fix_wheel_distance_y_);
+      }
+      else ROS_WARN("[ros_kinematics]-steering_fix_wheel_distance_y %f", steering_fix_wheel_distance_y_);
+
+      it = param_m.find("wheelAcceleration");
+      if (it != param_m.end())
+      {
+        wheel_acceleration_ = param_m["wheelAcceleration"];
+        ROS_INFO("[ros_kinematics]-wheelAcceleration %f", wheel_acceleration_);
+      }
+      else ROS_WARN("[ros_kinematics]-wheelAcceleration %f", wheel_acceleration_);
+
+      param_m.clear();
+      setup_ = true;
+    }
+    else ROS_WARN("dynamic_reconfigure param is null");
+    // reconfig_mutex.unlock();
+  }
+
+
+} // namespace ros_kinematics

Devices/Packages/ros_kinematics/src/speed_limiter.cpp

@@ -0,0 +1,137 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2013, PAL Robotics, S.L.
+ *  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 PAL Robotics 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: Enrique Fernández
+ */
+
+#include <algorithm>
+
+#include <ros_kinematics/speed_limiter.h>
+
+template<typename T>
+T clamp(T x, T min, T max)
+{
+  return std::min(std::max(min, x), max);
+}
+
+namespace ros_kinematics
+{
+
+  SpeedLimiter::SpeedLimiter(
+    bool has_velocity_limits,
+    bool has_acceleration_limits,
+    bool has_jerk_limits,
+    double min_velocity,
+    double max_velocity,
+    double min_acceleration,
+    double max_acceleration,
+    double min_jerk,
+    double max_jerk
+  )
+  : has_velocity_limits(has_velocity_limits)
+  , has_acceleration_limits(has_acceleration_limits)
+  , has_jerk_limits(has_jerk_limits)
+  , min_velocity(min_velocity)
+  , max_velocity(max_velocity)
+  , min_acceleration(min_acceleration)
+  , max_acceleration(max_acceleration)
+  , min_jerk(min_jerk)
+  , max_jerk(max_jerk)
+  {
+  }
+
+  double SpeedLimiter::limit(double& v, double v0, double v1, double dt)
+  {
+    const double tmp = v;
+
+    limit_jerk(v, v0, v1, dt);
+    limit_acceleration(v, v0, dt);
+    limit_velocity(v);
+
+    return tmp != 0.0 ? v / tmp : 1.0;
+  }
+
+  double SpeedLimiter::limit_velocity(double& v)
+  {
+    const double tmp = v;
+
+    if (has_velocity_limits)
+    {
+      v = clamp(v, min_velocity, max_velocity);
+    }
+
+    return tmp != 0.0 ? v / tmp : 1.0;
+  }
+
+  double SpeedLimiter::limit_acceleration(double& v, double v0, double dt)
+  {
+    const double tmp = v;
+
+    if (has_acceleration_limits)
+    {
+      const double dv_min = min_acceleration * dt;
+      const double dv_max = max_acceleration * dt;
+
+      const double dv = clamp(v - v0, dv_min, dv_max);
+
+      v = v0 + dv;
+    }
+
+    return tmp != 0.0 ? v / tmp : 1.0;
+  }
+
+  double SpeedLimiter::limit_jerk(double& v, double v0, double v1, double dt)
+  {
+    const double tmp = v;
+
+    if (has_jerk_limits)
+    {
+      const double dv  = v  - v0;
+      const double dv0 = v0 - v1;
+
+      const double dt2 = 2. * dt * dt;
+
+      const double da_min = min_jerk * dt2;
+      const double da_max = max_jerk * dt2;
+
+      const double da = clamp(dv - dv0, da_min, da_max);
+
+      v = v0 + dv0 + da;
+    }
+
+    return tmp != 0.0 ? v / tmp : 1.0;
+  }
+
+} // namespace ros_kinematics

Devices/Packages/sick_line_guidance/.gitignore

@@ -0,0 +1,10 @@
+
+turtlebotDemo/test/scripts/sick_line_guidance_demo\.avi
+
+turtlebotDemo/iam/cmake/decision_making_parsing\.cmake
+
+turtlebotDemo/test/scripts/log/
+
+\.idea/
+
+cmake-build-debug/

Devices/Packages/sick_line_guidance/CMakeLists.txt

@@ -0,0 +1,332 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(sick_line_guidance)
+
+##
+## Compile as C++11, supported in ROS Kinetic and newer
+add_compile_options(-std=c++11 -g -Wall -Wno-reorder -Wno-sign-compare -Wno-unused-local-typedefs)
+
+## 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
+  cmake_modules
+  can_msgs 
+  canopen_chain_node
+  message_generation
+  random_numbers
+  roscpp
+  rospy
+  sensor_msgs
+  std_msgs
+)
+
+## System dependencies are found with CMake's conventions
+# find_package(Boost REQUIRED COMPONENTS system)
+find_package(TinyXML REQUIRED)
+
+
+## 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
+  MLS_Measurement.msg
+  OLS_Measurement.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
+  sensor_msgs
+  std_msgs
+  can_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 sick_line_guidance_lib
+  CATKIN_DEPENDS can_msgs canopen_chain_node message_runtime random_numbers roscpp rospy sensor_msgs std_msgs
+  DEPENDS TinyXML # system_lib
+)
+
+###########
+## Build ##
+###########
+
+## Specify additional locations of header files
+## Your package locations should be listed before other locations
+include_directories(
+  include
+  ${catkin_INCLUDE_DIRS}
+  ${TinyXML_INCLUDE_DIRS}
+)
+
+## Declare a C++ library
+add_library(${PROJECT_NAME}_lib
+  src/sick_line_guidance_can_subscriber.cpp
+  src/sick_line_guidance_can_cia401_subscriber.cpp
+  src/sick_line_guidance_can_mls_subscriber.cpp
+  src/sick_line_guidance_can_ols_subscriber.cpp
+  src/sick_line_guidance_canopen_chain.cpp
+  src/sick_line_guidance_cloud_converter.cpp
+  src/sick_line_guidance_diagnostic.cpp
+  src/sick_line_guidance_eds_util.cpp
+  src/sick_line_guidance_msg_util.cpp
+  src/sick_canopen_simu_canstate.cpp
+  src/sick_canopen_simu_device.cpp
+  src/sick_canopen_simu_verify.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/sick_line_guidance_node.cpp)
+add_executable(${PROJECT_NAME}_can_chain_node  src/sick_line_guidance_can_chain_node.cpp)
+add_executable(${PROJECT_NAME}_cloud_publisher src/sick_line_guidance_cloud_publisher.cpp)
+add_executable(${PROJECT_NAME}_can2ros_node    src/sick_line_guidance_can2ros_node.cpp)
+add_executable(${PROJECT_NAME}_ros2can_node    src/sick_line_guidance_ros2can_node.cpp)
+add_executable(sick_canopen_simu_node          src/sick_canopen_simu_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
+  random_numbers
+  canopen_ros_chain
+  ${PROJECT_NAME}_lib
+  ${${PROJECT_NAME}_EXPORTED_TARGETS}
+  ${catkin_EXPORTED_TARGETS}
+)
+add_dependencies(${PROJECT_NAME}_can_chain_node
+  canopen_ros_chain
+  ${PROJECT_NAME}_lib
+  ${${PROJECT_NAME}_EXPORTED_TARGETS}
+  ${catkin_EXPORTED_TARGETS}
+)
+add_dependencies(${PROJECT_NAME}_cloud_publisher
+  ${PROJECT_NAME}_lib
+  ${${PROJECT_NAME}_EXPORTED_TARGETS}
+  ${catkin_EXPORTED_TARGETS}
+)
+add_dependencies(${PROJECT_NAME}_can2ros_node
+  canopen_ros_chain
+  ${PROJECT_NAME}_lib
+  ${${PROJECT_NAME}_EXPORTED_TARGETS}
+  ${catkin_EXPORTED_TARGETS}
+)
+add_dependencies(${PROJECT_NAME}_ros2can_node
+  canopen_ros_chain
+  ${PROJECT_NAME}_lib
+  ${${PROJECT_NAME}_EXPORTED_TARGETS}
+  ${catkin_EXPORTED_TARGETS}
+)
+add_dependencies(${PROJECT_NAME}_lib
+  ${${PROJECT_NAME}_EXPORTED_TARGETS}
+  ${catkin_EXPORTED_TARGETS}
+)
+add_dependencies(sick_canopen_simu_node
+  random_numbers
+  canopen_ros_chain
+  ${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
+  canopen_ros_chain
+  random_numbers
+  ${PROJECT_NAME}_lib
+  ${Boost_LIBRARIES}
+  ${catkin_LIBRARIES}
+  ${TinyXML_LIBRARIES}
+)
+target_link_libraries(${PROJECT_NAME}_can_chain_node
+  canopen_ros_chain
+  ${PROJECT_NAME}_lib
+  ${Boost_LIBRARIES}
+  ${catkin_LIBRARIES}
+  ${TinyXML_LIBRARIES}
+)
+target_link_libraries(${PROJECT_NAME}_cloud_publisher
+  ${PROJECT_NAME}_lib
+  ${Boost_LIBRARIES}
+  ${catkin_LIBRARIES}
+  ${TinyXML_LIBRARIES}
+)
+target_link_libraries(${PROJECT_NAME}_can2ros_node
+  canopen_ros_chain
+  ${PROJECT_NAME}_lib
+  ${Boost_LIBRARIES}
+  ${catkin_LIBRARIES}
+  ${TinyXML_LIBRARIES}
+)
+target_link_libraries(${PROJECT_NAME}_ros2can_node
+  canopen_ros_chain
+  ${PROJECT_NAME}_lib
+  ${Boost_LIBRARIES}
+  ${catkin_LIBRARIES}
+  ${TinyXML_LIBRARIES}
+)
+target_link_libraries(${PROJECT_NAME}_lib
+  ${Boost_LIBRARIES}
+  ${catkin_LIBRARIES}
+  ${TinyXML_LIBRARIES}
+)
+target_link_libraries(sick_canopen_simu_node
+  canopen_ros_chain
+  random_numbers
+  ${PROJECT_NAME}_lib
+  ${Boost_LIBRARIES}
+  ${catkin_LIBRARIES}
+  ${TinyXML_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
+# install(PROGRAMS
+#   scripts/my_python_script
+#   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark executables and/or libraries for installation
+install(TARGETS ${PROJECT_NAME}_node ${PROJECT_NAME}_can_chain_node ${PROJECT_NAME}_cloud_publisher ${PROJECT_NAME}_can2ros_node ${PROJECT_NAME}_ros2can_node sick_canopen_simu_node ${PROJECT_NAME}_lib
+  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  RUNTIME DESTINATION ${CATKIN_PACKAGE_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/sick_line_guidance.launch
+  launch/sick_line_guidance_can2ros_node.launch
+  launch/sick_line_guidance_ols20_twin.launch
+  launch/sick_line_guidance_ros2can_node.launch
+  launch/sick_canopen_simu.launch
+  mls/SICK-MLS.eds
+  mls/sick_line_guidance_mls.yaml
+  ols/SICK_OLS10_CO.eds
+  ols/SICK_OLS20.eds
+  ols/SICK_OLS20_CO.eds
+  ols/sick_line_guidance_ols10.yaml
+  ols/sick_line_guidance_ols20.yaml
+  ols/sick_line_guidance_ols20_twin.yaml
+  test/cfg/sick_canopen_simu_cfg.xml
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+)
+
+#############
+## Testing ##
+#############
+
+## Add gtest based cpp test target and link libraries
+# catkin_add_gtest(${PROJECT_NAME}-test test/test_sick_line_guidance.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)
+
+
+if( TURTLEBOT_DEMO )
+  add_subdirectory( turtlebotDemo/agc_radar )
+  add_subdirectory( turtlebotDemo/custom_messages )
+  add_subdirectory( turtlebotDemo/gpio_handling )
+  add_subdirectory( turtlebotDemo/lidar_obstacle_detection )
+  add_subdirectory( turtlebotDemo/sick_line_guidance_demo )
+endif( TURTLEBOT_DEMO )

Devices/Packages/sick_line_guidance/LICENSE

@@ -0,0 +1,201 @@
+                                 Apache License
+                           Version 2.0, January 2004
+                        http://www.apache.org/licenses/
+
+   TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION
+
+   1. Definitions.
+
+      "License" shall mean the terms and conditions for use, reproduction,
+      and distribution as defined by Sections 1 through 9 of this document.
+
+      "Licensor" shall mean the copyright owner or entity authorized by
+      the copyright owner that is granting the License.
+
+      "Legal Entity" shall mean the union of the acting entity and all
+      other entities that control, are controlled by, or are under common
+      control with that entity. For the purposes of this definition,
+      "control" means (i) the power, direct or indirect, to cause the
+      direction or management of such entity, whether by contract or
+      otherwise, or (ii) ownership of fifty percent (50%) or more of the
+      outstanding shares, or (iii) beneficial ownership of such entity.
+
+      "You" (or "Your") shall mean an individual or Legal Entity
+      exercising permissions granted by this License.
+
+      "Source" form shall mean the preferred form for making modifications,
+      including but not limited to software source code, documentation
+      source, and configuration files.
+
+      "Object" form shall mean any form resulting from mechanical
+      transformation or translation of a Source form, including but
+      not limited to compiled object code, generated documentation,
+      and conversions to other media types.
+
+      "Work" shall mean the work of authorship, whether in Source or
+      Object form, made available under the License, as indicated by a
+      copyright notice that is included in or attached to the work
+      (an example is provided in the Appendix below).
+
+      "Derivative Works" shall mean any work, whether in Source or Object
+      form, that is based on (or derived from) the Work and for which the
+      editorial revisions, annotations, elaborations, or other modifications
+      represent, as a whole, an original work of authorship. For the purposes
+      of this License, Derivative Works shall not include works that remain
+      separable from, or merely link (or bind by name) to the interfaces of,
+      the Work and Derivative Works thereof.
+
+      "Contribution" shall mean any work of authorship, including
+      the original version of the Work and any modifications or additions
+      to that Work or Derivative Works thereof, that is intentionally
+      submitted to Licensor for inclusion in the Work by the copyright owner
+      or by an individual or Legal Entity authorized to submit on behalf of
+      the copyright owner. For the purposes of this definition, "submitted"
+      means any form of electronic, verbal, or written communication sent
+      to the Licensor or its representatives, including but not limited to
+      communication on electronic mailing lists, source code control systems,
+      and issue tracking systems that are managed by, or on behalf of, the
+      Licensor for the purpose of discussing and improving the Work, but
+      excluding communication that is conspicuously marked or otherwise
+      designated in writing by the copyright owner as "Not a Contribution."
+
+      "Contributor" shall mean Licensor and any individual or Legal Entity
+      on behalf of whom a Contribution has been received by Licensor and
+      subsequently incorporated within the Work.
+
+   2. Grant of Copyright License. Subject to the terms and conditions of
+      this License, each Contributor hereby grants to You a perpetual,
+      worldwide, non-exclusive, no-charge, royalty-free, irrevocable
+      copyright license to reproduce, prepare Derivative Works of,
+      publicly display, publicly perform, sublicense, and distribute the
+      Work and such Derivative Works in Source or Object form.
+
+   3. Grant of Patent License. Subject to the terms and conditions of
+      this License, each Contributor hereby grants to You a perpetual,
+      worldwide, non-exclusive, no-charge, royalty-free, irrevocable
+      (except as stated in this section) patent license to make, have made,
+      use, offer to sell, sell, import, and otherwise transfer the Work,
+      where such license applies only to those patent claims licensable
+      by such Contributor that are necessarily infringed by their
+      Contribution(s) alone or by combination of their Contribution(s)
+      with the Work to which such Contribution(s) was submitted. If You
+      institute patent litigation against any entity (including a
+      cross-claim or counterclaim in a lawsuit) alleging that the Work
+      or a Contribution incorporated within the Work constitutes direct
+      or contributory patent infringement, then any patent licenses
+      granted to You under this License for that Work shall terminate
+      as of the date such litigation is filed.
+
+   4. Redistribution. You may reproduce and distribute copies of the
+      Work or Derivative Works thereof in any medium, with or without
+      modifications, and in Source or Object form, provided that You
+      meet the following conditions:
+
+      (a) You must give any other recipients of the Work or
+          Derivative Works a copy of this License; and
+
+      (b) You must cause any modified files to carry prominent notices
+          stating that You changed the files; and
+
+      (c) You must retain, in the Source form of any Derivative Works
+          that You distribute, all copyright, patent, trademark, and
+          attribution notices from the Source form of the Work,
+          excluding those notices that do not pertain to any part of
+          the Derivative Works; and
+
+      (d) If the Work includes a "NOTICE" text file as part of its
+          distribution, then any Derivative Works that You distribute must
+          include a readable copy of the attribution notices contained
+          within such NOTICE file, excluding those notices that do not
+          pertain to any part of the Derivative Works, in at least one
+          of the following places: within a NOTICE text file distributed
+          as part of the Derivative Works; within the Source form or
+          documentation, if provided along with the Derivative Works; or,
+          within a display generated by the Derivative Works, if and
+          wherever such third-party notices normally appear. The contents
+          of the NOTICE file are for informational purposes only and
+          do not modify the License. You may add Your own attribution
+          notices within Derivative Works that You distribute, alongside
+          or as an addendum to the NOTICE text from the Work, provided
+          that such additional attribution notices cannot be construed
+          as modifying the License.
+
+      You may add Your own copyright statement to Your modifications and
+      may provide additional or different license terms and conditions
+      for use, reproduction, or distribution of Your modifications, or
+      for any such Derivative Works as a whole, provided Your use,
+      reproduction, and distribution of the Work otherwise complies with
+      the conditions stated in this License.
+
+   5. Submission of Contributions. Unless You explicitly state otherwise,
+      any Contribution intentionally submitted for inclusion in the Work
+      by You to the Licensor shall be under the terms and conditions of
+      this License, without any additional terms or conditions.
+      Notwithstanding the above, nothing herein shall supersede or modify
+      the terms of any separate license agreement you may have executed
+      with Licensor regarding such Contributions.
+
+   6. Trademarks. This License does not grant permission to use the trade
+      names, trademarks, service marks, or product names of the Licensor,
+      except as required for reasonable and customary use in describing the
+      origin of the Work and reproducing the content of the NOTICE file.
+
+   7. Disclaimer of Warranty. Unless required by applicable law or
+      agreed to in writing, Licensor provides the Work (and each
+      Contributor provides its Contributions) on an "AS IS" BASIS,
+      WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or
+      implied, including, without limitation, any warranties or conditions
+      of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A
+      PARTICULAR PURPOSE. You are solely responsible for determining the
+      appropriateness of using or redistributing the Work and assume any
+      risks associated with Your exercise of permissions under this License.
+
+   8. Limitation of Liability. In no event and under no legal theory,
+      whether in tort (including negligence), contract, or otherwise,
+      unless required by applicable law (such as deliberate and grossly
+      negligent acts) or agreed to in writing, shall any Contributor be
+      liable to You for damages, including any direct, indirect, special,
+      incidental, or consequential damages of any character arising as a
+      result of this License or out of the use or inability to use the
+      Work (including but not limited to damages for loss of goodwill,
+      work stoppage, computer failure or malfunction, or any and all
+      other commercial damages or losses), even if such Contributor
+      has been advised of the possibility of such damages.
+
+   9. Accepting Warranty or Additional Liability. While redistributing
+      the Work or Derivative Works thereof, You may choose to offer,
+      and charge a fee for, acceptance of support, warranty, indemnity,
+      or other liability obligations and/or rights consistent with this
+      License. However, in accepting such obligations, You may act only
+      on Your own behalf and on Your sole responsibility, not on behalf
+      of any other Contributor, and only if You agree to indemnify,
+      defend, and hold each Contributor harmless for any liability
+      incurred by, or claims asserted against, such Contributor by reason
+      of your accepting any such warranty or additional liability.
+
+   END OF TERMS AND CONDITIONS
+
+   APPENDIX: How to apply the Apache License to your work.
+
+      To apply the Apache License to your work, attach the following
+      boilerplate notice, with the fields enclosed by brackets "[]"
+      replaced with your own identifying information. (Don't include
+      the brackets!)  The text should be enclosed in the appropriate
+      comment syntax for the file format. We also recommend that a
+      file or class name and description of purpose be included on the
+      same "printed page" as the copyright notice for easier
+      identification within third-party archives.
+
+   Copyright [yyyy] [name of copyright owner]
+
+   Licensed under the Apache License, Version 2.0 (the "License");
+   you may not use this file except in compliance with the License.
+   You may obtain a copy of the License at
+
+       http://www.apache.org/licenses/LICENSE-2.0
+
+   Unless required by applicable law or agreed to in writing, software
+   distributed under the License is distributed on an "AS IS" BASIS,
+   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+   See the License for the specific language governing permissions and
+   limitations under the License.

Devices/Packages/sick_line_guidance/README.md

@@ -0,0 +1,314 @@
+# sick_line_guidance
+
+SICK Line Guidance ROS Support
+
+# Introduction
+
+The aim of this project is to provide an ROS connection for lane guidance sensors of the OLS10, OLS20 and MLS family.
+
+Users should regularly inform themselves about updates to this driver (best subscribe under "Watch").
+
+# Supported Hardware
+
+SICK optical and magnetical line sensors OLS10, OLS20 and MLS.
+
+| Product Family  | Product Information and Manuals |
+| --- | --- |
+| OLS10 | https://www.sick.com/ols10 |
+| OLS20 | https://www.sick.com/ols20 |
+| MLS   | https://www.sick.com/mls |
+
+# Installation and setup
+
+## Setup CAN hardware and driver
+
+  1. Install can-utils and socketcan:
+
+```bash
+sudo apt-get install can-utils
+sudo modprobe can
+sudo modprobe vcan
+sudo modprobe slcan
+```
+Details can be found in the following links: https://wiki.linklayer.com/index.php/SocketCAN , https://gribot.org/installing-socketcan/ , 
+https://www.kernel.org/doc/Documentation/networking/can.txt
+
+  2. Install linux driver for your CAN hardware:
+
+ROS support for sick line guidance sensors has been developed and tested using PEAK CAN adapter for the CAN communication.
+Unless you're using other CAN hardware or driver, we recommend installation of pcan usb adapter by following the installation
+instructions on [doc/pcan-linux-installation.md](doc/pcan-linux-installation.md)
+
+## Installation from Source
+
+Run the following script to install sick_line_guidance including all dependancies and packages required:
+
+```bash
+source /opt/ros/noetic/setup.bash # currently ros distro melodic and noetic are supported
+cd ~ # or change to your project path
+mkdir -p catkin_ws/src/
+cd catkin_ws/src/
+git clone https://github.com/SICKAG/sick_line_guidance.git
+git clone https://github.com/ros-industrial/ros_canopen.git
+git clone https://github.com/CANopenNode/CANopenSocket.git
+git clone https://github.com/linux-can/can-utils.git
+git clone https://github.com/ros-planning/random_numbers.git
+cd ..
+sudo apt-get install libtinyxml-dev
+sudo apt-get install libmuparser-dev
+catkin_make install
+source ./install/setup.bash
+```
+
+# Configuration
+
+If not done before, you have to set the CAN bitrate, f.e. to 125 kbit/s (default bitrate for OLS and MLS):
+```bash
+sudo ip link set can0 up type can bitrate 125000
+ip -details link show can0 
+```
+
+The can node id of the OLS or MLS device is configured in a yaml-file:
+- catkin_ws/src/sick_line_guidance/ols/sick_line_guidance_ols10.yaml for OLS10 devices
+- catkin_ws/src/sick_line_guidance/ols/sick_line_guidance_ols20.yaml for OLS20 devices
+- catkin_ws/src/sick_line_guidance/mls/sick_line_guidance_mls.yaml for MLS devices
+
+The default can node id is 0x0A. To use a different can node id, set entry "id: 0x0A" to the appropriate value in the yaml-file:
+```bash
+  node1:
+    id: 0x0A # CAN-Node-ID of can device, default: Node-ID 10=0x0A for OLS and MLS
+```
+Install the new configuration with 
+```bash
+cd catkin_ws
+catkin_make install
+source ./install/setup.bash
+```
+after modifications.
+
+See [sick_line_guidance configuration](doc/sick_line_guidance_configuration.md) for details about the 
+sick_line_guidance driver and sensor configuration. See [Configuration for multiple devices](doc/sick_line_guidance_configuration.md#configuration-for-multiple-devices) for the configuration of multiple devices, f.e. of two OLS20 devices.
+
+# Starting
+
+To start the driver for MLS or OLS, the ros package sick_line_guidance and its launch-file has to be started by roslaunch:
+
+```bash
+cd ~/catkin_ws # change working directory to the project path
+source ./install/setup.bash # set environment
+roslaunch -v --screen sick_line_guidance sick_line_guidance.launch yaml:=sick_line_guidance_ols10.yaml # start OLS10 driver
+roslaunch -v --screen sick_line_guidance sick_line_guidance.launch yaml:=sick_line_guidance_ols20.yaml # start OLS20 driver
+roslaunch -v --screen sick_line_guidance sick_line_guidance.launch yaml:=sick_line_guidance_mls.yaml   # start MLS driver
+```
+
+After successful start, you can observe the sensor measurement data in a shell by subscribing to ros topics "/ols", "/mls" and "/cloud":
+
+```bash
+source ./install/setup.bash
+rostopic list 
+rostopic echo /mls
+rostopic echo /ols
+rostopic echo /cloud
+```
+
+or you can plot the sensor positions by 
+
+```bash
+source ./install/setup.bash
+rqt_plot /mls/position[0] /mls/position[1] /mls/position[2] # plot mls positions
+rqt_plot /ols/position[0] /ols/position[1] /ols/position[2] # plot ols positions
+```
+
+You can visualize the data by starting rviz, subscribe to topic "/cloud" (PointCloud2) and select "ols_frame" or "mls_frame":
+
+```bash
+source ./install/setup.bash
+rosrun rviz rviz 
+```
+
+# Troubleshooting and diagnostics
+
+All measurements are published continously on ros topic "/mls" resp. "/ols". In addition, the current status (ok or error) 
+and (in case of errors) an error code is published on topic "/diagnostics". Messages on these topics can be views by
+```bash
+rostopic echo /mls
+rostopic echo /ols
+rostopic echo /diagnostics
+```
+The following error codes are defined in header file sick_line_guidance_diagnostic.h:
+```bash
+  /*
+   * enum DIAGNOSTIC_STATUS enumerates the possible status values of diagnostic messages.
+   * Higher values mean more severe failures.
+   */
+  typedef enum DIAGNOSTIC_STATUS_ENUM
+  {
+    OK,                       // status okay, no errors
+    EXIT,                     // sick_line_guidance exiting
+    NO_LINE_DETECTED,         // device signaled "no line detected"
+    ERROR_STATUS,             // device signaled an error (error flag set in TPDO)
+    SDO_COMMUNICATION_ERROR,  // error in SDO query, timeout on receiving SDO response
+    CAN_COMMUNICATION_ERROR,  // can communication error, shutdown and reset communication
+    CONFIGURATION_ERROR,      // invalid configuration, check configuration files
+    INITIALIZATION_ERROR,     // initialization of CAN driver failed
+    INTERNAL_ERROR            // internal error, should never happen
+  } DIAGNOSTIC_STATUS;
+```
+
+All data transmitted on the CAN bus can be displayed by candump:
+
+```bash
+candump -ta can0
+```
+
+In case of a successful installation, heartbeats and PDO messages should be visible.
+Example output (can master requests object 1018sub4 from can node 8, device responds with its serial number 0x13015015):
+```bash
+$ candump -ta can0
+(1549455524.265601)  can0  77F   [1]  05                      # heartbeat
+(1549455524.294836)  can0  608   [8]  40 18 10 04 00 00 00 00 # SDO request (0x600+nodeid), 8 byte (0x40) data, object 0x101804
+(1549455524.301181)  can0  588   [8]  43 18 10 04 15 50 01 13 # SDO response (0x580+nodeid), 4 byte (0x43) value, object 0x101804, value 0x13015015
+```
+
+Values in the object dictionary of the CAN device can be viewed by
+
+```bash
+# rosservice call /driver/get_object node1 <object_index> <cached>
+rosservice call /driver/get_object node1 1018sub4 false # query serial number 
+```
+Example output:
+```bash
+$ rosservice call /driver/get_object node1 1018sub4 false # query serial number
+success: True
+message: ''
+value: "318853141"
+```
+
+The error register of a can device (object index 0x1001) and its pdo mapped objects are published on ros topic "node1_<object_index>"
+and can be printed by rostopic:
+
+```bash
+# rostopic echo -n 1 /node1_<object_index> # print object <object_index> of can device
+rostopic echo -n 1 /node1_1001 # print error register 0x1001 of can device
+```
+Example output:
+```bash
+$ rostopic echo -n 1 /node1_1001
+data: 0
+```
+
+For test purposes or in case of hardware problems, cansend can be used to send CAN messages. Example:
+
+```bash
+cansend can0 000#820A # NMT message to can device 0x0A: 0x82, reset communication
+cansend can0 60A#4F01100000000000 # PDO request: read error register 1001
+```
+
+## Simulation and testing
+
+A software simulation is available for test purposes. This simulation generates synthetical can frames from an input xml-file, and verifies the measurement messages published by
+the sick_line_guidance driver. By sending specified can frames, the complete processing chain of the ros driver can be verified by comparing the actual measurement messages 
+with the expected results. See https://github.com/SICKAG/sick_line_guidance/tree/master/doc/sick_canopen_simu.md for further details.
+
+## TurtleBot demonstration
+
+A demonstration of SICK line guidance with a TurtleBot robot is included in folder sick_line_guidance/turtlebotDemo. 
+Please see [turtlebotDemo/README.md](turtlebotDemo/README.md) for further details.
+
+## FAQ
+
+### "Network is down"
+
+:question: Question: 
+```bash
+candump -ta can0
+```
+gives the result
+```bash
+read: Network is down
+```
+
+:white_check_mark: Answer: 
+(Re-)start can interface by the following commands:
+```bash
+sudo ip link set can0 type can
+sudo ip link set can0 up type can bitrate 125000 # configure the CAN bitrate, f.e. 125000 bit/s
+```
+
+### "candump gives no answer"
+
+:question: Question: 
+```bash
+candump -ta can0
+```
+gives no results.
+
+:white_check_mark: Answer: 
+Check the baud rate of your device. For a brand new OLS10 this could be 250000 Baud. 
+For OLS10 please check the baud rate setting by using the device panel (read operation manual of your device).
+
+### "device or resource busy"
+
+:question: Question: 
+```bash
+sudo ip link set can0 up type can bitrate 125000
+```
+gives the result:
+```bash
+RTNETLINK answers: Device or resource busy
+```
+
+:white_check_mark: Answer: 
+Check the baud rate of your device. For a brand new OLS10 this could be 250000 Baud. 
+For OLS10 please check the baud rate setting by using the device panel (read operation manual of your device).
+After checking (and changing) the baud rate unplug and replug the usb connector.
+
+
+### "Device 'can0' does not exist"
+
+:question: Question: After start, the message
+```bash
+Device "can0" does not exist.
+```
+is displayed.
+
+:white_check_mark: Answer: 
+- Check power supply
+- Unplug, replug and restart PEAK-USB-Adapter
+- If you use a PEAK-USB-Adapter and the error message still displays, re-install the PCAN-driver. 
+PCAN driver can be overwritten by a default can driver due to system updates ("mainline drivers removed and blacklisted in /etc/modprobe.d/blacklist-peak.conf"). 
+In this case, the PCAN driver must be re-installed. See the quick installation guide https://github.com/SICKAG/sick_line_guidance/tree/master/doc/pcan-linux-installation.md
+
+### "Configuration changes do not take effect"
+
+:question: Question: After editing configuration files, the configuration changes do not take effect.
+
+:white_check_mark: Answer: Modified configuration files have to be installed by
+```bash
+cd catkin_ws
+catkin_make install
+source ./install/setup.bash
+```
+Restart the driver for MLS or OLS. To avoid potential errors due to previous ros nodes or processes still running, 
+you might kill all ros nodes and the ros core by 
+```bash
+rosnode kill -a    # kill all ros nodes
+killall rosmaster  # kill ros core
+```
+
+Please note, that this kills all ros processes, not just those required for sick_line_guidance.
+
+### "Debugging"
+
+:question: Question: How can I run sick_line_guidance or ros_canopen in a debugger, e.g. gdb
+
+:white_check_mark: Answer: A ros node can be started in gdb with prefix gdb, e.g.
+
+```bash
+rosrun --prefix 'gdb -ex run --args' sick_line_guidance ...
+```
+
+or with argument `launch-prefix="gdb -ex run - -args"` in the launchfile, e.g.
+```
+<node name="sick_line_guidance_can_chain_node" pkg="sick_line_guidance" type="sick_line_guidance_can_chain_node" launch-prefix="gdb -ex run - -args" output="screen" >
+```

Devices/Packages/sick_line_guidance/doc/copyright.txt

@@ -0,0 +1,54 @@
+/*
+ * 
+ * Copyright (C) 2019 Ing.-Buero Dr. Michael Lehning, Hildesheim
+ * Copyright (C) 2019 SICK AG, Waldkirch
+ * 
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ * 
+ *       http://www.apache.org/licenses/LICENSE-2.0
+ *
+ *   Unless required by applicable law or agreed to in writing, software
+ *   distributed under the License is distributed on an "AS IS" BASIS,
+ *   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ *   See the License for the specific language governing permissions and
+ *   limitations under the License.
+ *
+ * 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 SICK AG nor the names of its
+ *       contributors may be used to endorse or promote products derived from
+ *       this software without specific prior written permission
+ *     * Neither the name of Ing.-Buero Dr. Michael Lehning 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.
+ *
+ * Authors:
+ *         Michael Lehning <michael.lehning@lehning.de>
+ *
+ * Copyright 2019 SICK AG
+ * Copyright 2019 Ing.-Buero Dr. Michael Lehning
+ * 
+ */
+

Devices/Packages/sick_line_guidance/doc/pcan-linux-installation.md

@@ -0,0 +1,70 @@
+# pcan-linux-installation
+
+ROS support for sick line guidance sensors has been developed and tested using PEAK CAN adapter for the CAN communication. 
+This is a quick howto install linux driver for the pcan usb adapter.
+
+# Installation
+
+The following installation is recommended:
+
+  1. Download PCAN-View for Linux from https://www.peak-system.com/linux/ or https://www.peak-system.com/quick/ViewLinux_amd64/pcanview-ncurses_0.8.7-0_amd64.deb and install:
+```bash
+sudo apt-get install libncurses5
+sudo dpkg --install pcanview-ncurses_0.8.7-0_amd64.deb
+```
+
+  2. If you're running ROS in a virtual machine, make sure the usb-port for the PCAN-USB-adapter is connected to your VM. 
+
+  3. Download and unzip peak-linux-driver-8.17.0.tar.gz from https://www.peak-system.com/quick/PCAN-Linux-Driver
+
+
+  4. Install the linux driver and required packages:
+```bash
+cd peak-linux-driver-8.17.0
+# install required packages
+sudo apt-get install can-utils
+sudo apt-get install gcc-multilib
+sudo apt-get install libelf-dev
+sudo apt-get install libpopt-dev
+sudo apt-get install tree
+# build and install pcan driver
+make clean
+make NET=NETDEV_SUPPORT
+sudo make install
+# install the modules
+sudo modprobe pcan
+sudo modprobe can
+sudo modprobe vcan
+sudo modprobe slcan
+# setup and configure "can0" net device
+sudo ip link set can0 type can
+sudo ip link set can0 up type can bitrate 125000 # configure the CAN bitrate, f.e. 125000 bit/s
+# check installation
+./driver/lspcan --all       # should print "pcanusb32" and pcan version
+tree /dev/pcan-usb          # should show a pcan-usb device
+ip -a link                  # should print some "can0: ..." messages
+ip -details link show can0  # should print some details about "can0" net device
+```
+Example output after successfull installation of a pcan usb adapter:
+```bash
+user@ubuntu-ros:~/peak-linux-driver-8.17.0$ ./driver/lspcan --all
+pcanusb32	CAN1	-	8MHz	125k	ACTIVE	-	1969	0	0
+user@ubuntu-ros:~/peak-linux-driver-8.17.0$ tree /dev/pcan-usb
+/dev/pcan-usb
+├── 0
+│   └── can0 -> ../../pcanusb32
+└── devid=5 -> ../pcanusb32
+user@ubuntu-ros:~/peak-linux-driver-8.17.0$ ip -a link
+3: can0: <NOARP> mtu 16 qdisc noop state DOWN mode DEFAULT group default qlen 10
+    link/can 
+user@ubuntu-ros:~/peak-linux-driver-8.17.0$ ip -details link show can0
+3: can0: <NOARP> mtu 16 qdisc noop state DOWN mode DEFAULT group default qlen 10
+    link/can  promiscuity 0 
+    can state STOPPED restart-ms 0 
+	  bitrate 500000 sample-point 0.875 
+	  tq 125 prop-seg 6 phase-seg1 7 phase-seg2 2 sjw 1
+	  pcan: tseg1 1..16 tseg2 1..8 sjw 1..4 brp 1..64 brp-inc 1
+	  clock 8000000numtxqueues 1 numrxqueues 1 gso_max_size 65536 gso_max_segs 65535 
+```
+
+See https://www.peak-system.com/fileadmin/media/linux/files/PCAN-Driver-Linux_UserMan_eng.pdf for further details.

Devices/Packages/sick_line_guidance/doc/sick_canopen_simu.md

@@ -0,0 +1,128 @@
+# Simulation and testing
+
+sick_canopen_simu implements a software simulation of MLS and OLS devices for test purposes. This simulation generates synthetical can frames from an input xml-file,
+and verifies the measurement messages published by the sick_line_guidance driver. By sending specified can frames, the complete processing chain of the ros driver can be verified
+by comparing the actual measurement messages with the expected results. In addition, error messages and error handling can be tested, which otherwise can be challenging.
+
+Please note, that sick_canopen_simu does not implement or simulate the behavior of the MLS or OLS hardware. It just sends specified can frames and checks the measurement messages
+from the sick_line_guidance driver after entering the operational state. The purpose of sick_canopen_simu is driver verification and testing, not a fully functional simulation of
+can devices.
+
+## Usage
+
+To test the sick_line_guidance driver with simulated can frames, four ros nodes have to be started: 
+- sick_line_guidance_ros2can_node to send ros messages of type can::Frame to the can driver, which sends the frames to the can bus,
+- sick_line_guidance_can2ros_node to receive can frames and to publish them as ros messages of type can::Frame,
+- sick_canopen_simu to read the simulation data from file, to convert them to can::Frame messages and to verify the resulting sick_line_guidance messages from the driver,
+- sick_line_guidance_node, i.e. the sick_line_guidance driver.
+
+Example for simulation and test of the OLS20 driver:
+```bash
+# Start can2ros converter
+roslaunch -v sick_line_guidance sick_line_guidance_can2ros_node.launch &
+sleep 5
+# Start ros2can converter
+roslaunch -v sick_line_guidance sick_line_guidance_ros2can_node.launch &
+sleep 5
+# Start OLS20 simulation
+roslaunch -v --screen sick_line_guidance sick_canopen_simu.launch device:=OLS20 &
+sleep 5
+# Start OLS driver incl. canopen_chain_node, sick_line_guidance_node and sick_line_guidance_cloud_publisher
+roslaunch -v --screen sick_line_guidance sick_line_guidance.launch yaml:=sick_line_guidance_ols20.yaml
+```
+
+The same example works for MLS, too, with slightly different arguments (`device:=MLS` and `yaml:=sick_line_guidance_mls.yaml`):
+```bash
+# Start can2ros converter
+roslaunch -v sick_line_guidance sick_line_guidance_can2ros_node.launch &
+sleep 5
+# Start ros2can converter
+roslaunch -v sick_line_guidance sick_line_guidance_ros2can_node.launch &
+sleep 5
+# Start MLS simulation
+roslaunch -v --screen sick_line_guidance sick_canopen_simu.launch device:=MLS &
+sleep 5
+# Start MLS driver incl. canopen_chain_node, sick_line_guidance_node and sick_line_guidance_cloud_publisher
+roslaunch -v --screen sick_line_guidance sick_line_guidance.launch yaml:=sick_line_guidance_mls.yaml
+```
+
+To simulate and test the OLS10 driver, just use the parameter `device:=OLS10` and `yaml:=sick_line_guidance_ols10.yaml`.
+
+If the verification of measurement messages failed, an error message is displayed. 
+
+To run an automated unittest of both the MLS, OLS10 and OLS20 driver, script `runsimu.bash` in folder `sick_line_guidance/test/scripts` is provided: 
+```bash
+cd catkin_ws/src/sick_line_guidance/test/scripts
+./runsimu.bash
+```
+
+## Configuration
+
+Settings and testcases for the simulation are specified in file `sick_line_guidance/test/cfg/sick_canopen_simu_cfg.xml`. To modify the current unittest
+or to append further testcases, open this file in editor, save it and install the new configuration by 
+```bash
+cd catkin_ws
+catkin_make install
+source ./install/setup.bash
+```
+
+Configuration file sick_canopen_simu_cfg.xml has three sections:
+
+1. **sick_canopen_simu/SDO_config** : A lookup table for SDO response from SDO get request. 
+    Whenever the ros driver sends a SDO request, the simulation responds with a SDO request from this table.
+ 
+    Example:
+    ```bash
+    <SDO_config>    
+    <sdo request="0x4018100400000000" response="0x4318100411AE2201" /> <!-- 1018sub4: Serial number -->
+    <sdo request="0x4018200000000000" response="0x4F18200000000000" /> <!-- 2018: OLS dev_status -->
+    </SDO_config>    
+    ```
+    The simulation will send a SDO response 4318100411AE2201 when receiving SDO request 4018100400000000 (serial number, object 1018sub4).
+    SDO response 4F18200000000000 will be sent after receiving a SDO request 4018200000000000 (OLS device status, object 2018).
+    Note: SDO responses of PDO mapped objects are modified, whenever a PDO is generated by the simulation.
+
+2. **sick_canopen_simu/OLS20/PDO_config** : A list of sensor states transmitted by PDOs to simulate an OLS20 device. 
+    This list specifies the measurement data transmitted by PDOs (lcp1, lcp2, lcp3, width1, width2, width3, status, barcode, barcode center, line quality and line intensities).
+    Each measurement is transmitted 25 times with a rate of 50 hertz (PDOs in 20 milliseconds intervall, configurable in the launch file sick_canopen_simu.launch)
+    before switching to the next sensor state. All measurements are repeated in an endless loop while the simulation is running.
+    
+    Example:
+    ```bash
+    <OLS20>
+    <PDO_config>
+    <pdo lcp1="0.000" lcp2="0.002"  lcp3="0.000"  width1="0.000" width2="0.012" width3="0.000" status="0x02" barcode="0x00"       devstatus="0x00" error="0x00" barcodecenter="0.000" linequality="0" lineintensity1="0" lineintensity2="0" lineintensity3="0" frame_id="ols_simulation_frame"/>
+    <pdo lcp1="0.010" lcp2="-0.020" lcp3="-0.030" width1="0.031" width2="0.032" width3="0.033" status="0xC7" barcode="0x12345678" devstatus="0x00" error="0x00" barcodecenter="0.050" linequality="7" lineintensity1="3" lineintensity2="9" lineintensity3="8" frame_id="ols_simulation_frame"/>
+    </PDO_config>
+    </OLS20>
+    ```
+    The simulation will convert the specified sensor states and transmit PDO1 and PDO2 every 20 milliseconds.
+
+3. **sick_canopen_simu/OLS10/PDO_config** : A list of sensor states transmitted by PDOs to simulate an OLS10 device.
+   This list is identical to OLS20, except that barcode center, line quality and line intensities are not supported by OLS10 devices and therefor have to be set to 0. 
+
+    Example:
+    ```bash
+    <OLS10>
+    <PDO_config>
+    <pdo lcp1="0.000" lcp2="0.002"  lcp3="0.000"  width1="0.000" width2="0.012" width3="0.000" status="0x02" barcode="0x00"       devstatus="0x00" error="0x00" barcodecenter="0.000" linequality="0" lineintensity1="0" lineintensity2="0" lineintensity3="0" frame_id="ols_simulation_frame"/>
+    <pdo lcp1="0.010" lcp2="-0.020" lcp3="-0.030" width1="0.031" width2="0.032" width3="0.033" status="0xC7" barcode="0x12345678" devstatus="0x00" error="0x00" barcodecenter="0.000" linequality="0" lineintensity1="0" lineintensity2="0" lineintensity3="0" frame_id="ols_simulation_frame"/>
+    </PDO_config>
+    </OLS10>
+    ```
+
+4. **sick_canopen_simu/MLS/PDO_config** : A list of sensor states transmitted by PDOs to simulate a MLS device. 
+    This list specifies the measurement data transmitted by PDOs (lcp1, lcp2, lcp3, status and #lcp).
+    Each measurement is transmitted 5 times with a rate of 50 hertz (PDOs in 20 milliseconds intervall, configurable in the launch file sick_canopen_simu.launch)
+    before switching to the next sensor state. All measurements are repeated in an endless loop while the simulation is running.
+    
+    Example:
+    ```bash
+    <MLS>
+    <PDO_config>
+    <pdo lcp1="0.000"  lcp2="0.002"  lcp3="0.000"  status="0x01" lcp="0x02" error="0x00" frame_id="mls_simulation_frame"/>
+    <pdo lcp1="-0.005" lcp2="-0.006" lcp3="-0.007" status="0x41" lcp="0x57" error="0x00" frame_id="mls_simulation_frame"/>
+    </PDO_config>
+    </MLS>
+    ```
+    The simulation will convert the specified sensor states and transmit PDO1 every 20 milliseconds.

Devices/Packages/sick_line_guidance/doc/sick_line_guidance_configuration.md

@@ -0,0 +1,242 @@
+# sick_line_guidance configuration
+
+The ROS drivers for MLS and OLS devices are configured by launch and yaml files. You can modify the default configuration by editing these files.
+Please note, that an invalid or improper configuration may cause errors and unexpected results.
+
+The basic configuration uses the default values for OLS and MLS devices. All changes should be tested carefully.
+
+To start the MLS resp. OLS driver, just launch sick_line_guidance with a yaml-configuration file:
+```bash
+roslaunch sick_line_guidance sick_line_guidance.launch yaml:=sick_line_guidance_mls.yaml   # start MLS driver
+roslaunch sick_line_guidance sick_line_guidance.launch yaml:=sick_line_guidance_ols10.yaml # start OLS10 driver
+roslaunch sick_line_guidance sick_line_guidance.launch yaml:=sick_line_guidance_ols20.yaml # start OLS20 driver
+```
+
+The launch file "sick_line_guidance.launch" configures the common driver settings, f.e. the ros nodes to start and the ros topics to be used by 
+sick_line_guidance. Unless special purposes, these settings do not require customization.
+
+The 3rd argument to roslaunch provides the driver with a yaml-file: sick_line_guidance_mls.yaml for MLS resp. sick_line_guidance_ols10.yaml for OLS10
+or sick_line_guidance_ols20.yaml for OLS20.
+These two files contain the settings of the can devices and may require customization.
+
+## yaml configuration for MLS
+
+This is the default configuration for a MLS device in file mls/sick_line_guidance_mls.yaml:
+```yaml
+bus:
+  device: can0
+  driver_plugin: can::SocketCANInterface
+  master_allocator: canopen::SimpleMaster::Allocator
+sync:
+  interval_ms: 10
+  overflow: 0
+nodes:
+  node1:
+    id: 0x0A # CAN-Node-ID of can device, default: Node-ID 10=0x0A for OLS and MLS
+    eds_pkg: sick_line_guidance # package name for relative path
+    eds_file: SICK-MLS.eds      # path to EDS/DCF file
+    publish: ["1001","1018sub1","1018sub4","2021sub1!","2021sub2!","2021sub3!","2021sub4!","2022!"]
+    # MLS: 1001 = error register, 1018sub1 = VendorID, 1018sub4 = SerialNumber, TPDO1 = 0x2021sub1 to 0x2021sub4 and 0x2022
+    
+    # sick_line_guidance configuration of this node:
+    sick_device_family: "MLS"              # can devices of OLS10, OLS20 or MLS family currently supported
+    sick_topic: "mls"                      # MLS_Measurement messages are published in topic "/mls"
+    sick_frame_id: "mls_measurement_frame" # MLS_Measurement messages are published frame_id "mls_measurement_frame"
+
+    # device configuration of writable parameter by dcf_overlay: "objectindex": "parametervalue"
+    # example: "2027": "0x01" # Object 2027 (sensorFlipped, defaultvalue 0x00) will be configured with value 0x01
+    # dcf_overlay:
+    #   "2028sub1": "0x01" # UseMarkers (0 = disable marker detection, 1 = enable marker detection), UINT8, DataType=0x0005, defaultvalue=0
+    #   "2028sub2": "0x02" # MarkerStyle (0 = disable marker detection, 1 = standard mode, 2 = extended mode), UINT8, DataType=0x0005, defaultvalue=0
+    #   "2028sub3": "0x01" # FailSafeMode (0 = disabled, 1 = enabled), UINT8, DataType=0x0005, defaultvalue=0
+    #   "2025":     "1000" # Min.Level, UINT16, DataType=0x0006, defaultvalue=600
+    #   "2026":        "1" # Offset [mm] Nullpunkt, INT16, DataType=0x0003, defaultvalue=0
+    #   "2027":     "0x01" # sensorFlipped, UINT8, DataType=0x0005, defaultvalue=0
+    #   "2029":     "0x01" # LockTeach, UINT8, DataType=0x0005, defaultvalue=0
+```
+
+The following table describes these settings in detail:
+
+| parameter name | default value | details |
+| --- | --- | --- |
+| bus / device | can0 | System name of can interface: net device driver for can hardware (f.e. pcan adapter) are installed to a net device named "can0". Check by system command "ifconfig -a"  |
+| bus / driver_plugin | can:: SocketCANInterface | Implementation of interface SocketCAN, currently only can::SocketCANInterface is supported. |
+| bus / master_allocator | canopen:: SimpleMaster:: Allocator | Implementation of CAN master (network manager), currently only canopen::SimpleMaster::Allocator is supported. |
+| sync / interval_ms | 10 | Timeinterval for can sync messages in milliseconds (CAN master will send a nmt sync message each 10 ms) |
+| sync / overflow | 0 | Length of can sync message (0: sync message does not contain any data byte) |
+| nodes | node1 | Name of the first can node configured. If you're running more than 1 can device (multiple MLS or OLS devices), you can configure each device by providing a named section for each node. |
+| **nodes / node1 / id** | **0x0A** | **CAN-Node-ID of can device (default: Node-ID 10=0x0A for OLS and MLS). Change this id, if your MLS uses a different CAN-ID. :exclamation:** |
+| nodes / node1 / eds_pkg | sick_line_guidance | Name of sick_line_guidance ros package. Required to resolve filenames and should not be modified. |
+| nodes / node1 / eds_file | SICK-MLS.eds | Electronic datasheet for your MLS device  |
+| nodes / node1 / publish | ["1001", "1018sub1", "1018sub4", "2021sub1!", "2021sub2!", "2021sub3!", "2021sub4!", "2022!"] | List of published objects in the object dictionary of a can device. These objects are both published on ros topic <nodename>_<objectindex> and required by driver internal callbacks to handle PDO messages. Do not remove any PDO mapped objects from this list! |
+| nodes / node1 / sick_device_family | "MLS" | Informs the ros driver that this can device is a MLS. Currently supported values are "OLS10", "OLS20" or "MLS". |
+| nodes / node1 / sick_topic | "mls" | Name of the ros topic for publishing MLS measurement messages. If modified, the same topic must be set in file sick_line_guidance.launch for the sick_line_guidance_cloud_publisher node (param name="mls_topic_publish" type="str" value="mls"). Otherwise, measurement messges won't be transformed to PointCloud2 messsages. |
+| nodes / node1 / sick_frame_id | "mls_measurement_frame" | Ros frame id of the MLS measurement messages. |
+| **# nodes / node1 / dcf_overlay** |  | **Configuration of device parameter. To set an object in the object dictionary of a can device to a specific value at startup, append dcf_overlay entries with "objectindex": "parametervalue". This section is intensionally left empty to use default values. See operation manual for possible settings. :exclamation:** |
+
+Device specific settings can be configured in section "nodes/node1/dcf_overlay". Example to activate marker detection in extended mode (failsafe configuration):
+```yaml
+    dcf_overlay:
+      "2028sub1": "0x01" # UseMarkers (0 = disable marker detection, 1 = enable marker detection), UINT8, DataType=0x0005, defaultvalue=0
+      "2028sub2": "0x02" # MarkerStyle (0 = disable marker detection, 1 = standard mode, 2 = extended mode), UINT8, DataType=0x0005, defaultvalue=0
+      "2028sub3": "0x01" # FailSafeMode (0 = disabled, 1 = enabled), UINT8, DataType=0x0005, defaultvalue=0
+```
+
+## yaml configuration for OLS
+
+The configuration of an OLS device is pretty much the same to MLS, just with a different PDO mapping and a different eds-file. Therefore, the object list specified by `publish`
+and by `dcf_overlay` vary and the eds-file "SICK_OLS20.eds" is used. Otherwise, the same configuration apply.
+
+This is the default configuration for a OLS10 device in file ols/sick_line_guidance_ols10.yaml:
+```yaml
+bus:
+  device: can0
+  driver_plugin: can::SocketCANInterface
+  master_allocator: canopen::SimpleMaster::Allocator
+sync:
+  interval_ms: 10
+  overflow: 0
+nodes:
+  node1:
+    id: 0x0A # CAN-Node-ID of can device, default: Node-ID 10=0x0A for OLS and MLS
+    eds_pkg: sick_line_guidance # package name for relative path
+    eds_file: SICK_OLS10_CO.eds    # path to EDS/DCF file
+    publish: ["1001","1018sub1","1018sub4","2021sub1!","2021sub2!","2021sub3!","2021sub4!","2021sub5!","2021sub6!","2021sub7!","2021sub8!"]
+    # OLS10: 1001 = error register, 1018sub1 = VendorID, 1018sub4 = SerialNumber, TPDOs 0x2021sub1 to 0x2021sub8
+    
+    # sick_line_guidance configuration of this node:
+    sick_device_family: "OLS10"            # can devices currently supported: "OLS10", "OLS20" or "MLS"
+    sick_topic: "ols"                      # OLS_Measurement messages are published in topic "/ols"
+    sick_frame_id: "ols_measurement_frame" # OLS_Measurement messages are published with frame_id "ols_measurement_frame"
+
+    # device configuration of writable parameter by dcf_overlay: "objectindex": "parametervalue"
+    # example: "2001sub5": "0x01" # Object 2001sub5 (sensorFlipped, defaultvalue 0x00) will be configured with value 0x01
+    # dcf_overlay:
+    #   "2001sub5": "0x01"  # sensorFlipped, UINT8, DataType=0x0005, defaultvalue=0
+    #   "2002sub1": "0.015" # Typ. Width, FLOAT32, DataType=0x0008
+    #   "2002sub2": "0.001" # Min. Width, FLOAT32, DataType=0x0008
+    #   "2002sub3": "0.050" # Max. Width, FLOAT32, DataType=0x0008
+```
+
+This is the default configuration for a OLS20 device in file ols/sick_line_guidance_ols20.yaml:
+```yaml
+bus:
+  device: can0
+  driver_plugin: can::SocketCANInterface
+  master_allocator: canopen::SimpleMaster::Allocator
+sync:
+  interval_ms: 10
+  overflow: 0
+nodes:
+  node1:
+    id: 0x0A # CAN-Node-ID of can device, default: Node-ID 10=0x0A for OLS and MLS
+    eds_pkg: sick_line_guidance # package name for relative path
+    eds_file: SICK_OLS20_CO.eds    # path to EDS/DCF file
+    publish: ["1001","1018sub1","1018sub4","2021sub1!","2021sub2!","2021sub3!","2021sub4!","2021sub5!","2021sub6!","2021sub7!","2021sub8!"]
+    # OLS20: 1001 = error register, 1018sub1 = VendorID, 1018sub4 = SerialNumber, TPDOs 0x2021sub1 to 0x2021sub8
+    
+    # sick_line_guidance configuration of this node:
+    sick_device_family: "OLS20"            # can devices currently supported: "OLS10", "OLS20" or "MLS"
+    sick_topic: "ols"                      # OLS_Measurement messages are published in topic "/ols"
+    sick_frame_id: "ols_measurement_frame" # OLS_Measurement messages are published with frame_id "ols_measurement_frame"
+
+    # device configuration of writable parameter by dcf_overlay: "objectindex": "parametervalue"
+    # example: "2001sub5": "0x01" # Object 2001sub5 (sensorFlipped, defaultvalue 0x00) will be configured with value 0x01
+    # dcf_overlay:
+    #   "2001sub5": "0x01"  # sensorFlipped, UINT8, DataType=0x0005, defaultvalue=0
+    #   "2002sub1": "0.015" # Typ. Width, FLOAT32, DataType=0x0008
+    #   "2002sub2": "0.001" # Min. Width, FLOAT32, DataType=0x0008
+    #   "2002sub3": "0.050" # Max. Width, FLOAT32, DataType=0x0008
+```
+
+The following table describes these settings in detail:
+
+| parameter name | default value | details |
+| --- | --- | --- |
+| bus / device | can0 | System name of can interface: net device driver for can hardware (f.e. pcan adapter) are installed to a net device named "can0". Check by system command "ifconfig -a"  |
+| bus / driver_plugin | can:: SocketCANInterface | Implementation of interface SocketCAN, currently only can::SocketCANInterface is supported. |
+| bus / master_allocator | canopen:: SimpleMaster:: Allocator | Implementation of CAN master (network manager), currently only canopen::SimpleMaster::Allocator is supported. |
+| sync / interval_ms | 10 | Timeinterval for can sync messages in milliseconds (CAN master will send a nmt sync message each 10 ms) |
+| sync / overflow | 0 | Length of can sync message (0: sync message does not contain any data byte) |
+| nodes | node1 | Name of the first can node configured. If you're running more than 1 can device (multiple MLS or OLS devices), you can configure each device by providing a named section for each node. |
+| **nodes / node1 / id** | **0x0A** | **CAN-Node-ID of can device (default: Node-ID 10=0x0A for OLS and MLS). Change this id, if your OLS uses a different CAN-ID. :exclamation:** |
+| nodes / node1 / eds_pkg | sick_line_guidance | Name of sick_line_guidance ros package. Required to resolve filenames and should not be modified. |
+| nodes / node1 / eds_file | SICK_OLS20.eds | Electronic datasheet for your OLS device  |
+| nodes / node1 / publish | ["1001", "1018sub1", "1018sub4", "2021sub1!", "2021sub2!", "2021sub3!", "2021sub4!", "2021sub5!", "2021sub6!", "2021sub7!", "2021sub8!"] | List of published objects in the object dictionary of a can device. These objects are both published on ros topic <nodename>_<objectindex> and required by driver internal callbacks to handle PDO messages. Do not remove any PDO mapped objects from this list! |
+| nodes / node1 / sick_device_family | "OLS20" | Informs the ros driver that this can device is an OLS20. Currently supported values are "OLS10", "OLS20" or "MLS". |
+| nodes / node1 / sick_topic | "ols" | Name of the ros topic for publishing OLS measurement messages. If modified, the same topic must be set in file sick_line_guidance.launch for the sick_line_guidance_cloud_publisher node (param name="ols_topic_publish" type="str" value="ols"). Otherwise, measurement messges won't be transformed to PointCloud2 messsages. |
+| nodes / node1 / sick_frame_id | "ols_measurement_frame" | Ros frame id of the OLS measurement messages. |
+| **# nodes / node1 / dcf_overlay** |  | **Configuration of device parameter. To set an object in the object dictionary of a can device to a specific value at startup, append dcf_overlay entries with "objectindex": "parametervalue". This section is intensionally left empty to use default values. See operation manual for possible settings. :exclamation:** |
+
+Device specific settings can be configured in section "nodes/node1/dcf_overlay". Example to set object 2001sub5 (sensorFlipped):
+```yaml
+    dcf_overlay:
+      "2001sub5": "0x01" # sensorFlipped, UINT8, DataType=0x0005, defaultvalue=0
+```
+
+## Summary of the yaml configuration for MLS and OLS devices
+
+Set the CAN-ID of your device with `id: 0x0A # CAN-Node-ID of MLS (default: 0x0A)` and
+configure device specific settings in section `dcf_overlay` by appending a line `"objectindex": "parametervalue"` for each parameter.
+
+## Configuration for multiple devices
+
+If you want to use multiple can devices (MLS or OLS) on one system, you have to append multiple nodes in your yaml-file. Each node must contain the configuration of one can device as shown above. Make sure to use different node names for each device:
+```yaml
+nodes:
+  node1:
+    id: ... # CAN-ID for the 1. device
+  node2:
+    id: ... # CAN-ID for the 2. device
+```
+
+If you configure different measurement topics for each can device (f.e. `sick_topic: "ols20A"` for the first OLS20 and `sick_topic: "ols20B"` for the second OLS20 device), you have to start multiple sick_line_guidance_cloud_publisher nodes, too. Otherwise, measurement messges from this device can't be transformed to PointCloud2 messsages.
+
+Append multiple sick_line_guidance_cloud_publisher nodes in file sick_line_guidance.launch, each sick_line_guidance_cloud_publisher node configured with the corresponding 
+ros topic in parameter `"mls_topic_publish"` resp. `"ols_topic_publish"`.
+
+Launchfile [sick_line_guidance_ols20_twin.launch](../launch/sick_line_guidance_ols20_twin.launch) and yaml-configuration [sick_line_guidance_ols20_twin.yaml](../ols/sick_line_guidance_ols20_twin.yaml) show an example how to run two OLS20 devices with can node ids 0x0A and 0x0B. Run the example with
+```
+rosrun rviz rviz &
+roslaunch -v --screen sick_line_guidance sick_line_guidance_ols20_twin.launch
+```
+The point clouds of both OLS20 devices will be published on topic "cloudA" and "cloudB" with frame ids "olsA_frame" and "olsB_frame". Use a static_transform_publisher to setup a transform for each OLS frame to a reference coordinate system, e.g.
+```
+rosrun tf static_transform_publisher 0 0 0 0 0 0 olsA_frame cloud 10
+rosrun tf static_transform_publisher 0 0 0 0 0 0 olsB_frame cloud 10
+```
+
+## Read and write parameter during runtime
+
+You can read and write to the object dictionary of a can device on runtime, too, by using the ros services implemented by the canopen driver.
+
+To read a value from the object dictionary, you can run
+```bash
+rosservice call /driver/get_object "{node: '<nodename>', object: '<objectindex>', cached: <true|false>}"
+```
+in your terminal. Example to read object 2001sub5 (sensorFlipped) from node1:
+```bash
+rosservice call /driver/get_object "{node: 'node1', object: '2001sub5', cached: false}"
+```
+This command results in a can upload command using a SDO with object 2001sub5. The SDO response will be converted and displayed if the command succeded.
+
+To write a value to the object dictionary, you can run
+```bash
+rosservice call /driver/get_object "{node: '<nodename>', object: '<objectindex>', value: '<parametervalue>', cached: <true|false>}"
+```
+in the terminal. Example to write value 0x01 to object 2001sub5 (sensorFlipped):
+```bash
+rosservice call /driver/set_object "{node: 'node1', object: '2001sub5', value: '0x01', cached: false}"
+```
+This command results in a can upload command using a SDO with object 2001sub5.
+
+## Configuration of CAN node id
+
+The node id of a CAN device can be configured e.b. with [PCAN-View](https://www.peak-system.com/PCAN-View.242.0.html):<br/>
+![screenshot01.png](pcan-config/screenshot01.png)<br/>
+![screenshot02.png](pcan-config/screenshot02.png)<br/>
+
+## Installation and bus termination
+
+The following screenshot shows the installation and bus termination for two OLS devices:<br/>
+![screenshot03.png](pcan-config/screenshot03.png)

Devices/Packages/sick_line_guidance/include/sick_line_guidance/sick_canopen_simu_canstate.h

@@ -0,0 +1,130 @@
+/*
+ * sick_canopen_simu_canstate implements a state engine for can.
+ *
+ * Depending on input messages of type can_msgs::Frame,
+ * the current state is switched between INITIALIZATION,
+ * PRE_OPERATIONAL, OPERATIONAL and STOPPED.
+ * 
+ * Copyright (C) 2019 Ing.-Buero Dr. Michael Lehning, Hildesheim
+ * Copyright (C) 2019 SICK AG, Waldkirch
+ * 
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ * 
+ *       http://www.apache.org/licenses/LICENSE-2.0
+ *
+ *   Unless required by applicable law or agreed to in writing, software
+ *   distributed under the License is distributed on an "AS IS" BASIS,
+ *   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ *   See the License for the specific language governing permissions and
+ *   limitations under the License.
+ *
+ * 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 SICK AG nor the names of its
+ *       contributors may be used to endorse or promote products derived from
+ *       this software without specific prior written permission
+ *     * Neither the name of Ing.-Buero Dr. Michael Lehning 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.
+ *
+ *      Authors:
+ *         Michael Lehning <michael.lehning@lehning.de>
+ *
+ *  Copyright 2019 SICK AG
+ *  Copyright 2019 Ing.-Buero Dr. Michael Lehning
+ * 
+ */
+#ifndef __SICK_CANOPEN_SIMU_CANSTATE_H_INCLUDED
+#define __SICK_CANOPEN_SIMU_CANSTATE_H_INCLUDED
+
+#include <ros/ros.h>
+#include <can_msgs/Frame.h>
+
+namespace sick_canopen_simu
+{
+  typedef enum CAN_STATE_ENUM
+  {
+    INITIALIZATION,
+    PRE_OPERATIONAL,
+    OPERATIONAL,
+    STOPPED
+    
+  } CAN_STATE;
+  
+  
+  /*
+   * class CanState: handles can nmt messages and implements the state engine for can.
+   */
+  class CanState
+  {
+  public:
+  
+    /*
+     * Constructor.
+     *
+     * @param[in] can_node_id node id of OLS or MLS, default: 0x0A
+     */
+    CanState(int can_node_id);
+
+    /*
+     * Destructor.
+     */
+    virtual ~CanState();
+  
+    /*
+     * Constructor.
+     *
+     * @param[in] can_node_id node id of OLS or MLS, default: 0x0A
+     */
+    virtual CAN_STATE & state(void)
+    {
+      return m_can_state;
+    }
+  
+    /*
+     * @brief Callbacks for ros messages. Switches the state, and returns true, if msg_in is a can nmt message.
+     * Otherwise, false is returned.
+     *
+     * param[in] msg_in input message of type can_msgs::Frame
+     * param[out] msg_out output message of type can_msgs::Frame (if input message requires a response to send)
+     * param[out] send_msg true, if input message msg_in requires the response message msg_out to be send
+     *
+     * @return true, if input message handled, otherwise false.
+     */
+    virtual bool messageHandler(const can_msgs::Frame &msg_in, can_msgs::Frame & msg_out, bool & send_msg);
+
+  protected:
+
+    /*
+     * member data.
+     */
+
+    int m_can_node_id;
+    CAN_STATE m_can_state;
+
+  }; // class CanState
+
+} // namespace sick_canopen_simu
+#endif // __SICK_CANOPEN_SIMU_CANSTATE_H_INCLUDED

Devices/Packages/sick_line_guidance/include/sick_line_guidance/sick_canopen_simu_compare.h

@@ -0,0 +1,177 @@
+/*
+ * sick_canopen_simu_compare implements utility functions to compare measurements
+ * for verification of the sick_line_guidance ros driver.
+ * 
+ * Copyright (C) 2019 Ing.-Buero Dr. Michael Lehning, Hildesheim
+ * Copyright (C) 2019 SICK AG, Waldkirch
+ * 
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ * 
+ *       http://www.apache.org/licenses/LICENSE-2.0
+ *
+ *   Unless required by applicable law or agreed to in writing, software
+ *   distributed under the License is distributed on an "AS IS" BASIS,
+ *   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ *   See the License for the specific language governing permissions and
+ *   limitations under the License.
+ *
+ * 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 SICK AG nor the names of its
+ *       contributors may be used to endorse or promote products derived from
+ *       this software without specific prior written permission
+ *     * Neither the name of Ing.-Buero Dr. Michael Lehning 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.
+ *
+ *      Authors:
+ *         Michael Lehning <michael.lehning@lehning.de>
+ *
+ *  Copyright 2019 SICK AG
+ *  Copyright 2019 Ing.-Buero Dr. Michael Lehning
+ * 
+ */
+#ifndef __SICK_CANOPEN_SIMU_COMPARE_H_INCLUDED
+#define __SICK_CANOPEN_SIMU_COMPARE_H_INCLUDED
+
+#include <ros/ros.h>
+#include "sick_line_guidance/MLS_Measurement.h"
+#include "sick_line_guidance/OLS_Measurement.h"
+
+namespace sick_canopen_simu
+{
+  /*
+   * class MeasurementComparator implements utility functions to compare measurements
+   * for verification of the sick_line_guidance ros driver.
+   *
+   */
+  template<class MsgType>
+  class MeasurementComparator
+  {
+  public:
+
+    /*
+     * @brief Compares the positions of two measurements.
+     * @return true, if the positions of two measurements are identical (difference below 1 millimeter), or false otherwise.
+     */
+    static bool cmpPosition(const MsgType &A, const MsgType &B)
+    {
+      return (std::fabs(A.position[0] - B.position[0]) < 0.001) && (std::fabs(A.position[1] - B.position[1]) < 0.001) && (std::fabs(A.position[2] - B.position[2]) < 0.001);
+    }
+
+    /*
+     * @brief Compares the width of two measurements.
+     * @return true, if the width of two measurements are identical (difference below 1 millimeter), or false otherwise.
+     */
+    static bool cmpLinewidth(const MsgType &A, const MsgType &B)
+    {
+      return (std::fabs(A.width[0] - B.width[0]) < 0.001) && (std::fabs(A.width[1] - B.width[1]) < 0.001) && (std::fabs(A.width[2] - B.width[2]) < 0.001);
+    }
+
+    /*
+     * @brief Compares the status of two measurements.
+     * @return true, if the status of two measurements are identical, or false otherwise.
+     */
+    static bool cmpStatus(const MsgType &A, const MsgType &B)
+    {
+      return (A.status == B.status);
+    }
+
+    /*
+     * @brief Compares the lcp status of two measurements.
+     * @return true, if the lcp status of two measurements are identical, or false otherwise.
+     */
+    static bool cmpLcp(const MsgType &A, const MsgType &B)
+    {
+      return (A.lcp == B.lcp);
+    }
+
+    /*
+     * @brief Compares the barcode of two measurements.
+     * @return true, if the barcodes of two measurements are identical, or false otherwise.
+     */
+    static bool cmpBarcode(const MsgType &A, const MsgType &B)
+    {
+      return (A.barcode == B.barcode);
+    }
+
+    /*
+     * @brief Compares the device status of two measurements.
+     * @return true, if the device status of two measurements are identical, or false otherwise.
+     */
+    static bool cmpDevStatus(const MsgType &A, const MsgType &B)
+    {
+      return (A.dev_status == B.dev_status);
+    }
+
+    /*
+     * @brief Compares the extended code of two measurements.
+     * @return true, if the extended code of two measurements are identical, or false otherwise.
+     */
+    static bool cmpExtendedCode(const MsgType &A, const MsgType &B)
+    {
+      return (A.extended_code == B.extended_code);
+    }
+
+    /*
+     * @brief Compares the error status of two measurements.
+     * @return true, if the error status of two measurements are identical, or false otherwise.
+     */
+    static bool cmpError(const MsgType &A, const MsgType &B)
+    {
+      return (A.error == B.error);
+    }
+  
+    /*
+     * @brief Compares the barcode center points of two measurements.
+     * @return true, if the barcode center points of two measurements are identical, or false otherwise.
+     */
+    static bool cmpBarcodeCenter(const MsgType &A, const MsgType &B)
+    {
+      return (std::fabs(A.barcode_center_point - B.barcode_center_point) < 0.001);
+    }
+  
+    /*
+     * @brief Compares the line quality of two measurements.
+     * @return true, if the line quality of two measurements are identical, or false otherwise.
+     */
+    static bool cmpLineQuality(const MsgType &A, const MsgType &B)
+    {
+      return (A.quality_of_lines == B.quality_of_lines);
+    }
+  
+    /*
+     * @brief Compares the line intensities of two measurements.
+     * @return true, if the line intensities of two measurements are identical, or false otherwise.
+     */
+    static bool cmpLineIntensity(const MsgType &A, const MsgType &B)
+    {
+      return (A.intensity_of_lines[0] == B.intensity_of_lines[0] && A.intensity_of_lines[1] == B.intensity_of_lines[1] && A.intensity_of_lines[2] == B.intensity_of_lines[2]);
+    }
+    
+  };
+}
+#endif // __SICK_CANOPEN_SIMU_COMPARE_H_INCLUDED
+

Devices/Packages/sick_line_guidance/include/sick_line_guidance/sick_canopen_simu_device.h

@@ -0,0 +1,417 @@
+/*
+ * sick_canopen_simu_device implements simulation of SICK can devices (OLS20 and MLS)
+ * for tests of sick_line_guidance ros driver.
+ * 
+ * Copyright (C) 2019 Ing.-Buero Dr. Michael Lehning, Hildesheim
+ * Copyright (C) 2019 SICK AG, Waldkirch
+ * 
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ * 
+ *       http://www.apache.org/licenses/LICENSE-2.0
+ *
+ *   Unless required by applicable law or agreed to in writing, software
+ *   distributed under the License is distributed on an "AS IS" BASIS,
+ *   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ *   See the License for the specific language governing permissions and
+ *   limitations under the License.
+ *
+ * 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 SICK AG nor the names of its
+ *       contributors may be used to endorse or promote products derived from
+ *       this software without specific prior written permission
+ *     * Neither the name of Ing.-Buero Dr. Michael Lehning 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.
+ *
+ *      Authors:
+ *         Michael Lehning <michael.lehning@lehning.de>
+ *
+ *  Copyright 2019 SICK AG
+ *  Copyright 2019 Ing.-Buero Dr. Michael Lehning
+ * 
+ */
+#ifndef __SICK_CANOPEN_SIMU_DEVICE_H_INCLUDED
+#define __SICK_CANOPEN_SIMU_DEVICE_H_INCLUDED
+
+#include <boost/thread.hpp>
+#include <tinyxml.h>
+#include <ros/ros.h>
+#include <can_msgs/Frame.h>
+#include "sick_line_guidance/sick_canopen_simu_canstate.h"
+#include "sick_line_guidance/MLS_Measurement.h"
+#include "sick_line_guidance/OLS_Measurement.h"
+
+namespace sick_canopen_simu
+{
+  /*
+   * Class SimulatorBase implements the base class for simulation of SICK can devices (OLS20 and MLS).
+   *
+   * ROS messages of type can_msgs::Frame are read from ros topic "can0",
+   * handled to simulate an OLS20 or MLS device, and resulting can_msgs::Frame
+   * messages are published on ros topic "ros2can0".
+   *
+   * MsgType can be sick_line_guidance::OLS_Measurement (for simulation of OLS devices), or
+   * or sick_line_guidance::MLS_Measurement (for simulation of MLS devices).
+   *
+   * Assumption: ros nodes sick_line_guidance_ros2can_node and sick_line_guidance_can2ros_node
+   * have to be running to convert ros messages from and to canbus messages.
+   * sick_line_guidance_ros2can_node converts ros messages of type can_msgs::Frame to can frames,
+   * sick_line_guidance_can2ros_node converts can frames to ros messages of type can_msgs::Frame.
+   *
+   * Subclass MLSSimulator extends class SimulatorBase to simulate an MLS device.
+   *
+   * Subclass OLS20Simulator extends class SimulatorBase to simulate an OLS20 device.
+   */
+  template<class MsgType> class SimulatorBase
+  {
+  public:
+    
+    /*
+     * Constructor
+     *
+     * @param[in] nh ros node handle
+     * @param[in] config_file configuration file with testcases for OLS and MLS simulation
+     * @param[in] can_node_id node id of OLS or MLS, default: 0x0A
+     * @param[in] subscribe_topic ros topic to receive input messages of type can_msgs::Frame, default: "can0"
+     * @param[in] publish_topic ros topic to publish output messages of type can_msgs::Frame, default: "ros2can0"
+     * @param[in] pdo_rate rate of PDO (default: pdo_rate = 50, i.e. 20 ms between two PDOs or 50 PDOs per second)
+     * @param[in] pdo_repeat_cnt each sensor state spefied in sick_canopen_simu_cfg.xml is repeated N times before switching to the next state (default: 5 times)
+     * @param[in] subscribe_queue_size buffer size for ros messages
+     */
+    SimulatorBase(ros::NodeHandle & nh, const std::string & config_file, int can_node_id, const std::string & subscribe_topic, const std::string & publish_topic, const ros::Rate & pdo_rate, int pdo_repeat_cnt, int subscribe_queue_size);
+  
+    /*
+     * Destructor
+     *
+     */
+    virtual ~SimulatorBase();
+    
+    /*
+     * @brief Callbacks for ros messages. Converts incoming messages of type can_msgs::Frame to simulate a can device
+     * and publishes simulation results to the configured ros topic.
+     *
+     * param[in] msg ros message of type can_msgs::Frame
+     */
+    virtual void messageHandler(const can_msgs::Frame & msg_in);
+  
+    /*
+     * @brief Interface to listen to the simulated sensor state. A listener implementing this interface can be notified on
+     * PDOs sent by a simulation by registring using function registerSimulationListener.
+     * After receiving both the sensor state from the simulation and the following OLS/MLS-Measurement ros message from the driver,
+     * the listener can check the correctness by comparing both messages. The sensor state from simulation and from OLS/MLS-Measurement
+     * messages from the driver must be identical, otherwise some failure occured.
+     */
+    class SimulationListener
+    {
+    public:
+      /*
+       * @brief Notification callback of a listener. Whenever the simulation sends a PDO, registered listener are notified about the current sensor state
+       * by calling function pdoSent.
+       */
+      virtual void pdoSent(const MsgType & sensor_state) = 0;
+    };
+  
+    /*
+     * @brief Registers a listener to a simulation. Whenever the simulation sends a PDO, the listener is notified about the current sensor state.
+     * After receiving both the sensor state and the following OLS/MLS-Measurement ros message from the driver, the listener can check
+     * the correctness by comparing the sensor state from simulation and the OLS/MLS-Measurement from the driver. Both must be identical,
+     * otherwise some failure occured.
+     *
+     * param[in] pListener listener to current sensor states sent by PDO.
+     */
+    virtual void registerSimulationListener(SimulationListener* pListener);
+  
+    /*
+     * @brief Unregisters a listener from a simulation. The listener will not be notified about simulated sensor states.
+     * This function is the opposite to registerSimulationListener.
+     *
+     * param[in] pListener listener to current sensor states sent by PDO.
+     */
+    virtual void unregisterSimulationListener(SimulationListener* pListener);
+
+  protected:
+  
+    /*
+     * @brief Parses an pdo element from config file and appends it to m_vec_pdo_measurements.
+     *
+     * param[in] xml_pdo pdo element from config file
+     */
+    virtual bool parseXmlPDO(TiXmlElement* xml_pdo) = 0;
+
+    /*
+     * reads SDO configuration from xml-file
+     *
+     * @param[in] config_file configuration file with testcases for OLS and MLS simulation
+     */
+    virtual bool readSDOconfig(const std::string & config_file);
+  
+    /*
+     * reads the PDO configuration from xml-file
+     *
+     * @param[in] config_file configuration file with testcases for simulation
+     * @param[in] sick_device_family "OLS10", "OLS20" or "MLS"
+     */
+    virtual bool readPDOconfig(const std::string & config_file, const std::string & sick_device_family);
+  
+    /*
+     * @brief handles SDOs, i.e. sends a SDO response and returns true, if can frame is a SDO request.
+     * Otherwise, the can frame is not handled and false is returned.
+     * Note: The SDO response is simply taken from a lookup-table (input: SDO request frame data, output: SDO response frame data).
+     * If the SDO request frame data (frame.data) is not found in the lookup-table, the can frame is not handled and false is returned, too.
+     *
+     * @param[in] msg_in received can frame
+     *
+     * @return true if can frame was handled and a SDO request is sent, false otherwise.
+     */
+    virtual bool handleSDO(const can_msgs::Frame & msg_in);
+    
+    /*
+     * @brief Publishes PDOs to simulate a MLS or OLS20 device while can state is operational
+     */
+    virtual void runPDOthread(void);
+    
+    /*
+     * @brief Convertes an MLS_Measurement into a can_msgs::Frame TPDO1.
+     * @param[in] measurement MLS_Measurement to be converted
+     * @param[out] tpdo1_msg output can frame TPDO1,
+     * @param[out] tpdo2_msg dummy frame TPDO2, unused
+     * @return Returns the number of TPDOs, i.e. 1 for MLS devices.
+     */
+    virtual int convertToCanFrame(const sick_line_guidance::MLS_Measurement & measurement, can_msgs::Frame & tpdo1_msg, can_msgs::Frame & tpdo2_msg);
+  
+    /*
+     * @brief Convertes an OLS_Measurement into two can_msgs::Frame TPDO1 and TPDO2.
+     * @param[in] measurement OLS_Measurement to be converted
+     * @param[out] tpdo1_msg output can frame TPDO1, Byte 1-8 := LCP1(LSB,MSB,0x2021sub1), LCP2(LSB,MSB,0x2021sub2), LCP3(LSB,MSB,0x2021sub3), status(UINT8,0x2021sub4), barcode(UINT8,0x2021sub8)
+     * @param[out] tpdo2_msg output can frame TPDO2, Byte 1-6 := Width1(LSB,MSB,0x2021sub5), Width2(LSB,MSB,0x2021sub6), Width3(LSB,MSB,0x2021sub7)
+     * @return Returns the number of TPDOs, i.e. 2 for OLS devices.
+     */
+    virtual int convertToCanFrame(const sick_line_guidance::OLS_Measurement & measurement, can_msgs::Frame & tpdo1_msg, can_msgs::Frame & tpdo2_msg);
+  
+    /*
+     * @brief Converts a position or width (float value in meter) to lcp (INT16 value in millimeter),
+     * shortcut for std::round(lcp * 1000) with clipping to range INT16_MIN ... INT16_MAX.
+     * @param[in] lcp position or width (float value in meter)
+     * @return INT16 value in millimeter
+     */
+    virtual int convertLCP(float lcp);
+    
+    /*
+     * @brief Converts frame.data to uint64_t
+     * @param[in] frame can frame
+     * @return frame.data converted to uint64_t
+     */
+    virtual uint64_t frameDataToUInt64(const can_msgs::Frame & frame);
+    
+    /*
+     * @brief Converts uint64_t data to frame.data
+     * @param[in] u64data frame data (uint64_t)
+     * @param[in+out] frame can frame
+     */
+    virtual void uint64ToFrameData(uint64_t u64data, can_msgs::Frame & frame);
+    
+    /*
+     * @brief returns an unsigned integer in reverse byte order,
+     * f.e. revertByteorder<uint32_t>(0x12345678) returns 0x78563412.
+     * @param[in] data input data (unsigned integer)
+     * @return data in reverse byte order
+     */
+    template <class T> static T revertByteorder(T data)
+    {
+      T reverted = 0;
+      for(size_t n = 0; n < sizeof(T); n++)
+      {
+        reverted = (reverted << 8);
+        reverted = (reverted | (data & 0xFF));
+        data = data >> 8;
+      }
+      return reverted;
+    }
+    
+    /*
+     * @brief prints and returns a can_msgs::Frame in short format like candump (f.a. "18A#B4FFCCFF00000300")
+     */
+    virtual std::string tostring(const can_msgs::Frame & canframe);
+    
+    /*
+     * member variables
+     */
+    
+    sick_canopen_simu::CanState m_can_state; // the current can state: INITIALIZATION, PRE_OPERATIONAL, OPERATIONAL or STOPPED
+    uint8_t m_can_node_id; // node id of OLS or MLS device (default: 0x0A)
+    ros::Subscriber m_ros_subscriber; // subscriber to handle can messages from master (NMT messages and SDOs)
+    ros::Publisher m_ros_publisher;  // publishes can frames (PDO, SDO response, etc.)
+    boost::mutex m_ros_publisher_mutex; // lock guard for publishing messages with m_ros_publisher
+    boost::thread* m_pdo_publisher_thread; // thread to publish PDO messages in can state OPERATIONAL
+    bool m_pdo_publisher_thread_running; // true while m_pdo_publisher_thread is running
+    ros::Rate m_pdo_rate; // rate of PDOs (default: pdo_rate = 50, i.e. 20 ms between two PDOs or 50 PDOs per second)
+    int m_pdo_repeat_cnt; // each sensor state spefied in sick_canopen_simu_cfg.xml is repeated N times before switching to the next state (default: 5 times)
+    std::map<uint64_t, uint64_t> m_sdo_request_response_map; // lookup table: sdo response data := m_sdo_request_response_map[<sdo request data>]
+    std::vector<MsgType> m_vec_pdo_measurements; // list of PDOs to simulate OLS or MLS device
+    std::vector<SimulationListener*> m_vec_simu_listener; // list of registered listeners to the current sensor state simulated
+    int m_subscribe_queue_size; // buffer size for ros messages
+    uint64_t m_sdo_response_dev_state; // response to sdo request for dev_status (object 0x2018): MLS and OLS20: 0x4F18200000000000 (sdo response with UINT8 data), OLS10: 0x4B18200000000000 (sdo response with UINT16 data)
+    bool m_send_tpdo_immediately; // true (OLS10, MLS): send TPDOs immediately in all states (pre-operational and operational), false (default, OLS20): send TPDOs only in state operational
+  
+  }; // SimulatorBase
+  
+  /*
+   * Subclass MLSSimulator extends class SimulatorBase to simulate a MLS device.
+   *
+   */
+  class MLSSimulator : public SimulatorBase<sick_line_guidance::MLS_Measurement>
+  {
+  public:
+  
+    /*
+     * Constructor
+     *
+     * @param[in] nh ros node handle
+     * @param[in] config_file configuration file with testcases for OLS and MLS simulation
+     * @param[in] sick_device_family "OLS10", "OLS20" or "MLS"
+     * @param[in] can_node_id node id of OLS or MLS, default: 0x0A
+     * @param[in] subscribe_topic ros topic to receive input messages of type can_msgs::Frame, default: "can0"
+     * @param[in] publish_topic ros topic to publish output messages of type can_msgs::Frame, default: "ros2can0"
+     * @param[in] pdo_rate rate of PDO (default: pdo_rate = 50, i.e. 20 ms between two PDOs or 50 PDOs per second)
+     * @param[in] pdo_repeat_cnt each sensor state spefied in sick_canopen_simu_cfg.xml is repeated N times before switching to the next state (default: 5 times)
+     * @param[in] subscribe_queue_size buffer size for ros messages
+     */
+    MLSSimulator(ros::NodeHandle & nh, const std::string & config_file, const std::string & sick_device_family, int can_node_id, const std::string & subscribe_topic, const std::string & publish_topic, const ros::Rate & pdo_rate, int pdo_repeat_cnt, int subscribe_queue_size);
+  
+    /*
+     * @brief Callbacks for ros messages. Converts incoming messages of type can_msgs::Frame to simulate a MLS device
+     * and publishes simulation results to the configured ros topic.
+     *
+     * param[in] msg ros message of type can_msgs::Frame
+     */
+    virtual void messageHandler(const can_msgs::Frame & msg_in);
+
+  protected:
+  
+    /*
+     * @brief Parses an pdo element from config file and appends it to m_vec_pdo_measurements.
+     *
+     * param[in] xml_pdo pdo element from config file
+     */
+    virtual bool parseXmlPDO(TiXmlElement* xml_pdo);
+    
+  }; // MLSSimulator
+  
+  /*
+   * Subclass OLSSimulator extends class SimulatorBase to simulate an OLS devices.
+   *
+   */
+  class OLSSimulator : public SimulatorBase<sick_line_guidance::OLS_Measurement>
+  {
+  public:
+    
+    /*
+     * Constructor
+     *
+     * @param[in] nh ros node handle
+     * @param[in] config_file configuration file with testcases for OLS and MLS simulation
+     * @param[in] sick_device_family "OLS10", "OLS20" or "MLS"
+     * @param[in] can_node_id node id of OLS or MLS, default: 0x0A
+     * @param[in] subscribe_topic ros topic to receive input messages of type can_msgs::Frame, default: "can0"
+     * @param[in] publish_topic ros topic to publish output messages of type can_msgs::Frame, default: "ros2can0"
+     * @param[in] pdo_rate rate of PDO (default: pdo_rate = 50, i.e. 20 ms between two PDOs or 50 PDOs per second)
+     * @param[in] pdo_repeat_cnt each sensor state spefied in sick_canopen_simu_cfg.xml is repeated N times before switching to the next state (default: 5 times)
+     * @param[in] subscribe_queue_size buffer size for ros messages
+     */
+    OLSSimulator(ros::NodeHandle & nh, const std::string & config_file, const std::string & sick_device_family, int can_node_id, const std::string & subscribe_topic, const std::string & publish_topic, const ros::Rate & pdo_rate, int pdo_repeat_cnt, int subscribe_queue_size);
+    
+    /*
+     * @brief Callbacks for ros messages. Converts incoming messages of type can_msgs::Frame to simulate an OLS device
+     * and publishes simulation results to the configured ros topic.
+     *
+     * param[in] msg ros message of type can_msgs::Frame
+     */
+    virtual void messageHandler(const can_msgs::Frame & msg_in);
+
+  protected:
+  
+    /*
+     * @brief Parses an pdo element from config file and appends it to m_vec_pdo_measurements.
+     *
+     * param[in] xml_pdo pdo element from config file
+     */
+    virtual bool parseXmlPDO(TiXmlElement* xml_pdo);
+    
+  }; // OLSSimulator
+  
+  /*
+   * Subclass OLS10Simulator extends class OLSSimulator to simulate an OLS10 device.
+   *
+   */
+  class OLS10Simulator : public OLSSimulator
+  {
+  public:
+    
+    /*
+     * Constructor
+     *
+     * @param[in] nh ros node handle
+     * @param[in] config_file configuration file with testcases for OLS and MLS simulation
+     * @param[in] sick_device_family "OLS10", "OLS20" or "MLS"
+     * @param[in] can_node_id node id of OLS or MLS, default: 0x0A
+     * @param[in] subscribe_topic ros topic to receive input messages of type can_msgs::Frame, default: "can0"
+     * @param[in] publish_topic ros topic to publish output messages of type can_msgs::Frame, default: "ros2can0"
+     * @param[in] pdo_rate rate of PDO (default: pdo_rate = 50, i.e. 20 ms between two PDOs or 50 PDOs per second)
+     * @param[in] pdo_repeat_cnt each sensor state spefied in sick_canopen_simu_cfg.xml is repeated N times before switching to the next state (default: 5 times)
+     * @param[in] subscribe_queue_size buffer size for ros messages
+     */
+    OLS10Simulator(ros::NodeHandle & nh, const std::string & config_file, const std::string & sick_device_family, int can_node_id, const std::string & subscribe_topic, const std::string & publish_topic, const ros::Rate & pdo_rate, int pdo_repeat_cnt, int subscribe_queue_size);
+
+  }; // OLS10Simulator
+  
+  /*
+   * Subclass OLS20Simulator extends class OLSSimulator to simulate an OLS20 device.
+   *
+   */
+  class OLS20Simulator : public OLSSimulator
+  {
+  public:
+    
+    /*
+     * Constructor
+     *
+     * @param[in] nh ros node handle
+     * @param[in] config_file configuration file with testcases for OLS and MLS simulation
+     * @param[in] sick_device_family "OLS10", "OLS20" or "MLS"
+     * @param[in] can_node_id node id of OLS or MLS, default: 0x0A
+     * @param[in] subscribe_topic ros topic to receive input messages of type can_msgs::Frame, default: "can0"
+     * @param[in] publish_topic ros topic to publish output messages of type can_msgs::Frame, default: "ros2can0"
+     * @param[in] pdo_rate rate of PDO (default: pdo_rate = 50, i.e. 20 ms between two PDOs or 50 PDOs per second)
+     * @param[in] pdo_repeat_cnt each sensor state spefied in sick_canopen_simu_cfg.xml is repeated N times before switching to the next state (default: 5 times)
+     * @param[in] subscribe_queue_size buffer size for ros messages
+     */
+    OLS20Simulator(ros::NodeHandle & nh, const std::string & config_file, const std::string & sick_device_family, int can_node_id, const std::string & subscribe_topic, const std::string & publish_topic, const ros::Rate & pdo_rate, int pdo_repeat_cnt, int subscribe_queue_size);
+
+  }; // OLS20Simulator
+  
+} // sick_canopen_simu
+
+#endif // __SICK_CANOPEN_SIMU_DEVICE_H_INCLUDED

Devices/Packages/sick_line_guidance/include/sick_line_guidance/sick_canopen_simu_verify.h

@@ -0,0 +1,228 @@
+/*
+ * sick_canopen_simu_verify tests and verifies the sick_line_guidance ros driver.
+ * 
+ * sick_canopen_simu_verify listenes to both the simulation (interface sick_canopen_simu::SimulatorBase::SimulationListener)
+ * and to the ros messages of the sick_line_guidance driver. Whenever a MLS_Measurement or OLS_Measurement message
+ * is received, the measurement is compared to the current sensor state of the simulation.
+ * Measurement messages from the driver and sensor states from the simulation should be identical, otherwise an error occured.
+ * The sick_line_guidance driver test is passed, if no error occured (i.e. all measurement messages from the driver have been 
+ * handled correctly, no mismatches between simulated sensor states and published measurement messages).
+ * 
+ * Copyright (C) 2019 Ing.-Buero Dr. Michael Lehning, Hildesheim
+ * Copyright (C) 2019 SICK AG, Waldkirch
+ * 
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ * 
+ *       http://www.apache.org/licenses/LICENSE-2.0
+ *
+ *   Unless required by applicable law or agreed to in writing, software
+ *   distributed under the License is distributed on an "AS IS" BASIS,
+ *   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ *   See the License for the specific language governing permissions and
+ *   limitations under the License.
+ *
+ * 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 SICK AG nor the names of its
+ *       contributors may be used to endorse or promote products derived from
+ *       this software without specific prior written permission
+ *     * Neither the name of Ing.-Buero Dr. Michael Lehning 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.
+ *
+ *      Authors:
+ *         Michael Lehning <michael.lehning@lehning.de>
+ *
+ *  Copyright 2019 SICK AG
+ *  Copyright 2019 Ing.-Buero Dr. Michael Lehning
+ * 
+ */
+#ifndef __SICK_CANOPEN_SIMU_VERIFY_H_INCLUDED
+#define __SICK_CANOPEN_SIMU_VERIFY_H_INCLUDED
+
+#include <ros/ros.h>
+#include "sick_line_guidance/sick_canopen_simu_device.h"
+
+namespace sick_canopen_simu
+{
+  /*
+   * Class MeasurementVerification tests and verifies measurement messages from the sick_line_guidance ros driver.
+   * 
+   * MeasurementVerification listenes to both the simulation (interface sick_canopen_simu::SimulatorBase::SimulationListener)
+   * and to the ros messages of the sick_line_guidance driver. Whenever a MLS_Measurement or OLS_Measurement message
+   * is received, the measurement is compared to the current sensor state of the simulation.
+   * Measurement messages from the driver and sensor states from the simulation should be identical, otherwise an error occured.
+   * The sick_line_guidance driver test is passed, if no error occured (i.e. all measurement messages from the driver have been 
+   * handled correctly, no mismatches between simulated sensor states and published measurement messages).
+   */
+  template<class MsgType> class MeasurementVerification : public sick_canopen_simu::SimulatorBase<MsgType>::SimulationListener
+  {
+  public:
+    
+    /*
+     * Constructor
+     *
+     * @param[in] nh ros node handle
+     * @param[in] measurement_subscribe_topic ros topic for measurement messages, default: "mls" resp. "ols"
+     * @param[in] sensor_state_queue_size buffer size for simulated sensor states, default: 2
+     * @param[in] devicename descriptional device name, f.e. "OLS20" or "MLS"
+     */
+    MeasurementVerification(ros::NodeHandle & nh, const std::string & measurement_subscribe_topic, int sensor_state_queue_size, const std::string & devicename);
+  
+    /*
+     * Destructor
+     *
+     */
+    virtual ~MeasurementVerification();
+  
+    /*
+     * @brief Implements the notification callback for SimulationListener. Whenever the simulation sends a PDO,
+     * this function is called by the simulation to notify SimulationListeners about the current sensor state.
+     */
+    virtual void pdoSent(const MsgType & sensor_state);
+  
+    /*
+     * @brief Prints the number of verified measuremente and the number of verification failures.
+     * @return true in case of no verification failures (all measurements verified successfully), false otherwise.
+     */
+    virtual bool printStatistic(void);
+    
+  protected:
+  
+    /*
+     * @brief Callback for measurement messages, called whenever the sick_line_guidance ros driver publishes a
+     * new measurement message. Compares the measurement message with the sensor states from simulation,
+     * and reports an error, if an equivalent sensor state hasn't been sent by the simulation.
+     *
+     * @param[in] measurement measurement message from sick_line_guidance ros driver
+     *
+     * @return true, if the measurement message is equivalent to sensor states (verification passed), or false otherwise (verification failed).
+     */
+    virtual bool verifyMeasurement(const MsgType & measurement);
+
+    /*
+     * @brief Compares and verifies measurement messages from ros driver against sensor states from simulation.
+     *
+     * @param[in] measurement_messages list of measurement messages from ros driver (to be compared to sensor_states)
+     * @param[in] sensor_states list of sensor states from simulation (to be compared to measurement_messages)
+     *
+     * @return true, if the measurement messages are equivalent to sensor states (verification passed), or false otherwise (verification failed).
+     */
+    // virtual bool verifyMeasurements(std::list<MsgType> & measurement_messages, std::list<MsgType> & sensor_states);
+    virtual bool verifyMeasurements(std::list<sick_line_guidance::MLS_Measurement> & measurement_messages, std::list<sick_line_guidance::MLS_Measurement> & sensor_states);
+    virtual bool verifyMeasurements(std::list<sick_line_guidance::OLS_Measurement> & measurement_messages, std::list<sick_line_guidance::OLS_Measurement> & sensor_states);
+
+    /*
+     * @brief Compares measurement messages from ros driver against sensor states from simulation, using a specified compare function
+     * (f.e. floating point comparsion for positions with fabs(x-y) < 1 mm and integer comparsion with x == y for barcodes).
+     *
+     * @param[in] measurement_messages list of measurement messages from ros driver (to be compared to sensor_states)
+     * @param[in] sensor_states list of sensor states from simulation (to be compared to measurement_messages)
+     * @param[in] cmpfunction compare function, called to compare measurement message A to sensor state B
+     *
+     * @return true, if the measurement messages are equivalent to sensor states (verification passed), or false otherwise (verification failed).
+    */
+    template<typename T>
+    bool verifyMeasurementData(std::list<T> & measurement_messages, std::list<T> & sensor_states, bool(*cmpfunction)(const T & A, const T & B));
+    // virtual bool verifyMeasurementData(std::list<MsgType> & measurement_messages, std::list<MsgType> & sensor_states, bool(*cmpfunction)(const MsgType & A, const MsgType & B));
+  
+    /*
+     * member variables
+     */
+    std::string m_devicename;                   // descriptional device name, f.e. "OLS20" or "MLS"
+    ros::Subscriber m_measurement_subscriber;   // subscriber ros measurement messages from sick_line_guidance driver
+    std::list<MsgType> m_sensor_states;         // list of sensor states from simulation
+    std::list<MsgType> m_measurement_messages;  // list of measurement messages from sick_line_guidance driver
+    boost::mutex m_sensor_states_mutex;         // lock guard for access to m_vec_sensor_states
+    int m_measurement_messages_cnt;             // reporting and statistics: number of verified measurement messages
+    int m_measurement_verification_error_cnt;   // reporting and statistics: number of measurement messages with verification errors
+    int m_measurement_verification_ignored_cnt; // reporting and statistics: number of measurement messages with verification ignored (f.e. SDO response was still pending)
+    int m_measurement_verification_failed;      // reporting and statistics: messages with verification errors
+    int m_measurement_verification_jitter;      // reporting and statistics: verification jitter (max. 1 error tolerated, since measurement messages can be sent while a SDO response is still pending)
+    
+  }; // MeasurementVerification
+  
+  /*
+   * Subclass MLSMeasurementVerification extends class MeasurementVerification to verify MLS_Measurement messages.
+   *
+   */
+  class MLSMeasurementVerification : public MeasurementVerification<sick_line_guidance::MLS_Measurement>
+  {
+  public:
+  
+    /*
+     * Constructor
+     *
+     * @param[in] nh ros node handle
+     * @param[in] measurement_subscribe_topic ros topic for measurement messages, default: "mls"
+     * @param[in] sensor_state_queue_size buffer size for simulated sensor states, default: 2
+     * @param[in] devicename descriptional device name, f.e. "MLS"
+     */
+    MLSMeasurementVerification(ros::NodeHandle & nh, const std::string & measurement_subscribe_topic, int sensor_state_queue_size, const std::string & devicename);
+  
+    /*
+     * @brief Callback for measurement messages, called whenever the sick_line_guidance ros driver publishes a
+     * new measurement message. Compares the measurement message with the sensor states from simulation,
+     * and reports an error, if an equivalent sensor state hasn't been sent by the simulation.
+     *
+     * @param[in] measurement measurement message from sick_line_guidance ros driver
+     */
+    virtual void measurementCb(const sick_line_guidance::MLS_Measurement & measurement);
+    
+  }; // MLSMeasurementVerification
+  
+  /*
+   * Subclass OLSMeasurementVerification extends class MeasurementVerification to verify OLS_Measurement messages.
+   *
+   */
+  class OLSMeasurementVerification : public MeasurementVerification<sick_line_guidance::OLS_Measurement>
+  {
+  public:
+  
+    /*
+     * Constructor
+     *
+     * @param[in] nh ros node handle
+     * @param[in] measurement_subscribe_topic ros topic for measurement messages, default: "ols"
+     * @param[in] sensor_state_queue_size buffer size for simulated sensor states, default: 2
+     * @param[in] devicename descriptional device name, f.e. "OLS20"
+     */
+    OLSMeasurementVerification(ros::NodeHandle & nh, const std::string & measurement_subscribe_topic, int sensor_state_queue_size, const std::string & devicename);
+  
+    /*
+     * @brief Callback for measurement messages, called whenever the sick_line_guidance ros driver publishes a
+     * new measurement message. Compares the measurement message with the sensor states from simulation,
+     * and reports an error, if an equivalent sensor state hasn't been sent by the simulation.
+     *
+     * @param[in] measurement measurement message from sick_line_guidance ros driver
+     */
+    virtual void measurementCb(const sick_line_guidance::OLS_Measurement & measurement);
+
+  }; // OLSMeasurementVerification
+  
+} // sick_canopen_simu
+
+#endif // __SICK_CANOPEN_SIMU_VERIFY_H_INCLUDED
+

Devices/Packages/sick_line_guidance/include/sick_line_guidance/sick_line_guidance_can_cia401_subscriber.h

@@ -0,0 +1,163 @@
+/*
+ * sick_line_guidance_can_cia401_subscriber implements a ros subscriber to canopen ols messages (example CiA401 device for testing).
+ * 
+ * Copyright (C) 2019 Ing.-Buero Dr. Michael Lehning, Hildesheim
+ * Copyright (C) 2019 SICK AG, Waldkirch
+ * 
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ * 
+ *       http://www.apache.org/licenses/LICENSE-2.0
+ *
+ *   Unless required by applicable law or agreed to in writing, software
+ *   distributed under the License is distributed on an "AS IS" BASIS,
+ *   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ *   See the License for the specific language governing permissions and
+ *   limitations under the License.
+ *
+ * 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 SICK AG nor the names of its
+ *       contributors may be used to endorse or promote products derived from
+ *       this software without specific prior written permission
+ *     * Neither the name of Ing.-Buero Dr. Michael Lehning 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.
+ *
+ *      Authors:
+ *         Michael Lehning <michael.lehning@lehning.de>
+ *
+ *  Copyright 2019 SICK AG
+ *  Copyright 2019 Ing.-Buero Dr. Michael Lehning
+ * 
+ */
+#ifndef __SICK_LINE_GUIDANCE_CAN_CIA401_SUBSCRIBER_H_INCLUDED
+#define __SICK_LINE_GUIDANCE_CAN_CIA401_SUBSCRIBER_H_INCLUDED
+
+#include "sick_line_guidance/sick_line_guidance_can_ols_subscriber.h"
+
+namespace sick_line_guidance
+{
+
+  /*
+   * class CanCiA401Subscriber implements the ros subscriber to canopen ols messages (example CiA401 device for testing).
+   */
+  class CanCiA401Subscriber : public CanOlsSubscriber
+  {
+  public:
+  
+    /*
+     * Constructor.
+     * @param[in] nh ros::NodeHandle
+     * @param[in] can_nodeid can id for canopen_chain_node, f.e. "node1"
+     * @param[in] max_num_retries_after_sdo_error After SDO error, the query is repeated max. N times (default: N=2). If the SDO error persists, the can driver is shutdown and restarted.
+     * @param[in] max_sdo_rate max. sdo query and publish rate
+     * @param[in] ros_topic topic for ros messages, f.e. "mls" or "ols"
+     * @param[in] ros_frameid frameid for ros messages, f.e. "mls_measurement_frame" or "ols_measurement_frame"
+     * @param[in] initial_sensor_state initial sensor state (f.e. 0x07 for 3 detected lines, or (1 << 4) to indicate sensor error)
+     * @param[in] subscribe_queue_size buffer size for ros messages
+     */
+    CanCiA401Subscriber(ros::NodeHandle & nh, const std::string & can_nodeid, int max_num_retries_after_sdo_error, double max_sdo_rate, const std::string & ros_topic,
+      const std::string & ros_frameid, int initial_sensor_state = 0, int subscribe_queue_size = 1);
+
+    /*
+     * Destructor.
+     */
+    virtual ~CanCiA401Subscriber();
+  
+    /*
+     * @brief subsribes to canopen topics for ols messages and sets the callbacks to handle messages from canopen_chain_node
+     *        after PDO messages (LCP = line center point):
+     *
+     *        Mapping for OLS:
+     *
+     *        canopenchain-OLS     -> CanSubscriber-callback      -> OLS sensor state
+     *        -----------------------------------------------------------------------
+     *        <nodeid>+"_1001"     -> cancallbackError(UINT8)     -> Error register
+     *        <nodeid>+"_2018"     -> cancallbackDevState(UINT8)  -> Device state
+     *        <nodeid>+"_2021sub1" -> cancallbackLCP1(INT16)      -> LCP1
+     *        <nodeid>+"_2021sub2" -> cancallbackLCP2(INT16)      -> LCP2
+     *        <nodeid>+"_2021sub3" -> cancallbackLCP3(INT16)      -> LCP3
+     *        <nodeid>+"_2021sub4" -> cancallbackState(UINT8)     -> State
+     *        <nodeid>+"_2021sub5" -> cancallbackWidthLCP1(INT16) -> Width LCP1
+     *        <nodeid>+"_2021sub6" -> cancallbackWidthLCP2(INT16) -> Width LCP2
+     *        <nodeid>+"_2021sub7" -> cancallbackWidthLCP3(INT16) -> Width LCP3
+     *        <nodeid>+"_2021sub8" -> cancallbackCode(UINT8)      -> Code
+     *        <nodeid>+"_2021sub9" -> cancallbackExtCode(UINT32)  -> Extended Code
+     *
+     *        Mapping for MLS:
+     *
+     *        canopenchain-MLS     -> CanSubscriber-callback      -> MLS sensor state
+     *        -----------------------------------------------------------------------
+     *        <nodeid>+"_1001"     -> cancallbackError(UINT8)     -> Error register
+     *        <nodeid>+"_2021sub1" -> cancallbackLCP1(INT16)      -> LCP1
+     *        <nodeid>+"_2021sub2" -> cancallbackLCP2(INT16)      -> LCP2
+     *        <nodeid>+"_2021sub3" -> cancallbackLCP3(INT16)      -> LCP3
+     *        <nodeid>+"_2021sub4" -> cancallbackMarker(UINT8)    -> #LCP and marker
+     *        <nodeid>+"_2022"     -> cancallbackState(UINT8)     -> State
+     *
+     *        Mapping for CiA402 (example, testing only):
+     *
+     *        canopenchain-CiA402  -> CanSubscriber-callback      -> CiA402 state
+     *        ---------------------------------------------------------------------
+     *        <nodeid>+"_6000sub1" -> cancallbackError(UINT8)     -> Error register
+     *        <nodeid>+"_6000sub2" -> cancallbackState(UINT8)     -> State
+     *        <nodeid>+"_6000sub3" -> cancallbackCode(UINT8)      -> Code
+     *        <nodeid>+"_6000sub4" -> cancallbackExtCode(UINT8)   -> LSB Extended Code
+     *        <nodeid>+"_6401sub1" -> cancallbackLCP1(INT16)      -> LCP1
+     *        <nodeid>+"_6401sub2" -> cancallbackLCP2(INT16)      -> LCP2
+     *        <nodeid>+"_6401sub3" -> cancallbackLCP3(INT16)      -> LCP3
+     *        <nodeid>+"_6401sub4" -> cancallbackWidthLCP1(INT16) -> Width LCP1
+     *        <nodeid>+"_6401sub5" -> cancallbackWidthLCP2(INT16) -> Width LCP2
+     *        <nodeid>+"_6401sub6" -> cancallbackWidthLCP3(INT16) -> Width LCP3
+     *
+     * See operation manuals for details (file SICK-OLS-Operating_instructions_OLS10_de_IM0076955.PDF for OLS
+     * and file SICK-MLS-Operating_instructions_MLS_de_IM0076567.PDF for MLS)
+     *
+     * @return true on success, otherwise false.
+     */
+    virtual bool subscribeCanTopics(void);
+
+  protected:
+
+    /*
+     * Callbacks for ros messages from canopen_chain_node. There's one callback for each ros topic
+     * published by canopen_chain_node. Each callback updates the sensor state and publishes an
+     * OLS-Measurement message.
+     */
+
+    virtual void cancallbackError(const boost::shared_ptr<std_msgs::UInt8 const>& msg);
+    virtual void cancallbackLCP1(const boost::shared_ptr<std_msgs::Int16 const>& msg);
+    virtual void cancallbackLCP2(const boost::shared_ptr<std_msgs::Int16 const>& msg);
+    virtual void cancallbackLCP3(const boost::shared_ptr<std_msgs::Int16 const>& msg);
+    virtual void cancallbackState(const boost::shared_ptr<std_msgs::UInt8 const>& msg);
+    virtual void cancallbackWidthLCP1(const boost::shared_ptr<std_msgs::Int16 const>& msg);
+    virtual void cancallbackWidthLCP2(const boost::shared_ptr<std_msgs::Int16 const>& msg);
+    virtual void cancallbackWidthLCP3(const boost::shared_ptr<std_msgs::Int16 const>& msg);
+    virtual void cancallbackCode(const boost::shared_ptr<std_msgs::UInt8 const>& msg);
+
+  }; // class CanCiA401Subscriber
+
+} // namespace sick_line_guidance
+#endif // __SICK_LINE_GUIDANCE_CAN_CIA401_SUBSCRIBER_H_INCLUDED

Devices/Packages/sick_line_guidance/include/sick_line_guidance/sick_line_guidance_can_mls_subscriber.h

@@ -0,0 +1,160 @@
+/*
+ * sick_line_guidance_can_mls_subscriber implements a ros subscriber to canopen mls messages.
+ * 
+ * Copyright (C) 2019 Ing.-Buero Dr. Michael Lehning, Hildesheim
+ * Copyright (C) 2019 SICK AG, Waldkirch
+ * 
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ * 
+ *       http://www.apache.org/licenses/LICENSE-2.0
+ *
+ *   Unless required by applicable law or agreed to in writing, software
+ *   distributed under the License is distributed on an "AS IS" BASIS,
+ *   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ *   See the License for the specific language governing permissions and
+ *   limitations under the License.
+ *
+ * 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 SICK AG nor the names of its
+ *       contributors may be used to endorse or promote products derived from
+ *       this software without specific prior written permission
+ *     * Neither the name of Ing.-Buero Dr. Michael Lehning 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.
+ *
+ *      Authors:
+ *         Michael Lehning <michael.lehning@lehning.de>
+ *
+ *  Copyright 2019 SICK AG
+ *  Copyright 2019 Ing.-Buero Dr. Michael Lehning
+ * 
+ */
+#ifndef __SICK_LINE_GUIDANCE_CAN_MLS_SUBSCRIBER_H_INCLUDED
+#define __SICK_LINE_GUIDANCE_CAN_MLS_SUBSCRIBER_H_INCLUDED
+
+#include "sick_line_guidance/sick_line_guidance_can_subscriber.h"
+
+namespace sick_line_guidance
+{
+
+  /*
+   * class CanMlsSubscriber implements the ros subscriber to canopen mls messages.
+   */
+  class CanMlsSubscriber : public CanSubscriber
+  {
+  public:
+  
+    /*
+     * Constructor.
+     * @param[in] nh ros::NodeHandle
+     * @param[in] can_nodeid can id for canopen_chain_node, f.e. "node1"
+     * @param[in] max_num_retries_after_sdo_error After SDO error, the query is repeated max. N times (default: N=2). If the SDO error persists, the can driver is shutdown and restarted.
+     * @param[in] max_sdo_rate max. sdo query and publish rate
+     * @param[in] ros_topic topic for ros messages, f.e. "mls" or "ols"
+     * @param[in] ros_frameid frameid for ros messages, f.e. "mls_measurement_frame" or "ols_measurement_frame"
+     * @param[in] initial_sensor_state initial sensor state (f.e. 0x07 for 3 detected lines, or (1 << 4) to indicate sensor error)
+     * @param[in] subscribe_queue_size buffer size for ros messages
+     */
+    CanMlsSubscriber(ros::NodeHandle & nh, const std::string & can_nodeid, int max_num_retries_after_sdo_error, double max_sdo_rate, const std::string & ros_topic,
+      const std::string & ros_frameid, int initial_sensor_state = 0, int subscribe_queue_size = 1);
+
+    /*
+     * Destructor.
+     */
+    virtual ~CanMlsSubscriber();
+  
+    /*
+     * @brief subsribes to canopen topics for mls messages and sets the callbacks to handle messages from canopen_chain_node
+     *        after PDO messages (LCP = line center point):
+     *
+     *        Mapping for OLS:
+     *
+     *        canopenchain-OLS     -> CanSubscriber-callback      -> OLS sensor state
+     *        -----------------------------------------------------------------------
+     *        <nodeid>+"_1001"     -> cancallbackError(UINT8)     -> Error register
+     *        <nodeid>+"_2018"     -> cancallbackDevState(UINT8)  -> Device state
+     *        <nodeid>+"_2021sub1" -> cancallbackLCP1(INT16)      -> LCP1
+     *        <nodeid>+"_2021sub2" -> cancallbackLCP2(INT16)      -> LCP2
+     *        <nodeid>+"_2021sub3" -> cancallbackLCP3(INT16)      -> LCP3
+     *        <nodeid>+"_2021sub4" -> cancallbackState(UINT8)     -> State
+     *        <nodeid>+"_2021sub5" -> cancallbackWidthLCP1(INT16) -> Width LCP1
+     *        <nodeid>+"_2021sub6" -> cancallbackWidthLCP2(INT16) -> Width LCP2
+     *        <nodeid>+"_2021sub7" -> cancallbackWidthLCP3(INT16) -> Width LCP3
+     *        <nodeid>+"_2021sub8" -> cancallbackCode(UINT8)      -> Code
+     *        <nodeid>+"_2021sub9" -> cancallbackExtCode(UINT32)  -> Extended Code
+     *
+     *        Mapping for MLS:
+     *
+     *        canopenchain-MLS     -> CanSubscriber-callback      -> MLS sensor state
+     *        -----------------------------------------------------------------------
+     *        <nodeid>+"_1001"     -> cancallbackError(UINT8)     -> Error register
+     *        <nodeid>+"_2021sub1" -> cancallbackLCP1(INT16)      -> LCP1
+     *        <nodeid>+"_2021sub2" -> cancallbackLCP2(INT16)      -> LCP2
+     *        <nodeid>+"_2021sub3" -> cancallbackLCP3(INT16)      -> LCP3
+     *        <nodeid>+"_2021sub4" -> cancallbackMarker(UINT8)    -> #LCP and marker
+     *        <nodeid>+"_2022"     -> cancallbackState(UINT8)     -> State
+     *
+     *        Mapping for CiA402 (example, testing only):
+     *
+     *        canopenchain-CiA402  -> CanSubscriber-callback      -> CiA402 state
+     *        ---------------------------------------------------------------------
+     *        <nodeid>+"_6000sub1" -> cancallbackError(UINT8)     -> Error register
+     *        <nodeid>+"_6000sub2" -> cancallbackState(UINT8)     -> State
+     *        <nodeid>+"_6000sub3" -> cancallbackCode(UINT8)      -> Code
+     *        <nodeid>+"_6000sub4" -> cancallbackExtCode(UINT8)   -> LSB Extended Code
+     *        <nodeid>+"_6401sub1" -> cancallbackLCP1(INT16)      -> LCP1
+     *        <nodeid>+"_6401sub2" -> cancallbackLCP2(INT16)      -> LCP2
+     *        <nodeid>+"_6401sub3" -> cancallbackLCP3(INT16)      -> LCP3
+     *        <nodeid>+"_6401sub4" -> cancallbackWidthLCP1(INT16) -> Width LCP1
+     *        <nodeid>+"_6401sub5" -> cancallbackWidthLCP2(INT16) -> Width LCP2
+     *        <nodeid>+"_6401sub6" -> cancallbackWidthLCP3(INT16) -> Width LCP3
+     *
+     * See operation manuals for details (file SICK-OLS-Operating_instructions_OLS10_de_IM0076955.PDF for OLS
+     * and file SICK-MLS-Operating_instructions_MLS_de_IM0076567.PDF for MLS)
+     *
+     * @return true on success, otherwise false.
+     */
+    virtual bool subscribeCanTopics(void);
+
+  protected:
+
+    /*
+     * Callbacks for ros messages from canopen_chain_node. There's one callback for each ros topic
+     * published by canopen_chain_node. Each callback updates the sensor state and publishes an
+     * OLS-Measurement message.
+     */
+
+    virtual void cancallbackLCP1(const boost::shared_ptr<std_msgs::Int16 const>& msg);
+    virtual void cancallbackLCP2(const boost::shared_ptr<std_msgs::Int16 const>& msg);
+    virtual void cancallbackLCP3(const boost::shared_ptr<std_msgs::Int16 const>& msg);
+    virtual void cancallbackMarker(const boost::shared_ptr<std_msgs::UInt8 const>& msg);
+    virtual void cancallbackState(const boost::shared_ptr<std_msgs::UInt8 const>& msg);
+    // virtual void cancallbackError(const boost::shared_ptr<std_msgs::UInt8 const>& msg);
+  
+  }; // class CanMlsSubscriber
+
+} // namespace sick_line_guidance
+#endif // __SICK_LINE_GUIDANCE_CAN_MLS_SUBSCRIBER_H_INCLUDED

Devices/Packages/sick_line_guidance/include/sick_line_guidance/sick_line_guidance_can_ols_subscriber.h

@@ -0,0 +1,213 @@
+/*
+ * sick_line_guidance_can_ols_subscriber implements a ros subscriber to canopen ols messages.
+ * 
+ * Copyright (C) 2019 Ing.-Buero Dr. Michael Lehning, Hildesheim
+ * Copyright (C) 2019 SICK AG, Waldkirch
+ * 
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ * 
+ *       http://www.apache.org/licenses/LICENSE-2.0
+ *
+ *   Unless required by applicable law or agreed to in writing, software
+ *   distributed under the License is distributed on an "AS IS" BASIS,
+ *   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ *   See the License for the specific language governing permissions and
+ *   limitations under the License.
+ *
+ * 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 SICK AG nor the names of its
+ *       contributors may be used to endorse or promote products derived from
+ *       this software without specific prior written permission
+ *     * Neither the name of Ing.-Buero Dr. Michael Lehning 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.
+ *
+ *      Authors:
+ *         Michael Lehning <michael.lehning@lehning.de>
+ *
+ *  Copyright 2019 SICK AG
+ *  Copyright 2019 Ing.-Buero Dr. Michael Lehning
+ * 
+ */
+#ifndef __SICK_LINE_GUIDANCE_CAN_OLS_SUBSCRIBER_H_INCLUDED
+#define __SICK_LINE_GUIDANCE_CAN_OLS_SUBSCRIBER_H_INCLUDED
+
+#include "sick_line_guidance/sick_line_guidance_can_subscriber.h"
+
+namespace sick_line_guidance
+{
+
+  /*
+   * class CanOlsSubscriber implements the base ros subscriber to canopen ols messages.
+   */
+  class CanOlsSubscriber : public CanSubscriber
+  {
+  public:
+  
+    /*
+     * Constructor.
+     * @param[in] nh ros::NodeHandle
+     * @param[in] can_nodeid can id for canopen_chain_node, f.e. "node1"
+     * @param[in] max_num_retries_after_sdo_error After SDO error, the query is repeated max. N times (default: N=2). If the SDO error persists, the can driver is shutdown and restarted.
+     * @param[in] max_sdo_rate max. sdo query and publish rate
+     * @param[in] ros_topic topic for ros messages, f.e. "mls" or "ols"
+     * @param[in] ros_frameid frameid for ros messages, f.e. "mls_measurement_frame" or "ols_measurement_frame"
+     * @param[in] initial_sensor_state initial sensor state (f.e. 0x07 for 3 detected lines, or (1 << 4) to indicate sensor error)
+     * @param[in] subscribe_queue_size buffer size for ros messages
+     */
+    CanOlsSubscriber(ros::NodeHandle & nh, const std::string & can_nodeid, int max_num_retries_after_sdo_error, double max_sdo_rate, const std::string & ros_topic,
+      const std::string & ros_frameid, int initial_sensor_state = 0, int subscribe_queue_size = 1);
+
+    /*
+     * Destructor.
+     */
+    virtual ~CanOlsSubscriber();
+  
+    /*
+     * @brief subsribes to canopen topics for ols messages and sets the callbacks to handle messages from canopen_chain_node
+     *        after PDO messages (LCP = line center point):
+     *
+     *        Mapping for OLS:
+     *
+     *        canopenchain-OLS     -> CanSubscriber-callback      -> OLS sensor state
+     *        -----------------------------------------------------------------------
+     *        <nodeid>+"_1001"     -> cancallbackError(UINT8)     -> Error register
+     *        <nodeid>+"_2018"     -> cancallbackDevState(UINT8)  -> Device state
+     *        <nodeid>+"_2021sub1" -> cancallbackLCP1(INT16)      -> LCP1
+     *        <nodeid>+"_2021sub2" -> cancallbackLCP2(INT16)      -> LCP2
+     *        <nodeid>+"_2021sub3" -> cancallbackLCP3(INT16)      -> LCP3
+     *        <nodeid>+"_2021sub4" -> cancallbackState(UINT8)     -> State
+     *        <nodeid>+"_2021sub5" -> cancallbackWidthLCP1(INT16) -> Width LCP1
+     *        <nodeid>+"_2021sub6" -> cancallbackWidthLCP2(INT16) -> Width LCP2
+     *        <nodeid>+"_2021sub7" -> cancallbackWidthLCP3(INT16) -> Width LCP3
+     *        <nodeid>+"_2021sub8" -> cancallbackCode(UINT8)      -> Code
+     *        <nodeid>+"_2021sub9" -> cancallbackExtCode(UINT32)  -> Extended Code
+     *
+     *        Mapping for MLS:
+     *
+     *        canopenchain-MLS     -> CanSubscriber-callback      -> MLS sensor state
+     *        -----------------------------------------------------------------------
+     *        <nodeid>+"_1001"     -> cancallbackError(UINT8)     -> Error register
+     *        <nodeid>+"_2021sub1" -> cancallbackLCP1(INT16)      -> LCP1
+     *        <nodeid>+"_2021sub2" -> cancallbackLCP2(INT16)      -> LCP2
+     *        <nodeid>+"_2021sub3" -> cancallbackLCP3(INT16)      -> LCP3
+     *        <nodeid>+"_2021sub4" -> cancallbackMarker(UINT8)    -> #LCP and marker
+     *        <nodeid>+"_2022"     -> cancallbackState(UINT8)     -> State
+     *
+     *        Mapping for CiA402 (example, testing only):
+     *
+     *        canopenchain-CiA402  -> CanSubscriber-callback      -> CiA402 state
+     *        ---------------------------------------------------------------------
+     *        <nodeid>+"_6000sub1" -> cancallbackError(UINT8)     -> Error register
+     *        <nodeid>+"_6000sub2" -> cancallbackState(UINT8)     -> State
+     *        <nodeid>+"_6000sub3" -> cancallbackCode(UINT8)      -> Code
+     *        <nodeid>+"_6000sub4" -> cancallbackExtCode(UINT8)   -> LSB Extended Code
+     *        <nodeid>+"_6401sub1" -> cancallbackLCP1(INT16)      -> LCP1
+     *        <nodeid>+"_6401sub2" -> cancallbackLCP2(INT16)      -> LCP2
+     *        <nodeid>+"_6401sub3" -> cancallbackLCP3(INT16)      -> LCP3
+     *        <nodeid>+"_6401sub4" -> cancallbackWidthLCP1(INT16) -> Width LCP1
+     *        <nodeid>+"_6401sub5" -> cancallbackWidthLCP2(INT16) -> Width LCP2
+     *        <nodeid>+"_6401sub6" -> cancallbackWidthLCP3(INT16) -> Width LCP3
+     *
+     * See operation manuals for details (file SICK-OLS-Operating_instructions_OLS10_de_IM0076955.PDF for OLS
+     * and file SICK-MLS-Operating_instructions_MLS_de_IM0076567.PDF for MLS)
+     *
+     * @return true on success, otherwise false.
+     */
+    virtual bool subscribeCanTopics(void);
+
+  protected:
+
+    /*
+     * Callbacks for ros messages from canopen_chain_node. There's one callback for each ros topic
+     * published by canopen_chain_node. Each callback updates the sensor state and publishes an
+     * OLS-Measurement message.
+     */
+
+    virtual void cancallbackLCP1(const boost::shared_ptr<std_msgs::Int16 const>& msg);
+    virtual void cancallbackLCP2(const boost::shared_ptr<std_msgs::Int16 const>& msg);
+    virtual void cancallbackLCP3(const boost::shared_ptr<std_msgs::Int16 const>& msg);
+    virtual void cancallbackState(const boost::shared_ptr<std_msgs::UInt8 const>& msg);
+    virtual void cancallbackWidthLCP1(const boost::shared_ptr<std_msgs::Int16 const>& msg);
+    virtual void cancallbackWidthLCP2(const boost::shared_ptr<std_msgs::Int16 const>& msg);
+    virtual void cancallbackWidthLCP3(const boost::shared_ptr<std_msgs::Int16 const>& msg);
+    virtual void cancallbackCode(const boost::shared_ptr<std_msgs::UInt8 const>& msg);
+    // virtual void cancallbackError(const boost::shared_ptr<std_msgs::UInt8 const>& msg);
+    // virtual void cancallbackDevState(const boost::shared_ptr<std_msgs::UInt8 const>& msg);
+    // virtual void cancallbackExtCode(const boost::shared_ptr<std_msgs::UInt32 const>& msg);
+    
+    bool m_bOLS20queries; // OLS20 only: query objects 2021subA (barcode center point, INT16), 2021subB (quality of lines, UINT8) and 2023sub1 to 2023sub3 (intensity line 1 - 3, UINT8)
+  
+  }; // class CanOlsSubscriber
+  
+  /*
+   * class CanOls10Subscriber derives from CanOlsSubscriber to implements OLS10 specific handling of the ros canopen messages.
+   */
+  class CanOls10Subscriber : public CanOlsSubscriber
+  {
+  public:
+    
+    /*
+     * Constructor.
+     * @param[in] nh ros::NodeHandle
+     * @param[in] can_nodeid can id for canopen_chain_node, f.e. "node1"
+     * @param[in] max_num_retries_after_sdo_error After SDO error, the query is repeated max. N times (default: N=2). If the SDO error persists, the can driver is shutdown and restarted.
+     * @param[in] max_sdo_rate max. sdo query and publish rate
+     * @param[in] ros_topic topic for ros messages, f.e. "mls" or "ols"
+     * @param[in] ros_frameid frameid for ros messages, f.e. "mls_measurement_frame" or "ols_measurement_frame"
+     * @param[in] initial_sensor_state initial sensor state (f.e. 0x07 for 3 detected lines, or (1 << 4) to indicate sensor error)
+     * @param[in] subscribe_queue_size buffer size for ros messages
+     */
+    CanOls10Subscriber(ros::NodeHandle & nh, const std::string & can_nodeid, int max_num_retries_after_sdo_error, double max_sdo_rate, const std::string & ros_topic,
+                     const std::string & ros_frameid, int initial_sensor_state = 0, int subscribe_queue_size = 1);
+  
+  }; // class CanOls10Subscriber
+  
+  /*
+   * class CanOls20Subscriber derives from CanOlsSubscriber to implements OLS20 specific handling of the ros canopen messages.
+   */
+  class CanOls20Subscriber : public CanOlsSubscriber
+  {
+  public:
+    
+    /*
+     * Constructor.
+     * @param[in] nh ros::NodeHandle
+     * @param[in] can_nodeid can id for canopen_chain_node, f.e. "node1"
+     * @param[in] max_num_retries_after_sdo_error After SDO error, the query is repeated max. N times (default: N=2). If the SDO error persists, the can driver is shutdown and restarted.
+     * @param[in] max_sdo_rate max. sdo query and publish rate
+     * @param[in] ros_topic topic for ros messages, f.e. "mls" or "ols"
+     * @param[in] ros_frameid frameid for ros messages, f.e. "mls_measurement_frame" or "ols_measurement_frame"
+     * @param[in] initial_sensor_state initial sensor state (f.e. 0x07 for 3 detected lines, or (1 << 4) to indicate sensor error)
+     * @param[in] subscribe_queue_size buffer size for ros messages
+     */
+    CanOls20Subscriber(ros::NodeHandle & nh, const std::string & can_nodeid, int max_num_retries_after_sdo_error, double max_sdo_rate, const std::string & ros_topic,
+                       const std::string & ros_frameid, int initial_sensor_state = 0, int subscribe_queue_size = 1);
+    
+  }; // class CanOls20Subscriber
+  
+} // namespace sick_line_guidance
+#endif // __SICK_LINE_GUIDANCE_CAN_OLS_SUBSCRIBER_H_INCLUDED

Devices/Packages/sick_line_guidance/include/sick_line_guidance/sick_line_guidance_can_subscriber.h

@@ -0,0 +1,415 @@
+/*
+ * sick_line_guidance_can_subscriber implements the base class for ros subscriber to canopen messages for OLS and MLS.
+ * 
+ * Copyright (C) 2019 Ing.-Buero Dr. Michael Lehning, Hildesheim
+ * Copyright (C) 2019 SICK AG, Waldkirch
+ * 
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ * 
+ *       http://www.apache.org/licenses/LICENSE-2.0
+ *
+ *   Unless required by applicable law or agreed to in writing, software
+ *   distributed under the License is distributed on an "AS IS" BASIS,
+ *   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ *   See the License for the specific language governing permissions and
+ *   limitations under the License.
+ *
+ * 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 SICK AG nor the names of its
+ *       contributors may be used to endorse or promote products derived from
+ *       this software without specific prior written permission
+ *     * Neither the name of Ing.-Buero Dr. Michael Lehning 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.
+ *
+ *      Authors:
+ *         Michael Lehning <michael.lehning@lehning.de>
+ *
+ *  Copyright 2019 SICK AG
+ *  Copyright 2019 Ing.-Buero Dr. Michael Lehning
+ * 
+ */
+#ifndef __SICK_LINE_GUIDANCE_CAN_SUBSCRIBER_H_INCLUDED
+#define __SICK_LINE_GUIDANCE_CAN_SUBSCRIBER_H_INCLUDED
+
+#include <boost/thread.hpp>
+#include <ros/ros.h>
+#include <std_msgs/Int8.h>
+#include <std_msgs/Int16.h>
+#include <std_msgs/Int32.h>
+#include <std_msgs/Float32.h>
+#include <std_msgs/String.h>
+#include <std_msgs/UInt8.h>
+#include <std_msgs/UInt16.h>
+#include <std_msgs/UInt32.h>
+#include "sick_line_guidance/MLS_Measurement.h"
+#include "sick_line_guidance/OLS_Measurement.h"
+#include "sick_line_guidance/sick_line_guidance_msg_util.h"
+
+namespace sick_line_guidance
+{
+
+  /*
+   * class CanSubscriber: base class for CanOlsSubscriber and CanMlsSubscriber,
+   * implements the base class for ros subscriber to canopen messages for OLS and MLS.
+   * Converts canopen messages to MLS/OLS measurement messages and publishes
+   * MLS/OLS measurement messages on the configured ros topic ("/mls" or "/ols").
+   *
+   * class CanSubscriber::MeasurementHandler: queries SDOs (if required) and publishes MLS/OLS measurement messages in a background thread
+   */
+  class CanSubscriber
+  {
+  public:
+
+    /*
+     * Constructor.
+     * @param[in] nh ros::NodeHandle
+     * @param[in] can_nodeid can id for canopen_chain_node, f.e. "node1"
+     * @param[in] max_num_retries_after_sdo_error After SDO error, the query is repeated max. N times (default: N=2). If the SDO error persists, the can driver is shutdown and restarted.
+     * @param[in] max_sdo_rate max. sdo query and publish rate
+     * @param[in] initial_sensor_state initial sensor state (f.e. 0x07 for 3 detected lines, or (1 << 4) to indicate sensor error)
+     * @param[in] subscribe_queue_size buffer size for ros messages
+     */
+    CanSubscriber(ros::NodeHandle & nh, const std::string & can_nodeid, int max_num_retries_after_sdo_error = 2, double max_sdo_rate = 1000, int initial_sensor_state = 0, int subscribe_queue_size = 1);
+
+    /*
+     * Destructor.
+     */
+    virtual ~CanSubscriber();
+  
+    /*
+     * @brief subsribes to canopen topics for ols or mls messages and sets the callbacks to handle messages from canopen_chain_node
+     *        after PDO messages (LCP = line center point). Implemented by subclasses CanOlsSubscriber and CanMlsSubscriber
+     * @return true on success, otherwise false.
+     */
+    virtual bool subscribeCanTopics(void) = 0;
+  
+  protected:
+  
+    /*
+     * @brief Container class for sdo query support. Just collects the pending status of a query (query pending or not pending)
+     * and the time of the last successfull query.
+     */
+    class QuerySupport
+    {
+    public:
+    
+      /*
+       * Constructor
+       * @param[in] query_jitter jitter in seconds (default: 10 ms), i.e. a sdo is requested, if the query is pending and the last successful query is out of the time jitter.
+       * By default, a sdo request is send, if the query is pending and not done within the last 10 ms.
+       */
+      QuerySupport(double query_jitter = 0.01) : m_bQueryPending(false), m_tLastQueryTime(0), m_tQueryJitter(query_jitter) {}
+    
+      /*
+       * returns the pending status, i.e. returns true if the query is pending, returns false if the query is not pending.
+       */
+      virtual bool & pending(void){ return m_bQueryPending; }
+    
+      /*
+       * returns the time of the last successful query
+       */
+      virtual ros::Time & time(void){ return m_tLastQueryTime; }
+    
+      /*
+       * returns true, if a query is required, i.e. the query is pending and the last successful query is over time, otherwise false
+       */
+      virtual bool required(void) { return m_bQueryPending && (ros::Time::now() - m_tLastQueryTime) > m_tQueryJitter; }
+  
+    protected:
+    
+      bool m_bQueryPending;         // true if the query is pending, otherwise false
+      ros::Time m_tLastQueryTime;   // time of the last successful query
+      ros::Duration m_tQueryJitter; // jitter in seconds, i.e. a sdo is requested, if the query is pending and the last successful query is out of a time jitter.
+    };
+  
+    /*
+     * class MeasurementHandler: queries SDOs (if required) and publishes MLS/OLS measurement messages in a background thread
+     */
+    class MeasurementHandler
+    {
+    public:
+    
+      /*
+       * Constructor.
+       * @param[in] nh ros::NodeHandle
+       * @param[in] can_nodeid can id for canopen_chain_node, f.e. "node1"
+       * @param[in] max_num_retries_after_sdo_error After SDO error, the query is repeated max. N times (default: N=2). If the SDO error persists, the can driver is shutdown and restarted.
+       * @param[in] initial_sensor_state initial sensor state (f.e. 0x07 for 3 detected lines, or (1 << 4) to indicate sensor error)
+       * @param[in] max_publish_rate max rate to publish OLS/MLS measurement messages (default: min. 1 ms between two measurement messages)
+       * @param[in] max_query_rate max rate to query SDOs if required (default: min. 1 ms between sdo queries)
+       * @param[in] schedule_publish_delay MLS and OLS measurement message are scheduled to be published 5 milliseconds after first PDO is received
+       * @param[in] max_publish_delay MLS and OLS measurement message are scheduled to be published max. 2*20 milliseconds after first PDO is received, even if a sdo request is pending (max. 2 * tpdo rate)
+       * @param[in] query_jitter jitter in seconds (default: 10 ms), i.e. a sdo is requested, if the query is pending and the last successful query is out of the time jitter.
+       * By default, a sdo request is send, if the query is pending and not done within the last 10 ms.
+       */
+      MeasurementHandler(ros::NodeHandle & nh, const std::string & can_nodeid, int max_num_retries_after_sdo_error = 2, int initial_sensor_state = 0, double max_publish_rate = 1000, double max_query_rate = 1000, double schedule_publish_delay = 0.005, double max_publish_delay = 0.04, double query_jitter = 0.01);
+    
+      /*
+       * Destructor.
+       */
+      virtual ~MeasurementHandler();
+  
+      /*
+       * @brief Runs the background thread to publish MLS/OLS measurement messages
+      */
+      virtual void runMeasurementPublishThread(void);
+  
+      /*
+       * @brief Runs the background thread to query SDOs, if required
+      */
+      virtual void runMeasurementSDOqueryThread(void);
+  
+      /*
+       * @brief schedules the publishing of the current MLS measurement message.
+       * @param[in] schedule if true, publishing is scheduled, otherwise a possibly pending schedule is removed.
+       */
+      virtual void schedulePublishMLSMeasurement(bool schedule);
+
+      /*
+       * @brief schedules the publishing of the current OLS measurement message.
+      * @param[in] schedule if true, publishing is scheduled, otherwise a possibly pending schedule is removed.
+       */
+      virtual void schedulePublishOLSMeasurement(bool schedule);
+      
+      /*
+       * MeasurementHandler member data.
+       */
+    
+      ros::NodeHandle m_nh;                            // ros nodehandel
+      std::string m_can_nodeid;                        // can id for canopen_chain_node, f.e. "node1"
+      ros::Publisher m_ros_publisher;                  // ros publisher for ols or mls measurement messages
+      boost::mutex m_measurement_mutex;                // lock guard for publishing and setting mls/ols sensor state
+      sick_line_guidance::MLS_Measurement m_mls_state; // current state of an mls sensor
+      sick_line_guidance::OLS_Measurement m_ols_state; // current state of an ols sensor
+      ros::Rate m_max_publish_rate;                    // max. rate to publish OLS/MLS measurement message
+      ros::Rate m_max_sdo_query_rate;                  // max. rate to query SDOs if required
+      ros::Time m_publish_mls_measurement;             // time to publish next MLS measurement message
+      ros::Time m_publish_ols_measurement;             // time to publish next OLS measurement message
+      ros::Time m_publish_measurement_latest;          // latest time to publish a measurement message (even if a sdo request is pending)
+      boost::mutex m_publish_measurement_mutex;        // lock guard to schedule publishing measurements using m_publish_mls_measurement and m_publish_ols_measurement
+      ros::Duration m_schedule_publish_delay;          // MLS and OLS measurement message are scheduled to be published 5 milliseconds after first PDO is received
+      ros::Duration m_max_publish_delay;               // MLS and OLS measurement message are scheduled to be published max. 2*20 milliseconds after first PDO is received, even if a sdo request is pending (max. 2 * tpdo rate)
+      QuerySupport m_ols_query_extended_code;          // query object 0x2021sub9 (extended code, UINT32) in object dictionary by SDO, if pending
+      QuerySupport m_ols_query_device_status_u8;       // query object 0x2018 (device status register, OLS20: UINT8) in object dictionary by SDO (query runs in m_measurement in a background thread), if pending
+      QuerySupport m_ols_query_device_status_u16;      // query object 0x2018 (device status register, OLS10: UINT16) in object dictionary by SDO (query runs in m_measurement in a background thread), if pending
+      QuerySupport m_ols_query_error_register;         // query object 0x1001 (error register, UINT8) in object dictionary by SDO (query runs in m_measurement in a background thread), if pending
+      QuerySupport m_ols_query_barcode_center_point;   // OLS20 only: query object 2021subA (barcode center point, INT16), if pending
+      QuerySupport m_ols_query_quality_of_lines;       // OLS20 only: query object 2021subB (quality of lines, UINT8), if pending
+      QuerySupport m_ols_query_intensity_of_lines[3];  // OLS20 only: query object 2023sub1 to 2023sub3 (intensity lines 1 - 3, UINT8), if pending
+      boost::thread * m_measurement_publish_thread;    // background thread to publishes MLS/OLS measurement messages
+      boost::thread * m_measurement_sdo_query_thread;  // background thread to query SDOs if required
+      int m_max_num_retries_after_sdo_error;           // After SDO error, the query is repeated max. N times (default: N=2). If the SDO error persists, the can driver is shutdown and restarted.
+      
+    protected:
+  
+      /*
+       * @brief returns true, if publishing of a MLS measurement is scheduled and time has been reached for publishing the current MLS measurement.
+      */
+      virtual bool isMLSMeasurementTriggered(void);
+  
+      /*
+       * @brief returns true, if publishing of a OLS measurement is scheduled and time has been reached for publishing the current OLS measurement.
+       */
+      virtual bool isOLSMeasurementTriggered(void);
+
+      /*
+       * @brief returns true, if publishing of a measurement is scheduled and latest time for publishing has been reached.
+       */
+      virtual bool isLatestTimeForMeasurementPublishing(void);
+  
+      /*
+       * @brief returns true, if sdo query is pending, i.e. measurement is not yet completed (sdo request or sdo response still pending)
+       */
+      virtual bool isSDOQueryPending(void);
+  
+      /*
+       * @brief queries an object in the object dictionary by SDO and returns its value.
+       * @param[in] can_object_idx object index in object dictionary, f.e. "2018" (OLS device status) or "2021sub9" (OLS extended code)
+       * @param[in] max_num_retries_after_sdo_error After SDO error, the query is repeated max. N times (default: N=2). If the SDO error persists, the can driver is shutdown and restarted.
+       * @param[out] can_object_value object value from SDO response
+       * @return true on success (can_object_value set to objects value), false otherwise (can_object_value not set)
+       */
+      virtual bool querySDO(const std::string & can_object_idx, int max_num_retries_after_sdo_error, uint8_t & can_object_value);
+  
+      /*
+       * @brief queries an object in the object dictionary by SDO and returns its value.
+       * @param[in] can_object_idx object index in object dictionary, f.e. "2018" (OLS device status) or "2021sub9" (OLS extended code)
+       * @param[in] max_num_retries_after_sdo_error After SDO error, the query is repeated max. N times (default: N=2). If the SDO error persists, the can driver is shutdown and restarted.
+       * @param[out] can_object_value object value from SDO response
+       * @return true on success (can_object_value set to objects value), false otherwise (can_object_value not set)
+       */
+      virtual bool querySDO(const std::string & can_object_idx, int max_num_retries_after_sdo_error, uint16_t & can_object_value);
+  
+      /*
+       * @brief queries an object in the object dictionary by SDO and returns its value.
+       * @param[in] can_object_idx object index in object dictionary, f.e. "2018" (OLS device status) or "2021sub9" (OLS extended code)
+       * @param[in] max_num_retries_after_sdo_error After SDO error, the query is repeated max. N times (default: N=2). If the SDO error persists, the can driver is shutdown and restarted.
+       * @param[out] can_object_value object value from SDO response
+       * @return true on success (can_object_value set to objects value), false otherwise (can_object_value not set)
+       */
+      virtual bool querySDO(const std::string & can_object_idx, int max_num_retries_after_sdo_error, int16_t & can_object_value);
+  
+      /*
+       * @brief queries an object in the object dictionary by SDO and returns its value.
+       * @param[in] can_object_idx object index in object dictionary, f.e. "2018" (OLS device status) or "2021sub9" (OLS extended code)
+       * @param[in] max_num_retries_after_sdo_error After SDO error, the query is repeated max. N times (default: N=2). If the SDO error persists, the can driver is shutdown and restarted.
+       * @param[out] can_object_value object value from SDO response
+       * @return true on success (can_object_value set to objects value), false otherwise (can_object_value not set)
+       */
+      virtual bool querySDO(const std::string & can_object_idx, int max_num_retries_after_sdo_error, uint32_t & can_object_value);
+  
+      /*
+       * @brief queries an object in the object dictionary by SDO and returns its value.
+       * @param[in] can_object_idx object index in object dictionary, f.e. "2018" (OLS device status) or "2021sub9" (OLS extended code)
+       * @param[in] max_num_retries_after_sdo_error After SDO error, the query is repeated max. N times (default: N=2). If the SDO error persists, the can driver is shutdown and restarted.
+       * @param[out] can_object_value object value from SDO response
+       * @return true on success (can_object_value set to objects value), false otherwise (can_object_value not set)
+       */
+      virtual bool querySDO(const std::string & can_object_idx, int max_num_retries_after_sdo_error, std::string & can_object_value);
+  
+      /*
+      * @brief queries an object value by SDO, if bQueryPending==true. After SDO query returned, output_value is set and bQueryPending cleared
+      * (assume bQueryPending==false after this function returned).
+      *
+      * @param[in+out] query if query.required() is true, object value is queried by SDO and query is updated (not pending any more). Otherwise, nothing is done.
+      * @param[in] object_index index in object dictionary, f.e. "2021sub9" for object 0x2021 subindex 9
+      * @param[in] max_num_retries_after_sdo_error After SDO error, the query is repeated max. N times (default: N=2). If the SDO error persists, the can driver is shutdown and restarted.
+      * @param[out] output_value object value queried by SDO
+      * @param[in] norm_factor factor to convert object to output value, f.e. 0.001 to convert millimeter (object value) to meter (output value). Default: 1
+      *
+      * @return uint8_t value
+      */
+      template <class S, class T> void querySDOifPending(QuerySupport & query, const std::string & object_index, int max_num_retries_after_sdo_error, T & output_value, T norm_factor)
+      {
+        if(query.pending())
+        {
+          S sdo_value;
+          if(query.required() && querySDO(object_index, max_num_retries_after_sdo_error, sdo_value))
+          {
+            query.time() = ros::Time::now();
+            if(query.pending())
+            {
+              ROS_INFO_STREAM("sick_line_guidance::CanSubscriber::MeasurementHandler: [" << object_index << "]=" << sick_line_guidance::MsgUtil::toHexString(sdo_value));
+              boost::lock_guard<boost::mutex> publish_lockguard(m_measurement_mutex);
+              output_value = (norm_factor * sdo_value);
+            }
+          }
+          query.pending() = false;
+        }
+      }
+  
+      /*
+       * @brief converts a sdo response to uint8.
+       * Note: nh.serviceClient<canopen_chain_node::GetObject> returns SDO responses as strings.
+       * In case of 1 Byte (UINT8) values, the returned "string" contains this byte (uint8 value streamed to std::stringstream).
+       * Example: UINT8 with value 0 are returned as "\0", not "0". Parsing the SDO response has to handle the UINT8 case,
+       * which is done by this function
+       * Note: std::exception are caught (error message and return false in this case)
+       * @param[in] response sdo response as string
+       * @param[out] value uint8 value converted from SDO response
+       * @return true on success, false otherwise
+       */
+      virtual bool convertSDOresponse(const std::string & response, uint8_t & value);
+  
+      /*
+       * @brief converts a sdo response to uint32.
+       * Note: std::exception are caught (error message and return false in this case)
+       * @param[in] response sdo response as string
+       * @param[out] value uint32 value converted from SDO response
+       * @return true on success, false otherwise
+       */
+      virtual bool convertSDOresponse(const std::string & response, uint32_t & value);
+  
+      /*
+       * @brief converts a sdo response to int32.
+       * Note: std::exception are caught (error message and return false in this case)
+       * @param[in] response sdo response as string
+       * @param[out] value uint32 value converted from SDO response
+       * @return true on success, false otherwise
+       */
+      virtual bool convertSDOresponse(const std::string & response, int32_t & value);
+  
+    }; // class MeasurementHandler
+  
+    /*
+     * @brief converts an std_msgs::UInt8/UInt16/UInt32 message to a uint8_t/uint16_t/uint32_t value.
+     *
+     * @param[in] msg UINT8 message
+     * @param[in] defaultvalue default, is returned in case of an invalid message
+     * @param[in] maxvalue 0xFF, 0xFFFF or 0xFFFFFFFF
+     * @param[in] dbg_info informal debug message (prints to ROS_DEBUG or ROS_ERROR)
+     *
+     * @return uint8_t value
+     */
+    template <class S, class T> T convertIntegerMessage(const boost::shared_ptr<S const>& msg, const T & defaultvalue, unsigned maxvalue, const std::string & dbg_info)
+    {
+      T value = defaultvalue;
+      if(msg)
+      {
+        value = ((msg->data)&maxvalue);
+        ROS_DEBUG("sick_line_guidance::CanSubscriber(%s): data: 0x%02x", dbg_info.c_str(), (unsigned)value);
+      }
+      else
+      {
+        ROS_ERROR("## ERROR sick_line_guidance::CanSubscriber(%s): invalid message (%s:%d)", dbg_info.c_str(), __FILE__, __LINE__);
+      }
+      return value;
+    }
+  
+    /*
+     * @brief converts an INT16 message (line center point lcp in millimeter) to a float lcp in meter.
+     *
+     * @param[in] msg INT16 message (line center point lcp in millimeter)
+     * @param[in] defaultvalue default, is returned in case of an invalid message
+     * @param[in] dbg_info informal debug message (ROS_INFO/ROS_ERROR)
+     *
+     * @return float lcp in meter
+     */
+    virtual float convertToLCP(const boost::shared_ptr<std_msgs::Int16 const>& msg, const float & defaultvalue, const std::string & dbg_info);
+
+    /*
+     * @brief schedules the current MLS measurement message for publishing.
+     */
+    virtual void publishMLSMeasurement(void);
+    
+    /*
+     * @brief schedules the current OLS measurement message for publishing.
+     */
+    virtual void publishOLSMeasurement(void);
+
+    /*
+     * member data.
+     */
+
+    std::vector<ros::Subscriber> m_can_subscriber;   // list of subscriber to canopen_chain_node messages
+    int m_subscribe_queue_size;                      // buffer size for ros messages (default: 1)
+    MeasurementHandler m_measurement;                // handles MLS/OLS measurement messages, queries SDO if requiered and publishes ros measurement messages.
+    
+  }; // class CanSubscriber
+
+} // namespace sick_line_guidance
+#endif // __SICK_LINE_GUIDANCE_CAN_SUBSCRIBER_H_INCLUDED

Devices/Packages/sick_line_guidance/include/sick_line_guidance/sick_line_guidance_canopen_chain.h

@@ -0,0 +1,229 @@
+/*
+ * sick_line_guidance_canopen_chain wraps and adapts RosChain of package canopen_chain_node for sick_line_guidance.
+ * 
+ * Copyright (C) 2019 Ing.-Buero Dr. Michael Lehning, Hildesheim
+ * Copyright (C) 2019 SICK AG, Waldkirch
+ * 
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ * 
+ *       http://www.apache.org/licenses/LICENSE-2.0
+ *
+ *   Unless required by applicable law or agreed to in writing, software
+ *   distributed under the License is distributed on an "AS IS" BASIS,
+ *   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ *   See the License for the specific language governing permissions and
+ *   limitations under the License.
+ *
+ * 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 SICK AG nor the names of its
+ *       contributors may be used to endorse or promote products derived from
+ *       this software without specific prior written permission
+ *     * Neither the name of Ing.-Buero Dr. Michael Lehning 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.
+ *
+ *      Authors:
+ *         Michael Lehning <michael.lehning@lehning.de>
+ *
+ *  Copyright 2019 SICK AG
+ *  Copyright 2019 Ing.-Buero Dr. Michael Lehning
+ * 
+ */
+#ifndef __SICK_LINE_GUIDANCE_CANOPEN_CHAIN_H_INCLUDED
+#define __SICK_LINE_GUIDANCE_CANOPEN_CHAIN_H_INCLUDED
+
+#include <socketcan_interface/dispatcher.h>
+#include <socketcan_interface/socketcan.h>
+#include <canopen_chain_node/ros_chain.h>
+
+namespace sick_line_guidance
+{
+
+  /*
+   * class CanopenChain implements canopen support for sick_line_guidance
+   * based on RosChain of package canopen_chain_node (package ros_canopen).
+   */
+  class CanopenChain : public canopen::RosChain
+  {
+  public:
+  
+    /*
+     * Constructor
+     *
+     * @param[in] nh ros::NodeHandle
+     * @param[in] nh_priv ros::NodeHandle
+     */
+    CanopenChain(const ros::NodeHandle &nh, const ros::NodeHandle &nh_priv);
+  
+    /*
+     * Destructor
+     */
+    virtual ~CanopenChain();
+  
+    /*
+     * Connects to CAN bus by calling "init" of canopen_chain_node ros-service
+     *
+     * @param[in] nh ros::NodeHandle
+     *
+     * @return true, if initialization successful, otherwise false.
+     */
+    static bool connectInitService(ros::NodeHandle &nh);
+  
+    /*
+     * Sets all dcf overlays in the can devices. All dcf values are reread from the can device and checked to be identical
+     * to the configured object value. If all dcf overlays have been set successfully (i.e. successfull write operation,
+     * successfull re-read operation and object value identical to the configured value), true is returned.
+     * Otherwise, false is returned (i.e. some dcf overlays could not be set).
+     *
+     * dcf overlays are a list of key value pairs with "object_index" as key and "object_value" as value.
+     * Examples for dcf overlays can be found in the yaml configuration file. Example from sick_line_guidance_ols20.yaml:
+     *   node1:
+     *     id: 0x0A # CAN-Node-ID of can device, default: Node-ID 10=0x0A for OLS and MLS
+     *     eds_pkg: sick_line_guidance # package name for relative path
+     *     eds_file: SICK_OLS20.eds    # path to EDS/DCF file
+     *     ...
+     *     dcf_overlay:
+     *       "2001sub5": "0x01"  # sensorFlipped, UINT8, DataType=0x0005, defaultvalue 0x00
+     *       "2002sub1": "0.015" # Typ. Width, FLOAT32, DataType=0x0008
+     *       "2002sub2": "0.001" # Min. Width, FLOAT32, DataType=0x0008
+     *       "2002sub3": "0.050" # Max. Width, FLOAT32, DataType=0x0008
+     *
+     * @param[in] nh ros::NodeHandle
+     * @param[in] node_id can node id of the can device
+     * @param[in] dcf_overlays list of dcf overlays, format "object_index":"object_value"
+     *
+     * @return true, if dcf overlays set successfully, otherwise false (write error, read error
+     * or queried value differs from configured object value).
+     */
+    static bool writeDCFoverlays(ros::NodeHandle &nh, const std::string & node_id, XmlRpc::XmlRpcValue & dcf_overlays);
+  
+    /*
+     * Queries an object of a can node from canopen service by its object index.
+     *
+     * @param[in] nh ros::NodeHandle
+     * @param[in] node_id can node id of the can device
+     * @param[in] object index in the object dictonary of the can device, f.e. "1001sub", "1018sub4"
+     * @param[in] max_num_retries_after_sdo_error After SDO error, the query is repeated max. N times (default: N=2). If the SDO error persists, the can driver is shutdown and restarted.
+     * @param[out] output_message informational message in case of errors (responce from canopen service)
+     * @param[out] output_value value of the object (responce from canopen service)
+     *
+     * @return true, if query successful, otherwise false.
+     */
+    static bool queryCanObject(ros::NodeHandle &nh, const std::string & node_id, const std::string & objectidx, int max_num_retries_after_sdo_error,
+      std::string & output_message, std::string & output_value);
+
+    /*
+     * Sets the value of an object in the object dictionary of a can device.
+     *
+     * @param[in] nh ros::NodeHandle
+     * @param[in] node_id can node id of the can device
+     * @param[in] object index in the object dictonary of the can device, f.e. "1001sub", "1018sub4"
+     * @param[in] object_value value of the object
+     * @param[out] response value of the object (responce from canopen service)
+     *
+     * @return true, if SDO successful, otherwise false.
+     */
+    static bool setCanObjectValue(ros::NodeHandle &nh, const std::string & node_id, const std::string &  objectidx,
+      const std::string & object_value, std::string & response);
+
+    /*
+     * Recovers can driver after emergency or other errors. Calls "/driver/recover" from canopen_chain_node
+     *
+     * @param[in] nh ros::NodeHandle
+     *
+     * @return true, if recover command returned with success, otherwise false.
+     */
+    static bool recoverCanDriver(ros::NodeHandle &nh);
+
+    /*
+     * Shutdown can driver. Calls "/driver/shutdown" from canopen_chain_node
+     *
+     * @param[in] nh ros::NodeHandle
+     *
+     * @return true, if shutdown command returned with success, otherwise false.
+     */
+    static bool shutdownCanDriver(ros::NodeHandle & nh);
+    
+  protected:
+
+    /*
+     * Compares a destination value configured by dcf_overlay with the sdo response.
+     * Comparison by string, integer or float comparison.
+     * Returns true, if both values are identical, or otherwise false.
+     * Note: dcf_overlay_value and sdo_response may represent identical numbers, but different strings (different number formatting)
+     * Therefor, both string and numbers are compared in this function.
+     *
+     * @param[in] dcf_overlay_value configured value
+     * @param[in] sdo_response received value
+     *
+     * @return true, if dcf_overlay_value is identical to sdo_response or has identical values.
+     */
+    static bool cmpDCFoverlay(const std::string & dcf_overlay_value, const std::string & sdo_response);
+    
+    /*
+     * Converts a string to UINT32. Catches conversion exceptions and returns false in case of number parsing errors.
+     *
+     * @param[in] str input string to be parsed
+     * @param[out] value output value (== input converted to number in case of success)
+     *
+     * @return true in case of success, false otherwise
+     */
+    static uint32_t tryParseUINT32(const std::string & str, uint32_t & value);
+
+    /*
+     * Converts a string to INT32. Catches conversion exceptions and returns false in case of number parsing errors.
+     *
+     * @param[in] str input string to be parsed
+     * @param[out] value output value (== input converted to number in case of success)
+     *
+     * @return true in case of success, false otherwise
+     */
+    static int32_t tryParseINT32(const std::string & str, int32_t & value);
+
+    /*
+     * Converts a string to FLOAT32. Catches conversion exceptions and returns false in case of number parsing errors.
+     *
+     * @param[in] str input string to be parsed
+     * @param[out] value output value (== input converted to number in case of success)
+     *
+     * @return true in case of success, false otherwise
+     */
+    static int32_t tryParseFLOAT(const std::string & str, float & value);
+
+    /*
+     * Prints and converts a sdo response. 1 byte datatypes (UINT8) are converted to decimal (f.e. "\0" is returned as "0").
+     *
+     * @param[in] response sdo response
+     *
+     * @return printed response.
+     */
+    static std::string sdoResponseToString(const std::string & response);
+  
+  
+  }; // class CanopenChain
+  
+} // namespace sick_line_guidance
+#endif // __SICK_LINE_GUIDANCE_CANOPEN_CHAIN_H_INCLUDED

Devices/Packages/sick_line_guidance/include/sick_line_guidance/sick_line_guidance_cloud_converter.h

@@ -0,0 +1,154 @@
+/*
+ * sick_line_guidance_cloud_converter converts sensor measurement data to PointCloud2.
+ * 
+ * Copyright (C) 2019 Ing.-Buero Dr. Michael Lehning, Hildesheim
+ * Copyright (C) 2019 SICK AG, Waldkirch
+ * 
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ * 
+ *       http://www.apache.org/licenses/LICENSE-2.0
+ *
+ *   Unless required by applicable law or agreed to in writing, software
+ *   distributed under the License is distributed on an "AS IS" BASIS,
+ *   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ *   See the License for the specific language governing permissions and
+ *   limitations under the License.
+ *
+ * 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 SICK AG nor the names of its
+ *       contributors may be used to endorse or promote products derived from
+ *       this software without specific prior written permission
+ *     * Neither the name of Ing.-Buero Dr. Michael Lehning 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.
+ *
+ *      Authors:
+ *         Michael Lehning <michael.lehning@lehning.de>
+ *
+ *  Copyright 2019 SICK AG
+ *  Copyright 2019 Ing.-Buero Dr. Michael Lehning
+ * 
+ */
+#ifndef __SICK_LINE_GUIDANCE_CLOUD_CONVERTER_H_INCLUDED
+#define __SICK_LINE_GUIDANCE_CLOUD_CONVERTER_H_INCLUDED
+
+#include <sensor_msgs/PointCloud2.h>
+#include "sick_line_guidance/MLS_Measurement.h"
+#include "sick_line_guidance/OLS_Measurement.h"
+
+namespace sick_line_guidance
+{
+
+  /*
+   * class CloudConverter implements utility functions to convert measurement data to PointCloud2.
+   */
+  class CloudConverter
+  {
+  public:
+  
+    /*
+     * @brief converts OLS measurement data to PointCloud2.
+     *
+     * @param[in] measurement OLS measurement data
+     * @param[in]frame_id frame_id of PointCloud2 message
+     *
+     * @return sensor_msgs::PointCloud2 data converted from measurement
+     */
+    static sensor_msgs::PointCloud2 convert(const sick_line_guidance::OLS_Measurement & measurement, const std::string & frame_id);
+  
+    /*
+    * @brief converts OLS measurement data to PointCloud2.
+    *
+    * @param[in] measurement OLS measurement data
+    * @param[in]frame_id frame_id of PointCloud2 message
+    *
+    * @return sensor_msgs::PointCloud2 data converted from measurement
+    */
+    static sensor_msgs::PointCloud2 convert(const sick_line_guidance::MLS_Measurement & measurement, const std::string & frame_id);
+  
+    /*
+    * @brief prints a PointCloud2 data field to string.
+    *
+    * @param[in] cloud PointCloud2 data
+    *
+    * @return PointCloud2 data converted to string
+    */
+    static std::string cloudDataToString(const sensor_msgs::PointCloud2 & cloud);
+    
+  protected:
+  
+    /*
+     * @brief returns the status for each line (detected or not detected).
+     *
+     * @param[in] status status byte of measurement data
+     * @param[in] numlines number of lined (normally 3 lines)
+     *
+     * @return detection status for each line
+     */
+    static std::vector<bool> linestatus(uint8_t status, size_t numlines);
+
+    /*
+     * @brief returns the number of lines detected (1, 2 or 3 lines).
+     *
+     * @param[in] status status byte of measurement data
+     *
+     * @return number of lines detected by OLS or MLS
+     */
+    static int linenumber(uint8_t status);
+
+    /*
+     * @brief returns true, if MLS measurement status is okay, or false otherwise.
+     *
+     * @param[in] measurement MLS measurement data
+     *
+     * @return true (sensor status okay), or false (sensor status not okay)
+     */
+    static bool measurementstatus(const sick_line_guidance::MLS_Measurement & measurement);
+
+    /*
+     * @brief converts and prints a single field of PointCloud2 according to its dataype.
+     *
+     * @param[in] datatype enumeration of the datatye, see sensor_msgs::PointField
+     * @param[in] data pointer to the data to be converted and printed
+     * @param[in] end pointer to the end of PointCloud2 data
+     *
+     * @return data field converted to string
+     */
+    static std::string cloudDataFieldToString(uint8_t datatype, const uint8_t* data, const uint8_t* end);
+    
+    /*
+     * @brief flips the bits of a code, i.e. reverses the bits (output bit 0 := input bit 31, output bit 1 := input bit 30, and so on)
+     *
+     * @param[in] code bits to be flipped
+     *
+     * @return flipped code
+     */
+    static uint32_t flipBits(uint32_t code);
+  
+  }; // class CloudConverter
+  
+} // namespace sick_line_guidance
+#endif // __SICK_LINE_GUIDANCE_CLOUD_CONVERTER_H_INCLUDED

Devices/Packages/sick_line_guidance/include/sick_line_guidance/sick_line_guidance_diagnostic.h

@@ -0,0 +1,160 @@
+/*
+ * sick_line_guidance_diagnostic publishes diagnostic messages for sick_line_guidance
+ *
+ * Copyright (C) 2019 Ing.-Buero Dr. Michael Lehning, Hildesheim
+ * Copyright (C) 2019 SICK AG, Waldkirch
+ * 
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ * 
+ *       http://www.apache.org/licenses/LICENSE-2.0
+ *
+ *   Unless required by applicable law or agreed to in writing, software
+ *   distributed under the License is distributed on an "AS IS" BASIS,
+ *   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ *   See the License for the specific language governing permissions and
+ *   limitations under the License.
+ *
+ * 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 SICK AG nor the names of its
+ *       contributors may be used to endorse or promote products derived from
+ *       this software without specific prior written permission
+ *     * Neither the name of Ing.-Buero Dr. Michael Lehning 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.
+ *
+ *      Authors:
+ *         Michael Lehning <michael.lehning@lehning.de>
+ *
+ *  Copyright 2019 SICK AG
+ *  Copyright 2019 Ing.-Buero Dr. Michael Lehning
+ * 
+ */
+#ifndef __SICK_LINE_GUIDANCE_DIAGNOSTIC_H_INCLUDED
+#define __SICK_LINE_GUIDANCE_DIAGNOSTIC_H_INCLUDED
+
+#include <ros/ros.h>
+
+namespace sick_line_guidance
+{
+  /*
+   * enum DIAGNOSTIC_STATUS enumerates the possible status values of diagnostic messages.
+   * Higher values mean more severe failures.
+   */
+  typedef enum DIAGNOSTIC_STATUS_ENUM
+  {
+    OK,                       // status okay, no errors
+    EXIT,                     // sick_line_guidance exiting
+    NO_LINE_DETECTED,         // device signaled "no line detected"
+    ERROR_STATUS,             // device signaled an error (error flag set in TPDO)
+    SDO_COMMUNICATION_ERROR,  // error in SDO query, timeout on receiving SDO response
+    CAN_COMMUNICATION_ERROR,  // can communication error, shutdown and reset communication
+    CONFIGURATION_ERROR,      // invalid configuration, check configuration files
+    INITIALIZATION_ERROR,     // initialization of CAN 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 diagnostics 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 diagnostics
+      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 diagnostics for sick_line_guidance
+    
+  }; // class Diagnostic
+  
+} // namespace sick_line_guidance
+#endif // __SICK_LINE_GUIDANCE_DIAGNOSTIC_H_INCLUDED
+

Devices/Packages/sick_line_guidance/include/sick_line_guidance/sick_line_guidance_eds_util.h

@@ -0,0 +1,110 @@
+/*
+ * sick_line_guidance_eds_util implements utility functions for eds-files.
+ * 
+ * Copyright (C) 2019 Ing.-Buero Dr. Michael Lehning, Hildesheim
+ * Copyright (C) 2019 SICK AG, Waldkirch
+ * 
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ * 
+ *       http://www.apache.org/licenses/LICENSE-2.0
+ *
+ *   Unless required by applicable law or agreed to in writing, software
+ *   distributed under the License is distributed on an "AS IS" BASIS,
+ *   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ *   See the License for the specific language governing permissions and
+ *   limitations under the License.
+ *
+ * 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 SICK AG nor the names of its
+ *       contributors may be used to endorse or promote products derived from
+ *       this software without specific prior written permission
+ *     * Neither the name of Ing.-Buero Dr. Michael Lehning 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.
+ *
+ *      Authors:
+ *         Michael Lehning <michael.lehning@lehning.de>
+ *
+ *  Copyright 2019 SICK AG
+ *  Copyright 2019 Ing.-Buero Dr. Michael Lehning
+ * 
+ */
+#ifndef __SICK_LINE_GUIDANCE_EDS_UTIL_H_INCLUDED
+#define __SICK_LINE_GUIDANCE_EDS_UTIL_H_INCLUDED
+
+#include <canopen_master/objdict.h>
+
+namespace sick_line_guidance
+{
+
+  /*
+   * class EdsUtil implements utility functions for eds-files.
+   */
+  class EdsUtil
+  {
+  public:
+
+    /*
+     * @brief returns a full qualified filepath from package name and file name.
+     *
+     * @param package package name
+     *
+     * @return full qualified filepath (or filename if package is empty or couldn't be found).
+     */
+    static std::string filepathFromPackage(const std::string & package, const std::string & filename);
+  
+    /*
+     * @brief reads and parses an eds-file and prints all objects.
+     *
+     * @param eds_filepath path to eds-file
+     *
+     * @return object dictionary of a given eds-file printed to string.
+     */
+    static std::string readParsePrintEdsfile(const std::string & eds_filepath);
+
+    /*
+     * @brief prints the data of a canopen::ObjectDict::Entry value to std::string.
+     *
+     * @param entry object dictionary entry to be printed
+     *
+     * @return entry converted to string.
+     */
+    static std::string printObjectDictEntry(const canopen::ObjectDict::Entry & entry);
+
+    /*
+     * @brief prints the data bytes of a canopen::HoldAny value to std::string.
+     *
+     * @param parameter_name name of the parameter
+     * @param holdany value to be printed
+     *
+     * @return value converted to string.
+     */
+    static std::string printHoldAny(const std::string & parameter_name, const canopen::HoldAny & holdany);
+
+  }; // class EdsUtil
+
+} // namespace sick_line_guidance
+#endif // __SICK_LINE_GUIDANCE_EDS_UTIL_H_INCLUDED

Devices/Packages/sick_line_guidance/include/sick_line_guidance/sick_line_guidance_msg_util.h

@@ -0,0 +1,276 @@
+/*
+ * sick_line_guidance_msg_util implements utility functions for ros messages.
+ * 
+ * Copyright (C) 2019 Ing.-Buero Dr. Michael Lehning, Hildesheim
+ * Copyright (C) 2019 SICK AG, Waldkirch
+ * 
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ * 
+ *       http://www.apache.org/licenses/LICENSE-2.0
+ *
+ *   Unless required by applicable law or agreed to in writing, software
+ *   distributed under the License is distributed on an "AS IS" BASIS,
+ *   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ *   See the License for the specific language governing permissions and
+ *   limitations under the License.
+ *
+ * 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 SICK AG nor the names of its
+ *       contributors may be used to endorse or promote products derived from
+ *       this software without specific prior written permission
+ *     * Neither the name of Ing.-Buero Dr. Michael Lehning 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.
+ *
+ *      Authors:
+ *         Michael Lehning <michael.lehning@lehning.de>
+ *
+ *  Copyright 2019 SICK AG
+ *  Copyright 2019 Ing.-Buero Dr. Michael Lehning
+ * 
+ */
+#ifndef __SICK_LINE_GUIDANCE_MSG_UTIL_H_INCLUDED
+#define __SICK_LINE_GUIDANCE_MSG_UTIL_H_INCLUDED
+
+#include <sstream>
+#include <ros/ros.h>
+#include <std_msgs/Int8.h>
+#include <std_msgs/Int16.h>
+#include <std_msgs/Int32.h>
+#include <std_msgs/Float32.h>
+#include <std_msgs/String.h>
+#include <std_msgs/UInt8.h>
+#include <std_msgs/UInt16.h>
+#include <std_msgs/UInt32.h>
+#include "sick_line_guidance/MLS_Measurement.h"
+#include "sick_line_guidance/OLS_Measurement.h"
+
+namespace sick_line_guidance
+{
+
+  /*
+   * class MsgUtil implements utility functions for ros messages.
+   */
+  class MsgUtil
+  {
+  public:
+  
+    /*
+     * @brief compares two messages, returns true if content is equal, or false otherwise.
+     *
+     * @param[in] msg1 message to be compared to msg2
+     * @param[in] msg2 message to be compared to msg1
+     *
+     * @return true if message content is equal, false otherwise.
+     */
+    template<class T> static bool msgIdentical(const T & msg1, const T & msg2)
+    {
+      std::stringstream s1;
+      std::stringstream s2;
+      s1 << msg1;
+      s2 << msg2;
+      return s1.str() == s2.str();
+    }
+  
+    /*
+     * @brief shortcut to convert an uint8 value to hex string "0x...".
+     *
+     * @param[in] value value to be printed
+     *
+     * @return hex string .
+     */
+    static std::string toHexString(uint8_t value)
+    {
+      return toHexString(static_cast<unsigned>(value & 0xFF), 2);
+    }
+  
+    /*
+     * @brief shortcut to convert an uint16 value to hex string "0x...".
+     *
+     * @param[in] value value to be printed
+     *
+     * @return hex string .
+     */
+    static std::string toHexString(uint16_t value)
+    {
+      return toHexString(static_cast<unsigned>(value & 0xFFFF), 4);
+    }
+  
+    /*
+     * @brief shortcut to convert an uint16 value to hex string "0x...".
+     *
+     * @param[in] value value to be printed
+     *
+     * @return hex string .
+     */
+    static std::string toHexString(int16_t value)
+    {
+      std::stringstream str;
+      str << toHexString(static_cast<uint16_t>(value));
+      str << "=" << value << "d";
+      return str.str();
+    }
+  
+    /*
+     * @brief shortcut to convert an uint32 value to hex string "0x...".
+     *
+     * @param[in] value value to be printed
+     *
+     * @return hex string .
+     */
+    static std::string toHexString(uint32_t value, int w = 8)
+    {
+      std::stringstream str;
+      str << "0x" << std::uppercase << std::hex << std::setfill('0') << std::setw(w) << value;
+      return str.str();
+    }
+  
+    /*
+     * @brief prints and returns a MLS measurement as informational string
+     * @param[in] measurement_msg MLS measurement to print
+     * @return informational string containing the MLS measurement data
+    */
+    static std::string toInfo(const sick_line_guidance::MLS_Measurement & measurement_msg);
+  
+    /*
+     * @brief prints and returns a OLS measurement as informational string
+     * @param[in] measurement_msg OLS measurement to print
+     * @return informational string containing the OLS measurement data
+    */
+    static std::string toInfo(const sick_line_guidance::OLS_Measurement & measurement_msg);
+  
+    /*
+     * @brief initializes a MLS measurement with zero data
+     * @param[in+out] measurement_msg MLS measurement
+     */
+    static void zero(sick_line_guidance::MLS_Measurement & measurement_msg);
+  
+    /*
+     * @brief initializes an OLS measurement with zero data
+     * @param[in+out] measurement_msg OLS measurement
+     */
+    static void zero(sick_line_guidance::OLS_Measurement & measurement_msg);
+  
+    /*
+     * @brief Returns a MLS sensor measurement
+     *
+     * @param[in] lcp1 line center point LCP1 in meter, object 0x2021sub1 in object dictionary
+     * @param[in] lcp2 line center point LCP2 in meter, object 0x2021sub2 in object dictionary
+     * @param[in] lcp3 line center point LCP3 in meter, object 0x2021sub3 in object dictionary
+     * @param[in] status status of measurement, object 0x2022 in object dictionary (Bit7=MSBit to Bit0=LSBit): Bit7: 0, Bit6: ReadingCode, Bit5: Polarity, Bit4: SensorFlipped, Bit3: LineLevBit2, Bit2: LineLevBit1, Bit1: LineLevBit0, Bit0: Linegood(1:LineGood,0:NotGood)
+     * @param[in] lcp LCP-flags (signs and line assignment), in bits (MSB to LSB): Bit7: MarkerBit4, Bit6: MarkerBit3, Bit5: MarkerBit2, Bit4: MarkerBit1, Bit3: MarkerBit0, Bit2: #LCPBit2, Bit1: #LCPBit1, Bit0: #LCPBit0
+     * @param[in] error error register, object 0x1001 in object dictionary
+     * @param[in] msg_frame_id frame id of OLS_Measurement message
+     *
+     * @return parameter converted to MLS_Measurement
+     */
+    static sick_line_guidance::MLS_Measurement convertMLSMessage(float lcp1, float lcp2, float lcp3, uint8_t status, uint8_t lcp, uint8_t error, const std::string & msg_frame_id);
+  
+    /*
+     * @brief Returns an OLS sensor measurement
+     *
+     * @param[in] lcp1 line center point LCP1 in meter, object 0x2021sub1 in object dictionary
+     * @param[in] lcp2 line center point LCP2 in meter, object 0x2021sub2 in object dictionary
+     * @param[in] lcp3 line center point LCP3 in meter, object 0x2021sub3 in object dictionary
+     * @param[in] width1 width of line 1 in meter, object 0x2021sub5 in object dictionary
+     * @param[in] width2 width of line 2 in meter, object 0x2021sub6 in object dictionary
+     * @param[in] width3 width of line 3 in meter, object 0x2021sub7 in object dictionary
+     * @param[in] status status of measurement, object 0x2021sub4 in object dictionary, in bits (MSB to LSB): Bit7: CodeValid, Bit6: CodeFlipped, Bit5: x, Bit4: DeviceStatus, Bit3: x, Bit2: #LCPBit2, Bit1: #LCPBit1, Bit0: #LCPBit0
+     * @param[in] barcode Barcode (> 255: extended barcode), , object 0x2021sub8 and 0x2021sub9 in object dictionary
+     * @param[in] dev_status Device status, object 0x2018 in object dictionary
+     * @param[in] error error register, object 0x1001 in object dictionary
+     * @param[in] barcodecenter barcode_center_point,  OLS20 only (0x2021subA), OLS10: always 0
+     * @param[in] linequality quality_of_lines,         OLS20 only (0x2021subB), OLS10: always 0
+     * @param[in] lineintensity1 intensity_of_lines[0], OLS20 only (0x2023sub1), OLS10: always 0
+     * @param[in] lineintensity2 intensity_of_lines[1], OLS20 only (0x2023sub2), OLS10: always 0
+     * @param[in] lineintensity3 intensity_of_lines[2], OLS20 only (0x2023sub3), OLS10: always 0
+     * @param[in] msg_frame_id frame id of OLS_Measurement message
+     *
+     * @return parameter converted to OLS_Measurement
+     */
+    static sick_line_guidance::OLS_Measurement convertOLSMessage(float lcp1, float lcp2, float lcp3, float width1, float width2, float width3, uint8_t status, uint32_t barcode, uint8_t dev_status, uint8_t error,
+      float barcodecenter, uint8_t linequality, uint8_t lineintensity1, uint8_t lineintensity2, uint8_t lineintensity3, const std::string & msg_frame_id);
+
+    /*
+     * @brief returns true, if OLS device status is okay, or false otherwise.
+     *
+     * @param[in] measurement OLS measurement data
+     *
+     * @return true (sensor status okay), or false (sensor status not okay)
+     */
+    static bool statusOK(const sick_line_guidance::OLS_Measurement & measurement)
+    {
+      // OLS status bit 4: 0 => Sensor ok, 1 => Sensor not ok, see 0x2018 (measurement.dev_status)
+      return ((measurement.status & (0x1 << 4)) == 0);
+    }
+  
+    /*
+     * @brief returns true, if OLS device detected a line, or false otherwise.
+     *
+     * @param[in] measurement OLS measurement data
+     *
+     * @return true (line good), or false (no line detected)
+     */
+    static bool lineOK(const sick_line_guidance::OLS_Measurement & measurement)
+    {
+      return ((measurement.status & 0x7) != 0); // Bit 0-2 OLS status == 0 => no line found
+    }
+  
+    /*
+     * @brief returns true, if MLS device detected a line, or false otherwise.
+     *
+     * @param[in] measurement MLS measurement data
+     *
+     * @return true (line good), or false (no line detected)
+     */
+    static bool lineOK(const sick_line_guidance::MLS_Measurement & measurement)
+    {
+      // MLS status bit 0 ("Line good") == 0 => no line detected or line too weak, 1 => line detected, MLS #lcp (bit 0-2 == 0) => no line detected
+      return ((measurement.status & 0x1) != 0) && ((measurement.lcp & 0x7) != 0);
+    }
+  
+    /*
+     * @brief returns a gaussian destributed random line center position (lcp)
+     *
+     * @param stddev standard deviation of gaussian random generator
+     *
+     * @return random line center position
+     */
+    static float randomLCP(double stddev);
+  
+    /*
+     * @brief returns a gaussian destributed random line width
+     *
+     * @param stddev standard deviation of gaussian random generator
+     *
+     * @return random line width
+     */
+    static float randomLW(double min_linewidth, double max_linewidth);
+  
+  }; // class MsgUtil
+
+} // namespace sick_line_guidance
+#endif // __SICK_LINE_GUIDANCE_MSG_UTIL_H_INCLUDED

Devices/Packages/sick_line_guidance/include/sick_line_guidance/sick_line_guidance_version.h

@@ -0,0 +1,89 @@
+/*
+ * sick_line_guidance_version handles the version of sick_line_guidance ros driver.
+ * The version concates major version, minor version and patch level, each with three digits.
+ * 
+ * Copyright (C) 2019 Ing.-Buero Dr. Michael Lehning, Hildesheim
+ * Copyright (C) 2019 SICK AG, Waldkirch
+ * 
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ * 
+ *       http://www.apache.org/licenses/LICENSE-2.0
+ *
+ *   Unless required by applicable law or agreed to in writing, software
+ *   distributed under the License is distributed on an "AS IS" BASIS,
+ *   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ *   See the License for the specific language governing permissions and
+ *   limitations under the License.
+ *
+ * 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 SICK AG nor the names of its
+ *       contributors may be used to endorse or promote products derived from
+ *       this software without specific prior written permission
+ *     * Neither the name of Ing.-Buero Dr. Michael Lehning 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.
+ *
+ *      Authors:
+ *         Michael Lehning <michael.lehning@lehning.de>
+ *
+ *  Copyright 2019 SICK AG
+ *  Copyright 2019 Ing.-Buero Dr. Michael Lehning
+ * 
+ */
+#ifndef __SICK_LINE_GUIDANCE_VERSION_H_INCLUDED
+#define __SICK_LINE_GUIDANCE_VERSION_H_INCLUDED
+
+#define SICK_LINE_GUIDANCE_MAJOR_VER   1
+#define SICK_LINE_GUIDANCE_MINOR_VER   0
+#define SICK_LINE_GUIDANCE_PATCH_LEVEL 0
+
+namespace sick_line_guidance
+{
+
+  /*
+   * class Version handles the version of sick_line_guidance ros driver.
+   */
+  class Version
+  {
+  public:
+  
+    /*
+     * @brief returns the version as a string, f.e. "001.002.003".
+     *
+     * @return version string.
+     */
+    static std::string getVersionInfo(void)
+    {
+      std::stringstream str;
+      str << std::uppercase << std::setfill('0') << std::setw(3) << SICK_LINE_GUIDANCE_MAJOR_VER << ".";
+      str << std::uppercase << std::setfill('0') << std::setw(3) << SICK_LINE_GUIDANCE_MINOR_VER << ".";
+      str << std::uppercase << std::setfill('0') << std::setw(3) << SICK_LINE_GUIDANCE_PATCH_LEVEL;
+      return str.str();
+    }
+   
+  }; // class Version
+} // namespace sick_line_guidance
+#endif // __SICK_LINE_GUIDANCE_VERSION_H_INCLUDED

Devices/Packages/sick_line_guidance/launch/sick_canopen_simu.launch

@@ -0,0 +1,18 @@
+<launch>
+
+  <!-- sick_line_guidance: run sick_canopen_simu_node -->
+  <arg name="device"/>                                                  <!-- can device, either "OLS" or "MLS" -->
+  <node name="sick_canopen_simu_node" pkg="sick_line_guidance" type="sick_canopen_simu_node" />
+  <param name="sick_canopen_simu_cfg_file" type="str" value="$(find sick_line_guidance)/sick_canopen_simu_cfg.xml" />  <!-- configuration file and testcases for OLS and MLS simulation --> 
+  <param name="can_node_id" type="int" value="10" />                    <!-- can node id of OLS/MLS simulator -->
+  <param name="sick_device_family" type="str" value="$(arg device)" />  <!-- simulation of either "OLS10", "OLS20" or "MLS" device -->
+  <param name="can_subscribe_topic" type="str" value="can0" />          <!-- topic for ros can messages (input), message type can_msgs::Frame --> 
+  <param name="mls_subscribe_topic" type="str" value="mls" />           <!-- topic for ros measurement messages (input), message type MLS_Measurement -->
+  <param name="ols_subscribe_topic" type="str" value="ols" />           <!-- topic for ros measurement messages (input), message type OLS_Measurement -->
+  <param name="publish_topic" type="str" value="ros2can0" />            <!-- topic for ros messages (output), message type can_msgs::Frame --> 
+  <param name="pdo_rate" type="double" value="10" />                    <!-- rate of PDOs: 50 PDOs simulated per second, i.e. 20 ms between two PDOs --> 
+  <param name="pdo_repeat_cnt" type="int" value="25" />                 <!-- each sensor state spefied in sick_canopen_simu_cfg.xml is repeated 25 times before switching to the next state (sensor state changes after 0.5 seconds) --> 
+  <param name="can_message_queue_size" type="int" value="16" />         <!-- buffer size for can messages -->
+  <param name="sensor_state_queue_size" type="int" value="2" />         <!-- buffer size for simulated sensor states (OLS: 2 TPDOs) -->
+
+</launch>

Devices/Packages/sick_line_guidance/launch/sick_line_guidance.launch

@@ -0,0 +1,35 @@
+<launch>
+
+  <!-- sick_line_guidance: global configuration -->
+  <arg name="yaml" default="$(find sick_line_guidance)/sick_line_guidance_mls.yaml"/>
+  <rosparam command="load" file="$(arg yaml)" />                    <!-- Global CAN configuration by ols or mls yaml-file incl. link to eds-file -->
+  
+  <!-- sick_line_guidance: run canopen_chain_node -->
+  <!-- to run sick_line_guidance_can_chain_node in gdb: add parameter launch-prefix="gdb -ex run - -args": -->
+  <!-- node name="sick_line_guidance_can_chain_node" pkg="sick_line_guidance" type="sick_line_guidance_can_chain_node" launch-prefix="gdb -ex run - -args" output="screen" -->
+  <node name="sick_line_guidance_can_chain_node" pkg="sick_line_guidance" type="sick_line_guidance_can_chain_node" >
+    <rosparam command="load" file="$(arg yaml)" />                 <!-- Private CAN configuration for canopen_chain_node by ols or mls yaml-file -->
+    <param name="diagnostic_topic" type="str" value="diagnostics" />                          <!-- ROS topic for diagnostic messages -->
+  </node>
+
+  <!-- sick_line_guidance: run sick_line_guidance_node, which implements the ROS driver for OLS and MLS -->
+  <node name="sick_line_guidance_node" pkg="sick_line_guidance" type="sick_line_guidance_node" >
+    <param name="diagnostic_topic" type="str" value="diagnostics" />                           <!-- ROS topic for diagnostic messages -->
+    <param name="can_connect_init_at_startup" type="bool" value="true" />                      <!-- Additional CAN configuration: if true, canopen services are initialized at startup -->
+    <param name="initial_sensor_state" type="int" value="7" />                                 <!-- initial sensor states (f.e. 0x07 for 3 detected lines, or 8 to indicate sensor error) -->
+    <param name="subscribe_queue_size" type="int" value="16" />                                <!-- buffer size for ros messages -->
+    <param name="max_sdo_rate" type="double" value="100.0" />                                  <!-- max. sdo query and publish rate -->
+    <param name="max_num_retries_after_sdo_error" type="int" value="2" />                      <!-- After SDO error, the SDO query is repeated max. N times (default: N=2). If the SDO error persists, the can driver is shutdown and restarted. -->
+  </node>
+
+  <!-- sick_line_guidance: cloud_publisher_node to convert OLS_Measurement and MLS_Measurement messages to PointCloud2 -->
+  <node name="sick_line_guidance_cloud_publisher" pkg="sick_line_guidance" type="sick_line_guidance_cloud_publisher">
+    <param name="mls_topic_publish" type="str" value="mls" />                                  <!-- MLS_Measurement data are published in topic "/mls" -->
+    <param name="ols_topic_publish" type="str" value="ols" />                                  <!-- OLS_Measurement data are published in topic "/ols" -->
+    <param name="cloud_topic_publish" type="str" value="cloud" />                              <!-- sensor_msgs::PointCloud messages are published in topic "/cloud" -->
+    <param name="mls_cloud_frame_id" type="str" value="mls_frame" />                           <!-- MLS PointCloud data are published with frame id "mls_frame" -->
+    <param name="ols_cloud_frame_id" type="str" value="ols_frame" />                           <!-- OLS PointCloud data are published with frame id "ols_frame" -->
+    <param name="subscribe_queue_size" type="int" value="1" />                                 <!-- buffer size for ros messages -->
+  </node>
+
+</launch>

Devices/Packages/sick_line_guidance/launch/sick_line_guidance_can2ros_node.launch

@@ -0,0 +1,8 @@
+<launch>
+
+  <!-- sick_line_guidance: run can2ros_node support tool, converts can messages to ros messages -->
+  <node name="sick_line_guidance_can2ros_node" pkg="sick_line_guidance" type="sick_line_guidance_can2ros_node" />
+  <param name="can_device" type="str" value="can0" />    <!-- name of can net device (socketcan interface) -->
+  <param name="ros_topic" type="str" value="can0" />     <!-- topic for ros messages (output), message type can_msgs::Frame -->
+
+</launch>

Devices/Packages/sick_line_guidance/launch/sick_line_guidance_ols20_twin.launch

@@ -0,0 +1,45 @@
+<launch>
+
+  <!-- sick_line_guidance: configuration for two OLS20 devices (can node ids A and B) -->
+  <arg name="yaml" default="sick_line_guidance_ols20_twin.yaml"/>
+  <rosparam command="load" file="$(find sick_line_guidance)/$(arg yaml)" />                    <!-- Global CAN configuration two OLS20 devices (can node ids A and B) incl. link to eds-file -->
+  
+  <!-- sick_line_guidance: run canopen_chain_node -->
+  <!-- to run sick_line_guidance_can_chain_node in gdb: add parameter launch-prefix="gdb -ex run - -args": -->
+  <!-- node name="sick_line_guidance_can_chain_node" pkg="sick_line_guidance" type="sick_line_guidance_can_chain_node" launch-prefix="gdb -ex run - -args" output="screen" -->
+  <node name="sick_line_guidance_can_chain_node" pkg="sick_line_guidance" type="sick_line_guidance_can_chain_node" output="screen" >
+    <rosparam command="load" file="$(find sick_line_guidance)/$(arg yaml)" />                 <!-- Private CAN configuration for canopen_chain_node by ols or mls yaml-file -->
+    <param name="diagnostic_topic" type="str" value="diagnostics" />                          <!-- ROS topic for diagnostic messages -->
+  </node>
+
+  <!-- sick_line_guidance: run sick_line_guidance_node, which implements the ROS driver for OLS and MLS -->
+  <node name="sick_line_guidance_node" pkg="sick_line_guidance" type="sick_line_guidance_node" output="screen" >
+    <param name="diagnostic_topic" type="str" value="diagnostics" />                           <!-- ROS topic for diagnostic messages -->
+    <param name="can_connect_init_at_startup" type="bool" value="true" />                      <!-- Additional CAN configuration: if true, canopen services are initialized at startup -->
+    <param name="initial_sensor_state" type="int" value="7" />                                 <!-- initial sensor states (f.e. 0x07 for 3 detected lines, or 8 to indicate sensor error) -->
+    <param name="subscribe_queue_size" type="int" value="16" />                                <!-- buffer size for ros messages -->
+    <param name="max_sdo_rate" type="double" value="100.0" />                                  <!-- max. sdo query and publish rate -->
+    <param name="max_num_retries_after_sdo_error" type="int" value="2" />                      <!-- After SDO error, the SDO query is repeated max. N times (default: N=2). If the SDO error persists, the can driver is shutdown and restarted. -->
+  </node>
+
+  <!-- sick_line_guidance: cloud_publisher_node to convert OLS_Measurement messages of first OLS20 device (can node id A) to PointCloud2 (topic "cloudA", frame id "olsA_frame") -->
+  <node name="sick_line_guidance_cloudA_publisher" pkg="sick_line_guidance" type="sick_line_guidance_cloud_publisher" output="screen" >
+    <param name="mls_topic_publish" type="str" value="mlsA" />                                 <!-- MLS_Measurement data are published in topic "/mlsA" -->
+    <param name="ols_topic_publish" type="str" value="olsA" />                                 <!-- OLS_Measurement data are published in topic "/olsA" -->
+    <param name="cloud_topic_publish" type="str" value="cloudA" />                             <!-- sensor_msgs::PointCloud messages are published in topic "/cloudA" -->
+    <param name="mls_cloud_frame_id" type="str" value="mlsA_frame" />                          <!-- MLS PointCloud data are published with frame id "mlsA_frame" -->
+    <param name="ols_cloud_frame_id" type="str" value="olsA_frame" />                          <!-- OLS PointCloud data are published with frame id "olsA_frame" -->
+    <param name="subscribe_queue_size" type="int" value="1" />                                 <!-- buffer size for ros messages -->
+  </node>
+
+  <!-- sick_line_guidance: cloud_publisher_node to convert OLS_Measurement messages of second OLS20 device (can node id B) to PointCloud2 (topic "cloudB", frame id "olsB_frame") -->
+  <node name="sick_line_guidance_cloudB_publisher" pkg="sick_line_guidance" type="sick_line_guidance_cloud_publisher" output="screen" >
+    <param name="mls_topic_publish" type="str" value="mlsB" />                                 <!-- MLS_Measurement data are published in topic "/mlsB" -->
+    <param name="ols_topic_publish" type="str" value="olsB" />                                 <!-- OLS_Measurement data are published in topic "/olsB" -->
+    <param name="cloud_topic_publish" type="str" value="cloudB" />                             <!-- sensor_msgs::PointCloud messages are published in topic "/cloudB" -->
+    <param name="mls_cloud_frame_id" type="str" value="mlsB_frame" />                          <!-- MLS PointCloud data are published with frame id "mlsB_frame" -->
+    <param name="ols_cloud_frame_id" type="str" value="olsB_frame" />                          <!-- OLS PointCloud data are published with frame id "olsB_frame" -->
+    <param name="subscribe_queue_size" type="int" value="1" />                                 <!-- buffer size for ros messages -->
+  </node>
+
+</launch>

Devices/Packages/sick_line_guidance/launch/sick_line_guidance_ros2can_node.launch

@@ -0,0 +1,9 @@
+<launch>
+
+  <!-- sick_line_guidance: run ros2can_node support tool, converts ros messages to can messages -->
+  <node name="sick_line_guidance_ros2can_node" pkg="sick_line_guidance" type="sick_line_guidance_ros2can_node" />
+  <param name="can_device" type="str" value="can0" />       <!-- name of can net device (socketcan interface) -->
+  <param name="ros_topic" type="str" value="ros2can0" />    <!--  topic for ros messages (input), message type can_msgs::Frame -->
+  <param name="subscribe_queue_size" type="int" value="32" />    <!-- buffer size for ros messages -->
+
+</launch>

Devices/Packages/sick_line_guidance/mls/SICK-MLS-MLS.eds

@@ -0,0 +1,483 @@
+[FileInfo]
+CreatedBy=Tim Vogt
+ModifiedBy=Tim Vogt
+Description=Magnetic Lane Sensor
+CreationTime=11:15AM
+CreationDate=10-10-2017
+ModificationTime=11:15AM
+ModificationDate=10-10-2017
+FileName=MLS.eds
+FileVersion=0x01
+FileRevision=0x01
+EDSVersion=4.0
+
+[DeviceInfo]
+VendorName=SICK AG
+VendorNumber=0x01000056
+ProductName=Magnetic Lane Sensor
+BaudRate_10=0
+BaudRate_20=0
+BaudRate_50=0
+BaudRate_125=1
+BaudRate_250=0
+BaudRate_500=0
+BaudRate_800=0
+BaudRate_1000=0
+SimpleBootUpMaster=0
+SimpleBootUpSlave=1
+Granularity=0
+DynamicChannelsSupported=0
+CompactPDO=0
+GroupMessaging=0
+NrOfRXPDO=0
+NrOfTXPDO=1
+LSS_Supported=1
+
+[DummyUsage]
+Dummy0001=0
+Dummy0002=1
+Dummy0003=1
+Dummy0004=1
+Dummy0005=1
+Dummy0006=1
+Dummy0007=1
+
+[Comments]
+Lines=0
+
+[MandatoryObjects]
+SupportedObjects=3
+1=0x1000
+2=0x1001
+3=0x1018
+
+[1000]
+ParameterName=Device Type
+ObjectType=0x7
+DataType=0x0007
+AccessType=ro
+DefaultValue=0x00
+PDOMapping=0
+
+[1001]
+ParameterName=Error Register
+ObjectType=0x7
+DataType=0x0005
+AccessType=ro
+PDOMapping=0
+
+[1018]
+ParameterName=Identity Object
+ObjectType=0x9
+SubNumber=5
+
+[1018sub0]
+ParameterName=Number of entries
+ObjectType=0x7
+DataType=0x0005
+AccessType=ro
+DefaultValue=4
+PDOMapping=0
+
+[1018sub1]
+ParameterName=Vendor Id
+ObjectType=0x7
+DataType=0x0007
+AccessType=ro
+DefaultValue=0x01000056
+PDOMapping=0
+
+[1018sub2]
+ParameterName=Product Code
+ObjectType=0x7
+DataType=0x0007
+AccessType=ro
+DefaultValue=0x00001100
+PDOMapping=0
+
+[1018sub3]
+ParameterName=Revision number
+ObjectType=0x7
+DataType=0x0007
+AccessType=ro
+DefaultValue=0x00000002
+PDOMapping=0
+
+[1018sub4]
+ParameterName=Serial number
+ObjectType=0x7
+DataType=0x0007
+AccessType=ro
+PDOMapping=0
+
+[OptionalObjects]
+SupportedObjects=10
+1=0x1008
+2=0x1009
+3=0x100A
+4=0x100C
+5=0x100D
+6=0x1017
+7=0x1200
+8=0x1800
+9=0x1A00
+10=0x1F80
+
+[1008]
+ParameterName=Manufacturer Device Name
+ObjectType=0x7
+DataType=0x0009
+AccessType=const
+PDOMapping=0
+
+[1009]
+ParameterName=Manufacturer Hardware Version
+ObjectType=0x7
+DataType=0x0009
+AccessType=const
+PDOMapping=0
+
+[100A]
+ParameterName=Manufacturer Software Version
+ObjectType=0x7
+DataType=0x0009
+AccessType=const
+PDOMapping=0
+
+[100C]
+ParameterName=Guard Time
+ObjectType=0x7
+DataType=0x0006
+AccessType=rw
+DefaultValue=0
+PDOMapping=0
+
+[100D]
+ParameterName=Life Time Factor
+ObjectType=0x7
+DataType=0x0005
+AccessType=rw
+DefaultValue=0x00
+PDOMapping=0
+
+[1017]
+ParameterName=Producer Heartbeat Time
+ObjectType=0x7
+DataType=0x0006
+AccessType=rw
+DefaultValue=0
+PDOMapping=0
+
+[1200]
+ParameterName=Server SDO Parameter 0
+ObjectType=0x9
+SubNumber=3
+
+[1200sub0]
+ParameterName=Number of entries
+ObjectType=0x7
+DataType=0x0005
+AccessType=ro
+DefaultValue=2
+PDOMapping=0
+LowLimit=0x02
+HighLimit=0x02
+
+[1200sub1]
+ParameterName=COB ID Client to Server
+ObjectType=0x7
+DataType=0x0007
+AccessType=ro
+DefaultValue=$NODEID+0x600
+PDOMapping=0
+
+[1200sub2]
+ParameterName=COB ID Server to Client
+ObjectType=0x7
+DataType=0x0007
+AccessType=ro
+DefaultValue=$NODEID+0x580
+PDOMapping=0
+
+[1800]
+ParameterName=Transmit PDO Communication Parameter 0
+ObjectType=0x9
+SubNumber=5
+
+[1800sub0]
+ParameterName=Number of entries
+ObjectType=0x7
+DataType=0x0005
+AccessType=ro
+DefaultValue=5
+PDOMapping=0
+
+[1800sub1]
+ParameterName=COB ID
+ObjectType=0x7
+DataType=0x0007
+AccessType=rw
+DefaultValue=$NODEID+0x180
+PDOMapping=0
+LowLimit=0x00000181
+HighLimit=0xFFFFFFFF
+
+[1800sub2]
+ParameterName=Transmission Type
+ObjectType=0x7
+DataType=0x0005
+AccessType=ro
+DefaultValue=0xFF
+PDOMapping=0
+
+[1800sub4]
+ParameterName=Compatibility Entry
+ObjectType=0x7
+DataType=0x0005
+AccessType=rw
+PDOMapping=0
+
+[1800sub5]
+ParameterName=Event Timer
+ObjectType=0x7
+DataType=0x0006
+AccessType=rw
+PDOMapping=0
+
+[1A00]
+ParameterName=Transmit PDO Mapping Parameter 0
+ObjectType=0x9
+SubNumber=6
+
+[1A00sub0]
+ParameterName=Number of entries
+ObjectType=0x7
+DataType=0x0005
+AccessType=const
+DefaultValue=5
+PDOMapping=0
+LowLimit=0
+HighLimit=5
+
+[1A00sub1]
+ParameterName=PDO Mapping Entry
+ObjectType=0x7
+DataType=0x0007
+AccessType=const
+DefaultValue=0x20210110
+PDOMapping=0
+
+[1A00sub2]
+ParameterName=PDO Mapping Entry_2
+ObjectType=0x7
+DataType=0x0007
+AccessType=const
+DefaultValue=0x20210210
+PDOMapping=0
+
+[1A00sub3]
+ParameterName=PDO Mapping Entry_3
+ObjectType=0x7
+DataType=0x0007
+AccessType=const
+DefaultValue=0x20210310
+PDOMapping=0
+
+[1A00sub4]
+ParameterName=PDO Mapping Entry_4
+ObjectType=0x7
+DataType=0x0007
+AccessType=const
+DefaultValue=0x20210408
+PDOMapping=0
+
+[1A00sub5]
+ParameterName=PDO Mapping Entry_5
+ObjectType=0x7
+DataType=0x0007
+AccessType=const
+DefaultValue=0x20220008
+PDOMapping=0
+
+[1F80]
+ParameterName=NMTStartup
+ObjectType=0x7
+DataType=0x0007
+AccessType=rw
+PDOMapping=0
+
+[ManufacturerObjects]
+SupportedObjects=14
+1=0x2019
+2=0x2020
+3=0x2021
+4=0x2022
+5=0x2023
+6=0x2024
+7=0x2025
+8=0x2026
+9=0x2027
+10=0x2028
+11=0x2029
+12=0x202A
+13=0x202B
+14=0x202C
+
+[2019]
+ParameterName=Order Number
+ObjectType=0x7
+DataType=0x0007
+AccessType=ro
+PDOMapping=0
+
+[2020]
+ParameterName=Passwort
+ObjectType=0x7
+DataType=0x0009
+AccessType=wo
+PDOMapping=0
+
+[2021]
+ParameterName=SSPs
+ObjectType=0x9
+SubNumber=5
+
+[2021sub0]
+ParameterName=Number of SSPs
+ObjectType=0x7
+DataType=0x0005
+AccessType=ro
+DefaultValue=0x04
+PDOMapping=0
+
+[2021sub1]
+ParameterName=SSP1
+ObjectType=0x7
+DataType=0x0003
+AccessType=ro
+PDOMapping=1
+
+[2021sub2]
+ParameterName=SSP2
+ObjectType=0x7
+DataType=0x0003
+AccessType=ro
+PDOMapping=1
+
+[2021sub3]
+ParameterName=SSP3
+ObjectType=0x7
+DataType=0x0003
+AccessType=ro
+PDOMapping=1
+
+[2021sub4]
+ParameterName=#SSP
+ObjectType=0x7
+DataType=0x0005
+AccessType=ro
+PDOMapping=1
+
+[2022]
+ParameterName=Status
+ObjectType=0x7
+DataType=0x0005
+AccessType=ro
+PDOMapping=1
+
+[2023]
+ParameterName=Track Level
+ObjectType=0x7
+DataType=0x0006
+AccessType=ro
+PDOMapping=0
+
+[2024]
+ParameterName=Field Level
+ObjectType=0x7
+DataType=0x0006
+AccessType=ro
+PDOMapping=0
+
+[2025]
+ParameterName=Min Level
+ObjectType=0x7
+DataType=0x0006
+AccessType=rw
+PDOMapping=0
+
+[2026]
+ParameterName=Offset
+ObjectType=0x7
+DataType=0x0003
+AccessType=rw
+PDOMapping=0
+
+[2027]
+ParameterName=Sensor flipped
+ObjectType=0x7
+DataType=0x0001
+AccessType=rw
+PDOMapping=0
+
+[2028]
+ParameterName=Markers
+ObjectType=0x9
+SubNumber=4
+
+[2028sub0]
+ParameterName=NrOfObjects
+ObjectType=0x7
+DataType=0x0005
+AccessType=ro
+DefaultValue=3
+PDOMapping=0
+
+[2028sub1]
+ParameterName=Use Markers
+ObjectType=0x7
+DataType=0x0001
+AccessType=rw
+PDOMapping=0
+
+[2028sub2]
+ParameterName=Marker Style
+ObjectType=0x7
+DataType=0x0005
+AccessType=rw
+PDOMapping=0
+
+[2028sub3]
+ParameterName=Fail Safe Mode
+ObjectType=0x7
+DataType=0x0001
+AccessType=rw
+PDOMapping=0
+
+[2029]
+ParameterName=Lock Teach
+ObjectType=0x7
+DataType=0x0001
+AccessType=rw
+PDOMapping=0
+
+[202A]
+ParameterName=Set Param to Default
+ObjectType=0x7
+DataType=0x0001
+AccessType=wo
+PDOMapping=0
+
+[202B]
+ParameterName=Trigger User Offset Calibration
+ObjectType=0x7
+DataType=0x0001
+AccessType=wo
+PDOMapping=0
+
+[202C]
+ParameterName=Trigger Zero Position Teach
+ObjectType=0x7
+DataType=0x0001
+AccessType=wo
+PDOMapping=0

Devices/Packages/sick_line_guidance/mls/SICK-MLS.eds

@@ -0,0 +1,483 @@
+[FileInfo]
+CreatedBy=Tim Vogt
+ModifiedBy=Tim Vogt
+Description=Magnetic Lane Sensor
+CreationTime=11:15AM
+CreationDate=10-10-2017
+ModificationTime=11:15AM
+ModificationDate=10-10-2017
+FileName=MLS.eds
+FileVersion=0x01
+FileRevision=0x01
+EDSVersion=4.0
+
+[DeviceInfo]
+VendorName=SICK AG
+VendorNumber=0x01000056
+ProductName=Magnetic Lane Sensor
+BaudRate_10=0
+BaudRate_20=0
+BaudRate_50=0
+BaudRate_125=1
+BaudRate_250=0
+BaudRate_500=0
+BaudRate_800=0
+BaudRate_1000=0
+SimpleBootUpMaster=0
+SimpleBootUpSlave=1
+Granularity=0
+DynamicChannelsSupported=0
+CompactPDO=0
+GroupMessaging=0
+NrOfRXPDO=0
+NrOfTXPDO=1
+LSS_Supported=1
+
+[DummyUsage]
+Dummy0001=0
+Dummy0002=1
+Dummy0003=1
+Dummy0004=1
+Dummy0005=1
+Dummy0006=1
+Dummy0007=1
+
+[Comments]
+Lines=0
+
+[MandatoryObjects]
+SupportedObjects=3
+1=0x1000
+2=0x1001
+3=0x1018
+
+[1000]
+ParameterName=Device Type
+ObjectType=0x7
+DataType=0x0007
+AccessType=ro
+DefaultValue=0x00
+PDOMapping=0
+
+[1001]
+ParameterName=Error Register
+ObjectType=0x7
+DataType=0x0005
+AccessType=ro
+PDOMapping=0
+
+[1018]
+ParameterName=Identity Object
+ObjectType=0x9
+SubNumber=5
+
+[1018sub0]
+ParameterName=Number of entries
+ObjectType=0x7
+DataType=0x0005
+AccessType=ro
+DefaultValue=4
+PDOMapping=0
+
+[1018sub1]
+ParameterName=Vendor Id
+ObjectType=0x7
+DataType=0x0007
+AccessType=ro
+DefaultValue=0x01000056
+PDOMapping=0
+
+[1018sub2]
+ParameterName=Product Code
+ObjectType=0x7
+DataType=0x0007
+AccessType=ro
+DefaultValue=0x00001100
+PDOMapping=0
+
+[1018sub3]
+ParameterName=Revision number
+ObjectType=0x7
+DataType=0x0007
+AccessType=ro
+DefaultValue=0x00000002
+PDOMapping=0
+
+[1018sub4]
+ParameterName=Serial number
+ObjectType=0x7
+DataType=0x0007
+AccessType=ro
+PDOMapping=0
+
+[OptionalObjects]
+SupportedObjects=10
+1=0x1008
+2=0x1009
+3=0x100A
+4=0x100C
+5=0x100D
+6=0x1017
+7=0x1200
+8=0x1800
+9=0x1A00
+10=0x1F80
+
+[1008]
+ParameterName=Manufacturer Device Name
+ObjectType=0x7
+DataType=0x0009
+AccessType=const
+PDOMapping=0
+
+[1009]
+ParameterName=Manufacturer Hardware Version
+ObjectType=0x7
+DataType=0x0009
+AccessType=const
+PDOMapping=0
+
+[100A]
+ParameterName=Manufacturer Software Version
+ObjectType=0x7
+DataType=0x0009
+AccessType=const
+PDOMapping=0
+
+[100C]
+ParameterName=Guard Time
+ObjectType=0x7
+DataType=0x0006
+AccessType=rw
+DefaultValue=0
+PDOMapping=0
+
+[100D]
+ParameterName=Life Time Factor
+ObjectType=0x7
+DataType=0x0005
+AccessType=rw
+DefaultValue=0x00
+PDOMapping=0
+
+[1017]
+ParameterName=Producer Heartbeat Time
+ObjectType=0x7
+DataType=0x0006
+AccessType=rw
+DefaultValue=0
+PDOMapping=0
+
+[1200]
+ParameterName=Server SDO Parameter 0
+ObjectType=0x9
+SubNumber=3
+
+[1200sub0]
+ParameterName=Number of entries
+ObjectType=0x7
+DataType=0x0005
+AccessType=ro
+DefaultValue=2
+PDOMapping=0
+LowLimit=0x02
+HighLimit=0x02
+
+[1200sub1]
+ParameterName=COB ID Client to Server
+ObjectType=0x7
+DataType=0x0007
+AccessType=ro
+DefaultValue=$NODEID+0x600
+PDOMapping=0
+
+[1200sub2]
+ParameterName=COB ID Server to Client
+ObjectType=0x7
+DataType=0x0007
+AccessType=ro
+DefaultValue=$NODEID+0x580
+PDOMapping=0
+
+[1800]
+ParameterName=Transmit PDO Communication Parameter 0
+ObjectType=0x9
+SubNumber=5
+
+[1800sub0]
+ParameterName=Number of entries
+ObjectType=0x7
+DataType=0x0005
+AccessType=ro
+DefaultValue=5
+PDOMapping=0
+
+[1800sub1]
+ParameterName=COB ID
+ObjectType=0x7
+DataType=0x0007
+AccessType=rw
+DefaultValue=$NODEID+0x180
+PDOMapping=0
+LowLimit=0x00000181
+HighLimit=0xFFFFFFFF
+
+[1800sub2]
+ParameterName=Transmission Type
+ObjectType=0x7
+DataType=0x0005
+AccessType=ro
+DefaultValue=0xFF
+PDOMapping=0
+
+[1800sub4]
+ParameterName=Compatibility Entry
+ObjectType=0x7
+DataType=0x0005
+AccessType=rw
+PDOMapping=0
+
+[1800sub5]
+ParameterName=Event Timer
+ObjectType=0x7
+DataType=0x0006
+AccessType=rw
+PDOMapping=0
+
+[1A00]
+ParameterName=Transmit PDO Mapping Parameter 0
+ObjectType=0x9
+SubNumber=6
+
+[1A00sub0]
+ParameterName=Number of entries
+ObjectType=0x7
+DataType=0x0005
+AccessType=const
+DefaultValue=5
+PDOMapping=0
+LowLimit=0
+HighLimit=5
+
+[1A00sub1]
+ParameterName=PDO Mapping Entry
+ObjectType=0x7
+DataType=0x0007
+AccessType=const
+DefaultValue=0x20210110
+PDOMapping=0
+
+[1A00sub2]
+ParameterName=PDO Mapping Entry_2
+ObjectType=0x7
+DataType=0x0007
+AccessType=const
+DefaultValue=0x20210210
+PDOMapping=0
+
+[1A00sub3]
+ParameterName=PDO Mapping Entry_3
+ObjectType=0x7
+DataType=0x0007
+AccessType=const
+DefaultValue=0x20210310
+PDOMapping=0
+
+[1A00sub4]
+ParameterName=PDO Mapping Entry_4
+ObjectType=0x7
+DataType=0x0007
+AccessType=const
+DefaultValue=0x20210408
+PDOMapping=0
+
+[1A00sub5]
+ParameterName=PDO Mapping Entry_5
+ObjectType=0x7
+DataType=0x0007
+AccessType=const
+DefaultValue=0x20220008
+PDOMapping=0
+
+[1F80]
+ParameterName=NMTStartup
+ObjectType=0x7
+DataType=0x0007
+AccessType=rw
+PDOMapping=0
+
+[ManufacturerObjects]
+SupportedObjects=14
+1=0x2019
+2=0x2020
+3=0x2021
+4=0x2022
+5=0x2023
+6=0x2024
+7=0x2025
+8=0x2026
+9=0x2027
+10=0x2028
+11=0x2029
+12=0x202A
+13=0x202B
+14=0x202C
+
+[2019]
+ParameterName=Order Number
+ObjectType=0x7
+DataType=0x0007
+AccessType=ro
+PDOMapping=0
+
+[2020]
+ParameterName=Passwort
+ObjectType=0x7
+DataType=0x0009
+AccessType=wo
+PDOMapping=0
+
+[2021]
+ParameterName=SSPs
+ObjectType=0x9
+SubNumber=5
+
+[2021sub0]
+ParameterName=Number of SSPs
+ObjectType=0x7
+DataType=0x0005
+AccessType=ro
+DefaultValue=0x04
+PDOMapping=0
+
+[2021sub1]
+ParameterName=SSP1
+ObjectType=0x7
+DataType=0x0003
+AccessType=ro
+PDOMapping=1
+
+[2021sub2]
+ParameterName=SSP2
+ObjectType=0x7
+DataType=0x0003
+AccessType=ro
+PDOMapping=1
+
+[2021sub3]
+ParameterName=SSP3
+ObjectType=0x7
+DataType=0x0003
+AccessType=ro
+PDOMapping=1
+
+[2021sub4]
+ParameterName=#SSP
+ObjectType=0x7
+DataType=0x0005
+AccessType=ro
+PDOMapping=1
+
+[2022]
+ParameterName=Status
+ObjectType=0x7
+DataType=0x0005
+AccessType=ro
+PDOMapping=1
+
+[2023]
+ParameterName=Track Level
+ObjectType=0x7
+DataType=0x0006
+AccessType=ro
+PDOMapping=0
+
+[2024]
+ParameterName=Field Level
+ObjectType=0x7
+DataType=0x0006
+AccessType=ro
+PDOMapping=0
+
+[2025]
+ParameterName=Min Level
+ObjectType=0x7
+DataType=0x0006
+AccessType=rw
+PDOMapping=0
+
+[2026]
+ParameterName=Offset
+ObjectType=0x7
+DataType=0x0003
+AccessType=rw
+PDOMapping=0
+
+[2027]
+ParameterName=Sensor flipped
+ObjectType=0x7
+DataType=0x0005
+AccessType=rw
+PDOMapping=0
+
+[2028]
+ParameterName=Markers
+ObjectType=0x9
+SubNumber=4
+
+[2028sub0]
+ParameterName=NrOfObjects
+ObjectType=0x7
+DataType=0x0005
+AccessType=ro
+DefaultValue=3
+PDOMapping=0
+
+[2028sub1]
+ParameterName=Use Markers
+ObjectType=0x7
+DataType=0x0005
+AccessType=rw
+PDOMapping=0
+
+[2028sub2]
+ParameterName=Marker Style
+ObjectType=0x7
+DataType=0x0005
+AccessType=rw
+PDOMapping=0
+
+[2028sub3]
+ParameterName=Fail Safe Mode
+ObjectType=0x7
+DataType=0x0005
+AccessType=rw
+PDOMapping=0
+
+[2029]
+ParameterName=Lock Teach
+ObjectType=0x7
+DataType=0x0005
+AccessType=rw
+PDOMapping=0
+
+[202A]
+ParameterName=Set Param to Default
+ObjectType=0x7
+DataType=0x0005
+AccessType=wo
+PDOMapping=0
+
+[202B]
+ParameterName=Trigger User Offset Calibration
+ObjectType=0x7
+DataType=0x0005
+AccessType=wo
+PDOMapping=0
+
+[202C]
+ParameterName=Trigger Zero Position Teach
+ObjectType=0x7
+DataType=0x0005
+AccessType=wo
+PDOMapping=0

Devices/Packages/sick_line_guidance/mls/sick_line_guidance_mls.yaml

@@ -0,0 +1,35 @@
+bus:
+  device: can0
+  driver_plugin: can::SocketCANInterface
+  master_allocator: canopen::SimpleMaster::Allocator
+sync:
+  interval_ms: 10
+  overflow: 0
+#
+# heartbeat: # simple heartbeat producer (optional, not supported by OLS or MLS, do not enable heartbeats)
+#   rate: 100 # heartbeat rate (1/rate in seconds)
+#   msg: "77f#05" # message to send, cansend format: heartbeat of node 127 with status 5=Started
+nodes:
+  node1:
+    id: 0x01 # CAN-Node-ID of can device, default: Node-ID 10=0x0A for OLS and MLS
+    eds_pkg: sick_line_guidance # package name for relative path
+    eds_file: SICK-MLS.eds      # path to EDS/DCF file
+    publish: ["1001","1018sub1","1018sub4","2021sub1!","2021sub2!","2021sub3!","2021sub4!","2022!"]
+    # MLS: 1001 = error register, 1018sub1 = VendorID, 1018sub4 = SerialNumber, TPDO1 = 0x2021sub1 to 0x2021sub4 and 0x2022
+    
+    # sick_line_guidance configuration of this node:
+    sick_device_family: "MLS"              # can devices of OLS10, OLS20 or MLS family currently supported
+    sick_topic: "mls"                      # MLS_Measurement messages are published in topic "/mls"
+    sick_frame_id: "mls_measurement_frame" # MLS_Measurement messages are published frame_id "mls_measurement_frame"
+
+    # device configuration of writable parameter by dcf_overlay: "objectindex": "parametervalue"
+    # example: "2027": "0x01" # Object 2027 (sensorFlipped, defaultvalue 0x00) will be configured with value 0x01
+    # dcf_overlay:
+    #   "2028sub1": "0x01" # UseMarkers (0 = disable marker detection, 1 = enable marker detection), UINT8, DataType=0x0005, defaultvalue=0
+    #   "2028sub2": "0x02" # MarkerStyle (0 = disable marker detection, 1 = standard mode, 2 = extended mode), UINT8, DataType=0x0005, defaultvalue=0
+    #   "2028sub3": "0x01" # FailSafeMode (0 = disabled, 1 = enabled), UINT8, DataType=0x0005, defaultvalue=0
+    #   "2025":     "1000" # Min.Level, UINT16, DataType=0x0006, defaultvalue=600
+    #   "2026":        "1" # Offset [mm] Nullpunkt, INT16, DataType=0x0003, defaultvalue=0
+    #   "2027":     "0x01" # sensorFlipped, UINT8, DataType=0x0005, defaultvalue=0
+    #   "2029":     "0x01" # LockTeach, UINT8, DataType=0x0005, defaultvalue=0
+    #

Devices/Packages/sick_line_guidance/msg/MLS_Measurement.msg

@@ -0,0 +1,21 @@
+# MLS_Measurement.msg defines a single measurement of a SICK MLS.
+# See operation instructions for details (www.sick.com/mls).
+#
+
+# Header with sequence id, timestamp of the measurement and frame id 
+Header header
+
+# Array of measured positions for up to 3 lines.
+# Each position is the distance to the center of a line (line center point) in meter.
+# More than one line is detected in case of junctions.
+float32[] position      # distance to the line center point [m]
+
+# LCP-flags (signs and line assignment)
+uint8 lcp               # flags (signs and assignment, see chap. 8 of operation instructions)
+
+# Detection status
+uint8 status            # status (see chap. 8 of operation instructions)
+
+# Error register
+uint8 error             # error register (0x1001, value 0 = okay, see chap. 8 of operation instructions)
+

Devices/Packages/sick_line_guidance/msg/OLS_Measurement.msg

@@ -0,0 +1,39 @@
+# OLS_Measurement.msg defines a single measurement of a SICK OLS.
+# See operation instructions for details (www.sick.com/ols).
+
+# Header with sequence id, timestamp of the measurement and frame id 
+Header header
+
+# Array of measured positions for up to 3 lines.
+# Each position is the distance to the center of a line (line center point) in meter.
+# More than one line is detected in case of junctions.
+float32[] position           # distance to the line center point [m]
+
+# Array of up to 3 line widths.
+# For each detected line, its width is measured.
+float32[] width              # width of line [m]
+
+# Detection status
+uint8 status                 # status (see chap. 8 of operation instructions)
+
+# Barcode data
+uint8 barcode                # barcode data (0 indicates no barcode)
+
+# Device status
+uint16 dev_status            # device status (0x2018, value 0 = okay, see chap. 8 of operation instructions)
+
+# Extended code
+uint32 extended_code         # extended code (0x2021sub9, see chap. 8 of operation instructions)
+
+# Error register
+uint8 error                  # error register (0x1001, value 0 = okay, see chap. 8 of operation instructions)
+
+# barcode center point [m] (OLS20 only)
+float32 barcode_center_point # OLS20 only (0x2021subA), OLS10: always 0
+
+# quality of lines
+uint8 quality_of_lines       # OLS20 only (0x2021subB), OLS10: always 0
+
+# Intensity of lines 1 to 3
+uint8[] intensity_of_lines   # OLS20 only (0x2023sub1 to 0x2023sub3), OLS10: always 0
+

Devices/Packages/sick_line_guidance/ols/SICK_OLS10_CO.eds

@@ -0,0 +1,602 @@
+[FileInfo]
+CreatedBy=Tim Vogt
+ModifiedBy=Alexander Bierbaum
+Description=Optical Line Guidance Sensor
+CreationTime=09:47AM
+CreationDate=12-15-2017
+ModificationTime=02:25PM
+ModificationDate=03-13-2019
+FileName=SICK_OLS10_CO.eds
+FileVersion=0x02
+FileRevision=0x0A
+EDSVersion=4.0
+
+[DeviceInfo]
+VendorName=SICK AG
+VendorNumber=0x01000056
+ProductName=Optical Line Guidance Sensor
+ProductNumber=0x00001003
+RevisionNumber=0x00010001
+OrderCode=-
+BaudRate_10=1
+BaudRate_20=1
+BaudRate_50=1
+BaudRate_125=1
+BaudRate_250=1
+BaudRate_500=1
+BaudRate_800=1
+BaudRate_1000=1
+SimpleBootUpMaster=0
+SimpleBootUpSlave=1
+Granularity=0
+DynamicChannelsSupported=0
+CompactPDO=0
+GroupMessaging=0
+NrOfRXPDO=0
+NrOfTXPDO=2
+LSS_Supported=1
+
+[DummyUsage]
+Dummy0001=0
+Dummy0002=1
+Dummy0003=1
+Dummy0004=1
+Dummy0005=1
+Dummy0006=1
+Dummy0007=1
+
+[Comments]
+Lines=0
+
+[MandatoryObjects]
+SupportedObjects=3
+1=0x1000
+2=0x1001
+3=0x1018
+
+[1000]
+ParameterName=Device Type
+ObjectType=0x7
+DataType=0x0007
+AccessType=ro
+DefaultValue=0x00
+PDOMapping=0
+
+[1001]
+ParameterName=Error Register
+ObjectType=0x7
+DataType=0x0005
+AccessType=ro
+DefaultValue=0
+PDOMapping=0
+
+[1018]
+ParameterName=Identity Object
+ObjectType=0x9
+SubNumber=5
+
+[1018sub0]
+ParameterName=Number of entries
+ObjectType=0x7
+DataType=0x0005
+AccessType=ro
+DefaultValue=4
+PDOMapping=0
+LowLimit=1
+HighLimit=4
+
+[1018sub1]
+ParameterName=Vendor Id
+ObjectType=0x7
+DataType=0x0007
+AccessType=ro
+DefaultValue=0x01000056
+PDOMapping=0
+
+[1018sub2]
+ParameterName=Product Code
+ObjectType=0x7
+DataType=0x0007
+AccessType=ro
+DefaultValue=0x00001003
+PDOMapping=0
+
+[1018sub3]
+ParameterName=Revision number
+ObjectType=0x7
+DataType=0x0007
+AccessType=ro
+DefaultValue=0x00010001
+PDOMapping=0
+
+[1018sub4]
+ParameterName=Serial number
+ObjectType=0x7
+DataType=0x0007
+AccessType=ro
+PDOMapping=0
+
+[OptionalObjects]
+SupportedObjects=13
+1=0x1005
+2=0x1008
+3=0x1009
+4=0x100A
+5=0x100C
+6=0x100D
+7=0x1017
+8=0x1200
+9=0x1800
+10=0x1801
+11=0x1A00
+12=0x1A01
+13=0x1F80
+
+[1005]
+ParameterName=COB ID SYNC
+ObjectType=0x7
+DataType=0x0007
+AccessType=rw
+DefaultValue=0x00000080
+PDOMapping=0
+LowLimit=0x00000080
+
+[1008]
+ParameterName=Manufacturer Device Name
+ObjectType=0x7
+DataType=0x0009
+AccessType=const
+DefaultValue=OLS10-BP112311
+PDOMapping=0
+
+[1009]
+ParameterName=Manufacturer Hardware Version
+ObjectType=0x7
+DataType=0x0009
+AccessType=const
+DefaultValue=1.0
+PDOMapping=0
+
+[100A]
+ParameterName=Manufacturer Software Version
+ObjectType=0x7
+DataType=0x0009
+AccessType=const
+DefaultValue=Firmwareversion
+PDOMapping=0
+
+[100C]
+ParameterName=Guard Time
+ObjectType=0x7
+DataType=0x0006
+AccessType=rw
+DefaultValue=0
+PDOMapping=0
+
+[100D]
+ParameterName=Life Time Factor
+ObjectType=0x7
+DataType=0x0005
+AccessType=rw
+DefaultValue=0x00
+PDOMapping=0
+
+[1017]
+ParameterName=Producer Heartbeat Time
+ObjectType=0x7
+DataType=0x0006
+AccessType=rw
+DefaultValue=0
+PDOMapping=0
+
+[1200]
+ParameterName=Server SDO Parameter 0
+ObjectType=0x9
+SubNumber=3
+
+[1200sub0]
+ParameterName=Number of entries
+ObjectType=0x7
+DataType=0x0005
+AccessType=ro
+DefaultValue=2
+PDOMapping=0
+LowLimit=0x02
+HighLimit=0x02
+
+[1200sub1]
+ParameterName=COB ID Client to Server
+ObjectType=0x7
+DataType=0x0007
+AccessType=ro
+DefaultValue=$NODEID+0x600
+PDOMapping=0
+
+[1200sub2]
+ParameterName=COB ID Server to Client
+ObjectType=0x7
+DataType=0x0007
+AccessType=ro
+DefaultValue=$NODEID+0x580
+PDOMapping=0
+
+[1800]
+ParameterName=Transmit PDO Communication Parameter 0
+ObjectType=0x9
+SubNumber=5
+
+[1800sub0]
+ParameterName=Number of entries
+ObjectType=0x7
+DataType=0x0005
+AccessType=ro
+DefaultValue=5
+PDOMapping=0
+
+[1800sub1]
+ParameterName=COB ID
+ObjectType=0x7
+DataType=0x0007
+AccessType=rw
+DefaultValue=$NODEID+0x180
+PDOMapping=0
+
+[1800sub2]
+ParameterName=Transmission Type
+ObjectType=0x7
+DataType=0x0005
+AccessType=rw
+DefaultValue=0xFF
+PDOMapping=0
+
+[1800sub4]
+ParameterName=Compatibility Entry
+ObjectType=0x7
+DataType=0x0005
+AccessType=rw
+DefaultValue=0x00
+PDOMapping=0
+
+[1800sub5]
+ParameterName=Event Timer
+ObjectType=0x7
+DataType=0x0006
+AccessType=rw
+DefaultValue=20
+PDOMapping=0
+
+[1801]
+ParameterName=Transmit PDO Communication Parameter 1
+ObjectType=0x9
+SubNumber=5
+
+[1801sub0]
+ParameterName=Number of entries
+ObjectType=0x7
+DataType=0x0005
+AccessType=ro
+DefaultValue=5
+PDOMapping=0
+
+[1801sub1]
+ParameterName=COB ID
+ObjectType=0x7
+DataType=0x0007
+AccessType=rw
+DefaultValue=$NODEID+0x280
+PDOMapping=0
+
+[1801sub2]
+ParameterName=Transmission Type
+ObjectType=0x7
+DataType=0x0005
+AccessType=rw
+DefaultValue=0xFF
+PDOMapping=0
+
+[1801sub4]
+ParameterName=Compatibility Entry
+ObjectType=0x7
+DataType=0x0005
+AccessType=rw
+DefaultValue=0x00
+PDOMapping=0
+
+[1801sub5]
+ParameterName=Event Timer
+ObjectType=0x7
+DataType=0x0006
+AccessType=rw
+DefaultValue=0
+PDOMapping=0
+
+[1A00]
+ParameterName=Transmit PDO Mapping Parameter 0
+ObjectType=0x9
+SubNumber=6
+
+[1A00sub0]
+ParameterName=Number of entries
+ObjectType=0x7
+DataType=0x0005
+AccessType=const
+DefaultValue=5
+PDOMapping=0
+LowLimit=0
+HighLimit=5
+
+[1A00sub1]
+ParameterName=PDO Mapping Entry
+ObjectType=0x7
+DataType=0x0007
+AccessType=const
+DefaultValue=0x20210110
+PDOMapping=0
+
+[1A00sub2]
+ParameterName=PDO Mapping Entry_2
+ObjectType=0x7
+DataType=0x0007
+AccessType=const
+DefaultValue=0x20210210
+PDOMapping=0
+
+[1A00sub3]
+ParameterName=PDO Mapping Entry_3
+ObjectType=0x7
+DataType=0x0007
+AccessType=const
+DefaultValue=0x20210310
+PDOMapping=0
+
+[1A00sub4]
+ParameterName=PDO Mapping Entry_4
+ObjectType=0x7
+DataType=0x0007
+AccessType=const
+DefaultValue=0x20210408
+PDOMapping=0
+
+[1A00sub5]
+ParameterName=PDO Mapping Entry_5
+ObjectType=0x7
+DataType=0x0007
+AccessType=const
+DefaultValue=0x20210808
+PDOMapping=0
+
+[1A01]
+ParameterName=Transmit PDO Mapping Parameter 1
+ObjectType=0x9
+SubNumber=4
+
+[1A01sub0]
+ParameterName=Number of entries
+ObjectType=0x7
+DataType=0x0005
+AccessType=const
+DefaultValue=3
+PDOMapping=0
+LowLimit=0
+HighLimit=3
+
+[1A01sub1]
+ParameterName=PDO Mapping Entry
+ObjectType=0x7
+DataType=0x0007
+AccessType=const
+DefaultValue=0x20210510
+PDOMapping=0
+
+[1A01sub2]
+ParameterName=PDO Mapping Entry_2
+ObjectType=0x7
+DataType=0x0007
+AccessType=const
+DefaultValue=0x20210610
+PDOMapping=0
+
+[1A01sub3]
+ParameterName=PDO Mapping Entry_3
+ObjectType=0x7
+DataType=0x0007
+AccessType=const
+DefaultValue=0x20210710
+PDOMapping=0
+
+[1F80]
+ParameterName=NMTStartup
+ObjectType=0x7
+DataType=0x0007
+AccessType=rw
+PDOMapping=0
+
+[ManufacturerObjects]
+SupportedObjects=7
+1=0x2001
+2=0x2002
+3=0x2003
+4=0x2018
+5=0x2019
+6=0x2020
+7=0x2021
+
+[2001]
+ParameterName=Mounting parameters
+ObjectType=0x9
+SubNumber=2
+
+[2001sub0]
+ParameterName=Number of mounting parameters
+ObjectType=0x7
+DataType=0x0005
+AccessType=ro
+DefaultValue=5
+PDOMapping=0
+
+[2001sub5]
+ParameterName=Flipped upside down
+ObjectType=0x7
+DataType=0x0005
+AccessType=rw
+DefaultValue=0
+PDOMapping=0
+
+[2002]
+ParameterName=Tape parameters
+ObjectType=0x9
+SubNumber=4
+
+[2002sub0]
+ParameterName=Number of tape parameters
+ObjectType=0x7
+DataType=0x0005
+AccessType=ro
+DefaultValue=3
+PDOMapping=0
+
+[2002sub1]
+ParameterName=Typ. width [m]
+ObjectType=0x7
+DataType=0x0008
+AccessType=rw
+PDOMapping=0
+
+[2002sub2]
+ParameterName=Min. width [m]
+ObjectType=0x7
+DataType=0x0008
+AccessType=rw
+PDOMapping=0
+
+[2002sub3]
+ParameterName=Max. width [m]
+ObjectType=0x7
+DataType=0x0008
+AccessType=rw
+PDOMapping=0
+
+[2003]
+ParameterName=Advanced settings
+ObjectType=0x9
+SubNumber=3
+
+[2003sub0]
+ParameterName=Number of advanced settings
+ObjectType=0x7
+DataType=0x0005
+AccessType=ro
+DefaultValue=3
+PDOMapping=0
+
+[2003sub1]
+ParameterName=Max. number of missing track readings
+ObjectType=0x7
+DataType=0x0006
+AccessType=rw
+DefaultValue=10
+PDOMapping=0
+
+[2003sub3]
+ParameterName=Position smoothing filter coefficient (dt/tau) 
+ObjectType=0x7
+DataType=0x0008
+AccessType=rw
+PDOMapping=0
+
+[2018]
+ParameterName=Device status
+ObjectType=0x7
+DataType=0x0006
+AccessType=ro
+PDOMapping=0
+
+[2019]
+ParameterName=Order number
+ObjectType=0x7
+DataType=0x0007
+AccessType=ro
+PDOMapping=0
+
+[2020]
+ParameterName=Login
+ObjectType=0x7
+DataType=0x000a
+AccessType=wo
+PDOMapping=0
+
+[2021]
+ParameterName=Result data (LCPs)
+ObjectType=0x9
+SubNumber=10
+
+[2021sub0]
+ParameterName=Number of result data
+ObjectType=0x7
+DataType=0x0005
+AccessType=ro
+DefaultValue=9
+PDOMapping=0
+
+[2021sub1]
+ParameterName=LCP1
+ObjectType=0x7
+DataType=0x0003
+AccessType=ro
+PDOMapping=1
+
+[2021sub2]
+ParameterName=LCP2
+ObjectType=0x7
+DataType=0x0003
+AccessType=ro
+PDOMapping=1
+
+[2021sub3]
+ParameterName=LCP3
+ObjectType=0x7
+DataType=0x0003
+AccessType=ro
+PDOMapping=1
+
+[2021sub4]
+ParameterName=Status
+ObjectType=0x7
+DataType=0x0005
+AccessType=ro
+PDOMapping=1
+
+[2021sub5]
+ParameterName=Width line1
+ObjectType=0x7
+DataType=0x0003
+AccessType=ro
+PDOMapping=1
+
+[2021sub6]
+ParameterName=Width line2
+ObjectType=0x7
+DataType=0x0003
+AccessType=ro
+PDOMapping=1
+
+[2021sub7]
+ParameterName=Width line3
+ObjectType=0x7
+DataType=0x0003
+AccessType=ro
+PDOMapping=1
+
+[2021sub8]
+ParameterName=Code
+ObjectType=0x7
+DataType=0x0005
+AccessType=ro
+PDOMapping=1
+
+[2021sub9]
+ParameterName=Extended Code
+ObjectType=0x7
+DataType=0x0007
+AccessType=ro
+PDOMapping=0

Devices/Packages/sick_line_guidance/ols/SICK_OLS20.eds

@@ -0,0 +1,797 @@
+
+; This EDS file was created by the CANopen Design Tool 2.3.19.0.
+; port GmbH Halle/Saale Germany, http://www.port.de, mailto:service@port.de
+; DefaultValues added by Michael Lehning.
+
+[FileInfo]
+FileName=OLS20.eds
+FileVersion=1
+FileRevision=1
+EDSVersion=4.0
+Description=Optical Line Guidance
+CreationTime=09:08AM
+CreationDate=08-15-2018
+CreatedBy=herrmra
+ModificationTime=10:00AM
+ModificationDate=15-02-2019
+ModifiedBy=Michael Lehning
+
+[DeviceInfo]
+VendorName=SICK AG
+VendorNumber=0x01000056
+ProductName=OLS20
+ProductNumber=0x00001004
+RevisionNumber=1
+OrderCode=-
+BaudRate_10=1
+BaudRate_20=1
+BaudRate_50=1
+BaudRate_125=1
+BaudRate_250=1
+BaudRate_500=1
+BaudRate_800=0
+BaudRate_1000=1
+DynamicChannelsSupported=0
+GroupMessaging=0
+LSS_Supported=1
+Granularity=8
+SimpleBootUpSlave=1
+SimpleBootUpMaster=0
+NrOfRXPDO=0
+NrOfTXPDO=2
+
+[Comments]
+Lines=0
+
+[DummyUsage]
+Dummy0001=0
+Dummy0002=0
+Dummy0003=0
+Dummy0004=0
+Dummy0005=0
+Dummy0006=0
+Dummy0007=0
+
+[MandatoryObjects]
+SupportedObjects=3
+1=0x1000
+2=0x1001
+3=0x1018
+
+[OptionalObjects]
+SupportedObjects=16
+1=0x1005
+2=0x1008
+3=0x1009
+4=0x100A
+5=0x100C
+6=0x100D
+7=0x1014
+8=0x1015
+9=0x1016
+10=0x1017
+11=0x1200
+12=0x1800
+13=0x1801
+14=0x1A00
+15=0x1A01
+16=0x1F80
+
+[ManufacturerObjects]
+SupportedObjects=6
+1=0x2001
+2=0x2002
+3=0x2003
+4=0x2018
+5=0x2019
+6=0x2021
+
+[1000]
+ParameterName=Device Type
+ObjectType=0x07
+DataType=0x0007
+AccessType=const
+DefaultValue=0x0000000
+PDOMapping=0
+
+[1001]
+ParameterName=Error Register
+ObjectType=0x07
+DataType=0x0005
+AccessType=ro
+DefaultValue=0x00
+PDOMapping=0
+
+[1005]
+ParameterName=COB-ID SYNC
+ObjectType=0x07
+DataType=0x0007
+AccessType=rw
+DefaultValue=0x00000080
+PDOMapping=0
+
+[1008]
+ParameterName=Manufacturer Device Name
+ObjectType=0x07
+DataType=0x0009
+AccessType=const
+DefaultValue=OLS20
+PDOMapping=0
+
+[1009]
+ParameterName=Manufacturer Hardware Version
+ObjectType=0x07
+DataType=0x0009
+AccessType=const
+PDOMapping=0
+
+[100A]
+ParameterName=Manufacturer Software Version
+ObjectType=0x07
+DataType=0x0009
+AccessType=const
+PDOMapping=0
+
+[100C]
+ParameterName=Guard Time
+ObjectType=0x07
+DataType=0x0006
+AccessType=rw
+DefaultValue=0x0000
+PDOMapping=0
+
+[100D]
+ParameterName=Life Time Factor
+ObjectType=0x07
+DataType=0x0005
+AccessType=rw
+DefaultValue=0x00
+PDOMapping=0
+
+[1014]
+ParameterName=COB-ID EMCY
+ObjectType=0x07
+DataType=0x0007
+AccessType=rw
+DefaultValue=$NODEID+0x80
+PDOMapping=0
+
+[1015]
+ParameterName=Inhibit Time Emergency
+ObjectType=0x07
+DataType=0x0006
+AccessType=rw
+DefaultValue=0x0000
+PDOMapping=0
+
+[1016]
+SubNumber=3
+ParameterName=Heartbeat Consumer Entries
+ObjectType=0x08
+
+[1016sub0]
+ParameterName=Highest sub-index supported
+ObjectType=0x07
+DataType=0x0005
+AccessType=const
+DefaultValue=0x02
+PDOMapping=0
+
+[1016sub1]
+ParameterName=Consumer Heartbeat Time 1
+ObjectType=0x07
+DataType=0x0007
+AccessType=rw
+PDOMapping=0
+
+[1016sub2]
+ParameterName=Consumer Heartbeat Time 2
+ObjectType=0x07
+DataType=0x0007
+AccessType=rw
+PDOMapping=0
+
+[1017]
+ParameterName=Producer Heartbeat Time
+ObjectType=0x07
+DataType=0x0006
+AccessType=rw
+DefaultValue=0x00000000
+PDOMapping=0
+
+[1018]
+SubNumber=5
+ParameterName=Identity Object
+ObjectType=0x09
+
+[1018sub0]
+ParameterName=number of entries
+ObjectType=0x07
+DataType=0x0005
+AccessType=const
+DefaultValue=0x4
+PDOMapping=0
+
+[1018sub1]
+ParameterName=Vendor Id
+ObjectType=0x07
+DataType=0x0007
+AccessType=ro
+DefaultValue=0x01000056
+PDOMapping=0
+
+[1018sub2]
+ParameterName=Product Code
+ObjectType=0x07
+DataType=0x0007
+AccessType=ro
+DefaultValue=0x00001004
+PDOMapping=0
+
+[1018sub3]
+ParameterName=Revision number
+ObjectType=0x07
+DataType=0x0007
+AccessType=ro
+DefaultValue=0x00000001
+PDOMapping=0
+
+[1018sub4]
+ParameterName=Serial number
+ObjectType=0x07
+DataType=0x0007
+AccessType=ro
+PDOMapping=0
+
+[1200]
+SubNumber=3
+ParameterName=Server SDO Parameter 1
+ObjectType=0x09
+
+[1200sub0]
+ParameterName=Highest sub-index supported
+ObjectType=0x07
+DataType=0x0005
+AccessType=const
+DefaultValue=0x02
+PDOMapping=0
+
+[1200sub1]
+ParameterName=COB-ID Client -> Server
+ObjectType=0x07
+DataType=0x0007
+AccessType=ro
+DefaultValue=$NODEID+0x600
+PDOMapping=0
+
+[1200sub2]
+ParameterName=COB-ID Server -> Client
+ObjectType=0x07
+DataType=0x0007
+AccessType=ro
+DefaultValue=$NODEID+0x580
+PDOMapping=0
+
+[1800]
+SubNumber=6
+ParameterName=Transmit PDO Communication Parameter 1
+ObjectType=0x09
+
+[1800sub0]
+ParameterName=Highest sub-index supported
+ObjectType=0x07
+DataType=0x0005
+AccessType=const
+DefaultValue=0x05
+PDOMapping=0
+LowLimit=0x2
+HighLimit=0x6
+
+[1800sub1]
+ParameterName=COB-ID
+ObjectType=0x07
+DataType=0x0007
+AccessType=rw
+DefaultValue=$NODEID+0x180
+PDOMapping=0
+LowLimit=0x00000181
+HighLimit=0xFFFFFFFF
+
+[1800sub2]
+ParameterName=Transmission Type
+ObjectType=0x07
+DataType=0x0005
+AccessType=rw
+DefaultValue=0xFF
+PDOMapping=0
+LowLimit=0x0
+HighLimit=0xFF
+
+[1800sub3]
+ParameterName=Inhibit Time
+ObjectType=0x07
+DataType=0x0006
+AccessType=rw
+DefaultValue=0x0
+PDOMapping=0
+LowLimit=0x0000
+HighLimit=0xFFFF
+
+[1800sub4]
+ParameterName=Compatibility Entry
+ObjectType=0x07
+DataType=0x0005
+AccessType=rw
+DefaultValue=0x00
+PDOMapping=0
+LowLimit=0x00
+HighLimit=0xFF
+
+[1800sub5]
+ParameterName=Event Timer
+ObjectType=0x07
+DataType=0x0006
+AccessType=rw
+DefaultValue=0x14
+PDOMapping=0
+LowLimit=0x0
+HighLimit=0xFFFF
+
+[1801]
+SubNumber=6
+ParameterName=Transmit PDO Communication Parameter 2
+ObjectType=0x09
+
+[1801sub0]
+ParameterName=Highest sub-index supported
+ObjectType=0x07
+DataType=0x0005
+AccessType=const
+DefaultValue=0x05
+PDOMapping=0
+LowLimit=0x02
+HighLimit=0x06
+
+[1801sub1]
+ParameterName=COB-ID
+ObjectType=0x07
+DataType=0x0007
+AccessType=rw
+DefaultValue=$NODEID+0x280
+PDOMapping=0
+LowLimit=0x00000281
+HighLimit=0xFFFFFFFF
+
+[1801sub2]
+ParameterName=Transmission Type
+ObjectType=0x07
+DataType=0x0005
+AccessType=rw
+DefaultValue=0xFF
+PDOMapping=0
+LowLimit=0x0
+HighLimit=0xFF
+
+[1801sub3]
+ParameterName=Inhibit Time
+ObjectType=0x07
+DataType=0x0006
+AccessType=rw
+DefaultValue=0x0
+PDOMapping=0
+LowLimit=0x0000
+HighLimit=0xFFFF
+
+[1801sub4]
+ParameterName=Compatibility Entry
+ObjectType=0x07
+DataType=0x0005
+AccessType=rw
+DefaultValue=0x00
+PDOMapping=0
+LowLimit=0x00
+HighLimit=0xFF
+
+[1801sub5]
+ParameterName=Event Timer
+ObjectType=0x07
+DataType=0x0006
+AccessType=rw
+DefaultValue=0x14
+PDOMapping=0
+LowLimit=0x0
+HighLimit=0xFFFF
+
+[1A00]
+SubNumber=9
+ParameterName=Transmit PDO Mapping Parameter 1
+ObjectType=0x09
+
+[1A00sub0]
+ParameterName=Number of mapped objects
+ObjectType=0x07
+DataType=0x0005
+AccessType=rw
+DefaultValue=0x05
+PDOMapping=0
+LowLimit=0x00
+HighLimit=0x08
+
+[1A00sub1]
+ParameterName=Mapping Entry 1
+ObjectType=0x07
+DataType=0x0007
+AccessType=rw
+DefaultValue=0x20210110
+PDOMapping=0
+
+[1A00sub2]
+ParameterName=Mapping Entry 2
+ObjectType=0x07
+DataType=0x0007
+AccessType=rw
+DefaultValue=0x20210210
+PDOMapping=0
+
+[1A00sub3]
+ParameterName=Mapping Entry 3
+ObjectType=0x07
+DataType=0x0007
+AccessType=rw
+DefaultValue=0x20210310
+PDOMapping=0
+
+[1A00sub4]
+ParameterName=Mapping Entry 4
+ObjectType=0x07
+DataType=0x0007
+AccessType=rw
+DefaultValue=0x20210408
+PDOMapping=0
+
+[1A00sub5]
+ParameterName=Mapping Entry 5
+ObjectType=0x07
+DataType=0x0007
+AccessType=rw
+DefaultValue=0x20210808
+PDOMapping=0
+
+[1A00sub6]
+ParameterName=Mapping Entry 6
+ObjectType=0x07
+DataType=0x0007
+AccessType=rw
+DefaultValue=0x00000000
+PDOMapping=0
+
+[1A00sub7]
+ParameterName=Mapping Entry 7
+ObjectType=0x07
+DataType=0x0007
+AccessType=rw
+DefaultValue=0x00000000
+PDOMapping=0
+
+[1A00sub8]
+ParameterName=Mapping Entry 8
+ObjectType=0x07
+DataType=0x0007
+AccessType=rw
+DefaultValue=0x00000000
+PDOMapping=0
+
+[1A01]
+SubNumber=9
+ParameterName=Transmit PDO Mapping Parameter 2
+ObjectType=0x09
+
+[1A01sub0]
+ParameterName=Number of mapped objects
+ObjectType=0x07
+DataType=0x0005
+AccessType=rw
+DefaultValue=0x03
+PDOMapping=0
+LowLimit=0x0
+HighLimit=0x8
+
+[1A01sub1]
+ParameterName=Mapping Entry 1
+ObjectType=0x07
+DataType=0x0007
+AccessType=rw
+DefaultValue=0x20210510
+PDOMapping=0
+
+[1A01sub2]
+ParameterName=Mapping Entry 2
+ObjectType=0x07
+DataType=0x0007
+AccessType=rw
+DefaultValue=0x20210610
+PDOMapping=0
+
+[1A01sub3]
+ParameterName=Mapping Entry 3
+ObjectType=0x07
+DataType=0x0007
+AccessType=rw
+DefaultValue=0x20210710
+PDOMapping=0
+
+[1A01sub4]
+ParameterName=Mapping Entry 4
+ObjectType=0x07
+DataType=0x0007
+AccessType=rw
+DefaultValue=0x00000000
+PDOMapping=0
+
+[1A01sub5]
+ParameterName=Mapping Entry 5
+ObjectType=0x07
+DataType=0x0007
+AccessType=rw
+DefaultValue=0x00000000
+PDOMapping=0
+
+[1A01sub6]
+ParameterName=Mapping Entry 6
+ObjectType=0x07
+DataType=0x0007
+AccessType=rw
+DefaultValue=0x00000000
+PDOMapping=0
+
+[1A01sub7]
+ParameterName=Mapping Entry 7
+ObjectType=0x07
+DataType=0x0007
+AccessType=rw
+DefaultValue=0x00000000
+PDOMapping=0
+
+[1A01sub8]
+ParameterName=Mapping Entry 8
+ObjectType=0x07
+DataType=0x0007
+AccessType=rw
+DefaultValue=0x00000000
+PDOMapping=0
+
+[1F80]
+ParameterName=NMT Startup
+ObjectType=0x07
+DataType=0x0007
+AccessType=const
+PDOMapping=0
+LowLimit=0x0
+HighLimit=0x3F
+
+[2001]
+SubNumber=6
+ParameterName=Mounting parameters
+ObjectType=0x09
+
+[2001sub0]
+ParameterName=numOfEntries
+ObjectType=0x07
+DataType=0x0005
+AccessType=const
+DefaultValue=0x5
+PDOMapping=0
+
+[2001sub1]
+ParameterName=reserved1
+ObjectType=0x07
+DataType=0x0007
+AccessType=ro
+PDOMapping=0
+
+[2001sub2]
+ParameterName=reserved2
+ObjectType=0x07
+DataType=0x0007
+AccessType=ro
+PDOMapping=0
+
+[2001sub3]
+ParameterName=reserved3
+ObjectType=0x07
+DataType=0x0007
+AccessType=ro
+PDOMapping=0
+
+[2001sub4]
+ParameterName=reserved4
+ObjectType=0x07
+DataType=0x0007
+AccessType=ro
+PDOMapping=0
+
+[2001sub5]
+ParameterName=sensorFlipped
+ObjectType=0x07
+DataType=0x0005
+AccessType=rw
+DefaultValue=0x00
+PDOMapping=0
+LowLimit=0x0
+HighLimit=0xFF
+
+[2002]
+SubNumber=4
+ParameterName=Tape Parameters
+ObjectType=0x09
+
+[2002sub0]
+ParameterName=Number of mapped objects
+ObjectType=0x07
+DataType=0x0005
+AccessType=const
+DefaultValue=0x3
+PDOMapping=0
+
+[2002sub1]
+ParameterName=Typ. Width
+ObjectType=0x07
+DataType=0x0008
+AccessType=rw
+PDOMapping=0
+
+[2002sub2]
+ParameterName=Min. Width
+ObjectType=0x07
+DataType=0x0008
+AccessType=rw
+PDOMapping=0
+
+[2002sub3]
+ParameterName=Max. Width
+ObjectType=0x07
+DataType=0x0008
+AccessType=rw
+PDOMapping=0
+
+[2003]
+SubNumber=3
+ParameterName=Advanced Settings
+ObjectType=0x09
+
+[2003sub0]
+ParameterName=Number of mapped objects
+ObjectType=0x07
+DataType=0x0005
+AccessType=const
+DefaultValue=0x2
+PDOMapping=0
+
+[2003sub1]
+ParameterName=Off Delay Track Detection
+ObjectType=0x07
+DataType=0x0006
+AccessType=rw
+DefaultValue=0x0064
+PDOMapping=0
+LowLimit=0x0
+HighLimit=0xFFFF
+
+[2003sub2]
+ParameterName=Off Delay Code Detection
+ObjectType=0x07
+DataType=0x0006
+AccessType=rw
+DefaultValue=0x0064
+PDOMapping=0
+LowLimit=0x0
+HighLimit=0xFFFF
+
+[2018]
+ParameterName=Device Status
+ObjectType=0x07
+DataType=0x0005
+AccessType=ro
+PDOMapping=0
+
+[2019]
+ParameterName=Order number
+ObjectType=0x07
+DataType=0x0007
+AccessType=ro
+PDOMapping=0
+
+[2021]
+SubNumber=10
+ParameterName=Result data (LCPs)
+ObjectType=0x09
+
+[2021sub0]
+ParameterName=Number of mapped objects
+ObjectType=0x07
+DataType=0x0005
+AccessType=const
+DefaultValue=0x9
+PDOMapping=0
+
+[2021sub1]
+ParameterName=LCP1
+ObjectType=0x07
+DataType=0x0003
+AccessType=ro
+PDOMapping=1
+LowLimit=0x8000
+HighLimit=0x7FFF
+
+[2021sub2]
+ParameterName=LCP2
+ObjectType=0x07
+DataType=0x0003
+AccessType=ro
+PDOMapping=1
+LowLimit=0x8000
+HighLimit=0x7FFF
+
+[2021sub3]
+ParameterName=LCP3
+ObjectType=0x07
+DataType=0x0003
+AccessType=ro
+PDOMapping=1
+LowLimit=0x8000
+HighLimit=0x7FFF
+
+[2021sub4]
+ParameterName=Status
+ObjectType=0x07
+DataType=0x0005
+AccessType=ro
+PDOMapping=1
+LowLimit=0x00
+HighLimit=0xFF
+
+[2021sub5]
+ParameterName=Width LCP1
+ObjectType=0x07
+DataType=0x0003
+AccessType=ro
+PDOMapping=1
+LowLimit=0x8000
+HighLimit=0x7FFF
+
+[2021sub6]
+ParameterName=Width LCP2
+ObjectType=0x07
+DataType=0x0003
+AccessType=ro
+PDOMapping=1
+LowLimit=0x8000
+HighLimit=0x7FFF
+
+[2021sub7]
+ParameterName=Width LCP3
+ObjectType=0x07
+DataType=0x0003
+AccessType=ro
+PDOMapping=1
+LowLimit=0x8000
+HighLimit=0x7FFF
+
+[2021sub8]
+ParameterName=Code
+ObjectType=0x07
+DataType=0x0005
+AccessType=ro
+PDOMapping=1
+LowLimit=0x0
+HighLimit=0xFF
+
+[2021sub9]
+ParameterName=Extended Code
+ObjectType=0x07
+DataType=0x0007
+AccessType=ro
+PDOMapping=0
+LowLimit=0x0
+HighLimit=0xFFFFFFFF
+

Devices/Packages/sick_line_guidance/ols/SICK_OLS20_CO.eds

@@ -0,0 +1,867 @@
+
+; This EDS file was created by the CANopen Design Tool 2.3.26.0.
+; port GmbH Halle/Saale Germany, http://www.port.de, mailto:service@port.de
+
+[FileInfo]
+FileName=OLS20.eds
+FileVersion=1
+FileRevision=2
+EDSVersion=4.0
+Description=Optical Line Guidance
+CreationTime=09:08AM
+CreationDate=08-15-2018
+CreatedBy=herrmra
+ModificationTime=04:29PM
+ModificationDate=04-10-2019
+ModifiedBy=herrmra
+
+[DeviceInfo]
+VendorName=SICK AG
+VendorNumber=0x01000056
+ProductName=OLS20
+ProductNumber=0x00001004
+RevisionNumber=1
+OrderCode=-
+BaudRate_10=1
+BaudRate_20=1
+BaudRate_50=1
+BaudRate_125=1
+BaudRate_250=1
+BaudRate_500=1
+BaudRate_800=0
+BaudRate_1000=1
+DynamicChannelsSupported=0
+GroupMessaging=0
+LSS_Supported=1
+Granularity=8
+SimpleBootUpSlave=1
+SimpleBootUpMaster=0
+NrOfRXPDO=0
+NrOfTXPDO=2
+
+[Comments]
+Lines=0
+
+[DummyUsage]
+Dummy0001=0
+Dummy0002=0
+Dummy0003=0
+Dummy0004=0
+Dummy0005=0
+Dummy0006=0
+Dummy0007=0
+
+[MandatoryObjects]
+SupportedObjects=3
+1=0x1000
+2=0x1001
+3=0x1018
+
+[OptionalObjects]
+SupportedObjects=16
+1=0x1005
+2=0x1008
+3=0x1009
+4=0x100A
+5=0x100C
+6=0x100D
+7=0x1014
+8=0x1015
+9=0x1016
+10=0x1017
+11=0x1200
+12=0x1800
+13=0x1801
+14=0x1A00
+15=0x1A01
+16=0x1F80
+
+[ManufacturerObjects]
+SupportedObjects=7
+1=0x2001
+2=0x2002
+3=0x2003
+4=0x2018
+5=0x2019
+6=0x2021
+7=0x2023
+
+[1000]
+ParameterName=Device Type
+ObjectType=0x07
+DataType=0x0007
+AccessType=const
+DefaultValue=0x0000000
+PDOMapping=0
+
+[1001]
+ParameterName=Error Register
+ObjectType=0x07
+DataType=0x0005
+AccessType=ro
+DefaultValue=0
+PDOMapping=0
+
+[1005]
+ParameterName=COB-ID SYNC
+ObjectType=0x07
+DataType=0x0007
+AccessType=rw
+DefaultValue=0x00000080
+PDOMapping=0
+
+[1008]
+ParameterName=Manufacturer Device Name
+ObjectType=0x07
+DataType=0x0009
+AccessType=const
+DefaultValue=OLS20
+PDOMapping=0
+
+[1009]
+ParameterName=Manufacturer Hardware Version
+ObjectType=0x07
+DataType=0x0009
+AccessType=const
+PDOMapping=0
+
+[100A]
+ParameterName=Manufacturer Software Version
+ObjectType=0x07
+DataType=0x0009
+AccessType=const
+PDOMapping=0
+
+[100C]
+ParameterName=Guard Time
+ObjectType=0x07
+DataType=0x0006
+AccessType=rw
+DefaultValue=0x00
+PDOMapping=0
+
+[100D]
+ParameterName=Life Time Factor
+ObjectType=0x07
+DataType=0x0005
+AccessType=rw
+DefaultValue=0x00
+PDOMapping=0
+
+[1014]
+ParameterName=COB-ID EMCY
+ObjectType=0x07
+DataType=0x0007
+AccessType=rw
+DefaultValue=$NODEID+0x80
+PDOMapping=0
+
+[1015]
+ParameterName=Inhibit Time Emergency
+ObjectType=0x07
+DataType=0x0006
+AccessType=rw
+DefaultValue=0x0000
+PDOMapping=0
+
+[1016]
+SubNumber=3
+ParameterName=Heartbeat Consumer Entries
+ObjectType=0x08
+
+[1016sub0]
+ParameterName=Highest sub-index supported
+ObjectType=0x07
+DataType=5
+AccessType=const
+DefaultValue=0x02
+PDOMapping=0
+
+[1016sub1]
+ParameterName=Consumer Heartbeat Time 1
+ObjectType=0x07
+DataType=0x0007
+AccessType=rw
+PDOMapping=0
+
+[1016sub2]
+ParameterName=Consumer Heartbeat Time 2
+ObjectType=0x07
+DataType=0x0007
+AccessType=rw
+PDOMapping=0
+
+[1017]
+ParameterName=Producer Heartbeat Time
+ObjectType=0x07
+DataType=0x0006
+AccessType=rw
+DefaultValue=0x00
+PDOMapping=0
+
+[1018]
+SubNumber=5
+ParameterName=Identity Object
+ObjectType=0x09
+
+[1018sub0]
+ParameterName=Highest sub-index supported
+ObjectType=0x07
+DataType=0x0005
+AccessType=const
+DefaultValue=0x4
+PDOMapping=0
+
+[1018sub1]
+ParameterName=Vendor Id
+ObjectType=0x07
+DataType=0x0007
+AccessType=ro
+DefaultValue=0x01000056
+PDOMapping=0
+
+[1018sub2]
+ParameterName=Product Code
+ObjectType=0x07
+DataType=0x0007
+AccessType=ro
+DefaultValue=0x00001004
+PDOMapping=0
+
+[1018sub3]
+ParameterName=Revision number
+ObjectType=0x07
+DataType=0x0007
+AccessType=ro
+DefaultValue=1
+PDOMapping=0
+
+[1018sub4]
+ParameterName=Serial number
+ObjectType=0x07
+DataType=0x0007
+AccessType=ro
+PDOMapping=0
+
+[1200]
+SubNumber=3
+ParameterName=Server SDO Parameter 1
+ObjectType=0x09
+
+[1200sub0]
+ParameterName=Highest sub-index supported
+ObjectType=0x07
+DataType=0x0005
+AccessType=const
+DefaultValue=0x02
+PDOMapping=0
+
+[1200sub1]
+ParameterName=COB-ID Client -> Server
+ObjectType=0x07
+DataType=0x0007
+AccessType=ro
+DefaultValue=$NODEID+0x600
+PDOMapping=0
+
+[1200sub2]
+ParameterName=COB-ID Server -> Client
+ObjectType=0x07
+DataType=0x0007
+AccessType=ro
+DefaultValue=$NODEID+0x580
+PDOMapping=0
+
+[1800]
+SubNumber=6
+ParameterName=Transmit PDO Communication Parameter 1
+ObjectType=0x09
+
+[1800sub0]
+ParameterName=Highest sub-index supported
+ObjectType=0x07
+DataType=0x0005
+AccessType=const
+DefaultValue=0x05
+PDOMapping=0
+LowLimit=0x2
+HighLimit=0x6
+
+[1800sub1]
+ParameterName=COB-ID
+ObjectType=0x07
+DataType=0x0007
+AccessType=rw
+DefaultValue=$NODEID+0x180
+PDOMapping=0
+LowLimit=0x00000181
+HighLimit=0xFFFFFFFF
+
+[1800sub2]
+ParameterName=Transmission Type
+ObjectType=0x07
+DataType=0x0005
+AccessType=rw
+DefaultValue=0xFF
+PDOMapping=0
+LowLimit=0x0
+HighLimit=0xFF
+
+[1800sub3]
+ParameterName=Inhibit Time
+ObjectType=0x07
+DataType=0x0006
+AccessType=rw
+DefaultValue=0x0
+PDOMapping=0
+LowLimit=0x0000
+HighLimit=0xFFFF
+
+[1800sub4]
+ParameterName=Compatibility Entry
+ObjectType=0x07
+DataType=0x0005
+AccessType=rw
+DefaultValue=0x00
+PDOMapping=0
+LowLimit=0x00
+HighLimit=0xFF
+
+[1800sub5]
+ParameterName=Event Timer
+ObjectType=0x07
+DataType=0x0006
+AccessType=rw
+DefaultValue=0x14
+PDOMapping=0
+LowLimit=0x0
+HighLimit=0xFFFF
+
+[1801]
+SubNumber=6
+ParameterName=Transmit PDO Communication Parameter 2
+ObjectType=0x09
+
+[1801sub0]
+ParameterName=Highest sub-index supported
+ObjectType=0x07
+DataType=0x0005
+AccessType=const
+DefaultValue=0x05
+PDOMapping=0
+LowLimit=0x02
+HighLimit=0x06
+
+[1801sub1]
+ParameterName=COB-ID
+ObjectType=0x07
+DataType=0x0007
+AccessType=rw
+DefaultValue=$NODEID+0x280
+PDOMapping=0
+LowLimit=0x00000281
+HighLimit=0xFFFFFFFF
+
+[1801sub2]
+ParameterName=Transmission Type
+ObjectType=0x07
+DataType=0x0005
+AccessType=rw
+DefaultValue=0xFF
+PDOMapping=0
+LowLimit=0x0
+HighLimit=0xFF
+
+[1801sub3]
+ParameterName=Inhibit Time
+ObjectType=0x07
+DataType=0x0006
+AccessType=rw
+DefaultValue=0x0
+PDOMapping=0
+LowLimit=0x0000
+HighLimit=0xFFFF
+
+[1801sub4]
+ParameterName=Compatibility Entry
+ObjectType=0x07
+DataType=0x0005
+AccessType=rw
+DefaultValue=0x00
+PDOMapping=0
+LowLimit=0x00
+HighLimit=0xFF
+
+[1801sub5]
+ParameterName=Event Timer
+ObjectType=0x07
+DataType=0x0006
+AccessType=rw
+DefaultValue=0x14
+PDOMapping=0
+LowLimit=0x0
+HighLimit=0xFFFF
+
+[1A00]
+SubNumber=9
+ParameterName=Transmit PDO Mapping Parameter 1
+ObjectType=0x09
+
+[1A00sub0]
+ParameterName=Number of mapped objects
+ObjectType=0x07
+DataType=0x0005
+AccessType=rw
+DefaultValue=0x05
+PDOMapping=0
+LowLimit=0x00
+HighLimit=0x08
+
+[1A00sub1]
+ParameterName=Mapping Entry 1
+ObjectType=0x07
+DataType=0x0007
+AccessType=rw
+DefaultValue=0x20210110
+PDOMapping=0
+
+[1A00sub2]
+ParameterName=Mapping Entry 2
+ObjectType=0x07
+DataType=0x0007
+AccessType=rw
+DefaultValue=0x20210210
+PDOMapping=0
+
+[1A00sub3]
+ParameterName=Mapping Entry 3
+ObjectType=0x07
+DataType=0x0007
+AccessType=rw
+DefaultValue=0x20210310
+PDOMapping=0
+
+[1A00sub4]
+ParameterName=Mapping Entry 4
+ObjectType=0x07
+DataType=0x0007
+AccessType=rw
+DefaultValue=0x20210408
+PDOMapping=0
+
+[1A00sub5]
+ParameterName=Mapping Entry 5
+ObjectType=0x07
+DataType=0x0007
+AccessType=rw
+DefaultValue=0x20210808
+PDOMapping=0
+
+[1A00sub6]
+ParameterName=Mapping Entry 6
+ObjectType=0x07
+DataType=0x0007
+AccessType=rw
+DefaultValue=0x00000000
+PDOMapping=0
+
+[1A00sub7]
+ParameterName=Mapping Entry 7
+ObjectType=0x07
+DataType=0x0007
+AccessType=rw
+DefaultValue=0x00000000
+PDOMapping=0
+
+[1A00sub8]
+ParameterName=Mapping Entry 8
+ObjectType=0x07
+DataType=0x0007
+AccessType=rw
+DefaultValue=0x00000000
+PDOMapping=0
+
+[1A01]
+SubNumber=9
+ParameterName=Transmit PDO Mapping Parameter 2
+ObjectType=0x09
+
+[1A01sub0]
+ParameterName=Number of mapped objects
+ObjectType=0x07
+DataType=0x0005
+AccessType=rw
+DefaultValue=0x03
+PDOMapping=0
+LowLimit=0x0
+HighLimit=0x8
+
+[1A01sub1]
+ParameterName=Mapping Entry 1
+ObjectType=0x07
+DataType=0x0007
+AccessType=rw
+DefaultValue=0x20210510
+PDOMapping=0
+
+[1A01sub2]
+ParameterName=Mapping Entry 2
+ObjectType=0x07
+DataType=0x0007
+AccessType=rw
+DefaultValue=0x20210610
+PDOMapping=0
+
+[1A01sub3]
+ParameterName=Mapping Entry 3
+ObjectType=0x07
+DataType=0x0007
+AccessType=rw
+DefaultValue=0x20210710
+PDOMapping=0
+
+[1A01sub4]
+ParameterName=Mapping Entry 4
+ObjectType=0x07
+DataType=0x0007
+AccessType=rw
+DefaultValue=0x0
+PDOMapping=0
+
+[1A01sub5]
+ParameterName=Mapping Entry 5
+ObjectType=0x07
+DataType=0x0007
+AccessType=rw
+DefaultValue=0x0
+PDOMapping=0
+
+[1A01sub6]
+ParameterName=Mapping Entry 6
+ObjectType=0x07
+DataType=0x0007
+AccessType=rw
+DefaultValue=0x0
+PDOMapping=0
+
+[1A01sub7]
+ParameterName=Mapping Entry 7
+ObjectType=0x07
+DataType=0x0007
+AccessType=rw
+DefaultValue=0x0
+PDOMapping=0
+
+[1A01sub8]
+ParameterName=Mapping Entry 8
+ObjectType=0x07
+DataType=0x0007
+AccessType=rw
+DefaultValue=0x0
+PDOMapping=0
+
+[1F80]
+ParameterName=NMT Startup
+ObjectType=0x07
+DataType=0x0007
+AccessType=const
+PDOMapping=0
+LowLimit=0x0
+HighLimit=0x3F
+
+[2001]
+SubNumber=6
+ParameterName=Mounting parameters
+ObjectType=0x09
+
+[2001sub0]
+ParameterName=Highest sub-index supported
+ObjectType=0x07
+DataType=0x0005
+AccessType=const
+DefaultValue=0x5
+PDOMapping=0
+
+[2001sub1]
+ParameterName=Reserved 1
+ObjectType=0x07
+DataType=0x0007
+AccessType=ro
+PDOMapping=0
+
+[2001sub2]
+ParameterName=Reserved 2
+ObjectType=0x07
+DataType=0x0007
+AccessType=ro
+PDOMapping=0
+
+[2001sub3]
+ParameterName=Reserved 3
+ObjectType=0x07
+DataType=0x0007
+AccessType=ro
+PDOMapping=0
+
+[2001sub4]
+ParameterName=Reserved 4
+ObjectType=0x07
+DataType=0x0007
+AccessType=ro
+PDOMapping=0
+
+[2001sub5]
+ParameterName=Sensor flipped
+ObjectType=0x07
+DataType=0x0005
+AccessType=rw
+DefaultValue=0x0
+PDOMapping=0
+LowLimit=0x0
+HighLimit=0xFF
+
+[2002]
+SubNumber=4
+ParameterName=Tape parameters
+ObjectType=0x09
+
+[2002sub0]
+ParameterName=Highest sub-index supported
+ObjectType=0x07
+DataType=0x0005
+AccessType=const
+DefaultValue=0x3
+PDOMapping=0
+
+[2002sub1]
+ParameterName=Typ. width
+ObjectType=0x07
+DataType=0x0008
+AccessType=rw
+PDOMapping=0
+
+[2002sub2]
+ParameterName=Min. width
+ObjectType=0x07
+DataType=0x0008
+AccessType=rw
+PDOMapping=0
+
+[2002sub3]
+ParameterName=Max. width
+ObjectType=0x07
+DataType=0x0008
+AccessType=rw
+PDOMapping=0
+
+[2003]
+SubNumber=4
+ParameterName=Advanced settings
+ObjectType=0x09
+
+[2003sub0]
+ParameterName=Highest sub-index supported
+ObjectType=0x07
+DataType=0x0005
+AccessType=const
+DefaultValue=0x3
+PDOMapping=0
+
+[2003sub1]
+ParameterName=Off delay track detection
+ObjectType=0x07
+DataType=0x0006
+AccessType=rw
+DefaultValue=0x64
+PDOMapping=0
+LowLimit=0x0
+HighLimit=0xFFFF
+
+[2003sub2]
+ParameterName=Off delay code detection
+ObjectType=0x07
+DataType=0x0006
+AccessType=rw
+DefaultValue=0x64
+PDOMapping=0
+LowLimit=0x0
+HighLimit=0xFFFF
+
+[2003sub3]
+ParameterName=Position smoothing filter coefficient
+ObjectType=0x07
+DataType=0x0008
+AccessType=rw
+DefaultValue=0.0
+PDOMapping=0
+
+[2018]
+ParameterName=Device status
+ObjectType=0x07
+DataType=0x0005
+AccessType=ro
+PDOMapping=0
+
+[2019]
+ParameterName=Order number
+ObjectType=0x07
+DataType=0x0007
+AccessType=ro
+PDOMapping=0
+
+[2021]
+SubNumber=12
+ParameterName=Result data (LCPs)
+ObjectType=0x09
+
+[2021sub0]
+ParameterName=Highest sub-index supported
+ObjectType=0x07
+DataType=0x0005
+AccessType=const
+DefaultValue=0x0B
+PDOMapping=0
+
+[2021sub1]
+ParameterName=LCP 1
+ObjectType=0x07
+DataType=0x0003
+AccessType=ro
+PDOMapping=1
+LowLimit=0x8000
+HighLimit=0x7FFF
+
+[2021sub2]
+ParameterName=LCP 2
+ObjectType=0x07
+DataType=0x0003
+AccessType=ro
+PDOMapping=1
+LowLimit=0x8000
+HighLimit=0x7FFF
+
+[2021sub3]
+ParameterName=LCP 3
+ObjectType=0x07
+DataType=0x0003
+AccessType=ro
+PDOMapping=1
+LowLimit=0x8000
+HighLimit=0x7FFF
+
+[2021sub4]
+ParameterName=Status
+ObjectType=0x07
+DataType=0x0005
+AccessType=ro
+PDOMapping=1
+LowLimit=0x00
+HighLimit=0xFF
+
+[2021sub5]
+ParameterName=Width LCP 1
+ObjectType=0x07
+DataType=0x0003
+AccessType=ro
+PDOMapping=1
+LowLimit=0x8000
+HighLimit=0x7FFF
+
+[2021sub6]
+ParameterName=Width LCP 2
+ObjectType=0x07
+DataType=0x0003
+AccessType=ro
+PDOMapping=1
+LowLimit=0x8000
+HighLimit=0x7FFF
+
+[2021sub7]
+ParameterName=Width LCP 3
+ObjectType=0x07
+DataType=0x0003
+AccessType=ro
+PDOMapping=1
+LowLimit=0x8000
+HighLimit=0x7FFF
+
+[2021sub8]
+ParameterName=Code
+ObjectType=0x07
+DataType=0x0005
+AccessType=ro
+PDOMapping=1
+LowLimit=0x0
+HighLimit=0xFF
+
+[2021sub9]
+ParameterName=Extended code
+ObjectType=0x07
+DataType=0x0007
+AccessType=ro
+PDOMapping=0
+LowLimit=0x0
+HighLimit=0xFFFFFFFF
+
+[2021subA]
+ParameterName=Barcode center point
+ObjectType=0x07
+DataType=0x0003
+AccessType=ro
+DefaultValue=0x0000
+PDOMapping=0
+LowLimit=0x8000
+HighLimit=0x7FFF
+
+[2021subB]
+ParameterName=Quality of lines
+ObjectType=0x07
+DataType=0x0005
+AccessType=ro
+DefaultValue=0x64
+PDOMapping=0
+LowLimit=0x00
+HighLimit=0x64
+
+[2023]
+SubNumber=4
+ParameterName=Line level
+ObjectType=0x09
+
+[2023sub0]
+ParameterName=Highest sub-index supported
+ObjectType=0x07
+DataType=0x0005
+AccessType=const
+DefaultValue=0x3
+PDOMapping=0
+LowLimit=0x1
+HighLimit=0x3
+
+[2023sub1]
+ParameterName=Intensity line 1
+ObjectType=0x07
+DataType=0x0005
+AccessType=ro
+PDOMapping=0
+LowLimit=0x0
+HighLimit=0x82
+
+[2023sub2]
+ParameterName=Intensity line 2
+ObjectType=0x07
+DataType=0x0005
+AccessType=ro
+PDOMapping=0
+LowLimit=0x0
+HighLimit=0x82
+
+[2023sub3]
+ParameterName=Intensity line 3
+ObjectType=0x07
+DataType=0x0005
+AccessType=ro
+PDOMapping=0
+LowLimit=0x0
+HighLimit=0x82
+

Devices/Packages/sick_line_guidance/ols/sick_line_guidance_ols10.yaml

@@ -0,0 +1,32 @@
+bus:
+  device: can0
+  driver_plugin: can::SocketCANInterface
+  master_allocator: canopen::SimpleMaster::Allocator
+sync:
+  interval_ms: 10
+  overflow: 0
+#
+# heartbeat: # simple heartbeat producer (optional, not supported by OLS or MLS, do not enable heartbeats)
+#   rate: 100 # heartbeat rate (milliseconds)
+#   msg: "77f#05" # message to send, cansend format: heartbeat of node 127 with status 5=Started
+nodes:
+  node1:
+    id: 0x0A # CAN-Node-ID of can device, default: Node-ID 10=0x0A for OLS and MLS
+    eds_pkg: sick_line_guidance # package name for relative path
+    eds_file: SICK_OLS10_CO.eds    # path to EDS/DCF file
+    publish: ["1001","1018sub1","1018sub4","2021sub1!","2021sub2!","2021sub3!","2021sub4!","2021sub5!","2021sub6!","2021sub7!","2021sub8!"]
+    # OLS10: 1001 = error register, 1018sub1 = VendorID, 1018sub4 = SerialNumber, TPDOs 0x2021sub1 to 0x2021sub8
+    
+    # sick_line_guidance configuration of this node:
+    sick_device_family: "OLS10"            # can devices currently supported: "OLS10", "OLS20" or "MLS"
+    sick_topic: "ols"                      # OLS_Measurement messages are published in topic "/ols"
+    sick_frame_id: "ols_measurement_frame" # OLS_Measurement messages are published with frame_id "ols_measurement_frame"
+
+    # device configuration of writable parameter by dcf_overlay: "objectindex": "parametervalue"
+    # example: "2001sub5": "0x01" # Object 2001sub5 (sensorFlipped, defaultvalue 0x00) will be configured with value 0x01
+    # dcf_overlay:
+    #   "2001sub5": "0x01"  # sensorFlipped, UINT8, DataType=0x0005, defaultvalue=0
+    #   "2002sub1": "0.015" # Typ. Width, FLOAT32, DataType=0x0008
+    #   "2002sub2": "0.001" # Min. Width, FLOAT32, DataType=0x0008
+    #   "2002sub3": "0.050" # Max. Width, FLOAT32, DataType=0x0008
+

Devices/Packages/sick_line_guidance/ols/sick_line_guidance_ols20.yaml

@@ -0,0 +1,32 @@
+bus:
+  device: can0
+  driver_plugin: can::SocketCANInterface
+  master_allocator: canopen::SimpleMaster::Allocator
+sync:
+  interval_ms: 10
+  overflow: 0
+#
+# heartbeat: # simple heartbeat producer (optional, not supported by OLS or MLS, do not enable heartbeats)
+#   rate: 100 # heartbeat rate (milliseconds)
+#   msg: "77f#05" # message to send, cansend format: heartbeat of node 127 with status 5=Started
+nodes:
+  node1:
+    id: 0x0A # CAN-Node-ID of can device, default: Node-ID 10=0x0A for OLS and MLS
+    eds_pkg: sick_line_guidance # package name for relative path
+    eds_file: SICK_OLS20_CO.eds    # path to EDS/DCF file
+    publish: ["1001","1018sub1","1018sub4","2021sub1!","2021sub2!","2021sub3!","2021sub4!","2021sub5!","2021sub6!","2021sub7!","2021sub8!"]
+    # OLS20: 1001 = error register, 1018sub1 = VendorID, 1018sub4 = SerialNumber, TPDOs 0x2021sub1 to 0x2021sub8
+    
+    # sick_line_guidance configuration of this node:
+    sick_device_family: "OLS20"            # can devices currently supported: "OLS10", "OLS20" or "MLS"
+    sick_topic: "ols"                      # OLS_Measurement messages are published in topic "/ols"
+    sick_frame_id: "ols_measurement_frame" # OLS_Measurement messages are published with frame_id "ols_measurement_frame"
+
+    # device configuration of writable parameter by dcf_overlay: "objectindex": "parametervalue"
+    # example: "2001sub5": "0x01" # Object 2001sub5 (sensorFlipped, defaultvalue 0x00) will be configured with value 0x01
+    # dcf_overlay:
+    #   "2001sub5": "0x01"  # sensorFlipped, UINT8, DataType=0x0005, defaultvalue=0
+    #   "2002sub1": "0.015" # Typ. Width, FLOAT32, DataType=0x0008
+    #   "2002sub2": "0.001" # Min. Width, FLOAT32, DataType=0x0008
+    #   "2002sub3": "0.050" # Max. Width, FLOAT32, DataType=0x0008
+

Devices/Packages/sick_line_guidance/ols/sick_line_guidance_ols20_twin.yaml

@@ -0,0 +1,44 @@
+bus:
+  device: can0
+  driver_plugin: can::SocketCANInterface
+  master_allocator: canopen::SimpleMaster::Allocator
+sync:
+  interval_ms: 10
+  overflow: 0
+#
+# heartbeat: # simple heartbeat producer (optional, not supported by OLS or MLS, do not enable heartbeats)
+#   rate: 100 # heartbeat rate (milliseconds)
+#   msg: "77f#05" # message to send, cansend format: heartbeat of node 127 with status 5=Started
+nodes:
+  node1:
+    id: 0x0A # CAN-Node-ID of can device, default: Node-ID 10=0x0A for OLS and MLS
+    eds_pkg: sick_line_guidance # package name for relative path
+    eds_file: SICK_OLS20_CO.eds    # path to EDS/DCF file
+    publish: ["1001","1018sub1","1018sub4","2021sub1!","2021sub2!","2021sub3!","2021sub4!","2021sub5!","2021sub6!","2021sub7!","2021sub8!"]
+    # OLS20: 1001 = error register, 1018sub1 = VendorID, 1018sub4 = SerialNumber, TPDOs 0x2021sub1 to 0x2021sub8
+    
+    # sick_line_guidance configuration of this node:
+    sick_device_family: "OLS20"            # can devices currently supported: "OLS10", "OLS20" or "MLS"
+    sick_topic: "olsA"                     # OLS_Measurement messages are published in topic "/ols"
+    sick_frame_id: "ols20A_measurement_frame" # OLS_Measurement messages are published with frame_id "ols_measurement_frame"
+
+    # device configuration of writable parameter by dcf_overlay: "objectindex": "parametervalue"
+    # example: "2001sub5": "0x01" # Object 2001sub5 (sensorFlipped, defaultvalue 0x00) will be configured with value 0x01
+    # dcf_overlay:
+    #   "2001sub5": "0x01"  # sensorFlipped, UINT8, DataType=0x0005, defaultvalue=0
+    #   "2002sub1": "0.015" # Typ. Width, FLOAT32, DataType=0x0008
+    #   "2002sub2": "0.001" # Min. Width, FLOAT32, DataType=0x0008
+    #   "2002sub3": "0.050" # Max. Width, FLOAT32, DataType=0x0008
+
+  node2:
+    id: 0x0B # CAN-Node-ID of can device, default: Node-ID 10=0x0A for OLS and MLS
+    eds_pkg: sick_line_guidance # package name for relative path
+    eds_file: SICK_OLS20_CO.eds    # path to EDS/DCF file
+    publish: ["1001","1018sub1","1018sub4","2021sub1!","2021sub2!","2021sub3!","2021sub4!","2021sub5!","2021sub6!","2021sub7!","2021sub8!"]
+    # OLS20: 1001 = error register, 1018sub1 = VendorID, 1018sub4 = SerialNumber, TPDOs 0x2021sub1 to 0x2021sub8
+    
+    # sick_line_guidance configuration of this node:
+    sick_device_family: "OLS20"            # can devices currently supported: "OLS10", "OLS20" or "MLS"
+    sick_topic: "olsB"                     # OLS_Measurement messages are published in topic "/ols"
+    sick_frame_id: "ols20B_measurement_frame" # OLS_Measurement messages are published with frame_id "ols_measurement_frame"
+

Devices/Packages/sick_line_guidance/package.xml

@@ -0,0 +1,81 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>sick_line_guidance</name>
+  <version>0.0.0</version>
+  <description>SICK Line Guidance ROS Support</description>
+
+  <!-- One maintainer tag required, multiple allowed, one person per tag -->
+  <!-- Example:  -->
+  <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+  <maintainer email="ros.test@lehning.de">rostest</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>Apache</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/sick_line_guidance</url> -->
+  <url type="website">https://github.com/SICKAG/sick_line_guidance</url>
+  <url type="website">https://www.sick.com/ols</url>
+  <url type="website">https://www.sick.com/mls</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> -->
+  <author email="ros.test@lehning.de">Michael Lehning</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>cmake_modules</build_depend>
+  <build_depend>roscpp</build_depend>
+  <build_depend>rospy</build_depend>
+  <build_depend>std_msgs</build_depend>
+  <build_depend>sensor_msgs</build_depend>
+  <build_depend>message_generation</build_depend>
+  <build_depend>random_numbers</build_depend>
+  <build_depend>can_msgs</build_depend>
+  <build_depend>canopen_chain_node</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>sensor_msgs</build_export_depend>
+  <build_export_depend>message_generation</build_export_depend>
+  <build_export_depend>random_numbers</build_export_depend>
+  <build_export_depend>can_msgs</build_export_depend>
+  <build_export_depend>canopen_chain_node</build_export_depend>
+  <exec_depend>roscpp</exec_depend>
+  <exec_depend>rospy</exec_depend>
+  <exec_depend>std_msgs</exec_depend>
+  <exec_depend>message_runtime</exec_depend>
+  <exec_depend>random_numbers</exec_depend>
+  <exec_depend>can_msgs</exec_depend>
+  <exec_depend>canopen_chain_node</exec_depend>
+
+  <!-- The export tag contains other, unspecified, tags -->
+  <export>
+    <!-- Other tools can request additional information be placed here -->
+  </export>
+</package>

Devices/Packages/sick_line_guidance/src/sick_canopen_simu_canstate.cpp

@@ -0,0 +1,133 @@
+/*
+ * sick_canopen_simu_canstate implements a state engine for can.
+ *
+ * Depending on input messages of type can_msgs::Frame,
+ * the current state is switched between INITIALIZATION,
+ * PRE_OPERATIONAL, OPERATIONAL and STOPPED.
+ *
+ * class CanState: handles can nmt messages and implements the state engine for can.
+ * 
+ * Copyright (C) 2019 Ing.-Buero Dr. Michael Lehning, Hildesheim
+ * Copyright (C) 2019 SICK AG, Waldkirch
+ * 
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ * 
+ *       http://www.apache.org/licenses/LICENSE-2.0
+ *
+ *   Unless required by applicable law or agreed to in writing, software
+ *   distributed under the License is distributed on an "AS IS" BASIS,
+ *   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ *   See the License for the specific language governing permissions and
+ *   limitations under the License.
+ *
+ * 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 SICK AG nor the names of its
+ *       contributors may be used to endorse or promote products derived from
+ *       this software without specific prior written permission
+ *     * Neither the name of Ing.-Buero Dr. Michael Lehning 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.
+ *
+ *      Authors:
+ *         Michael Lehning <michael.lehning@lehning.de>
+ *
+ *  Copyright 2019 SICK AG
+ *  Copyright 2019 Ing.-Buero Dr. Michael Lehning
+ * 
+ */
+#include <ros/ros.h>
+#include <can_msgs/Frame.h>
+#include "sick_line_guidance/sick_canopen_simu_canstate.h"
+
+/*
+ * Constructor.
+ *
+ * @param[in] can_node_id node id of OLS or MLS, default: 0x0A
+ */
+sick_canopen_simu::CanState::CanState(int can_node_id) : m_can_node_id(can_node_id), m_can_state(INITIALIZATION)
+{
+}
+
+/*
+ * Destructor.
+ */
+sick_canopen_simu::CanState::~CanState()
+{
+}
+
+/*
+ * @brief Callbacks for ros messages. Switches the state, and returns true, if msg_in is a can nmt message.
+ * Otherwise, false is returned.
+ *
+ * param[in] msg_in input message of type can_msgs::Frame
+ * param[out] msg_out output message of type can_msgs::Frame (if input message requires a response to send)
+ * param[out] send_msg true, if input message msg_in requires the response message msg_out to be send
+ *
+ * @return true, if input message handled, otherwise false.
+ */
+bool sick_canopen_simu::CanState::messageHandler(const can_msgs::Frame &msg_in, can_msgs::Frame & msg_out, bool & send_msg)
+{
+  bool msg_handled = false;
+  send_msg = false;
+  // 000#... (msg_in.id == 0 && msg_in.dlc >= ): nmt message from can master (network manager) with 2 byte (command and nodeid)
+  // msg_in.data[0] : nmt command (0x1, 0x2, 0x80, 0x81 or 0x82)
+  // msg_in.data[1] : nodeid, message has to be handled if nodeid==0 (broadcast to all devices), or nodeid==m_can_node_id
+  if (msg_in.id == 0 && msg_in.dlc >= 2 && (msg_in.data[1] == 0 || msg_in.data[1] == m_can_node_id))
+  {
+    msg_out = msg_in;
+    switch (msg_in.data[0])
+    {
+      case 0x80: // 000#0x80...: Pre-operational -> switch to PRE_OPERATIONAL and send bootup message (0x700+$NODEID)#0x00
+      case 0x81: // 000#0x81...: Reset node -> switch to PRE_OPERATIONAL and send bootup message (0x700+$NODEID)#0x00
+      case 0x82: // 000#0x82...: Reset communication -> switch to PRE_OPERATIONAL and send bootup message (0x700+$NODEID)#0x00
+        msg_out.id = 0x700 + m_can_node_id;
+        msg_out.dlc = 1;
+        msg_out.data[0] = 0;
+        msg_out.header.stamp = ros::Time::now();
+        send_msg = true; // msg_out contains the bootup message to send
+        m_can_state = PRE_OPERATIONAL;
+        ROS_INFO_STREAM("sick_canopen_simu::CanState::messageHandler(): switched to state PRE_OPERATIONAL.");
+        msg_handled = true;
+        break;
+  
+      case 0x01: // 000#0x01: switch to OPERATIONAL
+        m_can_state = OPERATIONAL;
+        ROS_INFO_STREAM("sick_canopen_simu::CanState::messageHandler(): switched to state OPERATIONAL.");
+        msg_handled = true;
+        break;
+  
+      case 0x02: // 000#0x02: switch to STOPPED
+        m_can_state = STOPPED;
+        ROS_INFO_STREAM("sick_canopen_simu::CanState::messageHandler(): switched to state STOPPED.");
+        msg_handled = true;
+        break;
+
+      default:
+        break;
+    }
+  }
+  return msg_handled;
+}

Devices/Packages/sick_line_guidance/src/sick_canopen_simu_device.cpp

@@ -0,0 +1,763 @@
+/*
+ * Class SimulatorBase implements the base class for simulation of SICK can devices (OLS20 and MLS).
+ *
+ * ROS messages of type can_msgs::Frame are read from ros topic "can0",
+ * handled to simulate an OLS20 or MLS device, and resulting can_msgs::Frame
+ * messages are published on ros topic "ros2can0".
+ *
+ * MsgType can be sick_line_guidance::OLS_Measurement (for simulation of OLS devices), or
+ * or sick_line_guidance::MLS_Measurement (for simulation of MLS devices).
+ *
+ * Assumption: ros nodes sick_line_guidance_ros2can_node and sick_line_guidance_can2ros_node
+ * have to be running to convert ros messages from and to canbus messages.
+ * sick_line_guidance_ros2can_node converts ros messages of type can_msgs::Frame to can frames,
+ * sick_line_guidance_can2ros_node converts can frames to ros messages of type can_msgs::Frame.
+ *
+ * Subclass MLSSimulator extends class SimulatorBase to simulate an MLS device.
+ *
+ * Subclass OLS20Simulator extends class SimulatorBase to simulate an OLS20 device.
+ * 
+ * Copyright (C) 2019 Ing.-Buero Dr. Michael Lehning, Hildesheim
+ * Copyright (C) 2019 SICK AG, Waldkirch
+ * 
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ * 
+ *       http://www.apache.org/licenses/LICENSE-2.0
+ *
+ *   Unless required by applicable law or agreed to in writing, software
+ *   distributed under the License is distributed on an "AS IS" BASIS,
+ *   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ *   See the License for the specific language governing permissions and
+ *   limitations under the License.
+ *
+ * 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 SICK AG nor the names of its
+ *       contributors may be used to endorse or promote products derived from
+ *       this software without specific prior written permission
+ *     * Neither the name of Ing.-Buero Dr. Michael Lehning 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.
+ *
+ *      Authors:
+ *         Michael Lehning <michael.lehning@lehning.de>
+ *
+ *  Copyright 2019 SICK AG
+ *  Copyright 2019 Ing.-Buero Dr. Michael Lehning
+ * 
+ */
+#include <boost/algorithm/clamp.hpp>
+#include <ros/ros.h>
+#include "sick_line_guidance/sick_canopen_simu_device.h"
+#include "sick_line_guidance/sick_line_guidance_msg_util.h"
+
+/*
+ * Constructor
+ *
+ * @param[in] nh ros node handle
+ * @param[in] config_file configuration file with testcases for OLS and MLS simulation
+ * @param[in] can_node_id node id of OLS or MLS, default: 0x0A
+ * @param[in] subscribe_topic ros topic to receive input messages of type can_msgs::Frame, default: "can0"
+ * @param[in] publish_topic ros topic to publish output messages of type can_msgs::Frame, default: "ros2can0"
+ * @param[in] pdo_rate rate of PDO (default: pdo_rate = 50, i.e. 20 ms between two PDOs or 50 PDOs per second)
+ * @param[in] pdo_repeat_cnt each sensor state spefied in sick_canopen_simu_cfg.xml is repeated N times before switching to the next state (default: 5 times)
+ * @param[in] subscribe_queue_size buffer size for ros messages
+ */
+template<class MsgType>
+sick_canopen_simu::SimulatorBase<MsgType>::SimulatorBase(ros::NodeHandle & nh, const std::string & config_file, int can_node_id, const std::string & subscribe_topic, const std::string & publish_topic, const ros::Rate & pdo_rate, int pdo_repeat_cnt, int subscribe_queue_size)
+: m_can_state(can_node_id), m_can_node_id(can_node_id), m_pdo_rate(pdo_rate), m_pdo_repeat_cnt(pdo_repeat_cnt), m_pdo_publisher_thread(0), m_pdo_publisher_thread_running(false), m_subscribe_queue_size(subscribe_queue_size), m_send_tpdo_immediately(false)
+{
+  m_sdo_response_dev_state = 0x4F18200000000000; // response to sdo request for dev_status (object 0x2018): MLS and OLS20: 0x4F18200000000000 (sdo response with UINT8 data), OLS10: 0x4B18200000000000 (sdo response with UINT16 data)
+  m_ros_publisher = nh.advertise<can_msgs::Frame>(publish_topic, 1);
+  if(!readSDOconfig(config_file))
+  {
+    ROS_ERROR_STREAM("SimulatorBase: readSDOconfig(" << config_file << ") failed");
+  }
+}
+
+/*
+ * Destructor
+ *
+ */
+template<class MsgType>
+sick_canopen_simu::SimulatorBase<MsgType>::~SimulatorBase()
+{
+  m_pdo_publisher_thread_running = false;
+  if(m_pdo_publisher_thread)
+  {
+    m_pdo_publisher_thread->join();
+    delete(m_pdo_publisher_thread);
+    m_pdo_publisher_thread = 0;
+  }
+}
+
+/*
+ * @brief Callbacks for ros messages. Converts incoming messages of type can_msgs::Frame to simulate a can device
+ * and publishes simulation results to the configured ros topic.
+ *
+ * param[in] msg ros message of type can_msgs::Frame
+ */
+template<class MsgType>
+void sick_canopen_simu::SimulatorBase<MsgType>::messageHandler(const can_msgs::Frame & msg_in)
+{
+  // Handle NMT messages
+  bool send_msg = false;
+  can_msgs::Frame msg_out = msg_in;
+  if(m_can_state.messageHandler(msg_in, msg_out, send_msg))
+  {
+    if(send_msg)
+    {
+      boost::lock_guard<boost::mutex> publish_lockguard(m_ros_publisher_mutex);
+      m_ros_publisher.publish(msg_out);
+    }
+    return; // nmt message handled in m_can_state
+  }
+  // Ignore sync frames and bootup messages (node guarding frames) sent by can devices
+  if((msg_in.id == 0x80) || (msg_in.id >= 0x700 && msg_in.id <= 0x77F))
+  {
+    return;
+  }
+  // Ignore TPDO1 and TPDO2 frames sent by can devices
+  if((msg_in.id >= 0x180 && msg_in.id <= 0x1FF) || (msg_in.id >= 0x280 && msg_in.id <= 0x2FF))
+  {
+    return;
+  }
+  // Handle SDOs
+  if(handleSDO(msg_in))
+  {
+    return; // SDO request handled, SDO response sent
+  }
+  ROS_ERROR_STREAM("SimulatorBase::messageHandler(): message " << tostring(msg_in) << " not handled");
+}
+
+
+/*
+ * @brief Registers a listener to a simulation. Whenever the simulation sends a PDO, the listener is notified about the current sensor state.
+ * After receiving both the sensor state and the following OLS/MLS-Measurement ros message from the driver, the listener can check
+ * the correctness by comparing the sensor state from simulation and the OLS/MLS-Measurement from the driver. Both must be identical,
+ * otherwise some failure occured.
+ *
+ * param[in] pListener listener to current sensor states sent by PDO.
+ */
+template<class MsgType>
+void sick_canopen_simu::SimulatorBase<MsgType>::registerSimulationListener(sick_canopen_simu::SimulatorBase<MsgType>::SimulationListener* pListener)
+{
+  m_vec_simu_listener.push_back(pListener); // append pListener to the list of registered listeners
+}
+
+/*
+ * @brief Unregisters a listener from a simulation. The listener will not be notified about simulated sensor states.
+ * This function is the opposite to registerSimulationListener.
+ *
+ * param[in] pListener listener to current sensor states sent by PDO.
+ */
+template<class MsgType>
+void sick_canopen_simu::SimulatorBase<MsgType>::unregisterSimulationListener(sick_canopen_simu::SimulatorBase<MsgType>::SimulationListener* pListener)
+{
+  for(typename std::vector<SimulationListener*>::iterator iter = m_vec_simu_listener.begin(); iter != m_vec_simu_listener.end(); iter++)
+  {
+    if(*iter == pListener)
+    {
+      m_vec_simu_listener.erase(iter); // remove pListener from the list of registered listeners
+      break;
+    }
+  }
+}
+
+/*
+ * reads SDO configuration from xml-file
+ *
+ * @param[in] config_file configuration file with testcases for OLS and MLS simulation
+ */
+template<class MsgType>
+bool sick_canopen_simu::SimulatorBase<MsgType>::readSDOconfig(const std::string & config_file)
+{
+  try
+  {
+    TiXmlDocument xml_config(config_file);
+    if(xml_config.LoadFile())
+    {
+      ROS_INFO_STREAM("SimulatorBase::readSDOconfig(): reading SDO map from xml-file " << config_file);
+      TiXmlElement* xml_sdo_config = xml_config.FirstChild("sick_canopen_simu")->FirstChild("SDO_config")->ToElement();
+      if(xml_sdo_config)
+      {
+        TiXmlElement* xml_sdo = xml_sdo_config->FirstChildElement("sdo");
+        while(xml_sdo)
+        {
+          const std::string sdo_request = xml_sdo->Attribute("request");
+          const std::string sdo_response = xml_sdo->Attribute("response");
+          uint64_t u64_sdo_request = std::stoull(sdo_request, 0, 0);
+          uint64_t u64_sdo_response = std::stoull(sdo_response, 0, 0);
+          m_sdo_request_response_map[u64_sdo_request] = u64_sdo_response;
+          xml_sdo = xml_sdo->NextSiblingElement("sdo");
+          ROS_DEBUG_STREAM("SimulatorBase::readSDOconfig(): sdo_request_response_map[0x" << std::hex << u64_sdo_request << "] = 0x" << std::hex << u64_sdo_response
+            << " (sdo_request_response_map[\"" << sdo_request << "\"] = \"" << sdo_response<< "\")");
+        }
+        if(!m_sdo_request_response_map.empty())
+        {
+          ROS_INFO_STREAM("SimulatorBase::readSDOconfig(" << config_file << "): " << m_sdo_request_response_map.size() << " sdo elements found");
+          return true;
+        }
+        else
+        {
+          ROS_ERROR_STREAM("SimulatorBase::readSDOconfig(" << config_file << "): no sdo elements found");
+        }
+      }
+      else
+      {
+        ROS_ERROR_STREAM("SimulatorBase::readSDOconfig(" << config_file << "): can't read element sick_canopen_simu/SDO_config");
+      }
+    }
+    else
+    {
+      ROS_ERROR_STREAM("SimulatorBase: can't read xml-file " << config_file);
+    }
+  }
+  catch(const std::exception & exc)
+  {
+    ROS_ERROR_STREAM("SimulatorBase::readSDOconfig("<< config_file << ") failed: exception " << exc.what());
+  }
+  return false;
+}
+
+/*
+ * reads the PDO configuration from xml-file
+ *
+ * @param[in] config_file configuration file with testcases for simulation
+ * @param[in] sick_device_family "OLS10", OLS20" or "MLS"
+ */
+template<class MsgType>
+bool sick_canopen_simu::SimulatorBase<MsgType>::readPDOconfig(const std::string & config_file, const std::string & sick_device_family)
+{
+  try
+  {
+    TiXmlDocument xml_config(config_file);
+    if(xml_config.LoadFile())
+    {
+      ROS_INFO_STREAM("SimulatorBase::readPDOconfig(): reading PDO map from xml-file " << config_file);
+      TiXmlElement* xml_device_config = xml_config.FirstChild("sick_canopen_simu")->FirstChild(sick_device_family)->ToElement();
+      if(!xml_device_config)
+      {
+        ROS_ERROR_STREAM("SimulatorBase::readPDOconfig(" << config_file << "): element sick_canopen_simu/" << sick_device_family << " not found");
+        return false;
+      }
+      TiXmlElement* xml_pdo_config = xml_device_config->FirstChildElement("PDO_config")->ToElement();
+      if(xml_pdo_config)
+      {
+        TiXmlElement* xml_pdo = xml_pdo_config->FirstChildElement("pdo");
+        while(xml_pdo)
+        {
+          parseXmlPDO(xml_pdo);
+          xml_pdo = xml_pdo->NextSiblingElement("pdo");
+          ROS_DEBUG_STREAM("SimulatorBase::readPDOconfig(): " << sick_line_guidance::MsgUtil::toInfo(m_vec_pdo_measurements.back()));
+        }
+        if(!m_vec_pdo_measurements.empty())
+        {
+          ROS_INFO_STREAM("SimulatorBase::readPDOconfig(" << config_file << "): " << m_vec_pdo_measurements.size() << " pdo elements found");
+          return true;
+        }
+        else
+        {
+          ROS_ERROR_STREAM("SimulatorBase::readPDOconfig(" << config_file << "): no pdo elements found");
+        }
+      }
+      else
+      {
+        ROS_ERROR_STREAM("SimulatorBase::readPDOconfig(" << config_file << "): can't read element sick_canopen_simu/" << sick_device_family << "/PDO_config");
+      }
+    }
+    else
+    {
+      ROS_ERROR_STREAM("SimulatorBase::readPDOconfig(): can't read xml-file " << config_file);
+    }
+  }
+  catch(const std::exception & exc)
+  {
+    ROS_ERROR_STREAM("SimulatorBase::readPDOconfig("<< config_file << ") failed: exception " << exc.what());
+  }
+  return false;
+}
+
+/*
+ * @brief handles SDOs, i.e. sends a SDO response and returns true, if can frame is a SDO request.
+ * Otherwise, the can frame is not handled and false is returned.
+ * Note: The SDO response is simply taken from a lookup-table (input: SDO request frame data, output: SDO response frame data).
+ * If the SDO request frame data (frame.data) is not found in the lookup-table, the can frame is not handled and false is returned, too.
+ *
+ * @param[in] msg_in received can frame
+ *
+ * @return true if can frame was handled and a SDO request is sent, false otherwise.
+ */
+template<class MsgType>
+bool sick_canopen_simu::SimulatorBase<MsgType>::handleSDO(const can_msgs::Frame & msg_in)
+{
+  // can id == (0x600+$NODEID) with 8 byte data => SDO request => send SDO response (0x580+$NODEID)#...
+  if(msg_in.id == 0x600 + m_can_node_id && msg_in.dlc == 8)
+  {
+    boost::lock_guard<boost::mutex> publish_lockguard(m_ros_publisher_mutex);
+    can_msgs::Frame msg_out = msg_in;
+    uint64_t sdo_request_data = frameDataToUInt64(msg_in); // convert msg_in.data to uint64_t
+    uint64_t sdo_response_data = m_sdo_request_response_map[sdo_request_data]; // lookup table: sdo response data := m_sdo_request_response_map[<sdo request data>]
+    if(sdo_response_data != 0) // SDO response found in lookup table
+    {
+      // send SDO response (0x580+$NODEID)#<sdo_response_data>
+      msg_out.id = 0x580 + m_can_node_id;
+      uint64ToFrameData(sdo_response_data, msg_out);
+      msg_out.header.stamp = ros::Time::now();
+      m_ros_publisher.publish(msg_out);
+      ROS_INFO_STREAM("SimulatorBase::handleSDO(): SDO request " << tostring(msg_in) << " handled, SDO response " << tostring(msg_out) << " published.");
+      return true;
+    }
+    else if( (sdo_response_data = m_sdo_request_response_map[(sdo_request_data & 0xFFFFFFFF00000000ULL)]) != 0)
+    {
+      // Handle SDO response for a download request, f.e. SDO request = 0x23022003CDCC4C3D = 0x2302200300000000 + <4 byte data>:
+      // Set value 0xCDCC4C3D in object [2002sub03] -> SDO response = 0x6002200300000000, SDO request = 0x4002200300000000 -> SDO response = 0x43022003CDCC4C3D
+      uint64_t sdo_object_index = (sdo_request_data & 0x00FFFFFF00000000ULL);
+      uint64_t sdo_object_value = (sdo_request_data & 0x00000000FFFFFFFFULL);
+      uint64_t sdo_parameter_bytes = (sdo_request_data & 0x0F00000000000000ULL);
+      m_sdo_request_response_map[0x4000000000000000 | sdo_object_index] = (0x4000000000000000 | sdo_parameter_bytes | sdo_object_index | sdo_object_value);
+      // send SDO response (0x580+$NODEID)#<sdo_response_data>
+      msg_out.id = 0x580 + m_can_node_id;
+      uint64ToFrameData(sdo_response_data, msg_out);
+      msg_out.header.stamp = ros::Time::now();
+      m_ros_publisher.publish(msg_out);
+      ROS_INFO_STREAM("SimulatorBase::handleSDO(): SDO request " << tostring(msg_in) << " handled, SDO response " << tostring(msg_out) << " published.");
+      return true;
+    }
+    else
+    {
+      ROS_ERROR_STREAM("SimulatorBase::handleSDO(): SDO " << tostring(msg_in) << " not handled");
+      return false;
+    }
+  }
+  // Ignore SDO requests (0x600+$NODEID) or SDO responses (0x580+$NODEID) from other can devices
+  if((msg_in.id >= 0x600 && msg_in.id <= 0x67F) || (msg_in.id >= 0x580 && msg_in.id <= 0x5FF))
+  {
+    return true;
+  }
+  return false;
+}
+
+/*
+ * @brief Publishes PDOs to simulate a MLS or OLS20 device while can state is operational
+ */
+template<class MsgType>
+void sick_canopen_simu::SimulatorBase<MsgType>::runPDOthread(void)
+{
+  size_t pdo_cnt = 0;
+  while(ros::ok() && m_pdo_publisher_thread_running)
+  {
+    m_pdo_rate.sleep(); // ros::Duration(0.01).sleep();
+    if(m_can_state.state() == OPERATIONAL || m_send_tpdo_immediately ) // OLS10, MLS: send TPDOs immediately in all states (pre-operational and operational), OLS20: send TPDOs only in state operational
+    {
+      // Note: Depending on the transmission type, PDOs are transmitted either asynchronously or synchronously (https://www.canopensolutions.com/english/about_canopen/pdo.shtml):
+      // "Asynchronous PDOs are event-controlled ... when at least one of the process variables mapped in a PDO is altered, for example an input value, the PDO is immediately transmitted."
+      // "Synchronous PDOs are only transmitted after prior reception of a synchronization message (Sync Object)".
+      // Default transmission type for OLS and MLS is 0xFF (asynchronous).
+      boost::lock_guard<boost::mutex> publish_lockguard(m_ros_publisher_mutex);
+      can_msgs::Frame tpdo_msg[2];
+      int measurement_idx = ((pdo_cnt/m_pdo_repeat_cnt) % (m_vec_pdo_measurements.size())); // simulate a different measurement every 500 milliseconds (25 times, 20 milliseconds each)
+      MsgType & pdo_measurement = m_vec_pdo_measurements[measurement_idx];
+      // convert sensor measurement to can frames
+      int tpdo_msg_cnt = convertToCanFrame(pdo_measurement, tpdo_msg[0], tpdo_msg[1]);
+      // send can frames
+      for(int n = 0; n < tpdo_msg_cnt; n++)
+      {
+        tpdo_msg[n].header.stamp = ros::Time::now();
+        m_ros_publisher.publish(tpdo_msg[n]);
+        ros::Duration(0.001).sleep();
+        ROS_INFO_STREAM("SimulatorBase::runPDOthread(): pdo frame " << tostring(tpdo_msg[n]) << " published.");
+      }
+      // Notify all registered listener
+      for(typename std::vector<SimulationListener*>::iterator iter = m_vec_simu_listener.begin(); iter != m_vec_simu_listener.end(); iter++)
+      {
+        (*iter)->pdoSent(pdo_measurement);
+      }
+      pdo_cnt++;
+    }
+  }
+  m_pdo_publisher_thread_running = false;
+}
+
+/*
+ * @brief Convertes an MLS_Measurement into a can_msgs::Frame TPDO1.
+ * @param[in] measurement MLS_Measurement to be converted
+ * @param[out] tpdo1_msg output can frame TPDO1,
+ * @param[out] tpdo2_msg dummy frame TPDO2, unused
+ * @return Returns the number of TPDOs, i.e. 1 for MLS devices.
+ */
+template<class MsgType>
+int sick_canopen_simu::SimulatorBase<MsgType>::convertToCanFrame(const sick_line_guidance::MLS_Measurement & measurement, can_msgs::Frame & tpdo1_msg, can_msgs::Frame & tpdo2_msg)
+{
+  assert(tpdo1_msg.data.size() == 8);
+  assert(measurement.position.size() == 3);
+  // convert position and width in meter to lcp and width in millimeter
+  int lcp1 = convertLCP(measurement.position[0]);
+  int lcp2 = convertLCP(measurement.position[1]);
+  int lcp3 = convertLCP(measurement.position[2]);
+  // set TPDO1: (180+$NODEID)#<8 byte data>
+  tpdo1_msg.id = 0x180 + m_can_node_id;
+  tpdo1_msg.dlc = 8;
+  tpdo1_msg.is_error = false;
+  tpdo1_msg.is_rtr = false;
+  tpdo1_msg.is_extended = false;
+  tpdo1_msg.data[0] = (lcp1 & 0xFF);        // LSB LCP1
+  tpdo1_msg.data[1] = ((lcp1 >> 8) & 0xFF); // MSB LCP1
+  tpdo1_msg.data[2] = (lcp2 & 0xFF);        // LSB LCP2
+  tpdo1_msg.data[3] = ((lcp2 >> 8) & 0xFF); // MSB LCP2
+  tpdo1_msg.data[4] = (lcp3 & 0xFF);        // LSB LCP3
+  tpdo1_msg.data[5] = ((lcp3 >> 8) & 0xFF); // MSB LCP3
+  tpdo1_msg.data[6] = measurement.lcp;
+  tpdo1_msg.data[7] = measurement.status;
+  tpdo1_msg.header.stamp = ros::Time::now();
+  // set error register 1001 in object dictionary
+  uint32_t sdo_data_error = ((measurement.error & 0xFFUL) << 24);
+  m_sdo_request_response_map[0x4001100000000000] = (0x4F01100000000000 | (sdo_data_error & 0xFF000000ULL)); // measurement.error -> set 0x1001 in object dictionary
+  return 1; // one TPDO set
+}
+
+/*
+ * @brief Convertes an OLS_Measurement into two can_msgs::Frame TPDO1 and TPDO2.
+ * @param[in] measurement OLS_Measurement to be converted
+ * @param[out] tpdo1_msg output can frame TPDO1, Byte 1-8 := LCP1(LSB,MSB,0x2021sub1), LCP2(LSB,MSB,0x2021sub2), LCP3(LSB,MSB,0x2021sub3), status(UINT8,0x2021sub4), barcode(UINT8,0x2021sub8)
+ * @param[out] tpdo2_msg output can frame TPDO2, Byte 1-6 := Width1(LSB,MSB,0x2021sub5), Width2(LSB,MSB,0x2021sub6), Width3(LSB,MSB,0x2021sub7)
+ * @return Returns the number of TPDOs, i.e. 2 for OLS devices.
+ */
+template<class MsgType>
+int sick_canopen_simu::SimulatorBase<MsgType>::convertToCanFrame(const sick_line_guidance::OLS_Measurement & measurement, can_msgs::Frame & tpdo1_msg, can_msgs::Frame & tpdo2_msg)
+{
+  assert(tpdo1_msg.data.size() == 8);
+  assert(tpdo2_msg.data.size() == 8);
+  assert(measurement.position.size() == 3);
+  assert(measurement.width.size() == 3);
+  assert(revertByteorder<uint32_t>(0x12345678) == 0x78563412);
+  // convert position and width in meter to lcp and width in millimeter
+  int lcp1 = convertLCP(measurement.position[0]);
+  int lcp2 = convertLCP(measurement.position[1]);
+  int lcp3 = convertLCP(measurement.position[2]);
+  int width1 = convertLCP(measurement.width[0]);
+  int width2 = convertLCP(measurement.width[1]);
+  int width3 = convertLCP(measurement.width[2]);
+  // set TPDO1: (180+$NODEID)#<8 byte data>
+  tpdo1_msg.id = 0x180 + m_can_node_id;
+  tpdo1_msg.dlc = 8;
+  tpdo1_msg.is_error = false;
+  tpdo1_msg.is_rtr = false;
+  tpdo1_msg.is_extended = false;
+  tpdo1_msg.data[0] = (lcp1 & 0xFF);        // LSB LCP1
+  tpdo1_msg.data[1] = ((lcp1 >> 8) & 0xFF); // MSB LCP1
+  tpdo1_msg.data[2] = (lcp2 & 0xFF);        // LSB LCP2
+  tpdo1_msg.data[3] = ((lcp2 >> 8) & 0xFF); // MSB LCP2
+  tpdo1_msg.data[4] = (lcp3 & 0xFF);        // LSB LCP3
+  tpdo1_msg.data[5] = ((lcp3 >> 8) & 0xFF); // MSB LCP3
+  tpdo1_msg.data[6] = measurement.status;
+  tpdo1_msg.data[7] = measurement.barcode;
+  tpdo1_msg.header.stamp = ros::Time::now();
+  // set TPDO2: (280+$NODEID)#<6 byte data>
+  tpdo2_msg.id = 0x280 + m_can_node_id;
+  tpdo2_msg.dlc = 6;
+  tpdo2_msg.is_error = false;
+  tpdo2_msg.is_rtr = false;
+  tpdo2_msg.is_extended = false;
+  tpdo2_msg.data[0] = (width1 & 0xFF);        // LSB width1
+  tpdo2_msg.data[1] = ((width1 >> 8) & 0xFF); // MSB width1
+  tpdo2_msg.data[2] = (width2 & 0xFF);        // LSB width2
+  tpdo2_msg.data[3] = ((width2 >> 8) & 0xFF); // MSB width2
+  tpdo2_msg.data[4] = (width3 & 0xFF);        // LSB width3
+  tpdo2_msg.data[5] = ((width3 >> 8) & 0xFF); // MSB width3
+  tpdo2_msg.data[6] = 0;
+  tpdo2_msg.data[7] = 0;
+  tpdo2_msg.header.stamp = ros::Time::now();
+  // set objects in dictionary
+  uint32_t sdo_data_error = ((measurement.error & 0xFFUL) << 24);
+  uint32_t sdo_data_dev_status = ((measurement.dev_status & 0xFFUL) << 24);
+  uint32_t sdo_data_extended_code = revertByteorder<uint32_t>(measurement.extended_code);
+  m_sdo_request_response_map[0x4001100000000000] = (0x4F01100000000000 | (sdo_data_error & 0xFF000000ULL));            // measurement.error -> set 0x1001 in object dictionary
+  m_sdo_request_response_map[0x4018200000000000] = (m_sdo_response_dev_state | (sdo_data_dev_status & 0xFF000000ULL)); // measurement.dev_status -> set 0x2018 in object dictionary (OLS20: UINT8, OLS10: UINT16)
+  m_sdo_request_response_map[0x4021200900000000] = (0x4321200900000000 | (sdo_data_extended_code & 0xFFFFFFFFULL));    // measurement.extended_code -> set 0x2021sub9 in object dictionary
+  // OLS20 only: simulate barcode center point, object 0x2021subA (INT16), OLS10: always 0
+  int16_t barcodecenter = (int16_t)(measurement.barcode_center_point * 1000);
+  uint32_t sdo_data = ((barcodecenter & 0xFFUL) << 24) | ((barcodecenter & 0xFF00UL) << 8);
+  m_sdo_request_response_map[0x4021200A00000000] = (0x4B21200A00000000 | (sdo_data & 0xFFFF0000ULL));
+  // OLS20 only: simulate quality of lines, object 0x2021subB (UINT8), OLS10: always 0
+  sdo_data = ((measurement.quality_of_lines & 0xFFUL) << 24);
+  m_sdo_request_response_map[0x4021200B00000000] = (0x4F21200B00000000 | (sdo_data & 0xFF000000ULL));
+  // OLS20 only: simulate intensity of lines, object 0x2023sub1 to 0x2023sub3 (UINT8), OLS10: always 0
+  sdo_data = ((measurement.intensity_of_lines[0] & 0xFFUL) << 24);
+  m_sdo_request_response_map[0x4023200100000000] = (0x4F23200100000000 | (sdo_data & 0xFF000000ULL));
+  sdo_data = ((measurement.intensity_of_lines[1] & 0xFFUL) << 24);
+  m_sdo_request_response_map[0x4023200200000000] = (0x4F23200200000000 | (sdo_data & 0xFF000000ULL));
+  sdo_data = ((measurement.intensity_of_lines[2] & 0xFFUL) << 24);
+  m_sdo_request_response_map[0x4023200300000000] = (0x4F23200300000000 | (sdo_data & 0xFF000000ULL));
+  return 2; // both TPDOs set
+}
+
+/*
+ * @brief Converts a position or width (float value in meter) to lcp (INT16 value in millimeter),
+ * shortcut for std::round(lcp * 1000) with clipping to range INT16_MIN ... INT16_MAX.
+ * @param[in] lcp position or width (float value in meter)
+ * @return INT16 value in millimeter
+ */
+template<class MsgType>
+int sick_canopen_simu::SimulatorBase<MsgType>::convertLCP(float lcp)
+{
+  return boost::algorithm::clamp((int)(std::round(lcp * 1000)), INT16_MIN, INT16_MAX);
+}
+
+/*
+ * @brief Converts frame.data to uint64_t
+ * @param[in] frame can frame
+ * @return frame.data converted to uint64_t
+ */
+template<class MsgType>
+uint64_t sick_canopen_simu::SimulatorBase<MsgType>::frameDataToUInt64(const can_msgs::Frame & frame)
+{
+  assert(frame.data.size() == 8);
+  uint64_t u64data = 0;
+  for(size_t n = 0; n < std::min(frame.data.size(), (size_t)(frame.dlc & 0xFF)); n++)
+  {
+    u64data = ((u64data << 8) | (frame.data[n] & 0xFF));
+  }
+  return u64data;
+}
+
+/*
+ * @brief Converts uint64_t data to frame.data
+ * @param[in] u64data frame data (uint64_t)
+ * @param[in+out] frame can frame
+ */
+template<class MsgType>
+void sick_canopen_simu::SimulatorBase<MsgType>::uint64ToFrameData(uint64_t u64data, can_msgs::Frame & frame)
+{
+  assert(frame.data.size() == 8);
+  frame.dlc = 8;
+  for(int n = frame.data.size() - 1; n >= 0; n--)
+  {
+    frame.data[n] = (u64data & 0xFF);
+    u64data = (u64data >> 8);
+  }
+}
+
+/*
+ * @brief prints and returns a can_msgs::Frame in short format like candump (f.a. "18A#B4FFCCFF00000300")
+ */
+template<class MsgType>
+std::string sick_canopen_simu::SimulatorBase<MsgType>::tostring(const can_msgs::Frame & canframe)
+{
+  std::stringstream str;
+  str << std::uppercase << std::hex << std::setfill('0') << std::setw(3) << (unsigned)(canframe.id) << "#";
+  for(size_t n = 0; n < std::min((size_t)(canframe.dlc & 0xFF), canframe.data.size()); n++)
+    str << std::uppercase << std::hex << std::setfill('0') << std::setw(2) << (unsigned)(canframe.data[n] & 0xFF);
+  return str.str();
+}
+
+/*
+ * Subclass MLSSimulator extends class SimulatorBase to simulate a MLS device.
+ *
+ */
+ 
+/*
+ * Constructor
+ *
+ * @param[in] nh ros node handle
+ * @param[in] config_file configuration file with testcases for OLS and MLS simulation
+ * @param[in] sick_device_family "OLS10", "OLS20" or "MLS"
+ * @param[in] can_node_id node id of OLS or MLS, default: 0x0A
+ * @param[in] subscribe_topic ros topic to receive input messages of type can_msgs::Frame, default: "can0"
+ * @param[in] publish_topic ros topic to publish output messages of type can_msgs::Frame, default: "ros2can0"
+ * @param[in] pdo_rate rate of PDO (default: pdo_rate = 50, i.e. 20 ms between two PDOs or 50 PDOs per second)
+ * @param[in] pdo_repeat_cnt each sensor state spefied in sick_canopen_simu_cfg.xml is repeated N times before switching to the next state (default: 5 times)
+ * @param[in] subscribe_queue_size buffer size for ros messages
+ */
+sick_canopen_simu::MLSSimulator::MLSSimulator(ros::NodeHandle & nh, const std::string & config_file, const std::string & sick_device_family, int can_node_id, const std::string & subscribe_topic, const std::string & publish_topic, const ros::Rate & pdo_rate, int pdo_repeat_cnt, int subscribe_queue_size)
+  : SimulatorBase(nh, config_file, can_node_id, subscribe_topic, publish_topic, pdo_rate, pdo_repeat_cnt, subscribe_queue_size)
+{
+  m_send_tpdo_immediately = true; // true (OLS10, MLS): send TPDOs immediately in all states (pre-operational and operational), false (default, OLS20): send TPDOs only in state operational
+  m_ros_subscriber = nh.subscribe(subscribe_topic, m_subscribe_queue_size, &sick_canopen_simu::MLSSimulator::messageHandler, this);
+  if(!readPDOconfig(config_file, sick_device_family))
+  {
+    ROS_ERROR_STREAM("MLSSimulator: readPDOconfig(" << config_file << ") failed");
+  }
+  // Start thread for publishing PDOs
+  m_pdo_publisher_thread_running = true;
+  m_pdo_publisher_thread = new boost::thread(&sick_canopen_simu::MLSSimulator::runPDOthread, this);
+}
+
+/*
+ * @brief Callbacks for ros messages. Converts incoming messages of type can_msgs::Frame to simulate a MLS device
+ * and publishes simulation results to the configured ros topic.
+ *
+ * param[in] msg ros message of type can_msgs::Frame
+ */
+void sick_canopen_simu::MLSSimulator::messageHandler(const can_msgs::Frame & msg_in)
+{
+  SimulatorBase::messageHandler(msg_in);
+}
+
+/*
+ * @brief Parses an pdo element from config file and appends it to m_vec_pdo_measurements.
+ *
+ * param[in] xml_pdo pdo element from config file
+ */
+bool sick_canopen_simu::MLSSimulator::parseXmlPDO(TiXmlElement* xml_pdo)
+{
+  try
+  {
+    m_vec_pdo_measurements.push_back(sick_line_guidance::MsgUtil::convertMLSMessage(
+      std::stof(xml_pdo->Attribute("lcp1")), std::stof(xml_pdo->Attribute("lcp2")), std::stof(xml_pdo->Attribute("lcp3")),
+      std::stoul(xml_pdo->Attribute("status"), 0, 0),
+      std::stoul(xml_pdo->Attribute("lcp"), 0, 0),
+      std::stoul(xml_pdo->Attribute("error"), 0, 0),
+      xml_pdo->Attribute("frame_id")));
+    return true;
+  }
+  catch(const std::exception & exc)
+  {
+    ROS_ERROR_STREAM("MLSSimulator::parseXmlPDO() failed: exception " << exc.what());
+  }
+  return false;
+}
+
+/*
+ * Subclass OLSSimulator extends class SimulatorBase to simulate an OLS devices.
+ *
+ */
+
+/*
+ * Constructor
+ *
+ * @param[in] nh ros node handle
+ * @param[in] config_file configuration file with testcases for OLS and MLS simulation
+ * @param[in] sick_device_family "OLS10", "OLS20" or "MLS"
+ * @param[in] can_node_id node id of OLS, default: 0x0A
+ * @param[in] subscribe_topic ros topic to receive input messages of type can_msgs::Frame, default: "can0"
+ * @param[in] publish_topic ros topic to publish output messages of type can_msgs::Frame, default: "ros2can0"
+ * @param[in] pdo_rate rate of PDO (default: pdo_rate = 50, i.e. 20 ms between two PDOs or 50 PDOs per second)
+ * @param[in] pdo_repeat_cnt each sensor state spefied in sick_canopen_simu_cfg.xml is repeated N times before switching to the next state (default: 5 times)
+ * @param[in] subscribe_queue_size buffer size for ros messages
+ */
+sick_canopen_simu::OLSSimulator::OLSSimulator(ros::NodeHandle & nh, const std::string & config_file, const std::string & sick_device_family, int can_node_id, const std::string & subscribe_topic, const std::string & publish_topic, const ros::Rate & pdo_rate, int pdo_repeat_cnt, int subscribe_queue_size)
+  : SimulatorBase(nh, config_file, can_node_id, subscribe_topic, publish_topic, pdo_rate, pdo_repeat_cnt, subscribe_queue_size)
+{
+  m_ros_subscriber = nh.subscribe(subscribe_topic, m_subscribe_queue_size, &sick_canopen_simu::OLSSimulator::messageHandler, this);
+  if(!readPDOconfig(config_file, sick_device_family))
+  {
+    ROS_ERROR_STREAM("OLSSimulator: readPDOconfig(" << config_file << ") failed");
+  }
+  // Start thread for publishing PDOs
+  m_pdo_publisher_thread_running = true;
+  m_pdo_publisher_thread = new boost::thread(&sick_canopen_simu::OLSSimulator::runPDOthread, this);
+}
+
+/*
+ * @brief Callbacks for ros messages. Converts incoming messages of type can_msgs::Frame to simulate an OLS device
+ * and publishes simulation results to the configured ros topic.
+ *
+ * param[in] msg ros message of type can_msgs::Frame
+ */
+void sick_canopen_simu::OLSSimulator::messageHandler(const can_msgs::Frame & msg_in)
+{
+  SimulatorBase::messageHandler(msg_in);
+}
+
+/*
+ * @brief Parses an pdo element from config file and appends it to m_vec_pdo_measurements.
+ *
+ * param[in] xml_pdo pdo element from config file
+ */
+bool sick_canopen_simu::OLSSimulator::parseXmlPDO(TiXmlElement* xml_pdo)
+{
+  try
+  {
+    m_vec_pdo_measurements.push_back(sick_line_guidance::MsgUtil::convertOLSMessage(
+      std::stof(xml_pdo->Attribute("lcp1")), std::stof(xml_pdo->Attribute("lcp2")), std::stof(xml_pdo->Attribute("lcp3")),
+      std::stof(xml_pdo->Attribute("width1")), std::stof(xml_pdo->Attribute("width2")), std::stof(xml_pdo->Attribute("width3")),
+      std::stoul(xml_pdo->Attribute("status"), 0, 0),
+      std::stoul(xml_pdo->Attribute("barcode"), 0, 0),
+      std::stoul(xml_pdo->Attribute("devstatus"), 0, 0),
+      std::stoul(xml_pdo->Attribute("error"), 0, 0),
+      std::stof(xml_pdo->Attribute("barcodecenter")),         // OLS20 only: simulate barcode center point, object 0x2021subA (INT16), OLS10: always 0
+      std::stoul(xml_pdo->Attribute("linequality"), 0, 0),    // OLS20 only: simulate quality of lines, object 0x2021subB (UINT8), OLS10: always 0
+      std::stoul(xml_pdo->Attribute("lineintensity1"), 0, 0), // OLS20 only: simulate intensity of lines, object 0x2023sub1 (UINT8), OLS10: always 0
+      std::stoul(xml_pdo->Attribute("lineintensity2"), 0, 0), // OLS20 only: simulate intensity of lines, object 0x2023sub2 (UINT8), OLS10: always 0
+      std::stoul(xml_pdo->Attribute("lineintensity3"), 0, 0), // OLS20 only: simulate intensity of lines, object 0x2023sub3 (UINT8), OLS10: always 0
+      xml_pdo->Attribute("frame_id")));
+  }
+  catch(const std::exception & exc)
+  {
+    ROS_ERROR_STREAM("OLSSimulator::parseXmlPDO() failed: exception " << exc.what());
+  }
+  return false;
+}
+
+/*
+ * Subclass OLS10Simulator extends class OLSSimulator to simulate an OLS10 device.
+ *
+ */
+ 
+/*
+ * Constructor
+ *
+ * @param[in] nh ros node handle
+ * @param[in] config_file configuration file with testcases for OLS and MLS simulation
+ * @param[in] sick_device_family "OLS10", "OLS20" or "MLS"
+ * @param[in] can_node_id node id of OLS or MLS, default: 0x0A
+ * @param[in] subscribe_topic ros topic to receive input messages of type can_msgs::Frame, default: "can0"
+ * @param[in] publish_topic ros topic to publish output messages of type can_msgs::Frame, default: "ros2can0"
+ * @param[in] pdo_rate rate of PDO (default: pdo_rate = 50, i.e. 20 ms between two PDOs or 50 PDOs per second)
+ * @param[in] pdo_repeat_cnt each sensor state spefied in sick_canopen_simu_cfg.xml is repeated N times before switching to the next state (default: 5 times)
+ * @param[in] subscribe_queue_size buffer size for ros messages
+ */
+sick_canopen_simu::OLS10Simulator::OLS10Simulator(ros::NodeHandle & nh, const std::string & config_file, const std::string & sick_device_family, int can_node_id, const std::string & subscribe_topic, const std::string & publish_topic, const ros::Rate & pdo_rate, int pdo_repeat_cnt, int subscribe_queue_size)
+  : OLSSimulator(nh, config_file, sick_device_family, can_node_id, subscribe_topic, publish_topic, pdo_rate, pdo_repeat_cnt, subscribe_queue_size)
+{
+  m_send_tpdo_immediately = true; // true (OLS10, MLS): send TPDOs immediately in all states (pre-operational and operational), false (default, OLS20): send TPDOs only in state operational
+  m_sdo_response_dev_state = 0x4B18200000000000; // response to sdo request for dev_status (object 0x2018): MLS and OLS20: 0x4F18200000000000 (sdo response with UINT8 data), OLS10: 0x4B18200000000000 (sdo response with UINT16 data)
+}
+
+/*
+ * Subclass OLS20Simulator extends class OLSSimulator to simulate an OLS20 device.
+ *
+ */
+
+/*
+ * Constructor
+ *
+ * @param[in] nh ros node handle
+ * @param[in] config_file configuration file with testcases for OLS and MLS simulation
+ * @param[in] sick_device_family "OLS10", "OLS20" or "MLS"
+ * @param[in] can_node_id node id of OLS or MLS, default: 0x0A
+ * @param[in] subscribe_topic ros topic to receive input messages of type can_msgs::Frame, default: "can0"
+ * @param[in] publish_topic ros topic to publish output messages of type can_msgs::Frame, default: "ros2can0"
+ * @param[in] pdo_rate rate of PDO (default: pdo_rate = 50, i.e. 20 ms between two PDOs or 50 PDOs per second)
+ * @param[in] pdo_repeat_cnt each sensor state spefied in sick_canopen_simu_cfg.xml is repeated N times before switching to the next state (default: 5 times)
+ * @param[in] subscribe_queue_size buffer size for ros messages
+ */
+sick_canopen_simu::OLS20Simulator::OLS20Simulator(ros::NodeHandle & nh, const std::string & config_file, const std::string & sick_device_family, int can_node_id, const std::string & subscribe_topic, const std::string & publish_topic, const ros::Rate & pdo_rate, int pdo_repeat_cnt, int subscribe_queue_size)
+  : OLSSimulator(nh, config_file, sick_device_family, can_node_id, subscribe_topic, publish_topic, pdo_rate, pdo_repeat_cnt, subscribe_queue_size)
+{
+  m_send_tpdo_immediately = false; // true (OLS10, MLS): send TPDOs immediately in all states (pre-operational and operational), false (default, OLS20): send TPDOs only in state operational
+}
+

Devices/Packages/sick_line_guidance/src/sick_canopen_simu_node.cpp

@@ -0,0 +1,148 @@
+/*
+ * sick_canopen_simu_node: subscribes to ros topic "can0" (message type can_msgs::Frame),
+ * simulates an OLS20 or MLS device and publishes can_msgs::Frame messages on ros topic "ros2can0"
+ * for further transmission to the can bus.
+ * 
+ * Copyright (C) 2019 Ing.-Buero Dr. Michael Lehning, Hildesheim
+ * Copyright (C) 2019 SICK AG, Waldkirch
+ * 
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ * 
+ *       http://www.apache.org/licenses/LICENSE-2.0
+ *
+ *   Unless required by applicable law or agreed to in writing, software
+ *   distributed under the License is distributed on an "AS IS" BASIS,
+ *   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ *   See the License for the specific language governing permissions and
+ *   limitations under the License.
+ *
+ * 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 SICK AG nor the names of its
+ *       contributors may be used to endorse or promote products derived from
+ *       this software without specific prior written permission
+ *     * Neither the name of Ing.-Buero Dr. Michael Lehning 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.
+ *
+ *      Authors:
+ *         Michael Lehning <michael.lehning@lehning.de>
+ *
+ *  Copyright 2019 SICK AG
+ *  Copyright 2019 Ing.-Buero Dr. Michael Lehning
+ * 
+ */
+#include <ros/ros.h>
+#include "sick_line_guidance/sick_line_guidance_version.h"
+#include "sick_line_guidance/sick_canopen_simu_device.h"
+#include "sick_line_guidance/sick_canopen_simu_verify.h"
+
+int main(int argc, char** argv)
+{
+  // Setup and configuration
+  ros::init(argc, argv, "sick_canopen_simu_node");
+  ros::NodeHandle nh;
+  int can_node_id = 0x0A;          // default node id for OLS and MLS
+  int can_message_queue_size = 16; // buffer size for ros messages
+  int sensor_state_queue_size = 2; // buffer size for simulated sensor states
+  double pdo_rate = 50;            // rate of PDOs (default: pdo_rate = 50, i.e. 20 ms between two PDOs or 50 PDOs per second)
+  int pdo_repeat_cnt = 25;         // each sensor state spefied in sick_canopen_simu_cfg.xml is repeated 25 times before switching to the next state (sensor state changes after 0.5 seconds)
+  std::string sick_device_family = "OLS20"; // simulation of OLS10, OLS20 or MLS device
+  std::string can_subscribe_topic = "can0", publish_topic = "ros2can0"; // default can topics
+  std::string mls_subscribe_topic = "mls", ols_subscribe_topic = "ols"; // default measurement topics
+  std::string sick_canopen_simu_cfg_file = "sick_canopen_simu_cfg.xml"; // configuration file and testcases for OLS and MLS simulation
+  nh.param("sick_canopen_simu_cfg_file", sick_canopen_simu_cfg_file, sick_canopen_simu_cfg_file);
+  nh.param("can_node_id", can_node_id, can_node_id);
+  nh.param("sick_device_family", sick_device_family, sick_device_family);
+  nh.param("can_subscribe_topic", can_subscribe_topic, can_subscribe_topic);
+  nh.param("mls_subscribe_topic", mls_subscribe_topic, mls_subscribe_topic);
+  nh.param("ols_subscribe_topic", ols_subscribe_topic, ols_subscribe_topic);
+  nh.param("publish_topic", publish_topic, publish_topic);
+  nh.param("pdo_rate", pdo_rate, pdo_rate);
+  nh.param("pdo_repeat_cnt", pdo_repeat_cnt, pdo_repeat_cnt);
+  nh.param("can_message_queue_size", can_message_queue_size, can_message_queue_size);
+  nh.param("sensor_state_queue_size", sensor_state_queue_size, sensor_state_queue_size);
+  
+  ROS_INFO_STREAM("sick_canopen_simu_node: version " << sick_line_guidance::Version::getVersionInfo());
+  ROS_INFO_STREAM("sick_canopen_simu_node: initializing...");
+  sick_canopen_simu::MLSMeasurementVerification* mls_measurement_verification = 0;
+  sick_canopen_simu::OLSMeasurementVerification* ols_measurement_verification = 0;
+  sick_canopen_simu::MLSSimulator* mls_simulator = 0;
+  sick_canopen_simu::OLSSimulator* ols_simulator = 0;
+  if(sick_device_family == "MLS")
+  {
+    // Init MLS simulation
+    mls_simulator = new sick_canopen_simu::MLSSimulator(nh, sick_canopen_simu_cfg_file, sick_device_family, can_node_id, can_subscribe_topic, publish_topic, ros::Rate(pdo_rate), pdo_repeat_cnt,can_message_queue_size);
+    ROS_INFO_STREAM("sick_canopen_simu_node: MLSSimulator started, can node_id " << can_node_id << ", listening to can topic \""
+      << can_subscribe_topic << "\", measurement topic \"" << mls_subscribe_topic << "\", publishing on ros topic \"" << publish_topic << "\" ...");
+    mls_measurement_verification = new sick_canopen_simu::MLSMeasurementVerification(nh, mls_subscribe_topic, sensor_state_queue_size, sick_device_family);
+    mls_simulator->registerSimulationListener(mls_measurement_verification);
+  }
+  else if(sick_device_family == "OLS10")
+  {
+    // Init OLS10 simulation
+    ols_simulator = new sick_canopen_simu::OLS10Simulator(nh, sick_canopen_simu_cfg_file, sick_device_family, can_node_id, can_subscribe_topic, publish_topic, ros::Rate(pdo_rate), pdo_repeat_cnt, can_message_queue_size);
+    ROS_INFO_STREAM("sick_canopen_simu_node: OLS10Simulator started, can node_id " << can_node_id << ", listening to can topic \""
+      << can_subscribe_topic << "\", measurement topic \"" << ols_subscribe_topic << "\", publishing on ros topic \"" << publish_topic << "\" ...");
+    ols_measurement_verification = new sick_canopen_simu::OLSMeasurementVerification(nh, ols_subscribe_topic, sensor_state_queue_size, sick_device_family);
+    ols_simulator->registerSimulationListener(ols_measurement_verification);
+  }
+  else if(sick_device_family == "OLS20")
+  {
+    // Init OLS20 simulation
+    ols_simulator = new sick_canopen_simu::OLS20Simulator(nh, sick_canopen_simu_cfg_file, sick_device_family, can_node_id, can_subscribe_topic, publish_topic, ros::Rate(pdo_rate), pdo_repeat_cnt, can_message_queue_size);
+    ROS_INFO_STREAM("sick_canopen_simu_node: OLS20Simulator started, can node_id " << can_node_id << ", listening to can topic \""
+       << can_subscribe_topic << "\", measurement topic \"" << ols_subscribe_topic << "\", publishing on ros topic \"" << publish_topic << "\" ...");
+    ols_measurement_verification = new sick_canopen_simu::OLSMeasurementVerification(nh, ols_subscribe_topic, sensor_state_queue_size, sick_device_family);
+    ols_simulator->registerSimulationListener(ols_measurement_verification);
+  }
+  else
+  {
+    ROS_ERROR_STREAM("sick_canopen_simu_node: sick_device_family \"" << sick_device_family << "\" not supported, aborting ...");
+    return 1;
+  }
+  
+  // Run ros event loop
+  ROS_INFO_STREAM("sick_canopen_simu_node: running...");
+  ros::spin();
+  std::cout << "sick_canopen_simu_node: exiting..." << std::endl;
+  ROS_INFO_STREAM("sick_canopen_simu_node: exiting...");
+  if(mls_simulator)
+  {
+    mls_simulator->unregisterSimulationListener(mls_measurement_verification);
+    mls_measurement_verification->printStatistic();
+    delete(mls_measurement_verification);
+    delete(mls_simulator);
+  }
+  if(ols_simulator)
+  {
+    ols_simulator->unregisterSimulationListener(ols_measurement_verification);
+    ols_measurement_verification->printStatistic();
+    delete(ols_measurement_verification);
+    delete(ols_simulator);
+  }
+  return 0;
+}
+