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 <chrono>
23 #include <cstdlib>
24 #include <iostream>
25 #include <limits>
26 #include <map>
27 #include <memory>
28 #include <sstream>
29 #include <stdexcept>
30 #include <string>
31 #include <utility>
32 #include <vector>
33 
34 #include "rcl/publisher.h"
35 #include "rcl/subscription.h"
36 
38 #include "rclcpp/create_client.hpp"
41 #include "rclcpp/create_timer.hpp"
44 #include "rclcpp/parameter.hpp"
45 #include "rclcpp/qos.hpp"
46 #include "rclcpp/timer.hpp"
49 
50 #ifndef RCLCPP__NODE_HPP_
51 #include "node.hpp"
52 #endif
53 
54 namespace rclcpp
55 {
56 
58 inline
60 extend_name_with_sub_namespace(const std::string & name, const std::string & sub_namespace)
61 {
62  std::string name_with_sub_namespace(name);
63  if (sub_namespace != "" && name.front() != '/' && name.front() != '~') {
64  name_with_sub_namespace = sub_namespace + "/" + name;
65  }
66  return name_with_sub_namespace;
67 }
68 
69 template<typename MessageT, typename AllocatorT, typename PublisherT>
72  const std::string & topic_name,
73  const rclcpp::QoS & qos,
75 {
76  return rclcpp::create_publisher<MessageT, AllocatorT, PublisherT>(
77  *this,
79  qos,
80  options);
81 }
82 
83 template<
84  typename MessageT,
85  typename CallbackT,
86  typename AllocatorT,
87  typename CallbackMessageT,
88  typename SubscriptionT,
89  typename MessageMemoryStrategyT>
92  const std::string & topic_name,
93  const rclcpp::QoS & qos,
94  CallbackT && callback,
96  typename MessageMemoryStrategyT::SharedPtr msg_mem_strat)
97 {
98  return rclcpp::create_subscription<MessageT>(
99  *this,
101  qos,
102  std::forward<CallbackT>(callback),
103  options,
104  msg_mem_strat);
105 }
106 
107 template<typename DurationRepT, typename DurationT, typename CallbackT>
111  CallbackT callback,
112  rclcpp::CallbackGroup::SharedPtr group)
113 {
115  period,
116  std::move(callback),
117  group,
118  this->node_base_.get(),
119  this->node_timers_.get());
120 }
121 
122 template<typename ServiceT>
125  const std::string & service_name,
126  const rmw_qos_profile_t & qos_profile,
127  rclcpp::CallbackGroup::SharedPtr group)
128 {
129  return rclcpp::create_client<ServiceT>(
130  node_base_,
131  node_graph_,
132  node_services_,
133  extend_name_with_sub_namespace(service_name, this->get_sub_namespace()),
134  qos_profile,
135  group);
136 }
137 
138 template<typename ServiceT, typename CallbackT>
141  const std::string & service_name,
142  CallbackT && callback,
143  const rmw_qos_profile_t & qos_profile,
144  rclcpp::CallbackGroup::SharedPtr group)
145 {
146  return rclcpp::create_service<ServiceT, CallbackT>(
147  node_base_,
148  node_services_,
149  extend_name_with_sub_namespace(service_name, this->get_sub_namespace()),
150  std::forward<CallbackT>(callback),
151  qos_profile,
152  group);
153 }
154 
155 template<typename ParameterT>
156 auto
158  const std::string & name,
159  const ParameterT & default_value,
160  const rcl_interfaces::msg::ParameterDescriptor & parameter_descriptor,
161  bool ignore_override)
162 {
163  try {
164  return this->declare_parameter(
165  name,
166  rclcpp::ParameterValue(default_value),
167  parameter_descriptor,
168  ignore_override
169  ).get<ParameterT>();
170  } catch (const ParameterTypeException & ex) {
172  }
173 }
174 
175 template<typename ParameterT>
178  const std::string & namespace_,
179  const std::map<std::string, ParameterT> & parameters,
180  bool ignore_overrides)
181 {
183  std::string normalized_namespace = namespace_.empty() ? "" : (namespace_ + ".");
185  parameters.begin(), parameters.end(), std::back_inserter(result),
186  [this, &normalized_namespace, ignore_overrides](auto element) {
187  return this->declare_parameter(
188  normalized_namespace + element.first,
189  element.second,
190  rcl_interfaces::msg::ParameterDescriptor(),
191  ignore_overrides);
192  }
193  );
194  return result;
195 }
196 
197 template<typename ParameterT>
200  const std::string & namespace_,
201  const std::map<
202  std::string,
204  > & parameters,
205  bool ignore_overrides)
206 {
208  std::string normalized_namespace = namespace_.empty() ? "" : (namespace_ + ".");
210  parameters.begin(), parameters.end(), std::back_inserter(result),
211  [this, &normalized_namespace, ignore_overrides](auto element) {
212  return static_cast<ParameterT>(
213  this->declare_parameter(
214  normalized_namespace + element.first,
215  element.second.first,
216  element.second.second,
217  ignore_overrides)
218  );
219  }
220  );
221  return result;
222 }
223 
224 template<typename ParameterT>
225 bool
226 Node::get_parameter(const std::string & name, ParameterT & parameter) const
227 {
229 
230  rclcpp::Parameter parameter_variant;
231 
232  bool result = get_parameter(sub_name, parameter_variant);
233  if (result) {
234  parameter = static_cast<ParameterT>(parameter_variant.get_value<ParameterT>());
235  }
236 
237  return result;
238 }
239 
240 template<typename ParameterT>
241 bool
243  const std::string & name,
244  ParameterT & parameter,
245  const ParameterT & alternative_value) const
246 {
248 
249  bool got_parameter = get_parameter(sub_name, parameter);
250  if (!got_parameter) {
251  parameter = alternative_value;
252  }
253  return got_parameter;
254 }
255 
256 // this is a partially-specialized version of get_parameter above,
257 // where our concrete type for ParameterT is std::map, but the to-be-determined
258 // type is the value in the map.
259 template<typename ParameterT>
260 bool
262  const std::string & prefix,
263  std::map<std::string, ParameterT> & values) const
264 {
266  bool result = node_parameters_->get_parameters_by_prefix(prefix, params);
267  if (result) {
268  for (const auto & param : params) {
269  values[param.first] = static_cast<ParameterT>(param.second.get_value<ParameterT>());
270  }
271  }
272 
273  return result;
274 }
275 
276 } // namespace rclcpp
277 
278 #endif // RCLCPP__NODE_IMPL_HPP_
rclcpp::ParameterValue
Store the type and value of a parameter.
Definition: parameter_value.hpp:71
rclcpp::create_wall_timer
rclcpp::WallTimer< CallbackT >::SharedPtr create_wall_timer(std::chrono::duration< DurationRepT, DurationT > period, CallbackT callback, rclcpp::CallbackGroup::SharedPtr group, node_interfaces::NodeBaseInterface *node_base, node_interfaces::NodeTimersInterface *node_timers)
Convenience method to create a timer with node resources.
Definition: create_timer.hpp:90
std::string
std::shared_ptr
RCLCPP_LOCAL
#define RCLCPP_LOCAL
Definition: visibility_control.hpp:51
std::move
T move(T... args)
rmw.h
std::pair
rclcpp::Node::create_wall_timer
rclcpp::WallTimer< CallbackT >::SharedPtr create_wall_timer(std::chrono::duration< DurationRepT, DurationT > period, CallbackT callback, rclcpp::CallbackGroup::SharedPtr group=nullptr)
Create a timer.
Definition: node_impl.hpp:109
std::vector
rclcpp::ParameterValue::get
constexpr std::enable_if< type==ParameterType::PARAMETER_BOOL, const bool & >::type get() const
Definition: parameter_value.hpp:148
node.hpp
std::back_inserter
T back_inserter(T... args)
error_handling.h
rclcpp::Parameter::get_value
decltype(auto) get_value() const
Get value of parameter using rclcpp::ParameterType as template argument.
Definition: parameter.hpp:117
std::chrono::duration
rclcpp::Service
Definition: service.hpp:144
rclcpp::Node::create_subscription
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:91
create_client.hpp
rclcpp::Node::create_client
rclcpp::Client< ServiceT >::SharedPtr create_client(const std::string &service_name, const rmw_qos_profile_t &qos_profile=rmw_qos_profile_services_default, rclcpp::CallbackGroup::SharedPtr group=nullptr)
Create and return a Client.
rclcpp::SubscriptionOptionsWithAllocator
Structure containing optional configuration for Subscriptions.
Definition: subscription_options.hpp:79
rclcpp::Node::declare_parameter
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.
rclcpp::ParameterTypeException
Indicate the parameter type does not match the expected type.
Definition: parameter_value.hpp:56
rclcpp::extend_name_with_sub_namespace
RCLCPP_LOCAL std::string extend_name_with_sub_namespace(const std::string &name, const std::string &sub_namespace)
Definition: node_impl.hpp:60
std::string::front
T front(T... args)
rclcpp::Node::get_parameter
rclcpp::Parameter get_parameter(const std::string &name) const
Return the parameter by the given name.
rclcpp
This header provides the get_node_base_interface() template function.
Definition: allocator_common.hpp:24
qos.hpp
rclcpp::Node::create_publisher
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:71
timer.hpp
rclcpp::QoS
Encapsulation of Quality of Service settings.
Definition: qos.hpp:59
rclcpp::Node::get_parameters
std::vector< rclcpp::Parameter > get_parameters(const std::vector< std::string > &names) const
Return the parameters by the given parameter names.
rclcpp::Node::create_service
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::CallbackGroup::SharedPtr group=nullptr)
Create and return a Service.
Definition: node_impl.hpp:140
create_service.hpp
create_subscription.hpp
rclcpp::Node::get_parameter_or
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:242
std::map
std::transform
T transform(T... args)
rclcpp::Node::declare_parameters
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:177
create_timer.hpp
parameter.hpp
default_context.hpp
rclcpp::PublisherOptionsWithAllocator
Structure containing optional configuration for Publishers.
Definition: publisher_options.hpp:57
rclcpp::Client
Definition: client.hpp:178
type_support_decl.hpp
std::map::begin
T begin(T... args)
visibility_control.hpp
rclcpp::Node::get_sub_namespace
const std::string & get_sub_namespace() const
Return the sub-namespace, if this is a sub-node, otherwise an empty string.
rmw_qos_profile_t
std::string::empty
T empty(T... args)
rclcpp::WallTimer
Definition: timer.hpp:259
rclcpp::Parameter
Structure to store an arbitrary parameter with templated get/set methods.
Definition: parameter.hpp:52
std::map::end
T end(T... args)
rclcpp::exceptions::InvalidParameterTypeException
Thrown if requested parameter type is invalid.
Definition: exceptions.hpp:234
resolve_enable_topic_statistics.hpp
std::runtime_error::what
T what(T... args)
create_publisher.hpp