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 
40 #include "rclcpp/parameter.hpp"
46 
47 #ifndef RCLCPP__NODE_HPP_
48 #include "node.hpp"
49 #endif
50 
51 namespace rclcpp
52 {
53 
54 template<typename MessageT, typename Alloc, typename PublisherT>
57  const std::string & topic_name, size_t qos_history_depth,
58  std::shared_ptr<Alloc> allocator)
59 {
60  if (!allocator) {
61  allocator = std::make_shared<Alloc>();
62  }
63  rmw_qos_profile_t qos = rmw_qos_profile_default;
64  qos.depth = qos_history_depth;
65  return this->create_publisher<MessageT, Alloc, PublisherT>(topic_name, qos, allocator);
66 }
67 
68 template<typename MessageT, typename Alloc, typename PublisherT>
71  const std::string & topic_name, const rmw_qos_profile_t & qos_profile,
72  std::shared_ptr<Alloc> allocator)
73 {
74  if (!allocator) {
75  allocator = std::make_shared<Alloc>();
76  }
77  return rclcpp::create_publisher<MessageT, Alloc, PublisherT>(
78  this->node_topics_.get(),
79  topic_name,
80  qos_profile,
81  use_intra_process_comms_,
82  allocator);
83 }
84 
85 template<
86  typename MessageT,
87  typename CallbackT,
88  typename Alloc,
89  typename SubscriptionT>
92  const std::string & topic_name,
93  CallbackT && callback,
94  const rmw_qos_profile_t & qos_profile,
95  rclcpp::callback_group::CallbackGroup::SharedPtr group,
96  bool ignore_local_publications,
99  msg_mem_strat,
100  std::shared_ptr<Alloc> allocator)
101 {
102  using CallbackMessageT = typename rclcpp::subscription_traits::has_message_type<CallbackT>::type;
103 
104  if (!allocator) {
105  allocator = std::make_shared<Alloc>();
106  }
107 
108  if (!msg_mem_strat) {
111  }
112 
113  return rclcpp::create_subscription<MessageT, CallbackT, Alloc, CallbackMessageT, SubscriptionT>(
114  this->node_topics_.get(),
115  topic_name,
116  std::forward<CallbackT>(callback),
117  qos_profile,
118  group,
119  ignore_local_publications,
120  use_intra_process_comms_,
121  msg_mem_strat,
122  allocator);
123 }
124 
125 template<
126  typename MessageT,
127  typename CallbackT,
128  typename Alloc,
129  typename SubscriptionT>
132  const std::string & topic_name,
133  CallbackT && callback,
134  size_t qos_history_depth,
135  rclcpp::callback_group::CallbackGroup::SharedPtr group,
136  bool ignore_local_publications,
139  msg_mem_strat,
140  std::shared_ptr<Alloc> allocator)
141 {
142  rmw_qos_profile_t qos = rmw_qos_profile_default;
143  qos.depth = qos_history_depth;
144  return this->create_subscription<MessageT>(
145  topic_name,
146  std::forward<CallbackT>(callback),
147  qos,
148  group,
149  ignore_local_publications,
150  msg_mem_strat,
151  allocator);
152 }
153 
154 template<typename DurationT, typename CallbackT>
158  CallbackT callback,
159  rclcpp::callback_group::CallbackGroup::SharedPtr group)
160 {
162  std::chrono::duration_cast<std::chrono::nanoseconds>(period),
163  std::move(callback));
164  node_timers_->add_timer(timer, group);
165  return timer;
166 }
167 
168 template<typename ServiceT>
171  const std::string & service_name,
172  const rmw_qos_profile_t & qos_profile,
173  rclcpp::callback_group::CallbackGroup::SharedPtr group)
174 {
176  options.qos = qos_profile;
177 
178  using rclcpp::Client;
179  using rclcpp::ClientBase;
180 
182  node_base_.get(),
183  node_graph_,
184  service_name,
185  options);
186 
187  auto cli_base_ptr = std::dynamic_pointer_cast<ClientBase>(cli);
188  node_services_->add_client(cli_base_ptr, group);
189  return cli;
190 }
191 
192 template<typename ServiceT, typename CallbackT>
195  const std::string & service_name,
196  CallbackT && callback,
197  const rmw_qos_profile_t & qos_profile,
198  rclcpp::callback_group::CallbackGroup::SharedPtr group)
199 {
200  return rclcpp::create_service<ServiceT, CallbackT>(
201  node_base_, node_services_,
202  service_name, std::forward<CallbackT>(callback), qos_profile, group);
203 }
204 
205 template<typename CallbackT>
206 void
208 {
209  this->node_parameters_->register_param_change_callback(std::forward<CallbackT>(callback));
210 }
211 
212 template<typename ParameterT>
213 void
215  const std::string & name,
216  const ParameterT & value)
217 {
218  rclcpp::Parameter parameter;
219  if (!this->get_parameter(name, parameter)) {
220  this->set_parameters({
221  rclcpp::Parameter(name, value),
222  });
223  }
224 }
225 
226 template<typename ParameterT>
227 bool
228 Node::get_parameter(const std::string & name, ParameterT & value) const
229 {
230  rclcpp::Parameter parameter;
231  bool result = get_parameter(name, parameter);
232  if (result) {
233  value = parameter.get_value<ParameterT>();
234  }
235 
236  return result;
237 }
238 
239 template<typename ParameterT>
240 bool
242  const std::string & name,
243  ParameterT & value,
244  const ParameterT & alternative_value) const
245 {
246  bool got_parameter = get_parameter(name, value);
247  if (!got_parameter) {
248  value = alternative_value;
249  }
250  return got_parameter;
251 }
252 
253 } // namespace rclcpp
254 
255 #endif // RCLCPP__NODE_IMPL_HPP_
decltype(auto) get_value() const
Get value of parameter using rclcpp::ParameterType as template argument.
Definition: parameter.hpp:66
Generic timer templated on the clock type. Periodically executes a user-specified callback...
Definition: timer.hpp:105
rclcpp::WallTimer< CallbackT >::SharedPtr create_wall_timer(std::chrono::duration< int64_t, DurationT > period, CallbackT callback, rclcpp::callback_group::CallbackGroup::SharedPtr group=nullptr)
Create a timer.
Definition: node_impl.hpp:156
static SharedPtr create_default()
Default factory method.
Definition: message_memory_strategy.hpp:73
std::shared_ptr< PublisherT > create_publisher(const std::string &topic_name, size_t qos_history_depth, std::shared_ptr< Alloc > allocator=nullptr)
Create and return a Publisher.
Definition: node_impl.hpp:56
Definition: client.hpp:118
Definition: allocator_common.hpp:24
Structure to store an arbitrary parameter with templated get/set methods.
Definition: parameter.hpp:32
bool get_parameter_or(const std::string &name, ParameterT &value, const ParameterT &alternative_value) const
Get the parameter value, or the "alternative value" if not set, and assign it to "value".
Definition: node_impl.hpp:241
void set_parameter_if_not_set(const std::string &name, const ParameterT &value)
Definition: node_impl.hpp:214
rmw_qos_profile_t qos
typename std::remove_cv< rclcpp::function_traits::function_traits< CallbackT >::template argument_type< 0 > >::type type
Definition: subscription_traits.hpp:61
void register_param_change_callback(CallbackT &&callback)
Register the callback for parameter changes.
Definition: node_impl.hpp:207
std::shared_ptr< SubscriptionT > create_subscription(const std::string &topic_name, CallbackT &&callback, const rmw_qos_profile_t &qos_profile=rmw_qos_profile_default, rclcpp::callback_group::CallbackGroup::SharedPtr group=nullptr, bool ignore_local_publications=false, typename rclcpp::message_memory_strategy::MessageMemoryStrategy< typename rclcpp::subscription_traits::has_message_type< CallbackT >::type, Alloc >::SharedPtr msg_mem_strat=nullptr, std::shared_ptr< Alloc > allocator=nullptr)
Create and return a Subscription.
Definition: node_impl.hpp:91
rcl_client_options_t rcl_client_get_default_options(void)
T dynamic_pointer_cast(T... args)
T move(T... args)
Definition: service.hpp:88
Default allocation strategy for messages received by subscriptions.
Definition: message_memory_strategy.hpp:38
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)
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:194
rclcpp::Parameter get_parameter(const std::string &name) const
Definition: client.hpp:52
std::vector< rcl_interfaces::msg::SetParametersResult > set_parameters(const std::vector< rclcpp::Parameter > &parameters)