When using _rclc_support_init_withoptions instead of _rcl_init_optionsinit, the Micro-ROS client can successfully connect to the agent on the first and second attempt, but fails to connect on subsequent attempts.
#include <Arduino.h>
#include <micro_ros_platformio.h>
#include <rcl/rcl.h>
#include <rclc/rclc.h>
#include <rclc/executor.h>
#include <micro_ros_utilities/string_utilities.h>
#include <rmw_microros/rmw_microros.h>
#include <std_msgs/msg/bool.h>
#include <std_msgs/msg/u_int64.h>
#include <sensor_msgs/msg/range.h>
#include <Ultrasonic.h>
#include <Wire.h>
#include <ACROBOTIC_SSD1306.h>
#include <Int64String.h>
#if !defined(MICRO_ROS_TRANSPORT_ARDUINO_SERIAL)
#error This example is only avaliable for Arduino framework with serial transport.
#endif
#define ROS_DOMAIN_ID 57
...
rclc_executor_t executor;
rclc_support_t support;
rcl_allocator_t allocator;
rcl_node_t node;
rcl_timer_t timer;
rcl_init_options_t init_options;
#define RCCHECK(fn) { rcl_ret_t temp_rc = fn; if((temp_rc != RCL_RET_OK)){error_loop();}}
#define RCSOFTCHECK(fn) { rcl_ret_t temp_rc = fn; if((temp_rc != RCL_RET_OK)){}}
...
// Timeout for each ping attempt
const int timeout_ms = 100;
// Number of ping attempts
const uint8_t attempts = 1;
// Spin period
const unsigned int spin_timeout = RCL_MS_TO_NS(100);
// Enum with connection status
enum states {
WAITING_AGENT,
AGENT_AVAILABLE,
AGENT_CONNECTED,
AGENT_DISCONNECTED
} state;
// Error handle loop
void error_loop() {
while(1) {
delay(100);
}
}
bool create_entities(void) {
size_t domain_id = (size_t)(ROS_DOMAIN_ID);
// Initialize micro-ROS allocator
allocator = rcl_get_default_allocator();
// Initialize and modify options
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));
// Initialize rclc support object with custom options
RCCHECK(rclc_support_init_with_options(&support, 0, NULL, &init_options, &allocator));
// create node
RCCHECK(rclc_node_init_default(&node, "micro_ros_raspico_node", "", &support));
////////////////////////////////////////////////////////////////////////////////////////////////////////////
// allocator = rcl_get_default_allocator();
// // create init_options
// RCCHECK(rclc_support_init(&support, 0, NULL, &allocator));
// // create node
// RCCHECK(rclc_node_init_default(&node, "micro_ros_raspico_node", "", &support));
////////////////////////////////////////////////////////////////////////////////////////////////////////////
// create publisher
...
// create timer,
const unsigned int timer_timeout = 20;
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, 5, &allocator));
RCCHECK(rclc_executor_add_timer(&executor, &timer));
// Add a subscriber to the executor
...
return true;
}
void destroy_entities(void){
rmw_context_t * rmw_context = rcl_context_get_rmw_context(&support.context);
(void) rmw_uros_set_context_entity_destroy_session_timeout(rmw_context, 0);
RCCHECK(rcl_publisher_fini
...
RCCHECK(rcl_timer_fini(&timer));
RCCHECK(rclc_executor_fini(&executor));
RCCHECK(rcl_node_fini(&node));
RCCHECK(rclc_support_fini(&support));
}
void setup() {
Wire.begin();
...
// Configure serial transport
Serial.begin(921600);
set_microros_serial_transports(Serial);
delay(2000);
}
void loop() {
switch (state)
{
case WAITING_AGENT:
// Check for agent connection
state = (RMW_RET_OK == rmw_uros_ping_agent(timeout_ms, attempts)) ? AGENT_AVAILABLE : WAITING_AGENT;
break;
case AGENT_AVAILABLE:
// Create micro-ROS entities
state = (true == create_entities()) ? AGENT_CONNECTED : WAITING_AGENT;
if (state == WAITING_AGENT)
{
// Creation failed, release allocated resources
destroy_entities();
};
break;
case AGENT_CONNECTED:
// Check connection and spin on success
state = (RMW_RET_OK == rmw_uros_ping_agent(timeout_ms, attempts)) ? AGENT_CONNECTED : AGENT_DISCONNECTED;
if (state == AGENT_CONNECTED)
{
RCSOFTCHECK(rclc_executor_spin_some(&executor, RCL_MS_TO_NS(100))); }
break;
case AGENT_DISCONNECTED:
// Connection is lost, destroy entities and go back to first step
destroy_entities();
state = WAITING_AGENT;
break;
default:
break;
}
}
Issue template
When using _rclc_support_init_withoptions instead of _rcl_init_optionsinit, the Micro-ROS client can successfully connect to the agent on the first and second attempt, but fails to connect on subsequent attempts.