Skip to content

Commit e61d385

Browse files
gleichdickgleichdick
committed
conversion for tf2::TimePoint
1 parent 31df87b commit e61d385

File tree

3 files changed

+155
-0
lines changed

3 files changed

+155
-0
lines changed

‎tf2/include/tf2/convert.h‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -41,6 +41,7 @@
4141
#include "exceptions.h"
4242
#include "impl/convert.h"
4343
#include "impl/stamped_traits.hpp"
44+
#include "impl/time_convert.hpp"
4445
#include "time.h"
4546
#include "transform_datatypes.h"
4647
#include "visibility_control.h"

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

Lines changed: 144 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,144 @@
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+
/** \brief Convert a tf2::TimePoint to a Time message.
50+
* \param[in] t The tf2::TimePoint to convert.
51+
* \param[out] time_msg The converted tf2::TimePoint, as a Time message.
52+
*/
53+
static void toMsg(const tf2::TimePoint & t, builtin_interfaces::msg::Time & time_msg)
54+
{
55+
std::chrono::nanoseconds ns =
56+
std::chrono::duration_cast<std::chrono::nanoseconds>(t.time_since_epoch());
57+
std::chrono::seconds s = std::chrono::duration_cast<std::chrono::seconds>(t.time_since_epoch());
58+
59+
time_msg.sec = static_cast<int32_t>(s.count());
60+
time_msg.nanosec = static_cast<uint32_t>(ns.count() % 1000000000ull);
61+
}
62+
63+
/** \brief Convert a Time message to a tf2::TimePoint.
64+
* \param[in] time_msg The Time message to convert.
65+
* \param[out] t The converted message, as a tf2::TimePoint.
66+
*/
67+
static void fromMsg(const builtin_interfaces::msg::Time & time_msg, tf2::TimePoint & t)
68+
{
69+
int64_t d = time_msg.sec * 1000000000ull + time_msg.nanosec;
70+
std::chrono::nanoseconds ns(d);
71+
t = tf2::TimePoint(std::chrono::duration_cast<tf2::Duration>(ns));
72+
}
73+
};
74+
75+
/// \brief Conversion implementation for builtin_interfaces::msg::Duration and tf2::Duration.
76+
template<>
77+
struct ConversionImplementation<tf2::Duration, builtin_interfaces::msg::Duration>
78+
{
79+
/** \brief Convert a tf2::Duration to a Duration message.
80+
* \param[in] t The tf2::Duration to convert.
81+
* \param[out] duration_msg The converted tf2::Duration, as a Duration message.
82+
*/
83+
static void toMsg(const tf2::Duration & t, builtin_interfaces::msg::Duration & duration_msg)
84+
{
85+
std::chrono::nanoseconds ns = std::chrono::duration_cast<std::chrono::nanoseconds>(t);
86+
std::chrono::seconds s = std::chrono::duration_cast<std::chrono::seconds>(t);
87+
duration_msg.sec = static_cast<int32_t>(s.count());
88+
duration_msg.nanosec = static_cast<uint32_t>(ns.count() % 1000000000ull);
89+
}
90+
91+
/** \brief Convert a Duration message to a tf2::Duration.
92+
* \param[in] duration_msg The Duration message to convert.
93+
* \param[out] duration The converted message, as a tf2::Duration.
94+
*/
95+
static void fromMsg(
96+
const builtin_interfaces::msg::Duration & duration_msg, tf2::Duration & duration)
97+
{
98+
int64_t d = duration_msg.sec * 1000000000ull + duration_msg.nanosec;
99+
std::chrono::nanoseconds ns(d);
100+
duration = tf2::Duration(std::chrono::duration_cast<tf2::Duration>(ns));
101+
}
102+
};
103+
104+
/// \brief Default return type of tf2::toMsg() for tf2::TimePoint.
105+
template<>
106+
struct DefaultMessageForDatatype<tf2::TimePoint>
107+
{
108+
/// \brief Default return type of tf2::toMsg() for tf2::TimePoint.
109+
using type = builtin_interfaces::msg::Time;
110+
};
111+
112+
/// \brief Default return type of tf2::toMsg() for tf2::Duration.
113+
template<>
114+
struct DefaultMessageForDatatype<tf2::Duration>
115+
{
116+
/// \brief Default return type of tf2::toMsg() for tf2::Duration.
117+
using type = builtin_interfaces::msg::Duration;
118+
};
119+
} // namespace impl
120+
121+
/** \brief Convert a Time message to a tf2::TimePoint.
122+
* \param[in] time_msg The Time message to convert.
123+
* \return The converted message, as a tf2::TimePoint.
124+
*/
125+
inline tf2::TimePoint TimePointFromMsg(const builtin_interfaces::msg::Time & time_msg)
126+
{
127+
TimePoint tp;
128+
tf2::fromMsg<>(time_msg, tp);
129+
return tp;
130+
}
131+
132+
/** \brief Convert a Duration message to a tf2::Duration.
133+
* \param[in] duration_msg The Duration message to convert.
134+
* \return The converted message, as a tf2::Duration.
135+
*/
136+
inline tf2::Duration DurationFromMsg(const builtin_interfaces::msg::Duration & duration_msg)
137+
{
138+
Duration d;
139+
tf2::fromMsg<>(duration_msg, d);
140+
return d;
141+
}
142+
} // namespace tf2
143+
144+
#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),

0 commit comments

Comments
 (0)