ros_wrapper.h
Go to the documentation of this file.
1 /*
2  * @brief ros_wrapper encapsulates the ROS API and switches ROS specific implementation
3  * between ROS 1 and ROS 2.
4  *
5  * Copyright (C) 2020 Ing.-Buero Dr. Michael Lehning, Hildesheim
6  * Copyright (C) 2020 SICK AG, Waldkirch
7  *
8  * Licensed under the Apache License, Version 2.0 (the "License");
9  * you may not use this file except in compliance with the License.
10  * You may obtain a copy of the License at
11  *
12  * http://www.apache.org/licenses/LICENSE-2.0
13  *
14  * Unless required by applicable law or agreed to in writing, software
15  * distributed under the License is distributed on an "AS IS" BASIS,
16  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
17  * See the License for the specific language governing permissions and
18  * limitations under the License.
19  *
20  * All rights reserved.
21  *
22  * Redistribution and use in source and binary forms, with or without
23  * modification, are permitted provided that the following conditions are met:
24  *
25  * * Redistributions of source code must retain the above copyright
26  * notice, this list of conditions and the following disclaimer.
27  * * Redistributions in binary form must reproduce the above copyright
28  * notice, this list of conditions and the following disclaimer in the
29  * documentation and/or other materials provided with the distribution.
30  * * Neither the name of SICK AG nor the names of its
31  * contributors may be used to endorse or promote products derived from
32  * this software without specific prior written permission
33  * * Neither the name of Ing.-Buero Dr. Michael Lehning nor the names of its
34  * contributors may be used to endorse or promote products derived from
35  * this software without specific prior written permission
36  *
37  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
38  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
39  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
40  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
41  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
42  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
43  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
44  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
45  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
46  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
47  * POSSIBILITY OF SUCH DAMAGE.
48  *
49  * Authors:
50  * Michael Lehning <michael.lehning@lehning.de>
51  *
52  * Copyright 2020 SICK AG
53  * Copyright 2020 Ing.-Buero Dr. Michael Lehning
54  *
55  */
56 #ifndef __SIM_LOC_ROS_WRAPPER_H_INCLUDED
57 #define __SIM_LOC_ROS_WRAPPER_H_INCLUDED
58 
59 #include <cfloat>
60 #include <chrono>
61 #include <future>
62 #include <memory>
63 
64 #if defined __ROS_VERSION && __ROS_VERSION == 1
65 /*
66  * Support for ROS 1 API
67  */
68 #include <ros/ros.h>
69 #include <std_msgs/Header.h>
70 #include <geometry_msgs/Point.h>
71 #include <geometry_msgs/TransformStamped.h>
72 #include <nav_msgs/Odometry.h>
73 #include <sensor_msgs/PointCloud2.h>
74 #include <tf2/LinearMath/Quaternion.h>
75 #include <tf2_ros/transform_broadcaster.h>
76 
77 #define RCLCPP_DEBUG_STREAM(logger,msgstream) ROS_DEBUG_STREAM(msgstream)
78 #define RCLCPP_INFO_STREAM(logger,msgstream) ROS_INFO_STREAM(msgstream)
79 #define RCLCPP_WARN_STREAM(logger,msgstream) ROS_WARN_STREAM(msgstream)
80 #define RCLCPP_ERROR_STREAM(logger,msgstream) ROS_ERROR_STREAM(msgstream)
81 
82 // Message header
83 #include "sick_scan/SickLocColaTelegramMsg.h"
84 #include "sick_scan/SickLocDiagnosticMsg.h"
85 #include "sick_scan/SickLocResultPortCrcMsg.h"
86 #include "sick_scan/SickLocResultPortHeaderMsg.h"
87 #include "sick_scan/SickLocResultPortPayloadMsg.h"
88 #include "sick_scan/SickLocResultPortTelegramMsg.h"
89 #include "sick_scan/SickLocResultPortTestcaseMsg.h"
90 // Services supported in release 3 and later
91 #include "sick_scan/SickLocColaTelegramSrv.h"
92 #include "sick_scan/SickLocIsSystemReadySrv.h"
93 #include "sick_scan/SickLocRequestResultDataSrv.h"
94 #include "sick_scan/SickLocRequestTimestampSrv.h"
95 #include "sick_scan/SickLocSetPoseSrv.h"
96 #include "sick_scan/SickLocSetResultEndiannessSrv.h"
97 #include "sick_scan/SickLocSetResultModeSrv.h"
98 #include "sick_scan/SickLocSetResultPortSrv.h"
99 #include "sick_scan/SickLocSetResultPoseEnabledSrv.h"
100 #include "sick_scan/SickLocSetResultPoseIntervalSrv.h"
101 #include "sick_scan/SickLocStartLocalizingSrv.h"
102 #include "sick_scan/SickLocStateSrv.h"
103 #include "sick_scan/SickLocStopSrv.h"
104 #include "sick_scan/SickLocTimeSyncSrv.h"
105 // Services supported in release 4 and later
106 #include "sick_scan/SickDevGetLidarConfigSrv.h"
107 #include "sick_scan/SickDevGetLidarIdentSrv.h"
108 #include "sick_scan/SickDevGetLidarStateSrv.h"
109 #include "sick_scan/SickDevSetLidarConfigSrv.h"
110 #include "sick_scan/SickGetSoftwareVersionSrv.h"
111 #include "sick_scan/SickLocAutoStartActiveSrv.h"
112 #include "sick_scan/SickLocAutoStartSavePoseIntervalSrv.h"
113 #include "sick_scan/SickLocAutoStartSavePoseSrv.h"
114 #include "sick_scan/SickLocForceUpdateSrv.h"
115 #include "sick_scan/SickLocInitializePoseSrv.h"
116 #include "sick_scan/SickLocInitialPoseSrv.h"
117 #include "sick_scan/SickLocMapSrv.h"
118 #include "sick_scan/SickLocMapStateSrv.h"
119 #include "sick_scan/SickLocOdometryActiveSrv.h"
120 #include "sick_scan/SickLocOdometryPortSrv.h"
121 #include "sick_scan/SickLocOdometryRestrictYMotionSrv.h"
122 #include "sick_scan/SickLocReflectorsForSupportActiveSrv.h"
123 #include "sick_scan/SickLocResultEndiannessSrv.h"
124 #include "sick_scan/SickLocResultModeSrv.h"
125 #include "sick_scan/SickLocResultPortSrv.h"
126 #include "sick_scan/SickLocResultPoseIntervalSrv.h"
127 #include "sick_scan/SickLocResultStateSrv.h"
128 #include "sick_scan/SickLocRingBufferRecordingActiveSrv.h"
129 #include "sick_scan/SickLocSaveRingBufferRecordingSrv.h"
130 #include "sick_scan/SickLocSetAutoStartActiveSrv.h"
131 #include "sick_scan/SickLocSetAutoStartSavePoseIntervalSrv.h"
132 #include "sick_scan/SickLocSetMapSrv.h"
133 #include "sick_scan/SickLocSetOdometryActiveSrv.h"
134 #include "sick_scan/SickLocSetOdometryPortSrv.h"
135 #include "sick_scan/SickLocSetOdometryRestrictYMotionSrv.h"
136 #include "sick_scan/SickLocSetReflectorsForSupportActiveSrv.h"
137 #include "sick_scan/SickLocSetRingBufferRecordingActiveSrv.h"
138 #include "sick_scan/SickLocStartDemoMappingSrv.h"
139 #include "sick_scan/SickReportUserMessageSrv.h"
140 #include "sick_scan/SickSavePermanentSrv.h"
141 #include "sick_scan/SickDevSetIMUActiveSrv.h"
142 #include "sick_scan/SickDevIMUActiveSrv.h"
143 
144 namespace sick_scan
145 {
146  /*
147  typedef sensor_msgs::PointCloud2 PointCloud2Msg;
148  typedef ros::Publisher PointCloud2MsgPublisher;
149 
150  typedef nav_msgs::Odometry OdomMsg;
151  typedef ros::Subscriber OdomMsgSubscriber;
152  */
153  typedef ros::Publisher SickLocResultPortTelegramMsgPublisher;
154  typedef ros::Publisher SickLocDiagnosticMsgPublisher;
155  typedef ros::Publisher SickLocResultPortTestcaseMsgPublisher;
156 
157  typedef ros::Subscriber SickLocResultPortTelegramMsgSubscriber;
158  typedef ros::Subscriber SickLocResultPortTestcaseMsgSubscriber;
159 
160  // Service types supported in release 3 and later
161  typedef ros::ServiceClient SickLocColaTelegramSrvClient;
162  typedef ros::ServiceServer SickLocColaTelegramSrvServer;
163  typedef ros::ServiceClient SickLocIsSystemReadySrvClient;
164  typedef ros::ServiceServer SickLocIsSystemReadySrvServer;
165  typedef ros::ServiceClient SickLocRequestResultDataSrvClient;
166  typedef ros::ServiceServer SickLocRequestResultDataSrvServer;
167  typedef ros::ServiceClient SickLocRequestTimestampSrvClient;
168  typedef ros::ServiceServer SickLocRequestTimestampSrvServer;
169  typedef ros::ServiceClient SickLocSetPoseSrvClient;
170  typedef ros::ServiceServer SickLocSetPoseSrvServer;
171  typedef ros::ServiceClient SickLocSetResultEndiannessSrvClient;
172  typedef ros::ServiceServer SickLocSetResultEndiannessSrvServer;
173  typedef ros::ServiceClient SickLocSetResultModeSrvClient;
174  typedef ros::ServiceServer SickLocSetResultModeSrvServer;
175  typedef ros::ServiceClient SickLocSetResultPortSrvClient;
176  typedef ros::ServiceServer SickLocSetResultPortSrvServer;
177  typedef ros::ServiceClient SickLocSetResultPoseEnabledSrvClient;
178  typedef ros::ServiceServer SickLocSetResultPoseEnabledSrvServer;
179  typedef ros::ServiceClient SickLocSetResultPoseIntervalSrvClient;
180  typedef ros::ServiceServer SickLocSetResultPoseIntervalSrvServer;
181  typedef ros::ServiceClient SickLocStartLocalizingSrvClient;
182  typedef ros::ServiceServer SickLocStartLocalizingSrvServer;
183  typedef ros::ServiceClient SickLocStateSrvClient;
184  typedef ros::ServiceServer SickLocStateSrvServer;
185  typedef ros::ServiceClient SickLocStopSrvClient;
186  typedef ros::ServiceServer SickLocStopSrvServer;
187  typedef ros::ServiceClient SickLocTimeSyncSrvClient;
188  typedef ros::ServiceServer SickLocTimeSyncSrvServer;
189  // Service types supported in release 4 and later
190  typedef ros::ServiceClient SickDevGetLidarConfigSrvClient;
191  typedef ros::ServiceServer SickDevGetLidarConfigSrvServer;
192  typedef ros::ServiceClient SickDevGetLidarIdentSrvClient;
193  typedef ros::ServiceServer SickDevGetLidarIdentSrvServer;
194  typedef ros::ServiceClient SickDevGetLidarStateSrvClient;
195  typedef ros::ServiceServer SickDevGetLidarStateSrvServer;
196  typedef ros::ServiceClient SickDevSetLidarConfigSrvClient;
197  typedef ros::ServiceServer SickDevSetLidarConfigSrvServer;
198  typedef ros::ServiceClient SickGetSoftwareVersionSrvClient;
199  typedef ros::ServiceServer SickGetSoftwareVersionSrvServer;
200  typedef ros::ServiceClient SickLocAutoStartActiveSrvClient;
201  typedef ros::ServiceServer SickLocAutoStartActiveSrvServer;
202  typedef ros::ServiceClient SickLocAutoStartSavePoseIntervalSrvClient;
203  typedef ros::ServiceServer SickLocAutoStartSavePoseIntervalSrvServer;
204  typedef ros::ServiceClient SickLocAutoStartSavePoseSrvClient;
205  typedef ros::ServiceServer SickLocAutoStartSavePoseSrvServer;
206  typedef ros::ServiceClient SickLocForceUpdateSrvClient;
207  typedef ros::ServiceServer SickLocForceUpdateSrvServer;
208  typedef ros::ServiceClient SickLocInitializePoseSrvClient;
209  typedef ros::ServiceServer SickLocInitializePoseSrvServer;
210  typedef ros::ServiceClient SickLocInitialPoseSrvClient;
211  typedef ros::ServiceServer SickLocInitialPoseSrvServer;
212  typedef ros::ServiceClient SickLocMapSrvClient;
213  typedef ros::ServiceServer SickLocMapSrvServer;
214  typedef ros::ServiceClient SickLocMapStateSrvClient;
215  typedef ros::ServiceServer SickLocMapStateSrvServer;
216  typedef ros::ServiceClient SickLocOdometryActiveSrvClient;
217  typedef ros::ServiceServer SickLocOdometryActiveSrvServer;
218  typedef ros::ServiceClient SickLocOdometryPortSrvClient;
219  typedef ros::ServiceServer SickLocOdometryPortSrvServer;
220  typedef ros::ServiceClient SickLocOdometryRestrictYMotionSrvClient;
221  typedef ros::ServiceServer SickLocOdometryRestrictYMotionSrvServer;
222  typedef ros::ServiceClient SickLocReflectorsForSupportActiveSrvClient;
223  typedef ros::ServiceServer SickLocReflectorsForSupportActiveSrvServer;
224  typedef ros::ServiceClient SickLocResultEndiannessSrvClient;
225  typedef ros::ServiceServer SickLocResultEndiannessSrvServer;
226  typedef ros::ServiceClient SickLocResultModeSrvClient;
227  typedef ros::ServiceServer SickLocResultModeSrvServer;
228  typedef ros::ServiceClient SickLocResultPortSrvClient;
229  typedef ros::ServiceServer SickLocResultPortSrvServer;
230  typedef ros::ServiceClient SickLocResultPoseIntervalSrvClient;
231  typedef ros::ServiceServer SickLocResultPoseIntervalSrvServer;
232  typedef ros::ServiceClient SickLocResultStateSrvClient;
233  typedef ros::ServiceServer SickLocResultStateSrvServer;
234  typedef ros::ServiceClient SickLocRingBufferRecordingActiveSrvClient;
235  typedef ros::ServiceServer SickLocRingBufferRecordingActiveSrvServer;
236  typedef ros::ServiceClient SickLocSaveRingBufferRecordingSrvClient;
237  typedef ros::ServiceServer SickLocSaveRingBufferRecordingSrvServer;
238  typedef ros::ServiceClient SickLocSetAutoStartActiveSrvClient;
239  typedef ros::ServiceServer SickLocSetAutoStartActiveSrvServer;
240  typedef ros::ServiceClient SickLocSetAutoStartSavePoseIntervalSrvClient;
241  typedef ros::ServiceServer SickLocSetAutoStartSavePoseIntervalSrvServer;
242  typedef ros::ServiceClient SickLocSetMapSrvClient;
243  typedef ros::ServiceServer SickLocSetMapSrvServer;
244  typedef ros::ServiceClient SickLocSetOdometryActiveSrvClient;
245  typedef ros::ServiceServer SickLocSetOdometryActiveSrvServer;
246  typedef ros::ServiceClient SickLocSetOdometryPortSrvClient;
247  typedef ros::ServiceServer SickLocSetOdometryPortSrvServer;
248  typedef ros::ServiceClient SickLocSetOdometryRestrictYMotionSrvClient;
249  typedef ros::ServiceServer SickLocSetOdometryRestrictYMotionSrvServer;
250  typedef ros::ServiceClient SickLocSetReflectorsForSupportActiveSrvClient;
251  typedef ros::ServiceServer SickLocSetReflectorsForSupportActiveSrvServer;
252  typedef ros::ServiceClient SickLocSetRingBufferRecordingActiveSrvClient;
253  typedef ros::ServiceServer SickLocSetRingBufferRecordingActiveSrvServer;
254  typedef ros::ServiceClient SickLocStartDemoMappingSrvClient;
255  typedef ros::ServiceServer SickLocStartDemoMappingSrvServer;
256  typedef ros::ServiceClient SickReportUserMessageSrvClient;
257  typedef ros::ServiceServer SickReportUserMessageSrvServer;
258  typedef ros::ServiceClient SickSavePermanentSrvClient;
259  typedef ros::ServiceServer SickSavePermanentSrvServer;
260  typedef ros::ServiceClient SickDevSetIMUActiveSrvClient;
261  typedef ros::ServiceServer SickDevSetIMUActiveSrvServer;
262  typedef ros::ServiceClient SickDevIMUActiveSrvClient;
263  typedef ros::ServiceServer SickDevIMUActiveSrvServer;
264 
265 } // namespace sick_scan
266 
267 namespace ROS
268 {
269  using namespace ros; // map namespace ROS to ros in ROS1 and to rclcpp in ROS2, f.e. ROS::ok() to ROS::ok() in ROS1 and rclcpp::ok() in ROS2
270  typedef ros::NodeHandle* NodePtr;
271 
272  template<typename T> bool param(ROS::NodePtr & node, const std::string & param_name, T& value, const T& default_value)
273  {
274  return ros::param::param<T>(param_name, value, default_value);
275  }
276 
278  void spin(ROS:: NodePtr nh = 0);
279 
280 } // namespace ROS
281 
282 #define ROS_CREATE_SRV_CLIENT(nh,srv,name) nh->serviceClient<srv>(name)
283 #define ROS_CREATE_SRV_SERVER(nh,srv,name,cbfunction,cbobject) nh->advertiseService(name,cbfunction,cbobject)
284 
285 #define ROS_CREATE_PUBLISHER(nh,msgtype,topic) nh->advertise<msgtype>(topic,1);
286 #define ROS_PUBLISH(publisher,message) publisher.publish(message);
287 
288 #define ROS_CREATE_SUBSCRIBER(nh,msgtype,topic,cbfunction,cbobject) nh->subscribe(topic,1,cbfunction,cbobject)
289 
290 #define NSEC nsec // maps nanoseconds in std_msgs::Header::stamp to stamp.nsec in ROS1 resp. stamp.nanosec in ROS2
291 
292 #elif defined __ROS_VERSION && __ROS_VERSION == 2
293 /*
294  * Support for ROS 2 API
295  */
296 #include <rclcpp/rclcpp.hpp>
297 #include <rclcpp/executor.hpp>
298 #include <std_msgs/msg/header.hpp>
299 #include <geometry_msgs/msg/point.hpp>
300 #include <geometry_msgs/msg/transform_stamped.hpp>
301 #include <nav_msgs/msg/odometry.hpp>
302 #include <sensor_msgs/msg/point_cloud2.hpp>
303 #include <tf2/LinearMath/Quaternion.h>
304 #include <tf2_ros/transform_broadcaster.h>
305 
306 #define ROS_DEBUG_STREAM(msgstream) RCLCPP_DEBUG_STREAM(rclcpp::get_logger("sick_scan"),msgstream)
307 #define ROS_INFO_STREAM(msgstream) RCLCPP_INFO_STREAM(rclcpp::get_logger("sick_scan"),msgstream)
308 #define ROS_WARN_STREAM(msgstream) RCLCPP_WARN_STREAM(rclcpp::get_logger("sick_scan"),msgstream)
309 #define ROS_ERROR_STREAM(msgstream) RCLCPP_ERROR_STREAM(rclcpp::get_logger("sick_scan"),msgstream)
310 
311 // Message header
312 #include "sick_scan/msg/sick_loc_cola_telegram_msg.hpp"
313 #include "sick_scan/msg/sick_loc_diagnostic_msg.hpp"
314 #include "sick_scan/msg/sick_loc_result_port_crc_msg.hpp"
315 #include "sick_scan/msg/sick_loc_result_port_header_msg.hpp"
316 #include "sick_scan/msg/sick_loc_result_port_payload_msg.hpp"
317 #include "sick_scan/msg/sick_loc_result_port_telegram_msg.hpp"
318 #include "sick_scan/msg/sick_loc_result_port_testcase_msg.hpp"
319 // Services supported in release 3 and later
320 #include "sick_scan/srv/sick_loc_cola_telegram_srv.hpp"
321 #include "sick_scan/srv/sick_loc_is_system_ready_srv.hpp"
322 #include "sick_scan/srv/sick_loc_request_result_data_srv.hpp"
323 #include "sick_scan/srv/sick_loc_request_timestamp_srv.hpp"
324 #include "sick_scan/srv/sick_loc_set_pose_srv.hpp"
325 #include "sick_scan/srv/sick_loc_set_result_endianness_srv.hpp"
326 #include "sick_scan/srv/sick_loc_set_result_mode_srv.hpp"
327 #include "sick_scan/srv/sick_loc_set_result_port_srv.hpp"
328 #include "sick_scan/srv/sick_loc_set_result_pose_enabled_srv.hpp"
329 #include "sick_scan/srv/sick_loc_set_result_pose_interval_srv.hpp"
330 #include "sick_scan/srv/sick_loc_start_localizing_srv.hpp"
331 #include "sick_scan/srv/sick_loc_state_srv.hpp"
332 #include "sick_scan/srv/sick_loc_stop_srv.hpp"
333 #include "sick_scan/srv/sick_loc_time_sync_srv.hpp"
334 // Services supported in release 4 and later
335 #include "sick_scan/srv/sick_dev_get_lidar_config_srv.hpp"
336 #include "sick_scan/srv/sick_dev_get_lidar_ident_srv.hpp"
337 #include "sick_scan/srv/sick_dev_get_lidar_state_srv.hpp"
338 #include "sick_scan/srv/sick_dev_set_lidar_config_srv.hpp"
339 #include "sick_scan/srv/sick_get_software_version_srv.hpp"
340 #include "sick_scan/srv/sick_loc_auto_start_active_srv.hpp"
341 #include "sick_scan/srv/sick_loc_auto_start_save_pose_interval_srv.hpp"
342 #include "sick_scan/srv/sick_loc_auto_start_save_pose_srv.hpp"
343 #include "sick_scan/srv/sick_loc_force_update_srv.hpp"
344 #include "sick_scan/srv/sick_loc_initialize_pose_srv.hpp"
345 #include "sick_scan/srv/sick_loc_initial_pose_srv.hpp"
346 #include "sick_scan/srv/sick_loc_map_srv.hpp"
347 #include "sick_scan/srv/sick_loc_map_state_srv.hpp"
348 #include "sick_scan/srv/sick_loc_odometry_active_srv.hpp"
349 #include "sick_scan/srv/sick_loc_odometry_port_srv.hpp"
350 #include "sick_scan/srv/sick_loc_odometry_restrict_y_motion_srv.hpp"
351 #include "sick_scan/srv/sick_loc_reflectors_for_support_active_srv.hpp"
352 #include "sick_scan/srv/sick_loc_result_endianness_srv.hpp"
353 #include "sick_scan/srv/sick_loc_result_mode_srv.hpp"
354 #include "sick_scan/srv/sick_loc_result_port_srv.hpp"
355 #include "sick_scan/srv/sick_loc_result_pose_interval_srv.hpp"
356 #include "sick_scan/srv/sick_loc_result_state_srv.hpp"
357 #include "sick_scan/srv/sick_loc_ring_buffer_recording_active_srv.hpp"
358 #include "sick_scan/srv/sick_loc_save_ring_buffer_recording_srv.hpp"
359 #include "sick_scan/srv/sick_loc_set_auto_start_active_srv.hpp"
360 #include "sick_scan/srv/sick_loc_set_auto_start_save_pose_interval_srv.hpp"
361 #include "sick_scan/srv/sick_loc_set_map_srv.hpp"
362 #include "sick_scan/srv/sick_loc_set_odometry_active_srv.hpp"
363 #include "sick_scan/srv/sick_loc_set_odometry_port_srv.hpp"
364 #include "sick_scan/srv/sick_loc_set_odometry_restrict_y_motion_srv.hpp"
365 #include "sick_scan/srv/sick_loc_set_reflectors_for_support_active_srv.hpp"
366 #include "sick_scan/srv/sick_loc_set_ring_buffer_recording_active_srv.hpp"
367 #include "sick_scan/srv/sick_loc_start_demo_mapping_srv.hpp"
368 #include "sick_scan/srv/sick_report_user_message_srv.hpp"
369 #include "sick_scan/srv/sick_save_permanent_srv.hpp"
370 #include "sick_scan/srv/sick_dev_set_imu_active_srv.hpp"
371 #include "sick_scan/srv/sick_dev_imu_active_srv.hpp"
372 
373 namespace geometry_msgs
374 {
375  using namespace msg; // maps ROS2-namespace geometry_msgs::msg to ROS1-namespace geometry_msgs, f.e. geometry_msgs::Point works on both ROS1 and ROS2
376 }
377 
378 namespace sensor_msgs
379 {
380  using namespace msg; // maps ROS2-namespace sensor_msgs::msg to ROS1-namespace sensor_msgs, f.e. sensor_msgs::PointCloud2 works on both ROS1 and ROS2
381 }
382 
383 namespace nav_msgs
384 {
385  using namespace msg; // maps ROS2-namespace nav_msgs::msg to ROS1-namespace nav_msgs, f.e. nav_msgs::Odometry works on both ROS1 and ROS2
386 }
387 
388 namespace std_msgs
389 {
390  using namespace msg; // maps ROS2-namespace std_msgs::msg to ROS1-namespace std_msgs, f.e. std_msgs::Header works on both ROS1 and ROS2
391 }
392 
393 namespace sick_scan
394 {
395  using namespace msg;
396  using namespace srv;
397 
398  typedef sensor_msgs::msg::PointCloud2 PointCloud2Msg;
399  typedef rclcpp::Publisher<PointCloud2Msg>::SharedPtr PointCloud2MsgPublisher;
400 
401  typedef nav_msgs::msg::Odometry OdomMsg;
402  typedef rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr OdomMsgSubscriber;
403 
404  typedef rclcpp::Publisher<sick_scan::SickLocResultPortTelegramMsg>::SharedPtr SickLocResultPortTelegramMsgPublisher;
405  typedef rclcpp::Publisher<sick_scan::SickLocDiagnosticMsg>::SharedPtr SickLocDiagnosticMsgPublisher;
406  typedef rclcpp::Publisher<sick_scan::SickLocResultPortTestcaseMsg>::SharedPtr SickLocResultPortTestcaseMsgPublisher;
407 
408  typedef rclcpp::Subscription<sick_scan::SickLocResultPortTelegramMsg>::SharedPtr SickLocResultPortTelegramMsgSubscriber;
409  typedef rclcpp::Subscription<sick_scan::SickLocResultPortTestcaseMsg>::SharedPtr SickLocResultPortTestcaseMsgSubscriber;
410 
411  // Service types supported in release 3 and later
412  typedef rclcpp::Client<sick_scan::SickLocColaTelegramSrv>::SharedPtr SickLocColaTelegramSrvClient;
413  typedef rclcpp::Service<sick_scan::SickLocColaTelegramSrv>::SharedPtr SickLocColaTelegramSrvServer;
414  typedef rclcpp::Client<sick_scan::SickLocIsSystemReadySrv>::SharedPtr SickLocIsSystemReadySrvClient;
415  typedef rclcpp::Service<sick_scan::SickLocIsSystemReadySrv>::SharedPtr SickLocIsSystemReadySrvServer;
416  typedef rclcpp::Client<sick_scan::SickLocRequestResultDataSrv>::SharedPtr SickLocRequestResultDataSrvClient;
417  typedef rclcpp::Service<sick_scan::SickLocRequestResultDataSrv>::SharedPtr SickLocRequestResultDataSrvServer;
418  typedef rclcpp::Client<sick_scan::SickLocRequestTimestampSrv>::SharedPtr SickLocRequestTimestampSrvClient;
419  typedef rclcpp::Service<sick_scan::SickLocRequestTimestampSrv>::SharedPtr SickLocRequestTimestampSrvServer;
420  typedef rclcpp::Client<sick_scan::SickLocSetPoseSrv>::SharedPtr SickLocSetPoseSrvClient;
421  typedef rclcpp::Service<sick_scan::SickLocSetPoseSrv>::SharedPtr SickLocSetPoseSrvServer;
422  typedef rclcpp::Client<sick_scan::SickLocSetResultEndiannessSrv>::SharedPtr SickLocSetResultEndiannessSrvClient;
423  typedef rclcpp::Service<sick_scan::SickLocSetResultEndiannessSrv>::SharedPtr SickLocSetResultEndiannessSrvServer;
424  typedef rclcpp::Client<sick_scan::SickLocSetResultModeSrv>::SharedPtr SickLocSetResultModeSrvClient;
425  typedef rclcpp::Service<sick_scan::SickLocSetResultModeSrv>::SharedPtr SickLocSetResultModeSrvServer;
426  typedef rclcpp::Client<sick_scan::SickLocSetResultPortSrv>::SharedPtr SickLocSetResultPortSrvClient;
427  typedef rclcpp::Service<sick_scan::SickLocSetResultPortSrv>::SharedPtr SickLocSetResultPortSrvServer;
428  typedef rclcpp::Client<sick_scan::SickLocSetResultPoseEnabledSrv>::SharedPtr SickLocSetResultPoseEnabledSrvClient;
429  typedef rclcpp::Service<sick_scan::SickLocSetResultPoseEnabledSrv>::SharedPtr SickLocSetResultPoseEnabledSrvServer;
430  typedef rclcpp::Client<sick_scan::SickLocSetResultPoseIntervalSrv>::SharedPtr SickLocSetResultPoseIntervalSrvClient;
431  typedef rclcpp::Service<sick_scan::SickLocSetResultPoseIntervalSrv>::SharedPtr SickLocSetResultPoseIntervalSrvServer;
432  typedef rclcpp::Client<sick_scan::SickLocStartLocalizingSrv>::SharedPtr SickLocStartLocalizingSrvClient;
433  typedef rclcpp::Service<sick_scan::SickLocStartLocalizingSrv>::SharedPtr SickLocStartLocalizingSrvServer;
434  typedef rclcpp::Client<sick_scan::SickLocStateSrv>::SharedPtr SickLocStateSrvClient;
435  typedef rclcpp::Service<sick_scan::SickLocStateSrv>::SharedPtr SickLocStateSrvServer;
436  typedef rclcpp::Client<sick_scan::SickLocStopSrv>::SharedPtr SickLocStopSrvClient;
437  typedef rclcpp::Service<sick_scan::SickLocStopSrv>::SharedPtr SickLocStopSrvServer;
438  typedef rclcpp::Client<sick_scan::SickLocTimeSyncSrv>::SharedPtr SickLocTimeSyncSrvClient;
439  typedef rclcpp::Service<sick_scan::SickLocTimeSyncSrv>::SharedPtr SickLocTimeSyncSrvServer;
440  // Service types supported in release 4 and later
441  typedef rclcpp::Client <sick_scan::SickDevGetLidarConfigSrv>::SharedPtr SickDevGetLidarConfigSrvClient;
442  typedef rclcpp::Service<sick_scan::SickDevGetLidarConfigSrv>::SharedPtr SickDevGetLidarConfigSrvServer;
443  typedef rclcpp::Client <sick_scan::SickDevGetLidarIdentSrv>::SharedPtr SickDevGetLidarIdentSrvClient;
444  typedef rclcpp::Service<sick_scan::SickDevGetLidarIdentSrv>::SharedPtr SickDevGetLidarIdentSrvServer;
445  typedef rclcpp::Client <sick_scan::SickDevGetLidarStateSrv>::SharedPtr SickDevGetLidarStateSrvClient;
446  typedef rclcpp::Service<sick_scan::SickDevGetLidarStateSrv>::SharedPtr SickDevGetLidarStateSrvServer;
447  typedef rclcpp::Client <sick_scan::SickDevSetLidarConfigSrv>::SharedPtr SickDevSetLidarConfigSrvClient;
448  typedef rclcpp::Service<sick_scan::SickDevSetLidarConfigSrv>::SharedPtr SickDevSetLidarConfigSrvServer;
449  typedef rclcpp::Client <sick_scan::SickGetSoftwareVersionSrv>::SharedPtr SickGetSoftwareVersionSrvClient;
450  typedef rclcpp::Service<sick_scan::SickGetSoftwareVersionSrv>::SharedPtr SickGetSoftwareVersionSrvServer;
451  typedef rclcpp::Client <sick_scan::SickLocAutoStartActiveSrv>::SharedPtr SickLocAutoStartActiveSrvClient;
452  typedef rclcpp::Service<sick_scan::SickLocAutoStartActiveSrv>::SharedPtr SickLocAutoStartActiveSrvServer;
453  typedef rclcpp::Client <sick_scan::SickLocAutoStartSavePoseIntervalSrv>::SharedPtr SickLocAutoStartSavePoseIntervalSrvClient;
454  typedef rclcpp::Service<sick_scan::SickLocAutoStartSavePoseIntervalSrv>::SharedPtr SickLocAutoStartSavePoseIntervalSrvServer;
455  typedef rclcpp::Client <sick_scan::SickLocAutoStartSavePoseSrv>::SharedPtr SickLocAutoStartSavePoseSrvClient;
456  typedef rclcpp::Service<sick_scan::SickLocAutoStartSavePoseSrv>::SharedPtr SickLocAutoStartSavePoseSrvServer;
457  typedef rclcpp::Client <sick_scan::SickLocForceUpdateSrv>::SharedPtr SickLocForceUpdateSrvClient;
458  typedef rclcpp::Service<sick_scan::SickLocForceUpdateSrv>::SharedPtr SickLocForceUpdateSrvServer;
459  typedef rclcpp::Client <sick_scan::SickLocInitializePoseSrv>::SharedPtr SickLocInitializePoseSrvClient;
460  typedef rclcpp::Service<sick_scan::SickLocInitializePoseSrv>::SharedPtr SickLocInitializePoseSrvServer;
461  typedef rclcpp::Client <sick_scan::SickLocInitialPoseSrv>::SharedPtr SickLocInitialPoseSrvClient;
462  typedef rclcpp::Service<sick_scan::SickLocInitialPoseSrv>::SharedPtr SickLocInitialPoseSrvServer;
463  typedef rclcpp::Client <sick_scan::SickLocMapSrv>::SharedPtr SickLocMapSrvClient;
464  typedef rclcpp::Service<sick_scan::SickLocMapSrv>::SharedPtr SickLocMapSrvServer;
465  typedef rclcpp::Client <sick_scan::SickLocMapStateSrv>::SharedPtr SickLocMapStateSrvClient;
466  typedef rclcpp::Service<sick_scan::SickLocMapStateSrv>::SharedPtr SickLocMapStateSrvServer;
467  typedef rclcpp::Client <sick_scan::SickLocOdometryActiveSrv>::SharedPtr SickLocOdometryActiveSrvClient;
468  typedef rclcpp::Service<sick_scan::SickLocOdometryActiveSrv>::SharedPtr SickLocOdometryActiveSrvServer;
469  typedef rclcpp::Client <sick_scan::SickLocOdometryPortSrv>::SharedPtr SickLocOdometryPortSrvClient;
470  typedef rclcpp::Service<sick_scan::SickLocOdometryPortSrv>::SharedPtr SickLocOdometryPortSrvServer;
471  typedef rclcpp::Client <sick_scan::SickLocOdometryRestrictYMotionSrv>::SharedPtr SickLocOdometryRestrictYMotionSrvClient;
472  typedef rclcpp::Service<sick_scan::SickLocOdometryRestrictYMotionSrv>::SharedPtr SickLocOdometryRestrictYMotionSrvServer;
473  typedef rclcpp::Client <sick_scan::SickLocReflectorsForSupportActiveSrv>::SharedPtr SickLocReflectorsForSupportActiveSrvClient;
474  typedef rclcpp::Service<sick_scan::SickLocReflectorsForSupportActiveSrv>::SharedPtr SickLocReflectorsForSupportActiveSrvServer;
475  typedef rclcpp::Client <sick_scan::SickLocResultEndiannessSrv>::SharedPtr SickLocResultEndiannessSrvClient;
476  typedef rclcpp::Service<sick_scan::SickLocResultEndiannessSrv>::SharedPtr SickLocResultEndiannessSrvServer;
477  typedef rclcpp::Client <sick_scan::SickLocResultModeSrv>::SharedPtr SickLocResultModeSrvClient;
478  typedef rclcpp::Service<sick_scan::SickLocResultModeSrv>::SharedPtr SickLocResultModeSrvServer;
479  typedef rclcpp::Client <sick_scan::SickLocResultPortSrv>::SharedPtr SickLocResultPortSrvClient;
480  typedef rclcpp::Service<sick_scan::SickLocResultPortSrv>::SharedPtr SickLocResultPortSrvServer;
481  typedef rclcpp::Client <sick_scan::SickLocResultPoseIntervalSrv>::SharedPtr SickLocResultPoseIntervalSrvClient;
482  typedef rclcpp::Service<sick_scan::SickLocResultPoseIntervalSrv>::SharedPtr SickLocResultPoseIntervalSrvServer;
483  typedef rclcpp::Client <sick_scan::SickLocResultStateSrv>::SharedPtr SickLocResultStateSrvClient;
484  typedef rclcpp::Service<sick_scan::SickLocResultStateSrv>::SharedPtr SickLocResultStateSrvServer;
485  typedef rclcpp::Client <sick_scan::SickLocRingBufferRecordingActiveSrv>::SharedPtr SickLocRingBufferRecordingActiveSrvClient;
486  typedef rclcpp::Service<sick_scan::SickLocRingBufferRecordingActiveSrv>::SharedPtr SickLocRingBufferRecordingActiveSrvServer;
487  typedef rclcpp::Client <sick_scan::SickLocSaveRingBufferRecordingSrv>::SharedPtr SickLocSaveRingBufferRecordingSrvClient;
488  typedef rclcpp::Service<sick_scan::SickLocSaveRingBufferRecordingSrv>::SharedPtr SickLocSaveRingBufferRecordingSrvServer;
489  typedef rclcpp::Client <sick_scan::SickLocSetAutoStartActiveSrv>::SharedPtr SickLocSetAutoStartActiveSrvClient;
490  typedef rclcpp::Service<sick_scan::SickLocSetAutoStartActiveSrv>::SharedPtr SickLocSetAutoStartActiveSrvServer;
491  typedef rclcpp::Client <sick_scan::SickLocSetAutoStartSavePoseIntervalSrv>::SharedPtr SickLocSetAutoStartSavePoseIntervalSrvClient;
492  typedef rclcpp::Service<sick_scan::SickLocSetAutoStartSavePoseIntervalSrv>::SharedPtr SickLocSetAutoStartSavePoseIntervalSrvServer;
493  typedef rclcpp::Client <sick_scan::SickLocSetMapSrv>::SharedPtr SickLocSetMapSrvClient;
494  typedef rclcpp::Service<sick_scan::SickLocSetMapSrv>::SharedPtr SickLocSetMapSrvServer;
495  typedef rclcpp::Client <sick_scan::SickLocSetOdometryActiveSrv>::SharedPtr SickLocSetOdometryActiveSrvClient;
496  typedef rclcpp::Service<sick_scan::SickLocSetOdometryActiveSrv>::SharedPtr SickLocSetOdometryActiveSrvServer;
497  typedef rclcpp::Client <sick_scan::SickLocSetOdometryPortSrv>::SharedPtr SickLocSetOdometryPortSrvClient;
498  typedef rclcpp::Service<sick_scan::SickLocSetOdometryPortSrv>::SharedPtr SickLocSetOdometryPortSrvServer;
499  typedef rclcpp::Client <sick_scan::SickLocSetOdometryRestrictYMotionSrv>::SharedPtr SickLocSetOdometryRestrictYMotionSrvClient;
500  typedef rclcpp::Service<sick_scan::SickLocSetOdometryRestrictYMotionSrv>::SharedPtr SickLocSetOdometryRestrictYMotionSrvServer;
501  typedef rclcpp::Client <sick_scan::SickLocSetReflectorsForSupportActiveSrv>::SharedPtr SickLocSetReflectorsForSupportActiveSrvClient;
502  typedef rclcpp::Service<sick_scan::SickLocSetReflectorsForSupportActiveSrv>::SharedPtr SickLocSetReflectorsForSupportActiveSrvServer;
503  typedef rclcpp::Client <sick_scan::SickLocSetRingBufferRecordingActiveSrv>::SharedPtr SickLocSetRingBufferRecordingActiveSrvClient;
504  typedef rclcpp::Service<sick_scan::SickLocSetRingBufferRecordingActiveSrv>::SharedPtr SickLocSetRingBufferRecordingActiveSrvServer;
505  typedef rclcpp::Client <sick_scan::SickLocStartDemoMappingSrv>::SharedPtr SickLocStartDemoMappingSrvClient;
506  typedef rclcpp::Service<sick_scan::SickLocStartDemoMappingSrv>::SharedPtr SickLocStartDemoMappingSrvServer;
507  typedef rclcpp::Client <sick_scan::SickReportUserMessageSrv>::SharedPtr SickReportUserMessageSrvClient;
508  typedef rclcpp::Service<sick_scan::SickReportUserMessageSrv>::SharedPtr SickReportUserMessageSrvServer;
509  typedef rclcpp::Client <sick_scan::SickSavePermanentSrv>::SharedPtr SickSavePermanentSrvClient;
510  typedef rclcpp::Service<sick_scan::SickSavePermanentSrv>::SharedPtr SickSavePermanentSrvServer;
511  typedef rclcpp::Client <sick_scan::SickDevSetIMUActiveSrv>::SharedPtr SickDevSetIMUActiveSrvClient;
512  typedef rclcpp::Service<sick_scan::SickDevSetIMUActiveSrv>::SharedPtr SickDevSetIMUActiveSrvServer;
513  typedef rclcpp::Client <sick_scan::SickDevIMUActiveSrv>::SharedPtr SickDevIMUActiveSrvClient;
514  typedef rclcpp::Service<sick_scan::SickDevIMUActiveSrv>::SharedPtr SickDevIMUActiveSrvServer;
515 
516 } // namespace sick_scan
517 
518 namespace ROS
519 {
520  using namespace rclcpp; // map namespace ROS to ros in ROS1 and to rclcpp in ROS2, f.e. ROS::ok() to ros::ok() in ROS1 and rclcpp::ok() in ROS2
521  typedef rclcpp::Node::SharedPtr NodePtr;
522 
523  template<typename T> bool param(ROS::NodePtr & node, const std::string & param_name, T& value, const T& default_value)
524  {
525  ROS_INFO_STREAM("ROS::param(" << node->get_name() << "," << param_name << "," << default_value << ")");
526  try
527  {
528  if(!node->has_parameter(param_name))
529  {
530  node->declare_parameter<T>(param_name, default_value);
531  }
532  }
533  catch(const std::exception& exc)
534  {
535  ROS_WARN_STREAM("## ERROR ROS::param: declare_parameter(" << param_name << "," << default_value << ") failed, exception " << exc.what());
536  }
537  try
538  {
539  return node->get_parameter(param_name, value);
540  }
541  catch(const std::exception& exc)
542  {
543  ROS_WARN_STREAM("## ERROR ROS::param: get_parameter(" << param_name << "," << default_value << ") failed, exception " << exc.what());
544  }
545  return false;
546  }
547 
549  void init(int argc, char** argv, const std::string nodename = "");
550 
551 } // namespace ROS
552 
553 #define ROS_CREATE_SRV_CLIENT(nh,srv,name) nh->create_client<srv>(name)
554 #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))
555 
556 #define ROS_CREATE_PUBLISHER(nh,msgtype,topic) nh->create_publisher<msgtype>(topic,rclcpp::SystemDefaultsQoS());
557 #define ROS_PUBLISH(publisher,message) publisher->publish(message);
558 
559 #define ROS_CREATE_SUBSCRIBER(nh,msgtype,topic,cbfunction,cbobject) nh->create_subscription<msgtype>(topic,10,std::bind(cbfunction,cbobject,std::placeholders::_1))
560 
561 #define NSEC nanosec // maps nanoseconds in std_msgs::Header::stamp to stamp.nsec in ROS1 resp. stamp.nanosec in ROS2
562 
563 #else
564 #error __ROS_VERSION not defined, build with "--cmake-args -DROS_VERSION=1" or "--cmake-args -DROS_VERSION=2"
565 #endif
566 
567 namespace ROS
568 {
570  ROS::NodePtr createNode(const std::string& node_name = "sick_scan");
571 
573  void deleteNode(ROS::NodePtr & node);
574 
576  void sleep(double seconds);
577 
579  ROS::Time now(void);
580 
582  void splitTime(ROS::Duration time, uint32_t& seconds, uint32_t& nanoseconds);
583 
585  void splitTime(ROS::Time time, uint32_t& seconds, uint32_t& nanoseconds);
586 
588  ROS::Time timeFromHeader(const std_msgs::Header* msg_header);
589 
594  ROS::Time timeFromSec(double seconds);
595 
600  ROS::Duration durationFromSec(double seconds);
601 
603  double seconds(ROS::Duration duration);
604 
606  double secondsSinceStart(const ROS::Time& time);
607 
609  uint64_t timestampMilliseconds(const ROS::Time& time);
610 
611 } // namespace ROS
612 
613 #endif // __SIM_LOC_ROS_WRAPPER_H_INCLUDED
msg
msg
param
bool param(const std::string &param_name, T &param_val, const T &default_val)
ros::NodeHandle
ROS::now
ROS::Time now(void)
Definition: ros_wrapper.cpp:116
ros::init
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
transform_broadcaster.h
ros::spin
ROSCPP_DECL void spin(Spinner &spinner)
ROS::secondsSinceStart
double secondsSinceStart(const ROS::Time &time)
Definition: ros_wrapper.cpp:192
ROS::deleteNode
void deleteNode(ROS::NodePtr &node)
Definition: ros_wrapper.cpp:98
sensor_msgs
std_msgs
ROS::durationFromSec
ROS::Duration durationFromSec(double seconds)
Definition: ros_wrapper.cpp:172
ROS_WARN_STREAM
#define ROS_WARN_STREAM(args)
geometry_msgs
ROS::timeFromHeader
ROS::Time timeFromHeader(const std_msgs::Header *msg_header)
Definition: ros_wrapper.cpp:152
ROS::splitTime
void splitTime(ROS::Time time, uint32_t &seconds, uint32_t &nanoseconds)
Definition: ros_wrapper.cpp:140
ROS::timestampMilliseconds
uint64_t timestampMilliseconds(const ROS::Time &time)
Definition: ros_wrapper.cpp:199
ROS_INFO_STREAM
#define ROS_INFO_STREAM(args)
sick_scan
Definition: abstract_parser.cpp:50
ROS::createNode
ROS::NodePtr createNode(const std::string &node_name="sick_scan")
Definition: ros_wrapper.cpp:65
ROS::timeFromSec
ROS::Time timeFromSec(double seconds)
Definition: ros_wrapper.cpp:161
ROS::sleep
void sleep(double seconds)
Definition: ros_wrapper.cpp:110
ROS::seconds
double seconds(ROS::Duration duration)
Definition: ros_wrapper.cpp:180

sick_scan
Author(s): Michael Lehning , Jochen Sprickerhof , Martin Günther
autogenerated on Wed May 5 2021 03:05:48