Skip to content

Commit b6997b5

Browse files
pablogs9pablogs9
authored
Creating ping pong app (#22)
1 parent ab8c451 commit b6997b5

3 files changed

Lines changed: 170 additions & 0 deletions

File tree

‎rcl/CMakeLists.txt‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -40,6 +40,7 @@ export_executable(fibonacci_action_client)
4040
export_executable(int32_multinode)
4141
export_executable(guard_condition)
4242
export_executable(int32_publisher_subscriber)
43+
export_executable(ping_pong)
4344

4445
if(EXISTS ${CMAKE_BINARY_DIR}/temp_install/)
4546
install(

‎rcl/ping_pong/CMakeLists.txt‎

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

‎rcl/ping_pong/main.c‎

Lines changed: 149 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,149 @@
1+
#include <rcl/rcl.h>
2+
#include <rcl/error_handling.h>
3+
#include <std_msgs/msg/header.h>
4+
5+
#include <rmw_uros/options.h>
6+
7+
#include <stdio.h>
8+
#include <unistd.h>
9+
#include <pthread.h>
10+
11+
#define STRING_BUFFER_LEN 100
12+
13+
// Publishing ping guard condition thread
14+
void * trigger_guard_condition(void *args){
15+
rcl_guard_condition_t * guard_condition = (rcl_guard_condition_t *)args;
16+
17+
while(true){
18+
rcl_trigger_guard_condition(guard_condition);
19+
sleep(5);
20+
}
21+
}
22+
23+
int main(int argc, const char * const * argv)
24+
{
25+
//Init RCL options
26+
rcl_init_options_t options = rcl_get_zero_initialized_init_options();
27+
rcl_init_options_init(&options, rcl_get_default_allocator());
28+
29+
// Init RCL context
30+
rcl_context_t context = rcl_get_zero_initialized_context();
31+
rcl_init(0, NULL, &options, &context);
32+
33+
// Create a node
34+
rcl_node_options_t node_ops = rcl_node_get_default_options();
35+
rcl_node_t node = rcl_get_zero_initialized_node();
36+
rcl_node_init(&node, "pingpong_node", "", &context, &node_ops);
37+
38+
// Create a reliable ping publisher
39+
rcl_publisher_options_t ping_publisher_ops = rcl_publisher_get_default_options();
40+
rcl_publisher_t ping_publisher = rcl_get_zero_initialized_publisher();
41+
rcl_publisher_init(&ping_publisher, &node, ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Header), "/microROS/ping", &ping_publisher_ops);
42+
43+
// Create a best effort pong publisher
44+
rcl_publisher_options_t pong_publisher_ops = rcl_publisher_get_default_options();
45+
pong_publisher_ops.qos.reliability = RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT;
46+
rcl_publisher_t pong_publisher = rcl_get_zero_initialized_publisher();
47+
rcl_publisher_init(&pong_publisher, &node, ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Header), "/microROS/pong", &pong_publisher_ops);
48+
49+
// Create a best effort pong subscriber
50+
rcl_subscription_options_t pong_subscription_ops = rcl_subscription_get_default_options();
51+
pong_subscription_ops.qos.reliability = RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT;
52+
rcl_subscription_t pong_subscription = rcl_get_zero_initialized_subscription();
53+
rcl_subscription_init(&pong_subscription, &node, ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Header), "/microROS/pong", &pong_subscription_ops);
54+
55+
// Create a best effort ping subscriber
56+
rcl_subscription_options_t ping_subscription_ops = rcl_subscription_get_default_options();
57+
ping_subscription_ops.qos.reliability = RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT;
58+
rcl_subscription_t ping_subscription = rcl_get_zero_initialized_subscription();
59+
rcl_subscription_init(&ping_subscription, &node, ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Header), "/microROS/ping", &ping_subscription_ops);
60+
61+
// Create guard condition
62+
rcl_guard_condition_t guard_condition = rcl_get_zero_initialized_guard_condition();
63+
rcl_guard_condition_options_t guard_condition_options = rcl_guard_condition_get_default_options();
64+
rcl_guard_condition_init(&guard_condition, &context, guard_condition_options);
65+
66+
pthread_t guard_condition_thread;
67+
pthread_create(&guard_condition_thread, NULL, trigger_guard_condition, &guard_condition);
68+
69+
// Create a wait set
70+
rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set();
71+
rcl_wait_set_init(&wait_set, 2, 1, 0, 0, 0, 0, &context, rcl_get_default_allocator());
72+
73+
// Create and allocate the pingpong publication message
74+
std_msgs__msg__Header msg;
75+
char msg_buffer[STRING_BUFFER_LEN];
76+
msg.frame_id.data = msg_buffer;
77+
msg.frame_id.capacity = STRING_BUFFER_LEN;
78+
79+
// Create and allocate the pingpong subscription message
80+
std_msgs__msg__Header rcv_msg;
81+
char rcv_buffer[STRING_BUFFER_LEN];
82+
rcv_msg.frame_id.data = rcv_buffer;
83+
rcv_msg.frame_id.capacity = STRING_BUFFER_LEN;
84+
85+
// Set device id and sequence number;
86+
int device_id = rand();
87+
int seq_no;
88+
89+
int pong_count = 0;
90+
struct timespec ts;
91+
rcl_ret_t rc;
92+
93+
do {
94+
// Clear and set the waitset
95+
rcl_wait_set_clear(&wait_set);
96+
97+
size_t index_pong_subscription;
98+
rcl_wait_set_add_subscription(&wait_set, &pong_subscription, &index_pong_subscription);
99+
100+
size_t index_ping_subscription;
101+
rcl_wait_set_add_subscription(&wait_set, &ping_subscription, &index_ping_subscription);
102+
103+
size_t index_guardcondition;
104+
rcl_wait_set_add_guard_condition(&wait_set, &guard_condition, &index_guardcondition);
105+
106+
// Run session for 100 ms
107+
rcl_wait(&wait_set, RCL_MS_TO_NS(100));
108+
109+
// Check if some pong message is received
110+
if (wait_set.subscriptions[index_pong_subscription]) {
111+
rc = rcl_take(wait_set.subscriptions[index_pong_subscription], &rcv_msg, NULL, NULL);
112+
if(rc == RCL_RET_OK && strcmp(msg.frame_id.data,rcv_msg.frame_id.data) == 0) {
113+
pong_count++;
114+
printf("Pong for seq %s (%d)\n", rcv_msg.frame_id.data, pong_count);
115+
}
116+
}
117+
118+
// Check if some ping message is received and pong it
119+
if (wait_set.subscriptions[index_ping_subscription]) {
120+
rc = rcl_take(wait_set.subscriptions[index_ping_subscription], &rcv_msg, NULL, NULL);
121+
122+
// Dont pong my own pings
123+
if(rc == RCL_RET_OK && strcmp(msg.frame_id.data,rcv_msg.frame_id.data) != 0){
124+
printf("Ping received with seq %s. Answering.\n", rcv_msg.frame_id.data);
125+
rcl_publish(&pong_publisher, (const void*)&rcv_msg, NULL);
126+
}
127+
}
128+
129+
// Check if it is time to send a ping
130+
if (wait_set.guard_conditions[index_guardcondition]) {
131+
// Generate a new random sequence number
132+
seq_no = rand();
133+
sprintf(msg.frame_id.data, "%d_%d", seq_no, device_id);
134+
msg.frame_id.size = strlen(msg.frame_id.data);
135+
136+
// Fill the message timestamp
137+
clock_gettime(CLOCK_REALTIME, &ts);
138+
msg.stamp.sec = ts.tv_sec;
139+
msg.stamp.nanosec = ts.tv_nsec;
140+
141+
// Reset the pong count and publish the ping message
142+
pong_count = 0;
143+
rcl_publish(&ping_publisher, (const void*)&msg, NULL);
144+
printf("Ping send seq %s\n", msg.frame_id.data);
145+
}
146+
147+
usleep(10000);
148+
} while (true);
149+
}

0 commit comments

Comments
 (0)