micro-ROS / micro_ros_arduino

micro-ROS library for Arduino
Apache License 2.0
435 stars 113 forks source link

can not create multiple publishers for the same node #984

Closed roman-rs closed 2 years ago

roman-rs commented 2 years ago

Issue template

Steps to reproduce the issue

create more than two publishers

Expected behavior

several publishers are created and sending data to micro ros agent

Actual behavior

program failed at the 3rd call to rclc_publisher_init_default() with generic error RCL_RET_ERROR

Additional information

At first I successfully created two publishers 'encoders' and 'ina' to publish data from motor encoders and data from ina260 module (amp, volt, watts). Then I decided to add data from the on-board IMU sensor. created everything the same way as I did for ina, but program stopped at that call.

the code snippets below:

the macro I'm using to check return codes:

#define RCCHECK(fn) {               \
    rcl_ret_t temp_rc = fn;         \
    if ((temp_rc != RCL_RET_OK))    \
    {                               \
        error_loop();               \
    }                               \
}

...
some global vars:
rclc_executor_t executor;
rcl_subscription_t subscriber;
rcl_publisher_t enc_pub;
rcl_publisher_t ina_pub;
rcl_publisher_t imu_pub;
rclc_support_t support;
rcl_allocator_t allocator;
rcl_node_t node;

Adafruit_INA260                    ina260       = Adafruit_INA260();

std_msgs__msg__Float32MultiArray  *enc_msg, *ina_msg, *imu_msg;
...
at the setup() function I have the following initializations:
    Serial.begin(9600);

    while (!Serial) {
        delay(1000);
    }

    pinMode(LED_PIN, OUTPUT);
    digitalWrite(LED_PIN, HIGH);

    Serial.println("Starting");

    odom_timer = millis();

    if (!ina260.begin()) {
        Serial.println("Couldn't find INA260 chip");
        error_loop();
    }   

    if (!IMU.begin()) {
        Serial.println("Failed to initialize IMU!");
        error_loop();
    }

    set_microros_wifi_transports(WIFI_SSID, WIFI_PASS, ROS_AGENT_IP, ROS_AGENT_PORT);

    allocator = rcl_get_default_allocator();

    // create //TODO
    RCCHECK(rclc_support_init(&support, 0, NULL, &allocator));

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

    Serial.println("create publisher encoders...");
    // create publishers/subscriptions
    RCCHECK(rclc_publisher_init_default(
                &enc_pub,
                &node,
                ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Float32MultiArray),
                "encoders"));
    delay(100);
    Serial.println("create publisher ina...");
    RCCHECK(rclc_publisher_init_default(
                &ina_pub,
                &node,
                ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Float32MultiArray),
                "ina"));
    delay(100);
    Serial.println("create publisher imu...");
    RCCHECK(rclc_publisher_init_default(
                &imu_pub,
                &node,
                ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Float32MultiArray),
                "imu"));
    delay(100);
    Serial.println("create subscription twist...");
    RCCHECK(rclc_subscription_init_default(
                &subscriber,
                &node,
                ROSIDL_GET_MSG_TYPE_SUPPORT(geometry_msgs, msg, Twist),
                "twist"));

    Serial.println("init executor...");

with all this debug output I see that publisher for encoders and ina are initialized, while I never see it reach the subscription init step.

what's interesting, if I change the order of publisher initialization then I see that publishers for encoders and imu are initialized but publisher for ina failed. no matter the order - first two publishers are created fine, but the 3rd one fails. I don't expect any limitation of this kind.

micro ros agent logs also shows only two first topics created:

[1652238712.003314] info     | Root.cpp           | create_client            | create                 | client_key: 0x0292AA28, session_id: 0x81
[1652238712.003334] info     | SessionManager.hpp | establish_session        | session established    | client_key: 0x0292AA28, address: 10.1.4.96:47138
[1652238712.003444] debug    | UDPv4AgentLinux.cpp | send_message             | [** <<UDP>> **]        | client_key: 0x0292AA28, len: 19
[1652238712.031273] debug    | UDPv4AgentLinux.cpp | recv_message             | [==>> UDP <<==]        | client_key: 0x0292AA28, len: 40
[1652238712.037882] info     | ProxyClient.cpp    | create_participant       | participant created    | client_key: 0x0292AA28, participant_id: 0x000(1)
[1652238712.037984] debug    | UDPv4AgentLinux.cpp | send_message             | [** <<UDP>> **]        | client_key: 0x0292AA28, len: 14
[1652238712.038030] debug    | UDPv4AgentLinux.cpp | send_message             | [** <<UDP>> **]        | client_key: 0x0292AA28, len: 13
[1652238712.064279] debug    | UDPv4AgentLinux.cpp | recv_message             | [==>> UDP <<==]        | client_key: 0x0292AA28, len: 13
[1652238712.067401] debug    | UDPv4AgentLinux.cpp | recv_message             | [==>> UDP <<==]        | client_key: 0x0292AA28, len: 88
[1652238712.067492] info     | ProxyClient.cpp    | create_topic             | topic created          | client_key: 0x0292AA28, topic_id: 0x000(2), participant_id: 0x000(1)
[1652238712.067558] debug    | UDPv4AgentLinux.cpp | send_message             | [** <<UDP>> **]        | client_key: 0x0292AA28, len: 14
[1652238712.067574] debug    | UDPv4AgentLinux.cpp | send_message             | [** <<UDP>> **]        | client_key: 0x0292AA28, len: 13
[1652238712.105273] debug    | UDPv4AgentLinux.cpp | recv_message             | [==>> UDP <<==]        | client_key: 0x0292AA28, len: 13
[1652238712.319975] debug    | UDPv4AgentLinux.cpp | recv_message             | [==>> UDP <<==]        | client_key: 0x0292AA28, len: 24
[1652238712.320030] debug    | UDPv4AgentLinux.cpp | recv_message             | [==>> UDP <<==]        | client_key: 0x0292AA28, len: 13
[1652238712.320239] info     | ProxyClient.cpp    | create_publisher         | publisher created      | client_key: 0x0292AA28, publisher_id: 0x000(3), participant_id: 0x000(1)
[1652238712.320492] debug    | UDPv4AgentLinux.cpp | send_message             | [** <<UDP>> **]        | client_key: 0x0292AA28, len: 13
[1652238712.320559] debug    | UDPv4AgentLinux.cpp | send_message             | [** <<UDP>> **]        | client_key: 0x0292AA28, len: 14
[1652238712.320590] debug    | UDPv4AgentLinux.cpp | send_message             | [** <<UDP>> **]        | client_key: 0x0292AA28, len: 13
[1652238712.331371] debug    | UDPv4AgentLinux.cpp | send_message             | [** <<UDP>> **]        | client_key: 0x0292AA28, len: 13
[1652238712.348436] debug    | UDPv4AgentLinux.cpp | recv_message             | [==>> UDP <<==]        | client_key: 0x0292AA28, len: 13
[1652238712.348627] debug    | UDPv4AgentLinux.cpp | send_message             | [** <<UDP>> **]        | client_key: 0x0292AA28, len: 13
[1652238712.415952] debug    | UDPv4AgentLinux.cpp | recv_message             | [==>> UDP <<==]        | client_key: 0x0292AA28, len: 24
[1652238712.416152] debug    | UDPv4AgentLinux.cpp | send_message             | [** <<UDP>> **]        | client_key: 0x0292AA28, len: 13
[1652238712.456054] debug    | UDPv4AgentLinux.cpp | recv_message             | [==>> UDP <<==]        | client_key: 0x0292AA28, len: 13
[1652238712.456086] debug    | UDPv4AgentLinux.cpp | recv_message             | [==>> UDP <<==]        | client_key: 0x0292AA28, len: 36
[1652238712.456554] info     | ProxyClient.cpp    | create_datawriter        | datawriter created     | client_key: 0x0292AA28, datawriter_id: 0x000(5), publisher_id: 0x000(3)
[1652238712.456659] debug    | UDPv4AgentLinux.cpp | send_message             | [** <<UDP>> **]        | client_key: 0x0292AA28, len: 14
[1652238712.456705] debug    | UDPv4AgentLinux.cpp | send_message             | [** <<UDP>> **]        | client_key: 0x0292AA28, len: 13
[1652238712.457874] debug    | UDPv4AgentLinux.cpp | recv_message             | [==>> UDP <<==]        | client_key: 0x0292AA28, len: 13
[1652238712.457994] debug    | UDPv4AgentLinux.cpp | send_message             | [** <<UDP>> **]        | client_key: 0x0292AA28, len: 13
[1652238712.497290] debug    | UDPv4AgentLinux.cpp | recv_message             | [==>> UDP <<==]        | client_key: 0x0292AA28, len: 13
[1652238712.531583] debug    | UDPv4AgentLinux.cpp | send_message             | [** <<UDP>> **]        | client_key: 0x0292AA28, len: 13
[1652238712.553892] debug    | UDPv4AgentLinux.cpp | recv_message             | [==>> UDP <<==]        | client_key: 0x0292AA28, len: 13
[1652238712.691385] debug    | UDPv4AgentLinux.cpp | recv_message             | [==>> UDP <<==]        | client_key: 0x0292AA28, len: 84
[1652238712.691445] debug    | UDPv4AgentLinux.cpp | recv_message             | [==>> UDP <<==]        | client_key: 0x0292AA28, len: 13
[1652238712.691624] info     | ProxyClient.cpp    | create_topic             | topic created          | client_key: 0x0292AA28, topic_id: 0x001(2), participant_id: 0x000(1)
[1652238712.691655] debug    | UDPv4AgentLinux.cpp | send_message             | [** <<UDP>> **]        | client_key: 0x0292AA28, len: 13
[1652238712.691758] debug    | UDPv4AgentLinux.cpp | send_message             | [** <<UDP>> **]        | client_key: 0x0292AA28, len: 14
[1652238712.691787] debug    | UDPv4AgentLinux.cpp | send_message             | [** <<UDP>> **]        | client_key: 0x0292AA28, len: 13
[1652238712.702500] debug    | UDPv4AgentLinux.cpp | recv_message             | [==>> UDP <<==]        | client_key: 0x0292AA28, len: 13
[1652238712.715798] debug    | UDPv4AgentLinux.cpp | recv_message             | [==>> UDP <<==]        | client_key: 0x0292AA28, len: 84
[1652238712.715914] debug    | UDPv4AgentLinux.cpp | send_message             | [** <<UDP>> **]        | client_key: 0x0292AA28, len: 13
[1652238712.731681] debug    | UDPv4AgentLinux.cpp | send_message             | [** <<UDP>> **]        | client_key: 0x0292AA28, len: 13
[1652238712.736140] debug    | UDPv4AgentLinux.cpp | recv_message             | [==>> UDP <<==]        | client_key: 0x0292AA28, len: 13
[1652238712.736201] debug    | UDPv4AgentLinux.cpp | recv_message             | [==>> UDP <<==]        | client_key: 0x0292AA28, len: 24
[1652238712.736295] info     | ProxyClient.cpp    | create_publisher         | publisher created      | client_key: 0x0292AA28, publisher_id: 0x001(3), participant_id: 0x000(1)
[1652238712.736422] debug    | UDPv4AgentLinux.cpp | send_message             | [** <<UDP>> **]        | client_key: 0x0292AA28, len: 14
[1652238712.736465] debug    | UDPv4AgentLinux.cpp | send_message             | [** <<UDP>> **]        | client_key: 0x0292AA28, len: 13
[1652238712.795743] debug    | UDPv4AgentLinux.cpp | recv_message             | [==>> UDP <<==]        | client_key: 0x0292AA28, len: 13
[1652238712.822689] debug    | UDPv4AgentLinux.cpp | recv_message             | [==>> UDP <<==]        | client_key: 0x0292AA28, len: 13
[1652238712.822724] debug    | UDPv4AgentLinux.cpp | recv_message             | [==>> UDP <<==]        | client_key: 0x0292AA28, len: 36
[1652238712.823283] info     | ProxyClient.cpp    | create_datawriter        | datawriter created     | client_key: 0x0292AA28, datawriter_id: 0x001(5), publisher_id: 0x001(3)
[1652238712.823402] debug    | UDPv4AgentLinux.cpp | send_message             | [** <<UDP>> **]        | client_key: 0x0292AA28, len: 14
[1652238712.823470] debug    | UDPv4AgentLinux.cpp | send_message             | [** <<UDP>> **]        | client_key: 0x0292AA28, len: 13
[1652238712.853808] debug    | UDPv4AgentLinux.cpp | recv_message             | [==>> UDP <<==]        | client_key: 0x0292AA28, len: 13

I tried to debug with gdb-multiarch, but as soon as I reach the corresponding line in my code, 's' doesn't jump into the RCCHECK(rclc_publisher_init_default( &imu_pub, &node, ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Float32MultiArray), "imu")); delay(100); function call. instead it immediately place me into error_loop(). although 'step' in gdb supposed to make step in. sorry this is the first time I'm trying to use gdb to troubleshood arduino.

I also tried to create a publisher of different type Int32 - no luck.

commenting out that call RCCHECK(rclc_publisher_init_default( &imu_pub, &node, ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Float32MultiArray), "imu")); and everything works again and wheels are moving and odometry is published. and ina is reporting current.

replacing rclc_publisher_init_default with rclc_publisher_init_best_effort doesn't help either.

pablogs9 commented 2 years ago

RP2040 nano in micro-ROS for Arduino is prebuild using colcon_verylowmem.meta and as you can see here it only has preconfigured slots for 2 publishers.

You need to rebuild the precompiled library, modifying the values to fit your requirements.

roman-rs commented 2 years ago

RP2040 nano in micro-ROS for Arduino is prebuild using colcon_verylowmem.meta and as you can see here it only has preconfigured slots for 2 publishers.

You need to rebuild the precompiled library, modifying the values to fit your requirements.

Thank you for the reply.

Do you see any potential issues/problems increasing these default values? e.g. if I want to have 5 publishers? 15 publishers? and 5 subscriptions?

rp2040 seems to be powerful enough to handle such values? is it? I'll give it a try for sure (recompiling micro-ros with new values) and update the thread.

pablogs9 commented 2 years ago

It highly depends on the amount of memory available in the MCU. Each entity consumes an amount of RAM and you won't be able to link de binary if the memory overflows.

roman-rs commented 2 years ago

great, I modified the default values in colcon_verylowmem.meta and now 3 publishers are running and pushing data from microros.

Thanks again!