Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Workaround for micro-ros_publisher_wifi example hanging after 90 seconds #1499

Open
kaiaai opened this issue Aug 16, 2023 · 6 comments
Open

Workaround for micro-ros_publisher_wifi example hanging after 90 seconds #1499

kaiaai opened this issue Aug 16, 2023 · 6 comments

Comments

@kaiaai
Copy link

kaiaai commented Aug 16, 2023

Issue template

I'd like to share a workaround for micro-ros_publisher_wifi example hanging after 90 seconds.

  • Hardware description: a ESP32-WROOM-32 board connected to a Docker
  • RTOS: Arduino "ESP32 Dev Module" board, default settings, with Micro-ROS
  • Installation type: microros/micro-ros-agent:humble image, Arduino IDE 1.8.13 (Windows), v2.0.7-humble precompiled Micro-ROS
  • Version or commit hash: humble

Steps to reproduce the issue

  • Edit the sketch below to specify your WiFi SSID, password and IP of your PC that will run a Micro-ROS agent. Use the Arduino IDE (Windows) to compile and upload the sketch to an ESP32 module.
  • Launch the Micro-ROS agent. In my case, I'm using a Windows machine with Docker-for-Windows installed. Here is the Windows shell command to launch the Docker Micro-ROS agent image.
docker run --name micro-ros-agent-humble -it -p 8888:8888/udp microros/micro-ros-agent:humble udp4 -p 8888

Once ESP32 connects to WiFi and Micro-ROS agent, I launch another bash and run

ros2 topic echo int32pub
data: 1201
---
data: 1202
---
data: 1203
---
data: 1204
...

Expected behavior

micro-ros_publisher_wifi example keeps running indefinitely long, until powered down

Actual behavior

After received 90 seconds. ros2 topic echo int32pub hangs, does not output message data anymore.

ros2 topic echo int32pub
...
data: 1796
---
data: 1797
---
data: 1798
---
<OUTPUT STOPS>

Additional information

I've fixed this behavior by having ESP32 ping Micro-ROS agent periodically. Please see the code below.

I have also fixed this by

  • changing rclc_publisher_init_best_effort() to rclc_publisher_init_best_effort()
  • adding a subscriber to ESP32

Either of the fixes work, but the ping one is the most convenient for me.

#include <micro_ros_arduino.h>
#include <stdio.h>
#include <rcl/rcl.h>
#include <rcl/error_handling.h>
#include <rclc/rclc.h>
#include <std_msgs/msg/int32.h>

#if !defined(ESP32) && !defined(TARGET_PORTENTA_H7_M7) && !defined(ARDUINO_NANO_RP2040_CONNECT)
#error This example is only avaible for Arduino Portenta, Arduino Nano RP2040 Connect and ESP32 Dev module
#endif

rcl_publisher_t publisher;
std_msgs__msg__Int32 msg;
rclc_support_t support;
rcl_allocator_t allocator;
rcl_node_t node;

#define LED_PIN 2
#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(){
  Serial.println(F("error_loop()"));
  while(1){
    digitalWrite(LED_PIN, !digitalRead(LED_PIN));
    delay(100);
  }
}

void setup() {
  Serial.begin(115200);
  set_microros_wifi_transports((char*)"WIFI_SSID", (char*)"WIFI_PASSWORD",
    (char*)"192.168.1.UROS_AGENT_IP", 8888);

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

  delay(2000);

  allocator = rcl_get_default_allocator();
 
  RCCHECK(rclc_support_init(&support, 0, NULL, &allocator));
  RCCHECK(rclc_node_init_default(&node, "uros_arduino", "", &support));
  RCCHECK(rclc_publisher_init_best_effort(
    &publisher,
    &node,
    ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Int32),
    "int32pub"));

  msg.data = 0;
}

void loop() {
  rcl_ret_t rc = rcl_publish(&publisher, &msg, NULL);
  msg.data++;

  if ((rc == RCL_RET_OK) && (msg.data % 600 == 0)) {
    // pub freezes after 90sec if the line below is commented out
    rmw_uros_ping_agent(1, 1);
  }
    
  delay(50);
}
@pablogs9
Copy link
Member

If you remove the ping and the subscription, and include the publisher inside a timer and spin an executor, does it solve the isuee?

@kaiaai
Copy link
Author

kaiaai commented Aug 17, 2023

I have tried publishing inside a timer and spinning the executor in loop() - no, it doesn't resolve the issue. It still freezes exactly the same like the original example - 90 seconds after connecting to the agent.

Here is the timer-and-executor-spin code for your reference:

// Publisher hangs after 90 seconds

#include <micro_ros_arduino.h>
#include <rcl/rcl.h>
#include <rcl/error_handling.h>
#include <rclc/rclc.h>
#include <std_msgs/msg/int32.h>
#include <rclc/executor.h>

#if !defined(ESP32) && !defined(TARGET_PORTENTA_H7_M7) && !defined(ARDUINO_NANO_RP2040_CONNECT)
#error This example is only avaible for Arduino Portenta, Arduino Nano RP2040 Connect and ESP32 Dev module
#endif

rcl_publisher_t publisher;
std_msgs__msg__Int32 msg;
rclc_support_t support;
rcl_allocator_t allocator;
rcl_node_t node;
rcl_timer_t timer;
rclc_executor_t executor;

#define LED_PIN 2
#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(){
  Serial.println(F("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));
    msg.data++;
  }
}
void setup() {
  Serial.begin(115200);
  set_microros_wifi_transports((char*)"WIFI_SSID", (char*)"WIFI_PASSWORD",
    (char*)"192.168.1.UROS_AGENT_IP", 8888);

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

  delay(2000);

  allocator = rcl_get_default_allocator();
 
  RCCHECK(rclc_support_init(&support, 0, NULL, &allocator));
  RCCHECK(rclc_node_init_default(&node, "uros_arduino", "", &support));
  RCCHECK(rclc_publisher_init_best_effort(
    &publisher,
    &node,
    ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Int32),
    "int32pub"));

  // create timer,
  const unsigned int timer_timeout = 50;
  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));
  
  msg.data = 0;
}

void loop() {
  delay(50);
  RCSOFTCHECK(rclc_executor_spin_some(&executor, RCL_MS_TO_NS(100)));
}

The ros2 topic echo int32pub output is same:

PS C:\Users\test> docker exec -it micro-ros-agent-humble bash
root@f93fea7e8390:/uros_ws# ros2 topic list
/int32pub
/parameter_events
/rosout
root@f93fea7e8390:/uros_ws# ros2 topic echo int32pub
...
data: 1762
---
data: 1763
---
data: 1764
---
data: 1765
---
data: 1766
---
data: 1767
---
data: 1768
---
data: 1769
---
<HANGS, NO MORE OUTPUT>

And here is the Micro-ROS agent output FYI

PS C:\Users\test> docker run --name micro-ros-agent-humble -it --rm -p 8888:8888/udp microros/micro-ros-agent:humble udp4 -p 8888
[1692261265.759104] info     | UDPv4AgentLinux.cpp | init                     | running...             | port: 8888
[1692261265.759779] info     | Root.cpp           | set_verbose_level        | logger setup           | verbose_level: 4
[1692261336.530337] info     | Root.cpp           | create_client            | create                 | client_key: 0x63163AC8, session_id: 0x81
[1692261336.530427] info     | SessionManager.hpp | establish_session        | session established    | client_key: 0x63163AC8, address: 172.17.0.1:43676
[1692261336.546124] info     | ProxyClient.cpp    | create_participant       | participant created    | client_key: 0x63163AC8, participant_id: 0x000(1)
[1692261336.577806] info     | ProxyClient.cpp    | create_topic             | topic created          | client_key: 0x63163AC8, topic_id: 0x000(2), participant_id: 0x000(1)
[1692261336.586472] info     | ProxyClient.cpp    | create_publisher         | publisher created      | client_key: 0x63163AC8, publisher_id: 0x000(3), participant_id: 0x000(1)
[1692261336.594476] info     | ProxyClient.cpp    | create_datawriter        | datawriter created     | client_key: 0x63163AC8, datawriter_id: 0x000(5), publisher_id: 0x000(3)

@pablogs9
Copy link
Member

  • Could you paste the output of the agent with -v6.
  • Also does it works if you use a reliable publisher?

@kaiaai
Copy link
Author

kaiaai commented Aug 17, 2023

Simply changing rclc_publisher_init_best_effort() to rclc_publisher_init_default() in the original example fixes the problem.
Here is the -v6 output. The agent's output keeps running after ros2 topic echo int32pub hangs. Looks like the agent keeps getting the messages, but stops re-publishing them to /int32pub.

PS C:\Users\test> docker run --name micro-ros-agent-humble -it --rm -p 8888:8888/udp microros/micro-ros-agent:humble udp4 -p 8888 -v6
...
0000: 81 01 7C 13 07 01 08 00 13 8A 00 05 7C 13 00 00
[1692309875.235895] debug    | UDPv4AgentLinux.cpp | recv_message             | [==>> UDP <<==]        | client_key: 0x00000000, len: 16, data:
0000: 81 01 7D 13 07 01 08 00 13 8B 00 05 7D 13 00 00
[1692309875.287396] debug    | UDPv4AgentLinux.cpp | recv_message             | [==>> UDP <<==]        | client_key: 0x00000000, len: 16, data:
0000: 81 01 7E 13 07 01 08 00 13 8C 00 05 7E 13 00 00
[1692309875.336792] debug    | UDPv4AgentLinux.cpp | recv_message             | [==>> UDP <<==]        | client_key: 0x00000000, len: 16, data:
0000: 81 01 7F 13 07 01 08 00 13 8D 00 05 7F 13 00 00
[1692309875.388022] debug    | UDPv4AgentLinux.cpp | recv_message             | [==>> UDP <<==]        | client_key: 0x00000000, len: 16, data:
0000: 81 01 80 13 07 01 08 00 13 8E 00 05 80 13 00 00
[1692309875.438430] debug    | UDPv4AgentLinux.cpp | recv_message             | [==>> UDP <<==]        | client_key: 0x00000000, len: 16, data:
0000: 81 01 81 13 07 01 08 00 13 8F 00 05 81 13 00 00
[1692309875.492285] debug    | UDPv4AgentLinux.cpp | recv_message             | [==>> UDP <<==]        | client_key: 0x00000000, len: 16, data:
0000: 81 01 82 13 07 01 08 00 13 90 00 05 82 13 00 00
[1692309875.540532] debug    | UDPv4AgentLinux.cpp | recv_message             | [==>> UDP <<==]        | client_key: 0x00000000, len: 16, data:
0000: 81 01 83 13 07 01 08 00 13 91 00 05 83 13 00 00
[1692309875.591825] debug    | UDPv4AgentLinux.cpp | recv_message             | [==>> UDP <<==]        | client_key: 0x00000000, len: 16, data:
0000: 81 01 84 13 07 01 08 00 13 92 00 05 84 13 00 00
[1692309875.641564] debug    | UDPv4AgentLinux.cpp | recv_message             | [==>> UDP <<==]        | client_key: 0x00000000, len: 16, data:
0000: 81 01 85 13 07 01 08 00 13 93 00 05 85 13 00 00
[1692309875.693420] debug    | UDPv4AgentLinux.cpp | recv_message             | [==>> UDP <<==]        | client_key: 0x00000000, len: 16, data:
0000: 81 01 86 13 07 01 08 00 13 94 00 05 86 13 00 00
[1692309875.743523] debug    | UDPv4AgentLinux.cpp | recv_message             | [==>> UDP <<==]        | client_key: 0x00000000, len: 16, data:
0000: 81 01 87 13 07 01 08 00 13 95 00 05 87 13 00 00
[1692309875.794443] debug    | UDPv4AgentLinux.cpp | recv_message             | [==>> UDP <<==]        | client_key: 0x00000000, len: 16, data:
0000: 81 01 88 13 07 01 08 00 13 96 00 05 88 13 00 00
[1692309875.849060] debug    | UDPv4AgentLinux.cpp | recv_message             | [==>> UDP <<==]        | client_key: 0x00000000, len: 16, data:
0000: 81 01 89 13 07 01 08 00 13 97 00 05 89 13 00 00
[1692309875.895805] debug    | UDPv4AgentLinux.cpp | recv_message             | [==>> UDP <<==]        | client_key: 0x00000000, len: 16, data:
0000: 81 01 8A 13 07 01 08 00 13 98 00 05 8A 13 00 00
[1692309875.946143] debug    | UDPv4AgentLinux.cpp | recv_message             | [==>> UDP <<==]        | client_key: 0x00000000, len: 16, data:
0000: 81 01 8B 13 07 01 08 00 13 99 00 05 8B 13 00 00
[1692309875.996516] debug    | UDPv4AgentLinux.cpp | recv_message             | [==>> UDP <<==]        | client_key: 0x00000000, len: 16, data:
0000: 81 01 8C 13 07 01 08 00 13 9A 00 05 8C 13 00 00
^C[ros2run]: Interrupt

@pablogs9
Copy link
Member

Could you check if this PR solves this issue: micro-ROS/rmw_microxrcedds#305 ?

@kaiaai
Copy link
Author

kaiaai commented Aug 19, 2023

I'm afraid it's not obvious for me what and how to rebuild to get that PR in. I tried searching rmw_microxrcedds package in index.ros.org to see which packages depend on it and have to be rebuilt, but it's not listed. I've tried looked at micro_ros_setup .sh built scripts and it appears to be the right place, but still far from obvious.

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Assignees
No one assigned
Labels
None yet
Projects
None yet
Milestone
No milestone
Development

No branches or pull requests
2 participants
@pablogs9 @kaiaai