ping pong application example for esp32 w/idf (micro-ROS#79) (micro-R…

…OS#80)

* examples: adding ping pong

Signed-off-by: Alex M <[email protected]>

* 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 <[email protected]>

colcon.meta

@@ -36,8 +36,8 @@
                 "-DRMW_UXRCE_XML_BUFFER_LENGTH=400",
                 "-DRMW_UXRCE_TRANSPORT=udp",
                 "-DRMW_UXRCE_MAX_NODES=1",
-                "-DRMW_UXRCE_MAX_PUBLISHERS=1",
-                "-DRMW_UXRCE_MAX_SUBSCRIPTIONS=1",
+                "-DRMW_UXRCE_MAX_PUBLISHERS=2",
+                "-DRMW_UXRCE_MAX_SUBSCRIPTIONS=2",
                 "-DRMW_UXRCE_MAX_SERVICES=1",
                 "-DRMW_UXRCE_MAX_CLIENTS=1",
                 "-DRMW_UXRCE_MAX_HISTORY=1"

examples/ping_pong/.gitignore

@@ -0,0 +1,3 @@
+build
+sdkconfig
+sdkconfig.old

examples/ping_pong/CMakeLists.txt

@@ -0,0 +1,7 @@
+cmake_minimum_required(VERSION 3.5)
+
+set (EXTRA_COMPONENT_DIRS "./../../.")
+
+include($ENV{IDF_PATH}/tools/cmake/project.cmake)
+project(ping_pong)
+

examples/ping_pong/main/CMakeLists.txt

@@ -0,0 +1,3 @@
+idf_component_register(SRCS main.c  
+                    INCLUDE_DIRS ""
+                    )

examples/ping_pong/main/Kconfig.projbuild

@@ -0,0 +1,15 @@
+menu "micro-ROS example-app (ping-pong) settings"
+
+    config MICRO_ROS_APP_STACK
+        int "Stack the micro-ROS app (Bytes)"
+        default 16000
+        help
+        Stack size in Bytes of the micro-ROS app
+
+    config MICRO_ROS_APP_TASK_PRIO
+        int "Priority of the micro-ROS app"
+        default 5
+        help
+        Priority of micro-ros task higher value means higher priority
+
+endmenu

examples/ping_pong/main/component.mk

@@ -0,0 +1,5 @@
+#
+# "main" pseudo-component makefile.
+#
+# (Uses default behaviour of compiling all source files in directory, adding 'include' to include path.)
+

examples/ping_pong/main/main.c

@@ -0,0 +1,187 @@
+#include <string.h>
+#include <stdio.h>
+#include <unistd.h>
+#include <time.h>
+
+#ifdef ESP_PLATFORM
+#include "freertos/FreeRTOS.h"
+#include "freertos/task.h"
+#endif
+
+#include "esp_log.h"
+#include "esp_system.h"
+
+#include <uros_network_interfaces.h>
+#include <std_msgs/msg/header.h>
+#include <rcl/rcl.h>
+#include <rcl/error_handling.h>
+#include <rclc/rclc.h>
+#include <rclc/executor.h>
+#include <rmw_microros/rmw_microros.h>
+#include "uxr/client/config.h"
+
+
+#define STRING_BUFFER_LEN 50
+
+#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);}}
+#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);}}
+
+rcl_publisher_t ping_publisher;
+rcl_publisher_t pong_publisher;
+rcl_subscription_t ping_subscriber;
+rcl_subscription_t pong_subscriber;
+
+std_msgs__msg__Header incoming_ping;
+std_msgs__msg__Header outcoming_ping;
+std_msgs__msg__Header incoming_pong;
+
+int device_id;
+int seq_no;
+int pong_count;
+
+void ping_timer_callback(rcl_timer_t * timer, int64_t last_call_time)
+{
+	RCLC_UNUSED(last_call_time);
+
+	if (timer != NULL) {
+
+		seq_no = rand();
+		sprintf(outcoming_ping.frame_id.data, "%d_%d", seq_no, device_id);
+		outcoming_ping.frame_id.size = strlen(outcoming_ping.frame_id.data);
+
+		// Fill the message timestamp
+		struct timespec ts;
+		clock_gettime(CLOCK_REALTIME, &ts);
+		outcoming_ping.stamp.sec = ts.tv_sec;
+		outcoming_ping.stamp.nanosec = ts.tv_nsec;
+
+		// Reset the pong count and publish the ping message
+		pong_count = 0;
+		rcl_publish(&ping_publisher, (const void*)&outcoming_ping, NULL);
+		printf("Ping send seq %s\n", outcoming_ping.frame_id.data);
+	}
+}
+
+void ping_subscription_callback(const void * msgin)
+{
+	const std_msgs__msg__Header * msg = (const std_msgs__msg__Header *)msgin;
+
+	// Dont pong my own pings
+	if(strcmp(outcoming_ping.frame_id.data, msg->frame_id.data) != 0){
+		printf("Ping received with seq %s. Answering.\n", msg->frame_id.data);
+		rcl_publish(&pong_publisher, (const void*)msg, NULL);
+	}
+}
+
+
+void pong_subscription_callback(const void * msgin)
+{
+	const std_msgs__msg__Header * msg = (const std_msgs__msg__Header *)msgin;
+
+	if(strcmp(outcoming_ping.frame_id.data, msg->frame_id.data) == 0) {
+		pong_count++;
+		printf("Pong for seq %s (%d)\n", msg->frame_id.data, pong_count);
+	}
+}
+
+
+void micro_ros_task(void * arg)
+{
+	rcl_allocator_t allocator = rcl_get_default_allocator();
+	rclc_support_t support;
+
+#if 0
+	// create init_options
+	//RCCHECK(rclc_support_init(&support, 0, NULL, &allocator));
+#endif
+
+	rcl_init_options_t init_options = rcl_get_zero_initialized_init_options();
+	RCCHECK(rcl_init_options_init(&init_options, allocator));
+	rmw_init_options_t* rmw_options = rcl_init_options_get_rmw_init_options(&init_options);
+
+	// Static Agent IP and port can be used instead of autodisvery.
+	RCCHECK(rmw_uros_options_set_udp_address(CONFIG_MICRO_ROS_AGENT_IP,
+						  CONFIG_MICRO_ROS_AGENT_PORT, rmw_options));
+
+	// create init_options
+	RCCHECK(rclc_support_init_with_options(&support, 0, NULL, &init_options, &allocator));
+
+	// create node
+	rcl_node_t node = rcl_get_zero_initialized_node();
+	RCCHECK(rclc_node_init_default(&node, "pingpong_node", "", &support));
+
+	// Create a reliable ping publisher
+	RCCHECK(rclc_publisher_init_best_effort(&ping_publisher, &node,
+		ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Header), "/microROS/ping"));
+
+	// Create a best effort pong publisher
+	RCCHECK(rclc_publisher_init_best_effort(&pong_publisher, &node,
+		ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Header), "/microROS/pong"));
+
+	// Create a best effort ping subscriber
+	RCCHECK(rclc_subscription_init_best_effort(&ping_subscriber, &node,
+		ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Header), "/microROS/ping"));
+
+	// Create a best effort  pong subscriber
+	RCCHECK(rclc_subscription_init_best_effort(&pong_subscriber, &node,
+		ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Header), "/microROS/pong"));
+
+
+	// Create a 3 seconds ping timer timer,
+	rcl_timer_t timer;
+	RCCHECK(rclc_timer_init_default(&timer, &support, RCL_MS_TO_NS(2000), ping_timer_callback));
+
+
+	// Create executor
+	rclc_executor_t executor;
+	RCCHECK(rclc_executor_init(&executor, &support.context, 3, &allocator));
+	RCCHECK(rclc_executor_add_timer(&executor, &timer));
+	RCCHECK(rclc_executor_add_subscription(&executor, &ping_subscriber, &incoming_ping,
+		&ping_subscription_callback, ON_NEW_DATA));
+	RCCHECK(rclc_executor_add_subscription(&executor, &pong_subscriber, &incoming_pong,
+		&pong_subscription_callback, ON_NEW_DATA));
+
+	// Create and allocate the pingpong messages
+	char outcoming_ping_buffer[STRING_BUFFER_LEN];
+	outcoming_ping.frame_id.data = outcoming_ping_buffer;
+	outcoming_ping.frame_id.capacity = STRING_BUFFER_LEN;
+
+	char incoming_ping_buffer[STRING_BUFFER_LEN];
+	incoming_ping.frame_id.data = incoming_ping_buffer;
+	incoming_ping.frame_id.capacity = STRING_BUFFER_LEN;
+
+	char incoming_pong_buffer[STRING_BUFFER_LEN];
+	incoming_pong.frame_id.data = incoming_pong_buffer;
+	incoming_pong.frame_id.capacity = STRING_BUFFER_LEN;
+
+	device_id = rand();
+
+	while(1){
+		rclc_executor_spin_some(&executor, RCL_MS_TO_NS(10));
+		usleep(10000);
+	}
+
+	// Free resources
+	RCCHECK(rcl_publisher_fini(&ping_publisher, &node));
+	RCCHECK(rcl_publisher_fini(&pong_publisher, &node));
+	RCCHECK(rcl_subscription_fini(&ping_subscriber, &node));
+	RCCHECK(rcl_subscription_fini(&pong_subscriber, &node));
+	RCCHECK(rcl_node_fini(&node));
+}
+
+
+void app_main(void)
+{   
+#ifdef UCLIENT_PROFILE_UDP
+    // Start the networking if required
+    ESP_ERROR_CHECK(uros_network_interface_initialize());
+#endif  // UCLIENT_PROFILE_UDP
+
+    //pin micro-ros task in APP_CPU to make PRO_CPU to deal with wifi:
+    xTaskCreate(micro_ros_task, 
+            "uros_task", 
+            CONFIG_MICRO_ROS_APP_STACK, 
+            NULL,
+            CONFIG_MICRO_ROS_APP_TASK_PRIO, 
+            NULL); 
+}

examples/ping_pong/sdkconfig.defaults

@@ -0,0 +1,3 @@
+CONFIG_ESP_MAIN_TASK_STACK_SIZE=3000
+CONFIG_ESP_TASK_WDT=n
+