Firmware upload failing with Teensy 4.0, ROS Humble

Hi, calibration works fine but the firmware upload stopped working (had it previously running fine around November 2023 on the same robot). I saw the suggestions on #35 and also experimented with changing the lib_deps in linorobot2_hardware/firmware/platformio.ini to match the different branches (rolling, foxy and humble) but it still fails.

Also, I just tried a fresh install, linorobot2_hardware is outside my workspace folder. Any help is appreciated, thanks.

Here's my lib_deps:

lib_deps = https://github.com/micro-ROS/micro_ros_platformio#22cf9b631b0bbb8861c30469e33b24e9241819ef
    https://github.com/micro-ROS/micro_ros_utilities#2.0.0
    https://github.com/linorobot/rosidl_typesupport_introspection_c

And the output after running pio run --target upload -e teensy40:

rsp@novamob:~/linorobot2_hardware/firmware$ pio run --target upload -e teensy40
Processing teensy40 (board: teensy40; platform: teensy; framework: arduino)
----------------------------------------------------------------------------------------------------------------
Verbose mode can be enabled via `-v, --verbose` option
CONFIGURATION: https://docs.platformio.org/page/boards/teensy/teensy40.html
PLATFORM: Teensy (5.0.0) > Teensy 4.0
HARDWARE: IMXRT1062 600MHz, 512KB RAM, 1.94MB Flash
DEBUG: Current (jlink) External (jlink)
PACKAGES: 
 - framework-arduinoteensy @ 1.159.0 (1.59) 
 - tool-teensy @ 1.159.0 (1.59) 
 - toolchain-gccarmnoneeabi-teensy @ 1.110301.0 (11.3.1)
Converting firmware.ino
LDF: Library Dependency Finder -> https://bit.ly/configure-pio-ldf
LDF Modes: Finder ~ chain, Compatibility ~ soft
Installing importlib-resources with pip at PlatformIO environment
/home/rsp/.platformio/penv/bin/python -m pip install importlib-resources
Requirement already satisfied: importlib-resources in /home/rsp/.platformio/penv/lib/python3.10/site-packages (6.4.0)
Installing pyyaml with pip at PlatformIO environment
/home/rsp/.platformio/penv/bin/python -m pip install pyyaml
Requirement already satisfied: pyyaml in /home/rsp/.platformio/penv/lib/python3.10/site-packages (6.0.1)
Installing markupsafe==2.0.1 with pip at PlatformIO environment
/home/rsp/.platformio/penv/bin/python -m pip install markupsafe==2.0.1
Requirement already satisfied: markupsafe==2.0.1 in /home/rsp/.platformio/penv/lib/python3.10/site-packages (2.0.1)
Configuring teensy40 with transport serial
micro-ROS already built
Found 101 compatible libraries
Scanning dependencies...
Dependency Graph
|-- micro_ros_platformio @ 0.0.1+sha.22cf9b6
|-- micro_ros_utilities @ 0.0.0+20240406154232.sha.6781238
|-- rosidl_typesupport_introspection_c @ 0.0.0+20240406154234.sha.d97c54c
|-- encoder
|-- imu
|-- kinematics
|-- motor
|-- odometry
|-- pid
Building in release mode
Compiling .pio/build/teensy40/src/firmware.ino.cpp.o
/home/rsp/linorobot2_hardware/firmware/src/firmware.ino: In function 'bool destroyEntities()':
/home/rsp/linorobot2_hardware/firmware/src/firmware.ino:222:23: warning: ignoring return value of 'rcl_ret_t rcl_publisher_fini(rcl_publisher_t*, rcl_node_t*)' declared with attribute 'warn_unused_result' [-Wunused-result]
  222 |     rcl_publisher_fini(&odom_publisher, &node);
      |     ~~~~~~~~~~~~~~~~~~^~~~~~~~~~~~~~~~~~~~~~~~
/home/rsp/linorobot2_hardware/firmware/src/firmware.ino:223:23: warning: ignoring return value of 'rcl_ret_t rcl_publisher_fini(rcl_publisher_t*, rcl_node_t*)' declared with attribute 'warn_unused_result' [-Wunused-result]
  223 |     rcl_publisher_fini(&imu_publisher, &node);
      |     ~~~~~~~~~~~~~~~~~~^~~~~~~~~~~~~~~~~~~~~~~
/home/rsp/linorobot2_hardware/firmware/src/firmware.ino:224:26: warning: ignoring return value of 'rcl_ret_t rcl_subscription_fini(rcl_subscription_t*, rcl_node_t*)' declared with attribute 'warn_unused_result' [-Wunused-result]
  224 |     rcl_subscription_fini(&twist_subscriber, &node);
      |     ~~~~~~~~~~~~~~~~~~~~~^~~~~~~~~~~~~~~~~~~~~~~~~~
/home/rsp/linorobot2_hardware/firmware/src/firmware.ino:225:18: warning: ignoring return value of 'rcl_ret_t rcl_node_fini(rcl_node_t*)' declared with attribute 'warn_unused_result' [-Wunused-result]
  225 |     rcl_node_fini(&node);
      |     ~~~~~~~~~~~~~^~~~~~~
/home/rsp/linorobot2_hardware/firmware/src/firmware.ino:226:19: warning: ignoring return value of 'rcl_ret_t rcl_timer_fini(rcl_timer_t*)' declared with attribute 'warn_unused_result' [-Wunused-result]
  226 |     rcl_timer_fini(&control_timer);
      |     ~~~~~~~~~~~~~~^~~~~~~~~~~~~~~~
Linking .pio/build/teensy40/firmware.elf
Checking size .pio/build/teensy40/firmware.elf
Advanced Memory Usage is available via "PlatformIO Home > Project Inspect"
Building .pio/build/teensy40/firmware.hex
Configuring upload protocol...
AVAILABLE: jlink, teensy-cli, teensy-gui
CURRENT: upload_protocol = teensy-cli
Rebooting...
Uploading .pio/build/teensy40/firmware.hex
Teensy Loader, Command Line, Version 2.2
Read ".pio/build/teensy40/firmware.hex": 159744 bytes, 7.9% usage
Found HalfKay Bootloader
Programming..............................................................................error writing to Teensy

*** [upload] Error 1
========================================= [FAILED] Took 40.77 seconds =========================================

Environment    Status    Duration
-------------  --------  ------------
teensy40       FAILED    00:00:40.767
==================================== 1 failed, 0 succeeded in 00:00:40.767 ====================================

linorobot / linorobot2_hardware

Firmware upload failing with Teensy 4.0, ROS Humble #85