sick_ros_wrapper.h
Go to the documentation of this file.
1 #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. */
2 /*
3  * @brief Wrapper for systemdependent API to Windows/Linux native, ROS-1 and ROS-2
4  *
5  * Use
6  * #include <sick_scan/sick_ros_wrapper.h>
7  * for
8  * #include <ros/ros.h>
9  *
10  * Copyright (C) 2021, Ing.-Buero Dr. Michael Lehning, Hildesheim
11  * Copyright (C) 2021, SICK AG, Waldkirch
12  * All rights reserved.
13  *
14 * Licensed under the Apache License, Version 2.0 (the "License");
15 * you may not use this file except in compliance with the License.
16 * You may obtain a copy of the License at
17 *
18 * http://www.apache.org/licenses/LICENSE-2.0
19 *
20 * Unless required by applicable law or agreed to in writing, software
21 * distributed under the License is distributed on an "AS IS" BASIS,
22 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
23 * See the License for the specific language governing permissions and
24 * limitations under the License.
25 *
26 *
27 * All rights reserved.
28 *
29 * Redistribution and use in source and binary forms, with or without
30 * modification, are permitted provided that the following conditions are met:
31 *
32 * * Redistributions of source code must retain the above copyright
33 * notice, this list of conditions and the following disclaimer.
34 * * Redistributions in binary form must reproduce the above copyright
35 * notice, this list of conditions and the following disclaimer in the
36 * documentation and/or other materials provided with the distribution.
37 * * Neither the name of Osnabrueck University nor the names of its
38 * contributors may be used to endorse or promote products derived from
39 * this software without specific prior written permission.
40 * * Neither the name of SICK AG nor the names of its
41 * contributors may be used to endorse or promote products derived from
42 * this software without specific prior written permission
43 * * Neither the name of Ing.-Buero Dr. Michael Lehning nor the names of its
44 * contributors may be used to endorse or promote products derived from
45 * this software without specific prior written permission
46 *
47 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
48 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
49 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
50 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
51 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
52 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
53 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
54 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
55 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
56 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
57 * POSSIBILITY OF SUCH DAMAGE.
58  *
59  * Created on: 12.01.2021
60  *
61  * Authors:
62  * Michael Lehning <michael.lehning@lehning.de>
63  *
64  */
65 
66 #ifndef __SICK_ROS_WRAPPER_H_INCLUDED
67 #define __SICK_ROS_WRAPPER_H_INCLUDED
68 
69 #define _USE_MATH_DEFINES
70 #include <math.h>
71 #include <memory>
72 #include <mutex>
73 #include <string>
74 #include <sstream>
75 #include <thread>
76 #include <vector>
77 #include <chrono>
78 #include <cstdarg>
79 #include <cstdint>
80 
81 #if !defined __ROS_VERSION
82 #define __ROS_VERSION 0 // default: native Linux or Windows
83 #endif
84 
85 // Default target is little endian. Overwrite SICK_TARGET_ENDIANESS in CMakeLists.txt to build for big endian.
86 #define SICK_LITTLE_ENDIAN 1 // LITTLE_ENDIAN and BIG_ENDIAN might already be defined differently on a system or in a 3rd party headerfile,
87 #define SICK_BIG_ENDIAN 2 // SICK_TARGET_ENDIANESS, SICK_LITTLE_ENDIAN and SICK_BIG_ENDIAN are used to avoid conflicts
88 #ifndef SICK_TARGET_ENDIANESS
89 #define SICK_TARGET_ENDIANESS SICK_LITTLE_ENDIAN
90 #endif
91 #if SICK_TARGET_ENDIANESS==SICK_LITTLE_ENDIAN
92 #define TARGET_IS_LITTLE_ENDIAN 1
93 #else
94 #define TARGET_IS_LITTLE_ENDIAN 0
95 #endif
96 
97 #if defined _MSC_VER && defined min
98 #undef min
99 #endif
100 #if defined _MSC_VER && defined max
101 #undef max
102 #endif
103 
104 template <typename T> std::string paramToString(const std::vector<T>& param_value)
105 {
106  std::stringstream s;
107  s << param_value.size();
108  return s.str();
109 }
110 
111 template <typename T> std::string paramToString(const T& param_value)
112 {
113  std::stringstream s;
114  s << param_value;
115  return s.str();
116 }
117 
118 bool shutdownSignalReceived();
119 
120 #if __ROS_VERSION <= 1 // ROS-SIMU (native Linux or Windows) or ROS-1 (Linux only)
121 
122 #if __ROS_VERSION == 0 // native Linux or Windows uses ros simu
123 #include <sick_scan/rosconsole_simu.hpp>
124 #define diagnostic_msgs_DiagnosticStatus_OK (SICK_DIAGNOSTIC_STATUS::OK)
125 #define diagnostic_msgs_DiagnosticStatus_WARN (SICK_DIAGNOSTIC_STATUS::WARN)
126 #define diagnostic_msgs_DiagnosticStatus_ERROR (SICK_DIAGNOSTIC_STATUS::SICK_DIAG_ERROR)
127 #endif
128 
129 #include <ros/ros.h>
130 #include <geometry_msgs/Point.h>
131 #include <geometry_msgs/Pose2D.h>
132 #include <geometry_msgs/TransformStamped.h>
133 #include <nav_msgs/Odometry.h>
134 #include <sensor_msgs/LaserScan.h>
135 #include <sensor_msgs/PointCloud.h>
136 #include <sensor_msgs/PointCloud2.h>
137 #include <sensor_msgs/Imu.h>
138 #include <std_msgs/String.h>
139 #include <std_msgs/ColorRGBA.h>
140 #include <visualization_msgs/Marker.h>
141 #include <visualization_msgs/MarkerArray.h>
142 #include <tf2/LinearMath/Quaternion.h>
143 #include <tf2_ros/transform_broadcaster.h>
144 
145 typedef ros::NodeHandle* rosNodePtr;
146 
147 #define ros_nav_msgs nav_msgs
148 #define ros_sensor_msgs sensor_msgs
149 #define ros_std_msgs std_msgs
150 #define ros_geometry_msgs geometry_msgs
151 #define ros_visualization_msgs visualization_msgs
152 
153 #ifndef diagnostic_msgs_DiagnosticStatus_OK
154 #define diagnostic_msgs_DiagnosticStatus_OK diagnostic_msgs::DiagnosticStatus::OK
155 #endif
156 #ifndef diagnostic_msgs_DiagnosticStatus_WARN
157 #define diagnostic_msgs_DiagnosticStatus_WARN diagnostic_msgs::DiagnosticStatus::WARN
158 #endif
159 #ifndef diagnostic_msgs_DiagnosticStatus_ERROR
160 #define diagnostic_msgs_DiagnosticStatus_ERROR diagnostic_msgs::DiagnosticStatus::ERROR
161 #endif
162 #define ROS_HEADER_SEQ(msgheader,value) msgheader.seq=value
163 
164 #include "sick_scan/sick_scan_logging.h"
165 
166 template <typename T> void rosDeclareParam(rosNodePtr nh, const std::string& param_name, const T& param_value) { }
167 template <typename T> bool rosGetParam(rosNodePtr nh, const std::string& param_name, T& param_value) { return nh->getParam(param_name, param_value); }
168 template <typename T> void rosSetParam(rosNodePtr nh, const std::string& param_name, const T& param_value) { nh->setParam(param_name, param_value); }
169 
170 typedef int rosQoS;
171 typedef ros::Duration rosDuration;
172 typedef ros::Time rosTime;
173 inline rosTime rosTimeNow(void) { return ros::Time::now(); }
174 inline uint32_t sec(const rosTime& time) { return time.sec; } // return seconds part of ros::Time
175 inline uint32_t nsec(const rosTime& time) { return time.nsec; } // return nanoseconds part of ros::Time
176 inline uint32_t sec(const rosDuration& time) { return time.sec; } // return seconds part of ros::Duration
177 inline uint32_t nsec(const rosDuration& time) { return time.nsec; } // return nanoseconds part of ros::Duration
178 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); }
179 inline double rosTimeToSeconds(const rosTime& time) { return (double)sec(time) + 1.0e-9 * (double)nsec(time); }
180 
181 template <class T> class rosPublisher : public ros::Publisher
182 {
183 public:
184  rosPublisher() : ros::Publisher() {}
185  rosPublisher(ros::Publisher& _publisher) : ros::Publisher(_publisher) {}
186 };
187 template <typename T> rosPublisher<T> rosAdvertise(rosNodePtr nh, const std::string& topic, uint32_t queue_size = 10, rosQoS qos = 10)
188 {
189  int qos_val = -1;
190  rosDeclareParam(nh, "ros_qos", qos_val);
191  rosGetParam(nh, "ros_qos", qos_val);
192  if (qos_val >= 0)
193  qos = qos_val;
194  std::string topic2;
195  if(topic.empty() || topic[0] != '/')
196  topic2 = std::string("/") + topic;
197  else
198  topic2 = topic;
199  ROS_INFO_STREAM("Publishing on topic \"" << topic2 << "\", qos=" << qos);
200  ros::Publisher publisher = nh->advertise<T>(topic2, queue_size);
201  return rosPublisher<T>(publisher);
202 }
203 template <typename T> void rosPublish(rosPublisher<T>& publisher, const T& msg) { publisher.publish(msg); }
204 template <typename T> std::string rosTopicName(rosPublisher<T>& publisher) { return publisher.getTopic(); }
205 
206 inline bool rosOk(void) { return !ros::isShuttingDown() && ros::ok() && !shutdownSignalReceived(); }
207 inline void rosSpin(rosNodePtr nh) { ros::spin(); }
208 inline void rosSpinOnce(rosNodePtr nh) { ros::spinOnce(); }
209 inline void rosShutdown(void) { ros::shutdown(); }
210 inline void rosSleep(double seconds) { ros::Duration(seconds).sleep(); }
211 inline rosDuration rosDurationFromSec(double seconds) { return rosDuration(seconds); }
212 
213 #include <sick_scan_xd/RadarScan.h> // generated by msg-generator
214 #include <sick_scan_xd/Encoder.h> // generated by msg-generator
215 #include <sick_scan_xd/LIDinputstateMsg.h> // generated by msg-generator
216 #include <sick_scan_xd/LIDoutputstateMsg.h> // generated by msg-generator
217 #include <sick_scan_xd/LFErecMsg.h> // generated by msg-generator
218 #include <sick_scan_xd/NAVPoseData.h> // generated by msg-generator
219 #include <sick_scan_xd/NAVLandmarkData.h> // generated by msg-generator
220 #include <sick_scan_xd/NAVOdomVelocity.h> // generated by msg-generator
221 #define sick_scan_msg sick_scan_xd
222 
223 #include "sick_scan_xd/ColaMsgSrv.h" // generated by srv-generator
224 #include "sick_scan_xd/ECRChangeArrSrv.h" // generated by srv-generator
225 #include "sick_scan_xd/LIDoutputstateSrv.h" // generated by srv-generator
226 #include "sick_scan_xd/SCdevicestateSrv.h" // generated by srv-generator
227 #include "sick_scan_xd/SCrebootSrv.h" // generated by srv-generator
228 #include "sick_scan_xd/SCsoftresetSrv.h" // generated by srv-generator
229 #include "sick_scan_xd/SickScanExitSrv.h" // generated by srv-generator
230 #include "sick_scan_xd/GetContaminationResultSrv.h" // generated by srv-generator
231 #define sick_scan_srv sick_scan_xd
232 #if __ROS_VERSION == 1
233 #include "sick_scan_xd/FieldSetReadSrv.h" // generated by srv-generator
234 #include "sick_scan_xd/FieldSetWriteSrv.h" // generated by srv-generator
235 #endif
236 
237 template <class T> class rosServiceClient : public ros::ServiceClient
238 {
239 public:
240  rosServiceClient() : ros::ServiceClient() {}
241  template <class U> rosServiceClient(U& _client) : ros::ServiceClient(_client) {}
242 };
243 template <class T> class rosServiceServer : public ros::ServiceServer
244 {
245 public:
246  rosServiceServer() : ros::ServiceServer() {}
247  template <class U> rosServiceServer(U& _server) : ros::ServiceServer(_server) {}
248 };
249 #define ROS_CREATE_SRV_CLIENT(nh,srv,name) nh->serviceClient<srv>(name)
250 #define ROS_CREATE_SRV_SERVER(nh,srv,name,cbfunction,cbobject) nh->advertiseService(name,cbfunction,cbobject)
251 
252 #elif __ROS_VERSION == 2 // ROS-2 (Linux or Windows)
253 
254 #include <rclcpp/clock.hpp>
255 #include <rclcpp/rclcpp.hpp>
256 #include <rclcpp/time_source.hpp>
257 #include <geometry_msgs/msg/point.hpp>
258 #include <geometry_msgs/msg/transform_stamped.hpp>
259 #include <nav_msgs/msg/odometry.hpp>
260 #include <sensor_msgs/msg/laser_scan.hpp>
261 #include <sensor_msgs/msg/point_cloud.hpp>
262 #include <sensor_msgs/msg/point_cloud2.hpp>
263 #include <sensor_msgs/msg/imu.hpp>
264 #include <std_msgs/msg/string.hpp>
265 #include <std_msgs/msg/color_rgba.hpp>
266 #include <visualization_msgs/msg/marker.hpp>
267 #include <visualization_msgs/msg/marker_array.hpp>
268 #include <tf2/LinearMath/Quaternion.h>
269 #include <tf2_ros/transform_broadcaster.h>
270 
271 typedef rclcpp::Node::SharedPtr rosNodePtr;
272 
273 #define ros_nav_msgs nav_msgs::msg
274 #define ros_sensor_msgs sensor_msgs::msg
275 #define ros_std_msgs std_msgs::msg
276 #define ros_geometry_msgs geometry_msgs::msg
277 #define ros_visualization_msgs visualization_msgs::msg
278 
279 #ifndef diagnostic_msgs_DiagnosticStatus_OK
280 #define diagnostic_msgs_DiagnosticStatus_OK 0 //diagnostic_msgs::msg::DiagnosticStatus::OK
281 #endif
282 #ifndef diagnostic_msgs_DiagnosticStatus_WARN
283 #define diagnostic_msgs_DiagnosticStatus_WARN 1 // diagnostic_msgs::msg::DiagnosticStatus::WARN
284 #endif
285 #ifndef diagnostic_msgs_DiagnosticStatus_ERROR
286 #define diagnostic_msgs_DiagnosticStatus_ERROR 2 // diagnostic_msgs::msg::DiagnosticStatus::ERROR
287 #endif
288 #define ROS_HEADER_SEQ(msgheader,seq)
289 
290 #include "sick_scan/sick_scan_logging.h"
291 
292 inline void rosConvParam(const std::string& str_value, std::string& val){ val = str_value; }
293 inline void rosConvParam(const std::string& str_value, bool& val){ val = std::stoi(str_value) > 0; }
294 inline void rosConvParam(const std::string& str_value, int& val){ val = std::stoi(str_value); }
295 inline void rosConvParam(const std::string& str_value, float& val){ val = std::stof(str_value); }
296 inline void rosConvParam(const std::string& str_value, double& val){ val = std::stod(str_value); }
297 
298 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); }
299 template <typename T> bool rosGetParam(rosNodePtr nh, const std::string& param_name, T& param_value)
300 {
301  try
302  {
303  bool bRet = nh->get_parameter(param_name, param_value);
304  ROS_DEBUG_STREAM("rosGetParam(" << param_name << "): " << paramToString(param_value) << ", " << typeid(param_value).name());
305  return bRet;
306  }
307  catch(const std::exception& exc)
308  {
309  ROS_WARN_STREAM("## WARNING rosGetParam(" << param_name << ", " << paramToString(param_value) << ", " << typeid(param_value).name() << ") failed, " << typeid(exc).name() << ", exception " << exc.what());
310  }
311  try
312  {
313  std::string str_value;
314  bool bRet = nh->get_parameter(param_name, str_value);
315  if (std::is_arithmetic<T>::value)
316  {
317  rosConvParam(str_value, param_value);
318  ROS_INFO_STREAM("rosGetParam(" << param_name << "): converted to " << param_value);
319  return bRet;
320  }
321  else
322  {
323  ROS_WARN_STREAM("## WARNING rosGetParam(" << param_name << ", " << paramToString(param_value) << ") failed.");
324  }
325  }
326  catch(const std::exception& exc)
327  {
328  ROS_WARN_STREAM("## WARNING rosGetParam(" << param_name << ", " << paramToString(param_value) << ", " << typeid(param_value).name() << ") failed, " << typeid(exc).name() << ", exception " << exc.what());
329  }
330  return false;
331 }
332 template <typename T> void rosSetParam(rosNodePtr nh, const std::string& param_name, const T& param_value)
333 {
334  try
335  {
336  ROS_DEBUG_STREAM("rosSetParam(" << param_name << "," << paramToString(param_value) << ", " << typeid(param_value).name() << ")");
337  nh->set_parameter(rclcpp::Parameter(param_name, param_value));
338  }
339  catch(const std::exception& exc)
340  {
341  ROS_WARN_STREAM("## WARNING rosSetParam(" << param_name << ", " << paramToString(param_value) << ", " << typeid(param_value).name() << ") failed, exception " << exc.what());
342  }
343 }
344 
345 typedef rclcpp::QoS rosQoS;
346 typedef rclcpp::Duration rosDuration;
347 typedef rclcpp::Time rosTime; // typedef builtin_interfaces::msg::Time rosTime;
348 inline rosTime rosTimeNow(void) { return rclcpp::Clock().now(); }
349 inline uint32_t sec(const rosTime& time) { return (uint32_t)(time.nanoseconds() / 1000000000); } // return seconds part of rclcpp::Time
350 inline uint32_t nsec(const rosTime& time) { return (uint32_t)(time.nanoseconds() - 1000000000 * sec(time)); } // return nanoseconds part of rclcpp::Time
351 inline uint32_t sec(const rosDuration& time) { return (uint32_t)(time.nanoseconds() / 1000000000); } // return seconds part of rclcpp::Duration
352 inline uint32_t nsec(const rosDuration& time) { return (uint32_t)(time.nanoseconds() - 1000000000 * sec(time)); } // return nanoseconds part of rclcpp::Duration
353 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); }
354 inline double rosTimeToSeconds(const rosTime& time) { return (double)sec(time) + 1.0e-9 * (double)nsec(time); }
355 
356 class QoSConverter
357 {
358 public:
359  int convert(const rosQoS& qos) const
360  {
361  for (std::map<int,rosQoS>::const_iterator qos_iter = m_qos_map.cbegin(); qos_iter != m_qos_map.cend(); qos_iter++)
362  if (qos_iter->second == qos)
363  return qos_iter->first;
364  return 0;
365  }
366  rosQoS convert(const int& qos) const
367  {
368  for (std::map<int,rosQoS>::const_iterator qos_iter = m_qos_map.cbegin(); qos_iter != m_qos_map.cend(); qos_iter++)
369  if (qos_iter->first == qos)
370  return qos_iter->second;
371  return rclcpp::SystemDefaultsQoS();
372  }
373 protected:
374  std::map<int,rosQoS> m_qos_map = { {0, rclcpp::SystemDefaultsQoS()}, {1, rclcpp::ParameterEventsQoS()}, {2, rclcpp::ServicesQoS()}, {3, rclcpp::ParametersQoS()}, {4, rclcpp::SensorDataQoS()} };
375 };
376 
377 template <class T> class rosPublisher : public rclcpp::Publisher<T>::SharedPtr
378 {
379 public:
380  rosPublisher() : rclcpp::Publisher<T>::SharedPtr(0) {}
381  template <class U> rosPublisher(U& _publisher) : rclcpp::Publisher<T>::SharedPtr(_publisher) {}
382 };
383 inline void overwriteByOptionalQOSconfig(rosNodePtr nh, rosQoS& qos)
384 {
385  QoSConverter qos_converter;
386  int qos_val = -1;
387  rosDeclareParam(nh, "ros_qos", qos_val);
388  rosGetParam(nh, "ros_qos", qos_val);
389  if (qos_val >= 0)
390  qos = qos_converter.convert(qos_val);
391 }
392 template <class T> rosPublisher<T> rosAdvertise(rosNodePtr nh, const std::string& topic, uint32_t queue_size = 10, rosQoS qos = rclcpp::SystemDefaultsQoS())
393 {
394  overwriteByOptionalQOSconfig(nh, qos);
395  QoSConverter qos_converter;
396  ROS_INFO_STREAM("Publishing on topic \"" << topic << "\", qos=" << qos_converter.convert(qos));
397  auto publisher = nh->create_publisher<T>(topic, qos);
398  return rosPublisher<T>(publisher);
399 }
400 template <typename T> void rosPublish(rosPublisher<T>& publisher, const T& msg) { publisher->publish(msg); }
401 template <typename T> std::string rosTopicName(rosPublisher<T>& publisher) { return publisher->get_topic_name(); }
402 
403 inline bool rosOk(void) { return !shutdownSignalReceived() && rclcpp::ok(); }
404 inline void rosSpin(rosNodePtr nh) { rclcpp::spin(nh); }
405 inline void rosSpinOnce(rosNodePtr nh) { rclcpp::spin_some(nh); }
406 inline void rosShutdown(void) { rclcpp::shutdown(); }
407 inline void rosSleep(double seconds) { rclcpp::sleep_for(std::chrono::nanoseconds((int64_t)(seconds * 1.0e9))); }
408 inline rosDuration rosDurationFromSec(double seconds) { return rosDuration(std::chrono::nanoseconds((int64_t)(seconds * 1.0e9))); }
409 
410 #include <sick_scan_xd/msg/radar_scan.hpp> // generated by msg-generator
411 #include <sick_scan_xd/msg/encoder.hpp> // generated by msg-generator
412 #include <sick_scan_xd/msg/li_dinputstate_msg.hpp> // generated by msg-generator
413 #include <sick_scan_xd/msg/li_doutputstate_msg.hpp> // generated by msg-generator
414 #include <sick_scan_xd/msg/lf_erec_msg.hpp> // generated by msg-generator
415 #include <sick_scan_xd/msg/nav_pose_data.hpp> // generated by msg-generator
416 #include <sick_scan_xd/msg/nav_landmark_data.hpp> // generated by msg-generator
417 #include <sick_scan_xd/msg/nav_odom_velocity.hpp> // generated by msg-generator
418 #define sick_scan_msg sick_scan_xd::msg
419 
420 #include "sick_scan_xd/srv/cola_msg_srv.hpp" // generated by rosidl-generator
421 #include "sick_scan_xd/srv/ecr_change_arr_srv.hpp" // generated by rosidl-generator
422 #include "sick_scan_xd/srv/li_doutputstate_srv.hpp" // generated by rosidl-generator
423 #include "sick_scan_xd/srv/s_cdevicestate_srv.hpp" // generated by rosidl-generator
424 #include "sick_scan_xd/srv/s_creboot_srv.hpp" // generated by rosidl-generator
425 #include "sick_scan_xd/srv/s_csoftreset_srv.hpp" // generated by rosidl-generator
426 #include "sick_scan_xd/srv/sick_scan_exit_srv.hpp" // generated by rosidl-generator
427 #include "sick_scan_xd/srv/get_contamination_result_srv.hpp" // generated by rosidl-generator
428 #define sick_scan_srv sick_scan_xd::srv
429 #include "sick_scan_xd/srv/field_set_read_srv.hpp" // generated by srv-generator
430 #include "sick_scan_xd/srv/field_set_write_srv.hpp" // generated by srv-generator
431 
432 template <class T> class rosServiceClient : public rclcpp::Client<T>::SharedPtr
433 {
434 public:
435  rosServiceClient() : rclcpp::Client<T>::SharedPtr(0) {}
436  template <class U> rosServiceClient(U& _client) : rclcpp::Client<T>::SharedPtr(_client) {}
437 };
438 template <class T> class rosServiceServer : public rclcpp::Service<T>::SharedPtr
439 {
440 public:
441  rosServiceServer() : rclcpp::Service<T>::SharedPtr(0) {}
442  template <class U> rosServiceServer(U& _server) : rclcpp::Service<T>::SharedPtr(_server) {}
443 };
444 #define ROS_CREATE_SRV_CLIENT(nh,srv,name) nh->create_client<srv>(name)
445 #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))
446 
447 #else
448 
449 #error __ROS_VERSION undefined or unsupported, build with __ROS_VERSION 0, 1 or 2
450 
451 #endif //__ROS_VERSION
452 
453 /*
454 ** dynamic reconfiguration and diagnostic_updater currently supported on ROS-Linux only, todo...
455 */
456 #if __ROS_VERSION == 2 // ROS 2
457 #ifdef ROS_DIAGNOSTICS_UPDATER_AVAILABLE
458 #include <diagnostic_updater/diagnostic_updater.hpp> // part of diagnostic_msgs of ROS2, not available on ROS2-Windows until foxy patch 4
459 #include <diagnostic_updater/publisher.hpp>
460 #include <rcl_interfaces/msg/set_parameters_result.hpp>
461 #define USE_DYNAMIC_RECONFIGURE
462 #define USE_DIAGNOSTIC_UPDATER
463 namespace sick_scan_xd
464 {
465  // Adapter to combine publisher for sensor_msgs::msg::PointCloud2 and diagnostic_msgs::msg::DiagnosticArray.
466  // see https://github.com/ros/diagnostics/blob/eloquent/diagnostic_updater/include/diagnostic_updater/publisher.hpp
467  // and https://github.com/ros/diagnostics/issues/164
468  template <class DiagnosedPublisherT> class DiagnosedPublishAdapter : public diagnostic_updater::TopicDiagnostic
469  {
470  public:
471  template <typename PublisherType> DiagnosedPublishAdapter(PublisherType publisher, diagnostic_updater::Updater & diag,
472  const diagnostic_updater::FrequencyStatusParam & freq, const diagnostic_updater::TimeStampStatusParam & stamp)
473  : diagnostic_updater::TopicDiagnostic(rosTopicName(publisher), diag, freq, stamp), publisher_(publisher)
474  {
475  }
476  virtual ~DiagnosedPublishAdapter()
477  {
478  }
479  template <typename MessageType> void publish(const std::shared_ptr<MessageType> & message)
480  {
481  if(message)
482  publish(*message);
483  }
484  template <typename MessageType> void publish(const MessageType & message)
485  {
486  tick(message.header.stamp);
487  rosPublish(publisher_, message); // publisher_->publish(message);
488  }
489  protected:
490  DiagnosedPublisherT publisher_;
491  };
492 }
493 #endif // ROS_DIAGNOSTICS_UPDATER_AVAILABLE
494 #elif __ROS_VERSION == 1 // ROS 1
495 #ifndef WIN32
496 #include <dynamic_reconfigure/server.h>
497 #include <diagnostic_updater/diagnostic_updater.h>
498 #include <diagnostic_updater/publisher.h>
499 //#include <sick_scan/SickScanConfig.h>
500 #include <sick_scan_xd/SickLDMRSDriverConfig.h>
501 #define USE_DYNAMIC_RECONFIGURE
502 #define USE_DIAGNOSTIC_UPDATER
503 #endif // WIN32
504 #include <sick_scan_xd/SickScanConfig.h>
505 #else
506 #include <diagnostic_updater/diagnostic_updater.h>
507 #include <diagnostic_updater/publisher.h>
508 #endif
509 #if __ROS_VERSION != 1
510 namespace sick_scan_xd
511 {
512  class SickScanConfig { // sick_scan2/include/sick_scan/SickScanConfig.h
513  public:
514  std::string frame_id = "cloud";
515  std::string imu_frame_id = "imu_link";
516  bool intensity = true; // false;
517  bool auto_reboot = false;
518  double min_ang = -M_PI / 2;
519  double max_ang = +M_PI / 2;
520  double ang_res = 0;
521  int skip = 0;
522  bool sw_pll_only_publish = false;
523  bool use_generation_timestamp = true;
524  double time_offset = 0;
525  int cloud_output_mode = 0;
526  };
527 }
528 #endif
529 
530 #endif // __SICK_ROS_WRAPPER_H_INCLUDED
convert
void convert(const A &a, B &b)
ros::NodeHandle::setParam
void setParam(const std::string &key, bool b) const
rosServiceClient::rosServiceClient
rosServiceClient()
Definition: sick_ros_wrapper.h:240
shutdownSignalReceived
bool shutdownSignalReceived()
Definition: sick_generic_laser.cpp:206
SCdevicestateSrv.h
sick_scan_xd::SickScanConfig::auto_reboot
bool auto_reboot
Definition: sick_ros_wrapper.h:517
sick_scan_xd::SickScanConfig::max_ang
double max_ang
Definition: sick_ros_wrapper.h:519
roswrap::spin
ROSCPP_DECL void spin()
Enter simple event loop.
Definition: rossimu.cpp:260
Point.h
msg
msg
ros::Publisher
paramToString
std::string paramToString(const std::vector< T > &param_value)
Definition: sick_ros_wrapper.h:104
Imu.h
rosSetParam
void rosSetParam(rosNodePtr nh, const std::string &param_name, const T &param_value)
Definition: sick_ros_wrapper.h:168
diagnostic_updater::TimeStampStatusParam
A structure that holds the constructor parameters for the TimeStampStatus class.
Definition: eloquent/include/diagnostic_updater/update_functions.hpp:218
sick_scan_logging.h
s
XmlRpcServer s
ros
ros::NodeHandle::getParam
bool getParam(const std::string &key, bool &b) const
rosAdvertise
rosPublisher< T > rosAdvertise(rosNodePtr nh, const std::string &topic, uint32_t queue_size=10, rosQoS qos=10)
Definition: sick_ros_wrapper.h:187
rosServiceServer
Definition: sick_ros_wrapper.h:243
sick_scan_xd::SickScanConfig::sw_pll_only_publish
bool sw_pll_only_publish
Definition: sick_ros_wrapper.h:522
ColaMsgSrv.h
rosPublisher::rosPublisher
rosPublisher(ros::Publisher &_publisher)
Definition: sick_ros_wrapper.h:185
rosTopicName
std::string rosTopicName(rosPublisher< T > &publisher)
Definition: sick_ros_wrapper.h:204
ros::spinOnce
ROSCPP_DECL void spinOnce()
Process a single round of callbacks.
Definition: rossimu.cpp:255
diagnostic_updater::Updater
Manages a list of diagnostic tasks, and calls them in a rate-limited manner.
Definition: eloquent/include/diagnostic_updater/diagnostic_updater.hpp:353
DurationBase< Duration >::nsec
int32_t nsec
LIDinputstateMsg.h
ros::shutdown
ROSCPP_DECL void shutdown()
Disconnects everything and unregisters from the master. It is generally not necessary to call this fu...
Definition: rossimu.cpp:269
roswrap::console::shutdown
ROSCONSOLE_DECL void shutdown()
Definition: rossimu.cpp:269
rosSleep
void rosSleep(double seconds)
Definition: sick_ros_wrapper.h:210
rosPublisher::rosPublisher
rosPublisher()
Definition: sick_ros_wrapper.h:184
sick_scan_xd::SickScanConfig::min_ang
double min_ang
Definition: sick_ros_wrapper.h:518
SCsoftresetSrv.h
ros::Publisher::publish
void publish(const boost::shared_ptr< M > &message) const
ros::NodeHandle::advertise
Publisher advertise(AdvertiseOptions &ops)
Definition: rossimu.cpp:350
ros::ok
ROSCPP_DECL bool ok()
Check whether it's time to exit.
Definition: rossimu.cpp:273
sick_scan_xd
Definition: abstract_parser.cpp:65
rosQoS
int rosQoS
Definition: sick_ros_wrapper.h:170
ros::ServiceServer
Odometry.h
sick_scan_xd::SickScanConfig::ang_res
double ang_res
Definition: sick_ros_wrapper.h:520
rosServiceServer::rosServiceServer
rosServiceServer()
Definition: sick_ros_wrapper.h:246
sick_scan_xd::SickScanConfig::cloud_output_mode
int cloud_output_mode
Definition: sick_ros_wrapper.h:525
ROS::now
ROS::Time now(void)
Definition: ros_wrapper.cpp:116
sick_scan_xd::SickScanConfig::frame_id
std::string frame_id
Definition: sick_ros_wrapper.h:514
actionlib::SimpleActionClient
message
def message(msg, *args, **kwargs)
std_msgs::Duration
::std_msgs::Duration_< std::allocator< void > > Duration
Definition: Duration.h:48
LIDoutputstateMsg.h
nsec
uint32_t nsec(const rosTime &time)
Definition: sick_ros_wrapper.h:175
ROS_WARN_STREAM
#define ROS_WARN_STREAM(args)
Definition: sick_scan_logging.h:123
ROS_INFO_STREAM
#define ROS_INFO_STREAM(...)
Definition: sick_scan_ros2_example.cpp:71
api.setup.name
name
Definition: python/api/setup.py:12
rosTimeToSeconds
double rosTimeToSeconds(const rosTime &time)
Definition: sick_ros_wrapper.h:179
sick_scan_xd::SickScanConfig
Definition: sick_ros_wrapper.h:512
Pose2D.h
sick_scan_xd_api_test.queue_size
queue_size
Definition: sick_scan_xd_api_test.py:353
rosPublisher
Definition: sick_ros_wrapper.h:181
PointCloud2.h
rosServiceClient::rosServiceClient
rosServiceClient(U &_client)
Definition: sick_ros_wrapper.h:241
diagnostic_updater
Author: Blaise Gassend.
ros::ServiceServer::ServiceServer
ServiceServer()
sick_scan_xd::SickScanConfig::use_generation_timestamp
bool use_generation_timestamp
Definition: sick_ros_wrapper.h:523
RadarScan.h
rosPublish
void rosPublish(rosPublisher< T > &publisher, const T &msg)
Definition: sick_ros_wrapper.h:203
ros::ServiceClient
sick_scan_xd::SickScanConfig::intensity
bool intensity
Definition: sick_ros_wrapper.h:516
SCrebootSrv.h
ROS_DEBUG_STREAM
#define ROS_DEBUG_STREAM(args)
Definition: sick_scan_logging.h:113
ros::NodeHandle
sec
uint32_t sec(const rosTime &time)
Definition: sick_ros_wrapper.h:174
SickScanExitSrv.h
std_msgs::Time
::std_msgs::Time_< std::allocator< void > > Time
Definition: Time.h:48
rosServiceServer::rosServiceServer
rosServiceServer(U &_server)
Definition: sick_ros_wrapper.h:247
rosGetParam
bool rosGetParam(rosNodePtr nh, const std::string &param_name, T &param_value)
Definition: sick_ros_wrapper.h:167
TimeBase< Time, Duration >::sec
uint32_t sec
TimeBase< Time, Duration >::nsec
uint32_t nsec
LIDoutputstateSrv.h
rosSpinOnce
void rosSpinOnce(rosNodePtr nh)
Definition: sick_ros_wrapper.h:208
rosShutdown
void rosShutdown(void)
Definition: sick_ros_wrapper.h:209
ColorRGBA.h
rosSpin
void rosSpin(rosNodePtr nh)
Definition: sick_ros_wrapper.h:207
rosTimeNow
rosTime rosTimeNow(void)
Definition: sick_ros_wrapper.h:173
diagnostic_updater::TopicDiagnostic
A class to facilitate making diagnostics for a topic using a FrequencyStatus and TimeStampStatus.
Definition: eloquent/include/diagnostic_updater/publisher.hpp:107
Marker.h
PointCloud.h
ros::Publisher::getTopic
std::string getTopic() const
Definition: rossimu.cpp:416
sick_scan_xd::SickScanConfig::imu_frame_id
std::string imu_frame_id
Definition: sick_ros_wrapper.h:515
GetContaminationResultSrv.h
ros::Time
LaserScan.h
ros::Publisher::Publisher
Publisher()
ros::ServiceClient::ServiceClient
ServiceClient()
TransformStamped.h
rosServiceClient
Definition: sick_ros_wrapper.h:237
NAVOdomVelocity.h
ROS::seconds
double seconds(ROS::Duration duration)
Definition: ros_wrapper.cpp:180
String.h
ECRChangeArrSrv.h
ros::isShuttingDown
ROSCPP_DECL bool isShuttingDown()
Returns whether or not ros::shutdown() has been (or is being) called.
Definition: rossimu.cpp:277
sick_scan_base.h
NAVPoseData.h
rosNanosecTimestampNow
uint64_t rosNanosecTimestampNow(void)
Definition: sick_ros_wrapper.h:178
Encoder.h
rosDurationFromSec
rosDuration rosDurationFromSec(double seconds)
Definition: sick_ros_wrapper.h:211
ros::spin
ROSCPP_DECL void spin()
Enter simple event loop.
Definition: rossimu.cpp:260
rosDuration
ros::Duration rosDuration
Definition: sick_ros_wrapper.h:171
sick_scan_xd::SickScanConfig::time_offset
double time_offset
Definition: sick_ros_wrapper.h:524
roswrap::ok
ROSCPP_DECL bool ok()
Check whether it's time to exit.
Definition: rossimu.cpp:273
server.h
rosOk
bool rosOk(void)
Definition: sick_ros_wrapper.h:206
NAVLandmarkData.h
rosconsole_simu.hpp
ros::Duration
rosNodePtr
ros::NodeHandle * rosNodePtr
Definition: sick_ros_wrapper.h:145
rosDeclareParam
void rosDeclareParam(rosNodePtr nh, const std::string &param_name, const T &param_value)
Definition: sick_ros_wrapper.h:166
DurationBase< Duration >::sec
int32_t sec
rosTime
ros::Time rosTime
Definition: sick_ros_wrapper.h:172
ros::NodeHandle
LFErecMsg.h
ros::Time::now
static Time now()
diagnostic_updater::FrequencyStatusParam
A structure that holds the constructor parameters for the FrequencyStatus class.
Definition: eloquent/include/diagnostic_updater/update_functions.hpp:53
sick_scan_xd::SickScanConfig::skip
int skip
Definition: sick_ros_wrapper.h:521
MarkerArray.h

sick_scan_xd
Author(s): Michael Lehning , Jochen Sprickerhof , Martin Günther
autogenerated on Fri Oct 25 2024 02:47:10