Autoware.Auto
periodic_publisher.hpp
Go to the documentation of this file.
1 // Copyright 2019 Christopher Ho
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 #ifndef MOTION_TESTING_NODES__PERIODIC_PUBLISHER_HPP_
15 #define MOTION_TESTING_NODES__PERIODIC_PUBLISHER_HPP_
16 
19 
20 #include <rclcpp/rclcpp.hpp>
21 
22 #include <chrono>
23 #include <list>
24 #include <string>
25 
26 namespace motion
27 {
28 namespace motion_testing_nodes
29 {
30 
31 struct MOTION_TESTING_NODES_PUBLIC PeriodCountTopic
32 {
33  std::chrono::nanoseconds period;
34  std::size_t count;
35  std::string topic;
36 };
37 
38 template<typename T>
39 class MOTION_TESTING_NODES_PUBLIC PeriodicPublisher
40 {
41 public:
42  PeriodicPublisher(rclcpp::Node & node, const PeriodCountTopic & cfg, std::list<T> msgs = {})
43  : m_msgs{msgs}
44  {
45  // Prune list if too long
46  while (m_msgs.size() > cfg.count) {
47  m_msgs.pop_back();
48  }
49  // Fill remainder of list
50  for (auto idx = m_msgs.size(); idx < cfg.count; ++idx) {
51  m_msgs.emplace_back(T{});
52  }
53  // Publisher
54  m_pub =
55  node.create_publisher<T>(cfg.topic, rclcpp::QoS{cfg.count}.transient_local().reliable());
56  // Timer
57  m_timer = node.create_wall_timer(
58  cfg.period, [this]() -> void {
59  if (!m_msgs.empty()) {
60  const auto msg = m_msgs.front();
61  m_pub->publish(msg);
62  m_msgs.pop_front();
63  }
64  });
65  }
66 
67  bool done() const noexcept {return m_msgs.empty();}
68 
69  void match(const std::size_t min_subscribers) const
70  {
71  (void)wait_for_matched(*m_pub, min_subscribers, std::chrono::seconds{3LL});
72  }
73 
74 private:
75  std::list<T> m_msgs;
76  typename rclcpp::Publisher<T>::SharedPtr m_pub{};
77  rclcpp::TimerBase::SharedPtr m_timer{};
78 }; // class PeriodicPublisher
79 
80 } // namespace motion_testing_nodes
81 } // namespace motion
82 
83 #endif // MOTION_TESTING_NODES__PERIODIC_PUBLISHER_HPP_
motion::motion_testing_nodes::PeriodCountTopic::topic
std::string topic
Definition: periodic_publisher.hpp:35
wait_for_matched.hpp
motion::motion_testing_nodes::PeriodCountTopic::count
std::size_t count
Definition: periodic_publisher.hpp:34
motion::motion_testing_nodes::wait_for_matched
MOTION_TESTING_NODES_PUBLIC std::chrono::nanoseconds wait_for_matched(const rclcpp::PublisherBase &pub, const std::size_t minimum_match_count, const std::chrono::nanoseconds timeout=std::chrono::seconds{5LL}, const std::chrono::nanoseconds poll_period=std::chrono::milliseconds{1LL})
Definition: wait_for_matched.cpp:24
motion::motion_testing_nodes::PeriodicPublisher::PeriodicPublisher
PeriodicPublisher(rclcpp::Node &node, const PeriodCountTopic &cfg, std::list< T > msgs={})
Definition: periodic_publisher.hpp:42
sys_info_node.node
node
Definition: sys_info_node.py:23
motion::motion_testing_nodes::PeriodCountTopic::period
std::chrono::nanoseconds period
Definition: periodic_publisher.hpp:33
motion::motion_testing_nodes::PeriodicPublisher::match
void match(const std::size_t min_subscribers) const
Definition: periodic_publisher.hpp:69
visibility_control.hpp
motion::motion_testing_nodes::PeriodCountTopic
Definition: periodic_publisher.hpp:31
motion
Definition: controller_base.hpp:31
motion::motion_testing_nodes::PeriodicPublisher
Definition: periodic_publisher.hpp:39
motion::motion_testing_nodes::PeriodicPublisher::done
bool done() const noexcept
Definition: periodic_publisher.hpp:67