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"
45 
46 #ifndef RCLCPP__NODE_HPP_
47 #include "node.hpp"
48 #endif
49 
50 namespace rclcpp
51 {
52 namespace node
53 {
54 
55 template<typename MessageT, typename Alloc, typename PublisherT>
56 std::shared_ptr<PublisherT>
58  const std::string & topic_name, size_t qos_history_depth,
59  std::shared_ptr<Alloc> allocator)
60 {
61  if (!allocator) {
62  allocator = std::make_shared<Alloc>();
63  }
64  rmw_qos_profile_t qos = rmw_qos_profile_default;
65  qos.depth = qos_history_depth;
66  return this->create_publisher<MessageT, Alloc, PublisherT>(topic_name, qos, allocator);
67 }
68 
69 template<typename MessageT, typename Alloc, typename PublisherT>
70 std::shared_ptr<PublisherT>
72  const std::string & topic_name, const rmw_qos_profile_t & qos_profile,
73  std::shared_ptr<Alloc> allocator)
74 {
75  if (!allocator) {
76  allocator = std::make_shared<Alloc>();
77  }
78  return rclcpp::create_publisher<MessageT, Alloc, PublisherT>(
79  this->node_topics_.get(),
80  topic_name,
81  qos_profile,
82  use_intra_process_comms_,
83  allocator);
84 }
85 
86 template<typename MessageT, typename CallbackT, typename Alloc, typename SubscriptionT>
87 std::shared_ptr<SubscriptionT>
89  const std::string & topic_name,
90  CallbackT && callback,
91  const rmw_qos_profile_t & qos_profile,
92  rclcpp::callback_group::CallbackGroup::SharedPtr group,
93  bool ignore_local_publications,
95  msg_mem_strat,
96  std::shared_ptr<Alloc> allocator)
97 {
98  if (!allocator) {
99  allocator = std::make_shared<Alloc>();
100  }
101 
102  if (!msg_mem_strat) {
104  msg_mem_strat = MessageMemoryStrategy<MessageT, Alloc>::create_default();
105  }
106 
107  return rclcpp::create_subscription<MessageT, CallbackT, Alloc, SubscriptionT>(
108  this->node_topics_.get(),
109  topic_name,
110  std::forward<CallbackT>(callback),
111  qos_profile,
112  group,
113  ignore_local_publications,
114  use_intra_process_comms_,
115  msg_mem_strat,
116  allocator);
117 }
118 
119 template<typename MessageT, typename CallbackT, typename Alloc, typename SubscriptionT>
120 std::shared_ptr<SubscriptionT>
122  const std::string & topic_name,
123  size_t qos_history_depth,
124  CallbackT && callback,
125  rclcpp::callback_group::CallbackGroup::SharedPtr group,
126  bool ignore_local_publications,
128  msg_mem_strat,
129  std::shared_ptr<Alloc> allocator)
130 {
131  rmw_qos_profile_t qos = rmw_qos_profile_default;
132  qos.depth = qos_history_depth;
133  return this->create_subscription<MessageT, CallbackT, Alloc, SubscriptionT>(
134  topic_name,
135  std::forward<CallbackT>(callback),
136  qos,
137  group,
138  ignore_local_publications,
139  msg_mem_strat,
140  allocator);
141 }
142 
143 template<typename DurationT, typename CallbackT>
146  std::chrono::duration<int64_t, DurationT> period,
147  CallbackT callback,
148  rclcpp::callback_group::CallbackGroup::SharedPtr group)
149 {
151  std::chrono::duration_cast<std::chrono::nanoseconds>(period),
152  std::move(callback));
153  node_timers_->add_timer(timer, group);
154  return timer;
155 }
156 
157 template<typename ServiceT>
160  const std::string & service_name,
161  const rmw_qos_profile_t & qos_profile,
162  rclcpp::callback_group::CallbackGroup::SharedPtr group)
163 {
165  options.qos = qos_profile;
166 
169 
170  auto cli = Client<ServiceT>::make_shared(
171  node_base_.get(),
172  node_graph_,
173  service_name,
174  options);
175 
176  auto cli_base_ptr = std::dynamic_pointer_cast<ClientBase>(cli);
177  node_services_->add_client(cli_base_ptr, group);
178  return cli;
179 }
180 
181 template<typename ServiceT, typename CallbackT>
184  const std::string & service_name,
185  CallbackT && callback,
186  const rmw_qos_profile_t & qos_profile,
187  rclcpp::callback_group::CallbackGroup::SharedPtr group)
188 {
190  any_service_callback.set(std::forward<CallbackT>(callback));
191 
193  service_options.qos = qos_profile;
194 
196  node_base_->get_shared_rcl_node_handle(),
197  service_name, any_service_callback, service_options);
198  auto serv_base_ptr = std::dynamic_pointer_cast<service::ServiceBase>(serv);
199  node_services_->add_service(serv_base_ptr, group);
200  return serv;
201 }
202 
203 template<typename CallbackT>
204 void
206 {
207  this->node_parameters_->register_param_change_callback(std::forward<CallbackT>(callback));
208 }
209 
210 template<typename ParameterT>
211 void
213  const std::string & name,
214  const ParameterT & value)
215 {
216  rclcpp::parameter::ParameterVariant parameter_variant;
217  if (!this->get_parameter(name, parameter_variant)) {
218  this->set_parameters({
220  });
221  }
222 }
223 
224 template<typename ParameterT>
225 bool
226 Node::get_parameter(const std::string & name, ParameterT & value) const
227 {
228  rclcpp::parameter::ParameterVariant parameter_variant;
229  bool result = get_parameter(name, parameter_variant);
230  if (result) {
231  value = parameter_variant.get_value<ParameterT>();
232  }
233 
234  return result;
235 }
236 
237 template<typename ParameterT>
238 bool
240  const std::string & name,
241  ParameterT & value,
242  const ParameterT & alternative_value) const
243 {
244  bool got_parameter = get_parameter(name, value);
245  if (!got_parameter) {
246  value = alternative_value;
247  }
248  return got_parameter;
249 }
250 
251 } // namespace node
252 } // namespace rclcpp
253 
254 #endif // RCLCPP__NODE_IMPL_HPP_
void set_parameter_if_not_set(const std::string &name, const ParameterT &value)
Definition: node_impl.hpp:212
rclcpp::timer::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:145
std::vector< rcl_interfaces::msg::SetParametersResult > set_parameters(const std::vector< rclcpp::parameter::ParameterVariant > &parameters)
Default allocation strategy for messages received by subscriptions.
Definition: message_memory_strategy.hpp:33
Definition: service.hpp:89
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:57
rclcpp::client::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)
Definition: allocator_common.hpp:24
rclcpp::parameter::ParameterVariant get_parameter(const std::string &name) const
rmw_qos_profile_t qos
Definition: service.hpp:41
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:239
void set(CallbackT callback)
Definition: any_service_callback.hpp:66
rcl_client_options_t rcl_client_get_default_options(void)
std::enable_if< type==ParameterType::PARAMETER_INTEGER, int64_t >::type get_value() const
Definition: parameter.hpp:87
rclcpp::service::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:183
Definition: client.hpp:53
Generic timer templated on the clock type. Periodically executes a user-specified callback...
Definition: timer.hpp:107
Definition: parameter.hpp:45
rcl_service_options_t rcl_service_get_default_options(void)
Definition: client.hpp:113
void register_param_change_callback(CallbackT &&callback)
Register the callback for parameter changes.
Definition: node_impl.hpp:205
rmw_qos_profile_t qos
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< MessageT, Alloc >::SharedPtr msg_mem_strat=nullptr, std::shared_ptr< Alloc > allocator=nullptr)
Create and return a Subscription.
Definition: node_impl.hpp:88
Definition: any_service_callback.hpp:34