Skip to content

Commit 8d51350

Browse files
jamoralpjamoralp
authored
Add client-to-agent ping example (#38)
* Add client-to-agent ping example * Fix CI * Fix CI: fix checkout path
1 parent ca78714 commit 8d51350

File tree

3 files changed

+181
-0
lines changed

3 files changed

+181
-0
lines changed

‎rclc/CMakeLists.txt‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -43,6 +43,7 @@ export_executable(fibonacci_action_client)
4343
export_executable(int32_multinode)
4444
export_executable(int32_publisher_subscriber)
4545
export_executable(ping_pong)
46+
export_executable(ping_uros_agent)
4647
export_executable(fragmented_publication)
4748
export_executable(fragmented_subscription)
4849
export_executable(complex_msg_publisher)

‎rclc/ping_uros_agent/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(ping_uros_agent LANGUAGES C)
4+
5+
find_package(ament_cmake REQUIRED)
6+
find_package(rcl REQUIRED)
7+
find_package(rclc REQUIRED)
8+
find_package(rmw_microxrcedds REQUIRED)
9+
find_package(std_msgs REQUIRED)
10+
11+
add_executable(${PROJECT_NAME} main.c)
12+
13+
ament_target_dependencies(${PROJECT_NAME}
14+
rcl
15+
rclc
16+
rmw_microxrcedds
17+
std_msgs
18+
)
19+
20+
install(TARGETS ${PROJECT_NAME}
21+
DESTINATION ${PROJECT_NAME}
22+
)

‎rclc/ping_uros_agent/main.c‎

Lines changed: 158 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,158 @@
1+
#include <rcl/rcl.h>
2+
#include <rcl/error_handling.h>
3+
#include <rclc/rclc.h>
4+
#include <rclc/executor.h>
5+
6+
#include <std_msgs/msg/string.h>
7+
8+
#include <stdio.h>
9+
#include <unistd.h>
10+
11+
#include <rmw_uros/options.h>
12+
13+
#include <rosidl_runtime_c/string_functions.h>
14+
15+
#define RCCHECK(fn) \
16+
{\
17+
rcl_ret_t temp_rc = fn;\
18+
if (RCL_RET_OK != temp_rc) {\
19+
printf("Failed status on line %d: %d. Aborting.\n", __LINE__, (int)temp_rc);\
20+
return 1;\
21+
}\
22+
}
23+
24+
#define RCSOFTCHECK(fn) \
25+
{\
26+
rcl_ret_t temp_rc = fn;\
27+
if(RCL_RET_OK != temp_rc) {\
28+
printf("Failed status on line %d: %d. Continuing.\n", __LINE__, (int)temp_rc);\
29+
}\
30+
}
31+
32+
rcl_publisher_t publisher;
33+
std_msgs__msg__String msg;
34+
35+
int usage()
36+
{
37+
printf("Usage: ping_uros_agent <mode>\nModes:\n");
38+
printf("\t* 'basic': checks that the micro-ROS Agent is up or down, and exits\n");
39+
printf("\t* 'interactive': starts a basic micro-ROS application ");
40+
printf("if the Agent is up, or waits for the user to start it in a loop\n");
41+
return 1;
42+
}
43+
44+
void timer_callback(rcl_timer_t * timer, int64_t last_call_time)
45+
{
46+
(void) last_call_time;
47+
if (NULL != timer) {
48+
RCSOFTCHECK(rcl_publish(&publisher, &msg, NULL));
49+
printf("Sent: '%s'\n", msg.data.data);
50+
51+
/**
52+
* If agent suddently goes down, we will exit the application.
53+
* This proves that `rmw_uros_ping_agent` functionality can be used
54+
* also when a micro-ROS node is already up and running (and hence, the
55+
* transport has been initialized in the RMW context).
56+
*/
57+
printf("\nPinging agent again...\n");
58+
if (RMW_RET_OK != rmw_uros_ping_agent(100, 1)) {
59+
printf("micro-ROS agent has stopped. Exiting...\n");
60+
exit(1);
61+
} else {
62+
printf("Agent is still up!\n\n");
63+
}
64+
}
65+
}
66+
67+
int main(int argc, char ** argv)
68+
{
69+
/**
70+
* Parse arguments
71+
*/
72+
if (2 != argc) {
73+
return usage();
74+
}
75+
76+
const char * mode = argv[1];
77+
78+
/**
79+
* Basic mode
80+
*/
81+
if (0 == strcmp("basic", mode)) {
82+
/**
83+
* Ping should work even without a micro-ROS node active.
84+
*/
85+
rmw_ret_t ping_result = rmw_uros_ping_agent(1000, 5);
86+
87+
if (RMW_RET_OK == ping_result) {
88+
printf("Success! micro-ROS Agent is up.\n");
89+
return 0;
90+
} else {
91+
printf("Sorry, the micro-ROS Agent is not available.\n");
92+
return 1;
93+
}
94+
} else if (0 == strcmp("interactive", mode)) {
95+
/**
96+
* Loop until micro-ROS Agent is up
97+
*/
98+
while (RMW_RET_OK != rmw_uros_ping_agent(1000, 1)) {
99+
printf("Please, start your micro-ROS Agent first\n");
100+
}
101+
102+
/**
103+
* Start the micro-ROS basic application
104+
*/
105+
rcl_allocator_t allocator = rcl_get_default_allocator();
106+
rclc_support_t support;
107+
108+
// Create init options
109+
RCCHECK(rclc_support_init(&support, 0, NULL, &allocator));
110+
111+
// Create node
112+
rcl_node_t node;
113+
RCCHECK(rclc_node_init_default(
114+
&node, "ping_uros_agent_publisher", "", &support));
115+
116+
// Create publisher
117+
RCCHECK(rclc_publisher_init_default(
118+
&publisher,
119+
&node,
120+
ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, String),
121+
"ping_uros_agent_topic"));
122+
123+
// Create timer
124+
rcl_timer_t timer;
125+
const unsigned int timer_timeout = 1000;
126+
RCCHECK(rclc_timer_init_default(
127+
&timer,
128+
&support,
129+
RCL_MS_TO_NS(timer_timeout),
130+
timer_callback));
131+
132+
// Create executor
133+
rclc_executor_t executor;
134+
RCCHECK(rclc_executor_init(&executor, &support.context, 1, &allocator));
135+
RCCHECK(rclc_executor_add_timer(&executor, &timer));
136+
137+
// Init data
138+
(void) rosidl_runtime_c__String__init(&msg.data);
139+
(void) rosidl_runtime_c__String__assign(
140+
&msg.data,
141+
"This is a message sent after pinging micro-ROS Agent");
142+
143+
// Spin executor
144+
rclc_executor_spin(&executor);
145+
146+
// Free micro-ROS resources before exiting
147+
(void) rosidl_runtime_c__String__fini(&msg.data);
148+
149+
RCCHECK(rclc_executor_fini(&executor));
150+
RCCHECK(rcl_publisher_fini(&publisher, &node));
151+
RCCHECK(rcl_node_fini(&node));
152+
RCCHECK(rclc_support_fini(&support));
153+
} else {
154+
return usage();
155+
}
156+
157+
return 0;
158+
}

0 commit comments

Comments
 (0)