MicroROS: RTPS error
Hi, I'm running a MicroROS publisher on a Portenta H7, and I'm using a custom message type of the format:
EIClassification.msg
---
string label
float32 value
And it was working just fine for me: I was able to echo the MicroROS topic with no problem. I must have somehow changed some parameter somewhere because now when I try to echo the topic I am seeing:
2022-09-13 20:48:29.787 [RTPS_READER_HISTORY Error] Change payload size of '24' bytes is larger than the history payload size of '11' bytes and cannot be resized. -> Function can_change_be_added_nts
This runs repeatedly, seemingly upon every publish.
I would greatly appreciate any ideas as to where to look... I tried digging through whatever RTPS-related files I could find in /opt/ros
but didn't see anything obvious.
Arduino code:
#include <micro_ros_arduino.h>
#include <stdio.h>
#include <rcl/rcl.h>
#include <rcl/error_handling.h>
#include <rclc/rclc.h>
#include <rclc/executor.h>
#include <ei_interfaces/msg/ei_classification.h>
rcl_publisher_t publisher;
rclc_executor_t executor;
rclc_support_t support;
rcl_allocator_t allocator;
rcl_node_t node;
rcl_timer_t timer;
ei_interfaces__msg__EIClassification msg;
#define LED_PIN 13
#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)){}}
void error_loop(){
while(1){
digitalWrite(LED_PIN, !digitalRead(LED_PIN));
delay(100);
}
}
void timer_callback(rcl_timer_t * timer, int64_t last_call_time)
{
RCLC_UNUSED(last_call_time);
if (timer != NULL) {
RCSOFTCHECK(rcl_publish(&publisher, &msg, NULL));
}
}
void setup() {
set_microros_transports();
pinMode(LED_PIN, OUTPUT);
digitalWrite(LED_PIN, HIGH);
delay(1000);
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_arduino_node", "", &support));
// create publisher
RCCHECK(rclc_publisher_init_default(
&publisher,
&node,
ROSIDL_GET_MSG_TYPE_SUPPORT(ei_interfaces, msg, EIClassification),
"/micro_ros_arduino_node_publisher"));
// create timer,
const unsigned int timer_timeout = 200;
RCCHECK(rclc_timer_init_default(
&timer,
&support,
RCL_MS_TO_NS(timer_timeout),
timer_callback));
// create executor
RCCHECK(rclc_executor_init(&executor, &support.context, 1, &allocator));
RCCHECK(rclc_executor_add_timer(&executor, &timer));
// Fill the 'label' array with a known sequence
msg.label.data = (char * ) malloc(25 * sizeof(char));
msg.label.size = 0;
msg.label.capacity = 25;
strcpy(msg.label.data, "test_label");
msg.label.size = strlen(msg.label.data);
msg.value = 0.5;
}
void loop() {
delay(50);
RCSOFTCHECK(rclc_executor_spin_some(&executor, RCL_MS_TO_NS(100)));
}
Output from MicroROS agent (docker):
...
[1663092212.433801] debug | DataWriter.cpp | write | [** <<DDS>> **] | client_key: 0x00000000, len: 20, data:
0000: 0B 00 00 00 74 65 73 74 5F 6C 61 62 65 6C 00 00 00 00 00 3F
[1663092212.433928] debug | SerialAgentLinux.cpp | send_message | [** <<SER>> **] | client_key: 0x77247145, len: 13, data:
0000: 81 00 00 00 0A 01 05 00 99 01 00 00 80
[1663092212.586321] debug | SerialAgentLinux.cpp | recv_message | [==>> SER <<==] | client_key: 0x77247145, len: 32, data:
0000: 81 80 99 01 07 01 18 00 01 A3 00 05 0B 00 00 00 74 65 73 74 5F 6C 61 62 65 6C 00 6D 00 00 00 3F
[1663092212.586695] debug | DataWriter.cpp | write | [** <<DDS>> **] | client_key: 0x00000000, len: 20, data:
0000: 0B 00 00 00 74 65 73 74 5F 6C 61 62 65 6C 00 6D 00 00 00 3F
[1663092212.586837] debug | SerialAgentLinux.cpp | send_message | [** <<SER>> **] | client_key: 0x77247145, len: 13, data:
0000: 81 00 00 ...