forked from nachovizzo/cyclone_dds_leak
-
Notifications
You must be signed in to change notification settings - Fork 0
/
tf2_leaky_node.cpp
39 lines (33 loc) · 1.18 KB
/
tf2_leaky_node.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
#include <tf2_ros/buffer.h>
#include <tf2_ros/transform_listener.h>
#include <iostream>
#include <memory>
#include <rclcpp/node.hpp>
#include <string>
#include <vector>
class LeakyNode : public rclcpp::Node {
public:
explicit LeakyNode(const std::string &name) : Node(name) {
this->declare_parameter("my_parameter", "world");
tf_buffer = std::make_unique<tf2_ros::Buffer>(this->get_clock());
tf_listener = std::make_unique<tf2_ros::TransformListener>(*tf_buffer);
}
std::unique_ptr<tf2_ros::TransformListener> tf_listener;
std::unique_ptr<tf2_ros::Buffer> tf_buffer;
};
int main(int argc, char **argv) {
rclcpp::init(argc, argv);
rclcpp::executors::SingleThreadedExecutor exec;
std::vector<std::shared_ptr<LeakyNode>> nodes;
const size_t N_NODES = 10;
std::cerr << "Creating " << N_NODES << " nodes" << std::endl;
for (int i = 0; i < N_NODES; ++i) {
nodes.push_back(std::make_shared<LeakyNode>(("leaky_node" + std::to_string(i))));
}
std::cerr << "Adding to executor now" << std::endl;
for (auto &node : nodes) exec.add_node(node);
// spin and then shutdoun
exec.spin();
rclcpp::shutdown();
return 0;
}