niklas/rclcpp
1
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity Actions

update rclcpp to use the refactored TimeSource Clock logic (#371)

Browse source 
This implements a TimeSource in rclcpp, adds the Clock class.
This commit is contained in:
Tully Foote 2017-11-16 17:26:56 -08:00 • committed by GitHub
parent 24f39700c6
commit a215d2d22e
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
12 changed files with 1509 additions and 128 deletions
Download patch file Download diff file Expand all files Collapse all files

23
rclcpp/CMakeLists.txt
View file

@ -31,8 +31,10 @@ set(${PROJECT_NAME}_SRCS
src/rclcpp/any_executable.cpp
src/rclcpp/callback_group.cpp
src/rclcpp/client.cpp
src/rclcpp/clock.cpp
src/rclcpp/context.cpp
src/rclcpp/contexts/default_context.cpp
src/rclcpp/duration.cpp
src/rclcpp/event.cpp
src/rclcpp/exceptions.cpp
src/rclcpp/executor.cpp
@ -59,6 +61,7 @@ set(${PROJECT_NAME}_SRCS
src/rclcpp/service.cpp
src/rclcpp/subscription.cpp
src/rclcpp/time.cpp
src/rclcpp/time_source.cpp
src/rclcpp/timer.cpp
src/rclcpp/type_support.cpp
src/rclcpp/utilities.cpp
@ -266,6 +269,17 @@ if(BUILD_TESTING)
endforeach()
endif()
ament_add_gtest(test_duration test/test_duration.cpp
APPEND_LIBRARY_DIRS "${append_library_dirs}")
if(TARGET test_duration)
ament_target_dependencies(test_duration
"rcl")
target_link_libraries(test_duration ${PROJECT_NAME})
endif()
ament_add_gmock(test_logging test/test_logging.cpp)
target_link_libraries(test_logging ${PROJECT_NAME})
ament_add_gtest(test_time test/test_time.cpp
APPEND_LIBRARY_DIRS "${append_library_dirs}")
if(TARGET test_time)
@ -274,8 +288,13 @@ if(BUILD_TESTING)
target_link_libraries(test_time ${PROJECT_NAME})
endif()
ament_add_gmock(test_logging test/test_logging.cpp)
target_link_libraries(test_logging ${PROJECT_NAME})
ament_add_gtest(test_time_source test/test_time_source.cpp
APPEND_LIBRARY_DIRS "${append_library_dirs}")
if(TARGET test_time_source)
ament_target_dependencies(test_time_source
"rcl")
target_link_libraries(test_time_source ${PROJECT_NAME})
endif()
endif()
ament_package(

143
rclcpp/include/rclcpp/clock.hpp Normal file
View file

@ -0,0 +1,143 @@
// Copyright 2017 Open Source Robotics Foundation, Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef RCLCPP__CLOCK_HPP_
#define RCLCPP__CLOCK_HPP_
#include <functional>
#include <memory>
#include <mutex>
#include <vector>
#include "rclcpp/macros.hpp"
#include "rclcpp/time.hpp"
#include "rclcpp/visibility_control.hpp"
#include "rcl/time.h"
namespace rclcpp
{
class TimeSource;
/* \brief A struct to represent a timejump.
* It epresents the time jump duration and whether it changed clock type.
*/
struct TimeJump
{
typedef enum ClockChange_t
{
ROS_TIME_NO_CHANGE, ///< The time type before and after the jump is ROS_TIME
ROS_TIME_ACTIVATED, ///< The time type switched to ROS_TIME from SYSTEM_TIME
ROS_TIME_DEACTIVATED, ///< The time type switched to SYSTEM_TIME from ROS_TIME
SYSTEM_TIME_NO_CHANGE ///< The time type before and after the jump is SYSTEM_TIME
} ClockChange_t;
ClockChange_t jump_type_; ///< The change in clock_type if there is one.
rcl_duration_t delta_; ///< The change in time value.
};
/* \brief A class to store a threshold for a TimeJump
* This class can be used to evaluate a time jump's magnitude.
*/
class JumpThreshold
{
public:
uint64_t min_forward_; ///< The minimum jump forward to be considered exceeded..
uint64_t min_backward_; ///< The minimum backwards jump to be considered exceeded.
bool on_clock_change_; ///< Whether to trigger on any clock type change.
// Test if the threshold is exceeded by a TimeJump
RCLCPP_PUBLIC
bool
is_exceeded(const TimeJump & jump);
};
class JumpHandler
{
public:
RCLCPP_SMART_PTR_DEFINITIONS(JumpHandler)
JumpHandler(
std::function<void()> pre_callback,
std::function<void(TimeJump)> post_callback,
JumpThreshold & threshold);
std::function<void()> pre_callback;
std::function<void(const TimeJump &)> post_callback;
JumpThreshold notice_threshold;
};
class Clock
{
public:
RCLCPP_SMART_PTR_DEFINITIONS(Clock)
RCLCPP_PUBLIC
explicit Clock(rcl_clock_type_t clock_type = RCL_SYSTEM_TIME);
RCLCPP_PUBLIC
~Clock();
RCLCPP_PUBLIC
Time
now();
RCLCPP_PUBLIC
bool
ros_time_is_active();
RCLCPP_PUBLIC
rcl_clock_type_t
get_clock_type();
// Add a callback to invoke if the jump threshold is exceeded.
/**
* These callback functions must remain valid as long as the
* returned shared pointer is valid.
*/
RCLCPP_PUBLIC
JumpHandler::SharedPtr
create_jump_callback(
std::function<void()> pre_callback,
std::function<void(const TimeJump &)> post_callback,
JumpThreshold & threshold);
private:
// A method for TimeSource to get a list of callbacks to invoke while updating
RCLCPP_PUBLIC
std::vector<JumpHandler::SharedPtr>
get_triggered_callback_handlers(const TimeJump & jump);
// Invoke callbacks that are valid and outside threshold.
RCLCPP_PUBLIC
static void invoke_prejump_callbacks(
const std::vector<JumpHandler::SharedPtr> & callbacks);
RCLCPP_PUBLIC
static void invoke_postjump_callbacks(
const std::vector<JumpHandler::SharedPtr> & callbacks,
const TimeJump & jump);
/// Internal storage backed by rcl
rcl_clock_t rcl_clock_;
friend TimeSource; /// Allow TimeSource to access the rcl_clock_ datatype.
rcl_allocator_t allocator_;
std::mutex callback_list_mutex_;
std::vector<std::weak_ptr<JumpHandler>> active_jump_handlers_;
};
} // namespace rclcpp
#endif // RCLCPP__CLOCK_HPP_

102
rclcpp/include/rclcpp/duration.hpp Normal file
View file

@ -0,0 +1,102 @@
// Copyright 2017 Open Source Robotics Foundation, Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef RCLCPP__DURATION_HPP_
#define RCLCPP__DURATION_HPP_
#include <chrono>
#include "builtin_interfaces/msg/duration.hpp"
#include "rcl/time.h"
#include "rclcpp/visibility_control.hpp"
namespace rclcpp
{
class Duration
{
public:
RCLCPP_PUBLIC
Duration(int32_t seconds, uint32_t nanoseconds);
RCLCPP_PUBLIC
explicit Duration(
rcl_duration_value_t nanoseconds);
RCLCPP_PUBLIC
explicit Duration(
std::chrono::nanoseconds nanoseconds);
RCLCPP_PUBLIC
Duration(
const builtin_interfaces::msg::Duration & duration_msg);
RCLCPP_PUBLIC
explicit Duration(const rcl_duration_t & duration);
RCLCPP_PUBLIC
Duration(const Duration & rhs);
RCLCPP_PUBLIC
virtual ~Duration();
RCLCPP_PUBLIC
operator builtin_interfaces::msg::Duration() const;
RCLCPP_PUBLIC
Duration &
operator=(const Duration & rhs);
RCLCPP_PUBLIC
Duration &
operator=(const builtin_interfaces::msg::Duration & Duration_msg);
RCLCPP_PUBLIC
bool
operator==(const rclcpp::Duration & rhs) const;
RCLCPP_PUBLIC
bool
operator<(const rclcpp::Duration & rhs) const;
RCLCPP_PUBLIC
bool
operator<=(const rclcpp::Duration & rhs) const;
RCLCPP_PUBLIC
bool
operator>=(const rclcpp::Duration & rhs) const;
RCLCPP_PUBLIC
bool
operator>(const rclcpp::Duration & rhs) const;
RCLCPP_PUBLIC
Duration
operator+(const rclcpp::Duration & rhs) const;
RCLCPP_PUBLIC
Duration
operator-(const rclcpp::Duration & rhs) const;
RCLCPP_PUBLIC
rcl_duration_value_t
nanoseconds() const;
private:
rcl_duration_t rcl_duration_;
};
} // namespace rclcpp
#endif // RCLCPP__DURATION_HPP_

39
rclcpp/include/rclcpp/time.hpp
View file

@ -21,28 +21,32 @@
#include "rcl/time.h"
#include "rclcpp/duration.hpp"
namespace rclcpp
{
class Clock;
class Time
{
public:
RCLCPP_PUBLIC
static
Time
now(rcl_time_source_type_t clock = RCL_SYSTEM_TIME);
Time(int32_t seconds, uint32_t nanoseconds, rcl_clock_type_t clock_type = RCL_SYSTEM_TIME);
RCLCPP_PUBLIC
Time(int32_t seconds, uint32_t nanoseconds, rcl_time_source_type_t clock = RCL_SYSTEM_TIME);
RCLCPP_PUBLIC
explicit Time(uint64_t nanoseconds = 0, rcl_time_source_type_t clock = RCL_SYSTEM_TIME);
explicit Time(uint64_t nanoseconds = 0, rcl_clock_type_t clock = RCL_SYSTEM_TIME);
RCLCPP_PUBLIC
Time(const Time & rhs);
RCLCPP_PUBLIC
Time(const builtin_interfaces::msg::Time & time_msg); // NOLINT
Time(
const builtin_interfaces::msg::Time & time_msg,
rcl_clock_type_t ros_time = RCL_ROS_TIME);
RCLCPP_PUBLIC
explicit Time(const rcl_time_point_t & time_point);
RCLCPP_PUBLIC
virtual ~Time();
@ -84,21 +88,32 @@ public:
RCLCPP_PUBLIC
Time
operator+(const rclcpp::Time & rhs) const;
operator+(const rclcpp::Duration & rhs) const;
RCLCPP_PUBLIC
Time
Duration
operator-(const rclcpp::Time & rhs) const;
RCLCPP_PUBLIC
uint64_t
Time
operator-(const rclcpp::Duration & rhs) const;
RCLCPP_PUBLIC
rcl_time_point_value_t
nanoseconds() const;
RCLCPP_PUBLIC
rcl_clock_type_t
get_clock_type() const;
private:
rcl_time_source_t rcl_time_source_;
rcl_time_point_t rcl_time_;
friend Clock; // Allow clock to manipulate internal data
};
Time
operator+(const rclcpp::Duration & lhs, const rclcpp::Time & rhs);
} // namespace rclcpp
#endif // RCLCPP__TIME_HPP_

116
rclcpp/include/rclcpp/time_source.hpp Normal file
View file

@ -0,0 +1,116 @@
// Copyright 2017 Open Source Robotics Foundation, Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef RCLCPP__TIME_SOURCE_HPP_
#define RCLCPP__TIME_SOURCE_HPP_
#include <memory>
#include <vector>
#include "rcl/time.h"
#include "rcl_interfaces/msg/parameter_event.hpp"
#include "rclcpp/node.hpp"
#include "rclcpp/parameter_client.hpp"
namespace rclcpp
{
class Clock;
class TimeSource
{
public:
RCLCPP_PUBLIC
explicit TimeSource(rclcpp::node::Node::SharedPtr node);
RCLCPP_PUBLIC
TimeSource();
RCLCPP_PUBLIC
void attachNode(rclcpp::node::Node::SharedPtr node);
RCLCPP_PUBLIC
void detachNode();
/// Attach a clock to the time source to be updated
/**
* \throws std::invalid_argument if node is nullptr
*/
RCLCPP_PUBLIC
void attachClock(rclcpp::Clock::SharedPtr clock);
RCLCPP_PUBLIC
void detachClock(rclcpp::Clock::SharedPtr clock);
RCLCPP_PUBLIC
~TimeSource();
private:
// Preserve the node reference
rclcpp::node::Node::SharedPtr node_;
// The subscription for the clock callback
using MessageT = builtin_interfaces::msg::Time;
using Alloc = std::allocator<void>;
using SubscriptionT = rclcpp::subscription::Subscription<MessageT, Alloc>;
std::shared_ptr<SubscriptionT> clock_subscription_;
// The clock callback itself
void clock_cb(const builtin_interfaces::msg::Time::SharedPtr msg);
// Parameter Client pointer
std::shared_ptr<rclcpp::parameter_client::AsyncParametersClient> parameter_client_;
// Parameter Event subscription
using ParamMessageT = rcl_interfaces::msg::ParameterEvent;
using ParamSubscriptionT = rclcpp::subscription::Subscription<ParamMessageT, Alloc>;
std::shared_ptr<ParamSubscriptionT> parameter_subscription_;
// Callback for parameter updates
void on_parameter_event(const rcl_interfaces::msg::ParameterEvent::SharedPtr event);
// An enum to hold the parameter state
enum UseSimTimeParameterState {UNSET, SET_TRUE, SET_FALSE};
UseSimTimeParameterState parameter_state_;
// An internal method to use in the clock callback that iterates and enables all clocks
void enable_ros_time();
// An internal method to use in the clock callback that iterates and disables all clocks
void disable_ros_time();
// Internal helper functions used inside iterators
static void enable_ros_time(rclcpp::Clock::SharedPtr clock);
static void disable_ros_time(rclcpp::Clock::SharedPtr clock);
static void set_clock(
const builtin_interfaces::msg::Time::SharedPtr msg,
bool set_ros_time_enabled,
rclcpp::Clock::SharedPtr clock);
// Local storage of validity of ROS time
// This is needed when new clocks are added.
bool ros_time_active_;
// Last set message to be passed to newly registered clocks
builtin_interfaces::msg::Time::SharedPtr last_msg_set_;
// A lock to protect iterating the associated_clocks_ field.
std::mutex clock_list_lock_;
// A vector to store references to associated clocks.
std::vector<rclcpp::Clock::SharedPtr> associated_clocks_;
};
} // namespace rclcpp
#endif // RCLCPP__TIME_SOURCE_HPP_

164
rclcpp/src/rclcpp/clock.cpp Normal file
View file

@ -0,0 +1,164 @@
// Copyright 2017 Open Source Robotics Foundation, Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include "rclcpp/clock.hpp"
#include <memory>
#include <utility>
#include <vector>
#include "builtin_interfaces/msg/time.hpp"
#include "rcl/time.h"
#include "rclcpp/exceptions.hpp"
#include "rcutils/logging_macros.h"
namespace rclcpp
{
bool
JumpThreshold::is_exceeded(const TimeJump & jump)
{
if (on_clock_change_ &&
(jump.jump_type_ == TimeJump::ClockChange_t::ROS_TIME_ACTIVATED ||
jump.jump_type_ == TimeJump::ClockChange_t::ROS_TIME_DEACTIVATED))
{
return true;
}
if ((uint64_t)jump.delta_.nanoseconds > min_forward_ ||
(uint64_t)jump.delta_.nanoseconds < min_backward_)
{
return true;
}
return false;
}
JumpHandler::JumpHandler(
std::function<void()> pre_callback,
std::function<void(TimeJump)> post_callback,
JumpThreshold & threshold)
: pre_callback(pre_callback),
post_callback(post_callback),
notice_threshold(threshold)
{}
Clock::Clock(rcl_clock_type_t clock_type)
{
allocator_ = rcl_get_default_allocator();
auto ret = rcl_clock_init(clock_type, &rcl_clock_, &allocator_);
if (ret != RCL_RET_OK) {
rclcpp::exceptions::throw_from_rcl_error(
ret, "could not get current time stamp");
}
}
Clock::~Clock()
{
auto ret = rcl_clock_fini(&rcl_clock_);
if (ret != RCL_RET_OK) {
RCUTILS_LOG_ERROR("Failed to fini rcl clock.");
}
}
Time
Clock::now()
{
Time now(0, 0, rcl_clock_.type);
auto ret = rcl_clock_get_now(&rcl_clock_, &now.rcl_time_);
if (ret != RCL_RET_OK) {
rclcpp::exceptions::throw_from_rcl_error(
ret, "could not get current time stamp");
}
return now;
}
bool
Clock::ros_time_is_active()
{
if (!rcl_clock_valid(&rcl_clock_)) {
RCUTILS_LOG_ERROR("ROS time not valid!");
return false;
}
bool is_enabled;
auto ret = rcl_is_enabled_ros_time_override(&rcl_clock_, &is_enabled);
if (ret != RCL_RET_OK) {
rclcpp::exceptions::throw_from_rcl_error(
ret, "Failed to check ros_time_override_status");
}
return is_enabled;
}
rcl_clock_type_t
Clock::get_clock_type()
{
return rcl_clock_.type;
}
rclcpp::JumpHandler::SharedPtr
Clock::create_jump_callback(
std::function<void()> pre_callback,
std::function<void(const TimeJump &)> post_callback,
JumpThreshold & threshold)
{
// JumpHandler jump_callback;
auto jump_callback =
std::make_shared<rclcpp::JumpHandler>(pre_callback, post_callback, threshold);
{
std::lock_guard<std::mutex> guard(callback_list_mutex_);
active_jump_handlers_.push_back(jump_callback);
}
return jump_callback;
}
std::vector<JumpHandler::SharedPtr>
Clock::get_triggered_callback_handlers(const TimeJump & jump)
{
std::vector<JumpHandler::SharedPtr> callbacks;
std::lock_guard<std::mutex> guard(callback_list_mutex_);
for (auto wjcb = active_jump_handlers_.begin(); wjcb != active_jump_handlers_.end(); wjcb++) {
if (auto jcb = wjcb->lock()) {
if (jcb->notice_threshold.is_exceeded(jump)) {
callbacks.push_back(jcb);
}
} else {
active_jump_handlers_.erase(wjcb);
}
}
return callbacks;
}
void
Clock::invoke_prejump_callbacks(
const std::vector<JumpHandler::SharedPtr> & callbacks)
{
for (const auto cb : callbacks) {
cb->pre_callback();
}
}
void
Clock::invoke_postjump_callbacks(
const std::vector<JumpHandler::SharedPtr> & callbacks,
const TimeJump & jump)
{
for (auto cb : callbacks) {
cb->post_callback(jump);
}
}
} // namespace rclcpp

190
rclcpp/src/rclcpp/duration.cpp Normal file
View file

@ -0,0 +1,190 @@
// Copyright 2017 Open Source Robotics Foundation, Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include <utility>
#include <limits>
#include "rclcpp/clock.hpp"
#include "rclcpp/time.hpp"
#include "builtin_interfaces/msg/duration.hpp"
#include "rcl/time.h"
#include "rclcpp/exceptions.hpp"
#include "rcutils/logging_macros.h"
namespace rclcpp
{
Duration::Duration(int32_t seconds, uint32_t nanoseconds)
{
rcl_duration_.nanoseconds = RCL_S_TO_NS(static_cast<int64_t>(seconds));
rcl_duration_.nanoseconds += nanoseconds;
}
Duration::Duration(int64_t nanoseconds)
{
rcl_duration_.nanoseconds = nanoseconds;
}
Duration::Duration(std::chrono::nanoseconds nanoseconds)
{
rcl_duration_.nanoseconds = nanoseconds.count();
}
Duration::Duration(const Duration & rhs)
{
rcl_duration_.nanoseconds = rhs.rcl_duration_.nanoseconds;
}
Duration::Duration(
const builtin_interfaces::msg::Duration & duration_msg)
{
rcl_duration_.nanoseconds = RCL_S_TO_NS(static_cast<uint64_t>(duration_msg.sec));
rcl_duration_.nanoseconds += duration_msg.nanosec;
}
Duration::Duration(const rcl_duration_t & duration)
: rcl_duration_(duration)
{
// noop
}
Duration::~Duration()
{
}
Duration::operator builtin_interfaces::msg::Duration() const
{
builtin_interfaces::msg::Duration msg_duration;
msg_duration.sec = static_cast<std::int32_t>(RCL_NS_TO_S(rcl_duration_.nanoseconds));
msg_duration.nanosec =
static_cast<std::uint32_t>(rcl_duration_.nanoseconds % (1000 * 1000 * 1000));
return msg_duration;
}
Duration &
Duration::operator=(const Duration & rhs)
{
rcl_duration_.nanoseconds = rhs.rcl_duration_.nanoseconds;
return *this;
}
Duration &
Duration::operator=(const builtin_interfaces::msg::Duration & duration_msg)
{
if (duration_msg.sec < 0) {
throw std::runtime_error("cannot store a negative duration point in rclcpp::Duration");
}
rcl_duration_.nanoseconds = RCL_S_TO_NS(static_cast<int64_t>(duration_msg.sec));
rcl_duration_.nanoseconds += duration_msg.nanosec;
return *this;
}
bool
Duration::operator==(const rclcpp::Duration & rhs) const
{
return rcl_duration_.nanoseconds == rhs.rcl_duration_.nanoseconds;
}
bool
Duration::operator<(const rclcpp::Duration & rhs) const
{
return rcl_duration_.nanoseconds < rhs.rcl_duration_.nanoseconds;
}
bool
Duration::operator<=(const rclcpp::Duration & rhs) const
{
return rcl_duration_.nanoseconds <= rhs.rcl_duration_.nanoseconds;
}
bool
Duration::operator>=(const rclcpp::Duration & rhs) const
{
return rcl_duration_.nanoseconds >= rhs.rcl_duration_.nanoseconds;
}
bool
Duration::operator>(const rclcpp::Duration & rhs) const
{
return rcl_duration_.nanoseconds > rhs.rcl_duration_.nanoseconds;
}
void
bounds_check_duration_sum(int64_t lhsns, int64_t rhsns, uint64_t max)
{
auto abs_lhs = (uint64_t)std::abs(lhsns);
auto abs_rhs = (uint64_t)std::abs(rhsns);
if (lhsns > 0 && rhsns > 0) {
if (abs_lhs + abs_rhs > (uint64_t) max) {
throw std::overflow_error("addition leads to int64_t overflow");
}
} else if (lhsns < 0 && rhsns < 0) {
if (abs_lhs + abs_rhs > (uint64_t) max) {
throw std::underflow_error("addition leads to int64_t underflow");
}
}
}
Duration
Duration::operator+(const rclcpp::Duration & rhs) const
{
bounds_check_duration_sum(
this->rcl_duration_.nanoseconds,
rhs.rcl_duration_.nanoseconds,
std::numeric_limits<rcl_duration_value_t>::max());
return Duration(
rcl_duration_.nanoseconds + rhs.rcl_duration_.nanoseconds);
}
void
bounds_check_duration_difference(int64_t lhsns, int64_t rhsns, uint64_t max)
{
auto abs_lhs = (uint64_t)std::abs(lhsns);
auto abs_rhs = (uint64_t)std::abs(rhsns);
if (lhsns > 0 && rhsns < 0) {
if (abs_lhs + abs_rhs > (uint64_t) max) {
throw std::overflow_error("duration subtraction leads to int64_t overflow");
}
} else if (lhsns < 0 && rhsns > 0) {
if (abs_lhs + abs_rhs > (uint64_t) max) {
throw std::underflow_error("duration subtraction leads to int64_t underflow");
}
}
}
Duration
Duration::operator-(const rclcpp::Duration & rhs) const
{
bounds_check_duration_difference(
this->rcl_duration_.nanoseconds,
rhs.rcl_duration_.nanoseconds,
std::numeric_limits<rcl_duration_value_t>::max());
return Duration(
rcl_duration_.nanoseconds - rhs.rcl_duration_.nanoseconds);
}
rcl_duration_value_t
Duration::nanoseconds() const
{
return rcl_duration_.nanoseconds;
}
} // namespace rclcpp

177
rclcpp/src/rclcpp/time.cpp
View file

@ -12,10 +12,12 @@
// See the License for the specific language governing permissions and
// limitations under the License.
#include "rclcpp/time.hpp"
#include <limits>
#include <utility>
#include "rclcpp/clock.hpp"
#include "rclcpp/duration.hpp"
#include "builtin_interfaces/msg/time.hpp"
#include "rcl/time.h"
@ -27,30 +29,11 @@
namespace
{
rcl_time_source_t
init_time_source(rcl_time_source_type_t clock = RCL_SYSTEM_TIME)
{
rcl_time_source_t time_source;
auto ret = rcl_time_source_init(clock, &time_source);
if (ret != RCL_RET_OK) {
rclcpp::exceptions::throw_from_rcl_error(
ret, "could not initialize time source");
}
return time_source;
}
rcl_time_point_t
init_time_point(rcl_time_source_t & time_source)
init_time_point(rcl_clock_type_t & clock_type)
{
rcl_time_point_t time_point;
auto ret = rcl_time_point_init(&time_point, &time_source);
if (ret != RCL_RET_OK) {
rclcpp::exceptions::throw_from_rcl_error(
ret, "could not initialize time point");
}
time_point.clock_type = clock_type;
return time_point;
}
@ -60,32 +43,8 @@ init_time_point(rcl_time_source_t & time_source)
namespace rclcpp
{
Time
Time::now(rcl_time_source_type_t clock)
{
// TODO(karsten1987): This impl throws explicitely on RCL_ROS_TIME
// we have to do this, because rcl_time_source_init returns RCL_RET_OK
// for RCL_ROS_TIME, however defaults to system time under the hood.
// ref: https://github.com/ros2/rcl/blob/master/rcl/src/rcl/time.c#L66-L77
// This section can be removed when rcl supports ROS_TIME correctly.
if (clock == RCL_ROS_TIME) {
throw std::runtime_error("RCL_ROS_TIME is currently not implemented.");
}
Time now(0, 0, clock);
auto ret = rcl_time_point_get_now(&now.rcl_time_);
if (ret != RCL_RET_OK) {
rclcpp::exceptions::throw_from_rcl_error(
ret, "could not get current time stamp");
}
return now;
}
Time::Time(int32_t seconds, uint32_t nanoseconds, rcl_time_source_type_t clock)
: rcl_time_source_(init_time_source(clock)),
rcl_time_(init_time_point(rcl_time_source_))
Time::Time(int32_t seconds, uint32_t nanoseconds, rcl_clock_type_t clock_type)
: rcl_time_(init_time_point(clock_type))
{
if (seconds < 0) {
throw std::runtime_error("cannot store a negative time point in rclcpp::Time");
@ -95,24 +54,23 @@ Time::Time(int32_t seconds, uint32_t nanoseconds, rcl_time_source_type_t clock)
rcl_time_.nanoseconds += nanoseconds;
}
Time::Time(uint64_t nanoseconds, rcl_time_source_type_t clock)
: rcl_time_source_(init_time_source(clock)),
rcl_time_(init_time_point(rcl_time_source_))
Time::Time(uint64_t nanoseconds, rcl_clock_type_t clock_type)
: rcl_time_(init_time_point(clock_type))
{
rcl_time_.nanoseconds = nanoseconds;
}
Time::Time(const Time & rhs)
: rcl_time_source_(init_time_source(rhs.rcl_time_source_.type)),
rcl_time_(init_time_point(rcl_time_source_))
: rcl_time_(rhs.rcl_time_)
{
rcl_time_.nanoseconds = rhs.rcl_time_.nanoseconds;
}
Time::Time(const builtin_interfaces::msg::Time & time_msg) // NOLINT
: rcl_time_source_(init_time_source(RCL_SYSTEM_TIME)),
rcl_time_(init_time_point(rcl_time_source_))
Time::Time(
const builtin_interfaces::msg::Time & time_msg,
rcl_clock_type_t ros_time)
{
rcl_time_ = init_time_point(ros_time);
if (time_msg.sec < 0) {
throw std::runtime_error("cannot store a negative time point in rclcpp::Time");
}
@ -121,14 +79,14 @@ Time::Time(const builtin_interfaces::msg::Time & time_msg) // NOLINT
rcl_time_.nanoseconds += time_msg.nanosec;
}
Time::Time(const rcl_time_point_t & time_point)
: rcl_time_(time_point)
{
// noop
}
Time::~Time()
{
if (rcl_time_source_fini(&rcl_time_source_) != RCL_RET_OK) {
RCUTILS_LOG_FATAL("failed to reclaim rcl_time_source_t in destructor of rclcpp::Time")
}
if (rcl_time_point_fini(&rcl_time_) != RCL_RET_OK) {
RCUTILS_LOG_FATAL("failed to reclaim rcl_time_point_t in destructor of rclcpp::Time")
}
}
Time::operator builtin_interfaces::msg::Time() const
@ -142,9 +100,7 @@ Time::operator builtin_interfaces::msg::Time() const
Time &
Time::operator=(const Time & rhs)
{
rcl_time_source_ = init_time_source(rhs.rcl_time_source_.type);
rcl_time_ = init_time_point(rcl_time_source_);
rcl_time_.nanoseconds = rhs.rcl_time_.nanoseconds;
rcl_time_ = rhs.rcl_time_;
return *this;
}
@ -155,8 +111,9 @@ Time::operator=(const builtin_interfaces::msg::Time & time_msg)
throw std::runtime_error("cannot store a negative time point in rclcpp::Time");
}
this->rcl_time_source_ = init_time_source();
this->rcl_time_ = init_time_point(this->rcl_time_source_);
rcl_clock_type_t ros_time = RCL_ROS_TIME;
rcl_time_ = init_time_point(ros_time); // TODO(tfoote) hard coded ROS here
rcl_time_.nanoseconds = RCL_S_TO_NS(static_cast<uint64_t>(time_msg.sec));
rcl_time_.nanoseconds += time_msg.nanosec;
@ -166,7 +123,7 @@ Time::operator=(const builtin_interfaces::msg::Time & time_msg)
bool
Time::operator==(const rclcpp::Time & rhs) const
{
if (rcl_time_.time_source->type != rhs.rcl_time_.time_source->type) {
if (rcl_time_.clock_type != rhs.rcl_time_.clock_type) {
throw std::runtime_error("can't compare times with different time sources");
}
@ -182,7 +139,7 @@ Time::operator!=(const rclcpp::Time & rhs) const
bool
Time::operator<(const rclcpp::Time & rhs) const
{
if (rcl_time_.time_source->type != rhs.rcl_time_.time_source->type) {
if (rcl_time_.clock_type != rhs.rcl_time_.clock_type) {
throw std::runtime_error("can't compare times with different time sources");
}
@ -192,7 +149,7 @@ Time::operator<(const rclcpp::Time & rhs) const
bool
Time::operator<=(const rclcpp::Time & rhs) const
{
if (rcl_time_.time_source->type != rhs.rcl_time_.time_source->type) {
if (rcl_time_.clock_type != rhs.rcl_time_.clock_type) {
throw std::runtime_error("can't compare times with different time sources");
}
@ -202,7 +159,7 @@ Time::operator<=(const rclcpp::Time & rhs) const
bool
Time::operator>=(const rclcpp::Time & rhs) const
{
if (rcl_time_.time_source->type != rhs.rcl_time_.time_source->type) {
if (rcl_time_.clock_type != rhs.rcl_time_.clock_type) {
throw std::runtime_error("can't compare times with different time sources");
}
@ -212,7 +169,7 @@ Time::operator>=(const rclcpp::Time & rhs) const
bool
Time::operator>(const rclcpp::Time & rhs) const
{
if (rcl_time_.time_source->type != rhs.rcl_time_.time_source->type) {
if (rcl_time_.clock_type != rhs.rcl_time_.clock_type) {
throw std::runtime_error("can't compare times with different time sources");
}
@ -220,33 +177,55 @@ Time::operator>(const rclcpp::Time & rhs) const
}
Time
Time::operator+(const rclcpp::Time & rhs) const
Time::operator+(const rclcpp::Duration & rhs) const
{
if (rcl_time_.time_source->type != rhs.rcl_time_.time_source->type) {
throw std::runtime_error("can't add times with different time sources");
}
auto ns = rcl_time_.nanoseconds + rhs.rcl_time_.nanoseconds;
if (ns < rcl_time_.nanoseconds) {
if (rhs.nanoseconds() > 0 && (uint64_t)rhs.nanoseconds() >
std::numeric_limits<rcl_time_point_value_t>::max() -
(rcl_time_point_value_t)this->nanoseconds())
{
throw std::overflow_error("addition leads to uint64_t overflow");
}
return Time(this->nanoseconds() + rhs.nanoseconds(), this->get_clock_type());
}
return Time(rcl_time_.nanoseconds + rhs.rcl_time_.nanoseconds);
Duration
Time::operator-(const rclcpp::Time & rhs) const
{
if (rcl_time_.clock_type != rhs.rcl_time_.clock_type) {
throw std::runtime_error("can't subtract times with different time sources");
}
if (rcl_time_.nanoseconds >
(uint64_t)std::numeric_limits<rcl_duration_value_t>::max() + rhs.rcl_time_.nanoseconds)
{
throw std::underflow_error("time subtraction leads to int64_t overflow");
}
if (rcl_time_.nanoseconds < rhs.rcl_time_.nanoseconds) {
rcl_time_point_value_t negative_delta = rhs.rcl_time_.nanoseconds - rcl_time_.nanoseconds;
if (negative_delta > (uint64_t) std::numeric_limits<rcl_duration_value_t>::min()) {
throw std::underflow_error("time subtraction leads to int64_t underflow");
}
}
return Duration(rcl_time_.nanoseconds - rhs.rcl_time_.nanoseconds);
}
Time
Time::operator-(const rclcpp::Time & rhs) const
Time::operator-(const rclcpp::Duration & rhs) const
{
if (rcl_time_.time_source->type != rhs.rcl_time_.time_source->type) {
throw std::runtime_error("can't add times with different time sources");
if (rhs.nanoseconds() > 0 && rcl_time_.nanoseconds >
std::numeric_limits<rcl_time_point_value_t>::max() - (uint64_t)rhs.nanoseconds())
{
throw std::underflow_error("time subtraction leads to uint64_t overflow");
}
if (rcl_time_.nanoseconds < (uint64_t) std::numeric_limits<rcl_duration_value_t>::max() &&
(int64_t)rcl_time_.nanoseconds < (int64_t)rhs.nanoseconds())
{
throw std::underflow_error("time subtraction leads to uint64_t underflow");
}
auto ns = rcl_time_.nanoseconds - rhs.rcl_time_.nanoseconds;
if (ns > rcl_time_.nanoseconds) {
throw std::underflow_error("subtraction leads to uint64_t underflow");
}
return Time(rcl_time_.nanoseconds - rhs.rcl_time_.nanoseconds);
return Time(rcl_time_.nanoseconds - rhs.nanoseconds(), rcl_time_.clock_type);
}
uint64_t
@ -255,4 +234,22 @@ Time::nanoseconds() const
return rcl_time_.nanoseconds;
}
rcl_clock_type_t
Time::get_clock_type() const
{
return rcl_time_.clock_type;
}
Time
operator+(const rclcpp::Duration & lhs, const rclcpp::Time & rhs)
{
if (rhs.nanoseconds() >
std::numeric_limits<rcl_time_point_value_t>::max() - (rcl_time_point_value_t)lhs.nanoseconds())
{
throw std::overflow_error("addition leads to uint64_t overflow");
}
return Time(lhs.nanoseconds() + rhs.nanoseconds(), rhs.get_clock_type());
}
} // namespace rclcpp

262
rclcpp/src/rclcpp/time_source.cpp Normal file
View file

@ -0,0 +1,262 @@
// Copyright 2017 Open Source Robotics Foundation, Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include <memory>
#include <utility>
#include "builtin_interfaces/msg/time.hpp"
#include "rcl/time.h"
#include "rcutils/logging_macros.h"
#include "rclcpp/clock.hpp"
#include "rclcpp/exceptions.hpp"
#include "rclcpp/node.hpp"
#include "rclcpp/time.hpp"
#include "rclcpp/time_source.hpp"
namespace rclcpp
{
TimeSource::TimeSource(std::shared_ptr<rclcpp::node::Node> node)
: ros_time_active_(false)
{
this->attachNode(node);
}
TimeSource::TimeSource()
: ros_time_active_(false)
{
}
void TimeSource::attachNode(std::shared_ptr<rclcpp::node::Node> node)
{
node_ = node;
// TODO(tfoote): Update QOS
clock_subscription_ = node_->create_subscription<builtin_interfaces::msg::Time>(
"clock", std::bind(&TimeSource::clock_cb, this, std::placeholders::_1),
rmw_qos_profile_default);
parameter_client_ = std::make_shared<rclcpp::parameter_client::AsyncParametersClient>(node);
parameter_subscription_ =
parameter_client_->on_parameter_event(std::bind(&TimeSource::on_parameter_event,
this, std::placeholders::_1));
}
void TimeSource::detachNode()
{
this->ros_time_active_ = false;
clock_subscription_.reset();
parameter_client_.reset();
node_.reset();
disable_ros_time();
}
void TimeSource::attachClock(std::shared_ptr<rclcpp::Clock> clock)
{
if (clock->get_clock_type() != RCL_ROS_TIME) {
throw std::invalid_argument("Cannot attach clock to a time source that's not a ROS clock");
}
std::lock_guard<std::mutex> guard(clock_list_lock_);
associated_clocks_.push_back(clock);
// Set the clock if there's already data for it
if (last_msg_set_) {
set_clock(last_msg_set_, ros_time_active_, clock);
}
}
void TimeSource::detachClock(std::shared_ptr<rclcpp::Clock> clock)
{
std::lock_guard<std::mutex> guard(clock_list_lock_);
auto result = std::find(associated_clocks_.begin(), associated_clocks_.end(), clock);
if (result != associated_clocks_.end()) {
associated_clocks_.erase(result);
} else {
RCUTILS_LOG_ERROR("Failed to remove clock");
}
}
TimeSource::~TimeSource()
{
if (node_) {
this->detachNode();
}
}
void TimeSource::set_clock(
const builtin_interfaces::msg::Time::SharedPtr msg, bool set_ros_time_enabled,
std::shared_ptr<rclcpp::Clock> clock)
{
// Compute diff
rclcpp::Time msg_time = rclcpp::Time(*msg);
rclcpp::Time now = clock->now();
auto diff = now - msg_time;
rclcpp::TimeJump jump;
jump.delta_.nanoseconds = diff.nanoseconds();
// Compute jump type
if (clock->ros_time_is_active()) {
if (set_ros_time_enabled) {
jump.jump_type_ = TimeJump::ClockChange_t::ROS_TIME_NO_CHANGE;
} else {
jump.jump_type_ = TimeJump::ClockChange_t::ROS_TIME_DEACTIVATED;
}
} else if (!clock->ros_time_is_active()) {
if (set_ros_time_enabled) {
jump.jump_type_ = TimeJump::ClockChange_t::ROS_TIME_ACTIVATED;
} else {
jump.jump_type_ = TimeJump::ClockChange_t::SYSTEM_TIME_NO_CHANGE;
}
}
if (jump.jump_type_ == TimeJump::ClockChange_t::SYSTEM_TIME_NO_CHANGE) {
// No change/no updates don't act.
return;
}
auto active_callbacks = clock->get_triggered_callback_handlers(jump);
clock->invoke_prejump_callbacks(active_callbacks);
// Do change
if (jump.jump_type_ == TimeJump::ClockChange_t::ROS_TIME_DEACTIVATED) {
disable_ros_time(clock);
} else if (jump.jump_type_ == TimeJump::ClockChange_t::ROS_TIME_ACTIVATED) {
enable_ros_time(clock);
}
if (jump.jump_type_ == TimeJump::ClockChange_t::ROS_TIME_ACTIVATED ||
jump.jump_type_ == TimeJump::ClockChange_t::ROS_TIME_NO_CHANGE)
{
auto ret = rcl_set_ros_time_override(&(clock->rcl_clock_), msg_time.nanoseconds());
if (ret != RCL_RET_OK) {
rclcpp::exceptions::throw_from_rcl_error(
ret, "Failed to set ros_time_override_status");
}
}
// Post change callbacks
clock->invoke_postjump_callbacks(active_callbacks, jump);
}
void TimeSource::clock_cb(const builtin_interfaces::msg::Time::SharedPtr msg)
{
if (!this->ros_time_active_) {
enable_ros_time();
}
// Cache the last message in case a new clock is attached.
last_msg_set_ = msg;
std::lock_guard<std::mutex> guard(clock_list_lock_);
for (auto it = associated_clocks_.begin(); it != associated_clocks_.end(); ++it) {
set_clock(msg, true, *it);
}
}
void TimeSource::on_parameter_event(const rcl_interfaces::msg::ParameterEvent::SharedPtr event)
{
for (auto & new_parameter : event->new_parameters) {
if (new_parameter.value.type == rclcpp::parameter::ParameterType::PARAMETER_BOOL) {
if (new_parameter.name == "use_sim_time") {
bool value = new_parameter.value.bool_value;
if (value) {
parameter_state_ = SET_TRUE;
enable_ros_time();
} else {
parameter_state_ = SET_FALSE;
disable_ros_time();
}
}
}
}
for (auto & changed_parameter : event->changed_parameters) {
if (changed_parameter.value.type == rclcpp::parameter::ParameterType::PARAMETER_BOOL) {
if (changed_parameter.name == "use_sim_time") {
bool value = changed_parameter.value.bool_value;
if (value) {
parameter_state_ = SET_TRUE;
enable_ros_time();
} else {
parameter_state_ = SET_FALSE;
disable_ros_time();
}
}
}
}
for (auto & deleted_parameter : event->deleted_parameters) {
if (deleted_parameter.name == "use_sim_time") {
// If the parameter is deleted mark it as unset but dont' change state.
parameter_state_ = UNSET;
}
}
}
void TimeSource::enable_ros_time(std::shared_ptr<rclcpp::Clock> clock)
{
auto ret = rcl_enable_ros_time_override(&clock->rcl_clock_);
if (ret != RCL_RET_OK) {
rclcpp::exceptions::throw_from_rcl_error(
ret, "Failed to enable ros_time_override_status");
}
}
void TimeSource::disable_ros_time(std::shared_ptr<rclcpp::Clock> clock)
{
auto ret = rcl_disable_ros_time_override(&clock->rcl_clock_);
if (ret != RCL_RET_OK) {
rclcpp::exceptions::throw_from_rcl_error(
ret, "Failed to enable ros_time_override_status");
}
}
void TimeSource::enable_ros_time()
{
if (ros_time_active_) {
// already enabled no-op
return;
}
// Local storage
ros_time_active_ = true;
// Update all attached clocks
std::lock_guard<std::mutex> guard(clock_list_lock_);
for (auto it = associated_clocks_.begin(); it != associated_clocks_.end(); ++it) {
auto msg = std::make_shared<builtin_interfaces::msg::Time>();
msg->sec = 0;
msg->nanosec = 0;
set_clock(msg, true, *it);
}
}
void TimeSource::disable_ros_time()
{
if (!ros_time_active_) {
// already disabled no-op
return;
}
// Local storage
ros_time_active_ = false;
// Update all attached clocks
std::lock_guard<std::mutex> guard(clock_list_lock_);
for (auto it = associated_clocks_.begin(); it != associated_clocks_.end(); ++it) {
auto msg = std::make_shared<builtin_interfaces::msg::Time>();
set_clock(msg, false, *it);
}
}
} // namespace rclcpp

88
rclcpp/test/test_duration.cpp Normal file
View file

@ -0,0 +1,88 @@
// Copyright 2017 Open Source Robotics Foundation, Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include <gtest/gtest.h>
#include <algorithm>
#include <chrono>
#include <limits>
#include <string>
#include "rcl/error_handling.h"
#include "rcl/time.h"
#include "rclcpp/clock.hpp"
#include "rclcpp/rclcpp.hpp"
#include "rclcpp/duration.hpp"
using namespace std::chrono_literals;
class TestDuration : public ::testing::Test
{
};
// TEST(TestDuration, conversions) {
// TODO(tfoote) Implement conversion methods
// }
TEST(TestDuration, operators) {
rclcpp::Duration old(1, 0);
rclcpp::Duration young(2, 0);
EXPECT_TRUE(old < young);
EXPECT_TRUE(young > old);
EXPECT_TRUE(old <= young);
EXPECT_TRUE(young >= old);
EXPECT_FALSE(young == old);
rclcpp::Duration add = old + young;
EXPECT_EQ(add.nanoseconds(), (rcl_duration_value_t)(old.nanoseconds() + young.nanoseconds()));
EXPECT_EQ(add, old + young);
rclcpp::Duration sub = young - old;
EXPECT_EQ(sub.nanoseconds(), (rcl_duration_value_t)(young.nanoseconds() - old.nanoseconds()));
EXPECT_EQ(sub, young - old);
rclcpp::Duration time = rclcpp::Duration(0, 0);
rclcpp::Duration copy_constructor_duration(time);
rclcpp::Duration assignment_op_duration = rclcpp::Duration(1, 0);
assignment_op_duration = time;
EXPECT_TRUE(time == copy_constructor_duration);
EXPECT_TRUE(time == assignment_op_duration);
}
TEST(TestDuration, chrono_overloads) {
int64_t ns = 123456789l;
auto chrono_ns = std::chrono::nanoseconds(ns);
auto d1 = rclcpp::Duration(ns);
auto d2 = rclcpp::Duration(chrono_ns);
auto d3 = rclcpp::Duration(123456789ns);
EXPECT_EQ(d1, d2);
EXPECT_EQ(d1, d3);
EXPECT_EQ(d2, d3);
}
TEST(TestDuration, overflows) {
rclcpp::Duration max(std::numeric_limits<rcl_duration_value_t>::max());
rclcpp::Duration min(std::numeric_limits<rcl_duration_value_t>::min());
rclcpp::Duration one(1);
rclcpp::Duration negative_one(-1);
EXPECT_THROW(max + one, std::overflow_error);
EXPECT_THROW(min - one, std::underflow_error);
EXPECT_THROW(negative_one + min, std::underflow_error);
EXPECT_THROW(negative_one - max, std::underflow_error);
}

59
rclcpp/test/test_time.cpp
View file

@ -20,6 +20,7 @@
#include "rcl/error_handling.h"
#include "rcl/time.h"
#include "rclcpp/clock.hpp"
#include "rclcpp/rclcpp.hpp"
#include "rclcpp/time.hpp"
@ -32,27 +33,39 @@ protected:
}
};
TEST(TestTime, clock_type_access) {
rclcpp::Clock ros_clock(RCL_ROS_TIME);
EXPECT_EQ(RCL_ROS_TIME, ros_clock.get_clock_type());
rclcpp::Clock system_clock(RCL_SYSTEM_TIME);
EXPECT_EQ(RCL_SYSTEM_TIME, system_clock.get_clock_type());
rclcpp::Clock steady_clock(RCL_STEADY_TIME);
EXPECT_EQ(RCL_STEADY_TIME, steady_clock.get_clock_type());
}
TEST(TestTime, time_sources) {
using builtin_interfaces::msg::Time;
// TODO(Karsten1987): Fix this test once ROS_TIME is implemented
EXPECT_ANY_THROW(rclcpp::Time::now(RCL_ROS_TIME));
rclcpp::Clock ros_clock(RCL_ROS_TIME);
Time ros_now = ros_clock.now();
EXPECT_NE(0, ros_now.sec);
EXPECT_NE(0u, ros_now.nanosec);
Time system_now = rclcpp::Time::now(RCL_SYSTEM_TIME);
rclcpp::Clock system_clock(RCL_ROS_TIME);
Time system_now = system_clock.now();
EXPECT_NE(0, system_now.sec);
EXPECT_NE(0u, system_now.nanosec);
Time steady_now = rclcpp::Time::now(RCL_STEADY_TIME);
rclcpp::Clock steady_clock(RCL_STEADY_TIME);
Time steady_now = steady_clock.now();
EXPECT_NE(0, steady_now.sec);
EXPECT_NE(0u, steady_now.nanosec);
// default
Time default_now = rclcpp::Time::now();
EXPECT_NE(0, default_now.sec);
EXPECT_NE(0u, default_now.nanosec);
}
TEST(TestTime, conversions) {
rclcpp::Time now = rclcpp::Time::now();
rclcpp::Clock system_clock(RCL_ROS_TIME);
rclcpp::Time now = system_clock.now();
builtin_interfaces::msg::Time now_msg = now;
rclcpp::Time now_again = now_msg;
@ -95,12 +108,8 @@ TEST(TestTime, operators) {
EXPECT_FALSE(young == old);
EXPECT_TRUE(young != old);
rclcpp::Time add = old + young;
EXPECT_EQ(add.nanoseconds(), old.nanoseconds() + young.nanoseconds());
EXPECT_EQ(add, old + young);
rclcpp::Time sub = young - old;
EXPECT_EQ(sub.nanoseconds(), young.nanoseconds() - old.nanoseconds());
rclcpp::Duration sub = young - old;
EXPECT_EQ(sub.nanoseconds(), (rcl_duration_value_t)(young.nanoseconds() - old.nanoseconds()));
EXPECT_EQ(sub, young - old);
rclcpp::Time system_time(0, 0, RCL_SYSTEM_TIME);
@ -112,11 +121,13 @@ TEST(TestTime, operators) {
EXPECT_ANY_THROW((void)(system_time >= steady_time));
EXPECT_ANY_THROW((void)(system_time < steady_time));
EXPECT_ANY_THROW((void)(system_time > steady_time));
EXPECT_ANY_THROW((void)(system_time + steady_time));
EXPECT_ANY_THROW((void)(system_time - steady_time));
rclcpp::Time now = rclcpp::Time::now(RCL_SYSTEM_TIME);
rclcpp::Time later = rclcpp::Time::now(RCL_STEADY_TIME);
rclcpp::Clock system_clock(RCL_ROS_TIME);
rclcpp::Clock steady_clock(RCL_STEADY_TIME);
rclcpp::Time now = system_clock.now();
rclcpp::Time later = steady_clock.now();
EXPECT_ANY_THROW((void)(now == later));
EXPECT_ANY_THROW((void)(now != later));
@ -124,7 +135,6 @@ TEST(TestTime, operators) {
EXPECT_ANY_THROW((void)(now >= later));
EXPECT_ANY_THROW((void)(now < later));
EXPECT_ANY_THROW((void)(now > later));
EXPECT_ANY_THROW((void)(now + later));
EXPECT_ANY_THROW((void)(now - later));
for (auto time_source : {RCL_ROS_TIME, RCL_SYSTEM_TIME, RCL_STEADY_TIME}) {
@ -139,9 +149,10 @@ TEST(TestTime, operators) {
}
TEST(TestTime, overflows) {
rclcpp::Time max(std::numeric_limits<uint64_t>::max());
rclcpp::Time one(1);
rclcpp::Time max_time(std::numeric_limits<rcl_time_point_value_t>::max());
rclcpp::Time min_time(std::numeric_limits<rcl_time_point_value_t>::min());
rclcpp::Duration one(1);
EXPECT_THROW(max + one, std::overflow_error);
EXPECT_THROW(one - max, std::underflow_error);
EXPECT_THROW(max_time + one, std::overflow_error);
EXPECT_THROW(min_time - one, std::underflow_error);
}

274
rclcpp/test/test_time_source.cpp Normal file
View file

@ -0,0 +1,274 @@
// Copyright 2017 Open Source Robotics Foundation, Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include <gtest/gtest.h>
#include <algorithm>
#include <limits>
#include <memory>
#include <string>
#include "rcl/error_handling.h"
#include "rcl/time.h"
#include "rclcpp/clock.hpp"
#include "rclcpp/rclcpp.hpp"
#include "rclcpp/time.hpp"
#include "rclcpp/time_source.hpp"
class TestTimeSource : public ::testing::Test
{
protected:
static void SetUpTestCase()
{
rclcpp::init(0, nullptr);
}
void SetUp()
{
node = std::make_shared<rclcpp::node::Node>("my_node");
}
void TearDown()
{
node.reset();
}
rclcpp::node::Node::SharedPtr node;
};
TEST_F(TestTimeSource, detachUnattached) {
rclcpp::TimeSource ts;
ASSERT_NO_THROW(ts.detachNode());
// Try multiple detach to see if error
ASSERT_NO_THROW(ts.detachNode());
}
TEST_F(TestTimeSource, reattach) {
rclcpp::TimeSource ts;
// Try reattach
ASSERT_NO_THROW(ts.attachNode(node));
ASSERT_NO_THROW(ts.attachNode(node));
}
TEST_F(TestTimeSource, ROS_time_valid) {
rclcpp::TimeSource ts;
auto ros_clock = std::make_shared<rclcpp::Clock>(RCL_ROS_TIME);
EXPECT_FALSE(ros_clock->ros_time_is_active());
ts.attachClock(ros_clock);
auto now = ros_clock->now();
EXPECT_FALSE(ros_clock->ros_time_is_active());
ts.attachNode(node);
EXPECT_FALSE(ros_clock->ros_time_is_active());
ts.detachNode();
EXPECT_FALSE(ros_clock->ros_time_is_active());
ts.attachNode(node);
EXPECT_FALSE(ros_clock->ros_time_is_active());
}
TEST_F(TestTimeSource, clock) {
rclcpp::TimeSource ts(node);
auto ros_clock = std::make_shared<rclcpp::Clock>(RCL_ROS_TIME);
EXPECT_FALSE(ros_clock->ros_time_is_active());
ts.attachClock(ros_clock);
EXPECT_FALSE(ros_clock->ros_time_is_active());
auto clock_pub = node->create_publisher<builtin_interfaces::msg::Time>("clock",
rmw_qos_profile_default);
rclcpp::WallRate loop_rate(50);
for (int i = 0; i < 5; ++i) {
if (!rclcpp::ok()) {
break; // Break for ctrl-c
}
auto msg = std::make_shared<builtin_interfaces::msg::Time>();
msg->sec = i;
msg->nanosec = 1000;
clock_pub->publish(msg);
// std::cout << "Publishing: '" << msg->sec << ".000000" << msg->nanosec << "'" << std::endl;
rclcpp::spin_some(node);
loop_rate.sleep();
}
auto t_low = rclcpp::Time(1, 0, RCL_ROS_TIME);
auto t_high = rclcpp::Time(10, 100000, RCL_ROS_TIME);
// Now that we've recieved a message it should be active with parameter unset
EXPECT_TRUE(ros_clock->ros_time_is_active());
auto t_out = ros_clock->now();
EXPECT_NE(0UL, t_out.nanoseconds());
EXPECT_LT(t_low.nanoseconds(), t_out.nanoseconds());
EXPECT_GT(t_high.nanoseconds(), t_out.nanoseconds());
}
class CallbackObject
{
public:
CallbackObject()
: last_precallback_id_(0),
last_postcallback_id_(0)
{}
int last_precallback_id_;
void pre_callback(int id) {last_precallback_id_ = id;}
int last_postcallback_id_;
rclcpp::TimeJump last_timejump_;
void post_callback(const rclcpp::TimeJump & jump, int id)
{
last_postcallback_id_ = id; last_timejump_ = jump;
}
};
TEST_F(TestTimeSource, callbacks) {
CallbackObject cbo;
rclcpp::JumpThreshold jump_threshold;
jump_threshold.min_forward_ = 0;
jump_threshold.min_backward_ = 0;
jump_threshold.on_clock_change_ = true;
rclcpp::TimeSource ts(node);
auto ros_clock = std::make_shared<rclcpp::Clock>(RCL_ROS_TIME);
// Register a callback for time jumps
rclcpp::JumpHandler::SharedPtr callback_holder = ros_clock->create_jump_callback(
std::bind(&CallbackObject::pre_callback, &cbo, 1),
std::bind(&CallbackObject::post_callback, &cbo, std::placeholders::_1, 1),
jump_threshold);
EXPECT_EQ(0, cbo.last_precallback_id_);
EXPECT_EQ(0, cbo.last_postcallback_id_);
EXPECT_FALSE(ros_clock->ros_time_is_active());
ts.attachClock(ros_clock);
EXPECT_FALSE(ros_clock->ros_time_is_active());
auto clock_pub = node->create_publisher<builtin_interfaces::msg::Time>("clock",
rmw_qos_profile_default);
rclcpp::WallRate loop_rate(50);
for (int i = 0; i < 5; ++i) {
if (!rclcpp::ok()) {
break; // Break for ctrl-c
}
auto msg = std::make_shared<builtin_interfaces::msg::Time>();
msg->sec = i;
msg->nanosec = 1000;
clock_pub->publish(msg);
// std::cout << "Publishing: '" << msg->sec << ".000000" << msg->nanosec << "'" << std::endl;
rclcpp::spin_some(node);
loop_rate.sleep();
}
auto t_low = rclcpp::Time(1, 0, RCL_ROS_TIME);
auto t_high = rclcpp::Time(10, 100000, RCL_ROS_TIME);
EXPECT_EQ(1, cbo.last_precallback_id_);
EXPECT_EQ(1, cbo.last_postcallback_id_);
// Now that we've recieved a message it should be active with parameter unset
EXPECT_TRUE(ros_clock->ros_time_is_active());
auto t_out = ros_clock->now();
EXPECT_NE(0UL, t_out.nanoseconds());
EXPECT_LT(t_low.nanoseconds(), t_out.nanoseconds());
EXPECT_GT(t_high.nanoseconds(), t_out.nanoseconds());
// Change callbacks
callback_holder = ros_clock->create_jump_callback(
std::bind(&CallbackObject::pre_callback, &cbo, 2),
std::bind(&CallbackObject::post_callback, &cbo, std::placeholders::_1, 2),
jump_threshold);
for (int i = 0; i < 5; ++i) {
if (!rclcpp::ok()) {
break; // Break for ctrl-c
}
auto msg = std::make_shared<builtin_interfaces::msg::Time>();
msg->sec = i;
msg->nanosec = 2000;
clock_pub->publish(msg);
// std::cout << "Publishing: '" << msg->sec << ".000000" << msg->nanosec << "'" << std::endl;
rclcpp::spin_some(node);
loop_rate.sleep();
}
EXPECT_EQ(2, cbo.last_precallback_id_);
EXPECT_EQ(2, cbo.last_postcallback_id_);
// Now that we've recieved a message it should be active with parameter unset
EXPECT_TRUE(ros_clock->ros_time_is_active());
t_out = ros_clock->now();
EXPECT_NE(0UL, t_out.nanoseconds());
EXPECT_LT(t_low.nanoseconds(), t_out.nanoseconds());
EXPECT_GT(t_high.nanoseconds(), t_out.nanoseconds());
}
TEST_F(TestTimeSource, parameter_activation) {
rclcpp::TimeSource ts(node);
auto ros_clock = std::make_shared<rclcpp::Clock>(RCL_ROS_TIME);
EXPECT_FALSE(ros_clock->ros_time_is_active());
ts.attachClock(ros_clock);
EXPECT_FALSE(ros_clock->ros_time_is_active());
auto parameter_service = std::make_shared<rclcpp::parameter_service::ParameterService>(node);
auto parameters_client = std::make_shared<rclcpp::parameter_client::SyncParametersClient>(node);
using namespace std::chrono_literals;
EXPECT_TRUE(parameters_client->wait_for_service(2s));
auto set_parameters_results = parameters_client->set_parameters({
rclcpp::parameter::ParameterVariant("use_sim_time", true)
});
for (auto & result : set_parameters_results) {
EXPECT_TRUE(result.successful);
}
rclcpp::spin_some(node);
EXPECT_TRUE(ros_clock->ros_time_is_active());
set_parameters_results = parameters_client->set_parameters({
rclcpp::parameter::ParameterVariant("use_sim_time", rclcpp::parameter::PARAMETER_NOT_SET)
});
for (auto & result : set_parameters_results) {
EXPECT_TRUE(result.successful);
}
EXPECT_TRUE(ros_clock->ros_time_is_active());
set_parameters_results = parameters_client->set_parameters({
rclcpp::parameter::ParameterVariant("use_sim_time", false)
});
for (auto & result : set_parameters_results) {
EXPECT_TRUE(result.successful);
}
EXPECT_FALSE(ros_clock->ros_time_is_active());
set_parameters_results = parameters_client->set_parameters({
rclcpp::parameter::ParameterVariant("use_sim_time", rclcpp::parameter::PARAMETER_NOT_SET)
});
for (auto & result : set_parameters_results) {
EXPECT_TRUE(result.successful);
}
EXPECT_FALSE(ros_clock->ros_time_is_active());
}