Skip to content

Commit d78a56f

Browse files
RaphvKRaphvK
committed
add tests for message link instrumentation
Signed-off-by: Raphael van Kempen <[email protected]>
1 parent 1e289e6 commit d78a56f

File tree

5 files changed

+513
-0
lines changed

5 files changed

+513
-0
lines changed

‎test_tracetools/CMakeLists.txt‎

Lines changed: 32 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -103,6 +103,28 @@ if(BUILD_TESTING)
103103
rclcpp::rclcpp
104104
)
105105

106+
if(NOT TRACETOOLS_DISABLED)
107+
add_executable(test_message_link_partial_sync
108+
src/test_message_link_partial_sync.cpp
109+
)
110+
target_link_libraries(test_message_link_partial_sync PRIVATE
111+
${PROJECT_NAME}_mark_process
112+
rclcpp::rclcpp
113+
tracetools::tracetools
114+
${std_msgs_TARGETS}
115+
)
116+
117+
add_executable(test_message_link_periodic_async
118+
src/test_message_link_periodic_async.cpp
119+
)
120+
target_link_libraries(test_message_link_periodic_async PRIVATE
121+
${PROJECT_NAME}_mark_process
122+
rclcpp::rclcpp
123+
tracetools::tracetools
124+
${std_msgs_TARGETS}
125+
)
126+
endif()
127+
106128
add_executable(test_ping
107129
src/test_ping.cpp
108130
)
@@ -180,6 +202,14 @@ if(BUILD_TESTING)
180202
DESTINATION lib/${PROJECT_NAME}
181203
)
182204

205+
if(NOT TRACETOOLS_DISABLED)
206+
install(TARGETS
207+
test_message_link_partial_sync
208+
test_message_link_periodic_async
209+
DESTINATION lib/${PROJECT_NAME}
210+
)
211+
endif()
212+
183213
find_package(ament_lint_auto REQUIRED)
184214
ament_lint_auto_find_test_dependencies()
185215

@@ -224,6 +254,8 @@ if(BUILD_TESTING)
224254
test/test_client.py
225255
test/test_generic_pub_sub.py
226256
test/test_generic_subscription.py
257+
test/test_message_link_partial_sync.py
258+
test/test_message_link_periodic_async.py
227259
test/test_pub_sub.py
228260
test/test_publisher.py
229261
test/test_service.py

‎test_tracetools/src/test_message_link_partial_sync.cpp‎

Lines changed: 116 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,116 @@
1+
// Copyright 2019 Robert Bosch GmbH
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+
#include <memory>
16+
17+
#include "rclcpp/rclcpp.hpp"
18+
#include "std_msgs/msg/string.hpp"
19+
#include "test_tracetools/mark_process.hpp"
20+
#include "tracetools/tracetools.h"
21+
22+
#define NODE_NAME "test_message_link_partial_sync"
23+
#define SUB_TOPIC_NAME_1 "ping"
24+
#define SUB_TOPIC_NAME_2 "pong"
25+
#define PUB_TOPIC_NAME "combined"
26+
27+
class MessageLinkPartialNode : public rclcpp::Node
28+
{
29+
public:
30+
MessageLinkPartialNode(rclcpp::NodeOptions options, bool do_only_one)
31+
: Node(NODE_NAME, options), do_only_one_(do_only_one)
32+
{
33+
sub1_ = this->create_subscription<std_msgs::msg::String>(
34+
SUB_TOPIC_NAME_1,
35+
rclcpp::QoS(10).transient_local(),
36+
std::bind(&MessageLinkPartialNode::callback1, this, std::placeholders::_1));
37+
sub2_ = this->create_subscription<std_msgs::msg::String>(
38+
SUB_TOPIC_NAME_2,
39+
rclcpp::QoS(10),
40+
std::bind(&MessageLinkPartialNode::callback2, this, std::placeholders::_1));
41+
pub_ = this->create_publisher<std_msgs::msg::String>(
42+
PUB_TOPIC_NAME,
43+
rclcpp::QoS(10));
44+
45+
// Manually link sub1_ as additional input to pub_ for tracing. sub2_ is
46+
// directly traced via publication during callback function.
47+
std::vector<const void *> link_subs = {
48+
static_cast<const void *>(sub1_->get_subscription_handle().get())
49+
};
50+
std::vector<const void *> link_pubs = {
51+
static_cast<const void *>(pub_->get_publisher_handle().get())
52+
};
53+
TRACETOOLS_TRACEPOINT(
54+
message_link_partial_sync, link_subs.data(), link_subs.size(), link_pubs.data(),
55+
link_pubs.size()
56+
);
57+
}
58+
59+
explicit MessageLinkPartialNode(rclcpp::NodeOptions options)
60+
: MessageLinkPartialNode(options, true) {}
61+
62+
private:
63+
void callback1(const std_msgs::msg::String::ConstSharedPtr msg)
64+
{
65+
RCLCPP_INFO(this->get_logger(), "[sub1] %s", msg->data.c_str());
66+
sub1_message = msg;
67+
}
68+
69+
void callback2(const std_msgs::msg::String::ConstSharedPtr msg)
70+
{
71+
RCLCPP_INFO(this->get_logger(), "[sub2] %s", msg->data.c_str());
72+
if(!sub1_message) {
73+
RCLCPP_WARN(this->get_logger(), "Did not receive message from sub1 yet.");
74+
return;
75+
}
76+
auto next_msg = std::make_shared<std_msgs::msg::String>();
77+
next_msg->data = "combined string including both: " +
78+
sub1_message->data + " and: " + msg->data;
79+
RCLCPP_INFO(this->get_logger(), "%s", next_msg->data.c_str());
80+
pub_->publish(*next_msg);
81+
if (do_only_one_) {
82+
rclcpp::shutdown();
83+
}
84+
}
85+
86+
rclcpp::Subscription<std_msgs::msg::String>::SharedPtr sub1_;
87+
rclcpp::Subscription<std_msgs::msg::String>::SharedPtr sub2_;
88+
rclcpp::Publisher<std_msgs::msg::String>::SharedPtr pub_;
89+
bool do_only_one_;
90+
std_msgs::msg::String::ConstSharedPtr sub1_message = nullptr;
91+
};
92+
93+
int main(int argc, char * argv[])
94+
{
95+
test_tracetools::mark_trace_test_process();
96+
97+
bool do_only_one = true;
98+
for (int i = 0; i < argc; ++i) {
99+
if (strncmp(argv[i], "do_more", 7) == 0) {
100+
do_only_one = false;
101+
}
102+
}
103+
104+
rclcpp::init(argc, argv);
105+
106+
rclcpp::executors::SingleThreadedExecutor exec;
107+
auto pong_node = std::make_shared<MessageLinkPartialNode>(rclcpp::NodeOptions(), do_only_one);
108+
exec.add_node(pong_node);
109+
110+
printf("spinning\n");
111+
exec.spin();
112+
113+
// Will actually be called inside the node's callback
114+
rclcpp::shutdown();
115+
return 0;
116+
}

‎test_tracetools/src/test_message_link_periodic_async.cpp‎

Lines changed: 138 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,138 @@
1+
// Copyright 2019 Robert Bosch GmbH
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+
#include <memory>
16+
#include <chrono>
17+
18+
#include "rclcpp/rclcpp.hpp"
19+
#include "std_msgs/msg/string.hpp"
20+
#include "test_tracetools/mark_process.hpp"
21+
#include "tracetools/tracetools.h"
22+
23+
#define NODE_NAME "test_message_link_periodic_async"
24+
#define SUB_TOPIC_NAME_1 "ping"
25+
#define SUB_TOPIC_NAME_2 "pong"
26+
#define PUB_TOPIC_NAME "combined"
27+
28+
class MessageLinkPeriodicNode : public rclcpp::Node
29+
{
30+
public:
31+
MessageLinkPeriodicNode(rclcpp::NodeOptions options, bool do_only_one)
32+
: Node(NODE_NAME, options), do_only_one_(do_only_one)
33+
{
34+
sub1_ = this->create_subscription<std_msgs::msg::String>(
35+
SUB_TOPIC_NAME_1,
36+
rclcpp::QoS(10).transient_local(),
37+
std::bind(&MessageLinkPeriodicNode::callback1, this, std::placeholders::_1));
38+
sub2_ = this->create_subscription<std_msgs::msg::String>(
39+
SUB_TOPIC_NAME_2,
40+
rclcpp::QoS(10),
41+
std::bind(&MessageLinkPeriodicNode::callback2, this, std::placeholders::_1));
42+
pub_ = this->create_publisher<std_msgs::msg::String>(
43+
PUB_TOPIC_NAME,
44+
rclcpp::QoS(10));
45+
46+
// Manually link both subscribers to periodically publishing publisher
47+
std::vector<const void *> link_subs = {
48+
static_cast<const void *>(sub1_->get_subscription_handle().get()),
49+
static_cast<const void *>(sub2_->get_subscription_handle().get())
50+
};
51+
std::vector<const void *> link_pubs = {
52+
static_cast<const void *>(
53+
pub_->get_publisher_handle().get()
54+
)
55+
};
56+
TRACETOOLS_TRACEPOINT(
57+
message_link_periodic_async, link_subs.data(), link_subs.size(), link_pubs.data(),
58+
link_pubs.size()
59+
);
60+
61+
// Create periodic timer for time publishing
62+
timer_ = this->create_wall_timer(
63+
std::chrono::milliseconds(500),
64+
std::bind(&MessageLinkPeriodicNode::timer_callback, this));
65+
}
66+
67+
explicit MessageLinkPeriodicNode(rclcpp::NodeOptions options)
68+
: MessageLinkPeriodicNode(options, true) {}
69+
70+
private:
71+
void timer_callback()
72+
{
73+
if(!ping1_message || !ping2_message) {
74+
RCLCPP_WARN(this->get_logger(), "Waiting for both input messages...");
75+
return;
76+
}
77+
auto combined_msg = std::make_shared<std_msgs::msg::String>();
78+
combined_msg->data = "Periodic: " + ping1_message->data + " and " + ping2_message->data;
79+
RCLCPP_INFO(this->get_logger(), "Publishing: %s", combined_msg->data.c_str());
80+
pub_->publish(*combined_msg);
81+
}
82+
void callback1(const std_msgs::msg::String::ConstSharedPtr msg)
83+
{
84+
RCLCPP_INFO(this->get_logger(), "[sub1] %s", msg->data.c_str());
85+
ping1_message = msg;
86+
}
87+
88+
void callback2(const std_msgs::msg::String::ConstSharedPtr msg)
89+
{
90+
RCLCPP_INFO(this->get_logger(), "[sub2] %s", msg->data.c_str());
91+
ping2_message = msg;
92+
if(!ping1_message) {
93+
RCLCPP_WARN(this->get_logger(), "Did not receive message from ping1 yet.");
94+
return;
95+
}
96+
auto next_msg = std::make_shared<std_msgs::msg::String>();
97+
next_msg->data = "combined string including both: " +
98+
ping1_message->data + " and: " + msg->data;
99+
RCLCPP_INFO(this->get_logger(), "%s", next_msg->data.c_str());
100+
pub_->publish(*next_msg);
101+
if (do_only_one_) {
102+
rclcpp::shutdown();
103+
}
104+
}
105+
106+
rclcpp::Subscription<std_msgs::msg::String>::SharedPtr sub1_;
107+
rclcpp::Subscription<std_msgs::msg::String>::SharedPtr sub2_;
108+
rclcpp::Publisher<std_msgs::msg::String>::SharedPtr pub_;
109+
rclcpp::TimerBase::SharedPtr timer_;
110+
bool do_only_one_;
111+
std_msgs::msg::String::ConstSharedPtr ping1_message = nullptr;
112+
std_msgs::msg::String::ConstSharedPtr ping2_message = nullptr;
113+
};
114+
115+
int main(int argc, char * argv[])
116+
{
117+
test_tracetools::mark_trace_test_process();
118+
119+
bool do_only_one = true;
120+
for (int i = 0; i < argc; ++i) {
121+
if (strncmp(argv[i], "do_more", 7) == 0) {
122+
do_only_one = false;
123+
}
124+
}
125+
126+
rclcpp::init(argc, argv);
127+
128+
rclcpp::executors::SingleThreadedExecutor exec;
129+
auto pong_node = std::make_shared<MessageLinkPeriodicNode>(rclcpp::NodeOptions(), do_only_one);
130+
exec.add_node(pong_node);
131+
132+
printf("spinning\n");
133+
exec.spin();
134+
135+
// Will actually be called inside the node's callback
136+
rclcpp::shutdown();
137+
return 0;
138+
}

0 commit comments

Comments
 (0)