Program Listing for File sick_ros_wrapper.h

↰ Return to documentation for file (/tmp/ws/src/sick_scan_xd/include/sick_scan/sick_ros_wrapper.h)

#include "sick_scan/sick_scan_base.h" /* Base definitions included in all header files, added by add_sick_scan_base_header.py. Do not edit this line. */
/*
 * @brief Wrapper for systemdependent API to Windows/Linux native, ROS-1 and ROS-2
 *
 * Use
 *   #include <sick_scan/sick_ros_wrapper.h>
 * for
 *   #include <ros/ros.h>
 *
 * Copyright (C) 2021, Ing.-Buero Dr. Michael Lehning, Hildesheim
 * Copyright (C) 2021, SICK AG, Waldkirch
 * All rights reserved.
 *
* 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 Osnabrueck University 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 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.
 *
 *  Created on: 12.01.2021
 *
 *      Authors:
 *         Michael Lehning <michael.lehning@lehning.de>
 *
 */

#ifndef __SICK_ROS_WRAPPER_H_INCLUDED
#define __SICK_ROS_WRAPPER_H_INCLUDED

#define _USE_MATH_DEFINES
#include <math.h>
#include <memory>
#include <mutex>
#include <string>
#include <sstream>
#include <thread>
#include <vector>
#include <chrono>
#include <cstdarg>

#if !defined __ROS_VERSION
#define __ROS_VERSION 0 // default: native Linux or Windows
#endif

// Default target is little endian. Overwrite SICK_TARGET_ENDIANESS in CMakeLists.txt to build for big endian.
#define SICK_LITTLE_ENDIAN 1 // LITTLE_ENDIAN and BIG_ENDIAN might already be defined differently on a system or in a 3rd party headerfile,
#define SICK_BIG_ENDIAN    2 // SICK_TARGET_ENDIANESS, SICK_LITTLE_ENDIAN and SICK_BIG_ENDIAN are used to avoid conflicts
#ifndef SICK_TARGET_ENDIANESS
#define SICK_TARGET_ENDIANESS SICK_LITTLE_ENDIAN
#endif
#if SICK_TARGET_ENDIANESS==SICK_LITTLE_ENDIAN
#define TARGET_IS_LITTLE_ENDIAN 1
#else
#define TARGET_IS_LITTLE_ENDIAN 0
#endif

#if defined _MSC_VER && defined min
#undef min
#endif
#if defined _MSC_VER && defined max
#undef max
#endif

template <typename T> std::string paramToString(const std::vector<T>& param_value)
{
    std::stringstream s;
    s << param_value.size();
    return s.str();
}

template <typename T> std::string paramToString(const T& param_value)
{
    std::stringstream s;
    s << param_value;
    return s.str();
}

bool shutdownSignalReceived();

#if __ROS_VERSION <= 1 // ROS-SIMU (native Linux or Windows) or ROS-1 (Linux only)

#if __ROS_VERSION == 0  // native Linux or Windows uses ros simu
#include <sick_scan/rosconsole_simu.hpp>
#define diagnostic_msgs_DiagnosticStatus_OK    (SICK_DIAGNOSTIC_STATUS::OK)
#define diagnostic_msgs_DiagnosticStatus_WARN  (SICK_DIAGNOSTIC_STATUS::WARN)
#define diagnostic_msgs_DiagnosticStatus_ERROR (SICK_DIAGNOSTIC_STATUS::SICK_DIAG_ERROR)
#endif

#include <ros/ros.h>
#include <geometry_msgs/Point.h>
#include <geometry_msgs/TransformStamped.h>
#include <nav_msgs/Odometry.h>
#include <sensor_msgs/LaserScan.h>
#include <sensor_msgs/PointCloud.h>
#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/Imu.h>
#include <std_msgs/String.h>
#include <std_msgs/ColorRGBA.h>
#include <visualization_msgs/Marker.h>
#include <visualization_msgs/MarkerArray.h>
#include <tf2/LinearMath/Quaternion.h>
#include <tf2_ros/transform_broadcaster.h>

typedef ros::NodeHandle* rosNodePtr;

#define ros_nav_msgs nav_msgs
#define ros_sensor_msgs sensor_msgs
#define ros_std_msgs std_msgs
#define ros_geometry_msgs geometry_msgs
#define ros_visualization_msgs visualization_msgs

#ifndef diagnostic_msgs_DiagnosticStatus_OK
#define diagnostic_msgs_DiagnosticStatus_OK diagnostic_msgs::DiagnosticStatus::OK
#endif
#ifndef diagnostic_msgs_DiagnosticStatus_WARN
#define diagnostic_msgs_DiagnosticStatus_WARN diagnostic_msgs::DiagnosticStatus::WARN
#endif
#ifndef diagnostic_msgs_DiagnosticStatus_ERROR
#define diagnostic_msgs_DiagnosticStatus_ERROR diagnostic_msgs::DiagnosticStatus::ERROR
#endif
#define ROS_HEADER_SEQ(msgheader,value) msgheader.seq=value

#include "sick_scan/sick_scan_logging.h"

template <typename T> void rosDeclareParam(rosNodePtr nh, const std::string& param_name, const T& param_value) { }
template <typename T> bool rosGetParam(rosNodePtr nh, const std::string& param_name, T& param_value) { return nh->getParam(param_name, param_value); }
template <typename T> void rosSetParam(rosNodePtr nh, const std::string& param_name, const T& param_value) { nh->setParam(param_name, param_value); }

typedef int rosQoS;
typedef ros::Duration rosDuration;
typedef ros::Time rosTime;
inline rosTime rosTimeNow(void) { return ros::Time::now(); }
inline uint32_t sec(const rosTime& time) { return time.sec; }   // return seconds part of ros::Time
inline uint32_t nsec(const rosTime& time) { return time.nsec; } // return nanoseconds part of ros::Time
inline uint32_t sec(const rosDuration& time) { return time.sec; }   // return seconds part of ros::Duration
inline uint32_t nsec(const rosDuration& time) { return time.nsec; } // return nanoseconds part of ros::Duration
inline uint64_t rosNanosecTimestampNow(void) { rosTime now = rosTimeNow(); return (((uint64_t)sec(now)) * (uint64_t)1000000000) + std::min((uint64_t)nsec(now), (uint64_t)1000000000); }
inline double rosTimeToSeconds(const rosTime& time) { return (double)sec(time) + 1.0e-9 * (double)nsec(time); }

template <class T> class rosPublisher : public ros::Publisher
{
public:
    rosPublisher() : ros::Publisher() {}
    rosPublisher(ros::Publisher& _publisher) : ros::Publisher(_publisher) {}
};
template <typename T> rosPublisher<T> rosAdvertise(rosNodePtr nh, const std::string& topic, uint32_t queue_size = 10, rosQoS qos = 10)
{
    int qos_val = -1;
    rosDeclareParam(nh, "ros_qos", qos_val);
    rosGetParam(nh, "ros_qos", qos_val);
    if (qos_val >= 0)
        qos = qos_val;
    std::string topic2;
    if(topic.empty() || topic[0] != '/')
      topic2 = std::string("/") + topic;
    else
      topic2 = topic;
    ROS_INFO_STREAM("Publishing on topic \"" << topic2 << "\", qos=" << qos);
    ros::Publisher publisher = nh->advertise<T>(topic2, queue_size);
    return rosPublisher<T>(publisher);
}
template <typename T> void rosPublish(rosPublisher<T>& publisher, const T& msg) { publisher.publish(msg); }
template <typename T> std::string rosTopicName(rosPublisher<T>& publisher) { return publisher.getTopic(); }

inline bool rosOk(void) { return !ros::isShuttingDown() && ros::ok() && !shutdownSignalReceived(); }
inline void rosSpin(rosNodePtr nh) { ros::spin(); }
inline void rosSpinOnce(rosNodePtr nh) { ros::spinOnce(); }
inline void rosShutdown(void) { ros::shutdown(); }
inline void rosSleep(double seconds) { ros::Duration(seconds).sleep(); }
inline rosDuration rosDurationFromSec(double seconds) { return rosDuration(seconds); }

#include <sick_scan_xd/RadarScan.h>         // generated by msg-generator
#include <sick_scan_xd/Encoder.h>           // generated by msg-generator
#include <sick_scan_xd/LIDoutputstateMsg.h> // generated by msg-generator
#include <sick_scan_xd/LFErecMsg.h>         // generated by msg-generator
#include <sick_scan_xd/NAVPoseData.h>       // generated by msg-generator
#include <sick_scan_xd/NAVLandmarkData.h>   // generated by msg-generator
#include <sick_scan_xd/NAVOdomVelocity.h>   // generated by msg-generator
#define sick_scan_msg sick_scan_xd

#include "sick_scan_xd/ColaMsgSrv.h"        // generated by srv-generator
#include "sick_scan_xd/ECRChangeArrSrv.h"   // generated by srv-generator
#include "sick_scan_xd/LIDoutputstateSrv.h" // generated by srv-generator
#include "sick_scan_xd/SCdevicestateSrv.h"  // generated by srv-generator
#include "sick_scan_xd/SCrebootSrv.h"       // generated by srv-generator
#include "sick_scan_xd/SCsoftresetSrv.h"    // generated by srv-generator
#include "sick_scan_xd/SickScanExitSrv.h"   // generated by srv-generator
#include "sick_scan_xd/GetContaminationResultSrv.h" // generated by srv-generator
#define sick_scan_srv sick_scan_xd

template <class T> class rosServiceClient : public ros::ServiceClient
{
public:
    rosServiceClient() : ros::ServiceClient() {}
    template <class U> rosServiceClient(U& _client) : ros::ServiceClient(_client) {}
};
template <class T> class rosServiceServer : public ros::ServiceServer
{
public:
    rosServiceServer() : ros::ServiceServer() {}
    template <class U> rosServiceServer(U& _server) : ros::ServiceServer(_server) {}
};
#define ROS_CREATE_SRV_CLIENT(nh,srv,name) nh->serviceClient<srv>(name)
#define ROS_CREATE_SRV_SERVER(nh,srv,name,cbfunction,cbobject) nh->advertiseService(name,cbfunction,cbobject)

#elif __ROS_VERSION == 2 // ROS-2 (Linux or Windows)

#include <rclcpp/clock.hpp>
#include <rclcpp/rclcpp.hpp>
#include <rclcpp/time_source.hpp>
#include <geometry_msgs/msg/point.hpp>
#include <geometry_msgs/msg/transform_stamped.hpp>
#include <nav_msgs/msg/odometry.hpp>
#include <sensor_msgs/msg/laser_scan.hpp>
#include <sensor_msgs/msg/point_cloud.hpp>
#include <sensor_msgs/msg/point_cloud2.hpp>
#include <sensor_msgs/msg/imu.hpp>
#include <std_msgs/msg/string.hpp>
#include <std_msgs/msg/color_rgba.hpp>
#include <visualization_msgs/msg/marker.hpp>
#include <visualization_msgs/msg/marker_array.hpp>
#include <tf2/LinearMath/Quaternion.h>
#include <tf2_ros/transform_broadcaster.h>

typedef rclcpp::Node::SharedPtr rosNodePtr;

#define ros_nav_msgs nav_msgs::msg
#define ros_sensor_msgs sensor_msgs::msg
#define ros_std_msgs std_msgs::msg
#define ros_geometry_msgs geometry_msgs::msg
#define ros_visualization_msgs visualization_msgs::msg

#ifndef diagnostic_msgs_DiagnosticStatus_OK
#define diagnostic_msgs_DiagnosticStatus_OK    0 //diagnostic_msgs::msg::DiagnosticStatus::OK
#endif
#ifndef diagnostic_msgs_DiagnosticStatus_WARN
#define diagnostic_msgs_DiagnosticStatus_WARN  1 // diagnostic_msgs::msg::DiagnosticStatus::WARN
#endif
#ifndef diagnostic_msgs_DiagnosticStatus_ERROR
#define diagnostic_msgs_DiagnosticStatus_ERROR 2 // diagnostic_msgs::msg::DiagnosticStatus::ERROR
#endif
#define ROS_HEADER_SEQ(msgheader,seq)

#include "sick_scan/sick_scan_logging.h"

inline void rosConvParam(const std::string& str_value, std::string& val){ val = str_value; }
inline void rosConvParam(const std::string& str_value, bool& val){ val = std::stoi(str_value) > 0; }
inline void rosConvParam(const std::string& str_value, int& val){ val = std::stoi(str_value); }
inline void rosConvParam(const std::string& str_value, float& val){ val = std::stof(str_value); }
inline void rosConvParam(const std::string& str_value, double& val){ val = std::stod(str_value); }

template <typename T> void rosDeclareParam(rosNodePtr nh, const std::string& param_name, const T& param_value) { if(!nh->has_parameter(param_name)) nh->declare_parameter<T>(param_name, param_value); }
template <typename T> bool rosGetParam(rosNodePtr nh, const std::string& param_name, T& param_value)
{
    try
    {
        bool bRet = nh->get_parameter(param_name, param_value);
        ROS_DEBUG_STREAM("rosGetParam(" << param_name << "): " << paramToString(param_value) << ", " << typeid(param_value).name());
        return bRet;
    }
    catch(const std::exception& exc)
    {
        ROS_WARN_STREAM("## ERROR rosGetParam(" << param_name << ", " << paramToString(param_value) << ", " << typeid(param_value).name() << ") failed, " << typeid(exc).name() << ", exception " << exc.what());
    }
    try
    {
        std::string str_value;
        bool bRet = nh->get_parameter(param_name, str_value);
        if (std::is_arithmetic<T>::value)
        {
            rosConvParam(str_value, param_value);
            ROS_INFO_STREAM("rosGetParam(" << param_name << "): converted to " << param_value);
            return bRet;
        }
        else
        {
            ROS_WARN_STREAM("## ERROR rosGetParam(" << param_name << ", " << paramToString(param_value) << ") failed.");
        }
    }
    catch(const std::exception& exc)
    {
        ROS_WARN_STREAM("## ERROR rosGetParam(" << param_name << ", " << paramToString(param_value) << ", " << typeid(param_value).name() << ") failed, " << typeid(exc).name() << ", exception " << exc.what());
    }
    return false;
}
template <typename T> void rosSetParam(rosNodePtr nh, const std::string& param_name, const T& param_value)
{
    try
    {
        ROS_DEBUG_STREAM("rosSetParam(" << param_name << "," << paramToString(param_value) << ", " << typeid(param_value).name() << ")");
        nh->set_parameter(rclcpp::Parameter(param_name, param_value));
    }
    catch(const std::exception& exc)
    {
        ROS_WARN_STREAM("## ERROR rosSetParam(" << param_name << ", " << paramToString(param_value) << ", " << typeid(param_value).name() << ") failed, exception " << exc.what());
    }
}

typedef rclcpp::QoS rosQoS;
typedef rclcpp::Duration rosDuration;
typedef rclcpp::Time rosTime; // typedef builtin_interfaces::msg::Time rosTime;
inline rosTime rosTimeNow(void) { return rclcpp::Clock().now(); }
inline uint32_t sec(const rosTime& time) { return (uint32_t)(time.nanoseconds() / 1000000000); }              // return seconds part of rclcpp::Time
inline uint32_t nsec(const rosTime& time) { return (uint32_t)(time.nanoseconds() - 1000000000 * sec(time)); } // return nanoseconds part of rclcpp::Time
inline uint32_t sec(const rosDuration& time) { return (uint32_t)(time.nanoseconds() / 1000000000); }              // return seconds part of rclcpp::Duration
inline uint32_t nsec(const rosDuration& time) { return (uint32_t)(time.nanoseconds() - 1000000000 * sec(time)); } // return nanoseconds part of rclcpp::Duration
inline uint64_t rosNanosecTimestampNow(void) { rosTime now = rosTimeNow(); return (((uint64_t)sec(now)) * (uint64_t)1000000000) + std::min((uint64_t)nsec(now), (uint64_t)1000000000); }
inline double rosTimeToSeconds(const rosTime& time) { return (double)sec(time) + 1.0e-9 * (double)nsec(time); }

class QoSConverter
{
public:
    int convert(const rosQoS& qos) const
    {
        for (std::map<int,rosQoS>::const_iterator qos_iter = m_qos_map.cbegin(); qos_iter != m_qos_map.cend(); qos_iter++)
            if (qos_iter->second == qos)
                return qos_iter->first;
        return 0;
    }
    rosQoS convert(const int& qos) const
    {
        for (std::map<int,rosQoS>::const_iterator qos_iter = m_qos_map.cbegin(); qos_iter != m_qos_map.cend(); qos_iter++)
            if (qos_iter->first == qos)
                return qos_iter->second;
        return rclcpp::SystemDefaultsQoS();
    }
protected:
    std::map<int,rosQoS> m_qos_map = { {0, rclcpp::SystemDefaultsQoS()}, {1, rclcpp::ParameterEventsQoS()}, {2, rclcpp::ServicesQoS()}, {3, rclcpp::ParametersQoS()}, {4, rclcpp::SensorDataQoS()} };
};

template <class T> class rosPublisher : public rclcpp::Publisher<T>::SharedPtr
{
public:
    rosPublisher() : rclcpp::Publisher<T>::SharedPtr(0) {}
    template <class U> rosPublisher(U& _publisher) : rclcpp::Publisher<T>::SharedPtr(_publisher) {}
};
inline void overwriteByOptionalQOSconfig(rosNodePtr nh, rosQoS& qos)
{
    QoSConverter qos_converter;
    int qos_val = -1;
    rosDeclareParam(nh, "ros_qos", qos_val);
    rosGetParam(nh, "ros_qos", qos_val);
    if (qos_val >= 0)
        qos = qos_converter.convert(qos_val);
}
template <class T> rosPublisher<T> rosAdvertise(rosNodePtr nh, const std::string& topic, uint32_t queue_size = 10, rosQoS qos = rclcpp::SystemDefaultsQoS())
{
    overwriteByOptionalQOSconfig(nh, qos);
    QoSConverter qos_converter;
    ROS_INFO_STREAM("Publishing on topic \"" << topic << "\", qos=" << qos_converter.convert(qos));
    auto publisher = nh->create_publisher<T>(topic, qos);
    return rosPublisher<T>(publisher);
}
template <typename T> void rosPublish(rosPublisher<T>& publisher, const T& msg) { publisher->publish(msg); }
template <typename T> std::string rosTopicName(rosPublisher<T>& publisher) { return publisher->get_topic_name(); }

inline bool rosOk(void) { return !shutdownSignalReceived() && rclcpp::ok(); }
inline void rosSpin(rosNodePtr nh) { rclcpp::spin(nh); }
inline void rosSpinOnce(rosNodePtr nh) { rclcpp::spin_some(nh); }
inline void rosShutdown(void) { rclcpp::shutdown(); }
inline void rosSleep(double seconds) { rclcpp::sleep_for(std::chrono::nanoseconds((int64_t)(seconds * 1.0e9))); }
inline rosDuration rosDurationFromSec(double seconds) { return rosDuration(std::chrono::nanoseconds((int64_t)(seconds * 1.0e9))); }

#include <sick_scan_xd/msg/radar_scan.hpp>          // generated by msg-generator
#include <sick_scan_xd/msg/encoder.hpp>             // generated by msg-generator
#include <sick_scan_xd/msg/li_doutputstate_msg.hpp> // generated by msg-generator
#include <sick_scan_xd/msg/lf_erec_msg.hpp>         // generated by msg-generator
#include <sick_scan_xd/msg/nav_pose_data.hpp>       // generated by msg-generator
#include <sick_scan_xd/msg/nav_landmark_data.hpp>   // generated by msg-generator
#include <sick_scan_xd/msg/nav_odom_velocity.hpp>   // generated by msg-generator
#define sick_scan_msg sick_scan_xd::msg

#include "sick_scan_xd/srv/cola_msg_srv.hpp"        // generated by rosidl-generator
#include "sick_scan_xd/srv/ecr_change_arr_srv.hpp"  // generated by rosidl-generator
#include "sick_scan_xd/srv/li_doutputstate_srv.hpp" // generated by rosidl-generator
#include "sick_scan_xd/srv/s_cdevicestate_srv.hpp"  // generated by rosidl-generator
#include "sick_scan_xd/srv/s_creboot_srv.hpp"       // generated by rosidl-generator
#include "sick_scan_xd/srv/s_csoftreset_srv.hpp"    // generated by rosidl-generator
#include "sick_scan_xd/srv/sick_scan_exit_srv.hpp"  // generated by rosidl-generator
#include "sick_scan_xd/srv/get_contamination_result_srv.hpp" // generated by rosidl-generator
#define sick_scan_srv sick_scan_xd::srv

template <class T> class rosServiceClient : public rclcpp::Client<T>::SharedPtr
{
public:
    rosServiceClient() : rclcpp::Client<T>::SharedPtr(0) {}
    template <class U> rosServiceClient(U& _client) : rclcpp::Client<T>::SharedPtr(_client) {}
};
template <class T> class rosServiceServer : public rclcpp::Service<T>::SharedPtr
{
public:
    rosServiceServer() : rclcpp::Service<T>::SharedPtr(0) {}
    template <class U> rosServiceServer(U& _server) : rclcpp::Service<T>::SharedPtr(_server) {}
};
#define ROS_CREATE_SRV_CLIENT(nh,srv,name) nh->create_client<srv>(name)
#define ROS_CREATE_SRV_SERVER(nh,srv,name,cbfunction,cbobject) nh->create_service<srv>(name,std::bind(cbfunction,cbobject,std::placeholders::_1,std::placeholders::_2))

#else

#error __ROS_VERSION undefined or unsupported, build with __ROS_VERSION 0, 1 or 2

#endif //__ROS_VERSION

/*
** dynamic reconfiguration and diagnostic_updater currently supported on ROS-Linux only, todo...
*/
#if __ROS_VERSION == 2 // ROS 2
#ifdef ROS_DIAGNOSTICS_UPDATER_AVAILABLE
#include <diagnostic_updater/diagnostic_updater.hpp> // part of diagnostic_msgs of ROS2, not available on ROS2-Windows until foxy patch 4
#include <diagnostic_updater/publisher.hpp>
#include <rcl_interfaces/msg/set_parameters_result.hpp>
#define USE_DYNAMIC_RECONFIGURE
#define USE_DIAGNOSTIC_UPDATER
namespace sick_scan_xd
{
    // Adapter to combine publisher for sensor_msgs::msg::PointCloud2 and diagnostic_msgs::msg::DiagnosticArray.
    // see https://github.com/ros/diagnostics/blob/eloquent/diagnostic_updater/include/diagnostic_updater/publisher.hpp
    // and https://github.com/ros/diagnostics/issues/164
    template <class DiagnosedPublisherT> class DiagnosedPublishAdapter : public diagnostic_updater::TopicDiagnostic
    {
    public:
    template <typename PublisherType> DiagnosedPublishAdapter(PublisherType publisher, diagnostic_updater::Updater & diag,
        const diagnostic_updater::FrequencyStatusParam & freq, const diagnostic_updater::TimeStampStatusParam & stamp)
    : diagnostic_updater::TopicDiagnostic(rosTopicName(publisher), diag, freq, stamp), publisher_(publisher)
    {
    }
    virtual ~DiagnosedPublishAdapter()
    {
    }
    template <typename MessageType> void publish(const std::shared_ptr<MessageType> & message)
    {
        if(message)
        publish(*message);
    }
    template <typename MessageType> void publish(const MessageType & message)
    {
        tick(message.header.stamp);
        rosPublish(publisher_, message); // publisher_->publish(message);
    }
    protected:
    DiagnosedPublisherT publisher_;
    };
}
#endif // ROS_DIAGNOSTICS_UPDATER_AVAILABLE
#elif __ROS_VERSION == 1 // ROS 1
#ifndef WIN32
#include <dynamic_reconfigure/server.h>
#include <diagnostic_updater/diagnostic_updater.h>
#include <diagnostic_updater/publisher.h>
//#include <sick_scan/SickScanConfig.h>
#include <sick_scan_xd/SickLDMRSDriverConfig.h>
#define USE_DYNAMIC_RECONFIGURE
#define USE_DIAGNOSTIC_UPDATER
#endif // WIN32
#include <sick_scan_xd/SickScanConfig.h>
#else
#include <diagnostic_updater/diagnostic_updater.h>
#include <diagnostic_updater/publisher.h>
#endif
#if __ROS_VERSION != 1
namespace sick_scan_xd
{
    class SickScanConfig { // sick_scan2/include/sick_scan/SickScanConfig.h
    public:
        std::string frame_id = "cloud";
        std::string imu_frame_id = "imu_link";
        bool intensity = true; // false;
        bool auto_reboot = false;
        double min_ang = -M_PI / 2;
        double max_ang = +M_PI / 2;
        double ang_res = 0;
        int skip = 0;
        bool sw_pll_only_publish = false;
        bool use_generation_timestamp = true;
        double time_offset = 0;
        int cloud_output_mode = 0;
    };
}
#endif

#endif // __SICK_ROS_WRAPPER_H_INCLUDED