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

Publish parameter events

Browse source
This commit is contained in:
Esteve Fernandez 2015-06-08 09:52:15 -07:00
parent 5c6c61662a
commit dc82ef75c0
3 changed files with 40 additions and 12 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -22,6 +22,7 @@
#include <rcl_interfaces/msg/list_parameters_result.hpp>
#include <rcl_interfaces/msg/parameter_descriptor.hpp>
#include <rcl_interfaces/msg/parameter_event.hpp>
#include <rcl_interfaces/msg/set_parameters_result.hpp>
#include <rclcpp/callback_group.hpp>
@ -201,6 +202,8 @@ private:
std::map<std::string, rclcpp::parameter::ParameterVariant> parameters_;
publisher::Publisher::SharedPtr events_publisher_;
template<
typename ServiceT,
typename FunctorT,

26
rclcpp/include/rclcpp/node_impl.hpp
View file

@ -46,6 +46,9 @@ Node::Node(std::string node_name, context::Context::SharedPtr context)
using rclcpp::callback_group::CallbackGroupType;
default_callback_group_ = \
create_callback_group(CallbackGroupType::MutuallyExclusive);
// TODO(esteve): remove hardcoded values
events_publisher_ =
create_publisher<rcl_interfaces::msg::ParameterEvent>("parameter_events", 1000);
}
rclcpp::callback_group::CallbackGroup::SharedPtr
@ -217,13 +220,9 @@ std::vector<rcl_interfaces::msg::SetParametersResult>
Node::set_parameters(
const std::vector<rclcpp::parameter::ParameterVariant> & parameters)
{
std::lock_guard<std::mutex> lock(mutex_);
std::vector<rcl_interfaces::msg::SetParametersResult> results;
for (auto p : parameters) {
parameters_[p.get_name()] = p;
rcl_interfaces::msg::SetParametersResult result;
result.successful = true;
// TODO: handle parameter constraints
auto result = set_parameters_atomically({{p}});
results.push_back(result);
}
return results;
@ -235,14 +234,31 @@ Node::set_parameters_atomically(
{
std::lock_guard<std::mutex> lock(mutex_);
std::map<std::string, rclcpp::parameter::ParameterVariant> tmp_map;
auto parameter_event = std::make_shared<rcl_interfaces::msg::ParameterEvent>();
for (auto p : parameters) {
if (parameters_.find(p.get_name()) == parameters_.end()) {
if (p.get_type() != rclcpp::parameter::ParameterType::PARAMETER_NOT_SET) {
parameter_event->new_parameters.push_back(p.to_parameter());
}
} else if (p.get_type() != rclcpp::parameter::ParameterType::PARAMETER_NOT_SET) {
parameter_event->changed_parameters.push_back(p.to_parameter());
} else {
parameter_event->deleted_parameters.push_back(p.to_parameter());
}
tmp_map[p.get_name()] = p;
}
// std::map::insert will not overwrite elements, so we'll keep the new
// ones and add only those that already exist in the Node's internal map
tmp_map.insert(parameters_.begin(), parameters_.end());
std::swap(tmp_map, parameters_);
// TODO: handle parameter constraints
rcl_interfaces::msg::SetParametersResult result;
result.successful = true;
events_publisher_->publish(parameter_event);
return result;
}

23
rclcpp/include/rclcpp/parameter_client.hpp
View file

@ -25,6 +25,7 @@
#include <rclcpp/parameter.hpp>
#include <rcl_interfaces/msg/parameter.hpp>
#include <rcl_interfaces/msg/parameter_event.hpp>
#include <rcl_interfaces/msg/parameter_value.hpp>
#include <rcl_interfaces/srv/describe_parameters.hpp>
#include <rcl_interfaces/srv/get_parameters.hpp>
@ -136,9 +137,8 @@ public:
std::shared_future<std::vector<rcl_interfaces::msg::SetParametersResult>>
set_parameters(
std::vector<rclcpp::parameter::ParameterVariant> parameters,
std::function<
void(std::shared_future<std::vector<rcl_interfaces::msg::SetParametersResult>>)
> callback = nullptr)
std::function<void(std::shared_future<std::vector<rcl_interfaces::msg::SetParametersResult>>)> callback =
nullptr)
{
std::promise<std::vector<rcl_interfaces::msg::SetParametersResult>> promise_result;
auto future_result = promise_result.get_future().share();
@ -166,8 +166,8 @@ public:
std::shared_future<rcl_interfaces::msg::SetParametersResult>
set_parameters_atomically(
std::vector<rclcpp::parameter::ParameterVariant> parameters,
std::function<void(
std::shared_future<rcl_interfaces::msg::SetParametersResult>)> callback = nullptr)
std::function<void(std::shared_future<rcl_interfaces::msg::SetParametersResult>)> callback =
nullptr)
{
std::promise<rcl_interfaces::msg::SetParametersResult> promise_result;
auto future_result = promise_result.get_future().share();
@ -196,8 +196,8 @@ public:
list_parameters(
std::vector<std::string> prefixes,
uint64_t depth,
std::function<void(
std::shared_future<rcl_interfaces::msg::ListParametersResult>)> callback = nullptr)
std::function<void(std::shared_future<rcl_interfaces::msg::ListParametersResult>)> callback =
nullptr)
{
std::promise<rcl_interfaces::msg::ListParametersResult> promise_result;
auto future_result = promise_result.get_future().share();
@ -220,6 +220,15 @@ public:
return future_result;
}
template<typename FunctorT>
typename rclcpp::subscription::Subscription<rcl_interfaces::msg::ParameterEvent>::SharedPtr
on_parameter_event(FunctorT callback)
{
// TODO(esteve): remove hardcoded values
return node_->create_subscription<rcl_interfaces::msg::ParameterEvent>("parameter_events",
1000, callback);
}
private:
const rclcpp::node::Node::SharedPtr node_;
rclcpp::client::Client<rcl_interfaces::srv::GetParameters>::SharedPtr get_parameters_client_;