Skip to content

Commit 8f58373

Browse files
jamoralpjamoralp
authored
Add some examples for graph introspection (#34)
* Add example of graph usage in micro-ROS * Add topic endpoint info functions usage to the example * Add publisher/subscription count examples * Add rcl_count_publishers and rcl_count_subscribers usage examples * Graph service names and types example * Apply requested changes * Simplified rclc examples and moved into common graph folder
1 parent ef4cbca commit 8f58373

9 files changed

Lines changed: 376 additions & 1 deletion

File tree

‎rclc/CMakeLists.txt‎

Lines changed: 3 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -29,6 +29,9 @@ function(export_executable)
2929
)
3030
endfunction()
3131

32+
export_executable(graph_introspection graph_visualizer)
33+
export_executable(graph_introspection subscription_count)
34+
export_executable(graph_introspection publisher_count)
3235
export_executable(int32_publisher)
3336
export_executable(int32_subscriber)
3437
export_executable(configuration_example configured_publisher)

‎rclc/graph_introspection/graph_visualizer/CMakeLists.txt‎

Lines changed: 22 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,22 @@
1+
cmake_minimum_required(VERSION 3.5)
2+
3+
project(graph_visualizer LANGUAGES C)
4+
5+
find_package(ament_cmake REQUIRED)
6+
find_package(rcl REQUIRED)
7+
find_package(rclc REQUIRED)
8+
find_package(std_msgs REQUIRED)
9+
find_package(rmw_microxrcedds REQUIRED)
10+
11+
add_executable(${PROJECT_NAME} main.c)
12+
13+
ament_target_dependencies(${PROJECT_NAME}
14+
rcl
15+
rclc
16+
std_msgs
17+
rmw_microxrcedds
18+
)
19+
20+
install(TARGETS ${PROJECT_NAME}
21+
DESTINATION ${PROJECT_NAME}
22+
)

‎rclc/graph_introspection/graph_visualizer/main.c‎

Lines changed: 171 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,171 @@
1+
#include <rcl/rcl.h>
2+
#include <rcl/graph.h>
3+
#include <rcl/error_handling.h>
4+
5+
#include <stdio.h>
6+
#include <unistd.h>
7+
8+
#define RCCHECK(fn) { rcl_ret_t temp_rc = fn; if((temp_rc != RCL_RET_OK)){printf("Failed status on line %d: %d. Aborting.\n",__LINE__,(int)temp_rc); return 1;}}
9+
#define RCSOFTCHECK(fn) { rcl_ret_t temp_rc = fn; if((temp_rc != RCL_RET_OK)){printf("Failed status on line %d: %d. Continuing.\n",__LINE__,(int)temp_rc);}}
10+
11+
int main(int argc, const char * const * argv)
12+
{
13+
rcl_init_options_t options = rcl_get_zero_initialized_init_options();
14+
rcl_allocator_t allocator = rcl_get_default_allocator();
15+
RCCHECK(rcl_init_options_init(&options, allocator));
16+
17+
rcl_context_t context = rcl_get_zero_initialized_context();
18+
RCCHECK(rcl_init(argc, argv, &options, &context));
19+
20+
rcl_node_options_t node_ops = rcl_node_get_default_options();
21+
rcl_node_t node = rcl_get_zero_initialized_node();
22+
RCCHECK(rcl_node_init(&node, "microros_graph_tester", "", &context, &node_ops));
23+
24+
rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set();
25+
RCCHECK(rcl_wait_set_init(&wait_set, 0, 1, 0, 0, 0, 0, &context, allocator));
26+
27+
const rcl_guard_condition_t * graph_guard_condition =
28+
rcl_node_get_graph_guard_condition(&node);
29+
if (NULL == graph_guard_condition) {
30+
printf("Error: node graph guard condition is invalid\n");
31+
RCCHECK(rcl_node_fini(&node));
32+
RCCHECK(rcl_shutdown(&context));
33+
return 1;
34+
}
35+
36+
while (true)
37+
{
38+
RCSOFTCHECK(rcl_wait_set_clear(&wait_set));
39+
40+
size_t index;
41+
RCCHECK(rcl_wait_set_add_guard_condition(
42+
&wait_set, graph_guard_condition, &index));
43+
44+
rcl_ret_t unused = rcl_wait(&wait_set, RCL_MS_TO_NS(100));
45+
46+
for (size_t i = 0; i < wait_set.size_of_guard_conditions; ++i) {
47+
if (wait_set.guard_conditions[i]) {
48+
// Topic names and types
49+
rcl_names_and_types_t topic_names_and_types =
50+
rcl_get_zero_initialized_names_and_types();
51+
RCCHECK(rcl_get_topic_names_and_types(&node, &allocator, false, &topic_names_and_types));
52+
53+
printf("\n---------------------------------------------------------------\n");
54+
printf(" 'rcl_get_topic_names_and_types' result\n");
55+
printf("---------------------------------------------------------------\n");
56+
size_t topic_num = topic_names_and_types.names.size;
57+
printf("Current number of ROS 2 and micro-ROS topics: %lu\n", topic_num);
58+
59+
for (size_t j = 0; j < topic_num; ++j) {
60+
rcutils_string_array_t * topic_types = &topic_names_and_types.types[j];
61+
printf(" %lu) topic: '%s', types: ", j + 1, topic_names_and_types.names.data[j]);
62+
for (size_t k = 0; k < topic_types->size; ++k) {
63+
printf("%s", topic_types->data[k]);
64+
if (k < (topic_types->size - 1)) {
65+
printf(", ");
66+
} else {
67+
printf("\n");
68+
}
69+
}
70+
rcl_topic_endpoint_info_array_t publishers_info =
71+
rcl_get_zero_initialized_topic_endpoint_info_array();
72+
RCCHECK(rcl_get_publishers_info_by_topic(&node, &allocator,
73+
topic_names_and_types.names.data[j], false, &publishers_info));
74+
printf(" topic endpoint information:\n");
75+
for (size_t k = 0; k < publishers_info.size; ++k) {
76+
printf(" Node: '%s%s', type: '%s', '%s'\n",
77+
publishers_info.info_array[k].node_namespace,
78+
publishers_info.info_array[k].node_name,
79+
publishers_info.info_array[k].topic_type,
80+
RMW_ENDPOINT_PUBLISHER == publishers_info.info_array[k].endpoint_type ? "publisher" : "error");
81+
}
82+
rcl_topic_endpoint_info_array_fini(&publishers_info, &allocator);
83+
84+
rcl_topic_endpoint_info_array_t subscriptions_info =
85+
rcl_get_zero_initialized_topic_endpoint_info_array();
86+
RCCHECK(rcl_get_subscriptions_info_by_topic(&node, &allocator,
87+
topic_names_and_types.names.data[j], false, &subscriptions_info));
88+
for (size_t k = 0; k < subscriptions_info.size; ++k) {
89+
printf(" Node: '%s%s', type: '%s', '%s'\n",
90+
subscriptions_info.info_array[k].node_namespace,
91+
subscriptions_info.info_array[k].node_name,
92+
subscriptions_info.info_array[k].topic_type,
93+
RMW_ENDPOINT_SUBSCRIPTION == subscriptions_info.info_array[k].endpoint_type ? "subscription" : "error");
94+
}
95+
rcl_topic_endpoint_info_array_fini(&subscriptions_info, &allocator);
96+
}
97+
98+
RCSOFTCHECK(rcl_names_and_types_fini(&topic_names_and_types));
99+
100+
// Service names and types
101+
rcl_names_and_types_t service_names_and_types =
102+
rcl_get_zero_initialized_names_and_types();
103+
RCCHECK(rcl_get_service_names_and_types(&node, &allocator, &service_names_and_types));
104+
105+
printf("\n---------------------------------------------------------------\n");
106+
printf(" 'rcl_get_service_names_and_types' result\n");
107+
printf("---------------------------------------------------------------\n");
108+
size_t service_num = service_names_and_types.names.size;
109+
printf("Current number of ROS 2 and micro-ROS services: %lu\n", service_num);
110+
111+
for (size_t j = 0; j < service_num; ++j) {
112+
rcutils_string_array_t * service_types = &service_names_and_types.types[j];
113+
printf(" %lu) service: '%s', types: ", j + 1, service_names_and_types.names.data[j]);
114+
for (size_t k = 0; k < service_types->size; ++k) {
115+
printf("%s", service_types->data[k]);
116+
if (k < (service_types->size - 1)) {
117+
printf(", ");
118+
} else {
119+
printf("\n");
120+
}
121+
}
122+
}
123+
124+
RCSOFTCHECK(rcl_names_and_types_fini(&service_names_and_types));
125+
126+
// Node names
127+
rcutils_string_array_t node_names = rcutils_get_zero_initialized_string_array();
128+
rcutils_string_array_t node_namespaces = rcutils_get_zero_initialized_string_array();
129+
RCCHECK(rcl_get_node_names(&node, allocator, &node_names, &node_namespaces));
130+
131+
printf("\n---------------------------------------------------------------\n");
132+
printf(" 'rcl_get_node_names' result\n");
133+
printf("---------------------------------------------------------------\n");
134+
size_t node_num = node_names.size;
135+
printf("Current number of ROS 2 and micro-ROS nodes: %lu\n", node_num);
136+
137+
for (size_t j = 0; j < node_num; ++j) {
138+
printf(" %lu) node: '%s%s'\n", j + 1, node_namespaces.data[j], node_names.data[j]);
139+
printf(" 'rcl_get_publisher_names_and_types_by_node' result for this node:\n");
140+
rcl_names_and_types_t publisher_names_and_types = rcl_get_zero_initialized_names_and_types();
141+
RCCHECK(rcl_get_publisher_names_and_types_by_node(
142+
&node, &allocator, false, node_names.data[j], node_namespaces.data[j],
143+
&publisher_names_and_types));
144+
for (size_t k = 0; k < publisher_names_and_types.names.size; ++k) {
145+
printf(" %lu.%lu) %s, with types: ", j + 1, k + 1,
146+
publisher_names_and_types.names.data[k]);
147+
for (size_t l = 0; l < publisher_names_and_types.types[k].size; ++l) {
148+
printf("%s", publisher_names_and_types.types[k].data[l]);
149+
if (l < (publisher_names_and_types.types[k].size - 1)) {
150+
printf(", ");
151+
} else {
152+
printf("\n");
153+
}
154+
}
155+
}
156+
RCSOFTCHECK(rcl_names_and_types_fini(&publisher_names_and_types));
157+
}
158+
159+
if (RCUTILS_RET_OK != rcutils_string_array_fini(&node_names) ||
160+
RCUTILS_RET_OK != rcutils_string_array_fini(&node_namespaces)) {
161+
printf("Error while freeing rcutils resources\n");
162+
return 1;
163+
}
164+
}
165+
}
166+
}
167+
168+
RCCHECK(rcl_node_fini(&node));
169+
RCCHECK(rcl_shutdown(&context));
170+
return 0;
171+
}

‎rclc/graph_introspection/publisher_count/CMakeLists.txt‎

Lines changed: 22 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,22 @@
1+
cmake_minimum_required(VERSION 3.5)
2+
3+
project(publisher_count LANGUAGES C)
4+
5+
find_package(ament_cmake REQUIRED)
6+
find_package(rcl REQUIRED)
7+
find_package(rclc REQUIRED)
8+
find_package(rcl_interfaces REQUIRED)
9+
find_package(rmw_microxrcedds REQUIRED)
10+
11+
add_executable(${PROJECT_NAME} main.c)
12+
13+
ament_target_dependencies(${PROJECT_NAME}
14+
rcl
15+
rclc
16+
rcl_interfaces
17+
rmw_microxrcedds
18+
)
19+
20+
install(TARGETS ${PROJECT_NAME}
21+
DESTINATION ${PROJECT_NAME}
22+
)

‎rclc/graph_introspection/publisher_count/main.c‎

Lines changed: 67 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,67 @@
1+
#include <rcl/rcl.h>
2+
#include <rcl/graph.h>
3+
#include <rcl/error_handling.h>
4+
#include <rclc/rclc.h>
5+
#include <rclc/executor.h>
6+
7+
#include <rcl_interfaces/msg/parameter_event.h>
8+
9+
#include <stdio.h>
10+
#include <unistd.h>
11+
12+
#define RCCHECK(fn) { rcl_ret_t temp_rc = fn; if((temp_rc != RCL_RET_OK)){printf("Failed status on line %d: %d. Aborting.\n",__LINE__,(int)temp_rc); return 1;}}
13+
#define RCSOFTCHECK(fn) { rcl_ret_t temp_rc = fn; if((temp_rc != RCL_RET_OK)){printf("Failed status on line %d: %d. Continuing.\n",__LINE__,(int)temp_rc);}}
14+
15+
rcl_subscription_t subscription;
16+
rcl_node_t node;
17+
18+
void timer_callback(rcl_timer_t * timer, int64_t last_call_time)
19+
{
20+
(void) last_call_time;
21+
printf("Publisher count for subscription '/parameter_events':\n");
22+
size_t publisher_count = 0;
23+
RCSOFTCHECK(rcl_subscription_get_publisher_count(&subscription, &publisher_count));
24+
printf(" * Using 'rcl_subscription_get_publisher_count': %lu\n", publisher_count);
25+
RCSOFTCHECK(rcl_count_publishers(&node, "/parameter_events", &publisher_count));
26+
printf(" * Using 'rcl_count_publishers': %lu\n", publisher_count);
27+
}
28+
29+
int main(int argc, const char * const * argv)
30+
{
31+
printf("***Hint: test this example using 'ros2 run demo_nodes_cpp talker/listener' in another terminal***\n");
32+
rcl_allocator_t allocator = rcl_get_default_allocator();
33+
rclc_support_t support;
34+
35+
// create init options
36+
RCCHECK(rclc_support_init(&support, 0, NULL, &allocator));
37+
38+
// create_node
39+
RCCHECK(rclc_node_init_default(&node, "publisher_count_node", "", &support));
40+
41+
// create subscription
42+
RCCHECK(rclc_subscription_init_best_effort(
43+
&subscription,
44+
&node,
45+
ROSIDL_GET_MSG_TYPE_SUPPORT(rcl_interfaces, msg, ParameterEvent),
46+
"parameter_events"));
47+
48+
// create timer
49+
rcl_timer_t timer;
50+
const unsigned int timer_timeout = 1000;
51+
RCCHECK(rclc_timer_init_default(
52+
&timer,
53+
&support,
54+
RCL_MS_TO_NS(timer_timeout),
55+
timer_callback));
56+
57+
// create executor
58+
rclc_executor_t executor;
59+
RCCHECK(rclc_executor_init(&executor, &support.context, 1, &allocator));
60+
61+
RCCHECK(rclc_executor_add_timer(&executor, &timer));
62+
63+
rclc_executor_spin(&executor);
64+
65+
RCCHECK(rcl_subscription_fini(&subscription, &node));
66+
RCCHECK(rcl_node_fini(&node));
67+
}

‎rclc/graph_introspection/subscription_count/CMakeLists.txt‎

Lines changed: 22 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,22 @@
1+
cmake_minimum_required(VERSION 3.5)
2+
3+
project(subscription_count LANGUAGES C)
4+
5+
find_package(ament_cmake REQUIRED)
6+
find_package(rcl REQUIRED)
7+
find_package(rclc REQUIRED)
8+
find_package(rcl_interfaces REQUIRED)
9+
find_package(rmw_microxrcedds REQUIRED)
10+
11+
add_executable(${PROJECT_NAME} main.c)
12+
13+
ament_target_dependencies(${PROJECT_NAME}
14+
rcl
15+
rclc
16+
rcl_interfaces
17+
rmw_microxrcedds
18+
)
19+
20+
install(TARGETS ${PROJECT_NAME}
21+
DESTINATION ${PROJECT_NAME}
22+
)

‎rclc/graph_introspection/subscription_count/main.c‎

Lines changed: 67 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,67 @@
1+
#include <rcl/rcl.h>
2+
#include <rcl/graph.h>
3+
#include <rcl/error_handling.h>
4+
#include <rclc/rclc.h>
5+
#include <rclc/executor.h>
6+
7+
#include <rcl_interfaces/msg/parameter_event.h>
8+
9+
#include <stdio.h>
10+
#include <unistd.h>
11+
12+
#define RCCHECK(fn) { rcl_ret_t temp_rc = fn; if((temp_rc != RCL_RET_OK)){printf("Failed status on line %d: %d. Aborting.\n",__LINE__,(int)temp_rc); return 1;}}
13+
#define RCSOFTCHECK(fn) { rcl_ret_t temp_rc = fn; if((temp_rc != RCL_RET_OK)){printf("Failed status on line %d: %d. Continuing.\n",__LINE__,(int)temp_rc);}}
14+
15+
rcl_publisher_t publisher;
16+
rcl_node_t node;
17+
18+
void timer_callback(rcl_timer_t * timer, int64_t last_call_time)
19+
{
20+
(void) last_call_time;
21+
printf("Subscriptions count for publisher '/parameter_events':\n");
22+
size_t subscription_count = 0;
23+
RCSOFTCHECK(rcl_publisher_get_subscription_count(&publisher, &subscription_count));
24+
printf(" * Using 'rcl_publisher_get_subscription_count': %lu\n", subscription_count);
25+
RCSOFTCHECK(rcl_count_subscribers(&node, "/parameter_events", &subscription_count));
26+
printf(" * Using 'rcl_count_subscribers': %lu\n", subscription_count);
27+
}
28+
29+
int main(int argc, const char * const * argv)
30+
{
31+
printf("***Hint: test this example using 'ros2 run demo_nodes_cpp talker/listener' in another terminal***\n");
32+
rcl_allocator_t allocator = rcl_get_default_allocator();
33+
rclc_support_t support;
34+
35+
// create init options
36+
RCCHECK(rclc_support_init(&support, 0, NULL, &allocator));
37+
38+
// create_node
39+
RCCHECK(rclc_node_init_default(&node, "subscription_count_node", "", &support));
40+
41+
// create publisher
42+
RCCHECK(rclc_publisher_init_best_effort(
43+
&publisher,
44+
&node,
45+
ROSIDL_GET_MSG_TYPE_SUPPORT(rcl_interfaces, msg, ParameterEvent),
46+
"parameter_events"));
47+
48+
// create timer
49+
rcl_timer_t timer;
50+
const unsigned int timer_timeout = 1000;
51+
RCCHECK(rclc_timer_init_default(
52+
&timer,
53+
&support,
54+
RCL_MS_TO_NS(timer_timeout),
55+
timer_callback));
56+
57+
// create executor
58+
rclc_executor_t executor;
59+
RCCHECK(rclc_executor_init(&executor, &support.context, 1, &allocator));
60+
61+
RCCHECK(rclc_executor_add_timer(&executor, &timer));
62+
63+
rclc_executor_spin(&executor);
64+
65+
RCCHECK(rcl_publisher_fini(&publisher, &node));
66+
RCCHECK(rcl_node_fini(&node));
67+
}

‎rclc/int32_subscriber/main.c‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -45,7 +45,7 @@ int main()
4545
unsigned int rcl_wait_timeout = 1000; // in ms
4646
RCCHECK(rclc_executor_set_timeout(&executor, RCL_MS_TO_NS(rcl_wait_timeout)));
4747
RCCHECK(rclc_executor_add_subscription(&executor, &subscriber, &msg, &subscription_callback, ON_NEW_DATA));
48-
48+
4949
rclc_executor_spin(&executor);
5050

5151
RCCHECK(rcl_subscription_fini(&subscriber, &node));

0 commit comments

Comments
 (0)