From 49f872daeea4cb6a92ea50c843d37c2bb83a6c2a Mon Sep 17 00:00:00 2001 From: Pablo Garrido Date: Mon, 4 Apr 2022 15:23:09 +0200 Subject: [PATCH] Update reconnection example (#913) * Update reconnection example Signed-off-by: Pablo Garrido * FIx indentation Signed-off-by: Pablo Garrido * Typo Signed-off-by: Pablo Garrido * Typo Signed-off-by: Pablo Garrido * Typo Signed-off-by: Pablo Garrido (cherry picked from commit 561c615ea57b31cf3408f4af6cb470c6e1b18fa8) --- .../micro-ros_reconnection_example.ino | 147 ++++++++++-------- 1 file changed, 86 insertions(+), 61 deletions(-) diff --git a/examples/micro-ros_reconnection_example/micro-ros_reconnection_example.ino b/examples/micro-ros_reconnection_example/micro-ros_reconnection_example.ino index 1253a3761..31a910b41 100755 --- a/examples/micro-ros_reconnection_example/micro-ros_reconnection_example.ino +++ b/examples/micro-ros_reconnection_example/micro-ros_reconnection_example.ino @@ -11,6 +11,11 @@ #define LED_PIN 13 #define RCCHECK(fn) { rcl_ret_t temp_rc = fn; if((temp_rc != RCL_RET_OK)){return false;}} +#define EXECUTE_EVERY_N_MS(MS, X) do { \ + static volatile int64_t init = -1; \ + if (init == -1) { init = uxr_millis();} \ + if (uxr_millis() - init > MS) { X; init = uxr_millis();} \ +} while (0)\ rclc_support_t support; rcl_node_t node; @@ -21,89 +26,109 @@ rcl_publisher_t publisher; std_msgs__msg__Int32 msg; bool micro_ros_init_successful; +enum states { + WAITING_AGENT, + AGENT_AVAILABLE, + AGENT_CONNECTED, + AGENT_DISCONNECTED +} state; + void timer_callback(rcl_timer_t * timer, int64_t last_call_time) { - (void) last_call_time; - if (timer != NULL) { - rcl_publish(&publisher, &msg, NULL); - msg.data++; - } + (void) last_call_time; + if (timer != NULL) { + rcl_publish(&publisher, &msg, NULL); + msg.data++; + } } // Functions create_entities and destroy_entities can take several seconds. -// In order to reduce this rebuild the library with +// In order to reduce this rebuild the library with // - RMW_UXRCE_ENTITY_CREATION_DESTROY_TIMEOUT=0 // - UCLIENT_MAX_SESSION_CONNECTION_ATTEMPTS=3 bool create_entities() -{ - allocator = rcl_get_default_allocator(); - - // create init_options - RCCHECK(rclc_support_init(&support, 0, NULL, &allocator)); - - // create node - RCCHECK(rclc_node_init_default(&node, "int32_publisher_rclc", "", &support)); - - // create publisher - RCCHECK(rclc_publisher_init_best_effort( - &publisher, - &node, - ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Int32), - "std_msgs_msg_Int32")); - - // create timer, - const unsigned int timer_timeout = 1000; - RCCHECK(rclc_timer_init_default( - &timer, - &support, - RCL_MS_TO_NS(timer_timeout), - timer_callback)); - - // create executor - executor = rclc_executor_get_zero_initialized_executor(); - RCCHECK(rclc_executor_init(&executor, &support.context, 1, &allocator)); - RCCHECK(rclc_executor_add_timer(&executor, &timer)); - - micro_ros_init_successful = true; +{ + allocator = rcl_get_default_allocator(); + + // create init_options + RCCHECK(rclc_support_init(&support, 0, NULL, &allocator)); + + // create node + RCCHECK(rclc_node_init_default(&node, "int32_publisher_rclc", "", &support)); + + // create publisher + RCCHECK(rclc_publisher_init_best_effort( + &publisher, + &node, + ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Int32), + "std_msgs_msg_Int32")); + + // create timer, + const unsigned int timer_timeout = 1000; + RCCHECK(rclc_timer_init_default( + &timer, + &support, + RCL_MS_TO_NS(timer_timeout), + timer_callback)); + + // create executor + executor = rclc_executor_get_zero_initialized_executor(); + RCCHECK(rclc_executor_init(&executor, &support.context, 1, &allocator)); + RCCHECK(rclc_executor_add_timer(&executor, &timer)); + + return true; } void destroy_entities() { - rcl_publisher_fini(&publisher, &node); - rcl_node_fini(&node); - rcl_timer_fini(&timer); - rclc_executor_fini(&executor); - rclc_support_fini(&support); - - micro_ros_init_successful = false; + rmw_context_t * rmw_context = rcl_context_get_rmw_context(&support.context); + (void) rmw_uros_set_context_entity_destroy_session_timeout(rmw_context, 0); + + rcl_publisher_fini(&publisher, &node); + rcl_timer_fini(&timer); + rclc_executor_fini(&executor); + rcl_node_fini(&node); + rclc_support_fini(&support); } void setup() { - set_microros_transports(); + set_microros_transports(); pinMode(LED_PIN, OUTPUT); - digitalWrite(LED_PIN, HIGH); - micro_ros_init_successful = false; + state = WAITING_AGENT; + msg.data = 0; } void loop() { - uint32_t delay = 100000; - if (RMW_RET_OK == rmw_uros_ping_agent(50, 2)) - { - delay = 500000; - if (!micro_ros_init_successful) { - create_entities(); - } else { - rclc_executor_spin_some(&executor, RCL_MS_TO_NS(10)); - } - } - else if (micro_ros_init_successful) - { - destroy_entities(); + switch (state) { + case WAITING_AGENT: + EXECUTE_EVERY_N_MS(500, state = (RMW_RET_OK == rmw_uros_ping_agent(100, 1)) ? AGENT_AVAILABLE : WAITING_AGENT;); + break; + case AGENT_AVAILABLE: + state = (true == create_entities()) ? AGENT_CONNECTED : WAITING_AGENT; + if (state == WAITING_AGENT) { + destroy_entities(); + }; + break; + case AGENT_CONNECTED: + EXECUTE_EVERY_N_MS(200, state = (RMW_RET_OK == rmw_uros_ping_agent(100, 1)) ? AGENT_CONNECTED : AGENT_DISCONNECTED;); + if (state == AGENT_CONNECTED) { + rclc_executor_spin_some(&executor, RCL_MS_TO_NS(100)); + } + break; + case AGENT_DISCONNECTED: + destroy_entities(); + state = WAITING_AGENT; + break; + default: + break; } - digitalWrite(LED_PIN, !digitalRead(LED_PIN)); - delayMicroseconds(delay); + if (state == AGENT_CONNECTED) { + digitalWrite(LED_PIN, 1); + } else { + digitalWrite(LED_PIN, 0); + } }