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

Add callback handler for use_sim_time parameter #802 (#875)

Browse source 
* Add callback handler for use_sim_time parameter #802

Signed-off-by: Brian Ezequiel Marchi <brian.marchi65@gmail.com>
This commit is contained in:
Brian Marchi 2019-10-01 10:14:55 -03:00 • committed by GitHub
parent 4afd1cd5ad
commit 9723576821
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
3 changed files with 30 additions and 7 deletions
Download patch file Download diff file Expand all files Collapse all files

3
rclcpp/include/rclcpp/time_source.hpp
View file

@ -25,6 +25,7 @@
#include "rcl_interfaces/msg/parameter_event.hpp"
#include "rclcpp/node.hpp"
#include "rclcpp/node_interfaces/node_parameters_interface.hpp"
namespace rclcpp
@ -133,6 +134,8 @@ private:
std::mutex clock_list_lock_;
// A vector to store references to associated clocks.
std::vector<rclcpp::Clock::SharedPtr> associated_clocks_;
// A handler for the use_sim_time parameter callback.
rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr sim_time_cb_handler_ = nullptr;
};
} // namespace rclcpp

28
rclcpp/src/rclcpp/time_source.cpp
View file

@ -82,7 +82,7 @@ void TimeSource::attachNode(
// Though this defaults to false, it can be overridden by initial parameter values for the node,
// which may be given by the user at the node's construction or even by command-line arguments.
rclcpp::ParameterValue use_sim_time_param;
const char * use_sim_time_name = "use_sim_time";
const std::string use_sim_time_name = "use_sim_time";
if (!node_parameters_->has_parameter(use_sim_time_name)) {
use_sim_time_param = node_parameters_->declare_parameter(
use_sim_time_name,
@ -98,12 +98,26 @@ void TimeSource::attachNode(
create_clock_sub();
}
} else {
// TODO(wjwwood): use set_on_parameters_set_callback to catch the type mismatch,
// before the use_sim_time parameter can ever be set to an invalid value
RCLCPP_ERROR(
logger_, "Invalid type '%s' for parameter 'use_sim_time', should be 'bool'",
rclcpp::to_string(use_sim_time_param.get_type()).c_str());
}
sim_time_cb_handler_ = node_parameters_->add_on_set_parameters_callback(
[use_sim_time_name](const std::vector<rclcpp::Parameter> & parameters) {
rcl_interfaces::msg::SetParametersResult result;
result.successful = true;
for (const auto & parameter : parameters) {
if (
parameter.get_name() == use_sim_time_name &&
parameter.get_type() != rclcpp::PARAMETER_BOOL)
{
result.successful = false;
result.reason = "'" + use_sim_time_name + "' must be a bool";
break;
}
}
return result;
});
// TODO(tfoote) use parameters interface not subscribe to events via topic ticketed #609
parameter_subscription_ = rclcpp::AsyncParametersClient::on_parameter_event(
@ -122,6 +136,10 @@ void TimeSource::detachNode()
node_services_.reset();
node_logging_.reset();
node_clock_.reset();
if (sim_time_cb_handler_ && node_parameters_) {
node_parameters_->remove_on_set_parameters_callback(sim_time_cb_handler_.get());
}
sim_time_cb_handler_.reset();
node_parameters_.reset();
disable_ros_time();
}
@ -231,10 +249,6 @@ void TimeSource::on_parameter_event(const rcl_interfaces::msg::ParameterEvent::S
{rclcpp::ParameterEventsFilter::EventType::NEW,
rclcpp::ParameterEventsFilter::EventType::CHANGED});
for (auto & it : filter.get_events()) {
if (it.second->value.type != ParameterType::PARAMETER_BOOL) {
RCLCPP_ERROR(logger_, "use_sim_time parameter cannot be set to anything but a bool");
continue;
}
if (it.second->value.bool_value) {
parameter_state_ = SET_TRUE;
enable_ros_time();

6
rclcpp/test/test_time_source.cpp
View file

@ -242,6 +242,12 @@ TEST_F(TestTimeSource, ROS_time_valid_sim_time) {
EXPECT_TRUE(ros_clock2->ros_time_is_active());
}
TEST_F(TestTimeSource, ROS_invalid_sim_time) {
rclcpp::TimeSource ts;
ts.attachNode(node);
EXPECT_FALSE(node->set_parameter(rclcpp::Parameter("use_sim_time", "not boolean")).successful);
}
TEST_F(TestTimeSource, clock) {
rclcpp::TimeSource ts(node);
auto ros_clock = std::make_shared<rclcpp::Clock>(RCL_ROS_TIME);