Skip to content

Commit

Permalink
Merge pull request #30 from ros2/internal-parameters-api
Browse files Browse the repository at this point in the history 
Internal parameters api
  • Loading branch information
@esteve
esteve committed May 19, 2015
2 parents 198b40a + 1e51642 commit de2f707
Show file tree
Hide file tree
Showing 5 changed files with 360 additions and 155 deletions.
27 changes: 23 additions & 4 deletions rclcpp/include/rclcpp/node.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -24,11 +24,13 @@
#include <rclcpp/client.hpp>
#include <rclcpp/context.hpp>
#include <rclcpp/macros.hpp>
#include <rclcpp/parameter.hpp>
#include <rclcpp/publisher.hpp>
#include <rclcpp/service.hpp>
#include <rclcpp/subscription.hpp>
#include <rclcpp/timer.hpp>


// Forward declaration of ROS middleware class
namespace rmw
{
Expand Down Expand Up @@ -146,6 +148,24 @@ class Node
FunctorT callback,
rclcpp::callback_group::CallbackGroup::SharedPtr group = nullptr);

const std::vector<rcl_interfaces::SetParametersResult> set_parameters(
const std::vector<rcl_interfaces::Parameter> & parameters);

const rcl_interfaces::SetParametersResult set_parameters_atomically(
const std::vector<rcl_interfaces::Parameter> & parameters);

const std::vector<rclcpp::parameter::ParameterVariant> get_parameters(
const std::vector<std::string> & names);

const std::vector<rcl_interfaces::ParameterDescriptor> describe_parameters(
const std::vector<std::string> & names);

const std::vector<uint8_t> get_parameter_types(
const std::vector<std::string> & names);

const std::vector<rcl_interfaces::ListParametersResult> list_parameters(
const std::vector<std::string> & prefixes, uint64_t depth);

private:
RCLCPP_DISABLE_COPY(Node);

Expand All @@ -166,10 +186,9 @@ class Node
size_t number_of_services_;
size_t number_of_clients_;

void register_service(
const std::string & service_name,
std::shared_ptr<rclcpp::service::ServiceBase> serv_base_ptr,
rclcpp::callback_group::CallbackGroup::SharedPtr group);
std::mutex mutex_;

std::map<std::string, rclcpp::parameter::ParameterVariant> parameters_;

template<
typename ServiceT,
Expand Down
122 changes: 122 additions & 0 deletions rclcpp/include/rclcpp/node_impl.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -15,6 +15,7 @@
#ifndef RCLCPP_RCLCPP_NODE_IMPL_HPP_
#define RCLCPP_RCLCPP_NODE_IMPL_HPP_

#include <algorithm>
#include <memory>
#include <string>

Expand Down Expand Up @@ -212,4 +213,125 @@ Node::create_service(
return serv;
}

const std::vector<rcl_interfaces::SetParametersResult>
Node::set_parameters(
const std::vector<rcl_interfaces::Parameter> & parameters)
{
std::lock_guard<std::mutex> lock(mutex_);
std::vector<rcl_interfaces::SetParametersResult> results;
for (auto p : parameters) {
parameters_[p.name] = rclcpp::parameter::ParameterVariant::from_parameter(p);
rcl_interfaces::SetParametersResult result;
result.successful = true;
// TODO: handle parameter constraints
results.push_back(result);
}
return results;
}

const rcl_interfaces::SetParametersResult
Node::set_parameters_atomically(
const std::vector<rcl_interfaces::Parameter> & parameters)
{
std::lock_guard<std::mutex> lock(mutex_);
std::map<std::string, rclcpp::parameter::ParameterVariant> tmp_map;
for (auto p : parameters) {
tmp_map[p.name] = rclcpp::parameter::ParameterVariant::from_parameter(p);
}
tmp_map.insert(parameters_.begin(), parameters_.end());
std::swap(tmp_map, parameters_);
// TODO: handle parameter constraints
rcl_interfaces::SetParametersResult result;
result.successful = true;
return result;
}

const std::vector<rclcpp::parameter::ParameterVariant>
Node::get_parameters(
const std::vector<std::string> & names)
{
std::lock_guard<std::mutex> lock(mutex_);
std::vector<rclcpp::parameter::ParameterVariant> results;
for (auto & kv : parameters_) {
if (std::any_of(names.cbegin(), names.cend(), [&kv](const std::string & name) {
return name == kv.first;
}))
{
results.push_back(kv.second);
}
}
return results;
}

const std::vector<rcl_interfaces::ParameterDescriptor>
Node::describe_parameters(
const std::vector<std::string> & names)
{
std::lock_guard<std::mutex> lock(mutex_);
std::vector<rcl_interfaces::ParameterDescriptor> results;
for (auto & kv : parameters_) {
if (std::any_of(names.cbegin(), names.cend(), [&kv](const std::string & name) {
return name == kv.first;
}))
{
rcl_interfaces::ParameterDescriptor parameter_descriptor;
parameter_descriptor.name = kv.first;
parameter_descriptor.parameter_type = kv.second.get_type();
results.push_back(parameter_descriptor);
}
}
return results;
}

const std::vector<uint8_t>
Node::get_parameter_types(
const std::vector<std::string> & names)
{
std::lock_guard<std::mutex> lock(mutex_);
std::vector<uint8_t> results;
for (auto & kv : parameters_) {
if (std::any_of(names.cbegin(), names.cend(), [&kv](const std::string & name) {
return name == kv.first;
}))
{
results.push_back(kv.second.get_type());
} else {
results.push_back(rcl_interfaces::ParameterType::PARAMETER_NOT_SET);
}
}
return results;
}

const std::vector<rcl_interfaces::ListParametersResult>
Node::list_parameters(
const std::vector<std::string> & prefixes, uint64_t depth)
{
std::lock_guard<std::mutex> lock(mutex_);
std::vector<rcl_interfaces::ListParametersResult> results;

// TODO: define parameter separator, use "." for now
for (auto & kv : parameters_) {
if (std::any_of(prefixes.cbegin(), prefixes.cend(), [&kv, &depth](const std::string & prefix) {
if (kv.first.find(prefix + ".") == 0) {
size_t length = prefix.length();
std::string substr = kv.first.substr(length);
return std::count(substr.begin(), substr.end(), '.') < depth;
}
return false;
}))
{
rcl_interfaces::ListParametersResult result;
result.parameter_names.push_back(kv.first);
size_t last_separator = kv.first.find_last_of('.');
std::string prefix = kv.first.substr(0, last_separator);
if (std::find(result.parameter_prefixes.cbegin(), result.parameter_prefixes.cend(),
prefix) == result.parameter_prefixes.cend())
{
result.parameter_prefixes.push_back(prefix);
}
results.push_back(result);
}
}
return results;
}
#endif /* RCLCPP_RCLCPP_NODE_IMPL_HPP_ */
171 changes: 20 additions & 151 deletions rclcpp/include/rclcpp/parameter.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -19,10 +19,6 @@

#include <rmw/rmw.h>

#include <rclcpp/executors.hpp>
#include <rclcpp/macros.hpp>
#include <rclcpp/node.hpp>

#include <rcl_interfaces/GetParameters.h>
#include <rcl_interfaces/GetParameterTypes.h>
#include <rcl_interfaces/Parameter.h>
Expand Down Expand Up @@ -174,156 +170,29 @@ class ParameterVariant
return get_value<ParameterType::PARAMETER_BYTES>();
}

private:
std::string name_;
rcl_interfaces::ParameterValue value_;
};

class AsyncParametersClient
{

public:
RCLCPP_MAKE_SHARED_DEFINITIONS(AsyncParametersClient);

AsyncParametersClient(const rclcpp::node::Node::SharedPtr & node)
: node_(node)
{
get_parameters_client_ = node_->create_client<rcl_interfaces::GetParameters>(
"get_parameters");
get_parameter_types_client_ = node_->create_client<rcl_interfaces::GetParameterTypes>(
"get_parameter_types");
set_parameters_client_ = node_->create_client<rcl_interfaces::SetParameters>(
"set_parameters");
list_parameters_client_ = node_->create_client<rcl_interfaces::ListParameters>(
"list_parameters");
describe_parameters_client_ = node_->create_client<rcl_interfaces::DescribeParameters>(
"describe_parameters");
}

std::shared_future<std::vector<ParameterVariant>>
get_parameters(
std::vector<std::string> names,
std::function<void(
std::shared_future<std::vector<ParameterVariant>>)> callback = nullptr)
{
std::shared_future<std::vector<ParameterVariant>> f;
return f;
}

std::shared_future<std::vector<ParameterType>>
get_parameter_types(
std::vector<std::string> parameter_names,
std::function<void(
std::shared_future<std::vector<ParameterType>>)> callback = nullptr)
{
std::shared_future<std::vector<ParameterType>> f;
return f;
}

std::shared_future<std::vector<rcl_interfaces::SetParametersResult>>
set_parameters(
std::vector<ParameterVariant> parameters,
std::function<void(
std::shared_future<std::vector<rcl_interfaces::SetParametersResult>>)> callback = nullptr)
{
std::shared_future<std::vector<rcl_interfaces::SetParametersResult>> f;
return f;
}

std::shared_future<rcl_interfaces::SetParametersResult>
set_parameters_atomically(
std::vector<ParameterVariant> parameters,
std::function<void(
std::shared_future<rcl_interfaces::SetParametersResult>)> callback = nullptr)
{
std::shared_future<rcl_interfaces::SetParametersResult> f;
return f;
}

std::shared_future<rcl_interfaces::ListParametersResult>
list_parameters(
std::vector<std::string> parameter_prefixes,
uint64_t depth,
std::function<void(
std::shared_future<rcl_interfaces::ListParametersResult>)> callback = nullptr)
{
std::shared_future<rcl_interfaces::ListParametersResult> f;
return f;
}

private:
const rclcpp::node::Node::SharedPtr node_;
rclcpp::client::Client<rcl_interfaces::GetParameters>::SharedPtr get_parameters_client_;
rclcpp::client::Client<rcl_interfaces::GetParameterTypes>::SharedPtr get_parameter_types_client_;
rclcpp::client::Client<rcl_interfaces::SetParameters>::SharedPtr set_parameters_client_;
rclcpp::client::Client<rcl_interfaces::SetParametersAtomically>::SharedPtr
set_parameters_atomically_client_;
rclcpp::client::Client<rcl_interfaces::ListParameters>::SharedPtr list_parameters_client_;
rclcpp::client::Client<rcl_interfaces::DescribeParameters>::SharedPtr describe_parameters_client_;
};

class SyncParametersClient
{
public:
RCLCPP_MAKE_SHARED_DEFINITIONS(SyncParametersClient);

SyncParametersClient(
rclcpp::node::Node::SharedPtr & node)
: node_(node)
{
executor_ = std::make_shared<rclcpp::executors::SingleThreadedExecutor>();
async_parameters_client_ = std::make_shared<AsyncParametersClient>(node);
}

SyncParametersClient(
rclcpp::executor::Executor::SharedPtr & executor,
rclcpp::node::Node::SharedPtr & node)
: executor_(executor), node_(node)
{
async_parameters_client_ = std::make_shared<AsyncParametersClient>(node);
}

std::vector<ParameterVariant>
get_parameters(std::vector<std::string> parameter_names)
{
auto f = async_parameters_client_->get_parameters(parameter_names);
return rclcpp::executors::spin_node_until_future_complete(*executor_, node_, f).get();
}

std::vector<ParameterType>
get_parameter_types(std::vector<std::string> parameter_names)
{
auto f = async_parameters_client_->get_parameter_types(parameter_names);
return rclcpp::executors::spin_node_until_future_complete(*executor_, node_, f).get();
}

std::vector<rcl_interfaces::SetParametersResult>
set_parameters(std::vector<ParameterVariant> parameters)
{
auto f = async_parameters_client_->set_parameters(parameters);
return rclcpp::executors::spin_node_until_future_complete(*executor_, node_, f).get();
}

rcl_interfaces::SetParametersResult
set_parameters_atomically(std::vector<ParameterVariant> parameters)
{
auto f = async_parameters_client_->set_parameters_atomically(parameters);
return rclcpp::executors::spin_node_until_future_complete(*executor_, node_, f).get();
}

rcl_interfaces::ListParametersResult
list_parameters(
std::vector<std::string> parameter_prefixes,
uint64_t depth)
{
auto f = async_parameters_client_->list_parameters(parameter_prefixes, depth);
return rclcpp::executors::spin_node_until_future_complete(*executor_, node_, f).get();
static ParameterVariant from_parameter(const rcl_interfaces::Parameter & parameter)
{
switch (parameter.value.parameter_type) {
case PARAMETER_BOOL:
return ParameterVariant(parameter.name, parameter.value.bool_value);
case PARAMETER_INTEGER:
return ParameterVariant(parameter.name, parameter.value.integer_value);
case PARAMETER_DOUBLE:
return ParameterVariant(parameter.name, parameter.value.double_value);
case PARAMETER_STRING:
return ParameterVariant(parameter.name, parameter.value.string_value);
case PARAMETER_BYTES:
return ParameterVariant(parameter.name, parameter.value.bytes_value);
case PARAMETER_NOT_SET:
default:
// TODO: use custom exception
throw std::runtime_error("Invalid type from ParameterValue");
}
}

private:
rclcpp::executor::Executor::SharedPtr executor_;
rclcpp::node::Node::SharedPtr node_;
AsyncParametersClient::SharedPtr async_parameters_client_;
std::string name_;
rcl_interfaces::ParameterValue value_;
};

} /* namespace parameter */
Expand Down
Loading

0 comments on commit de2f707

Please sign in to comment.