uros spin function does not work when using static library

masoudir commented 3 years ago

Issue Configurations

Hardware description: Teensy 4.0 & Teensy 4.1 (Platformio IDE) & STM32G474 (with CubeIDE)
RTOS: NO
Installation type: using static library
Version or commit hash: foxy

Steps to reproduce the issue

Just follow this link and then add the library that is createwd by uros to your project. For cmake, I have used the flags exactly the same with this link.

Expected behavior

I have to see that the uros timer must publish every 1 second.

Actual behavior

We can not see any ROS2 messages in ROS2 network. But if we remove the spin function and then run rcl_publish(&publisher, &msg, NULL) function manually, we can see the ROS2 message in ROS2 network.

Also I have to note that this project was successfully implemented on STM32G474 in platformio using Arduino framework, but when we are using Platformio IDE for Teensy, or STM32Cube for STM32 microcontrollers, we can see this problem!!!!!

So we can understand that platformio does work generally, but why it is not working for other microcontrollers such as Teensy? Also I have tested for Teensy4.0 but nothing has changed.

Also I have tested with other cmake files (used for building static library of uros). And I have added these two functions (mkdir & usleep) to "default_transport.cpp" but nothing has changed:

`
  int clock_gettime(clockid_t unused, struct timespec *tp) __attribute__ ((weak));
  int usleep(unsigned int ms) __attribute__ ((weak));
  int mkdir(const char * path, int value) __attribute__ ((weak));

 int mkdir(const char * path, int value)
  {
    return 0;
  }

  int usleep(unsigned int ms)
  {
    delayMicroseconds((uint32_t)ms);
    return ms;
  }

  int clock_gettime(clockid_t unused, struct timespec *tp)
  {
    (void)unused;
    static uint32_t rollover = 0;
    static uint64_t last_measure = 0;

    uint64_t m = micros();
    tp->tv_sec = m / 1000000;
    tp->tv_nsec = (m % 1000000) * 1000;

    // Rollover handling
    rollover += (m < last_measure) ? 1 : 0;
    uint64_t rollover_extra_us = rollover * micro_rollover_useconds;
    tp->tv_sec += rollover_extra_us / 1000000;
    tp->tv_nsec += (rollover_extra_us % 1000000) * 1000;
    last_measure = m;

    return 0;
  }
`

Uros Agent output

    root@fa05c9c1d36e:/microros_ws# ros2 run micro_ros_agent micro_ros_agent serial --dev /dev/ttyACM0
    [1628654979.489550] info     | TermiosAgentLinux.cpp | init                     | running...             | fd: 3
    [1628654981.353120] info     | SessionManager.hpp | establish_session        | session re-established | client_key: 0x7D248957, address: 0
    [1628654982.375272] info     | ProxyClient.cpp    | create_participant       | participant created    | client_key: 0x7D248957, participant_id: 0x000(1)
    [1628654982.375782] info     | ProxyClient.cpp    | create_topic             | topic created          | client_key: 0x7D248957, topic_id: 0x000(2), participant_id: 0x000(1)
    [1628654982.376126] info     | ProxyClient.cpp    | create_publisher         | publisher created      | client_key: 0x7D248957, publisher_id: 0x000(3), participant_id: 0x000(1)
    [1628654982.376709] info     | ProxyClient.cpp    | create_datawriter        | datawriter created     | client_key: 0x7D248957, datawriter_id: 0x000(5), publisher_id: 0x000(3)

Code

We are simply using the code in this link.

pablogs9 commented 3 years ago

Can you confirm whether the timer callback is being executed?
Can you provide the output of the agent with the flag -v6?
Platform.io is not an official supported framework, have you tried with Arduino IDE or Arduino CLI?

masoudir commented 3 years ago

Timer callback is not executed during run-time. Because the number of counter is always 0 and does not increase over time.

Also the program hangs when it reaches to "RCSOFTCHECK(rclc_executor_spin_some(&executor, RCL_MS_TO_NS(100)));" function. (spin function).

Also I put "rcl_publish(&publisher, &msg, NULL);" before the spin function to realize that publishing works or not. And I saw that it works, but when it reaches the spin function, it hangs, so I can saw the data=0 in ROS2 only once.

I tried with Arduino IDE but using the precompiled static library. I do not know how to use my new static library instead of the old.

I want to run my project with Platformio not Arduino due to the convenience.

uros agent output with -v6 flag:

  root@c6dc8152fe1d:/microros_ws# ros2 run micro_ros_agent micro_ros_agent serial --dev /dev/ttyACM0 -v6
  [1628665579.684294] info     | TermiosAgentLinux.cpp | init                     | running...             | fd: 3
  [1628665579.684627] info     | Root.cpp           | set_verbose_level        | logger setup           | verbose_level: 6
  [1628665579.684704] info     | Root.cpp           | create_client            | create                 | client_key: 0x7D248957, session_id: 0x81
  [1628665579.684761] info     | SessionManager.hpp | establish_session        | session established    | client_key: 0x7D248957, address: 0
  [1628665579.684851] debug    | SerialAgentLinux.cpp | send_message             | [** <<SER>> **]        | client_key: 0x7D248957, len: 19, data: 
  0000: 81 00 00 00 04 01 0B 00 00 00 58 52 43 45 01 00 01 0F 00
  [1628665579.685049] debug    | SerialAgentLinux.cpp | recv_message             | [==>> SER <<==]        | client_key: 0x7D248957, len: 24, data: 
  0000: 80 00 00 00 00 01 10 00 58 52 43 45 01 00 01 0F 7D 24 89 57 81 00 FC 01
  [1628665579.685085] info     | SessionManager.hpp | establish_session        | session re-established | client_key: 0x7D248957, address: 0
  [1628665579.685145] debug    | SerialAgentLinux.cpp | send_message             | [** <<SER>> **]        | client_key: 0x7D248957, len: 19, data: 
  0000: 81 00 00 00 04 01 0B 00 00 00 58 52 43 45 01 00 01 0F 00
  [1628665580.684325] debug    | SerialAgentLinux.cpp | recv_message             | [==>> SER <<==]        | client_key: 0x7D248957, len: 56, data: 
  0000: 81 80 00 00 01 07 2E 00 00 0A 00 01 01 03 00 00 1F 00 00 00 00 01 00 20 17 00 00 00 6D 69 63 72
  0020: 6F 5F 72 6F 73 5F 61 72 64 75 69 6E 6F 5F 6E 6F 64 65 00 00 00 00 00 00
  [1628665580.695630] info     | ProxyClient.cpp    | create_participant       | participant created    | client_key: 0x7D248957, participant_id: 0x000(1)
  [1628665580.695813] debug    | SerialAgentLinux.cpp | send_message             | [** <<SER>> **]        | client_key: 0x7D248957, len: 14, data: 
  0000: 81 80 00 00 05 01 06 00 00 0A 00 01 00 00
  [1628665580.695889] debug    | SerialAgentLinux.cpp | send_message             | [** <<SER>> **]        | client_key: 0x7D248957, len: 13, data: 
  0000: 81 00 00 00 0A 01 05 00 01 00 00 00 80
  [1628665580.696117] debug    | SerialAgentLinux.cpp | recv_message             | [==>> SER <<==]        | client_key: 0x7D248957, len: 13, data: 
  0000: 81 00 00 00 0B 01 05 00 00 00 00 00 80
  [1628665580.696203] debug    | SerialAgentLinux.cpp | send_message             | [** <<SER>> **]        | client_key: 0x7D248957, len: 13, data: 
  0000: 81 00 00 00 0A 01 05 00 01 00 00 00 80
  [1628665580.696357] debug    | SerialAgentLinux.cpp | recv_message             | [==>> SER <<==]        | client_key: 0x7D248957, len: 13, data: 
  0000: 81 00 00 00 0A 01 05 00 01 00 00 00 80
  [1628665580.696407] debug    | SerialAgentLinux.cpp | recv_message             | [==>> SER <<==]        | client_key: 0x7D248957, len: 100, data: 
  0000: 81 80 01 00 01 07 5A 00 00 0B 00 02 02 03 00 00 4C 00 00 00 24 00 00 00 72 74 2F 6D 69 63 72 6F
  0020: 5F 72 6F 73 5F 61 72 64 75 69 6E 6F 5F 6E 6F 64 65 5F 70 75 62 6C 69 73 68 65 72 00 00 01 00 00
  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
  [1628665580.696607] info     | ProxyClient.cpp    | create_topic             | topic created          | client_key: 0x7D248957, topic_id: 0x000(2), participant_id: 0x000(1)
  [1628665580.696655] debug    | SerialAgentLinux.cpp | send_message             | [** <<SER>> **]        | client_key: 0x7D248957, len: 14, data: 
  0000: 81 80 01 00 05 01 06 00 00 0B 00 02 00 00
  [1628665580.696676] debug    | SerialAgentLinux.cpp | send_message             | [** <<SER>> **]        | client_key: 0x7D248957, len: 13, data: 
  0000: 81 00 00 00 0A 01 05 00 02 00 00 00 80
  [1628665580.696982] debug    | SerialAgentLinux.cpp | recv_message             | [==>> SER <<==]        | client_key: 0x7D248957, len: 13, data: 
  0000: 81 00 00 00 0A 01 05 00 02 00 00 00 80
  [1628665580.697090] debug    | SerialAgentLinux.cpp | recv_message             | [==>> SER <<==]        | client_key: 0x7D248957, 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
  [1628665580.697117] debug    | SerialAgentLinux.cpp | recv_message             | [==>> SER <<==]        | client_key: 0x7D248957, len: 13, data: 
  0000: 81 00 00 00 0B 01 05 00 01 00 02 00 80
  [1628665580.697289] info     | ProxyClient.cpp    | create_publisher         | publisher created      | client_key: 0x7D248957, publisher_id: 0x000(3), participant_id: 0x000(1)
  [1628665580.697315] debug    | SerialAgentLinux.cpp | send_message             | [** <<SER>> **]        | client_key: 0x7D248957, len: 14, data: 
  0000: 81 80 02 00 05 01 06 00 00 0C 00 03 00 00
  [1628665580.697343] debug    | SerialAgentLinux.cpp | send_message             | [** <<SER>> **]        | client_key: 0x7D248957, len: 13, data: 
  0000: 81 00 00 00 0A 01 05 00 03 00 00 00 80
  [1628665580.697426] debug    | SerialAgentLinux.cpp | send_message             | [** <<SER>> **]        | client_key: 0x7D248957, len: 13, data: 
  0000: 81 00 00 00 0A 01 05 00 03 00 00 00 80
  [1628665580.697569] debug    | SerialAgentLinux.cpp | recv_message             | [==>> SER <<==]        | client_key: 0x7D248957, len: 13, data: 
  0000: 81 00 00 00 0A 01 05 00 03 00 00 00 80
  [1628665580.697603] debug    | SerialAgentLinux.cpp | recv_message             | [==>> SER <<==]        | client_key: 0x7D248957, len: 36, data: 
  0000: 81 80 03 00 01 07 19 00 00 0D 00 05 05 03 00 00 0B 00 00 00 00 02 01 20 03 00 00 00 00 00 00 00
  0020: 03 00 00 00
  [1628665580.697825] info     | ProxyClient.cpp    | create_datawriter        | datawriter created     | client_key: 0x7D248957, datawriter_id: 0x000(5), publisher_id: 0x000(3)
  [1628665580.697852] debug    | SerialAgentLinux.cpp | send_message             | [** <<SER>> **]        | client_key: 0x7D248957, len: 14, data: 
  0000: 81 80 03 00 05 01 06 00 00 0D 00 05 00 00
  [1628665580.697861] debug    | SerialAgentLinux.cpp | send_message             | [** <<SER>> **]        | client_key: 0x7D248957, len: 13, data: 
  0000: 81 00 00 00 0A 01 05 00 04 00 00 00 80
  [1628665580.698047] debug    | SerialAgentLinux.cpp | recv_message             | [==>> SER <<==]        | client_key: 0x7D248957, len: 13, data: 
  0000: 81 00 00 00 0B 01 05 00 02 00 03 00 80
  [1628665580.698077] debug    | SerialAgentLinux.cpp | send_message             | [** <<SER>> **]        | client_key: 0x7D248957, len: 13, data: 
  0000: 81 00 00 00 0A 01 05 00 04 00 00 00 80
  [1628665580.699123] debug    | SerialAgentLinux.cpp | recv_message             | [==>> SER <<==]        | client_key: 0x7D248957, len: 13, data: 
  0000: 81 00 00 00 0A 01 05 00 04 00 00 00 80
  [1628665580.798191] debug    | SerialAgentLinux.cpp | recv_message             | [==>> SER <<==]        | client_key: 0x7D248957, len: 16, data: 
  0000: 81 80 04 00 07 01 08 00 00 0E 00 05 00 00 00 00
  [1628665580.798235] debug    | SerialAgentLinux.cpp | recv_message             | [==>> SER <<==]        | client_key: 0x7D248957, len: 13, data: 
  0000: 81 00 00 00 0B 01 05 00 03 00 04 00 80
  [1628665580.798318] debug    | DataWriter.cpp     | write                    | [** <<DDS>> **]        | client_key: 0x00000000, len: 4, data: 
  0000: 00 00 00 00
  [1628665580.798405] debug    | SerialAgentLinux.cpp | send_message             | [** <<SER>> **]        | client_key: 0x7D248957, len: 13, data: 
  0000: 81 00 00 00 0A 01 05 00 05 00 00 00 80
  [1628665580.798423] debug    | SerialAgentLinux.cpp | send_message             | [** <<SER>> **]        | client_key: 0x7D248957, len: 13, data: 
  0000: 81 00 00 00 0A 01 05 00 05 00 00 00 80

pablogs9 commented 3 years ago

Provide a replicable environment and instructions, so I can check what is happening.

masoudir commented 3 years ago

I have created a repository regarding this project. You can find this from this link

I have described all the details and I hope there is no problem to run this project. Also as I mentioned before, I have done this project for STM32G474 using arduino & Platformio and everything was well. But in this case we have problems for Teensy as same as STM32Cube framework.

Looking for hearing from you...

Thank you very much

pablogs9 commented 3 years ago

Where are the transports in this repo?

masoudir commented 3 years ago

default_transport is in this

micro_ros_arduino.h is in this

I have mentioned in readme that after ceaning the uros lib these files must be downloaded into the lib/microros folder.

But I have not used these functions in this repo:

    int usleep(unsigned int ms) __attribute__ ((weak));
    int mkdir(const char * path, int value) __attribute__ ((weak));

   int mkdir(const char * path, int value)
    {
      return 0;
    }

    int usleep(unsigned int ms)
    {
      delayMicroseconds((uint32_t)ms);
      return ms;
    }

These functions are needed when a different cmake is introduced to uros for build the static library.

masoudir commented 3 years ago

Hi @pablogs9 , Have you found any solutions? Our project is strictly bonded to this point because we want to create custom messages in uros and we have to use a compiler to build the library and attach it to our IDE.

Have you found why spin function does not work properly?

Thank you very much for your help...

pablogs9 commented 3 years ago

have you checked where is your execution being stopped inside the executor functions?

masoudir commented 3 years ago

I do not know where it is stopped. But I know that that must be inside "rclc_executor_spin_some" function, because it hangs the system and prevent the code to be executed inside loop() function in arduino. Also there is no way to debug the code with Teensy while using USB cable of the board. So I do not know what is the problem.

I guess the timer implemented inside uros does not work properly and it avoids the counter timer to increase its value.

Maybe there is differences between the compiler flags for Platformio & ArduinoIDE.

Is it possible to use custom static libraries inside arduino IDE?

masoudir commented 3 years ago

Also I have tested a uros project with platformio using the prebuilt of uros in arduino (in this link). And I see that this runs very well.

But I have created my own uros static library using this as cmake and this as colcon.meta file. But again I see that spin function hangs.

So the problem is that when I was using your prebuilt static library works very well inside platformio but when I create my own static library, that does not work.

Do you use the same comand to build uros static library?

I am using these commands:

  ros2 run micro_ros_setup create_firmware_ws.sh generate_lib
  export TOOLCHAIN_PREFIX=/usr/bin/arm-none-eabi-
  ros2 run micro_ros_setup build_firmware.sh teensy4.cmake colcon.meta

So why I see these differences between your build and my build?

masoudir commented 3 years ago

Finally I found the solution for this problem.

We have to use the old version of gcc-arm-none-eabi- as below to build the uros static library: gcc-arm-none-eabi-5_4-2016q3

You can download this from this.

The new version of gcc-arm-none-eabi which you can download that from this command DOES NOT work for building static library for uros.

  sudo apt install gcc-arm-none-eabi

For finding the best compiler for each board I got help from this.

Thank you very much

pablogs9 commented 3 years ago

Hello @masoudir, thanks for sharing the solution. In the Arduino platform, each board has an associated and fixed compiler so, the micro-ROS library for each board is built using the correct compiler.

You are right, if you are using a different major version of GCC for building for your target, you will need to rebuild the micro-ROS precompiled library using the proper version.

I'm closing, do not hesitate to reopen if you have any other problems.

micro-ROS / micro_ros_arduino