rclc_executor_spin_some no time out when agent crashes

[yoneyoneclub]

rclc_executor_spin_some no time out when agent crashes

• Hardware description: Arduino Portenta H7
• RTOS:
• Installation type: micro_ros_setup,Arduino IDE
• Version or commit hash: humble 2.0.5

Steps to reproduce the issue

Include it in your project using Sketch -> Include library -> Add .ZIP Library... You can test micro-ROS examples located in this repo examples folder.

Follow the installation instructions to build the Adruino IDE environment I wrote the following code

#include <micro_ros_arduino.h>
#include <rcl/rcl.h>
#include <rcl/error_handling.h>
#include <rclc/rclc.h>
#include <rclc/executor.h>
#include <rcl_logging_interface/rcl_logging_interface.h>
#include <rmw_microros/rmw_microros.h>

//type for sub/pub
#include <std_msgs/msg/u_int8.h>
#include <std_msgs/msg/u_int32.h>
#include <std_msgs/msg/int16.h>
#define DEBUG Serial1
#define ROS_DEBUG 0
#define DOMAIN_ID 20
#define RCCHECK(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);return false;}}
#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);}}

#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)\

//pub
rcl_publisher_t hb_micro_ros_host_pub;
//topic msg for pub
std_msgs__msg__UInt8   hb_micro_ros_host_msg;
//sub
rcl_subscription_t hb_aw_sub;//empty
//topic msg for sub
std_msgs__msg__UInt8 hb_aw_msg;
// global data
rclc_executor_t executor;
rclc_support_t support;
rcl_allocator_t allocator;
rcl_node_t node;
rcl_init_options_t init_options;
rcl_timer_t timer_10;

enum states {
  WAITING_AGENT,
  AGENT_AVAILABLE,
  AGENT_CONNECTED,
  AGENT_DISCONNECTED
} state;

unsigned long time_main;
unsigned long time_timer;
unsigned long span;
unsigned long span_timer;

void HbAwCb(const void * msgin)
{
  Serial1.println("Hb aw sub");
}

void TimerCb10(rcl_timer_t * timer, int64_t last_call_time)
{
  span_timer = millis() - time_timer;
  Serial1.print("timer:");
  Serial1.println(span_timer);
  time_timer = millis();

  if (NULL != timer)
  {
    Serial1.print("pub");
    RCSOFTCHECK(rcl_publish(&hb_micro_ros_host_pub, &hb_micro_ros_host_msg, NULL));
    Serial1.println("done");
  }

}

void setup()
{
  set_microros_transports();//USB経由
  DEBUG.begin(115200);
  delay(2000);
  state = WAITING_AGENT;

  time_main = 0;
}

bool create_entities()
{
  unsigned int rcl_wait_timeout = 50;   // in ms
  allocator = rcl_get_default_allocator();//micro-ROSのためのメモリ管理
  init_options = rcl_get_zero_initialized_init_options();
  RCCHECK(rcl_init_options_init(&init_options, allocator));
  RCCHECK(rcl_init_options_set_domain_id(&init_options, DOMAIN_ID));
  
  //create init_options
  // rclc_support_t support;
  RCCHECK(rclc_support_init_with_options(&support, 0, NULL, &init_options, &allocator));

  // create node
  RCCHECK(rclc_node_init_default(&node, "microros_arduino_host_node", "", &support));

  /* Subscriber */
  RCCHECK(rclc_subscription_init_best_effort(
    &hb_aw_sub,
    &node,
    ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, UInt8),
    "hb_aw"));
  
  // create executor
  executor = rclc_executor_get_zero_initialized_executor();
  RCCHECK(rclc_executor_init(&executor, &support.context, 8, &allocator));
  RCCHECK(rclc_executor_set_timeout(&executor, RCL_MS_TO_NS(rcl_wait_timeout)));
  RCCHECK(rclc_executor_add_subscription(&executor, &hb_aw_sub,          &hb_aw_msg,          &HbAwCb,   ON_NEW_DATA));

  /* Publisher */
  /* =============== Publisher ===============*/

  //heart_beat arduino -> main pc
  RCCHECK(rclc_publisher_init_default(
    &hb_micro_ros_host_pub,
    &node,
    ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, UInt8),
    "hb_micro_ros_host"));//1byte

  
    /* =============== Publisher =============== */

  const unsigned int timer_timeout = 10;
  RCCHECK(rclc_timer_init_default(
    &timer_10,
    &support,
    RCL_MS_TO_NS(timer_timeout),
    TimerCb10));
  RCCHECK(rclc_executor_add_timer(&executor, &timer_10));
  return true;
}

void destroy_entities()
{
  rmw_context_t * rmw_context = rcl_context_get_rmw_context(&support.context);
  (void) rmw_uros_set_context_entity_destroy_session_timeout(rmw_context, 0);
  RCSOFTCHECK(rcl_publisher_fini(&hb_micro_ros_host_pub, &node));
  RCSOFTCHECK(rclc_support_fini(&support));
  RCSOFTCHECK(rcl_timer_fini(&timer_10));
  RCSOFTCHECK(rclc_executor_fini(&executor));
  RCSOFTCHECK(rcl_init_options_fini(&init_options));
}

void loop()
{
  // ubuntu pcとの接続をpingで確認しながら状態を決める
  // これでhostからの通信が途絶えてもrebootして正常起動できる
  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) {
        DEBUG.println("AGENT_CONNECTED");
        span = millis() - time_main;
        Serial1.print("main:");
        Serial1.println(span);
        time_main = millis();
        rclc_executor_spin_some(&executor, RCL_MS_TO_NS(10));
      }
      else
      {
        DEBUG.println("AGENT_DISCONNECTED"); 
      }
      
      break;
      
      
    case AGENT_DISCONNECTED:
      
      destroy_entities();
      DEBUG.println("AGENT_DISCONNECTED DONE");
      state = WAITING_AGENT;
      break;
      
    default:
      break;
  }
}

When starting the agent, I build the agent and start it with the following command

docker run -it --rm -v /dev:/dev --privileged --net=host microros/micro-ros-agent:humble serial --dev [YOUR BOARD PORT] -v6

Expected behavior

In the above code I want to switch to state=WAITING_AGENT when the agent suddenly stops.

Actual behavior

The loop is running normally before and during the startup of the agent. However, when the agent stops during the loop startup, the program stops at rcl_publish(&hb_micro_ros_host_pub, &hb_micro_ros_host_msg, NULL) in this code, and the state=WAITING_AGENT is set to Not looping. Also expecting to timeout on rclc_executor_spin_somed

[yashi]

Hi,

I don't use Arduino at all but would love to help. As I understand your problem, ping is working OK but rcl_publish() somehow stops when your agent is gone. Am I right? If so, would you mind reducing your code down to a bare minimum?

I noticed that set_microros_transports();//USB経由 in your code, but I can't find any transport on USB for Arduino. What transport are you using?

If rcl_publish() doesn't return, would you mind chekcing that your transport's write function return when agent is not there?

Hope this helps.

[yoneyoneclub]

Sorry for the late reply. Thanks for the advice.

but I can't find any transport on USB for Arduino.

I'm using the articles around here to create this code. Also, if it is a hardware question, I am using a usb type c cable to communicate with the pc.