search

micro-ROS / micro_ros_espidf_component

micro-ROS ESP32 IDF component and sample code
Apache License 2.0
229 stars 53 forks source link

FreeRTOS tasks are not working #236

Open SynKolbasyn opened 3 months ago

SynKolbasyn commented 3 months ago

Issue template

Steps to reproduce the issue

  1. I created the microros_ws/firmware/freeertos_apps/apps/test_project/ folder
  2. I have created two files in this folder: app-colcon.meta: 
    {
"names": {
    "rmw_microxrcedds": {
        "cmake-args": [
            "-DRMW_UXRCE_MAX_NODES=1",
            "-DRMW_UXRCE_MAX_PUBLISHERS=2",
            "-DRMW_UXRCE_MAX_SUBSCRIPTIONS=0",
            "-DRMW_UXRCE_MAX_SERVICES=0",
            "-DRMW_UXRCE_MAX_CLIENTS=0",
            "-DRMW_UXRCE_MAX_HISTORY=1",
        ]
    }
}
}

main.c:

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

#include <rclc/rclc.h>
#include <rclc/executor.h>

#include "freertos/FreeRTOS.h"
#include "freertos/task.h"

#define RCCHECK(fn) { rcl_ret_t temp_rc = fn; if((temp_rc != RCL_RET_OK)){printf("Failed status on line %d: %d. Aborting.\n",__LINE__,(int)temp_rc);vTaskDelete(NULL);}}
#define RCSOFTCHECK(fn) { rcl_ret_t temp_rc = fn; if((temp_rc != RCL_RET_OK)){printf("Failed status on line %d: %d. Continuing.\n",__LINE__,(int)temp_rc);}}

rcl_publisher_t publisher1;
rcl_publisher_t publisher2;

void task_1(void* param);
void task_2(void* param);

void appMain(void* arg) {
  // support & allocator
  rcl_allocator_t allocator = rcl_get_default_allocator();
  rclc_support_t support;
  RCCHECK(rclc_support_init(&support, 0, NULL, &allocator));

  // node
  rcl_node_t node;
  RCCHECK(rclc_node_init_default(&node, "esp32", "", &support));

  // publishers
  RCCHECK(rclc_publisher_init_default(&publisher1, &node, ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Int32), "/pub1"));
  RCCHECK(rclc_publisher_init_default(&publisher2, &node, ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Int32), "/pub2"));

  // tasks
  xTaskCreate(task_1, "task_1", 2048, NULL, 1, NULL);
  xTaskCreate(task_2, "task_2", 2048, NULL, 1, NULL);

  while (1) {
    vTaskDelay(1000000 / portTICK_PERIOD_MS);
  }

  // clean
  RCCHECK(rcl_publisher_fini(&publisher1, &node));
  RCCHECK(rcl_publisher_fini(&publisher2, &node));
  RCCHECK(rcl_node_fini(&node));
  vTaskDelete(NULL);
}

void task_1(void* param) {
  std_msgs__msg__Int32 msg;
  msg.data = 0;

  while (1) {
    RCSOFTCHECK(rcl_publish(&publisher1, &msg, NULL));
    ++msg.data;
    vTaskDelay(1000 / portTICK_PERIOD_MS);
  }

  vTaskDelete(NULL);
}

void task_2(void* param) {
  std_msgs__msg__Int32 msg;
  msg.data = 0;

  while (1) {
    RCSOFTCHECK(rcl_publish(&publisher2, &msg, NULL));
    ++msg.data;
    vTaskDelay(1000 / portTICK_PERIOD_MS);
  }

  vTaskDelete(NULL);
}
  1. Ran the following commands in microros_ws/ 
    ros2 run micro_ros_setup configure_firmware.sh test_app -t serial
ros2 run micro_ros_setup build_firmware.sh
ros2 run micro_ros_setup flash_firmware.sh
source install/local_setup.bash
ros2 run micro_ros_agent micro_ros_agent serial --dev /dev/ttyACM0 -v 6

Expected behavior

I expect to create 2 tasks on esp32 that will independently send messages to different topics

Actual behavior

the agent sends me this message non-stop:

[1711220922.525154] info     | Root.cpp           | create_client            | create                 | client_key: 0x0102790A, session_id: 0x81
[1711220922.525164] info     | SessionManager.hpp | establish_session        | session established    | client_key: 0x0102790A, address: 0
[1711220922.525312] debug    | SerialAgentLinux.cpp | send_message             | [** <<SER>> **]        | client_key: 0x0102790A, len: 19, data:
0000: 81 00 00 00 04 01 0B 00 00 00 58 52 43 45 01 00 01 0F 00
[1711220922.536865] debug    | SerialAgentLinux.cpp | recv_message             | [==>> SER <<==]        | client_key: 0x0102790A, len: 36, data:
0000: 81 80 00 00 01 07 1C 00 00 0A 00 01 01 03 00 00 0E 00 00 00 00 01 A5 A5 06 00 00 00 65 73 70 33
0020: 32 00 00 00
[1711220922.540084] info     | ProxyClient.cpp    | create_participant       | participant created    | client_key: 0x0102790A, participant_id: 0x000(1)
[1711220922.540383] debug    | SerialAgentLinux.cpp | send_message             | [** <<SER>> **]        | client_key: 0x0102790A, len: 14, data:
0000: 81 80 00 00 05 01 06 00 00 0A 00 01 00 00
[1711220922.540409] debug    | SerialAgentLinux.cpp | send_message             | [** <<SER>> **]        | client_key: 0x0102790A, len: 13, data:
0000: 81 00 00 00 0A 01 05 00 01 00 00 00 80
[1711220922.549822] debug    | SerialAgentLinux.cpp | recv_message             | [==>> SER <<==]        | client_key: 0x0102790A, len: 13, data:
0000: 81 00 00 00 0A 01 05 00 01 00 00 00 80
[1711220922.556567] debug    | SerialAgentLinux.cpp | recv_message             | [==>> SER <<==]        | client_key: 0x0102790A, len: 72, data:
0000: 81 80 01 00 01 07 3E 00 00 0B 00 02 02 03 00 00 30 00 00 00 08 00 00 00 72 74 2F 70 75 62 31 00
0020: 00 01 FB 3F 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
0040: 33 32 5F 00 00 01 00 00
[1711220922.556891] info     | ProxyClient.cpp    | create_topic             | topic created          | client_key: 0x0102790A, topic_id: 0x000(2), participant_id: 0x000(1)
[1711220922.557048] debug    | SerialAgentLinux.cpp | send_message             | [** <<SER>> **]        | client_key: 0x0102790A, len: 14, data:
0000: 81 80 01 00 05 01 06 00 00 0B 00 02 00 00
[1711220922.557088] debug    | SerialAgentLinux.cpp | send_message             | [** <<SER>> **]        | client_key: 0x0102790A, len: 13, data:
0000: 81 00 00 00 0A 01 05 00 02 00 00 00 80
[1711220922.567315] debug    | SerialAgentLinux.cpp | recv_message             | [==>> SER <<==]        | client_key: 0x0102790A, len: 13, data:
0000: 81 00 00 00 0A 01 05 00 02 00 00 00 80
[1711220922.567381] debug    | SerialAgentLinux.cpp | recv_message             | [==>> SER <<==]        | client_key: 0x0102790A, len: 24, data:
0000: 81 80 02 00 01 07 10 00 00 0C 00 03 03 03 00 00 02 00 00 00 00 00 00 01
[1711220922.567630] info     | ProxyClient.cpp    | create_publisher         | publisher created      | client_key: 0x0102790A, publisher_id: 0x000(3), participant_id: 0x000(1)
[1711220922.567720] debug    | SerialAgentLinux.cpp | send_message             | [** <<SER>> **]        | client_key: 0x0102790A, len: 14, data:
0000: 81 80 02 00 05 01 06 00 00 0C 00 03 00 00
[1711220922.567747] debug    | SerialAgentLinux.cpp | send_message             | [** <<SER>> **]        | client_key: 0x0102790A, len: 13, data:
0000: 81 00 00 00 0A 01 05 00 03 00 00 00 80
[1711220922.579117] debug    | SerialAgentLinux.cpp | recv_message             | [==>> SER <<==]        | client_key: 0x0102790A, len: 13, data:
0000: 81 00 00 00 0A 01 05 00 03 00 00 00 80
[1711220922.579199] debug    | SerialAgentLinux.cpp | recv_message             | [==>> SER <<==]        | client_key: 0x0102790A, len: 36, data:
0000: 81 80 03 00 01 07 1C 00 00 0D 00 05 05 03 00 00 0E 00 00 00 00 02 01 00 03 00 01 3F 0A 00 00 00
0020: 00 00 00 03
[1711220922.579590] info     | ProxyClient.cpp    | create_datawriter        | datawriter created     | client_key: 0x0102790A, datawriter_id: 0x000(5), publisher_id: 0x000(3)
[1711220922.579658] debug    | SerialAgentLinux.cpp | send_message             | [** <<SER>> **]        | client_key: 0x0102790A, len: 14, data:
0000: 81 80 03 00 05 01 06 00 00 0D 00 05 00 00
[1711220922.579672] debug    | SerialAgentLinux.cpp | send_message             | [** <<SER>> **]        | client_key: 0x0102790A, len: 13, data:
0000: 81 00 00 00 0A 01 05 00 04 00 00 00 80
[1711220922.593951] debug    | SerialAgentLinux.cpp | recv_message             | [==>> SER <<==]        | client_key: 0x0102790A, len: 13, data:
0000: 81 00 00 00 0A 01 05 00 04 00 00 00 80
[1711220922.593998] debug    | SerialAgentLinux.cpp | recv_message             | [==>> SER <<==]        | client_key: 0x0102790A, len: 72, data:
0000: 81 80 04 00 01 07 3E 00 00 0E 00 12 02 03 00 00 30 00 00 00 08 00 00 00 72 74 2F 70 75 62 32 00
0020: 00 01 00 00 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
0040: 33 32 5F 00 00 01 00 00
[1711220922.594114] info     | ProxyClient.cpp    | create_topic             | topic created          | client_key: 0x0102790A, topic_id: 0x001(2), participant_id: 0x000(1)
[1711220922.594195] debug    | SerialAgentLinux.cpp | send_message             | [** <<SER>> **]        | client_key: 0x0102790A, len: 14, data:
0000: 81 80 04 00 05 01 06 00 00 0E 00 12 00 00
[1711220922.594214] debug    | SerialAgentLinux.cpp | send_message             | [** <<SER>> **]        | client_key: 0x0102790A, len: 13, data:
0000: 81 00 00 00 0A 01 05 00 05 00 00 00 80
[1711220922.672631] debug    | SerialAgentLinux.cpp | recv_message             | [==>> SER <<==]        | client_key: 0x0102790A, len: 13, data:
0000: 81 00 00 00 0A 01 05 00 05 00 00 00 80
[1711220922.672714] debug    | SerialAgentLinux.cpp | recv_message             | [==>> SER <<==]        | client_key: 0x0102790A, len: 24, data:
0000: 81 80 05 00 01 07 10 00 00 0F 00 13 03 03 00 00 02 00 00 00 00 00 00 01
[1711220922.672917] info     | ProxyClient.cpp    | create_publisher         | publisher created      | client_key: 0x0102790A, publisher_id: 0x001(3), participant_id: 0x000(1)
[1711220922.673138] debug    | SerialAgentLinux.cpp | send_message             | [** <<SER>> **]        | client_key: 0x0102790A, len: 14, data:
0000: 81 80 05 00 05 01 06 00 00 0F 00 13 00 00
[1711220922.673168] debug    | SerialAgentLinux.cpp | send_message             | [** <<SER>> **]        | client_key: 0x0102790A, len: 13, data:
0000: 81 00 00 00 0A 01 05 00 06 00 00 00 80
[1711220922.696431] debug    | SerialAgentLinux.cpp | recv_message             | [==>> SER <<==]        | client_key: 0x0102790A, len: 13, data:
0000: 81 00 00 00 0A 01 05 00 06 00 00 00 80
[1711220922.696608] debug    | SerialAgentLinux.cpp | recv_message             | [==>> SER <<==]        | client_key: 0x0102790A, len: 36, data:
0000: 81 80 06 00 01 07 1C 00 00 10 00 15 05 03 00 00 0E 00 00 00 00 12 01 00 03 00 01 3F 0A 00 00 00
0020: 00 00 00 13
[1711220922.697019] info     | ProxyClient.cpp    | create_datawriter        | datawriter created     | client_key: 0x0102790A, datawriter_id: 0x001(5), publisher_id: 0x001(3)
[1711220922.697103] debug    | SerialAgentLinux.cpp | send_message             | [** <<SER>> **]        | client_key: 0x0102790A, len: 14, data:
0000: 81 80 06 00 05 01 06 00 00 10 00 15 00 00
[1711220922.697123] debug    | SerialAgentLinux.cpp | send_message             | [** <<SER>> **]        | client_key: 0x0102790A, len: 13, data:
0000: 81 00 00 00 0A 01 05 00 07 00 00 00 80
[1711220922.733249] debug    | SerialAgentLinux.cpp | recv_message             | [==>> SER <<==]        | client_key: 0x0102790A, len: 13, data:
0000: 81 00 00 00 0A 01 05 00 07 00 00 00 80
[1711220922.733306] debug    | SerialAgentLinux.cpp | recv_message             | [==>> SER <<==]        | client_key: 0x0102790A, len: 16, data:
0000: 81 80 07 00 07 01 08 00 00 12 00 05 00 00 00 00
[1711220922.733414] debug    | DataWriter.cpp     | write                    | [** <<DDS>> **]        | client_key: 0x00000000, len: 4, data:
0000: 00 00 00 00
[1711220922.733499] debug    | SerialAgentLinux.cpp | send_message             | [** <<SER>> **]        | client_key: 0x0102790A, len: 13, data:
0000: 81 00 00 00 0A 01 05 00 08 00 00 00 80
[1711220923.147581] debug    | SerialAgentLinux.cpp | recv_message             | [==>> SER <<==]        | client_key: 0x0102790A, len: 24, data:
0000: 80 00 00 00 00 01 10 00 58 52 43 45 01 00 01 0F 06 DA 84 66 81 00 FC 01
[1711220923.149737] info     | Root.cpp           | delete_client            | delete                 | client_key: 0x0102790A
[1711220923.149774] info     | SessionManager.hpp | destroy_session          | session closed         | client_key: 0x0102790A, address: 0

and messages in topics are constantly = 0

Additional information

Examples from the microros_ws/firmware/freertos_apps/apps/ folder run without problems

pablogs9 commented 3 months ago

You are not using micro-ROS component for ESP-IDF:

ros2 run micro_ros_setup configure_firmware.sh test_app -t serial
ros2 run micro_ros_setup build_firmware.sh
ros2 run micro_ros_setup flash_firmware.sh
source install/local_setup.bash
ros2 run micro_ros_agent micro_ros_agent serial --dev /dev/ttyACM0 -v 6
SynKolbasyn commented 3 months ago

Oh my God, what a fool I am. I apologize for this misunderstanding.