Skip to content

Commit 110b96b

Browse files
gleichdickgleichdick
committed
conversion for tf2::TimePoint
1 parent ca47b56 commit 110b96b

File tree

5 files changed

+165
-1
lines changed

5 files changed

+165
-1
lines changed

‎tf2/CMakeLists.txt‎

Lines changed: 3 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -11,6 +11,7 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
1111
endif()
1212

1313
find_package(ament_cmake)
14+
find_package(builtin_interfaces REQUIRED)
1415
find_package(console_bridge_vendor REQUIRED) # Provides console_bridge 0.4.0 on platforms without it.
1516
find_package(console_bridge REQUIRED)
1617
find_package(geometry_msgs REQUIRED)
@@ -25,6 +26,7 @@ target_include_directories(tf2 PUBLIC
2526
"$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/src/bt>"
2627
"$<INSTALL_INTERFACE:include>")
2728
ament_target_dependencies(tf2
29+
"builtin_interfaces"
2830
"console_bridge"
2931
"geometry_msgs"
3032
"rcutils"
@@ -89,7 +91,7 @@ if(BUILD_TESTING)
8991

9092
endif()
9193

92-
ament_export_dependencies(console_bridge geometry_msgs rcutils)
94+
ament_export_dependencies(builtin_interfaces console_bridge geometry_msgs rcutils)
9395
ament_export_include_directories(include)
9496
ament_export_libraries(tf2)
9597
ament_export_targets(tf2)

‎tf2/include/tf2/convert.h‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -43,6 +43,7 @@
4343
#include "exceptions.h"
4444
#include "impl/convert.h"
4545
#include "impl/stamped_traits.hpp"
46+
#include "impl/time_convert.hpp"
4647
#include "time.h"
4748
#include "transform_datatypes.h"
4849
#include "visibility_control.h"

‎tf2/include/tf2/impl/time_convert.hpp‎

Lines changed: 150 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,150 @@
1+
// Copyright 2021, Bjarne von Horn. All rights reserved.
2+
//
3+
// Redistribution and use in source and binary forms, with or without
4+
// modification, are permitted provided that the following conditions are met:
5+
//
6+
// * Redistributions of source code must retain the above copyright
7+
// notice, this list of conditions and the following disclaimer.
8+
//
9+
// * Redistributions in binary form must reproduce the above copyright
10+
// notice, this list of conditions and the following disclaimer in the
11+
// documentation and/or other materials provided with the distribution.
12+
//
13+
// * Neither the name of the Open Source Robotics Foundation nor the names of its
14+
// contributors may be used to endorse or promote products derived from
15+
// this software without specific prior written permission.
16+
//
17+
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18+
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19+
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20+
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
21+
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
22+
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
23+
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
24+
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
25+
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
26+
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
27+
// POSSIBILITY OF SUCH DAMAGE.
28+
29+
/** \author dhood, Bjarne von Horn */
30+
31+
#ifndef TF2__IMPL__TIME_CONVERT_HPP_
32+
#define TF2__IMPL__TIME_CONVERT_HPP_
33+
34+
#include <builtin_interfaces/msg/duration.hpp>
35+
#include <builtin_interfaces/msg/time.hpp>
36+
#include <chrono>
37+
38+
#include "../time.h"
39+
#include "forward.hpp"
40+
41+
namespace tf2
42+
{
43+
namespace impl
44+
{
45+
/// \brief Conversion implementation for builtin_interfaces::msg::Time and tf2::TimePoint.
46+
template<>
47+
struct ConversionImplementation<tf2::TimePoint, builtin_interfaces::msg::Time>
48+
{
49+
/**
50+
* \brief Convert a tf2::TimePoint to a Time message.
51+
* \param[in] t The tf2::TimePoint to convert.
52+
* \param[out] time_msg The converted tf2::TimePoint, as a Time message.
53+
*/
54+
static void toMsg(const tf2::TimePoint & t, builtin_interfaces::msg::Time & time_msg)
55+
{
56+
std::chrono::nanoseconds ns =
57+
std::chrono::duration_cast<std::chrono::nanoseconds>(t.time_since_epoch());
58+
std::chrono::seconds s = std::chrono::duration_cast<std::chrono::seconds>(t.time_since_epoch());
59+
60+
time_msg.sec = static_cast<int32_t>(s.count());
61+
time_msg.nanosec = static_cast<uint32_t>(ns.count() % 1000000000ull);
62+
}
63+
64+
/**
65+
* \brief Convert a Time message to a tf2::TimePoint.
66+
* \param[in] time_msg The Time message to convert.
67+
* \param[out] t The converted message, as a tf2::TimePoint.
68+
*/
69+
static void fromMsg(const builtin_interfaces::msg::Time & time_msg, tf2::TimePoint & t)
70+
{
71+
int64_t d = time_msg.sec * 1000000000ull + time_msg.nanosec;
72+
std::chrono::nanoseconds ns(d);
73+
t = tf2::TimePoint(std::chrono::duration_cast<tf2::Duration>(ns));
74+
}
75+
};
76+
77+
/// \brief Conversion implementation for builtin_interfaces::msg::Duration and tf2::Duration.
78+
template<>
79+
struct ConversionImplementation<tf2::Duration, builtin_interfaces::msg::Duration>
80+
{
81+
/**
82+
* \brief Convert a tf2::Duration to a Duration message.
83+
* \param[in] t The tf2::Duration to convert.
84+
* \param[out] duration_msg The converted tf2::Duration, as a Duration message.
85+
*/
86+
static void toMsg(const tf2::Duration & t, builtin_interfaces::msg::Duration & duration_msg)
87+
{
88+
std::chrono::nanoseconds ns = std::chrono::duration_cast<std::chrono::nanoseconds>(t);
89+
std::chrono::seconds s = std::chrono::duration_cast<std::chrono::seconds>(t);
90+
duration_msg.sec = static_cast<int32_t>(s.count());
91+
duration_msg.nanosec = static_cast<uint32_t>(ns.count() % 1000000000ull);
92+
}
93+
94+
/**
95+
* \brief Convert a Duration message to a tf2::Duration.
96+
* \param[in] duration_msg The Duration message to convert.
97+
* \param[out] duration The converted message, as a tf2::Duration.
98+
*/
99+
static void fromMsg(
100+
const builtin_interfaces::msg::Duration & duration_msg, tf2::Duration & duration)
101+
{
102+
int64_t d = duration_msg.sec * 1000000000ull + duration_msg.nanosec;
103+
std::chrono::nanoseconds ns(d);
104+
duration = tf2::Duration(std::chrono::duration_cast<tf2::Duration>(ns));
105+
}
106+
};
107+
108+
/// \brief Default return type of tf2::toMsg() for tf2::TimePoint.
109+
template<>
110+
struct DefaultMessageForDatatype<tf2::TimePoint>
111+
{
112+
/// \brief Default return type of tf2::toMsg() for tf2::TimePoint.
113+
using type = builtin_interfaces::msg::Time;
114+
};
115+
116+
/// \brief Default return type of tf2::toMsg() for tf2::Duration.
117+
template<>
118+
struct DefaultMessageForDatatype<tf2::Duration>
119+
{
120+
/// \brief Default return type of tf2::toMsg() for tf2::Duration.
121+
using type = builtin_interfaces::msg::Duration;
122+
};
123+
} // namespace impl
124+
125+
/**
126+
* \brief Convert a Time message to a tf2::TimePoint.
127+
* \param[in] time_msg The Time message to convert.
128+
* \return The converted message, as a tf2::TimePoint.
129+
*/
130+
inline tf2::TimePoint TimePointFromMsg(const builtin_interfaces::msg::Time & time_msg)
131+
{
132+
TimePoint tp;
133+
tf2::fromMsg<>(time_msg, tp);
134+
return tp;
135+
}
136+
137+
/**
138+
* \brief Convert a Duration message to a tf2::Duration.
139+
* \param[in] duration_msg The Duration message to convert.
140+
* \return The converted message, as a tf2::Duration.
141+
*/
142+
inline tf2::Duration DurationFromMsg(const builtin_interfaces::msg::Duration & duration_msg)
143+
{
144+
Duration d;
145+
tf2::fromMsg<>(duration_msg, d);
146+
return d;
147+
}
148+
} // namespace tf2
149+
150+
#endif // TF2__IMPL__TIME_CONVERT_HPP_

‎tf2/include/tf2/transform_datatypes.h‎

Lines changed: 10 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -36,6 +36,8 @@
3636
#include <string>
3737

3838
#include "tf2/time.h"
39+
#include "impl/time_convert.hpp"
40+
#include <builtin_interfaces/msg/time.hpp>
3941

4042
namespace tf2
4143
{
@@ -61,6 +63,14 @@ class Stamped : public T
6163
{
6264
}
6365

66+
/** Full constructor */
67+
Stamped(
68+
const T & input, const builtin_interfaces::msg::Time & timestamp,
69+
const std::string & frame_id)
70+
: T(input), stamp_(TimePointFromMsg(timestamp)), frame_id_(frame_id)
71+
{
72+
}
73+
6474
/** Copy Constructor */
6575
Stamped(const Stamped<T> & s)
6676
: T(s),

‎tf2/package.xml‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -20,6 +20,7 @@
2020

2121
<buildtool_depend>ament_cmake</buildtool_depend>
2222

23+
<depend>builtin_interfaces</depend>
2324
<depend>console_bridge_vendor</depend>
2425
<depend>geometry_msgs</depend>
2526
<depend>libconsole-bridge-dev</depend>

0 commit comments

Comments
 (0)