Skip to content

Commit 5d81a98

Browse files
destogldestogl
committed
Added controller parameters structures and movement interfaces.
1 parent 1b278b9 commit 5d81a98

File tree

3 files changed

+338
-0
lines changed

3 files changed

+338
-0
lines changed

‎controller_interface/include/controller_interface/controller_parameters.hpp‎

Lines changed: 212 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,212 @@
1+
// Copyright (c) 2021, Stogl Robotics Consulting UG (haftungsbeschränkt)
2+
//
3+
// Licensed under the Apache License, Version 2.0 (the "License");
4+
// you may not use this file except in compliance with the License.
5+
// You may obtain a copy of the License at
6+
//
7+
// http://www.apache.org/licenses/LICENSE-2.0
8+
//
9+
// Unless required by applicable law or agreed to in writing, software
10+
// distributed under the License is distributed on an "AS IS" BASIS,
11+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
// See the License for the specific language governing permissions and
13+
// limitations under the License.
14+
//
15+
/// \author: Denis Stogl
16+
17+
#ifndef CONTROLLER_INTERFACE__CONTROLLER_PARAMETERS_HPP_
18+
#define CONTROLLER_INTERFACE__CONTROLLER_PARAMETERS_HPP_
19+
20+
#include <limits>
21+
#include <string>
22+
#include <utility>
23+
#include <vector>
24+
25+
#include "rclcpp/node.hpp"
26+
#include "rcutils/logging_macros.h"
27+
28+
namespace controller_interface
29+
{
30+
struct Parameter
31+
{
32+
Parameter() = default;
33+
34+
explicit Parameter(const std::string & name, bool configurable = false)
35+
: name(name), configurable(configurable)
36+
{
37+
}
38+
39+
std::string name;
40+
bool configurable;
41+
};
42+
43+
class ControllerParameters
44+
{
45+
public:
46+
ControllerParameters(
47+
int nr_bool_params = 0, int nr_double_params = 0, int nr_string_params = 0,
48+
int nr_string_list_params = 0);
49+
50+
virtual ~ControllerParameters() = default;
51+
52+
virtual void declare_parameters(rclcpp::Node::SharedPtr node);
53+
54+
/**
55+
* Gets all defined parameters from.
56+
*
57+
* \param[node] shared pointer to the node where parameters should be read.
58+
* \return true if all parameters are read Successfully, false if a parameter is not provided or
59+
* parameter configuration is wrong.
60+
*/
61+
virtual bool get_parameters(rclcpp::Node::SharedPtr node, bool check_validity = true);
62+
63+
virtual rcl_interfaces::msg::SetParametersResult set_parameter_callback(
64+
const std::vector<rclcpp::Parameter> & parameters);
65+
66+
/**
67+
* Default implementation of parameter check. No check is done. Always returns true.
68+
*
69+
* \return true
70+
*/
71+
virtual bool check_if_parameters_are_valid() { return true; }
72+
73+
virtual void update() = 0;
74+
75+
// TODO(destogl): Test this: "const rclcpp::Node::SharedPtr & node"
76+
77+
protected:
78+
void add_bool_parameter(
79+
const std::string & name, bool configurable = false, bool default_value = false)
80+
{
81+
bool_parameters_.push_back({Parameter(name, configurable), default_value});
82+
}
83+
84+
void add_double_parameter(
85+
const std::string & name, bool configurable = false,
86+
double default_value = std::numeric_limits<double>::quiet_NaN())
87+
{
88+
double_parameters_.push_back({Parameter(name, configurable), default_value});
89+
}
90+
91+
void add_string_parameter(
92+
const std::string & name, bool configurable = false, const std::string & default_value = "")
93+
{
94+
string_parameters_.push_back({Parameter(name, configurable), default_value});
95+
}
96+
97+
void add_string_list_parameter(
98+
const std::string & name, bool configurable = false,
99+
const std::vector<std::string> & default_value = {})
100+
{
101+
string_list_parameters_.push_back({Parameter(name, configurable), default_value});
102+
}
103+
104+
template <typename ParameterT>
105+
bool get_parameters_from_list(
106+
const rclcpp::Node::SharedPtr node, std::vector<std::pair<Parameter, ParameterT>> & parameters)
107+
{
108+
bool ret = true;
109+
for (auto & parameter : parameters)
110+
{
111+
try
112+
{
113+
rclcpp::Parameter input_parameter; // TODO(destogl): will this be allocated each time?
114+
ret = node->get_parameter(parameter.first.name, input_parameter);
115+
parameter.second = input_parameter.get_value<ParameterT>();
116+
}
117+
catch (rclcpp::ParameterTypeException & e)
118+
{
119+
RCUTILS_LOG_ERROR_NAMED(
120+
logger_name_.c_str(), "'%s' parameter has wrong type", parameter.first.name.c_str());
121+
ret = false;
122+
break;
123+
}
124+
}
125+
return ret;
126+
}
127+
128+
template <typename ParameterT>
129+
bool empty_parameter_in_list(const std::vector<std::pair<Parameter, ParameterT>> & parameters)
130+
{
131+
bool ret = false;
132+
for (const auto & parameter : parameters)
133+
{
134+
if (parameter.second.empty())
135+
{
136+
RCUTILS_LOG_ERROR_NAMED(
137+
logger_name_.c_str(), "'%s' parameter is empty", parameter.first.name.c_str());
138+
ret = true;
139+
}
140+
}
141+
return ret;
142+
}
143+
144+
bool empty_parameter_in_list(const std::vector<std::pair<Parameter, double>> & parameters)
145+
{
146+
bool ret = false;
147+
for (const auto & parameter : parameters)
148+
{
149+
if (std::isnan(parameter.second))
150+
{
151+
RCUTILS_LOG_ERROR_NAMED(
152+
logger_name_.c_str(), "'%s' parameter is not set", parameter.first.name.c_str());
153+
ret = true;
154+
}
155+
}
156+
return ret;
157+
}
158+
159+
template <typename ParameterT>
160+
bool find_and_assign_parameter_value(
161+
std::vector<std::pair<Parameter, ParameterT>> & parameter_list,
162+
const rclcpp::Parameter & input_parameter)
163+
{
164+
auto is_in_list = [&](const auto & parameter)
165+
{ return parameter.first.name == input_parameter.get_name(); };
166+
167+
auto it = std::find_if(parameter_list.begin(), parameter_list.end(), is_in_list);
168+
169+
if (it != parameter_list.end())
170+
{
171+
if (!it->first.configurable)
172+
{
173+
throw std::runtime_error(
174+
"Parameter " + input_parameter.get_name() + " is not configurable.");
175+
}
176+
else
177+
{
178+
it->second = input_parameter.get_value<ParameterT>();
179+
RCUTILS_LOG_ERROR_NAMED(
180+
logger_name_.c_str(), "'%s' parameter is updated to value: %s", it->first.name.c_str(),
181+
input_parameter.value_to_string().c_str());
182+
return true;
183+
}
184+
}
185+
else
186+
{
187+
return false;
188+
}
189+
}
190+
191+
std::vector<std::pair<Parameter, bool>> bool_parameters_;
192+
std::vector<std::pair<Parameter, double>> double_parameters_;
193+
std::vector<std::pair<Parameter, std::string>> string_parameters_;
194+
std::vector<std::pair<Parameter, std::vector<std::string>>> string_list_parameters_;
195+
196+
std::string logger_name_;
197+
198+
private:
199+
template <typename ParameterT>
200+
void declare_parameters_from_list(
201+
rclcpp::Node::SharedPtr node, const std::vector<std::pair<Parameter, ParameterT>> & parameters)
202+
{
203+
for (const auto & parameter : parameters)
204+
{
205+
node->declare_parameter<ParameterT>(parameter.first.name, parameter.second);
206+
}
207+
}
208+
};
209+
210+
} // namespace controller_interface
211+
212+
#endif // CONTROLLER_INTERFACE__CONTROLLER_PARAMETERS_HPP_

‎controller_interface/src/controller_parameters.cpp‎

Lines changed: 119 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,119 @@
1+
// Copyright (c) 2021, Stogl Robotics Consulting UG (haftungsbeschränkt)
2+
//
3+
// Licensed under the Apache License, Version 2.0 (the "License");
4+
// you may not use this file except in compliance with the License.
5+
// You may obtain a copy of the License at
6+
//
7+
// http://www.apache.org/licenses/LICENSE-2.0
8+
//
9+
// Unless required by applicable law or agreed to in writing, software
10+
// distributed under the License is distributed on an "AS IS" BASIS,
11+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
// See the License for the specific language governing permissions and
13+
// limitations under the License.
14+
//
15+
/// \author: Denis Stogl
16+
17+
#include "controller_interface/controller_parameters.hpp"
18+
19+
#include <string>
20+
#include <vector>
21+
22+
#include "rcutils/logging_macros.h"
23+
24+
namespace controller_interface
25+
{
26+
ControllerParameters::ControllerParameters(
27+
int nr_bool_params, int nr_double_params, int nr_string_params, int nr_string_list_params)
28+
{
29+
bool_parameters_.reserve(nr_bool_params);
30+
double_parameters_.reserve(nr_double_params);
31+
string_parameters_.reserve(nr_string_params);
32+
string_list_parameters_.reserve(nr_string_list_params);
33+
}
34+
35+
void ControllerParameters::declare_parameters(rclcpp::Node::SharedPtr node)
36+
{
37+
logger_name_ = std::string(node->get_name()) + "::parameters";
38+
39+
declare_parameters_from_list(node, bool_parameters_);
40+
declare_parameters_from_list(node, double_parameters_);
41+
declare_parameters_from_list(node, string_parameters_);
42+
declare_parameters_from_list(node, string_list_parameters_);
43+
}
44+
45+
/**
46+
* Gets all defined parameters from.
47+
*
48+
* \param[node] shared pointer to the node where parameters should be read.
49+
* \return true if all parameters are read Successfully, false if a parameter is not provided or
50+
* parameter configuration is wrong.
51+
*/
52+
bool ControllerParameters::get_parameters(rclcpp::Node::SharedPtr node, bool check_validity)
53+
{
54+
bool ret = false;
55+
56+
ret = get_parameters_from_list(node, bool_parameters_) &&
57+
get_parameters_from_list(node, double_parameters_) &&
58+
get_parameters_from_list(node, string_parameters_) &&
59+
get_parameters_from_list(node, string_list_parameters_);
60+
61+
if (ret && check_validity)
62+
{
63+
ret = check_if_parameters_are_valid();
64+
}
65+
66+
return ret;
67+
}
68+
69+
rcl_interfaces::msg::SetParametersResult ControllerParameters::set_parameter_callback(
70+
const std::vector<rclcpp::Parameter> & parameters)
71+
{
72+
rcl_interfaces::msg::SetParametersResult result;
73+
result.successful = true;
74+
75+
for (const auto & input_parameter : parameters)
76+
{
77+
bool found = false;
78+
79+
try
80+
{
81+
found = find_and_assign_parameter_value(bool_parameters_, input_parameter);
82+
if (!found)
83+
{
84+
found = find_and_assign_parameter_value(double_parameters_, input_parameter);
85+
}
86+
if (!found)
87+
{
88+
found = find_and_assign_parameter_value(string_parameters_, input_parameter);
89+
}
90+
if (!found)
91+
{
92+
found = find_and_assign_parameter_value(string_list_parameters_, input_parameter);
93+
}
94+
95+
RCUTILS_LOG_INFO_EXPRESSION_NAMED(
96+
found, logger_name_.c_str(),
97+
"Dynamic parameters got changed! To update the parameters internally please "
98+
"restart the controller.");
99+
}
100+
catch (const rclcpp::exceptions::InvalidParameterTypeException & e)
101+
{
102+
result.successful = false;
103+
result.reason = e.what();
104+
RCUTILS_LOG_ERROR_NAMED(logger_name_.c_str(), "%s", result.reason.c_str());
105+
break;
106+
}
107+
catch (const std::runtime_error & e)
108+
{
109+
result.successful = false;
110+
result.reason = e.what();
111+
RCUTILS_LOG_ERROR_NAMED(logger_name_.c_str(), "%s", result.reason.c_str());
112+
break;
113+
}
114+
}
115+
116+
return result;
117+
}
118+
119+
} // namespace controller_interface

‎hardware_interface/include/hardware_interface/types/hardware_interface_type_values.hpp‎

Lines changed: 7 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -15,6 +15,8 @@
1515
#ifndef HARDWARE_INTERFACE__TYPES__HARDWARE_INTERFACE_TYPE_VALUES_HPP_
1616
#define HARDWARE_INTERFACE__TYPES__HARDWARE_INTERFACE_TYPE_VALUES_HPP_
1717

18+
#include <vector>
19+
1820
namespace hardware_interface
1921
{
2022
/// Constant defining position interface
@@ -25,6 +27,11 @@ constexpr char HW_IF_VELOCITY[] = "velocity";
2527
constexpr char HW_IF_ACCELERATION[] = "acceleration";
2628
/// Constant defining effort interface
2729
constexpr char HW_IF_EFFORT[] = "effort";
30+
31+
// TODO(destogl): use "inline static const"-type when switched to C++17
32+
/// Definition of standard names for movement interfaces
33+
const std::vector<const char *> MOVEMENT_INTERFACES = {
34+
HW_IF_POSITION, HW_IF_VELOCITY, HW_IF_ACCELERATION, HW_IF_EFFORT};
2835
} // namespace hardware_interface
2936

3037
#endif // HARDWARE_INTERFACE__TYPES__HARDWARE_INTERFACE_TYPE_VALUES_HPP_

0 commit comments

Comments
 (0)