rclcpp  master
C++ ROS Client Library API
node_impl.hpp
Go to the documentation of this file.
1 // Copyright 2014 Open Source Robotics Foundation, Inc.
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 #ifndef RCLCPP__NODE_IMPL_HPP_
16 #define RCLCPP__NODE_IMPL_HPP_
17 
18 #include <rmw/error_handling.h>
19 #include <rmw/rmw.h>
20 
21 #include <algorithm>
22 #include <cstdlib>
23 #include <iostream>
24 #include <limits>
25 #include <map>
26 #include <memory>
27 #include <sstream>
28 #include <stdexcept>
29 #include <string>
30 #include <utility>
31 #include <vector>
32 
33 #include "rcl/publisher.h"
34 #include "rcl/subscription.h"
35 
36 #include "rcl_interfaces/msg/intra_process_message.hpp"
37 
39 #include "rclcpp/create_client.hpp"
43 #include "rclcpp/parameter.hpp"
44 #include "rclcpp/qos.hpp"
47 
48 #ifndef RCLCPP__NODE_HPP_
49 #include "node.hpp"
50 #endif
51 
52 namespace rclcpp
53 {
54 
56 inline
58 extend_name_with_sub_namespace(const std::string & name, const std::string & sub_namespace)
59 {
60  std::string name_with_sub_namespace(name);
61  if (sub_namespace != "" && name.front() != '/' && name.front() != '~') {
62  name_with_sub_namespace = sub_namespace + "/" + name;
63  }
64  return name_with_sub_namespace;
65 }
66 
67 template<typename MessageT, typename AllocatorT, typename PublisherT>
70  const std::string & topic_name,
71  const rclcpp::QoS & qos,
73 {
74  return rclcpp::create_publisher<MessageT, AllocatorT, PublisherT>(
75  *this,
77  qos,
78  options);
79 }
80 
81 template<
82  typename MessageT,
83  typename CallbackT,
84  typename AllocatorT,
85  typename CallbackMessageT,
86  typename SubscriptionT,
87  typename MessageMemoryStrategyT>
90  const std::string & topic_name,
91  const rclcpp::QoS & qos,
92  CallbackT && callback,
94  typename MessageMemoryStrategyT::SharedPtr msg_mem_strat)
95 {
96  return rclcpp::create_subscription<MessageT>(
97  *this,
99  qos,
100  std::forward<CallbackT>(callback),
101  options,
102  msg_mem_strat);
103 }
104 
105 template<typename DurationRepT, typename DurationT, typename CallbackT>
109  CallbackT callback,
110  rclcpp::callback_group::CallbackGroup::SharedPtr group)
111 {
113  std::chrono::duration_cast<std::chrono::nanoseconds>(period),
114  std::move(callback),
115  this->node_base_->get_context());
116  node_timers_->add_timer(timer, group);
117  return timer;
118 }
119 
120 template<typename ServiceT>
123  const std::string & service_name,
124  const rmw_qos_profile_t & qos_profile,
125  rclcpp::callback_group::CallbackGroup::SharedPtr group)
126 {
127  return rclcpp::create_client<ServiceT>(
128  node_base_,
129  node_graph_,
130  node_services_,
131  extend_name_with_sub_namespace(service_name, this->get_sub_namespace()),
132  qos_profile,
133  group);
134 }
135 
136 template<typename ServiceT, typename CallbackT>
139  const std::string & service_name,
140  CallbackT && callback,
141  const rmw_qos_profile_t & qos_profile,
142  rclcpp::callback_group::CallbackGroup::SharedPtr group)
143 {
144  return rclcpp::create_service<ServiceT, CallbackT>(
145  node_base_,
146  node_services_,
147  extend_name_with_sub_namespace(service_name, this->get_sub_namespace()),
148  std::forward<CallbackT>(callback),
149  qos_profile,
150  group);
151 }
152 
153 template<typename ParameterT>
154 auto
156  const std::string & name,
157  const ParameterT & default_value,
158  const rcl_interfaces::msg::ParameterDescriptor & parameter_descriptor,
159  bool ignore_override)
160 {
161  return this->declare_parameter(
162  name,
163  rclcpp::ParameterValue(default_value),
164  parameter_descriptor,
165  ignore_override
166  ).get<ParameterT>();
167 }
168 
169 template<typename ParameterT>
172  const std::string & namespace_,
173  const std::map<std::string, ParameterT> & parameters,
174  bool ignore_overrides)
175 {
177  std::string normalized_namespace = namespace_.empty() ? "" : (namespace_ + ".");
179  parameters.begin(), parameters.end(), std::back_inserter(result),
180  [this, &normalized_namespace, ignore_overrides](auto element) {
181  return this->declare_parameter(
182  normalized_namespace + element.first,
183  element.second,
184  rcl_interfaces::msg::ParameterDescriptor(),
185  ignore_overrides);
186  }
187  );
188  return result;
189 }
190 
191 template<typename ParameterT>
194  const std::string & namespace_,
195  const std::map<
196  std::string,
198  > & parameters,
199  bool ignore_overrides)
200 {
202  std::string normalized_namespace = namespace_.empty() ? "" : (namespace_ + ".");
204  parameters.begin(), parameters.end(), std::back_inserter(result),
205  [this, &normalized_namespace, ignore_overrides](auto element) {
206  return static_cast<ParameterT>(
207  this->declare_parameter(
208  normalized_namespace + element.first,
209  element.second.first,
210  element.second.second,
211  ignore_overrides)
212  );
213  }
214  );
215  return result;
216 }
217 
218 template<typename ParameterT>
219 bool
220 Node::get_parameter(const std::string & name, ParameterT & parameter) const
221 {
223 
224  rclcpp::Parameter parameter_variant;
225 
226  bool result = get_parameter(sub_name, parameter_variant);
227  if (result) {
228  parameter = static_cast<ParameterT>(parameter_variant.get_value<ParameterT>());
229  }
230 
231  return result;
232 }
233 
234 template<typename ParameterT>
235 bool
237  const std::string & name,
238  ParameterT & parameter,
239  const ParameterT & alternative_value) const
240 {
242 
243  bool got_parameter = get_parameter(sub_name, parameter);
244  if (!got_parameter) {
245  parameter = alternative_value;
246  }
247  return got_parameter;
248 }
249 
250 // this is a partially-specialized version of get_parameter above,
251 // where our concrete type for ParameterT is std::map, but the to-be-determined
252 // type is the value in the map.
253 template<typename ParameterT>
254 bool
256  const std::string & prefix,
257  std::map<std::string, ParameterT> & values) const
258 {
260  bool result = node_parameters_->get_parameters_by_prefix(prefix, params);
261  if (result) {
262  for (const auto & param : params) {
263  values[param.first] = static_cast<ParameterT>(param.second.get_value<ParameterT>());
264  }
265  }
266 
267  return result;
268 }
269 
270 } // namespace rclcpp
271 
272 #endif // RCLCPP__NODE_IMPL_HPP_
std::vector< ParameterT > declare_parameters(const std::string &namespace_, const std::map< std::string, ParameterT > &parameters, bool ignore_overrides=false)
Declare and initialize several parameters with the same namespace and type.
Definition: node_impl.hpp:171
T empty(T... args)
rclcpp::Client< ServiceT >::SharedPtr create_client(const std::string &service_name, const rmw_qos_profile_t &qos_profile=rmw_qos_profile_services_default, rclcpp::callback_group::CallbackGroup::SharedPtr group=nullptr)
bool get_parameter_or(const std::string &name, ParameterT &parameter, const ParameterT &alternative_value) const
Get the parameter value, or the "alternative_value" if not set, and assign it to "parameter".
Definition: node_impl.hpp:236
Encapsulation of Quality of Service settings.
Definition: qos.hpp:55
Structure to store an arbitrary parameter with templated get/set methods.
Definition: parameter.hpp:51
Definition: timer.hpp:214
T front(T... args)
std::shared_ptr< SubscriptionT > create_subscription(const std::string &topic_name, const rclcpp::QoS &qos, CallbackT &&callback, const SubscriptionOptionsWithAllocator< AllocatorT > &options=SubscriptionOptionsWithAllocator< AllocatorT >(), typename MessageMemoryStrategyT::SharedPtr msg_mem_strat=(MessageMemoryStrategyT::create_default()))
Create and return a Subscription.
Definition: node_impl.hpp:89
const std::string & get_sub_namespace() const
Return the sub-namespace, if this is a sub-node, otherwise an empty string.
This header provides the get_node_base_interface() template function.
Definition: allocator_common.hpp:24
T end(T... args)
Definition: service.hpp:89
RCLCPP_LOCAL std::string extend_name_with_sub_namespace(const std::string &name, const std::string &sub_namespace)
Definition: node_impl.hpp:58
Definition: client.hpp:119
decltype(auto) get_value() const
Get value of parameter using rclcpp::ParameterType as template argument.
Definition: parameter.hpp:109
const rclcpp::ParameterValue & declare_parameter(const std::string &name, const rclcpp::ParameterValue &default_value=rclcpp::ParameterValue(), const rcl_interfaces::msg::ParameterDescriptor &parameter_descriptor=rcl_interfaces::msg::ParameterDescriptor(), bool ignore_override=false)
Declare and initialize a parameter, return the effective value.
#define RCLCPP_LOCAL
Definition: visibility_control.hpp:51
T move(T... args)
std::shared_ptr< PublisherT > create_publisher(const std::string &topic_name, const rclcpp::QoS &qos, const PublisherOptionsWithAllocator< AllocatorT > &options=PublisherOptionsWithAllocator< AllocatorT >())
Create and return a Publisher.
Definition: node_impl.hpp:69
rclcpp::WallTimer< CallbackT >::SharedPtr create_wall_timer(std::chrono::duration< DurationRepT, DurationT > period, CallbackT callback, rclcpp::callback_group::CallbackGroup::SharedPtr group=nullptr)
Create a timer.
Definition: node_impl.hpp:107
Set the data type used in the intra-process buffer as std::shared_ptr<MessageT>
T begin(T... args)
T back_inserter(T... args)
constexpr std::enable_if< type==ParameterType::PARAMETER_BOOL, const bool & >::type get() const
Definition: parameter_value.hpp:148
Store the type and value of a parameter.
Definition: parameter_value.hpp:71
rclcpp::Parameter get_parameter(const std::string &name) const
Return the parameter by the given name.
T transform(T... args)
std::vector< rclcpp::Parameter > get_parameters(const std::vector< std::string > &names) const
Return the parameters by the given parameter names.
rclcpp::Service< ServiceT >::SharedPtr create_service(const std::string &service_name, CallbackT &&callback, const rmw_qos_profile_t &qos_profile=rmw_qos_profile_services_default, rclcpp::callback_group::CallbackGroup::SharedPtr group=nullptr)
Definition: node_impl.hpp:138