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/header.h>
7+
8+ #include <stdio.h>
9+ #include <unistd.h>
10+ #include <time.h>
11+
12+ #define STRING_BUFFER_LEN 100
13+
14+ #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;}}
15+ #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);}}
16+
17+ rcl_publisher_t ping_publisher ;
18+ rcl_publisher_t pong_publisher ;
19+ rcl_subscription_t ping_subscriber ;
20+ rcl_subscription_t pong_subscriber ;
21+
22+ std_msgs__msg__Header incoming_ping ;
23+ std_msgs__msg__Header outcoming_ping ;
24+ std_msgs__msg__Header incoming_pong ;
25+
26+ int device_id ;
27+ int seq_no ;
28+ int pong_count ;
29+
30+ void ping_timer_callback (rcl_timer_t * timer , int64_t last_call_time )
31+ {
32+ (void ) last_call_time ;
33+
34+ if (timer != NULL ) {
35+
36+ seq_no = rand ();
37+ sprintf (outcoming_ping .frame_id .data , "%d_%d" , seq_no , device_id );
38+ outcoming_ping .frame_id .size = strlen (outcoming_ping .frame_id .data );
39+
40+ // Fill the message timestamp
41+ struct timespec ts ;
42+ clock_gettime (CLOCK_REALTIME , & ts );
43+ outcoming_ping .stamp .sec = ts .tv_sec ;
44+ outcoming_ping .stamp .nanosec = ts .tv_nsec ;
45+
46+ // Reset the pong count and publish the ping message
47+ pong_count = 0 ;
48+ rcl_publish (& ping_publisher , (const void * )& outcoming_ping , NULL );
49+ printf ("Ping send seq %s\n" , outcoming_ping .frame_id .data );
50+ }
51+ }
52+
53+ void ping_subscription_callback (const void * msgin )
54+ {
55+ const std_msgs__msg__Header * msg = (const std_msgs__msg__Header * )msgin ;
56+
57+ // Dont pong my own pings
58+ if (strcmp (outcoming_ping .frame_id .data , msg -> frame_id .data ) != 0 ){
59+ printf ("Ping received with seq %s. Answering.\n" , msg -> frame_id .data );
60+ rcl_publish (& pong_publisher , (const void * )msg , NULL );
61+ }
62+ }
63+
64+
65+ void pong_subscription_callback (const void * msgin )
66+ {
67+ const std_msgs__msg__Header * msg = (const std_msgs__msg__Header * )msgin ;
68+
69+ if (strcmp (outcoming_ping .frame_id .data , msg -> frame_id .data ) == 0 ) {
70+ pong_count ++ ;
71+ printf ("Pong for seq %s (%d)\n" , msg -> frame_id .data , pong_count );
72+ }
73+ }
74+
75+
76+ #if defined(BUILD_MODULE )
77+ int main (int argc , char * argv [])
78+ #else
79+ int uros_pingpong_main (int argc , char * argv [])
80+ #endif
81+ {
82+ rcl_allocator_t allocator = rcl_get_default_allocator ();
83+ rclc_support_t support ;
84+
85+ // create init_options
86+ RCCHECK (rclc_support_init (& support , 0 , NULL , & allocator ));
87+
88+ // create node
89+ rcl_node_t node = rcl_get_zero_initialized_node ();
90+ RCCHECK (rclc_node_init_default (& node , "pingpong_node" , "" , & support ));
91+
92+ // Create a reliable ping publisher
93+ RCCHECK (rclc_publisher_init_default (& ping_publisher , & node , ROSIDL_GET_MSG_TYPE_SUPPORT (std_msgs , msg , Header ), "/microROS/ping" ));
94+
95+ // Create a best effort pong publisher
96+ RCCHECK (rclc_publisher_init_best_effort (& pong_publisher , & node , ROSIDL_GET_MSG_TYPE_SUPPORT (std_msgs , msg , Header ), "/microROS/pong" ));
97+
98+ // Create a best effort ping subscriber
99+ RCCHECK (rclc_subscription_init_best_effort (& ping_subscriber , & node , ROSIDL_GET_MSG_TYPE_SUPPORT (std_msgs , msg , Header ), "/microROS/ping" ));
100+
101+ // Create a best effort pong subscriber
102+ RCCHECK (rclc_subscription_init_best_effort (& pong_subscriber , & node , ROSIDL_GET_MSG_TYPE_SUPPORT (std_msgs , msg , Header ), "/microROS/pong" ));
103+
104+
105+ // Create a 3 seconds ping timer timer,
106+ rcl_timer_t timer = rcl_get_zero_initialized_timer ();
107+ RCCHECK (rclc_timer_init_default (& timer , & support , RCL_MS_TO_NS (2000 ), ping_timer_callback ));
108+
109+
110+ // Create executor
111+ rclc_executor_t executor = rclc_executor_get_zero_initialized_executor ();
112+ RCCHECK (rclc_executor_init (& executor , & support .context , 3 , & allocator ));
113+
114+ unsigned int rcl_wait_timeout = 1000 ; // in ms
115+ RCCHECK (rclc_executor_set_timeout (& executor , RCL_MS_TO_NS (rcl_wait_timeout )));
116+ RCCHECK (rclc_executor_add_timer (& executor , & timer ));
117+ RCCHECK (rclc_executor_add_subscription (& executor , & ping_subscriber , & incoming_ping , & ping_subscription_callback , ON_NEW_DATA ));
118+ RCCHECK (rclc_executor_add_subscription (& executor , & pong_subscriber , & incoming_pong , & pong_subscription_callback , ON_NEW_DATA ));
119+
120+ // Create and allocate the pingpong messages
121+
122+ char outcoming_ping_buffer [STRING_BUFFER_LEN ];
123+ outcoming_ping .frame_id .data = outcoming_ping_buffer ;
124+ outcoming_ping .frame_id .capacity = STRING_BUFFER_LEN ;
125+
126+ char incoming_ping_buffer [STRING_BUFFER_LEN ];
127+ incoming_ping .frame_id .data = incoming_ping_buffer ;
128+ incoming_ping .frame_id .capacity = STRING_BUFFER_LEN ;
129+
130+ char incoming_pong_buffer [STRING_BUFFER_LEN ];
131+ incoming_pong .frame_id .data = incoming_pong_buffer ;
132+ incoming_pong .frame_id .capacity = STRING_BUFFER_LEN ;
133+
134+ device_id = rand ();
135+
136+ rclc_executor_spin (& executor );
137+
138+ RCCHECK (rcl_publisher_fini (& ping_publisher , & node ));
139+ RCCHECK (rcl_publisher_fini (& pong_publisher , & node ));
140+ RCCHECK (rcl_subscription_fini (& ping_subscriber , & node ));
141+ RCCHECK (rcl_subscription_fini (& pong_subscriber , & node ));
142+ RCCHECK (rcl_node_fini (& node ));
143+ }
0 commit comments