ros_wrapper.cpp
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 #include <thread>
57 
58 #include "sick_scan/ros_wrapper.h"
59 
60 #if defined __ROS_VERSION && __ROS_VERSION == 2
61 static rclcpp::Clock s_wrapper_clock;
62 #endif
63 
65 ROS::NodePtr ROS::createNode(const std::string& node_name)
66 {
67  ROS::NodePtr nh = 0;
68 #if defined __ROS_VERSION && __ROS_VERSION == 1
69  nh = new ros::NodeHandle();
70 #elif defined __ROS_VERSION && __ROS_VERSION == 2
71  rclcpp::NodeOptions node_options;
72  node_options.allow_undeclared_parameters(true);
73  nh = rclcpp::Node::make_shared(node_name, "", node_options);
74 #endif
75  double timestamp_sec = ROS::secondsSinceStart(ROS::now());
76  if ((timestamp_sec = ROS::secondsSinceStart(ROS::now())) < 0) // insure start timestamp after initialization of the first node
77  ROS_WARN_STREAM("## ERROR ROS::createNode(): time confusion, ROS::secondsSinceStart(ROS::now()) returned negative seconds " << timestamp_sec);
78  return nh;
79 }
80 
81 #if defined __ROS_VERSION && __ROS_VERSION == 2
82 
83 void ROS::init(int argc, char** argv, const std::string nodename)
84 {
85  rclcpp::init(argc, argv);
86 }
87 #endif
88 
90 #if defined __ROS_VERSION && __ROS_VERSION == 1
91 void ROS::spin(ROS::NodePtr nh)
92 {
93  ros::spin();
94 }
95 #endif
96 
98 void ROS::deleteNode(ROS::NodePtr & node)
99 {
100 #if defined __ROS_VERSION && __ROS_VERSION == 1
101  if(node)
102  {
103  delete(node);
104  node = 0;
105  }
106 #endif
107 }
108 
110 void ROS::sleep(double seconds)
111 {
112  std::this_thread::sleep_for(std::chrono::microseconds((int64_t)(1.0e6*seconds)));
113 }
114 
116 ROS::Time ROS::now(void)
117 {
118 #if defined __ROS_VERSION && __ROS_VERSION == 1
119  return ros::Time::now();
120 #elif defined __ROS_VERSION && __ROS_VERSION == 2
121  return s_wrapper_clock.now();
122 #else
123  return 0;
124 #endif
125 }
126 
128 void ROS::splitTime(ROS::Duration time, uint32_t& seconds, uint32_t& nanoseconds)
129 {
130 #if defined __ROS_VERSION && __ROS_VERSION == 1
131  seconds = time.sec;
132  nanoseconds = time.nsec;
133 #elif defined __ROS_VERSION && __ROS_VERSION == 2
134  seconds = (uint32_t)(time.nanoseconds() / 1000000000);
135  nanoseconds = (uint32_t)(time.nanoseconds() - 1000000000 * seconds);
136 #endif
137 }
138 
140 void ROS::splitTime(ROS::Time time, uint32_t& seconds, uint32_t& nanoseconds)
141 {
142 #if defined __ROS_VERSION && __ROS_VERSION == 1
143  seconds = time.sec;
144  nanoseconds = time.nsec;
145 #elif defined __ROS_VERSION && __ROS_VERSION == 2
146  seconds = (uint32_t)(time.nanoseconds() / 1000000000);
147  nanoseconds = (uint32_t)(time.nanoseconds() - 1000000000 * seconds);
148 #endif
149 }
150 
152 ROS::Time ROS::timeFromHeader(const std_msgs::Header* msg_header)
153 {
154  return ROS::Time(std::max(0,(int32_t)msg_header->stamp.sec), msg_header->stamp.NSEC);
155 }
156 
161 ROS::Time ROS::timeFromSec(double seconds)
162 {
163  int32_t i32seconds = (int32_t)(seconds);
164  int32_t i32nanoseconds = (int32_t)(1000000000 * (seconds - i32seconds));
165  return ROS::Time(std::max(0,i32seconds), std::max(0,i32nanoseconds));
166 }
167 
172 ROS::Duration ROS::durationFromSec(double seconds)
173 {
174  int32_t i32seconds = (int32_t)(seconds);
175  int32_t i32nanoseconds = (int32_t)(1000000000 * (seconds - i32seconds));
176  return ROS::Duration(i32seconds, i32nanoseconds);
177 }
178 
180 double ROS::seconds(ROS::Duration duration)
181 {
182 #if defined __ROS_VERSION && __ROS_VERSION == 1
183  return duration.toSec();
184 #elif defined __ROS_VERSION && __ROS_VERSION == 2
185  return duration.seconds();
186 #else
187  return 0;
188 #endif
189 }
190 
192 double ROS::secondsSinceStart(const ROS::Time& time)
193 {
194  static ROS::Time s_wrapper_start_time = ROS::now();
195  return ROS::seconds(time - s_wrapper_start_time);
196 }
197 
199 uint64_t ROS::timestampMilliseconds(const ROS::Time& time)
200 {
201 #if defined __ROS_VERSION && __ROS_VERSION == 1
202  return (uint64_t)(time.sec * 1000) + (uint64_t)(time.nsec / 1000000);
203 #elif defined __ROS_VERSION && __ROS_VERSION == 2
204  return (uint64_t)(time.nanoseconds() / 1000000);
205 #endif
206 }
ROS::Time now(void)
double secondsSinceStart(const ROS::Time &time)
void deleteNode(ROS::NodePtr &node)
Definition: ros_wrapper.cpp:98
void splitTime(ROS::Duration time, uint32_t &seconds, uint32_t &nanoseconds)
ROS::Duration durationFromSec(double seconds)
ROSCPP_DECL void spin()
#define ROS_WARN_STREAM(args)
ROS::Time timeFromHeader(const std_msgs::Header *msg_header)
uint64_t timestampMilliseconds(const ROS::Time &time)
static Time now()
ROS::NodePtr createNode(const std::string &node_name="sick_scan")
Definition: ros_wrapper.cpp:65
ROS::Time timeFromSec(double seconds)
void sleep(double seconds)
double seconds(ROS::Duration duration)


sick_scan
Author(s): Michael Lehning , Jochen Sprickerhof , Martin Günther
autogenerated on Wed Sep 7 2022 02:25:06