git commit -m "first commit for v2"

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;
+}
+

Devices/Packages/sick_line_guidance/src/sick_canopen_simu_verify.cpp

@@ -0,0 +1,320 @@
+/*
+ * 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
+ * 
+ */
+#include <ros/ros.h>
+#include "sick_line_guidance/sick_canopen_simu_verify.h"
+#include "sick_line_guidance/sick_line_guidance_msg_util.h"
+#include "sick_line_guidance/sick_canopen_simu_compare.h"
+
+/*
+ * 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"
+ */
+template<class MsgType>
+sick_canopen_simu::MeasurementVerification<MsgType>::MeasurementVerification(ros::NodeHandle & nh, const std::string & measurement_subscribe_topic, int sensor_state_queue_size, const std::string & devicename)
+{
+  m_devicename = devicename;
+  m_sensor_states.resize(sensor_state_queue_size);
+  m_measurement_messages.resize(sensor_state_queue_size);
+  m_measurement_verification_error_cnt = 0;
+  m_measurement_verification_ignored_cnt = 0;
+  m_measurement_verification_failed = 0;
+  m_measurement_verification_jitter = 4; // max. 4 consecutive errors tolerated, since measurement messages can be sent while a SDO response is still pending (OLS20: up to 9 SDO queries required per TPDO measurement)
+  m_measurement_messages_cnt = -sensor_state_queue_size; // start verification after the first two measurements
+  for(typename std::list<MsgType>::iterator iter = m_sensor_states.begin(); iter != m_sensor_states.end(); iter++)
+    sick_line_guidance::MsgUtil::zero(*iter);
+  for (typename std::list<MsgType>::iterator iter = m_measurement_messages.begin(); iter != m_measurement_messages.end(); iter++)
+    sick_line_guidance::MsgUtil::zero(*iter);
+}
+
+/*
+ * Destructor
+ *
+ */
+template<class MsgType>
+sick_canopen_simu::MeasurementVerification<MsgType>::~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.
+ */
+template<class MsgType>
+void sick_canopen_simu::MeasurementVerification<MsgType>::pdoSent(const MsgType & sensor_state)
+{
+  ROS_INFO_STREAM("MeasurementVerification::pdoSent(" << sick_line_guidance::MsgUtil::toInfo(sensor_state) << ")");
+  // push sensor_state to m_sensor_states (const list size, pop first element and push new sensor state at the back)
+  boost::lock_guard<boost::mutex> state_lockguard(m_sensor_states_mutex);
+  m_sensor_states.pop_front();
+  m_sensor_states.push_back(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.
+ */
+template<class MsgType>
+bool sick_canopen_simu::MeasurementVerification<MsgType>::printStatistic(void)
+{
+  std::stringstream message;
+  if(m_measurement_messages_cnt > 0 && m_measurement_verification_error_cnt > 0)
+  {
+    message << m_devicename << " MeasurementVerificationStatistic failures: " << m_measurement_verification_error_cnt << " of " << m_measurement_messages_cnt << " measurements failed, "
+      << std::fixed << std::setprecision(2) << (m_measurement_verification_error_cnt*100.00/m_measurement_messages_cnt) << " % errors, " << m_measurement_verification_ignored_cnt << " measurements ignored.";
+    std::cerr << message.str() << std::endl;
+    ROS_ERROR_STREAM(message.str());
+  }
+  else if(m_measurement_messages_cnt > 0)
+  {
+    message << m_devicename << " MeasurementVerificationStatistic okay: " << m_measurement_verification_failed << " of " << m_measurement_messages_cnt << " measurements failed, " << m_measurement_verification_ignored_cnt << " measurements ignored.";
+    std::cout << message.str() << std::endl;
+    ROS_INFO_STREAM(message.str());
+  }
+  return (m_measurement_verification_error_cnt == 0);
+}
+
+/*
+ * @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).
+ */
+template<class MsgType>
+bool sick_canopen_simu::MeasurementVerification<MsgType>::verifyMeasurement(const MsgType & measurement)
+{
+  ROS_INFO_STREAM(m_devicename << " MeasurementVerification::verifyMeasurement(" << sick_line_guidance::MsgUtil::toInfo(measurement) << ")");
+  // push measurement to m_measurement_messages (const list size, pop first element and push new measurement at the back)
+  boost::lock_guard<boost::mutex> state_lockguard(m_sensor_states_mutex);
+  m_measurement_messages.pop_front();
+  m_measurement_messages.push_back(measurement);
+  bool measurement_verified = true;
+  if(m_measurement_messages_cnt > 0) // start verification after 2 measurements
+  {
+    measurement_verified = verifyMeasurements(m_measurement_messages, m_sensor_states);
+    if(measurement_verified)
+    {
+      ROS_INFO_STREAM(m_devicename << " MeasurementVerification::verifyMeasurement(" << sick_line_guidance::MsgUtil::toInfo(measurement) << ") succeeded.");
+      m_measurement_verification_failed = 0;
+    }
+    else
+    {
+      std::stringstream errormsg;
+      errormsg << m_devicename << " MeasurementVerification::verifyMeasurement(" << sick_line_guidance::MsgUtil::toInfo(measurement) << ") failed.";
+      for (typename std::list<MsgType>::iterator iter_measurement = m_measurement_messages.begin(); iter_measurement != m_measurement_messages.end(); iter_measurement++)
+        errormsg << m_devicename << " MeasurementVerification: measurement  " << sick_line_guidance::MsgUtil::toInfo(*iter_measurement);
+      for(typename std::list<MsgType>::iterator iter_state = m_sensor_states.begin(); iter_state != m_sensor_states.end(); iter_state++)
+        errormsg << m_devicename << " MeasurementVerification: sensor state " << sick_line_guidance::MsgUtil::toInfo(*iter_state);
+      m_measurement_verification_failed++;
+      if(m_measurement_verification_failed > m_measurement_verification_jitter)
+      {
+        m_measurement_verification_error_cnt++; // error: 2 consecutive failures (measurement message different to simulated sensor state)
+        ROS_ERROR_STREAM(errormsg.str());
+      }
+      else
+      {
+        m_measurement_verification_ignored_cnt++; // possible error (max. 1 error tolerated, since measurement messages can be sent while a SDO response is still pending)
+        ROS_WARN_STREAM(errormsg.str());
+      }
+    }
+  }
+  m_measurement_messages_cnt++;
+  return measurement_verified;
+}
+
+/*
+ * @brief Compares and verifies MLS 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).
+ */
+template<class MsgType>
+bool sick_canopen_simu::MeasurementVerification<MsgType>::verifyMeasurements(std::list<sick_line_guidance::MLS_Measurement> & measurement_messages, std::list<sick_line_guidance::MLS_Measurement> & sensor_states)
+{
+  // If the sensor state changed between two PDOs, sensor data from PDOs (position and width) might represent the new state, while sensor data from SDOs (barcode)
+  // are still from the previous sensor state (SDO still pending or SDO response not yet received). Or vice versa, after receiving the PDOs, the simuulation
+  // switches to a new state, which is returned by the next SDO (but before the new state is published by a new PDO). In this case, measurement verification
+  // might fail exactly once when SDO based sensor data (barcodes) are switched right between two PDOs. To avoid verification problems when entries in the
+  // object dictionary are modified between two PDOs, we compare the current and the last sensor state. One of them always has to be verified, either the
+  // current or the previous state. This way, we tolerate a 10-milliseconds-jitter in our verification.
+  sick_canopen_simu::MeasurementComparator<sick_line_guidance::MLS_Measurement> comparator;
+  return verifyMeasurementData(measurement_messages, sensor_states, comparator.cmpPosition)
+    && verifyMeasurementData(measurement_messages, sensor_states, comparator.cmpLcp)
+      && verifyMeasurementData(measurement_messages, sensor_states, comparator.cmpStatus)
+        && verifyMeasurementData(measurement_messages, sensor_states, comparator.cmpError);
+}
+
+/*
+ * @brief Compares and verifies OLS 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).
+ */
+template<class MsgType>
+bool sick_canopen_simu::MeasurementVerification<MsgType>::verifyMeasurements(std::list<sick_line_guidance::OLS_Measurement> & measurement_messages, std::list<sick_line_guidance::OLS_Measurement> & sensor_states)
+{
+  // If the sensor state changed between two PDOs, sensor data from PDOs (position and width) might represent the new state, while sensor data from SDOs (barcode)
+  // are still from the previous sensor state (SDO still pending or SDO response not yet received). Or vice versa, after receiving the PDOs, the simuulation
+  // switches to a new state, which is returned by the next SDO (but before the new state is published by a new PDO). In this case, measurement verification
+  // might fail exactly once when SDO based sensor data (barcodes) are switched right between two PDOs. To avoid verification problems when entries in the
+  // object dictionary are modified between two PDOs, we compare the current and the last sensor state. One of them always has to be verified, either the
+  // current or the previous state. This way, we tolerate a 10-milliseconds-jitter in our verification.
+  sick_canopen_simu::MeasurementComparator<sick_line_guidance::OLS_Measurement> comparator;
+  return verifyMeasurementData(measurement_messages, sensor_states, comparator.cmpPosition)
+    && verifyMeasurementData(measurement_messages, sensor_states, comparator.cmpLinewidth)
+       && verifyMeasurementData(measurement_messages, sensor_states, comparator.cmpStatus)
+          && verifyMeasurementData(measurement_messages, sensor_states, comparator.cmpBarcode)
+             && verifyMeasurementData(measurement_messages, sensor_states, comparator.cmpDevStatus)
+                && verifyMeasurementData(measurement_messages, sensor_states, comparator.cmpExtendedCode)
+                   && verifyMeasurementData(measurement_messages, sensor_states, comparator.cmpError)
+                      && verifyMeasurementData(measurement_messages, sensor_states, comparator.cmpBarcodeCenter)
+                         && verifyMeasurementData(measurement_messages, sensor_states, comparator.cmpLineQuality)
+                            && verifyMeasurementData(measurement_messages, sensor_states, comparator.cmpLineIntensity);
+}
+
+/*
+ * @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<class MsgType> template<typename T>
+bool sick_canopen_simu::MeasurementVerification<MsgType>::verifyMeasurementData(std::list<T> & measurement_messages, std::list<T> & sensor_states, bool(*cmpfunction)(const T & A, const T & B))
+{
+  for (typename std::list<T>::iterator iter_measurement = measurement_messages.begin(); iter_measurement != measurement_messages.end(); iter_measurement++)
+  {
+    for (typename std::list<T>::iterator iter_state = sensor_states.begin(); iter_state != sensor_states.end(); iter_state++)
+    {
+      if (cmpfunction(*iter_state, *iter_measurement))
+      {
+        return true;
+      }
+    }
+  }
+  return false;
+}
+
+/*
+ * Constructor. Subclass MLSMeasurementVerification extends class MeasurementVerification to verify MLS_Measurement messages.
+ *
+ * @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"
+ */
+sick_canopen_simu::MLSMeasurementVerification::MLSMeasurementVerification(ros::NodeHandle & nh, const std::string & measurement_subscribe_topic, int sensor_state_queue_size, const std::string & devicename)
+  : sick_canopen_simu::MeasurementVerification<sick_line_guidance::MLS_Measurement>::MeasurementVerification(nh, measurement_subscribe_topic, sensor_state_queue_size, devicename)
+{
+  m_measurement_subscriber = nh.subscribe(measurement_subscribe_topic, sensor_state_queue_size, &sick_canopen_simu::MLSMeasurementVerification::measurementCb, this);
+}
+  
+/*
+ * @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
+ */
+void sick_canopen_simu::MLSMeasurementVerification::measurementCb(const sick_line_guidance::MLS_Measurement & measurement)
+{
+  verifyMeasurement(measurement);
+}
+  
+/*
+ * Constructor. Subclass OLSMeasurementVerification extends class MeasurementVerification to verify OLS_Measurement messages.
+ *
+ * @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"
+ */
+sick_canopen_simu::OLSMeasurementVerification::OLSMeasurementVerification(ros::NodeHandle & nh, const std::string & measurement_subscribe_topic, int sensor_state_queue_size, const std::string & devicename)
+  : sick_canopen_simu::MeasurementVerification<sick_line_guidance::OLS_Measurement>::MeasurementVerification(nh, measurement_subscribe_topic, sensor_state_queue_size, devicename)
+{
+  m_measurement_subscriber = nh.subscribe(measurement_subscribe_topic, sensor_state_queue_size, &sick_canopen_simu::OLSMeasurementVerification::measurementCb, this);
+}
+  
+/*
+ * @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
+ */
+void sick_canopen_simu::OLSMeasurementVerification::measurementCb(const sick_line_guidance::OLS_Measurement & measurement)
+{
+  verifyMeasurement(measurement);
+}

Devices/Packages/sick_line_guidance/src/sick_line_guidance_can2ros_node.cpp

@@ -0,0 +1,242 @@
+/*
+ * sick_line_guidance_can2ros_node: implements a ros node, which publishes all can messages.
+ * A simple candump to 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
+ * 
+ */
+#include <ros/ros.h>
+#include <can_msgs/Frame.h>
+#include <socketcan_interface/socketcan.h>
+#include <socketcan_interface/filter.h>
+#include <canopen_chain_node/ros_chain.h>
+#include "sick_line_guidance/sick_line_guidance_version.h"
+
+namespace sick_line_guidance
+{
+  /*
+   * class SocketCANListener: implements a simple listener to a SocketCANInterface.
+   */
+  class SocketCANListener
+  {
+  public:
+    
+    /*
+     * Constructor
+     */
+    SocketCANListener() : m_socketcan_interface(0), m_socketcan_thread(0), m_socketcan_running(false), m_socketcan_state_listener(0), m_socketcan_frame_listener(0), m_filter_sync(true)
+    {
+    }
+    
+    /*
+     * Register state and frame listener to the SocketCANInterface and create ros publisher.
+     * param[in] nh ros handle
+     * param[in] topic topic for ros messages, message type can_msgs::Frame
+     * param[in] p_socketcan_interface pointer to SocketCANInterface
+     * param[in] filter_sync if true (default), can sync messages are not published
+     * @return true on success, otherwise false;
+     */
+    bool init(ros::NodeHandle & nh, const std::string & topic, can::DriverInterfaceSharedPtr p_socketcan_interface, bool filter_sync = true)
+    {
+      m_socketcan_interface = p_socketcan_interface;
+      m_filter_sync = filter_sync;
+      m_ros_publisher = nh.advertise<can_msgs::Frame>(topic, 1);
+      m_socketcan_state_listener = m_socketcan_interface->createStateListener(can::StateInterface::StateDelegate(this, &SocketCANListener::socketcanStateCallback));
+      m_socketcan_frame_listener = m_socketcan_interface->createMsgListener(can::CommInterface::FrameDelegate(this, &SocketCANListener::socketcanFrameCallback));
+      return m_socketcan_state_listener != 0 && m_socketcan_frame_listener != 0;
+    }
+  
+    /*
+     * @brief create a thread and run can::SocketCANInterface::run() in a background task
+     */
+    void start()
+    {
+      m_socketcan_running = true;
+      m_socketcan_thread = new boost::thread(&sick_line_guidance::SocketCANListener::run, this);
+    }
+  
+    /*
+     * @brief shuts down can::SocketCANInterface and stops the thread running can::SocketCANInterface::run()
+     */
+    void stop()
+    {
+      m_socketcan_running = false;
+      if(m_socketcan_thread)
+      {
+        m_socketcan_interface->shutdown();
+        m_socketcan_thread->join();
+        delete(m_socketcan_thread);
+      }
+      m_socketcan_thread = 0;
+    }
+
+  protected:
+  
+    /*
+     * member variables and functions
+     */
+
+    can::DriverInterfaceSharedPtr m_socketcan_interface; // can::SocketCANInterface instance
+    boost::thread* m_socketcan_thread; // thread to run can::SocketCANInterface in background
+    bool m_socketcan_running; // flag indicating start and stop of m_socketcan_thread
+    can::FrameListenerConstSharedPtr m_socketcan_frame_listener; // can frame listener
+    can::StateListenerConstSharedPtr m_socketcan_state_listener; // can state listener
+    ros::Publisher m_ros_publisher; // publishes a ros message for each received can frame
+    bool m_filter_sync; // if true (default), can sync messages are not published
+    
+    /*
+     * Callback for can state messages, called by SocketCANInterface.
+     * param[in] canstate can state message
+     */
+    void socketcanStateCallback(const can::State & canstate)
+    {
+      // ROS_DEBUG_STREAM("sick_line_guidance::SocketCANListener::socketcanStateCallback(): can state message: " << canstate.driver_state << " (error code: " << canstate.error_code << ")");
+      if(canstate.error_code)
+        ROS_ERROR_STREAM("sick_line_guidance::SocketCANListener::socketcanStateCallback(): can error code: " << canstate.error_code);
+    }
+  
+    /*
+     * Callback for can frame messages, called by SocketCANInterface.
+     * param[in] canframe can frame message
+     */
+    void socketcanFrameCallback(const can::Frame & canframe)
+    {
+      if(canframe.isValid())
+      {
+        if(m_filter_sync && canframe.id == 0x80) // can sync message
+        {
+          ROS_DEBUG_STREAM("sick_line_guidance::SocketCANListener::socketcanFrameCallback(): can sync message: " << can::tostring(canframe, false));
+          return;
+        }
+        ROS_DEBUG_STREAM("sick_line_guidance::SocketCANListener::socketcanFrameCallback(): can frame message: " << can::tostring(canframe, false));
+      }
+      else
+      {
+        ROS_ERROR_STREAM("sick_line_guidance::SocketCANListener::socketcanFrameCallback(): invalid can frame message: " << can::tostring(canframe, false));
+      }
+      can_msgs::Frame canframe_msg;
+      canframe_msg.id = canframe.id;
+      canframe_msg.dlc = std::min(static_cast<uint8_t>(canframe_msg.data.size()), canframe.dlc);
+      canframe_msg.is_error = canframe.is_error;
+      canframe_msg.is_rtr = canframe.is_rtr;
+      canframe_msg.is_extended = canframe.is_extended;
+      canframe_msg.data.assign(0);
+      for(size_t n = 0, n_max = std::min(canframe_msg.data.size(),canframe.data.size()); n < n_max; n++)
+        canframe_msg.data[n] = canframe.data[n];
+      canframe_msg.header.stamp = ros::Time::now();
+      m_ros_publisher.publish(canframe_msg);
+    }
+    
+    /*
+     * @brief runs can::SocketCANInterface::run() in an endless loop
+     */
+    void run()
+    {
+      while(m_socketcan_running && ros::ok())
+      {
+        m_socketcan_interface->run();
+      }
+    }
+
+  }; // class SocketCANListener
+} // namespace sick_line_guidance
+
+/*
+ * sick_line_guidance_can2ros_node: implements a ros node, which publishes all can messages.
+ * A simple candump to ros messages.
+ */
+int main(int argc, char** argv)
+{
+  // Setup and configuration
+  ros::init(argc, argv, "sick_line_guidance_can2ros_node");
+  ros::NodeHandle nh;
+  std::string can_device = "can0", ros_topic = "can0";
+  nh.param("can_device", can_device, can_device); // name of can net device (socketcan interface)
+  nh.param("ros_topic", ros_topic, ros_topic); // topic for ros messages, message type can_msgs::Frame
+  ROS_INFO_STREAM("sick_line_guidance_can2ros_node: version " << sick_line_guidance::Version::getVersionInfo());
+  ROS_INFO_STREAM("sick_line_guidance_can2ros_node: starting...");
+  
+  // Create the SocketCANInterface
+  sick_line_guidance::SocketCANListener socketcan_listener;
+  can::DriverInterfaceSharedPtr p_socketcan_interface = 0;
+  canopen::GuardedClassLoader<can::DriverInterface> driver_loader("socketcan_interface", "can::DriverInterface");
+  try
+  {
+    ROS_INFO("sick_line_guidance_can2ros_node: initializing SocketCANInterface...");
+    p_socketcan_interface =  driver_loader.createInstance("can::SocketCANInterface");
+    if(!p_socketcan_interface->init(can_device, false, can::NoSettings::create()))
+      ROS_ERROR("sick_line_guidance_can2ros_node: SocketCANInterface::init() failed.");
+    ROS_INFO("sick_line_guidance_can2ros_node: initializing socketcan listener ...");
+    if(!socketcan_listener.init(nh, ros_topic, p_socketcan_interface, true))
+      ROS_ERROR("sick_line_guidance_can2ros_node: SocketCANListener::registerListener() failed.");
+    socketcan_listener.start();
+  }
+  catch(pluginlib::PluginlibException& ex)
+  {
+    ROS_ERROR_STREAM("sick_line_guidance_can2ros_node: createInstance(\"can::SocketCANInterface\") failed: " << ex.what() << ", exit with error");
+    return 1;
+  }
+  ROS_INFO_STREAM("sick_line_guidance_can2ros_node: SocketCANInterface created, state = " << p_socketcan_interface->getState().driver_state);
+  
+  // Run interface event loop
+  // while(ros::ok())
+  //   p_socketcan_interface->run();
+  
+  // Run ros event loop
+  ros::spin();
+  
+  std::cout << "sick_line_guidance_can2ros_node: exiting..." << std::endl;
+  ROS_INFO("sick_line_guidance_can2ros_node: exiting...");
+  socketcan_listener.stop();
+  p_socketcan_interface = 0;
+  return 0;
+}
+

Devices/Packages/sick_line_guidance/src/sick_line_guidance_can_chain_node.cpp

@@ -0,0 +1,94 @@
+/*
+ * sick_line_guidance_can_chain_node wraps CanopenChain for 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
+ * 
+ */
+#include <ros/ros.h>
+#include <ros/console.h>
+#include <console_bridge/console.h>
+#include <log4cxx/logger.h>
+#include "sick_line_guidance/sick_line_guidance_version.h"
+#include "sick_line_guidance/sick_line_guidance_canopen_chain.h"
+#include "sick_line_guidance/sick_line_guidance_diagnostic.h"
+
+int main(int argc, char** argv)
+{
+  ros::init(argc, argv, "sick_line_guidance_can_chain_node");
+  // log4cxx::LoggerPtr node_logger = log4cxx::Logger::getLogger(ROSCONSOLE_DEFAULT_NAME);
+  // node_logger->setLevel(ros::console::g_level_lookup[ros::console::levels::Debug]);
+  // console_bridge::setLogLevel(console_bridge::CONSOLE_BRIDGE_LOG_DEBUG);
+
+  ros::NodeHandle nh;
+  ros::NodeHandle nh_priv("~");
+  ROS_INFO_STREAM("sick_line_guidance_can_chain_node: version " << sick_line_guidance::Version::getVersionInfo());
+  
+  // Start canopen_chain_node
+  std::string diagnostic_topic = "diagnostics";
+  nh.param("/sick_line_guidance_can_chain_node/diagnostic_topic", diagnostic_topic, diagnostic_topic);
+  sick_line_guidance::Diagnostic::init(nh, diagnostic_topic, "sick_line_guidance_can_chain_node");
+  sick_line_guidance::CanopenChain canopen_chain(nh, nh_priv);
+  
+  ROS_INFO_STREAM("sick_line_guidance_can_chain_node: canopen setup...");
+  if(!canopen_chain.setup())
+  {
+    ROS_ERROR_STREAM("sick_line_guidance_can_chain_node: CanopenChain::setup() failed.");
+    sick_line_guidance::Diagnostic::update(sick_line_guidance::DIAGNOSTIC_STATUS::INITIALIZATION_ERROR, "CanopenChain::setup() failed");
+    return 1;
+  }
+  ROS_INFO_STREAM("sick_line_guidance_can_chain_node: canopen setup successfull.");
+  sick_line_guidance::Diagnostic::update(sick_line_guidance::DIAGNOSTIC_STATUS::OK);
+  
+  // Run ros event loop
+  ros::spin();
+  sick_line_guidance::Diagnostic::update(sick_line_guidance::DIAGNOSTIC_STATUS::EXIT);
+  return 0;
+}

Devices/Packages/sick_line_guidance/src/sick_line_guidance_can_cia401_subscriber.cpp

@@ -0,0 +1,194 @@
+/*
+ * class CanCiA401Subscriber implements the 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
+ * 
+ */
+#include <ros/ros.h>
+#include <canopen_chain_node/ros_chain.h>
+#include "sick_line_guidance/sick_line_guidance_can_cia401_subscriber.h"
+
+/*
+ * 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
+ */
+sick_line_guidance::CanCiA401Subscriber::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, int subscribe_queue_size)
+  : sick_line_guidance::CanOlsSubscriber::CanOlsSubscriber(nh, can_nodeid, max_num_retries_after_sdo_error, max_sdo_rate, ros_topic, ros_frameid, initial_sensor_state, subscribe_queue_size)
+{
+}
+
+/*
+ * Destructor.
+ */
+sick_line_guidance::CanCiA401Subscriber::~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.
+ */
+bool sick_line_guidance::CanCiA401Subscriber::subscribeCanTopics(void)
+{
+  m_can_subscriber.push_back(m_measurement.m_nh.subscribe(m_measurement.m_can_nodeid + "_6000sub1", m_subscribe_queue_size, &sick_line_guidance::CanCiA401Subscriber::cancallbackError, this));
+  m_can_subscriber.push_back(m_measurement.m_nh.subscribe(m_measurement.m_can_nodeid + "_6401sub1", m_subscribe_queue_size, &sick_line_guidance::CanCiA401Subscriber::cancallbackLCP1, this));
+  m_can_subscriber.push_back(m_measurement.m_nh.subscribe(m_measurement.m_can_nodeid + "_6401sub2", m_subscribe_queue_size, &sick_line_guidance::CanCiA401Subscriber::cancallbackLCP2, this));
+  m_can_subscriber.push_back(m_measurement.m_nh.subscribe(m_measurement.m_can_nodeid + "_6401sub3", m_subscribe_queue_size, &sick_line_guidance::CanCiA401Subscriber::cancallbackLCP3, this));
+  m_can_subscriber.push_back(m_measurement.m_nh.subscribe(m_measurement.m_can_nodeid + "_6000sub2", m_subscribe_queue_size, &sick_line_guidance::CanCiA401Subscriber::cancallbackState, this));
+  m_can_subscriber.push_back(m_measurement.m_nh.subscribe(m_measurement.m_can_nodeid + "_6401sub4", m_subscribe_queue_size, &sick_line_guidance::CanCiA401Subscriber::cancallbackWidthLCP1, this));
+  m_can_subscriber.push_back(m_measurement.m_nh.subscribe(m_measurement.m_can_nodeid + "_6401sub5", m_subscribe_queue_size, &sick_line_guidance::CanCiA401Subscriber::cancallbackWidthLCP2, this));
+  m_can_subscriber.push_back(m_measurement.m_nh.subscribe(m_measurement.m_can_nodeid + "_6401sub6", m_subscribe_queue_size, &sick_line_guidance::CanCiA401Subscriber::cancallbackWidthLCP3, this));
+  m_can_subscriber.push_back(m_measurement.m_nh.subscribe(m_measurement.m_can_nodeid + "_6000sub3", m_subscribe_queue_size, &sick_line_guidance::CanCiA401Subscriber::cancallbackCode, this));
+  return true;
+}
+
+/*
+ * 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.
+ */
+
+void sick_line_guidance::CanCiA401Subscriber::cancallbackError(const boost::shared_ptr<std_msgs::UInt8 const>& msg)
+{
+  // Note: Object 0x1001 (error register) is NOT PDO mapped -> Query 0x1001 ((error register) in case of error flag in m_ols_state.status
+  // In CanCiA401Subscriber we simulate error status by PDO mapped object 6000sub1
+  boost::lock_guard<boost::mutex> publish_lockguard(m_measurement.m_measurement_mutex);
+  m_measurement.m_ols_state.error = convertIntegerMessage<std_msgs::UInt8,uint8_t>(msg, m_measurement.m_ols_state.error, 0xFF, "OLS/error");
+  publishOLSMeasurement();
+}
+void sick_line_guidance::CanCiA401Subscriber::cancallbackLCP1(const boost::shared_ptr<std_msgs::Int16 const>& msg)
+{
+  sick_line_guidance::CanOlsSubscriber::cancallbackLCP1(msg);
+}
+void sick_line_guidance::CanCiA401Subscriber::cancallbackLCP2(const boost::shared_ptr<std_msgs::Int16 const>& msg)
+{
+  sick_line_guidance::CanOlsSubscriber::cancallbackLCP2(msg);
+}
+void sick_line_guidance::CanCiA401Subscriber::cancallbackLCP3(const boost::shared_ptr<std_msgs::Int16 const>& msg)
+{
+  sick_line_guidance::CanOlsSubscriber::cancallbackLCP3(msg);
+}
+void sick_line_guidance::CanCiA401Subscriber::cancallbackState(const boost::shared_ptr<std_msgs::UInt8 const>& msg)
+{
+  sick_line_guidance::CanOlsSubscriber::cancallbackState(msg);
+}
+void sick_line_guidance::CanCiA401Subscriber::cancallbackWidthLCP1(const boost::shared_ptr<std_msgs::Int16 const>& msg)
+{
+  sick_line_guidance::CanOlsSubscriber::cancallbackWidthLCP1(msg);
+}
+void sick_line_guidance::CanCiA401Subscriber::cancallbackWidthLCP2(const boost::shared_ptr<std_msgs::Int16 const>& msg)
+{
+  sick_line_guidance::CanOlsSubscriber::cancallbackWidthLCP2(msg);
+}
+void sick_line_guidance::CanCiA401Subscriber::cancallbackWidthLCP3(const boost::shared_ptr<std_msgs::Int16 const>& msg)
+{
+  sick_line_guidance::CanOlsSubscriber::cancallbackWidthLCP3(msg);
+}
+void sick_line_guidance::CanCiA401Subscriber::cancallbackCode(const boost::shared_ptr<std_msgs::UInt8 const>& msg)
+{
+  sick_line_guidance::CanOlsSubscriber::cancallbackCode(msg);
+}
+

Devices/Packages/sick_line_guidance/src/sick_line_guidance_can_mls_subscriber.cpp

@@ -0,0 +1,200 @@
+/*
+ * class CanMlsSubscriber implements the 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
+ * 
+ */
+#include <ros/ros.h>
+#include <canopen_chain_node/ros_chain.h>
+#include "sick_line_guidance/sick_line_guidance_can_mls_subscriber.h"
+
+/*
+ * 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
+ */
+sick_line_guidance::CanMlsSubscriber::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, int subscribe_queue_size)
+  : sick_line_guidance::CanSubscriber::CanSubscriber(nh, can_nodeid, max_num_retries_after_sdo_error, max_sdo_rate, initial_sensor_state, subscribe_queue_size)
+{
+  m_measurement.m_mls_state.header.frame_id = ros_frameid;
+  m_measurement.m_ros_publisher = nh.advertise<sick_line_guidance::MLS_Measurement>(ros_topic, 1);
+}
+
+/*
+ * Destructor.
+ */
+sick_line_guidance::CanMlsSubscriber::~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.
+ */
+bool sick_line_guidance::CanMlsSubscriber::subscribeCanTopics(void)
+{
+  // m_can_subscriber.push_back(m_measurement.m_nh.subscribe(m_measurement.m_can_nodeid + "_1001",     m_subscribe_queue_size, &sick_line_guidance::CanMlsSubscriber::cancallbackError, this));
+  m_can_subscriber.push_back(m_measurement.m_nh.subscribe(m_measurement.m_can_nodeid + "_2021sub1", m_subscribe_queue_size, &sick_line_guidance::CanMlsSubscriber::cancallbackLCP1, this));
+  m_can_subscriber.push_back(m_measurement.m_nh.subscribe(m_measurement.m_can_nodeid + "_2021sub2", m_subscribe_queue_size, &sick_line_guidance::CanMlsSubscriber::cancallbackLCP2, this));
+  m_can_subscriber.push_back(m_measurement.m_nh.subscribe(m_measurement.m_can_nodeid + "_2021sub3", m_subscribe_queue_size, &sick_line_guidance::CanMlsSubscriber::cancallbackLCP3, this));
+  m_can_subscriber.push_back(m_measurement.m_nh.subscribe(m_measurement.m_can_nodeid + "_2021sub4", m_subscribe_queue_size, &sick_line_guidance::CanMlsSubscriber::cancallbackMarker, this));
+  m_can_subscriber.push_back(m_measurement.m_nh.subscribe(m_measurement.m_can_nodeid + "_2022",     m_subscribe_queue_size, &sick_line_guidance::CanMlsSubscriber::cancallbackState, this));
+  return true;
+}
+
+/*
+ * 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.
+ */
+
+void sick_line_guidance::CanMlsSubscriber::cancallbackLCP1(const boost::shared_ptr<std_msgs::Int16 const>& msg)
+{
+  boost::lock_guard<boost::mutex> publish_lockguard(m_measurement.m_measurement_mutex);
+  m_measurement.m_mls_state.position[0] = convertToLCP(msg, m_measurement.m_mls_state.position[0], "MLS/LCP1");
+  publishMLSMeasurement();
+}
+
+void sick_line_guidance::CanMlsSubscriber::cancallbackLCP2(const boost::shared_ptr<std_msgs::Int16 const>& msg)
+{
+  boost::lock_guard<boost::mutex> publish_lockguard(m_measurement.m_measurement_mutex);
+  m_measurement.m_mls_state.position[1] = convertToLCP(msg, m_measurement.m_mls_state.position[1], "MLS/LCP2");
+  publishMLSMeasurement();
+}
+
+void sick_line_guidance::CanMlsSubscriber::cancallbackLCP3(const boost::shared_ptr<std_msgs::Int16 const>& msg)
+{
+  boost::lock_guard<boost::mutex> publish_lockguard(m_measurement.m_measurement_mutex);
+  m_measurement.m_mls_state.position[2] = convertToLCP(msg, m_measurement.m_mls_state.position[2], "MLS/LCP3");
+  publishMLSMeasurement();
+}
+
+void sick_line_guidance::CanMlsSubscriber::cancallbackMarker(const boost::shared_ptr<std_msgs::UInt8 const>& msg)
+{
+  boost::lock_guard<boost::mutex> publish_lockguard(m_measurement.m_measurement_mutex);
+  m_measurement.m_mls_state.lcp = convertIntegerMessage<std_msgs::UInt8,uint8_t>(msg, m_measurement.m_mls_state.lcp, 0xFF, "MLS/lcp");
+  publishMLSMeasurement();
+}
+
+void sick_line_guidance::CanMlsSubscriber::cancallbackState(const boost::shared_ptr<std_msgs::UInt8 const>& msg)
+{
+  boost::lock_guard<boost::mutex> publish_lockguard(m_measurement.m_measurement_mutex);
+  m_measurement.m_mls_state.status = convertIntegerMessage<std_msgs::UInt8,uint8_t>(msg, m_measurement.m_mls_state.status, 0xFF, "MLS/status");
+  // m_measurement.m_mls_state.error = 0;
+  // m_measurement.m_mls_query_error_register = false;
+  // if((m_measurement.m_mls_state.status & 0x0F) == 0) // Bit0 to Bit 3 == 0: bad line, no magnetic field recognized -> SDO request 0x1001 (error register, UINT8)
+  // {
+  //   m_measurement.m_mls_query_error_register = true; // query object 0x1001 (error register, UINT8) in object dictionary by SDO (query runs in m_measurement in a background thread)
+  // }
+  publishMLSMeasurement();
+}
+
+// void sick_line_guidance::CanMlsSubscriber::cancallbackError(const boost::shared_ptr<std_msgs::UInt8 const>& msg)
+// {
+//   // Note: Object 0x1001 (error register) is NOT PDO mapped -> Query 0x1001 ((error register) in case of error flag in m_mls_state.status
+//   boost::lock_guard<boost::mutex> publish_lockguard(m_measurement.m_measurement_mutex);
+//   m_measurement.m_mls_state.error = convertIntegerMessage<std_msgs::UInt8,uint8_t>(msg, m_measurement.m_mls_state.error, 0xFF, "MLS/error");
+//   publishMLSMeasurement();
+// }

Devices/Packages/sick_line_guidance/src/sick_line_guidance_can_ols_subscriber.cpp

@@ -0,0 +1,367 @@
+/*
+ * class CanOlsSubscriber implements the 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
+ * 
+ */
+#include <ros/ros.h>
+#include <canopen_chain_node/ros_chain.h>
+#include "sick_line_guidance/sick_line_guidance_canopen_chain.h"
+#include "sick_line_guidance/sick_line_guidance_can_ols_subscriber.h"
+#include "sick_line_guidance/sick_line_guidance_msg_util.h"
+
+/*
+ * class CanOlsSubscriber implements the base ros subscriber to canopen ols messages.
+ */
+
+/*
+ * 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
+ */
+sick_line_guidance::CanOlsSubscriber::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, int subscribe_queue_size)
+  : sick_line_guidance::CanSubscriber::CanSubscriber(nh, can_nodeid, max_num_retries_after_sdo_error, max_sdo_rate, initial_sensor_state, subscribe_queue_size)
+{
+  m_bOLS20queries = false;
+  m_measurement.m_ols_state.header.frame_id = ros_frameid;
+  m_measurement.m_ros_publisher = nh.advertise<sick_line_guidance::OLS_Measurement>(ros_topic, 1);
+}
+
+/*
+ * Destructor.
+ */
+sick_line_guidance::CanOlsSubscriber::~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.
+ */
+bool sick_line_guidance::CanOlsSubscriber::subscribeCanTopics(void)
+{
+  // m_can_subscriber.push_back(m_measurement.m_nh.subscribe(m_measurement.m_can_nodeid + "_1001",     m_subscribe_queue_size, &sick_line_guidance::CanOlsSubscriber::cancallbackError, this));
+  // m_can_subscriber.push_back(m_measurement.m_nh.subscribe(m_measurement.m_can_nodeid + "_2018",     m_subscribe_queue_size, &sick_line_guidance::CanOlsSubscriber::cancallbackDevState, this));
+  m_can_subscriber.push_back(m_measurement.m_nh.subscribe(m_measurement.m_can_nodeid + "_2021sub1", m_subscribe_queue_size, &sick_line_guidance::CanOlsSubscriber::cancallbackLCP1, this));
+  m_can_subscriber.push_back(m_measurement.m_nh.subscribe(m_measurement.m_can_nodeid + "_2021sub2", m_subscribe_queue_size, &sick_line_guidance::CanOlsSubscriber::cancallbackLCP2, this));
+  m_can_subscriber.push_back(m_measurement.m_nh.subscribe(m_measurement.m_can_nodeid + "_2021sub3", m_subscribe_queue_size, &sick_line_guidance::CanOlsSubscriber::cancallbackLCP3, this));
+  m_can_subscriber.push_back(m_measurement.m_nh.subscribe(m_measurement.m_can_nodeid + "_2021sub4", m_subscribe_queue_size, &sick_line_guidance::CanOlsSubscriber::cancallbackState, this));
+  m_can_subscriber.push_back(m_measurement.m_nh.subscribe(m_measurement.m_can_nodeid + "_2021sub5", m_subscribe_queue_size, &sick_line_guidance::CanOlsSubscriber::cancallbackWidthLCP1, this));
+  m_can_subscriber.push_back(m_measurement.m_nh.subscribe(m_measurement.m_can_nodeid + "_2021sub6", m_subscribe_queue_size, &sick_line_guidance::CanOlsSubscriber::cancallbackWidthLCP2, this));
+  m_can_subscriber.push_back(m_measurement.m_nh.subscribe(m_measurement.m_can_nodeid + "_2021sub7", m_subscribe_queue_size, &sick_line_guidance::CanOlsSubscriber::cancallbackWidthLCP3, this));
+  m_can_subscriber.push_back(m_measurement.m_nh.subscribe(m_measurement.m_can_nodeid + "_2021sub8", m_subscribe_queue_size, &sick_line_guidance::CanOlsSubscriber::cancallbackCode, this));
+  // m_can_subscriber.push_back(m_measurement.m_nh.subscribe(m_measurement.m_can_nodeid + "_2021sub9", m_subscribe_queue_size, &sick_line_guidance::CanOlsSubscriber::cancallbackExtCode, this));
+  return true;
+}
+
+/*
+ * 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.
+ */
+
+void sick_line_guidance::CanOlsSubscriber::cancallbackLCP1(const boost::shared_ptr<std_msgs::Int16 const>& msg)
+{
+  boost::lock_guard<boost::mutex> publish_lockguard(m_measurement.m_measurement_mutex);
+  m_measurement.m_ols_state.position[0] = convertToLCP(msg, m_measurement.m_ols_state.position[0], "OLS/LCP1");
+  if(m_bOLS20queries)
+  {
+    m_measurement.m_ols_query_quality_of_lines.pending() = true;      // OLS20 only: query object 2021subB (quality of lines, UINT8) in object dictionary by SDO (query runs in m_measurement in a background thread)
+    m_measurement.m_ols_query_intensity_of_lines[0].pending() = true; // OLS20 only: query object 2023sub1 (intensity line 1, UINT8) in object dictionary by SDO (query runs in m_measurement in a background thread)
+  }
+  else
+  {
+    m_measurement.m_ols_state.quality_of_lines = 0;
+    m_measurement.m_ols_state.intensity_of_lines[0] = 0;
+    m_measurement.m_ols_query_quality_of_lines.pending() = false;
+    m_measurement.m_ols_query_intensity_of_lines[0].pending() = false;
+  }
+  publishOLSMeasurement();
+}
+
+void sick_line_guidance::CanOlsSubscriber::cancallbackLCP2(const boost::shared_ptr<std_msgs::Int16 const>& msg)
+{
+  boost::lock_guard<boost::mutex> publish_lockguard(m_measurement.m_measurement_mutex);
+  m_measurement.m_ols_state.position[1] = convertToLCP(msg, m_measurement.m_ols_state.position[1], "OLS/LCP2");
+  if(m_bOLS20queries)
+  {
+    m_measurement.m_ols_query_quality_of_lines.pending() = true;      // OLS20 only: query object 2021subB (quality of lines, UINT8) in object dictionary by SDO (query runs in m_measurement in a background thread)
+    m_measurement.m_ols_query_intensity_of_lines[1].pending() = true; // OLS20 only: query object 2023sub2 (intensity line 2, UINT8) in object dictionary by SDO (query runs in m_measurement in a background thread)
+  }
+  else
+  {
+    m_measurement.m_ols_state.quality_of_lines = 0;
+    m_measurement.m_ols_state.intensity_of_lines[1] = 0;
+    m_measurement.m_ols_query_quality_of_lines.pending() = false;
+    m_measurement.m_ols_query_intensity_of_lines[1].pending() = false;
+  }
+  publishOLSMeasurement();
+}
+
+void sick_line_guidance::CanOlsSubscriber::cancallbackLCP3(const boost::shared_ptr<std_msgs::Int16 const>& msg)
+{
+  boost::lock_guard<boost::mutex> publish_lockguard(m_measurement.m_measurement_mutex);
+  m_measurement.m_ols_state.position[2] = convertToLCP(msg, m_measurement.m_ols_state.position[2], "OLS/LCP3");
+  if(m_bOLS20queries)
+  {
+    m_measurement.m_ols_query_quality_of_lines.pending() = true;      // OLS20 only: query object 2021subB (quality of lines, UINT8) in object dictionary by SDO (query runs in m_measurement in a background thread)
+    m_measurement.m_ols_query_intensity_of_lines[2].pending() = true; // OLS20 only: query object 2023sub3 (intensity line 3, UINT8) in object dictionary by SDO (query runs in m_measurement in a background thread)
+  }
+  else
+  {
+    m_measurement.m_ols_state.quality_of_lines = 0;
+    m_measurement.m_ols_state.intensity_of_lines[2] = 0;
+    m_measurement.m_ols_query_quality_of_lines.pending() = false;
+    m_measurement.m_ols_query_intensity_of_lines[2].pending() = false;
+  }
+  publishOLSMeasurement();
+}
+
+void sick_line_guidance::CanOlsSubscriber::cancallbackState(const boost::shared_ptr<std_msgs::UInt8 const>& msg)
+{
+  boost::lock_guard<boost::mutex> publish_lockguard(m_measurement.m_measurement_mutex);
+  m_measurement.m_ols_state.status = convertIntegerMessage<std_msgs::UInt8,uint8_t>(msg, m_measurement.m_ols_state.status, 0xFF, "OLS/status");
+  // If Bit 4 of m_ols_state.status is set (i.e. device status != 0), an error occured. In this case, we have to query SDOs:
+  // a) object 0x1001 in object dictionary -> measurement.error
+  // b) object 0x2018 in object dictionary -> measurement.dev_status
+  if(!sick_line_guidance::MsgUtil::statusOK(m_measurement.m_ols_state)) // Bit 4 OLS status != 0 -> SDO request 0x1001 (error register, UINT8) and 0x2018 (device status register, UINT8)
+  {
+    m_measurement.m_ols_query_error_register.pending() = true; // query object 0x1001 (error register, UINT8) in object dictionary by SDO (query runs in m_measurement in a background thread)
+    if(m_bOLS20queries) // OLS20: query object 0x2018 (device status register, UINT8) in object dictionary by SDO (query runs in m_measurement in a background thread)
+    {
+      m_measurement.m_ols_query_device_status_u8.pending() = true;
+      m_measurement.m_ols_query_device_status_u16.pending() = false;
+    }
+    else // OLS10: query object 0x2018 (device status register, UINT16) in object dictionary by SDO (query runs in m_measurement in a background thread)
+    {
+      m_measurement.m_ols_query_device_status_u8.pending() = false;
+      m_measurement.m_ols_query_device_status_u16.pending() = true;
+    }
+  }
+  else
+  {
+    m_measurement.m_ols_state.error = 0;
+    m_measurement.m_ols_state.dev_status = 0;
+    m_measurement.m_ols_query_device_status_u8.pending() = false;
+    m_measurement.m_ols_query_device_status_u16.pending() = false;
+    m_measurement.m_ols_query_error_register.pending() = false;
+  }
+  publishOLSMeasurement();
+}
+
+void sick_line_guidance::CanOlsSubscriber::cancallbackWidthLCP1(const boost::shared_ptr<std_msgs::Int16 const>& msg)
+{
+  boost::lock_guard<boost::mutex> publish_lockguard(m_measurement.m_measurement_mutex);
+  m_measurement.m_ols_state.width[0] = convertToLCP(msg, m_measurement.m_ols_state.width[0], "OLS/WidthLCP1");
+  publishOLSMeasurement();
+}
+
+void sick_line_guidance::CanOlsSubscriber::cancallbackWidthLCP2(const boost::shared_ptr<std_msgs::Int16 const>& msg)
+{
+  boost::lock_guard<boost::mutex> publish_lockguard(m_measurement.m_measurement_mutex);
+  m_measurement.m_ols_state.width[1] = convertToLCP(msg, m_measurement.m_ols_state.width[1], "OLS/WidthLCP2");
+  publishOLSMeasurement();
+}
+
+void sick_line_guidance::CanOlsSubscriber::cancallbackWidthLCP3(const boost::shared_ptr<std_msgs::Int16 const>& msg)
+{
+  boost::lock_guard<boost::mutex> publish_lockguard(m_measurement.m_measurement_mutex);
+  m_measurement.m_ols_state.width[2] = convertToLCP(msg, m_measurement.m_ols_state.width[2], "OLS/WidthLCP3");
+  publishOLSMeasurement();
+}
+
+void sick_line_guidance::CanOlsSubscriber::cancallbackCode(const boost::shared_ptr<std_msgs::UInt8 const>& msg)
+{
+  boost::lock_guard<boost::mutex> publish_lockguard(m_measurement.m_measurement_mutex);
+  m_measurement.m_ols_state.barcode = convertIntegerMessage<std_msgs::UInt8,uint8_t>(msg, m_measurement.m_ols_state.barcode, 0xFF, "OLS/barcode");
+  // If barcode >= 255, we have to query object 0x2021sub9 (extended code, UINT32) in object dictionary by SDOs
+  if(m_measurement.m_ols_state.barcode >= 255)
+  {
+    m_measurement.m_ols_query_extended_code.pending() = true; // query object 0x2021sub9 (extended code, UINT32) in object dictionary by SDO (query runs in m_measurement in a background thread)
+  }
+  else
+  {
+    m_measurement.m_ols_query_extended_code.pending() = false;
+    m_measurement.m_ols_state.extended_code = 0;
+  }
+  // OLS20 only: query object 2021subA (barcode center point, INT16) in object dictionary by SDO (query runs in m_measurement in a background thread)
+  if(m_measurement.m_ols_state.barcode > 0 && m_bOLS20queries)
+  {
+    m_measurement.m_ols_query_barcode_center_point.pending() = true;
+  }
+  else
+  {
+    m_measurement.m_ols_state.barcode_center_point = 0;
+    m_measurement.m_ols_query_barcode_center_point.pending() = false;
+  }
+  publishOLSMeasurement();
+}
+
+// void sick_line_guidance::CanOlsSubscriber::cancallbackError(const boost::shared_ptr<std_msgs::UInt8 const>& msg)
+// {
+//   // Note: Object 0x1001 (error register) is NOT PDO mapped -> Query 0x1001 ((error register) in case of error flag in m_ols_state.status
+//   boost::lock_guard<boost::mutex> publish_lockguard(m_measurement.m_measurement_mutex);
+//   m_measurement.m_ols_state.error = convertIntegerMessage<std_msgs::UInt8,uint8_t>(msg, m_measurement.m_ols_state.error, 0xFF, "OLS/error");
+//   publishOLSMeasurement();
+// }
+
+// void sick_line_guidance::CanOlsSubscriber::cancallbackDevState(const boost::shared_ptr<std_msgs::UInt8 const>& msg)
+// {
+//   // Note: Object 0x2018 is NOT PDO mapped -> Query 0x2018 (device state) in case of error flag in m_ols_state.status
+//   boost::lock_guard<boost::mutex> publish_lockguard(m_measurement.m_measurement_mutex);
+//   m_measurement.m_ols_state.dev_status = convertIntegerMessage<std_msgs::UInt8,uint8_t>(msg, m_measurement.m_ols_state.dev_status, 0xFF, "OLS/DevState");
+//   publishOLSMeasurement();
+// }
+
+// void sick_line_guidance::CanOlsSubscriber::cancallbackExtCode(const boost::shared_ptr<std_msgs::UInt32 const>& msg)
+// {
+//   // Note: Object 0x2021sub9 is NOT PDO mapped -> Query 0x2021sub9 (extended code) in case of m_ols_state.barcode == 0xFF
+//   boost::lock_guard<boost::mutex> publish_lockguard(m_measurement.m_measurement_mutex);
+//   m_measurement.m_ols_state.extended_code = convertIntegerMessage<std_msgs::UInt32,uint32_t>(msg, m_measurement.m_ols_state.extended_code, 0xFFFFFFFF, "OLS/extcode");
+//   publishOLSMeasurement();
+// }
+
+/*
+ * class CanOls10Subscriber derives from CanOlsSubscriber to implements OLS10 specific handling of the ros canopen messages.
+ */
+
+/*
+ * 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
+ */
+sick_line_guidance::CanOls10Subscriber::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, int subscribe_queue_size)
+  : sick_line_guidance::CanOlsSubscriber::CanOlsSubscriber(nh, can_nodeid, max_num_retries_after_sdo_error, max_sdo_rate, ros_topic, ros_frameid, initial_sensor_state, subscribe_queue_size)
+{
+}
+
+/*
+ * class CanOls20Subscriber derives from CanOlsSubscriber to implements OLS20 specific handling of the ros canopen messages.
+ */
+
+/*
+ * 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
+ */
+sick_line_guidance::CanOls20Subscriber::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, int subscribe_queue_size)
+  : sick_line_guidance::CanOlsSubscriber::CanOlsSubscriber(nh, can_nodeid, max_num_retries_after_sdo_error, max_sdo_rate, ros_topic, ros_frameid, initial_sensor_state, subscribe_queue_size)
+{
+  m_bOLS20queries = true; // OLS20 only: query objects 2021subA (barcode center point, INT16), 2021subB (quality of lines, UINT8) and 2023sub1 to 2023sub3 (intensity line 1 - 3, UINT8)
+}

Devices/Packages/sick_line_guidance/src/sick_line_guidance_can_subscriber.cpp

@@ -0,0 +1,523 @@
+/*
+ * 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
+ * 
+ * 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 <cstdint>
+#include "sick_line_guidance/sick_line_guidance_canopen_chain.h"
+#include "sick_line_guidance/sick_line_guidance_can_subscriber.h"
+#include "sick_line_guidance/sick_line_guidance_diagnostic.h"
+#include "sick_line_guidance/sick_line_guidance_msg_util.h"
+
+/*
+ * Constructor.
+ * @param[in] nh ros::NodeHandle
+ * @param[in] can_nodeid can id for canopen_chain_node, f.e. "node1"
+ * @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.
+ * @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.
+ * By default, a sdo request is send, if the query is pending and not done within the last 10 ms.
+ */
+sick_line_guidance::CanSubscriber::MeasurementHandler::MeasurementHandler(ros::NodeHandle &nh, const std::string &can_nodeid, int max_num_retries_after_sdo_error, int initial_sensor_state, double max_publish_rate, double max_query_rate, double schedule_publish_delay, double max_publish_delay, double query_jitter)
+  : m_nh(nh), m_can_nodeid(can_nodeid), m_max_publish_rate(ros::Rate(max_publish_rate)), m_max_sdo_query_rate(ros::Rate(max_query_rate)), 
+    m_schedule_publish_delay(ros::Duration(schedule_publish_delay)), m_max_publish_delay(ros::Duration(max_publish_delay)), m_max_num_retries_after_sdo_error(max_num_retries_after_sdo_error)
+{
+  // initialize MLS/OLS sensor states
+  sick_line_guidance::MsgUtil::zero(m_mls_state);
+  m_mls_state.header.stamp = ros::Time::now();
+  m_mls_state.lcp = static_cast<uint8_t>(initial_sensor_state);
+  m_mls_state.status = ((initial_sensor_state & 0x7) ? 1 : 0);
+  sick_line_guidance::MsgUtil::zero(m_ols_state);
+  m_ols_state.header.stamp = ros::Time::now();
+  m_ols_state.status = initial_sensor_state;
+  // initialize publisher thread
+  m_publish_mls_measurement = ros::Time(0);
+  m_publish_ols_measurement = ros::Time(0);
+  m_publish_measurement_latest = ros::Time(0);
+  m_ols_query_extended_code = QuerySupport(query_jitter);
+  m_ols_query_device_status_u8 = QuerySupport(query_jitter);
+  m_ols_query_device_status_u16 = QuerySupport(query_jitter);
+  m_ols_query_error_register = QuerySupport(query_jitter);
+  m_ols_query_barcode_center_point = QuerySupport(query_jitter);
+  m_ols_query_quality_of_lines = QuerySupport(query_jitter);
+  m_ols_query_intensity_of_lines[0] = QuerySupport(query_jitter);
+  m_ols_query_intensity_of_lines[1] = QuerySupport(query_jitter);
+  m_ols_query_intensity_of_lines[2] = QuerySupport(query_jitter);
+  m_measurement_publish_thread = new boost::thread(&sick_line_guidance::CanSubscriber::MeasurementHandler::runMeasurementPublishThread, this);
+  m_measurement_sdo_query_thread = new boost::thread(&sick_line_guidance::CanSubscriber::MeasurementHandler::runMeasurementSDOqueryThread, this);
+}
+
+/*
+ * Destructor.
+ */
+sick_line_guidance::CanSubscriber::MeasurementHandler::~MeasurementHandler()
+{
+  if (m_measurement_sdo_query_thread)
+  {
+    m_measurement_sdo_query_thread->join();
+    delete (m_measurement_sdo_query_thread);
+    m_measurement_sdo_query_thread = 0;
+  }
+  if (m_measurement_publish_thread)
+  {
+    m_measurement_publish_thread->join();
+    delete (m_measurement_publish_thread);
+    m_measurement_publish_thread = 0;
+  }
+}
+
+/*
+ * @brief Runs the background thread to publish MLS/OLS measurement messages
+ */
+void sick_line_guidance::CanSubscriber::MeasurementHandler::runMeasurementPublishThread(void)
+{
+  while(ros::ok())
+  {
+    // Publish mls measurement (if one has been triggered, i.e. a pdo has been received recently)
+    if(isMLSMeasurementTriggered())
+    {
+      sick_line_guidance::MLS_Measurement measurement_msg;
+      {
+        boost::lock_guard<boost::mutex> publish_lockguard(m_measurement_mutex);
+        m_mls_state.header.stamp = ros::Time::now();
+        measurement_msg = m_mls_state;
+      }
+      m_ros_publisher.publish(measurement_msg);
+      schedulePublishMLSMeasurement(false);
+      bool line_good = sick_line_guidance::MsgUtil::lineOK(measurement_msg); // 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
+      ROS_INFO_STREAM("sick_line_guidance::MLS_Measurement: {" << sick_line_guidance::MsgUtil::toInfo(measurement_msg) << ",line_good=" << line_good << "}");
+      /* if(line_good)
+        sick_line_guidance::Diagnostic::update(sick_line_guidance::DIAGNOSTIC_STATUS::OK, "MLS Measurement published");
+      else
+        sick_line_guidance::Diagnostic::update(sick_line_guidance::DIAGNOSTIC_STATUS::NO_LINE_DETECTED, "MLS Measurement published, no line"); */
+    }
+    // Publish ols measurement (if one has been triggered, i.e. a pdo has been received recently)
+    if(isOLSMeasurementTriggered() && (!isSDOQueryPending() || isLatestTimeForMeasurementPublishing())) // no sdo requests are pending or latest time to publish a new measurement is reached
+    {
+      sick_line_guidance::OLS_Measurement measurement_msg;
+      {
+        boost::lock_guard<boost::mutex> publish_lockguard(m_measurement_mutex);
+        m_ols_state.header.stamp = ros::Time::now();
+        measurement_msg = m_ols_state;
+      }
+      m_ros_publisher.publish(measurement_msg);
+      schedulePublishOLSMeasurement(false);
+      bool status_ok = sick_line_guidance::MsgUtil::statusOK(measurement_msg); // OLS status bit 4: 0 => Sensor ok, 1 => Sensor not ok => 0x2018 (measurement_msg.dev_status)
+      bool line_good = status_ok && sick_line_guidance::MsgUtil::lineOK(measurement_msg); // Bit 0-2 OLS status == 0 => no line found
+      ROS_INFO_STREAM("sick_line_guidance::OLS_Measurement: {" << sick_line_guidance::MsgUtil::toInfo(measurement_msg) << ",status_ok=" << status_ok << ",line_good=" << line_good << "}");
+      if(!status_ok)
+        sick_line_guidance::Diagnostic::update(sick_line_guidance::DIAGNOSTIC_STATUS::ERROR_STATUS, "OLS Measurement published, status error " + sick_line_guidance::MsgUtil::toHexString(measurement_msg.dev_status));
+      /* else if(!line_good)
+        sick_line_guidance::Diagnostic::update(sick_line_guidance::DIAGNOSTIC_STATUS::NO_LINE_DETECTED, "OLS Measurement published, no line");
+      else
+        sick_line_guidance::Diagnostic::update(sick_line_guidance::DIAGNOSTIC_STATUS::OK, "OLS Measurement published"); */
+    }
+    m_max_publish_rate.sleep();
+  }
+}
+
+/*
+ * @brief Runs the background thread to query SDOs, if required
+ */
+void sick_line_guidance::CanSubscriber::MeasurementHandler::runMeasurementSDOqueryThread(void)
+{
+  while(ros::ok())
+  {
+    // Query SDOs if required
+    querySDOifPending<uint32_t, uint32_t>(m_ols_query_extended_code,         "2021sub9", m_max_num_retries_after_sdo_error, m_ols_state.extended_code,         1);      // OLS: query object 0x2021sub9 (extended code, UINT32) in object dictionary by SDO
+    querySDOifPending<uint8_t,  uint16_t>(m_ols_query_device_status_u8,      "2018",     m_max_num_retries_after_sdo_error, m_ols_state.dev_status,            1);      // OLS20: query object 0x2018 (device status register, UINT8) in object dictionary by SDO
+    querySDOifPending<uint16_t, uint16_t>(m_ols_query_device_status_u16,     "2018",     m_max_num_retries_after_sdo_error, m_ols_state.dev_status,            1);      // OLS10: query object 0x2018 (device status register, UINT16) in object dictionary by SDO
+    querySDOifPending<uint8_t,  uint8_t> (m_ols_query_error_register,        "1001",     m_max_num_retries_after_sdo_error, m_ols_state.error,                 1);      // OLS: query object 0x1001 (error register, UINT8) in object dictionary by SDO
+    querySDOifPending<int16_t,  float>   (m_ols_query_barcode_center_point,  "2021subA", m_max_num_retries_after_sdo_error, m_ols_state.barcode_center_point,  0.001f); // OLS20 only: query object 2021subA (barcode center point, INT16) in object dictionary by SDO
+    querySDOifPending<uint8_t,  uint8_t> (m_ols_query_quality_of_lines,      "2021subB", m_max_num_retries_after_sdo_error, m_ols_state.quality_of_lines,      1);      // OLS20 only: query object 2021subB (quality of lines, UINT8) in object dictionary by SDO
+    querySDOifPending<uint8_t,  uint8_t> (m_ols_query_intensity_of_lines[0], "2023sub1", m_max_num_retries_after_sdo_error, m_ols_state.intensity_of_lines[0], 1);      // OLS20 only: query object 2023sub1 (intensity line 1, UINT8)
+    querySDOifPending<uint8_t,  uint8_t> (m_ols_query_intensity_of_lines[1], "2023sub2", m_max_num_retries_after_sdo_error, m_ols_state.intensity_of_lines[1], 1);      // OLS20 only: query object 2023sub2 (intensity line 2, UINT8)
+    querySDOifPending<uint8_t,  uint8_t> (m_ols_query_intensity_of_lines[2], "2023sub3", m_max_num_retries_after_sdo_error, m_ols_state.intensity_of_lines[2], 1);      // OLS20 only: query object 2023sub3 (intensity line 3, UINT8)
+    // Clear all pending status
+    m_ols_query_extended_code.pending() = false;
+    m_ols_query_device_status_u8.pending() = false;
+    m_ols_query_device_status_u16.pending() = false;
+    m_ols_query_error_register.pending() = false;
+    m_ols_query_barcode_center_point.pending() = false;
+    m_ols_query_quality_of_lines.pending() = false;
+    m_ols_query_intensity_of_lines[0].pending() = false;
+    m_ols_query_intensity_of_lines[1].pending() = false;
+    m_ols_query_intensity_of_lines[2].pending() = false;
+    m_max_sdo_query_rate.sleep();
+  }
+}
+
+/*
+ * @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)
+ */
+bool sick_line_guidance::CanSubscriber::MeasurementHandler::querySDO(const std::string & can_object_idx, int max_num_retries_after_sdo_error, uint8_t & can_object_value)
+{
+  std::string can_object_entry = "";
+  if(querySDO(can_object_idx, max_num_retries_after_sdo_error, can_object_entry) && convertSDOresponse(can_object_entry, can_object_value))
+   return true;
+  ROS_ERROR_STREAM("querySDO(" << can_object_idx << ",uint8_t) failed, value=" << sick_line_guidance::MsgUtil::toHexString(can_object_value) );
+  return false;
+}
+
+/*
+ * @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)
+ */
+bool sick_line_guidance::CanSubscriber::MeasurementHandler::querySDO(const std::string & can_object_idx, int max_num_retries_after_sdo_error, int16_t & can_object_value)
+{
+  int32_t value = 0;
+  std::string can_object_entry = "";
+  if(querySDO(can_object_idx, max_num_retries_after_sdo_error, can_object_entry) && convertSDOresponse(can_object_entry, value) && value >= INT16_MIN && value <= INT16_MAX)
+  {
+    can_object_value = (int16_t)value;
+    return true;
+  }
+  ROS_ERROR_STREAM("querySDO(" << can_object_idx << ",int16_t) failed, value=" << sick_line_guidance::MsgUtil::toHexString(can_object_value) );
+  return false;
+}
+
+/*
+ * @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)
+ */
+bool sick_line_guidance::CanSubscriber::MeasurementHandler::querySDO(const std::string & can_object_idx, int max_num_retries_after_sdo_error, uint16_t & can_object_value)
+{
+  uint32_t value = 0;
+  std::string can_object_entry = "";
+  if(querySDO(can_object_idx, max_num_retries_after_sdo_error, can_object_entry) && convertSDOresponse(can_object_entry, value) && value <= UINT16_MAX)
+  {
+    can_object_value = (uint16_t)value;
+    return true;
+  }
+  ROS_ERROR_STREAM("querySDO(" << can_object_idx << ",uint16_t) failed, value=" << sick_line_guidance::MsgUtil::toHexString(can_object_value) );
+  return false;
+}
+
+/*
+ * @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)
+ */
+bool sick_line_guidance::CanSubscriber::MeasurementHandler::querySDO(const std::string & can_object_idx, int max_num_retries_after_sdo_error, uint32_t & can_object_value)
+{
+  std::string can_object_entry = "";
+  if(querySDO(can_object_idx, max_num_retries_after_sdo_error, can_object_entry) && convertSDOresponse(can_object_entry, can_object_value))
+    return true;
+  ROS_ERROR_STREAM("querySDO(" << can_object_idx << ",uint32_t) failed, value=" << sick_line_guidance::MsgUtil::toHexString(can_object_value) );
+  return false;
+}
+
+/*
+ * @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)
+ */
+bool sick_line_guidance::CanSubscriber::MeasurementHandler::querySDO(const std::string & can_object_idx, int max_num_retries_after_sdo_error, std::string & can_object_value)
+{
+  std::string can_msg = "";
+  can_object_value = "";
+  bool sdo_success = sick_line_guidance::CanopenChain::queryCanObject(m_nh, m_can_nodeid, can_object_idx, max_num_retries_after_sdo_error, can_msg, can_object_value);
+  if(sdo_success)
+  {
+    ROS_INFO_STREAM("sick_line_guidance::CanopenChain::queryCanObject(" << m_can_nodeid << "): [" << can_object_idx << "]=" << can_object_value );
+    /* sick_line_guidance::Diagnostic::update(sick_line_guidance::DIAGNOSTIC_STATUS::OK, "SDO"); */
+  }
+  else
+  {
+    ROS_ERROR_STREAM("sick_line_guidance::CanopenChain::queryCanObject(" << m_can_nodeid << "): [" << can_object_idx << "]=\"" << can_object_value << "\" failed " << can_msg);
+    sick_line_guidance::Diagnostic::update(sick_line_guidance::DIAGNOSTIC_STATUS::SDO_COMMUNICATION_ERROR, "querySDO failed");
+  }
+  return sdo_success;
+}
+
+/*
+ * @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
+ */
+bool sick_line_guidance::CanSubscriber::MeasurementHandler::convertSDOresponse(const std::string & response, uint8_t & value)
+{
+  value = 0;
+  try
+  {
+    if(response.empty())
+      value = 0;
+    else if(response.size() == 1)
+      value = static_cast<uint8_t>(response[0]);
+    else
+    {
+      uint32_t u32val = std::stoul(response, 0, 0);
+      if((u32val & 0xFFFFFF00) != 0)
+        ROS_WARN_STREAM("convertSDOresponse(" << response << ") to UINT8: value " << u32val << " out of UINT8 range, possible loss of data.");
+      value = static_cast<uint8_t>(u32val & 0xFF);
+    }
+    return true;
+  }
+  catch(const std::exception & exc)
+  {
+    ROS_ERROR_STREAM("convertSDOresponse(" << response << ") to UINT8 failed: exception = " << exc.what());
+  }
+  return false;
+}
+
+/*
+ * @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
+ */
+bool sick_line_guidance::CanSubscriber::MeasurementHandler::convertSDOresponse(const std::string & response, uint32_t & value)
+{
+  value = 0;
+  try
+  {
+    value = std::stoul(response, 0, 0);
+    return true;
+  }
+  catch(const std::exception & exc)
+  {
+    ROS_ERROR_STREAM("convertSDOresponse(" << response << ") to UINT32 failed: exception = " << exc.what());
+  }
+  return false;
+}
+
+/*
+ * @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
+ */
+bool sick_line_guidance::CanSubscriber::MeasurementHandler::convertSDOresponse(const std::string & response, int32_t & value)
+{
+  value = 0;
+  try
+  {
+    value = std::stol(response, 0, 0);
+    return true;
+  }
+  catch(const std::exception & exc)
+  {
+    ROS_ERROR_STREAM("convertSDOresponse(" << response << ") to INT32 failed: exception = " << exc.what());
+  }
+  return false;
+}
+
+/*
+ * @brief returns true, if publishing of a MLS measurement is scheduled and time has been reached for publishing the current MLS measurement.
+ */
+bool sick_line_guidance::CanSubscriber::MeasurementHandler::isMLSMeasurementTriggered(void)
+{
+  boost::lock_guard<boost::mutex> schedule_lockguard(m_publish_measurement_mutex);
+  return !m_publish_mls_measurement.isZero() && ros::Time::now() > m_publish_mls_measurement;
+}
+
+/*
+ * @brief returns true, if publishing of a OLS measurement is scheduled and time has been reached for publishing the current OLS measurement.
+ */
+bool sick_line_guidance::CanSubscriber::MeasurementHandler::isOLSMeasurementTriggered(void)
+{
+  boost::lock_guard<boost::mutex> schedule_lockguard(m_publish_measurement_mutex);
+  return !m_publish_ols_measurement.isZero() && ros::Time::now() > m_publish_ols_measurement;
+}
+
+/*
+ * @brief returns true, if publishing of a measurement is scheduled and latest time for publishing has been reached.
+ */
+bool sick_line_guidance::CanSubscriber::MeasurementHandler::isLatestTimeForMeasurementPublishing(void)
+{
+  boost::lock_guard<boost::mutex> schedule_lockguard(m_publish_measurement_mutex);
+  return !m_publish_measurement_latest.isZero() && ros::Time::now() > m_publish_measurement_latest;
+}
+
+
+/*
+ * @brief returns true, if sdo query is pending, i.e. measurement is not yet completed (sdo request or sdo response still pending)
+ */
+bool sick_line_guidance::CanSubscriber::MeasurementHandler::isSDOQueryPending(void)
+{
+  boost::lock_guard<boost::mutex> schedule_lockguard(m_publish_measurement_mutex);
+  return (m_ols_query_extended_code.pending() || m_ols_query_device_status_u8.pending() || m_ols_query_device_status_u16.pending() || m_ols_query_error_register.pending() || m_ols_query_barcode_center_point.pending()
+    || m_ols_query_quality_of_lines.pending() || m_ols_query_intensity_of_lines[0].pending() || m_ols_query_intensity_of_lines[1].pending() || m_ols_query_intensity_of_lines[2].pending());
+}
+
+/*
+ * @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.
+ */
+void sick_line_guidance::CanSubscriber::MeasurementHandler::schedulePublishMLSMeasurement(bool schedule)
+{
+  boost::lock_guard<boost::mutex> schedule_lockguard(m_publish_measurement_mutex);
+  if(schedule && m_publish_mls_measurement.isZero()) // otherwise publishing the measurement is already scheduled
+  {
+    m_publish_mls_measurement = ros::Time::now() + m_schedule_publish_delay;
+    m_publish_measurement_latest = ros::Time::now() + m_max_publish_delay;
+  }
+  if(!schedule && !m_publish_mls_measurement.isZero()) // remove pending schedule
+  {
+    m_publish_mls_measurement = ros::Time(0);
+    m_publish_measurement_latest = ros::Time(0);
+  }
+}
+
+/*
+ * @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.
+ */
+void sick_line_guidance::CanSubscriber::MeasurementHandler::schedulePublishOLSMeasurement(bool schedule)
+{
+  boost::lock_guard<boost::mutex> schedule_lockguard(m_publish_measurement_mutex);
+  if(schedule && m_publish_ols_measurement.isZero()) // otherwise publishing the measurement is already scheduled
+  {
+    m_publish_ols_measurement = ros::Time::now() + m_schedule_publish_delay;
+    m_publish_measurement_latest = ros::Time::now() + m_max_publish_delay;
+  }
+  if(!schedule && !m_publish_ols_measurement.isZero()) // remove pending schedule
+  {
+    m_publish_ols_measurement = ros::Time(0);
+    m_publish_measurement_latest = ros::Time(0);
+  }
+}
+
+/*
+ * 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
+ */
+sick_line_guidance::CanSubscriber::CanSubscriber(ros::NodeHandle & nh, const std::string & can_nodeid, int max_num_retries_after_sdo_error, double max_sdo_rate, int initial_sensor_state, int subscribe_queue_size)
+  : m_measurement(nh, can_nodeid, max_num_retries_after_sdo_error, initial_sensor_state, max_sdo_rate, max_sdo_rate), m_subscribe_queue_size(subscribe_queue_size)
+{
+}
+
+/*
+ * Destructor.
+ */
+sick_line_guidance::CanSubscriber::~CanSubscriber()
+{
+}
+
+/*
+ * @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
+ */
+float sick_line_guidance::CanSubscriber::convertToLCP(const boost::shared_ptr<std_msgs::Int16 const>& msg,
+  const float & defaultvalue, const std::string & dbg_info)
+{
+  float value = defaultvalue;
+  if(msg)
+  {
+    int16_t data = ((msg->data)&0xFFFF);
+    value = data / 1000.0;
+    ROS_DEBUG("sick_line_guidance::CanSubscriber(%s): data: 0x%04x -> %.3f", dbg_info.c_str(), (int)(data), value);
+  }
+  else
+  {
+    ROS_ERROR("## ERROR sick_line_guidance::CanSubscriber(%s): invalid message (%s:%d)", dbg_info.c_str(), __FILE__, __LINE__);
+  }
+  return value;
+}
+
+/*
+ * @brief schedules the current MLS measurement message for publishing.
+ */
+void sick_line_guidance::CanSubscriber::publishMLSMeasurement(void)
+{
+  m_measurement.schedulePublishMLSMeasurement(true);
+}
+
+/*
+ * @brief schedules the current OLS measurement message for publishing.
+ */
+void sick_line_guidance::CanSubscriber::publishOLSMeasurement(void)
+{
+  m_measurement.schedulePublishOLSMeasurement(true);
+}
+

Devices/Packages/sick_line_guidance/src/sick_line_guidance_canopen_chain.cpp

@@ -0,0 +1,550 @@
+/*
+ * sick_line_guidance_canopen_chain wraps and adapts RosChain of package canopen_chain_node for sick_line_guidance.
+ *
+ * class CanopenChain implements canopen support for sick_line_guidance
+ * based on RosChain of package canopen_chain_node (package ros_canopen).
+ * 
+ * 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 <std_srvs/Trigger.h>
+#include "sick_line_guidance/sick_line_guidance_canopen_chain.h"
+#include "sick_line_guidance/sick_line_guidance_diagnostic.h"
+
+/*
+ * CanopenChain constructor
+ *
+ * @param[in] nh ros::NodeHandle
+ * @param[in] nh_priv ros::NodeHandle
+ */
+sick_line_guidance::CanopenChain::CanopenChain(const ros::NodeHandle &nh, const ros::NodeHandle &nh_priv)
+  : canopen::RosChain(nh, nh_priv)
+{
+}
+
+/*
+ * CanopenChain destructor
+ */
+sick_line_guidance::CanopenChain::~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.
+ */
+bool sick_line_guidance::CanopenChain::connectInitService(ros::NodeHandle &nh)
+{
+  bool success = false;
+  try
+  {
+    ros::ServiceClient servclient = nh.serviceClient<std_srvs::Trigger>("/driver/init");
+    std_srvs::Trigger servtrigger;
+    success = servclient.call(servtrigger);
+    if(!success)
+    {
+      ROS_ERROR_STREAM("sick_line_guidance::CanopenChain::connectInitService(): ServiceClient::call(\"/driver/init\") failed.");
+      sick_line_guidance::Diagnostic::update(sick_line_guidance::DIAGNOSTIC_STATUS::CAN_COMMUNICATION_ERROR, "CanopenChain: \"/driver/init\" failed");
+    }
+    else
+    {
+      ROS_INFO_STREAM("sick_line_guidance::CanopenChain::connectInitService(): ServiceClient::call(\"/driver/init\") successfull.");
+      sick_line_guidance::Diagnostic::update(sick_line_guidance::DIAGNOSTIC_STATUS::OK, "CanopenChain: \"/driver/init\" successfull");
+    }
+  }
+  catch(const std::exception & exc)
+  {
+    success = false;
+    ROS_ERROR_STREAM("sick_line_guidance::CanopenChain::connectInitService(): ServiceClient::call(\"/driver/init\") failed: exception = " << exc.what());
+    sick_line_guidance::Diagnostic::update(sick_line_guidance::DIAGNOSTIC_STATUS::CAN_COMMUNICATION_ERROR, "CanopenChain: \"/driver/init\" failed");
+  }
+  return success;
+}
+
+/*
+ * 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).
+ */
+bool sick_line_guidance::CanopenChain::writeDCFoverlays(ros::NodeHandle &nh, const std::string & node_id, XmlRpc::XmlRpcValue & dcf_overlays)
+{
+  // int32_t i32_val = 0;
+  // uint32_t u32_val = 0;
+  // float f32_val = 0;
+  // assert(tryParseUINT32("1", u32_val) && u32_val == 1);
+  // assert(tryParseUINT32("0x01", u32_val) && u32_val == 1);
+  // assert(tryParseUINT32("255", u32_val) && u32_val == 255);
+  // assert(tryParseUINT32("0xFF", u32_val) && u32_val == 0xFF);
+  // assert(tryParseUINT32("0xFFFFFFFF", u32_val) && u32_val == 0xFFFFFFFF);
+  // assert(tryParseINT32("1", i32_val) && i32_val == 1);
+  // assert(tryParseINT32("-1", i32_val) && i32_val == -1);
+  // assert(tryParseINT32("0x7FFFFFFF", i32_val) && i32_val == 0x7FFFFFFF);
+  // assert(tryParseINT32("0xFFFFFFFF", i32_val) && i32_val == -1);
+  // assert(tryParseFLOAT("1.0", f32_val) && std::fabs(f32_val - 1) < 1.0e-6);
+  // assert(tryParseFLOAT("-1.0", f32_val) && std::fabs(f32_val + 1) < 1.0e-6);
+  // assert(tryParseFLOAT("1e6", f32_val) && std::fabs(f32_val - 1.0E6) < 1.0e-6);
+  // assert(tryParseFLOAT("1.0E06", f32_val) && std::fabs(f32_val - 1.0E6) < 1.0e-6);
+  // assert(tryParseFLOAT("1e-6", f32_val) && std::fabs(f32_val - 1.0E-6) < 1.0e-6);
+  // assert(tryParseFLOAT("1.0E-06", f32_val) && std::fabs(f32_val - 1.0E-6) < 1.0e-6);
+  bool success = true;
+  for(XmlRpc::XmlRpcValue::iterator dcf_overlay_iter = dcf_overlays.begin(); dcf_overlay_iter != dcf_overlays.end(); ++dcf_overlay_iter)
+  {
+    bool dcf_success = true;
+    std::string dcf_overlay_object_index = dcf_overlay_iter->first;
+    std::string dcf_overlay_object_value = dcf_overlay_iter->second;
+    std::string sdo_message = "", sdo_response = "";
+    // Set value in object dictionary
+    if(!setCanObjectValue(nh, node_id, dcf_overlay_object_index, dcf_overlay_object_value, sdo_response))
+    {
+      dcf_success = false;
+      ROS_ERROR_STREAM("sick_line_guidance::CanopenChain::writeDCFoverlays(): setCanObjectValue(" << node_id << "," << dcf_overlay_object_index
+        << ") failed: sdo_response:\"" << sdoResponseToString(sdo_response) << "\"(\"" << sdo_response << "\")");
+      sick_line_guidance::Diagnostic::update(sick_line_guidance::DIAGNOSTIC_STATUS::SDO_COMMUNICATION_ERROR, "writeDCFoverlays failed");
+    }
+    // Re-read value in object dictionary
+    if(!queryCanObject(nh, node_id, dcf_overlay_object_index, 0, sdo_message, sdo_response))
+    {
+      dcf_success = false;
+      ROS_ERROR_STREAM("sick_line_guidance::CanopenChain::writeDCFoverlays(): queryCanObject(" << node_id << "," << dcf_overlay_object_index
+        << ") failed: sdo_message:\"" << sdo_message << "\", sdo_response:\"" << sdoResponseToString(sdo_response) << "\"(\"" << sdo_response << "\")");
+      sick_line_guidance::Diagnostic::update(sick_line_guidance::DIAGNOSTIC_STATUS::SDO_COMMUNICATION_ERROR, "writeDCFoverlays failed");
+    }
+    else
+    {
+      ROS_INFO_STREAM("sick_line_guidance::CanopenChain::writeDCFoverlays(): queryCanObject(" << node_id << "," << dcf_overlay_object_index
+        << ") successfull re-read, sdo_response:\"" << sdoResponseToString(sdo_response) << "\"(\"" << sdo_response << "\")");
+    }
+    // Compare destination value dcf_overlay_object_value with queried sdo response
+    if(!cmpDCFoverlay(dcf_overlay_object_value, sdo_response))
+    {
+      ROS_WARN_STREAM("sick_line_guidance::CanopenChain::writeDCFoverlays(): dcf_overlay \"" << dcf_overlay_object_index<< "\": \""
+        << dcf_overlay_object_value << "\" possibly failed: queried value=\"" << sdoResponseToString(sdo_response) << "\", expected value=\"" << dcf_overlay_object_value << "\"");
+    }
+    if(dcf_success)
+    {
+      ROS_INFO_STREAM("sick_line_guidance::CanopenChain::writeDCFoverlays(): dcf_overlay \"" << dcf_overlay_object_index<< "\": \""
+        << dcf_overlay_object_value << "\" sucessfull, value=\"" << sdoResponseToString(sdo_response) << "\"(\"" << sdo_response << "\")");
+    }
+    else
+    {
+      ROS_ERROR_STREAM("sick_line_guidance::CanopenChain::writeDCFoverlays(): dcf_overlay \"" << dcf_overlay_object_index<< "\": \""
+        << dcf_overlay_object_value << "\" failed, value=\"" << sdoResponseToString(sdo_response) << "\"(\"" << sdo_response << "\")");
+    }
+    success = dcf_success && success;
+  }
+  if(success)
+    sick_line_guidance::Diagnostic::update(sick_line_guidance::DIAGNOSTIC_STATUS::OK);
+  else
+    sick_line_guidance::Diagnostic::update(sick_line_guidance::DIAGNOSTIC_STATUS::SDO_COMMUNICATION_ERROR, "writeDCFoverlays failed");
+  return success;
+}
+
+/*
+ * 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.
+ */
+bool sick_line_guidance::CanopenChain::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)
+{
+  bool sdo_success = false;
+  for(int retry_cnt = 0; sdo_success == false; retry_cnt++)
+  {
+    try
+    {
+      output_message = "";
+      output_value = "";
+      ros::ServiceClient servclient = nh.serviceClient<canopen_chain_node::GetObject>("/driver/get_object");
+      canopen_chain_node::GetObject servobject;
+      servobject.request.node = node_id;
+      servobject.request.object = objectidx;
+      servobject.request.cached = false;
+      if(servclient.call(servobject))
+      {
+        output_message = servobject.response.message;
+        output_value = servobject.response.value;
+        sdo_success = servobject.response.success;
+        if(!sdo_success)
+        {
+          ROS_ERROR_STREAM("sick_line_guidance::CanopenChain::queryCanObject(): ServiceClient::call(\"/driver/get_object\") failed: " << output_message);
+        }
+      }
+      else
+      {
+        ROS_ERROR_STREAM("sick_line_guidance::CanopenChain::queryCanObject(): ServiceClient::call(\"/driver/get_object\") failed.");
+      }
+    }
+    catch(const std::exception & exc)
+    {
+      sdo_success = false;
+      ROS_ERROR_STREAM("sick_line_guidance::CanopenChain::queryCanObject(" << node_id << "): [" << objectidx << "]=\"" << output_value << "\" failed: " << output_message << " exception = " << exc.what());
+    }
+    if(!sdo_success)
+    {
+      // SDO failed
+      sick_line_guidance::Diagnostic::update(sick_line_guidance::DIAGNOSTIC_STATUS::SDO_COMMUNICATION_ERROR, "CanopenChain::queryCanObject failed");
+      if (retry_cnt < max_num_retries_after_sdo_error)
+      {
+        ROS_ERROR_STREAM("sick_line_guidance::CanopenChain::queryCanObject(" << node_id << ") failed: SDO error, retrying...");
+        continue;
+      }
+      else
+      {
+        // SDO failed, reset communication ("driver/recover")
+        // ROS_ERROR_STREAM("sick_line_guidance::CanopenChain::queryCanObject(\" << node_id << \"): resetting can communication after SDO error");
+        // if(recoverCanDriver(nh))
+        //   ROS_INFO_STREAM("sick_line_guidance::CanopenChain::queryCanObject(\" << node_id << \"): recoverCanDriver() successfull.");
+        // else
+        //   ROS_ERROR_STREAM("sick_line_guidance::CanopenChain::queryCanObject(\" << node_id << \"): recoverCanDriver() failed.");
+        // SDO failed, shutdown communication ("driver/shutdown") and re-initialize ("driver/init")
+        ROS_ERROR_STREAM("sick_line_guidance::CanopenChain::queryCanObject(" << node_id << "): unrecoverable SDO error, retries not successful, giving up, initiating can driver shutdown and restart after SDO error");
+        if(shutdownCanDriver(nh))
+          ROS_INFO_STREAM("sick_line_guidance::CanopenChain::queryCanObject(" << node_id << "): shutdownCanDriver() successfull.");
+        else
+          ROS_ERROR_STREAM("sick_line_guidance::CanopenChain::queryCanObject(" << node_id << "): shutdownCanDriver() failed.");
+        if(connectInitService(nh))
+          ROS_INFO_STREAM("sick_line_guidance::CanopenChain::queryCanObject(" << node_id << "): connectInitService() successfull.");
+        else
+          ROS_ERROR_STREAM("sick_line_guidance::CanopenChain::queryCanObject(" << node_id << "): connectInitService() failed.");
+        break;
+      }
+    }
+  }
+  return sdo_success;
+}
+
+/*
+ * 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.
+ */
+bool sick_line_guidance::CanopenChain::setCanObjectValue(ros::NodeHandle &nh, const std::string & node_id, const std::string &  objectidx,
+  const std::string & object_value, std::string & response)
+{
+  bool sdo_success = false;
+  try
+  {
+    response = "";
+    ros::ServiceClient servclient = nh.serviceClient<canopen_chain_node::SetObject>("/driver/set_object");
+    canopen_chain_node::SetObject servobject;
+    servobject.request.node = node_id;
+    servobject.request.object = objectidx;
+    servobject.request.value = object_value;
+    servobject.request.cached = false;
+    if(servclient.call(servobject))
+    {
+      response = servobject.response.message;
+      sdo_success = servobject.response.success;
+      if(!sdo_success)
+      {
+        ROS_ERROR_STREAM("sick_line_guidance::CanopenChain::setCanObjectValue(): ServiceClient::call(\"/driver/set_object\") failed: " << response);
+      }
+    }
+    else
+    {
+      ROS_ERROR_STREAM("sick_line_guidance::CanopenChain::setCanObjectValue(): ServiceClient::call(\"/driver/set_object\") failed.");
+    }
+  }
+  catch(const std::exception & exc)
+  {
+    sdo_success = false;
+    ROS_ERROR_STREAM("sick_line_guidance::CanopenChain::setCanObjectValue(" << node_id << ", " << objectidx << ") failed: exception = " << exc.what());
+  }
+  if(!sdo_success)
+    sick_line_guidance::Diagnostic::update(sick_line_guidance::DIAGNOSTIC_STATUS::SDO_COMMUNICATION_ERROR, "CanopenChain::setCanObjectValue failed");
+  /* else
+    sick_line_guidance::Diagnostic::update(sick_line_guidance::DIAGNOSTIC_STATUS::OK); */
+  return sdo_success;
+}
+
+/*
+ * 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.
+ */
+bool sick_line_guidance::CanopenChain::recoverCanDriver(ros::NodeHandle & nh)
+{
+  bool success = false;
+  try
+  {
+    ROS_WARN_STREAM("sick_line_guidance::CanopenChain::recoverCanDriver(): resetting can communication by ServiceClient::call(\"/driver/recover\")...");
+    ros::ServiceClient servclient = nh.serviceClient<std_srvs::Trigger>("/driver/recover");
+    std_srvs::Trigger servobject;
+    if(servclient.call(servobject))
+    {
+      success = servobject.response.success;
+      if(success)
+      {
+        ROS_INFO_STREAM("sick_line_guidance::CanopenChain::recoverCanDriver(): ServiceClient::call(\"/driver/recover\") successfull " << servobject.response.message.c_str());
+      }
+      else
+      {
+        ROS_ERROR_STREAM("sick_line_guidance::CanopenChain::recoverCanDriver(): ServiceClient::call(\"/driver/recover\") failed: " << servobject.response.message.c_str());
+      }
+    }
+    else
+    {
+      ROS_ERROR_STREAM("sick_line_guidance::CanopenChain::recoverCanDriver(): ServiceClient::call(\"/driver/recover\") failed.");
+    }
+  }
+  catch(const std::exception & exc)
+  {
+    success = false;
+    ROS_ERROR_STREAM("sick_line_guidance::CanopenChain::recoverCanDriver() failed: exception = " << exc.what());
+  }
+  if(success)
+    sick_line_guidance::Diagnostic::update(sick_line_guidance::DIAGNOSTIC_STATUS::OK);
+  else
+    sick_line_guidance::Diagnostic::update(sick_line_guidance::DIAGNOSTIC_STATUS::SDO_COMMUNICATION_ERROR, "CanopenChain::recoverCanDriver failed");
+  return success;
+}
+
+/*
+ * 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.
+ */
+bool sick_line_guidance::CanopenChain::shutdownCanDriver(ros::NodeHandle & nh)
+{
+  bool success = false;
+  try
+  {
+    ROS_WARN_STREAM("sick_line_guidance::CanopenChain::shutdownCanDriver(): shutdown can communication by ServiceClient::call(\"/driver/shutdown\")...");
+    ros::ServiceClient servclient = nh.serviceClient<std_srvs::Trigger>("/driver/shutdown");
+    std_srvs::Trigger servobject;
+    if(servclient.call(servobject))
+    {
+      success = servobject.response.success;
+      if(success)
+      {
+        ROS_INFO_STREAM("sick_line_guidance::CanopenChain::shutdownCanDriver(): ServiceClient::call(\"/driver/shutdown\") successfull " << servobject.response.message.c_str());
+      }
+      else
+      {
+        ROS_ERROR_STREAM("sick_line_guidance::CanopenChain::shutdownCanDriver(): ServiceClient::call(\"/driver/shutdown\") failed: " << servobject.response.message.c_str());
+      }
+    }
+    else
+    {
+      ROS_ERROR_STREAM("sick_line_guidance::CanopenChain::shutdownCanDriver(): ServiceClient::call(\"/driver/shutdown\") failed.");
+    }
+  }
+  catch(const std::exception & exc)
+  {
+    success = false;
+    ROS_ERROR_STREAM("sick_line_guidance::CanopenChain::shutdownCanDriver() failed: exception = " << exc.what());
+  }
+  return success;
+}
+
+/*
+ * 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.
+ */
+bool sick_line_guidance::CanopenChain::cmpDCFoverlay(const std::string & dcf_overlay_value, const std::string & sdo_response)
+{
+  // compare strings
+  if(dcf_overlay_value == sdo_response)
+    return true;
+  float f_dcf_val = 0, f_sdo_val = 0;
+  int32_t i32_dcf_val = 0, i32_sdo_val = 0;
+  uint32_t u32_dcf_val = 0, u32_sdo_val = 0;
+  // try UINT8 comparison (1 byte sdo response)
+  if(sdo_response.empty())
+    return tryParseUINT32(dcf_overlay_value, u32_dcf_val) && u32_dcf_val == 0;
+  if(sdo_response.size() == 1)
+    return tryParseUINT32(dcf_overlay_value, u32_dcf_val) && u32_dcf_val == ((static_cast<uint8_t>(sdo_response[0])) & 0xFF);
+  // try UINT32, INT32 and FLOAT comparison (2, 3, or 4 byte sdo response)
+  return (tryParseUINT32(dcf_overlay_value, u32_dcf_val) && tryParseUINT32(sdo_response, u32_sdo_val) && u32_dcf_val == u32_sdo_val) // identical uint32
+    || (tryParseINT32(dcf_overlay_value, i32_dcf_val) && tryParseINT32(sdo_response, i32_sdo_val) && i32_dcf_val == i32_sdo_val)     // identical int32
+    || (tryParseFLOAT(dcf_overlay_value, f_dcf_val) && tryParseFLOAT(sdo_response, f_sdo_val) && std::fabs(f_dcf_val - f_sdo_val) < 0.001); // float diff < 1 mm (OLS/MLS resolution)
+}
+
+/*
+ * 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
+ */
+uint32_t sick_line_guidance::CanopenChain::tryParseUINT32(const std::string & str, uint32_t & value)
+{
+  bool success = false;
+  try
+  {
+    value = std::stoul(str, 0, 0);
+    success = true;
+  }
+  catch(const std::exception & exc)
+  {
+    success = false;
+  }
+  return success;
+}
+
+/*
+ * 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
+ */
+int32_t sick_line_guidance::CanopenChain::tryParseINT32(const std::string & str, int32_t & value)
+{
+  bool success = false;
+  try
+  {
+    value = std::stol(str, 0, 0);
+    success = true;
+  }
+  catch(const std::exception & exc)
+  {
+    success = false;
+  }
+  return success;
+}
+
+/*
+ * 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
+ */
+int32_t sick_line_guidance::CanopenChain::tryParseFLOAT(const std::string & str, float & value)
+{
+  bool success = false;
+  try
+  {
+    value = std::stof(str);
+    success = true;
+  }
+  catch(const std::exception & exc)
+  {
+    success = false;
+  }
+  return success;
+}
+
+/*
+ * 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.
+ */
+std::string sick_line_guidance::CanopenChain::sdoResponseToString(const std::string & response)
+{
+  if(response.empty())
+    return "0";
+  if(response.size() == 1)
+  {
+    uint32_t value = static_cast<uint32_t>((static_cast<uint8_t>(response[0])) & 0xFF);
+    std::stringstream str;
+    str << value;
+    return str.str();
+  }
+  return response;
+}

Devices/Packages/sick_line_guidance/src/sick_line_guidance_cloud_converter.cpp

@@ -0,0 +1,413 @@
+/*
+ * class CloudConverter implements utility functions to convert 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
+ * 
+ */
+#include <algorithm>
+#include <sstream>
+#include <string>
+#include <vector>
+#include <ros/ros.h>
+
+#include "sick_line_guidance/sick_line_guidance_cloud_converter.h"
+#include "sick_line_guidance/sick_line_guidance_msg_util.h"
+
+/*
+ * @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
+ */
+std::vector<bool> sick_line_guidance::CloudConverter::linestatus(uint8_t status, size_t numlines)
+{
+  std::vector<bool> lines_valid;
+  numlines = std::max((size_t)3,numlines);
+  lines_valid.resize(numlines);
+  // Spezifikation OLS+MLS: Es gilt folgende Zuordnung:
+  // 0=000b => keine Spur gefunden
+  // 2=010b => eine Spur gefunden
+  // 3=011b => zwei Spuren gefunden: Weiche links
+  // 6=110b => zwei Spuren gefunden: Weiche rechts
+  // 7=111b => drei Spuren gefunden
+  lines_valid[0] = ((status & 0x01) > 0);
+  lines_valid[1] = ((status & 0x02) > 0);
+  lines_valid[2] = ((status & 0x04) > 0);
+  for(size_t n = 3; n < numlines; n++)
+    lines_valid[n] = false;
+  return lines_valid;
+}
+
+/*
+ * @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
+ */
+int sick_line_guidance::CloudConverter::linenumber(uint8_t status)
+{
+  std::vector<bool> lines_valid = linestatus(status, 3);
+  int linecnt = 0;
+  for(size_t n = 0; n < lines_valid.size(); n++)
+  {
+    if(lines_valid[n])
+      linecnt++;
+  }
+  return linecnt;
+}
+
+/*
+ * @brief returns true, if MLS measurement status is okay, or false otherwise.
+ *
+ * @param[in] measurement MLS measurement data
+ *
+ * @return true (measurement status okay), or false (measurement status not okay)
+ */
+bool sick_line_guidance::CloudConverter::measurementstatus(const sick_line_guidance::MLS_Measurement & measurement)
+{
+  // Spezifikation status bit 0 MLS ("Line good"):
+  // 0 => keine Spur oder Spur zu schwach
+  // 1 => ausreichend starke Spur erkannt
+  return ((measurement.status & 0x1) != 0);
+}
+
+/*
+ * @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
+ */
+sensor_msgs::PointCloud2 sick_line_guidance::CloudConverter::convert(const sick_line_guidance::OLS_Measurement &measurement, const std::string & frame_id)
+{
+  ROS_DEBUG("CloudConverter::convert(OLS_Measurement)");
+  assert(sizeof(float) == sizeof(uint32_t));
+  // #ifdef DEBUG
+  // assert(flipBits(0x04030201) == 0x8040C020);
+  // assert(flipBits(0xFFAA5500) == 0x00AA55FF);
+  // #endif
+  sensor_msgs::PointCloud2 cloud;
+  // set header
+  cloud.header.stamp = measurement.header.stamp; // timestamp of measurement
+  cloud.header.frame_id = frame_id;
+  cloud.header.seq = 0;
+  // clear cloud data
+  cloud.height = 0;
+  cloud.width = 0;
+  cloud.data.clear();
+  // set data header
+  bool sensorOkay = sick_line_guidance::MsgUtil::statusOK(measurement);
+  int numMeasuredLines = linenumber(measurement.status);
+  if(!sensorOkay || numMeasuredLines < 1) // no lines detected
+    return cloud; // return empty PointCloud2
+  int numChannels = 12; // "x", "y", "z", "linewidth", "lineidx", "barcode", "status", "dev_status", "error", "barcode_center", "line_quality", "line_intensity"
+  std::string channelId[] = { "x", "y", "z", "linewidth", "lineidx", "barcode", "status", "dev_status", "error", "barcode_center", "line_quality", "line_intensity" };
+  cloud.height = 1;
+  cloud.width = numMeasuredLines; // normally we have 3 positions (3 lines, one position for each line)
+  cloud.is_bigendian = false;
+  cloud.is_dense = true;
+  cloud.point_step = numChannels * sizeof(float);
+  cloud.row_step = cloud.point_step * cloud.width;
+  cloud.fields.resize(numChannels);
+  for (int i = 0; i < numChannels; i++)
+  {
+    cloud.fields[i].name = channelId[i];
+    cloud.fields[i].offset = i * sizeof(float);
+    cloud.fields[i].count = 1;
+  }
+  for (int i = 0; i < numChannels; i++)
+  {
+    cloud.fields[i].datatype = sensor_msgs::PointField::UINT32; // default: datatype UINT32
+  }
+  cloud.fields[0].datatype = sensor_msgs::PointField::FLOAT32; // "x"
+  cloud.fields[1].datatype = sensor_msgs::PointField::FLOAT32; // "y"
+  cloud.fields[2].datatype = sensor_msgs::PointField::FLOAT32; // "z"
+  cloud.fields[3].datatype = sensor_msgs::PointField::FLOAT32; // "linewidth"
+  cloud.fields[9].datatype = sensor_msgs::PointField::FLOAT32; // "barcode_center"
+  // get barcode: barcode valid, if bit 7 of measurement.status is set
+  uint32_t barcode = 0;
+  if((measurement.status & 0x80) != 0)
+  {
+    barcode = ((measurement.barcode < 255) ? (measurement.barcode) : (measurement.extended_code));
+  }
+  // if((measurement.status & 0x40) != 0) // barcode flipped, if bit 6 of measurement.status is set)
+  // {
+  //   uint32_t flipped_barcode = barcode;
+  //   barcode = flipBits(flipped_barcode);
+  //   #ifdef DEBUG
+  //   assert(flipBits(barcode) == flipped_barcode);
+  //   #endif
+  // }
+  // set data values
+  cloud.data.resize(cloud.row_step * cloud.height);
+  float* pfdata = reinterpret_cast<float*>(&cloud.data[0]);
+  uint32_t* pidata = reinterpret_cast<uint32_t*>(&cloud.data[0]);
+  std::vector<bool> lines_valid = linestatus(measurement.status, measurement.position.size());
+  for(size_t cloudIdx = 0, meaIdx = 0; meaIdx < measurement.position.size(); meaIdx++)
+  {
+    if(lines_valid[meaIdx])
+    {
+      pfdata[cloudIdx++] = 0;                                       // "x" := 0
+      pfdata[cloudIdx++] = measurement.position[meaIdx];            // "y" := measurement.position[i]
+      pfdata[cloudIdx++] = 0;                                       // "z" := 0
+      pfdata[cloudIdx++] = measurement.width[meaIdx];               // "linewidth" := measurement.width[i]
+      pidata[cloudIdx++] = meaIdx + 1;                              // "lineidx": 1:=LCP1, 2:=LCP2, 3:=LCP3
+      pidata[cloudIdx++] = barcode;                                 // "barcode"
+      pidata[cloudIdx++] = measurement.status;                      // "status"
+      pidata[cloudIdx++] = measurement.dev_status;                  // "dev_status"
+      pidata[cloudIdx++] = measurement.error;                       // "error"
+      pfdata[cloudIdx++] = measurement.barcode_center_point;        // "barcode_center" (OLS20 only, OLS10: always 0)
+      pidata[cloudIdx++] = measurement.quality_of_lines;            // "line_quality"   (OLS20 only, OLS10: always 0)
+      pidata[cloudIdx++] = measurement.intensity_of_lines[meaIdx];  // "line_intensity" (OLS20 only, OLS10: always 0)
+    }
+  }
+  ROS_DEBUG("CloudConverter::convert(OLS_Measurement): sensorOkay=%d, numMeasuredLines=%d, numChannels=%d, numBytes=%d",
+    (int)sensorOkay, numMeasuredLines, numChannels, (int)cloud.data.size());
+  return cloud;
+}
+
+/*
+* @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
+*/
+sensor_msgs::PointCloud2 sick_line_guidance::CloudConverter::convert(const sick_line_guidance::MLS_Measurement &measurement, const std::string & frame_id)
+{
+  ROS_DEBUG("CloudConverter::convert(MLS_Measurement)");
+  assert(sizeof(float) == sizeof(uint32_t));
+  sensor_msgs::PointCloud2 cloud;
+  // set header
+  cloud.header.stamp = measurement.header.stamp; // timestamp of measurement
+  cloud.header.frame_id = frame_id;
+  cloud.header.seq = 0;
+  // clear cloud data
+  cloud.height = 0;
+  cloud.width = 0;
+  cloud.data.clear();
+  // set data header
+  bool sensorOkay = measurementstatus(measurement);
+  int numMeasuredLines = linenumber(measurement.lcp);
+  if(!sensorOkay || numMeasuredLines < 1) // no lines detected
+    return cloud; // return empty PointCloud2
+  int numChannels = 8; // "x", "y", "z", "lineidx", "barcode", "lcp", "status", "error"
+  std::string channelId[] = { "x", "y", "z", "lineidx", "marker", "lcp", "status", "error" };
+  cloud.height = 1;
+  cloud.width = numMeasuredLines; // normally we have 3 positions (3 lines, one position for each line)
+  cloud.is_bigendian = false;
+  cloud.is_dense = true;
+  cloud.point_step = numChannels * sizeof(float);  // length of a point in bytes
+  cloud.row_step = cloud.point_step * cloud.width; // length of a row in bytes
+  cloud.fields.resize(numChannels);
+  for (int i = 0; i < numChannels; i++)
+  {
+    cloud.fields[i].name = channelId[i];
+    cloud.fields[i].offset = i * sizeof(float);
+    cloud.fields[i].count = 1;
+  }
+  for (int i = 0; i < 3; i++)
+  {
+    cloud.fields[i].datatype = sensor_msgs::PointField::FLOAT32; // "x", "y", "z"
+  }
+  cloud.fields[3].datatype = sensor_msgs::PointField::UINT32;// "lineidx"
+  cloud.fields[4].datatype = sensor_msgs::PointField::INT32;// "marker"
+  for (int i = 5; i < numChannels; i++)
+  {
+    cloud.fields[i].datatype = sensor_msgs::PointField::UINT32; // "lcp", "status", "error"
+  }
+  // get marker: marker valid, if bit 6 of measurement.status is set
+  int32_t marker = 0;
+  if((measurement.status & 0x40) != 0)
+  {
+    marker = ((measurement.lcp >> 4) & 0x0F); // Bit 4 - bit 7 of lcp: marker bit
+    if((measurement.lcp & 0x08) != 0)  // // Bit 3 of lcp: sign bit of marker
+      marker = -marker;
+  }
+  // set data values
+  cloud.data.resize(cloud.row_step * cloud.height);
+  float* pfdata = reinterpret_cast<float*>(&cloud.data[0]);
+  uint32_t* pidata = reinterpret_cast<uint32_t*>(&cloud.data[0]);
+  std::vector<bool> lines_valid = linestatus(measurement.lcp, measurement.position.size());
+  for(size_t cloudIdx = 0, meaIdx = 0; meaIdx < measurement.position.size(); meaIdx++)
+  {
+    if(lines_valid[meaIdx])
+    {
+      pfdata[cloudIdx++] = 0;                             // "x" := 0
+      pfdata[cloudIdx++] =  measurement.position[meaIdx]; // "y" := measurement.position[i]
+      pfdata[cloudIdx++] = 0;                             // "z" := 0
+      pidata[cloudIdx++] = meaIdx + 1;                    // "lineidx": 1:=LCP1, 2:=LCP2, 3:=LCP3
+      pidata[cloudIdx++] = marker;                        // "marker"
+      pidata[cloudIdx++] = measurement.lcp;               // "lcp"
+      pidata[cloudIdx++] = measurement.status;            // "status"
+      pidata[cloudIdx++] = measurement.error;             // "error"
+    }
+  }
+  ROS_DEBUG("CloudConverter::convert(MLS_Measurement): sensorOkay=%d, numMeasuredLines=%d, numPositions=%d, numChannels=%d, numBytes=%d",
+    (int)sensorOkay, numMeasuredLines, (int)measurement.position.size(), numChannels, (int)cloud.data.size());
+  return cloud;
+}
+
+template<class T> static std::string convertCloudDataElement(const uint8_t* data, const uint8_t* end, bool printhex)
+{
+  std::stringstream out;
+  const T* pElement = reinterpret_cast<const T*>(data);
+  if(data + sizeof(*pElement) <= end)
+  {
+    if(printhex)
+    {
+      out << "0x" << std::uppercase << std::hex << std::setfill('0') << std::setw(2*sizeof(*pElement)) << (uint32_t)(*pElement);
+    }
+    else
+    {
+      out << (double)(*pElement);
+    }
+    data += sizeof(*pElement);
+  }
+  return out.str();
+}
+
+/*
+ * @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
+ */
+std::string sick_line_guidance::CloudConverter::cloudDataFieldToString(uint8_t datatype, const uint8_t* data, const uint8_t* end)
+{
+  switch(datatype)
+  {
+    case sensor_msgs::PointField::INT8:
+      return convertCloudDataElement<int8_t>(data, end, true);
+    case sensor_msgs::PointField::UINT8:
+      return convertCloudDataElement<uint8_t>(data, end, true);
+    case sensor_msgs::PointField::INT16:
+      return convertCloudDataElement<int16_t>(data, end, true);
+    case sensor_msgs::PointField::UINT16:
+      return convertCloudDataElement<uint16_t>(data, end, true);
+    case sensor_msgs::PointField::INT32:
+      return convertCloudDataElement<int32_t>(data, end, true);
+    case sensor_msgs::PointField::UINT32:
+      return convertCloudDataElement<uint32_t>(data, end, true);
+    case sensor_msgs::PointField::FLOAT32:
+      return convertCloudDataElement<float>(data, end, false);
+    case sensor_msgs::PointField::FLOAT64:
+      return convertCloudDataElement<double>(data, end, false);
+    default:
+      break;
+  }
+  return "";
+}
+
+/*
+* @brief prints a PointCloud2 data field to string.
+*
+* @param[in] cloud PointCloud2 data
+*
+* @return PointCloud2 data converted string
+*/
+std::string sick_line_guidance::CloudConverter::cloudDataToString(const sensor_msgs::PointCloud2 & cloud)
+{
+  std::stringstream out;
+  size_t numChannels = cloud.fields.size();
+  size_t numDataBytes = cloud.data.size();
+  const uint8_t* pCloudData = reinterpret_cast<const uint8_t*>(&cloud.data[0]);
+  const uint8_t* pCloudLast = &pCloudData[numDataBytes];
+  ROS_DEBUG("CloudConverter::cloudDataToString(): cloud.height=%d, cloud.width=%d, numChannels=%d, numDataBytes=%d",(int)cloud.height, (int)cloud.width, (int)numChannels, (int)numDataBytes);
+  if(cloud.height > 0 && cloud.width > 0 && numChannels > 0 && numDataBytes > 0 && pCloudData != 0)
+  {
+    for(size_t y = 0; y < cloud.height; y++)
+    {
+      for (size_t x = 0; x < cloud.width; x++)
+      {
+        const uint8_t* pFieldData = pCloudData + y * cloud.row_step + x * cloud.point_step;
+        for (size_t n = 0; n < numChannels && pFieldData < pCloudLast; n++)
+        {
+          out << cloud.fields[n].name << ":";
+          const uint8_t* pChannelData = pFieldData + cloud.fields[n].offset;
+          for (size_t m = 0; m < cloud.fields[n].count && pChannelData < pCloudLast; m++)
+          {
+            std::string element = cloudDataFieldToString(cloud.fields[n].datatype, pChannelData, pCloudLast);
+            out << element << ",";
+          }
+        }
+      }
+    }
+  }
+  return out.str();
+}
+
+/*
+ * @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
+ */
+uint32_t sick_line_guidance::CloudConverter::flipBits(uint32_t code)
+{
+  uint32_t flipped = 0;
+  for(uint32_t shift = 0; shift < 32; shift++)
+  {
+    flipped = (flipped << 1);
+    flipped = flipped | (code & 0x1);
+    code = code >> 1;
+  }
+  return flipped;
+}

Devices/Packages/sick_line_guidance/src/sick_line_guidance_cloud_publisher.cpp

@@ -0,0 +1,162 @@
+/*
+ * sick_line_guidance_cloud_publisher implements a ros-node to convert sensor measurement data to PointCloud2 data.
+ * Subscribes topics "/ols" and "/mls", converts the sensor data and publishes sensor positions on topic "/cloud".
+ * 
+ * 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 <sensor_msgs/PointCloud2.h>
+#include <vector>
+
+#include "sick_line_guidance/sick_line_guidance_version.h"
+#include "sick_line_guidance/sick_line_guidance_cloud_converter.h"
+#include "sick_line_guidance/sick_line_guidance_msg_util.h"
+
+ros::Publisher cloud_publisher;
+std::string mls_cloud_frame_id ="mls_frame";
+std::string ols_cloud_frame_id ="ols_frame";
+
+/*
+ * Callback for OLS measurement messages (topic ("/ols").
+ * Converts the sensor data of type OLS_Measurement and publishes a PointCloud2 message on topic "/cloud".
+ *
+ * @param[in] msg OLS measurement messages (topic ("/ols")
+ */
+void olsMessageCb(const boost::shared_ptr<sick_line_guidance::OLS_Measurement const>& msg)
+{
+  if(msg)
+  {
+    sensor_msgs::PointCloud2 cloud = sick_line_guidance::CloudConverter::convert(*msg, ols_cloud_frame_id);
+    if(!cloud.data.empty())
+    {
+      cloud_publisher.publish(cloud);
+      std::string cloudmsg = sick_line_guidance::CloudConverter::cloudDataToString(cloud);
+      ROS_INFO("sick_line_guidance_cloud_publisher: OLS PointCloud data: {%s}", cloudmsg.c_str());
+    }
+    else
+    {
+      ROS_INFO("sick_line_guidance_cloud_publisher: invalid OLS measurement data, no line detected.");
+    }
+  }
+  else
+  {
+    ROS_ERROR("## ERROR sick_line_guidance_cloud_publisher::olsMessageCb(): invalid message (%s:%d)", __FILE__, __LINE__);
+  }
+}
+
+/*
+ * Callback for MLS measurement messages (topic ("/mls").
+ * Converts the sensor data of type MLS_Measurement and publishes a PointCloud2 message on topic "/cloud".
+ *
+ * @param[in] msg MLS measurement messages (topic ("/mls")
+ */
+void mlsMessageCb(const boost::shared_ptr<sick_line_guidance::MLS_Measurement const>& msg)
+{
+  if(msg)
+  {
+    sensor_msgs::PointCloud2 cloud = sick_line_guidance::CloudConverter::convert(*msg, mls_cloud_frame_id);
+    if(!cloud.data.empty())
+    {
+      cloud_publisher.publish(cloud);
+      std::string cloudmsg = sick_line_guidance::CloudConverter::cloudDataToString(cloud);
+      ROS_INFO("sick_line_guidance_cloud_publisher: MLS PointCloud data: {%s}", cloudmsg.c_str());
+    }
+    else
+    {
+      ROS_INFO("sick_line_guidance_cloud_publisher: invalid MLS measurement data, no line detected.");
+    }
+  }
+  else
+  {
+    ROS_ERROR("## ERROR sick_line_guidance_cloud_publisher::mlsMessageCb(): invalid message (%s:%d)", __FILE__, __LINE__);
+  }
+}
+
+/*
+ * sick_line_guidance_cloud_publisher implements a ros-node to convert sensor measurement data to PointCloud2 data.
+ * Subscribes topics "/ols" and "/mls", converts the sensor data and publishes sensor positions on topic "/cloud".
+ */
+int main(int argc, char** argv)
+{
+  ros::init(argc, argv, "sick_line_guidance_cloud_publisher");
+  ros::NodeHandle nh;
+  ros::NodeHandle nh_priv("~");
+  
+  int subscribe_queue_size = 1;
+  std::string ols_topic_publish = "ols", mls_topic_publish = "mls", cloud_topic_publish = "cloud";
+  nh_priv.param("mls_topic_publish", mls_topic_publish, mls_topic_publish);
+  nh_priv.param("ols_topic_publish", ols_topic_publish, ols_topic_publish);
+  nh_priv.param("cloud_topic_publish", cloud_topic_publish, cloud_topic_publish);
+  nh_priv.param("mls_cloud_frame_id", mls_cloud_frame_id, mls_cloud_frame_id);
+  nh_priv.param("ols_cloud_frame_id", ols_cloud_frame_id, ols_cloud_frame_id);
+  nh_priv.param("subscribe_queue_size", subscribe_queue_size, subscribe_queue_size); // buffer size for ros messages
+
+
+  ROS_INFO_STREAM("sick_line_guidance_cloud_publisher: version " << sick_line_guidance::Version::getVersionInfo());
+  ROS_INFO_STREAM("sick_line_guidance_cloud_publisher started.");
+  ROS_INFO_STREAM("sick_line_guidance_cloud_publisher: mls_topic=" << mls_topic_publish);
+  ROS_INFO_STREAM("sick_line_guidance_cloud_publisher: ols_topic=" << ols_topic_publish);
+  ROS_INFO_STREAM("sick_line_guidance_cloud_publisher: cloud_topic=" << cloud_topic_publish);
+  ROS_INFO_STREAM("sick_line_guidance_cloud_publisher: mls_frame_id=" << mls_cloud_frame_id);
+  ROS_INFO_STREAM("sick_line_guidance_cloud_publisher: ols_frame_id=" << ols_cloud_frame_id);
+  ROS_INFO_STREAM("sick_line_guidance_cloud_publisher: queue_size=" << subscribe_queue_size);
+  cloud_publisher = nh.advertise<sensor_msgs::PointCloud2>(cloud_topic_publish, 1);
+  ros::Subscriber ols_subscriber = nh.subscribe(ols_topic_publish, subscribe_queue_size, olsMessageCb);
+  ros::Subscriber mls_subscriber = nh.subscribe(mls_topic_publish, subscribe_queue_size, mlsMessageCb);
+  
+  ros::spin();
+  
+  std::cout << "sick_line_guidance_cloud_publisher finished." << std::endl;
+  ROS_INFO_STREAM("sick_line_guidance_cloud_publisher finished.");
+  return 0;
+}
+

Devices/Packages/sick_line_guidance/src/sick_line_guidance_diagnostic.cpp

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

Devices/Packages/sick_line_guidance/src/sick_line_guidance_eds_util.cpp

@@ -0,0 +1,170 @@
+/*
+ * class EdsUtil 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
+ * 
+ */
+#include <iomanip>
+#include <boost/filesystem.hpp>
+#include <ros/ros.h>
+#include <ros/package.h>
+#include "sick_line_guidance/sick_line_guidance_eds_util.h"
+
+/*
+ * @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).
+ */
+std::string sick_line_guidance::EdsUtil::filepathFromPackage(const std::string & package, const std::string & filename)
+{
+  std::string filepath = filename;
+  if(!package.empty())
+  {
+    std::string pkg_path = ros::package::getPath(package);
+    if(!pkg_path.empty())
+    {
+      filepath = (boost::filesystem::path(pkg_path)/filepath).make_preferred().native();
+    }
+  }
+  return filepath;
+}
+
+/*
+ * @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
+ */
+std::string sick_line_guidance::EdsUtil::readParsePrintEdsfile(const std::string & eds_filepath)
+{
+  std::stringstream object_dictionary_msg;
+  try
+  {
+    // ROS_DEBUG("sick_line_guidance: object_dictionary from eds_file \"%s\":", eds_filepath.c_str());
+    canopen::ObjectDict::Overlay object_overlay;
+    canopen::ObjectDictSharedPtr object_dictionary = canopen::ObjectDict::fromFile(eds_filepath, object_overlay);
+    canopen::ObjectDict::ObjectDictMap::const_iterator object_dictionary_iter;
+    while (object_dictionary->iterate(object_dictionary_iter))
+    {
+      const canopen::ObjectDict::Key &key = object_dictionary_iter->first;
+      const canopen::ObjectDict::Entry &entry = *object_dictionary_iter->second;
+      object_dictionary_msg << key << "=(" << printObjectDictEntry(entry) << ")\n";
+    }
+    // ROS_DEBUG("%s", object_dictionary_msg.str().c_str());
+  }
+  catch (const canopen::ParseException & err)
+  {
+    std::stringstream err_msg;
+    err_msg << "sick_line_guidance::readParsePrintEdsfile(" << eds_filepath << ") failed: " << err.what();
+    ROS_ERROR("%s", err_msg.str().c_str());
+  }
+  return object_dictionary_msg.str();
+}
+
+/*
+ * @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.
+ */
+std::string sick_line_guidance::EdsUtil::printObjectDictEntry(const canopen::ObjectDict::Entry & entry)
+{
+  std::stringstream msg;
+  msg << "desc:" << entry.desc << ",";
+  msg << "data_type:" << (int)(entry.data_type) << ",";
+  msg << "index:" << canopen::ObjectDict::Key(entry.index, entry.sub_index) << ",";
+  msg << "constant:" << entry.constant << ",";
+  msg << "readable:" << entry.readable << ",";
+  msg << "writable:" << entry.writable << ",";
+  msg << "mappable:" << entry.mappable << ",";
+  msg << "obj_code:" << (int)(entry.obj_code) << ",";
+  msg << printHoldAny("value", entry.value()) << ",";
+  msg << printHoldAny("def_val", entry.def_val) << ",";
+  msg << printHoldAny("init_val", entry.init_val);
+  return msg.str();
+}
+
+/*
+ * @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.
+ */
+std::string sick_line_guidance::EdsUtil::printHoldAny(const std::string & parameter_name, const canopen::HoldAny & holdany)
+{
+  std::stringstream msg;
+  if(!holdany.is_empty())
+  {
+    size_t value_size = holdany.type().get_size();
+    msg << parameter_name << "_size:" << value_size << "," << parameter_name << "_data:0x";
+    const canopen::String & value_data = holdany.data();
+    canopen::String::const_iterator iter=value_data.cbegin();
+    for(size_t n = 0; iter != value_data.cend() && n < value_size; iter++, n++)
+    {
+      msg << std::uppercase << std::hex << std::setfill('0') << std::setw(2) << (unsigned)((*iter)&0xFF);
+    }
+    if(iter != value_data.cend())
+    {
+      msg << "...";
+    }
+  }
+  else
+  {
+    msg << parameter_name << ":none";
+  }
+  return msg.str();
+}

Devices/Packages/sick_line_guidance/src/sick_line_guidance_msg_util.cpp

@@ -0,0 +1,238 @@
+/*
+ * class EdsUtil 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
+ * 
+ */
+#include <ros/ros.h>
+#include <random_numbers/random_numbers.h>
+#include "sick_line_guidance/sick_line_guidance_msg_util.h"
+
+/*
+ * @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
+*/
+std::string sick_line_guidance::MsgUtil::toInfo(const sick_line_guidance::MLS_Measurement & measurement_msg)
+{
+  std::stringstream info;
+  info << "position:[" << std::fixed << std::setprecision(3) << measurement_msg.position[0]
+       << "," << std::fixed << std::setprecision(3) << measurement_msg.position[1]
+       << "," << std::fixed << std::setprecision(3) << measurement_msg.position[2]
+       << "],status=" << sick_line_guidance::MsgUtil::toHexString(measurement_msg.status)
+       << ",lcpflags=" << sick_line_guidance::MsgUtil::toHexString(measurement_msg.lcp)
+       << ",error=" << sick_line_guidance::MsgUtil::toHexString(measurement_msg.error);
+  return info.str();
+}
+
+/*
+ * @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
+*/
+std::string sick_line_guidance::MsgUtil::toInfo(const sick_line_guidance::OLS_Measurement & measurement_msg)
+{
+  uint32_t barcode = measurement_msg.barcode;
+  if(measurement_msg.barcode >= 255)
+    barcode = measurement_msg.extended_code;
+  std::stringstream info;
+  info << "position:[" << std::fixed << std::setprecision(3) << measurement_msg.position[0]
+       << "," << std::fixed << std::setprecision(3) << measurement_msg.position[1]
+       << "," << std::fixed << std::setprecision(3) << measurement_msg.position[2]
+       << "],width:[" << std::fixed << std::setprecision(3) << measurement_msg.width[0]
+       << "," << std::fixed << std::setprecision(3) << measurement_msg.width[1]
+       << "," << std::fixed << std::setprecision(3) << measurement_msg.width[2]
+       << "],status=" << sick_line_guidance::MsgUtil::toHexString(measurement_msg.status)
+       << ",devstatus=" << sick_line_guidance::MsgUtil::toHexString(measurement_msg.dev_status)
+       << ",error=" << sick_line_guidance::MsgUtil::toHexString(measurement_msg.error)
+       << ",barcode=0x" << std::uppercase << std::hex << std::setfill('0') << std::setw(2) << barcode
+       << ",barcodecenter=" << std::fixed << std::setprecision(3) << measurement_msg.barcode_center_point
+       << ",linequality=" << sick_line_guidance::MsgUtil::toHexString(measurement_msg.quality_of_lines)
+       << ",lineintensity=[" << sick_line_guidance::MsgUtil::toHexString(measurement_msg.intensity_of_lines[0])
+       << "," << sick_line_guidance::MsgUtil::toHexString(measurement_msg.intensity_of_lines[1])
+       << "," << sick_line_guidance::MsgUtil::toHexString(measurement_msg.intensity_of_lines[2]) << "]";
+  return info.str();
+}
+
+/*
+ * @brief initializes a MLS measurement with zero data
+ * @param[in+out] measurement_msg MLS measurement
+ */
+void sick_line_guidance::MsgUtil::zero(sick_line_guidance::MLS_Measurement & measurement_msg)
+{
+  measurement_msg.header.stamp = ros::Time(0);
+  measurement_msg.header.frame_id = "";
+  measurement_msg.position = { 0, 0, 0 };
+  measurement_msg.status = 0;
+  measurement_msg.lcp = 0;
+  measurement_msg.error = 0;
+}
+
+/*
+ * @brief initializes an OLS measurement with zero data
+ * @param[in+out] measurement_msg OLS measurement
+ */
+void sick_line_guidance::MsgUtil::zero(sick_line_guidance::OLS_Measurement & measurement_msg)
+{
+  measurement_msg.header.stamp = ros::Time(0);
+  measurement_msg.header.frame_id = "";
+  measurement_msg.position = { 0, 0, 0 };
+  measurement_msg.width = { 0, 0, 0 };
+  measurement_msg.status = 0;
+  measurement_msg.barcode = 0;
+  measurement_msg.extended_code = 0;
+  measurement_msg.dev_status = 0;
+  measurement_msg.error = 0;
+  measurement_msg.barcode_center_point = 0;
+  measurement_msg.quality_of_lines = 0;
+  measurement_msg.intensity_of_lines = { 0, 0, 0 };
+}
+
+/*
+ * @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
+ */
+sick_line_guidance::MLS_Measurement sick_line_guidance::MsgUtil::convertMLSMessage(float lcp1, float lcp2, float lcp3, uint8_t status, uint8_t lcp, uint8_t error, const std::string & msg_frame_id)
+{
+  sick_line_guidance::MLS_Measurement mls_message;
+  mls_message.header.stamp = ros::Time::now();
+  mls_message.header.frame_id = msg_frame_id;
+  mls_message.position = { lcp1, lcp2, lcp3 };
+  mls_message.status = status;
+  mls_message.lcp = lcp;
+  mls_message.error = error;
+  return mls_message;
+}
+
+/*
+ * @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
+ */
+sick_line_guidance::OLS_Measurement sick_line_guidance::MsgUtil::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)
+{
+  sick_line_guidance::OLS_Measurement ols_message;
+  ols_message.header.stamp = ros::Time::now();
+  ols_message.header.frame_id = msg_frame_id;
+  ols_message.position = { lcp1, lcp2, lcp3 };
+  ols_message.width = { width1, width2, width3 };
+  ols_message.status = status;
+  if(barcode > 255)
+  {
+    ols_message.barcode = 255;
+    ols_message.extended_code = barcode;
+  }
+  else
+  {
+    ols_message.barcode = barcode;
+    ols_message.extended_code = 0;
+  }
+  ols_message.dev_status = dev_status;
+  ols_message.error = error;
+  ols_message.barcode_center_point = barcodecenter;
+  ols_message.quality_of_lines = linequality;
+  ols_message.intensity_of_lines = { lineintensity1, lineintensity2, lineintensity3 };
+  return ols_message;
+}
+
+
+/*
+ * @brief returns a gaussian destributed random line center position (lcp)
+ *
+ * @param stddev standard deviation of gaussian random generator
+ *
+ * @return random line center position
+ */
+float sick_line_guidance::MsgUtil::randomLCP(double stddev)
+{
+  static random_numbers::RandomNumberGenerator mea_random_generator;
+  return (float)mea_random_generator.gaussian(0.0, stddev);
+}
+
+/*
+ * @brief returns a gaussian destributed random line width
+ *
+ * @param stddev standard deviation of gaussian random generator
+ *
+ * @return random line width
+ */
+float sick_line_guidance::MsgUtil::randomLW(double min_linewidth, double max_linewidth)
+{
+  static random_numbers::RandomNumberGenerator mea_random_generator;
+  return (float)mea_random_generator.uniformReal(min_linewidth, max_linewidth);
+}

Devices/Packages/sick_line_guidance/src/sick_line_guidance_node.cpp

@@ -0,0 +1,224 @@
+/*
+ * @brief sick_line_guidance_node implements ros driver for OLS20 and MLS devices.
+ *
+ * It implements the can master using CanopenChain in package ros_canopen,
+ * parses and converts the can messages from OLS20 and MLS devices (SDOs and PDOs)
+ * and publishes sensor messages (type OLS_Measurement or MLS_Measurement)
+ * 
+ * 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 <std_msgs/Int16.h>
+#include <sensor_msgs/PointCloud.h>
+#include <canopen_chain_node/ros_chain.h>
+#include <vector>
+#include "sick_line_guidance/sick_line_guidance_version.h"
+#include "sick_line_guidance/sick_line_guidance_canopen_chain.h"
+#include "sick_line_guidance/sick_line_guidance_can_mls_subscriber.h"
+#include "sick_line_guidance/sick_line_guidance_can_ols_subscriber.h"
+#include "sick_line_guidance/sick_line_guidance_can_cia401_subscriber.h"
+#include "sick_line_guidance/sick_line_guidance_diagnostic.h"
+#include "sick_line_guidance/sick_line_guidance_eds_util.h"
+#include "sick_line_guidance/sick_line_guidance_msg_util.h"
+
+typedef enum SICK_LINE_GUIDANCE_EXIT_CODES_ENUM
+{
+  EXIT_OK = 0,
+  EXIT_ERROR = 1,
+  EXIT_CONFIG_ERROR
+} SICK_LINE_GUIDANCE_EXIT_CODES;
+
+// shortcut to read a member from a XmlRpcValue, or to return a defaultvalue, it the member does not exist
+template<class T> static T readMember(XmlRpc::XmlRpcValue & value, const std::string & member, const T & defaultvalue)
+{
+  if(value.hasMember(member))
+    return value[member];
+  return defaultvalue;
+}
+
+/*
+ * @brief sick_line_guidance_node implements ros driver for OLS20 and MLS devices.
+ * It implements the can master using CanopenChain in package ros_canopen,
+ * parses and converts the can messages from OLS20 and MLS devices (SDOs and PDOs)
+ * and publishes sensor messages (type OLS_Measurement or MLS_Measurement)
+ */
+int main(int argc, char** argv)
+{
+  ros::init(argc, argv, "sick_line_guidance_node");
+  
+  // Configuration
+  ros::NodeHandle nh;
+  bool can_connect_init_at_startup = true; // Connect and initialize canopen service
+  int initial_sensor_state = 0x07; // initial sensor state (f.e. 0x07 for 3 detected lines, or 8 to indicate sensor error)
+  int subscribe_queue_size = 16;   // buffer size for ros messages
+  int max_num_retries_after_sdo_error = 2; // 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.
+  double max_sdo_rate = 1000; // max_sdo_rate max. sdo query and publish rate
+  std::string diagnostic_topic = "diagnostics";
+  nh.param("/sick_line_guidance_node/can_connect_init_at_startup", can_connect_init_at_startup, can_connect_init_at_startup); // Connect and initialize canopen service at startup
+  nh.param("/sick_line_guidance_node/initial_sensor_state", initial_sensor_state, initial_sensor_state); // initial sensor state
+  nh.param("/sick_line_guidance_node/subscribe_queue_size", subscribe_queue_size, subscribe_queue_size);
+  nh.param("/sick_line_guidance_node/diagnostic_topic", diagnostic_topic, diagnostic_topic);
+  nh.param("/sick_line_guidance_node/max_num_retries_after_sdo_error", max_num_retries_after_sdo_error, max_num_retries_after_sdo_error);
+  nh.param("/sick_line_guidance_node/max_sdo_rate", max_sdo_rate, max_sdo_rate);
+  ROS_INFO_STREAM("sick_line_guidance_node: version " << sick_line_guidance::Version::getVersionInfo());
+  sick_line_guidance::Diagnostic::init(nh, diagnostic_topic, "sick_line_guidance_node");
+  if(can_connect_init_at_startup)
+  {
+    ROS_INFO_STREAM("sick_line_guidance_node: connect and init canopen ros-service...");
+    do
+    {
+      ros::Duration(1.0).sleep();
+    } while (!sick_line_guidance::CanopenChain::connectInitService(nh));
+    ROS_INFO_STREAM("sick_line_guidance_node: canopen ros-service connected and initialized.");
+  }
+  else
+  {
+    ROS_INFO_STREAM("sick_line_guidance_node: starting...");
+  }
+  
+  // Read configuration of can nodes from yaml-file (there can be more than just "node1")
+  std::vector<sick_line_guidance::CanSubscriber*> vecCanSubscriber;
+  XmlRpc::XmlRpcValue nodes;
+  if(!nh.getParam("nodes", nodes) || nodes.size() < 1)
+  {
+    ROS_ERROR("sick_line_guidance_node: no can nodes found in yaml-file, please check configuration. Aborting...");
+    sick_line_guidance::Diagnostic::update(sick_line_guidance::DIAGNOSTIC_STATUS::CONFIGURATION_ERROR, "no can nodes found in yaml-file");
+    return EXIT_CONFIG_ERROR;
+  }
+  for(XmlRpc::XmlRpcValue::iterator can_node_iter = nodes.begin(); can_node_iter != nodes.end(); ++can_node_iter)
+  {
+    std::string can_node_id = readMember<std::string>(can_node_iter->second, "name", can_node_iter->first);
+    ROS_INFO("sick_line_guidance_node: initializing can_node_id \"%s\"...", can_node_id.c_str());
+    
+    // Test: Query some common objects from can device
+    std::string can_msg = "", can_object_value = "";
+    std::vector <std::string> vec_object_idx = {"1001", "1018sub1", "1018sub4"};
+    for (std::vector<std::string>::iterator object_iter = vec_object_idx.begin(); object_iter != vec_object_idx.end(); object_iter++)
+    {
+      if (!sick_line_guidance::CanopenChain::queryCanObject(nh, can_node_id, *object_iter, max_num_retries_after_sdo_error, can_msg, can_object_value))
+        ROS_ERROR("sick_line_guidance_node: CanopenChain::queryCanObject(%s, %s) failed: %s", can_node_id.c_str(), object_iter->c_str(), can_msg.c_str());
+      else
+        ROS_INFO("sick_line_guidance_node: can %s[%s]=%s", can_node_id.c_str(), object_iter->c_str(), can_object_value.c_str());
+    }
+    
+    // Test: read eds-file
+    std::string eds_file = sick_line_guidance::EdsUtil::filepathFromPackage(
+      readMember<std::string>(can_node_iter->second, "eds_pkg", ""),
+      readMember<std::string>(can_node_iter->second, "eds_file", ""));
+    ROS_INFO("sick_line_guidance_node: eds_file \"%s\"", eds_file.c_str());
+    std::string eds_file_content = sick_line_guidance::EdsUtil::readParsePrintEdsfile(eds_file);
+    if(eds_file_content == "")
+    {
+      ROS_ERROR("sick_line_guidance_node: read/parse eds file \"%s\" failed", eds_file.c_str());
+      sick_line_guidance::Diagnostic::update(sick_line_guidance::DIAGNOSTIC_STATUS::CONFIGURATION_ERROR, "read/parse error in eds file");
+    }
+    else
+      ROS_DEBUG("%s", eds_file_content.c_str());
+  
+    // Parse dcf_overlays, read and overwrite parameter specified in dcf_overlays
+    if (can_node_iter->second.hasMember("dcf_overlay") &&
+        !sick_line_guidance::CanopenChain::writeDCFoverlays(nh, can_node_id, can_node_iter->second["dcf_overlay"]))
+    {
+      ROS_ERROR("sick_line_guidance: failed to set one or more dcf overlays.");
+    }
+  
+    // Subscribe to canopen_chain_node topics, handle PDO messages and publish OLS/MLS measurement messages
+    std::string sick_device_family = readMember<std::string>(can_node_iter->second, "sick_device_family", "");
+    std::string sick_topic = readMember<std::string>(can_node_iter->second, "sick_topic", "");
+    std::string sick_frame_id = readMember<std::string>(can_node_iter->second, "sick_frame_id", "");
+    boost::to_upper(sick_device_family);
+    if(sick_device_family.empty() || sick_topic.empty() || sick_frame_id.empty())
+    {
+      sick_line_guidance::Diagnostic::update(sick_line_guidance::DIAGNOSTIC_STATUS::CONFIGURATION_ERROR, "invalid node configuration in yaml file");
+      ROS_ERROR("sick_line_guidance_node: invalid node configuration: sick_device_family=%s, sick_topic=%s, sick_frame_id=%s", sick_device_family.c_str(), sick_topic.c_str(), sick_frame_id.c_str());
+      continue;
+    }
+    sick_line_guidance::Diagnostic::update(sick_line_guidance::DIAGNOSTIC_STATUS::OK, "");
+    sick_line_guidance::CanSubscriber * p_can_subscriber = NULL;
+    if (sick_device_family == "MLS")
+      p_can_subscriber = new sick_line_guidance::CanMlsSubscriber(nh, can_node_id, max_num_retries_after_sdo_error, max_sdo_rate, sick_topic, sick_frame_id, initial_sensor_state, subscribe_queue_size);
+    else if (sick_device_family == "OLS10")
+      p_can_subscriber = new sick_line_guidance::CanOls10Subscriber(nh, can_node_id, max_num_retries_after_sdo_error, max_sdo_rate, sick_topic, sick_frame_id, initial_sensor_state, subscribe_queue_size);
+    else if (sick_device_family == "OLS20")
+      p_can_subscriber = new sick_line_guidance::CanOls20Subscriber(nh, can_node_id, max_num_retries_after_sdo_error, max_sdo_rate, sick_topic, sick_frame_id, initial_sensor_state, subscribe_queue_size);
+    else if (sick_device_family == "CIA401")
+      p_can_subscriber = new sick_line_guidance::CanCiA401Subscriber(nh, can_node_id, max_num_retries_after_sdo_error, max_sdo_rate, sick_topic, sick_frame_id, initial_sensor_state, subscribe_queue_size);
+    if(p_can_subscriber && p_can_subscriber->subscribeCanTopics())
+    {
+      vecCanSubscriber.push_back(p_can_subscriber);
+    }
+    else
+    {
+      ROS_ERROR("sick_line_guidance_node: sick_device_family \"%s\" not supported.", sick_device_family.c_str());
+    }
+    
+  }
+  if(vecCanSubscriber.size() < 1)
+  {
+    sick_line_guidance::Diagnostic::update(sick_line_guidance::DIAGNOSTIC_STATUS::CONFIGURATION_ERROR, " no can nodes for sick_line_guidance found in yaml-file");
+    ROS_ERROR("sick_line_guidance_node: no can nodes for sick_line_guidance found in yaml-file, please check configuration. Aborting...");
+    return EXIT_CONFIG_ERROR;
+  }
+  
+  // Run ros event loop
+  ros::spin();
+  
+  // Deallocate resources
+  std::cout << "sick_line_guidance_node: exiting..." << std::endl;
+  ROS_INFO("sick_line_guidance_node: exiting...");
+  sick_line_guidance::Diagnostic::update(sick_line_guidance::DIAGNOSTIC_STATUS::EXIT);
+  for(std::vector<sick_line_guidance::CanSubscriber*>::iterator iter = vecCanSubscriber.begin(); iter != vecCanSubscriber.end(); iter++)
+    delete(*iter);
+  
+  return EXIT_OK;
+}
+

Devices/Packages/sick_line_guidance/src/sick_line_guidance_ros2can_node.cpp

@@ -0,0 +1,206 @@
+/*
+ * sick_line_guidance_ros2can_node: subscribes to ros topic (message type can_msgs::Frame),
+ * converts all messages to can frames (type can::Frame) and writes them to can bus.
+ * A simple cansend with input by 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
+ * 
+ */
+#include <ros/ros.h>
+#include <can_msgs/Frame.h>
+#include <socketcan_interface/socketcan.h>
+#include <socketcan_interface/filter.h>
+#include <canopen_chain_node/ros_chain.h>
+#include "sick_line_guidance/sick_line_guidance_version.h"
+
+namespace sick_line_guidance
+{
+  /*
+   * class SocketCANSender implements a callback for ros messages, converts them to can frames and writes the data to can bus.
+   */
+  class SocketCANSender
+  {
+  public:
+  
+    /*
+     * Constructor
+     */
+    SocketCANSender(can::DriverInterfaceSharedPtr p_socketcan_interface) : m_socketcan_interface(p_socketcan_interface), m_socketcan_thread(0), m_socketcan_running(false)
+    {
+    }
+  
+    /*
+     * @brief create a thread and run can::SocketCANInterface::run() in a background task
+     */
+    void start()
+    {
+      m_socketcan_running = true;
+      m_socketcan_thread = new boost::thread(&sick_line_guidance::SocketCANSender::run, this);
+    }
+  
+    /*
+     * @brief shuts down can::SocketCANInterface and stops the thread running can::SocketCANInterface::run()
+     */
+    void stop()
+    {
+      m_socketcan_running = true;
+      if(m_socketcan_thread)
+      {
+        m_socketcan_interface->shutdown();
+        m_socketcan_thread->join();
+        delete(m_socketcan_thread);
+      }
+      m_socketcan_thread = 0;
+    }
+    
+    /*
+     * @brief Callbacks for ros messages. Converts a ros message to datatype can::Frame and writes it to can bus.
+     * param[in] msg ros message of type can_msgs::Frame
+     */
+    void messageCallback(const can_msgs::Frame & msg)
+    {
+      can::Frame canframe;
+      canframe.id = msg.id;
+      canframe.dlc = msg.dlc;
+      canframe.is_error = msg.is_error;
+      canframe.is_rtr = msg.is_rtr;
+      canframe.is_extended = msg.is_extended;
+      size_t n = 0, n_max = std::min(msg.data.size(),canframe.data.size());
+      for(n = 0; n < n_max; n++)
+        canframe.data[n] = msg.data[n];
+      for(; n < canframe.data.size(); n++)
+        canframe.data[n] = 0;
+      if(!canframe.isValid())
+      {
+        ROS_ERROR_STREAM("sick_line_guidance::SocketCANSender::messageCallback(): received invalid can_msgs::Frame message: " << msg);
+        ROS_ERROR_STREAM("sick_line_guidance::SocketCANSender::messageCallback(): sending invalid can frame " << can::tostring(canframe, false) << " ...");
+      }
+      if(m_socketcan_interface->send(canframe))
+      {
+        ROS_DEBUG_STREAM("sick_line_guidance::SocketCANSender::messageCallback(): sent can message " << can::tostring(canframe, false));
+      }
+      else
+      {
+        ROS_ERROR_STREAM("sick_line_guidance::SocketCANSender::messageCallback(): send can message " << can::tostring(canframe, false) << " failed.");
+      }
+    }
+    
+  protected:
+  
+    can::DriverInterfaceSharedPtr m_socketcan_interface; // can::SocketCANInterface instance
+    boost::thread* m_socketcan_thread; // thread to run can::SocketCANInterface in background
+    bool m_socketcan_running; // flag indicating start and stop of m_socketcan_thread
+  
+    /*
+     * @brief runs can::SocketCANInterface::run() in an endless loop
+     */
+    void run()
+    {
+      while(m_socketcan_running && ros::ok())
+      {
+        m_socketcan_interface->run();
+      }
+    }
+    
+  }; // SocketCANSender
+} // sick_line_guidance
+
+/*
+ * sick_line_guidance_ros2can_node: subscribes to ros topic (message type can_msgs::Frame),
+ * converts all messages to can frames (type can::Frame) and writes them to can bus.
+ * A simple cansend with input by ros messages.
+ */
+int main(int argc, char** argv)
+{
+  // Setup and configuration
+  ros::init(argc, argv, "sick_line_guidance_ros2can_node");
+  ros::NodeHandle nh;
+  std::string can_device = "can0", ros_topic = "ros2can0";
+  int subscribe_queue_size = 32;
+  nh.param("can_device", can_device, can_device); // name of can net device (socketcan interface)
+  nh.param("ros_topic", ros_topic, ros_topic); // topic for ros messages, message type can_msgs::Frame
+  nh.param("subscribe_queue_size", subscribe_queue_size, subscribe_queue_size); // buffer size for ros messages
+  ROS_INFO_STREAM("sick_line_guidance_ros2can_node: version " << sick_line_guidance::Version::getVersionInfo());
+  ROS_INFO_STREAM("sick_line_guidance_ros2can_node: starting...");
+  
+  // Create the SocketCANInterface
+  can::DriverInterfaceSharedPtr p_socketcan_interface = 0;
+  canopen::GuardedClassLoader<can::DriverInterface> driver_loader("socketcan_interface", "can::DriverInterface");
+  try
+  {
+    ROS_INFO_STREAM("sick_line_guidance_ros2can_node: initializing SocketCANInterface...");
+    p_socketcan_interface =  driver_loader.createInstance("can::SocketCANInterface");
+    if(!p_socketcan_interface->init(can_device, false, can::NoSettings::create()))
+      ROS_ERROR_STREAM("sick_line_guidance_ros2can_node: SocketCANInterface::init() failed.");
+  }
+  catch(pluginlib::PluginlibException& ex)
+  {
+    ROS_ERROR_STREAM("sick_line_guidance_ros2can_node: createInstance(\"can::SocketCANInterface\") failed: " << ex.what() << ", exit with error");
+    return 1;
+  }
+  ROS_INFO_STREAM("sick_line_guidance_ros2can_node: SocketCANInterface created, state = " << p_socketcan_interface->getState().driver_state);
+  
+  // Subscribe to ros topic
+  sick_line_guidance::SocketCANSender socketcan_sender(p_socketcan_interface);
+  ros::Subscriber ros_subscriber = nh.subscribe(ros_topic, subscribe_queue_size, &sick_line_guidance::SocketCANSender::messageCallback, &socketcan_sender);
+  socketcan_sender.start();
+  ROS_INFO_STREAM("sick_line_guidance_ros2can_node: SocketCANSender started, listening to ros topic \"" << ros_topic << "\" ...");
+  
+  // Run ros event loop
+  ros::spin();
+  
+  std::cout << "sick_line_guidance_ros2can_node: exiting..." << std::endl;
+  ROS_INFO_STREAM("sick_line_guidance_ros2can_node: exiting...");
+  socketcan_sender.stop();
+  p_socketcan_interface = 0;
+  return 0;
+}
+