search

micro-ROS / micro_ros_stm32cubemx_utils

A set of utilities for integrating micro-ROS in a STM32CubeMX project
Apache License 2.0
140 stars 56 forks source link

Subscriber number seems to be limited #98

Closed martonmiklos closed 1 year ago

martonmiklos commented 1 year ago

Hi folks,

I am trying to use this library on an STM32F411 Discovery.

I added the following subscribers:

void arm_ros_init()
{
  rcl_ret_t ret = rclc_subscription_init_default(&gripperSubscriber, &node,
                 ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Int32), "gripper");
  ret = rclc_executor_add_subscription(&executor, &gripperSubscriber, &msg_gripper, &gripper_callback, ON_NEW_DATA);

  ret = rclc_subscription_init_default(&armLowSubscriber, &node,
                 ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Int32), "arm_low");
  ret = rclc_executor_add_subscription(&executor, &armLowSubscriber, &msg_arm_low, &arm_low_callback, ON_NEW_DATA);

  ret = rclc_subscription_init_default(&armEnableSubscriber, &node,
                 ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Bool), "arm_enable");
  ret = rclc_executor_add_subscription(&executor, &armEnableSubscriber, &msg_arm_enable, &arm_enable_callback, ON_NEW_DATA);
}

void motor_ros_init()
{
  // 4 motor velocity
  rcl_ret_t ret = rclc_subscription_init_default(&motorFLSubscriber, &node,
      ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Int32), "motor_vel_fl");
  ret = rclc_subscription_init_default(&motorBLSubscriber, &node,
        ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Int32), "motor_vel_bl");
  ret = rclc_subscription_init_default(&motorFRSubscriber, &node,
        ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Int32), "motor_vel_fr");
  ret = rclc_subscription_init_default(&motorBRSubscriber, &node,
        ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Int32), "motor_vel_br");

  ret = rclc_executor_add_subscription(&executor, &motorFLSubscriber, &msg_motor_fl, &motor_velocity_fl_callback, ON_NEW_DATA);
  ret = rclc_executor_add_subscription(&executor, &motorFRSubscriber, &msg_motor_fr, &motor_velocity_fr_callback, ON_NEW_DATA);
  ret = rclc_executor_add_subscription(&executor, &motorBLSubscriber, &msg_motor_bl, &motor_velocity_bl_callback, ON_NEW_DATA);
  ret = rclc_executor_add_subscription(&executor, &motorBRSubscriber, &msg_motor_br, &motor_velocity_br_callback, ON_NEW_DATA);

  // Motor enable
  ret = rclc_subscription_init_default(&motorEnableSubscriber, &node,
                 ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Bool), "motor_enable");

  ret = rclc_executor_add_subscription(&executor, &motorEnableSubscriber, &msg_motor_enable, &motor_enable_callback, ON_NEW_DATA);
}

The ret = rclc_executor_add_subscription(&executor, &motorBRSubscriber, &msg_motor_br, &motor_velocity_br_callback, ON_NEW_DATA); and ret = rclc_executor_add_subscription(&executor, &motorEnableSubscriber, &msg_motor_enable, &motor_enable_callback, ON_NEW_DATA); are returning with 1.

I have increased the RMW_UXRCE_MAX_SUBSCRIPTIONS to 10, rebuilt the libmicroros.a but it does not changed the behavior (it changed hash of the .a file so the parameter really changed something) https://github.com/micro-ROS/micro_ros_stm32cubemx_utils/blob/humble/microros_static_library_ide/library_generation/colcon.meta#L45

The rclc_executor_add_subscription calls do not generate anything in the host side, only the rclc_subscription_init_default calls.

The first rclc_subscription_init_default of the failing rclc_executor_add_subscription call has this in the output:

[1681571752.829693] debug    | SerialAgentLinux.cpp | recv_message             | [==>> SER <<==]        | client_key: 0x5851F42D, len: 100, data: 
0000: 81 80 13 00 08 01 10 00 00 22 00 56 80 00 00 01 FF FF 00 00 00 00 00 00 01 07 46 00 00 23 00 62
0020: 02 03 00 00 38 00 00 00 10 00 00 00 72 74 2F 6D 6F 74 6F 72 5F 76 65 6C 5F 62 72 00 00 01 00 20
0040: 1C 00 00 00 73 74 64 5F 6D 73 67 73 3A 3A 6D 73 67 3A 3A 64 64 73 5F 3A 3A 49 6E 74 33 32 5F 00
0060: 00 01 00 00
[1681571752.833811] debug    | ProxyClient.cpp    | create_object            | objects matched        | client_key: 0x5851F42D, object_id: 0x0006
[1681571752.833923] debug    | SerialAgentLinux.cpp | send_message             | [** <<SER>> **]        | client_key: 0x5851F42D, len: 14, data: 
0000: 81 80 13 00 05 01 06 00 00 23 00 62 01 00
[1681571752.833971] debug    | SerialAgentLinux.cpp | send_message             | [** <<SER>> **]        | client_key: 0x5851F42D, len: 13, data: 
0000: 81 00 00 00 0A 01 05 00 14 00 00 00 80
[1681571752.845665] debug    | SerialAgentLinux.cpp | recv_message             | [==>> SER <<==]        | client_key: 0x5851F42D, len: 13, data: 
0000: 81 00 00 00 0A 01 05 00 14 00 00 00 80
[1681571752.845714] debug    | SerialAgentLinux.cpp | recv_message             | [==>> SER <<==]        | client_key: 0x5851F42D, len: 24, data: 
0000: 81 80 14 00 01 07 10 00 00 24 00 64 04 03 00 01 02 00 00 00 00 00 00 01
[1681571752.845848] debug    | ProxyClient.cpp    | create_object            | objects matched        | client_key: 0x5851F42D, object_id: 0x0006
[1681571752.846022] debug    | SerialAgentLinux.cpp | send_message             | [** <<SER>> **]        | client_key: 0x5851F42D, len: 14, data: 
0000: 81 80 14 00 05 01 06 00 00 24 00 64 01 00
[1681571752.846076] debug    | SerialAgentLinux.cpp | send_message             | [** <<SER>> **]        | client_key: 0x5851F42D, len: 13, data: 
0000: 81 00 00 00 0A 01 05 00 15 00 00 00 80
[1681571752.870728] debug    | SerialAgentLinux.cpp | recv_message             | [==>> SER <<==]        | client_key: 0x5851F42D, len: 13, data: 
0000: 81 00 00 00 0A 01 05 00 15 00 00 00 80
[1681571752.870789] debug    | SerialAgentLinux.cpp | recv_message             | [==>> SER <<==]        | client_key: 0x5851F42D, len: 40, data: 
0000: 81 80 15 00 01 07 1D 00 00 25 00 66 06 03 00 01 0F 00 00 00 00 62 01 00 03 00 01 00 0A 00 00 00
0020: 00 00 00 00 64 00 00 00
[1681571752.870948] debug    | ProxyClient.cpp    | create_object            | objects matched        | client_key: 0x5851F42D, object_id: 0x0006
[1681571752.871089] debug    | SerialAgentLinux.cpp | send_message             | [** <<SER>> **]        | client_key: 0x5851F42D, len: 14, data: 
0000: 81 80 15 00 05 01 06 00 00 25 00 66 01 00
[1681571752.871139] debug    | SerialAgentLinux.cpp | send_message             | [** <<SER>> **]        | client_key: 0x5851F42D, len: 13, data: 
0000: 81 00 00 00 0A 01 05 00 16 00 00 00 80
[1681571752.886670] debug    | SerialAgentLinux.cpp | recv_message             | [==>> SER <<==]        | client_key: 0x5851F42D, len: 13, data: 
0000: 81 00 00 00 0A 01 05 00 16 00 00 00 80
martonmiklos commented 1 year ago

Dawg. The ROS_HANDLE_NUM needed to be adjusted in the rclc_executor_init(&executor, &support.context, ROS_HANDLE_NUM, &allocator); call.

https://github.com/ros2/rclc/blob/master/rclc/src/rclc/executor.c#L221

Sorry for the noise, but it took me a while to figure this out.