search

micro-ROS / micro_ros_zephyr_module

micro-ROS Zephyr module and sample code
Apache License 2.0
46 stars 22 forks source link

Ping does not work after `rclc_support_init` is called #62

Closed JiangengDong closed 1 year ago

JiangengDong commented 2 years ago

Description

It seems the rmw_uros_ping_agent always returns RMW_RET_ERROR after the rclc_support_init is called.

(Please correct me if this is not the right place for this issue)

Steps to reproduce the issue

Two tests were tried to reproduce the problem. The agent was kept alive during the test.

  1. First, I tried the apps/micro-ros_reconnection from this repository.
  2. Second, I tried to simplify that program to reveal the real cause. The simplified program is attached in the "Additional information" part below. What it does is to wait until the agent is available, initialize the rclc_support_t, and then check the agent's availability again with ping.

Expected behavior

  1. For the first test (official demo), the state machine should stay at the AGENT_CONNECTED state.
  2. For the second test (my custom test), it should keep printing loop 0|1|2|....

Actual behavior

The firmware keeps connecting to and disconnecting from the agent repeatedly.

The log from the first test (official demo)

[00:00:00.000,000] <inf> can_stm32: Init of CAN_1 done
[00:00:02.478,000] <err> eth_stm32_hal: Failed to enqueue frame into RX queue: -115
[00:00:02.480,000] <inf> uros_transport: Waiting for uROS agent.

[00:00:03.487,000] <inf> uros_transport: Connected to uROS agent and created entities.
[00:00:03.988,000] <inf> uros_transport: Disconnected from agent. Switching to waiting for uROS agent

[00:00:03.996,000] <inf> uros_transport: Waiting for uROS agent.

[00:00:05.004,000] <inf> uros_transport: Connected to uROS agent and created entities.
[00:00:05.504,000] <inf> uros_transport: Disconnected from agent. Switching to waiting for uROS agent

[00:00:05.513,000] <inf> uros_transport: Waiting for uROS agent.

The log from the second test (my custom test)

[00:00:00.000,000] <inf> can_stm32: Init of CAN_1 done
[00:00:02.393,000] <err> eth_stm32_hal: Failed to enqueue frame into RX queue: -115
[00:00:02.394,000] <inf> microros: Found the microros agent.
[00:00:04.396,000] <inf> microros: Disconnect from the microros agent
[00:00:05.397,000] <inf> microros: Found the microros agent.
[00:00:07.400,000] <inf> microros: Disconnect from the microros agent
[00:00:08.400,000] <inf> microros: Found the microros agent.
[00:00:10.402,000] <inf> microros: Disconnect from the microros agent
[00:00:11.403,000] <inf> microros: Found the microros agent.

Additional information

Test program

The program I used for the test is as follows. 

#include <stdint.h>

#include "zephyr.h"
#include "zephyr/logging/log.h"

// microros
#include "microros_transports.h"
#include "rcl/rcl.h"
#include "rclc/rclc.h"
#include "rmw_microros/rmw_microros.h"

LOG_MODULE_REGISTER(microros);

#define RCCHECK(fn)                                              \
  {                                                              \
    rcl_ret_t temp_rc = fn;                                      \
    if ((temp_rc != RCL_RET_OK)) {                               \
      LOG_ERR("line %d: error %d.", __LINE__, (int)(temp_rc));   \
    }                                                            \
  }

void main() {
  rmw_uros_set_custom_transport(MICRO_ROS_FRAMING_REQUIRED, (void*)(&default_params), &zephyr_transport_open,
                                &zephyr_transport_close, &zephyr_transport_write, &zephyr_transport_read);
  while (true) {
    while (rmw_uros_ping_agent(100, 1) != RMW_RET_OK) {
      LOG_INF("Waiting for microros agent...");
      k_msleep(1000);
    }
    LOG_INF("Found the microros agent.");

    rcl_allocator_t allocator;
    rclc_support_t support;
    allocator = rcl_get_default_allocator();
    RCCHECK(rclc_support_init(&support, 0, NULL, &allocator))
    k_msleep(1000);

    size_t loop_count = 0;
    while (rmw_uros_ping_agent(1000, 1) == RMW_RET_OK) {
      LOG_INF("loop %d", loop_count++);
      k_msleep(1000);
    }

    RCCHECK(rclc_support_fini(&support))
    LOG_INF("Disconnect from the microros agent");
    k_msleep(1000);
  }
}

Agent log

The agent verbose level is set to 4, and the INFO level agent log is as follows. 

[1656710159.408739] info     | UDPv4AgentLinux.cpp | init                     | running...             | port: 8888
[1656710159.408878] info     | Root.cpp           | set_verbose_level        | logger setup           | verbose_level: 4
[1656710160.147612] info     | Root.cpp           | create_client            | create                 | client_key: 0x57995EE5, session_id: 0x81
[1656710160.147740] info     | SessionManager.hpp | establish_session        | session established    | client_key: 0x57995EE5, address: 10.42.0.2:19125
[1656710162.149116] info     | Root.cpp           | delete_client            | delete                 | client_key: 0x57995EE5
[1656710162.149203] info     | SessionManager.hpp | destroy_session          | session closed         | client_key: 0x57995EE5, address: 10.42.0.2:19125
[1656710163.151464] info     | Root.cpp           | create_client            | create                 | client_key: 0x567B653D, session_id: 0x81
[1656710163.151542] info     | SessionManager.hpp | establish_session        | session established    | client_key: 0x567B653D, address: 10.42.0.2:4599
[1656710165.152946] info     | Root.cpp           | delete_client            | delete                 | client_key: 0x567B653D
[1656710165.153018] info     | SessionManager.hpp | destroy_session          | session closed         | client_key: 0x567B653D, address: 10.42.0.2:4599
pablogs9 commented 2 years ago

Which version of the micro-ROS Agent are you using?

pablogs9 commented 2 years ago

I have tested it with micro-ROS Agent Docker for Humble version and this works as expected. Can you update your micro-ROS agent to the latest version?

aditya2592 commented 2 years ago

I have tested it with micro-ROS Agent Docker for Humble version and this works as expected. Can you update your micro-ROS agent to the latest version?

We will test with Humble and get back, might take a little while because we are in the process of upgrading the ROS2 side to Humble as well, which I assume would be a requirement for having micro-ROS on Humble.

JiangengDong commented 1 year ago

The problem disappears after upgrading to the Humble version. Thanks for your kindly help!