Skip to content

Commit 15c9a3f

Browse files
mergify[bot]amx-piap
mergify[bot]
and
amx-piap
authored
ping pong application example for esp32 w/idf (#79) (#80)
* examples: adding ping pong Signed-off-by: Alex M <amalki@piap.pl> * ping_pong: add ip using rmw options * ping_pong: increasing the number of publisher/subscriber (cherry picked from commit a84e159) Co-authored-by: amx-piap <48675389+amx-piap@users.noreply.github.com>
1 parent 35058f2 commit 15c9a3f

8 files changed

Lines changed: 225 additions & 2 deletions

File tree

‎colcon.meta‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -36,8 +36,8 @@
3636
"-DRMW_UXRCE_XML_BUFFER_LENGTH=400",
3737
"-DRMW_UXRCE_TRANSPORT=udp",
3838
"-DRMW_UXRCE_MAX_NODES=1",
39-
"-DRMW_UXRCE_MAX_PUBLISHERS=1",
40-
"-DRMW_UXRCE_MAX_SUBSCRIPTIONS=1",
39+
"-DRMW_UXRCE_MAX_PUBLISHERS=2",
40+
"-DRMW_UXRCE_MAX_SUBSCRIPTIONS=2",
4141
"-DRMW_UXRCE_MAX_SERVICES=1",
4242
"-DRMW_UXRCE_MAX_CLIENTS=1",
4343
"-DRMW_UXRCE_MAX_HISTORY=1"

‎examples/ping_pong/.gitignore‎

Lines changed: 3 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,3 @@
1+
build
2+
sdkconfig
3+
sdkconfig.old

‎examples/ping_pong/CMakeLists.txt‎

Lines changed: 7 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,7 @@
1+
cmake_minimum_required(VERSION 3.5)
2+
3+
set (EXTRA_COMPONENT_DIRS "./../../.")
4+
5+
include($ENV{IDF_PATH}/tools/cmake/project.cmake)
6+
project(ping_pong)
7+

‎examples/ping_pong/main/CMakeLists.txt‎

Lines changed: 3 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,3 @@
1+
idf_component_register(SRCS main.c
2+
INCLUDE_DIRS ""
3+
)

‎examples/ping_pong/main/Kconfig.projbuild‎

Lines changed: 15 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,15 @@
1+
menu "micro-ROS example-app (ping-pong) settings"
2+
3+
config MICRO_ROS_APP_STACK
4+
int "Stack the micro-ROS app (Bytes)"
5+
default 16000
6+
help
7+
Stack size in Bytes of the micro-ROS app
8+
9+
config MICRO_ROS_APP_TASK_PRIO
10+
int "Priority of the micro-ROS app"
11+
default 5
12+
help
13+
Priority of micro-ros task higher value means higher priority
14+
15+
endmenu

‎examples/ping_pong/main/component.mk‎

Lines changed: 5 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,5 @@
1+
#
2+
# "main" pseudo-component makefile.
3+
#
4+
# (Uses default behaviour of compiling all source files in directory, adding 'include' to include path.)
5+

‎examples/ping_pong/main/main.c‎

Lines changed: 187 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,187 @@
1+
#include <string.h>
2+
#include <stdio.h>
3+
#include <unistd.h>
4+
#include <time.h>
5+
6+
#ifdef ESP_PLATFORM
7+
#include "freertos/FreeRTOS.h"
8+
#include "freertos/task.h"
9+
#endif
10+
11+
#include "esp_log.h"
12+
#include "esp_system.h"
13+
14+
#include <uros_network_interfaces.h>
15+
#include <std_msgs/msg/header.h>
16+
#include <rcl/rcl.h>
17+
#include <rcl/error_handling.h>
18+
#include <rclc/rclc.h>
19+
#include <rclc/executor.h>
20+
#include <rmw_microros/rmw_microros.h>
21+
#include "uxr/client/config.h"
22+
23+
24+
#define STRING_BUFFER_LEN 50
25+
26+
#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); vTaskDelete(NULL);}}
27+
#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);}}
28+
29+
rcl_publisher_t ping_publisher;
30+
rcl_publisher_t pong_publisher;
31+
rcl_subscription_t ping_subscriber;
32+
rcl_subscription_t pong_subscriber;
33+
34+
std_msgs__msg__Header incoming_ping;
35+
std_msgs__msg__Header outcoming_ping;
36+
std_msgs__msg__Header incoming_pong;
37+
38+
int device_id;
39+
int seq_no;
40+
int pong_count;
41+
42+
void ping_timer_callback(rcl_timer_t * timer, int64_t last_call_time)
43+
{
44+
RCLC_UNUSED(last_call_time);
45+
46+
if (timer != NULL) {
47+
48+
seq_no = rand();
49+
sprintf(outcoming_ping.frame_id.data, "%d_%d", seq_no, device_id);
50+
outcoming_ping.frame_id.size = strlen(outcoming_ping.frame_id.data);
51+
52+
// Fill the message timestamp
53+
struct timespec ts;
54+
clock_gettime(CLOCK_REALTIME, &ts);
55+
outcoming_ping.stamp.sec = ts.tv_sec;
56+
outcoming_ping.stamp.nanosec = ts.tv_nsec;
57+
58+
// Reset the pong count and publish the ping message
59+
pong_count = 0;
60+
rcl_publish(&ping_publisher, (const void*)&outcoming_ping, NULL);
61+
printf("Ping send seq %s\n", outcoming_ping.frame_id.data);
62+
}
63+
}
64+
65+
void ping_subscription_callback(const void * msgin)
66+
{
67+
const std_msgs__msg__Header * msg = (const std_msgs__msg__Header *)msgin;
68+
69+
// Dont pong my own pings
70+
if(strcmp(outcoming_ping.frame_id.data, msg->frame_id.data) != 0){
71+
printf("Ping received with seq %s. Answering.\n", msg->frame_id.data);
72+
rcl_publish(&pong_publisher, (const void*)msg, NULL);
73+
}
74+
}
75+
76+
77+
void pong_subscription_callback(const void * msgin)
78+
{
79+
const std_msgs__msg__Header * msg = (const std_msgs__msg__Header *)msgin;
80+
81+
if(strcmp(outcoming_ping.frame_id.data, msg->frame_id.data) == 0) {
82+
pong_count++;
83+
printf("Pong for seq %s (%d)\n", msg->frame_id.data, pong_count);
84+
}
85+
}
86+
87+
88+
void micro_ros_task(void * arg)
89+
{
90+
rcl_allocator_t allocator = rcl_get_default_allocator();
91+
rclc_support_t support;
92+
93+
#if 0
94+
// create init_options
95+
//RCCHECK(rclc_support_init(&support, 0, NULL, &allocator));
96+
#endif
97+
98+
rcl_init_options_t init_options = rcl_get_zero_initialized_init_options();
99+
RCCHECK(rcl_init_options_init(&init_options, allocator));
100+
rmw_init_options_t* rmw_options = rcl_init_options_get_rmw_init_options(&init_options);
101+
102+
// Static Agent IP and port can be used instead of autodisvery.
103+
RCCHECK(rmw_uros_options_set_udp_address(CONFIG_MICRO_ROS_AGENT_IP,
104+
CONFIG_MICRO_ROS_AGENT_PORT, rmw_options));
105+
106+
// create init_options
107+
RCCHECK(rclc_support_init_with_options(&support, 0, NULL, &init_options, &allocator));
108+
109+
// create node
110+
rcl_node_t node = rcl_get_zero_initialized_node();
111+
RCCHECK(rclc_node_init_default(&node, "pingpong_node", "", &support));
112+
113+
// Create a reliable ping publisher
114+
RCCHECK(rclc_publisher_init_best_effort(&ping_publisher, &node,
115+
ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Header), "/microROS/ping"));
116+
117+
// Create a best effort pong publisher
118+
RCCHECK(rclc_publisher_init_best_effort(&pong_publisher, &node,
119+
ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Header), "/microROS/pong"));
120+
121+
// Create a best effort ping subscriber
122+
RCCHECK(rclc_subscription_init_best_effort(&ping_subscriber, &node,
123+
ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Header), "/microROS/ping"));
124+
125+
// Create a best effort pong subscriber
126+
RCCHECK(rclc_subscription_init_best_effort(&pong_subscriber, &node,
127+
ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Header), "/microROS/pong"));
128+
129+
130+
// Create a 3 seconds ping timer timer,
131+
rcl_timer_t timer;
132+
RCCHECK(rclc_timer_init_default(&timer, &support, RCL_MS_TO_NS(2000), ping_timer_callback));
133+
134+
135+
// Create executor
136+
rclc_executor_t executor;
137+
RCCHECK(rclc_executor_init(&executor, &support.context, 3, &allocator));
138+
RCCHECK(rclc_executor_add_timer(&executor, &timer));
139+
RCCHECK(rclc_executor_add_subscription(&executor, &ping_subscriber, &incoming_ping,
140+
&ping_subscription_callback, ON_NEW_DATA));
141+
RCCHECK(rclc_executor_add_subscription(&executor, &pong_subscriber, &incoming_pong,
142+
&pong_subscription_callback, ON_NEW_DATA));
143+
144+
// Create and allocate the pingpong messages
145+
char outcoming_ping_buffer[STRING_BUFFER_LEN];
146+
outcoming_ping.frame_id.data = outcoming_ping_buffer;
147+
outcoming_ping.frame_id.capacity = STRING_BUFFER_LEN;
148+
149+
char incoming_ping_buffer[STRING_BUFFER_LEN];
150+
incoming_ping.frame_id.data = incoming_ping_buffer;
151+
incoming_ping.frame_id.capacity = STRING_BUFFER_LEN;
152+
153+
char incoming_pong_buffer[STRING_BUFFER_LEN];
154+
incoming_pong.frame_id.data = incoming_pong_buffer;
155+
incoming_pong.frame_id.capacity = STRING_BUFFER_LEN;
156+
157+
device_id = rand();
158+
159+
while(1){
160+
rclc_executor_spin_some(&executor, RCL_MS_TO_NS(10));
161+
usleep(10000);
162+
}
163+
164+
// Free resources
165+
RCCHECK(rcl_publisher_fini(&ping_publisher, &node));
166+
RCCHECK(rcl_publisher_fini(&pong_publisher, &node));
167+
RCCHECK(rcl_subscription_fini(&ping_subscriber, &node));
168+
RCCHECK(rcl_subscription_fini(&pong_subscriber, &node));
169+
RCCHECK(rcl_node_fini(&node));
170+
}
171+
172+
173+
void app_main(void)
174+
{
175+
#ifdef UCLIENT_PROFILE_UDP
176+
// Start the networking if required
177+
ESP_ERROR_CHECK(uros_network_interface_initialize());
178+
#endif // UCLIENT_PROFILE_UDP
179+
180+
//pin micro-ros task in APP_CPU to make PRO_CPU to deal with wifi:
181+
xTaskCreate(micro_ros_task,
182+
"uros_task",
183+
CONFIG_MICRO_ROS_APP_STACK,
184+
NULL,
185+
CONFIG_MICRO_ROS_APP_TASK_PRIO,
186+
NULL);
187+
}

‎examples/ping_pong/sdkconfig.defaults‎

Lines changed: 3 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,3 @@
1+
CONFIG_ESP_MAIN_TASK_STACK_SIZE=3000
2+
CONFIG_ESP_TASK_WDT=n
3+

0 commit comments

Comments
 (0)