-
Notifications
You must be signed in to change notification settings - Fork 7
/
minimal_atomic_message.cpp
81 lines (71 loc) · 2.54 KB
/
minimal_atomic_message.cpp
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
// Copyright 2022 Carlos San Vicente
// Copyright 2016 Open Source Robotics Foundation, Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include <chrono>
#include <memory>
#include <string>
#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/u_int32.hpp"
using namespace std::chrono_literals;
static_assert(std::atomic<std_msgs::msg::UInt32>::is_always_lock_free);
// Bigger data types are not lock free, this asserts would fail
// static_assert(std::atomic<std_msgs::msg::String>::is_always_lock_free);
class MinimalPublisher : public rclcpp::Node
{
public:
MinimalPublisher()
: Node("minimal_publisher"), msg_(std_msgs::msg::UInt32())
{
// Make the callback groups reentrant so they can run concurrently
auto reentrant_callback_group =
this->create_callback_group(rclcpp::CallbackGroupType::Reentrant);
rclcpp::PublisherOptions options;
options.callback_group = reentrant_callback_group;
for (auto pub_index = 0; pub_index < 10; pub_index++) {
auto publisher = this->create_publisher<std_msgs::msg::UInt32>("topic", 10, options);
auto timer_callback =
[this, pub_index, publisher]() -> void {
auto msg = increment_message_data();
RCLCPP_INFO(this->get_logger(), "Publisher %d: '%d'", pub_index, msg.data);
publisher->publish(msg);
};
timers_.push_back(this->create_wall_timer(100ms, timer_callback, reentrant_callback_group));
}
}
std_msgs::msg::UInt32 increment_message_data()
{
auto old_value = msg_.load();
auto new_value = old_value;
new_value.data++;
do {
if (msg_.compare_exchange_strong(old_value, new_value)) {
break;
}
} while(true);
return new_value;
}
private:
std::vector<rclcpp::TimerBase::SharedPtr> timers_;
std::atomic<std_msgs::msg::UInt32> msg_;
};
int main(int argc, char * argv[])
{
rclcpp::init(argc, argv);
auto node = std::make_shared<MinimalPublisher>();
rclcpp::executors::MultiThreadedExecutor executor;
executor.add_node(node);
executor.spin();
rclcpp::shutdown();
return 0;
}