rclcpp_lifecycle  master
C++ ROS Lifecycle Library API
lifecycle_node_impl.hpp
Go to the documentation of this file.
1 // Copyright 2016 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_LIFECYCLE__LIFECYCLE_NODE_IMPL_HPP_
16 #define RCLCPP_LIFECYCLE__LIFECYCLE_NODE_IMPL_HPP_
17 
18 #include <map>
19 #include <memory>
20 #include <string>
21 #include <utility>
22 #include <vector>
23 
25 #include "rclcpp/event.hpp"
27 #include "rclcpp/parameter.hpp"
33 
34 #include "lifecycle_publisher.hpp"
36 
38 
39 namespace rclcpp_lifecycle
40 {
41 
42 template<typename MessageT, typename AllocatorT>
45  const std::string & topic_name,
46  const rclcpp::QoS & qos,
48 {
50  return rclcpp::create_publisher<MessageT, AllocatorT, PublisherT>(
51  *this,
52  topic_name,
53  qos,
54  options);
55 }
56 
57 // TODO(karsten1987): Create LifecycleSubscriber
58 template<
59  typename MessageT,
60  typename CallbackT,
61  typename AllocatorT,
62  typename CallbackMessageT,
63  typename SubscriptionT,
64  typename MessageMemoryStrategyT>
67  const std::string & topic_name,
68  const rclcpp::QoS & qos,
69  CallbackT && callback,
71  typename MessageMemoryStrategyT::SharedPtr msg_mem_strat)
72 {
73  return rclcpp::create_subscription<MessageT>(
74  *this,
75  topic_name,
76  qos,
77  std::forward<CallbackT>(callback),
78  options,
79  msg_mem_strat);
80 }
81 
82 template<typename DurationRepT, typename DurationT, typename CallbackT>
86  CallbackT callback,
87  rclcpp::CallbackGroup::SharedPtr group)
88 {
90  std::chrono::duration_cast<std::chrono::nanoseconds>(period),
91  std::move(callback), this->node_base_->get_context());
92  node_timers_->add_timer(timer, group);
93  return timer;
94 }
95 
96 template<typename ServiceT>
99  const std::string & service_name,
100  const rmw_qos_profile_t & qos_profile,
101  rclcpp::CallbackGroup::SharedPtr group)
102 {
103  rcl_client_options_t options = rcl_client_get_default_options();
104  options.qos = qos_profile;
105 
106  using rclcpp::Client;
107  using rclcpp::ClientBase;
108 
109  auto cli = Client<ServiceT>::make_shared(
110  node_base_.get(),
111  node_graph_,
112  service_name,
113  options);
114 
115  auto cli_base_ptr = std::dynamic_pointer_cast<ClientBase>(cli);
116  node_services_->add_client(cli_base_ptr, group);
117  return cli;
118 }
119 
120 template<typename ServiceT, typename CallbackT>
123  const std::string & service_name,
124  CallbackT && callback,
125  const rmw_qos_profile_t & qos_profile,
126  rclcpp::CallbackGroup::SharedPtr group)
127 {
128  return rclcpp::create_service<ServiceT, CallbackT>(
129  node_base_, node_services_,
130  service_name, std::forward<CallbackT>(callback), qos_profile, group);
131 }
132 
133 template<typename ParameterT>
134 auto
136  const std::string & name,
137  const ParameterT & default_value,
138  const rcl_interfaces::msg::ParameterDescriptor & parameter_descriptor)
139 {
140  return this->declare_parameter(
141  name,
142  rclcpp::ParameterValue(default_value),
143  parameter_descriptor
144  ).get<ParameterT>();
145 }
146 
147 template<typename ParameterT>
150  const std::string & namespace_,
151  const std::map<std::string, ParameterT> & parameters)
152 {
154  std::string normalized_namespace = namespace_.empty() ? "" : (namespace_ + ".");
156  parameters.begin(), parameters.end(), std::back_inserter(result),
157  [this, &normalized_namespace](auto element) {
158  return this->declare_parameter(normalized_namespace + element.first, element.second);
159  }
160  );
161  return result;
162 }
163 
164 template<typename ParameterT>
167  const std::string & namespace_,
168  const std::map<
169  std::string,
171  > & parameters)
172 {
174  std::string normalized_namespace = namespace_.empty() ? "" : (namespace_ + ".");
176  parameters.begin(), parameters.end(), std::back_inserter(result),
177  [this, &normalized_namespace](auto element) {
178  return static_cast<ParameterT>(
179  this->declare_parameter(
180  normalized_namespace + element.first,
181  element.second.first,
182  element.second.second)
183  );
184  }
185  );
186  return result;
187 }
188 
189 template<typename ParameterT>
190 bool
191 LifecycleNode::get_parameter(const std::string & name, ParameterT & parameter) const
192 {
193  rclcpp::Parameter param(name, parameter);
194  bool result = get_parameter(name, param);
195  parameter = param.get_value<ParameterT>();
196 
197  return result;
198 }
199 
200 // this is a partially-specialized version of get_parameter above,
201 // where our concrete type for ParameterT is std::map, but the to-be-determined
202 // type is the value in the map.
203 template<typename MapValueT>
204 bool
206  const std::string & prefix,
207  std::map<std::string, MapValueT> & values) const
208 {
210  bool result = node_parameters_->get_parameters_by_prefix(prefix, params);
211  if (result) {
212  for (const auto & param : params) {
213  values[param.first] = param.second.get_value<MapValueT>();
214  }
215  }
216 
217  return result;
218 }
219 
220 template<typename ParameterT>
221 bool
223  const std::string & name,
224  ParameterT & value,
225  const ParameterT & alternative_value) const
226 {
227  bool got_parameter = get_parameter(name, value);
228  if (!got_parameter) {
229  value = alternative_value;
230  }
231  return got_parameter;
232 }
233 
234 } // namespace rclcpp_lifecycle
235 #endif // RCLCPP_LIFECYCLE__LIFECYCLE_NODE_IMPL_HPP_
default_context.hpp
lifecycle_node.hpp
rclcpp::ParameterValue
std::string
std::shared_ptr
std::move
T move(T... args)
create_publisher.hpp
rclcpp_lifecycle::LifecycleNode::declare_parameters
std::vector< ParameterT > declare_parameters(const std::string &namespace_, const std::map< std::string, ParameterT > &parameters)
Declare and initialize several parameters with the same namespace and type.
Definition: lifecycle_node_impl.hpp:149
std::pair
lifecycle_publisher.hpp
std::vector
rclcpp::ParameterValue::get
constexpr std::enable_if< type==ParameterType::PARAMETER_BOOL, const bool & >::type get() const
std::back_inserter
T back_inserter(T... args)
rclcpp::Parameter::get_value
decltype(auto) get_value() const
std::chrono::duration
create_subscription.hpp
rclcpp::Service
rclcpp::SubscriptionOptionsWithAllocator
type_support_decl.hpp
rclcpp_lifecycle::LifecycleNode::get_parameter
rclcpp::Parameter get_parameter(const std::string &name) const
Return the parameter by the given name.
create_service.hpp
rclcpp_lifecycle::LifecycleNode::create_publisher
std::shared_ptr< rclcpp_lifecycle::LifecyclePublisher< MessageT, AllocatorT > > create_publisher(const std::string &topic_name, const rclcpp::QoS &qos, const PublisherOptionsWithAllocator< AllocatorT > &options=(create_default_publisher_options< AllocatorT >()))
Create and return a Publisher.
Definition: lifecycle_node_impl.hpp:44
rclcpp_lifecycle::LifecycleNode::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: lifecycle_node_impl.hpp:84
subscription_options.hpp
rclcpp::QoS
rclcpp_lifecycle
Definition: lifecycle_node.hpp:90
rclcpp::ClientBase
rclcpp_lifecycle::LifecycleNode::create_subscription
std::shared_ptr< SubscriptionT > create_subscription(const std::string &topic_name, const rclcpp::QoS &qos, CallbackT &&callback, const SubscriptionOptionsWithAllocator< AllocatorT > &options=create_default_subscription_options< AllocatorT >(), typename MessageMemoryStrategyT::SharedPtr msg_mem_strat=(MessageMemoryStrategyT::create_default()))
Create and return a Subscription.
rclcpp_lifecycle::LifecycleNode::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: lifecycle_node_impl.hpp:122
rclcpp_lifecycle::LifecycleNode::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())
Declare and initialize a parameter, return the effective value.
std::map
std::transform
T transform(T... args)
parameter.hpp
visibility_control.h
rclcpp::PublisherOptionsWithAllocator< AllocatorT >
rclcpp::Client
event.hpp
std::map::begin
T begin(T... args)
rmw_qos_profile_t
std::string::empty
T empty(T... args)
rclcpp::WallTimer
intra_process_manager.hpp
rclcpp::Parameter
std::map::end
T end(T... args)
rclcpp_lifecycle::LifecycleNode::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.
Definition: lifecycle_node_impl.hpp:98
rclcpp_lifecycle::LifecycleNode::get_parameters
std::vector< rclcpp::Parameter > get_parameters(const std::vector< std::string > &names) const
Return the parameters by the given parameter names.
rclcpp_lifecycle::LifecycleNode::get_parameter_or
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 "parameter".
Definition: lifecycle_node_impl.hpp:222
rclcpp_lifecycle::LifecyclePublisher
brief child class of rclcpp Publisher class.
Definition: lifecycle_publisher.hpp:50