-
Notifications
You must be signed in to change notification settings - Fork 0
Expand file tree
/
Copy pathstress_test.cpp
More file actions
81 lines (67 loc) · 1.96 KB
/
stress_test.cpp
File metadata and controls
81 lines (67 loc) · 1.96 KB
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 2019 Apex.AI, Inc.
// All rights reserved.
#include <iostream>
#include <vector>
#include <rclcpp/rclcpp.hpp>
#include <rclcpp/executors.hpp>
#include <std_msgs/msg/int32.hpp>
constexpr std::chrono::milliseconds dds_participant_discovery_time{200};
rmw_qos_profile_t qos() {
auto profile = rmw_qos_profile_default;
profile.reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE;
profile.depth = 1000;
profile.history = RMW_QOS_POLICY_HISTORY_KEEP_LAST;
profile.durability = RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL;
return profile;
}
class pub_sub
{
public:
pub_sub()
: node(std::make_shared<rclcpp::Node>("pub_sub_test_node")),
node1(std::make_shared<rclcpp::Node>("pub_sub_test_node1")),
sub_ptr(node->create_subscription<std_msgs::msg::Int32>(
"pub_sub_test_topic",
std::bind(&pub_sub::cb, this, std::placeholders::_1),
qos())),
pub_ptr(node1->create_publisher<std_msgs::msg::Int32>("pub_sub_test_topic", qos())),
vect()
{
vect.reserve(100);
}
void cb(const std_msgs::msg::Int32::SharedPtr msg)
{
vect.push_back(*msg);
}
void publish(int i)
{
msg.data = i;
pub_ptr->publish(msg);
}
rclcpp::Node::SharedPtr node;
rclcpp::Node::SharedPtr node1;
rclcpp::Subscription<std_msgs::msg::Int32>::SharedPtr sub_ptr;
rclcpp::Publisher<std_msgs::msg::Int32>::SharedPtr pub_ptr;
std::vector<std_msgs::msg::Int32> vect;
std_msgs::msg::Int32 msg;
}; // class pub_sub
int main(int argc, char * argv[])
{
rclcpp::init(argc, argv);
pub_sub ps;
std::this_thread::sleep_for(dds_participant_discovery_time);
while(true) {
ps.vect.clear();
for(int i = 0; i < 100; ++i) {
ps.publish(i);
}
std::cout << "Done publishing" << std::endl;
rclcpp::executors::SingleThreadedExecutor exec;
exec.add_node(ps.node);
while(ps.vect.size() < 100) {
exec.spin_once();
std::cout << ps.vect.size() << std::endl;
}
std::cout << "Done receiving" << std::endl;
}
}