search

micro-ROS / micro_ros_arduino

micro-ROS library for Arduino
Apache License 2.0
435 stars 112 forks source link

Custom message crashes Portenta H7 #903

Closed tnagyzambo closed 2 years ago

tnagyzambo commented 2 years ago

Issue template

Start with the publisher example.

On platformio IDE with the fixed linker script add a custom package to .pio/libdeps/portneta_h7_m7/micro_ros_arduino/extras/library_generation/extra_packages/

custom_msgs
   msg
     Float64.msg
   CmakeLists.txt
   package.xml

Float64.msg: float64 data

CmakeLists.txt:

cmake_minimum_required(VERSION 3.16)
project(custom_msgs)

# Set compiler options
set(CMAKE_CXX_STANDARD 17)
set(CMAKE_CXX_FLAGS "-Wall -Werror -Wextra -Wpedantic -Wvla -Wextra -Wnull-dereference -Wswitch-enum")

set(CMAKE_EXPORT_COMPILE_COMMANDS ON)

find_package(ament_cmake REQUIRED)
find_package(rosidl_default_generators REQUIRED)

set(msg_files
    "msg/Float64.msg")

# Create ros2 interfaces
rosidl_generate_interfaces(${PROJECT_NAME}
                           ${msg_files}
                           ADD_LINTER_TESTS)

if(BUILD_TESTING)
        find_package(ament_lint_auto REQUIRED)
        ament_lint_auto_find_test_dependencies()
endif()

ament_export_dependencies(rosidl_default_runtime)

ament_package()

package.xml:

<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
  <name>custom_msgs</name>
  <version>3.0.0</version>
  <description>TODO: Package description</description>
  <maintainer email="root@todo.todo">root</maintainer>
  <license>TODO: License declaration</license>

  <buildtool_depend>ament_cmake</buildtool_depend>

  <test_depend>ament_lint_auto</test_depend>
  <test_depend>ament_lint_common</test_depend>

  <build_depend>rosidl_default_generators</build_depend>
  <exec_depend>rosidl_default_runtime</exec_depend>

  <member_of_group>rosidl_interface_packages</member_of_group>

  <export>
    <build_type>ament_cmake</build_type>
  </export>
</package>

Build lib with cd .pio/libdeps/portenta_h7_m7/micro_ros_arduino && docker run -it --rm -v $(pwd):/project --env MICROROS_LIBRARY_FOLDER=extras microros/micro_ros_static_library_builder:galactic -p portenta-m7

Build and upload a modified example

#include <micro_ros_arduino.h>

#include <stdio.h>
#include <rcl/rcl.h>
#include <rcl/error_handling.h>
#include <rclc/rclc.h>
#include <rclc/executor.h>
#include <custom_msgs/msg/float64.h>

#if !defined(TARGET_STM32F4) && !defined(ARDUINO_TEENSY41) && !defined(TARGET_PORTENTA_H7_M7)
#error This example is only available for Arduino Portenta, Arduino Teensy41 and STM32F4
#endif

rcl_publisher_t publisher;
custom_msgs__msg__Float64 msg;
rclc_support_t support;
rcl_allocator_t allocator;
rcl_node_t node;

#define LED_PIN 13

#define RCCHECK(fn) { rcl_ret_t temp_rc = fn; if((temp_rc != RCL_RET_OK)){error_loop();}}
#define RCSOFTCHECK(fn) { rcl_ret_t temp_rc = fn; if((temp_rc != RCL_RET_OK)){}}

void error_loop(){
  while(1){
    digitalWrite(LED_PIN, !digitalRead(LED_PIN));
    delay(100);
  }
}

void timer_callback(rcl_timer_t * timer, int64_t last_call_time)
{
  RCLC_UNUSED(last_call_time);
  if (timer != NULL) {
    RCSOFTCHECK(rcl_publish(&publisher, &msg, NULL));
    msg.data = msg.data + 1.0;
  }
}

void setup() {
  byte arduino_mac[] = { 0xAA, 0xBB, 0xCC, 0xEE, 0xDD, 0xFF };
  IPAddress arduino_ip(192, 168, 1, 177);
  IPAddress agent_ip(192, 168, 1, 113);
  set_microros_native_ethernet_udp_transports(arduino_mac, arduino_ip, agent_ip, 9999);

  pinMode(LED_PIN, OUTPUT);
  digitalWrite(LED_PIN, HIGH);

  delay(2000);

  allocator = rcl_get_default_allocator();

  //create init_options
  RCCHECK(rclc_support_init(&support, 0, NULL, &allocator));

  // create node
  RCCHECK(rclc_node_init_default(&node, "micro_ros_arduino_ethernet_node", "namespace", &support));

  // create publisher
  RCCHECK(rclc_publisher_init_best_effort(
    &publisher,
    &node,
    ROSIDL_GET_MSG_TYPE_SUPPORT(custom_msgs, msg, Float64),
    "topic_name"));

  msg.data = 0.0;
}

void loop() {
    RCSOFTCHECK(rcl_publish(&publisher, &msg, NULL));
    msg.data = msg.data + 1.0;
    delay(100);
}

Expected behavior

Portenta works

Actual behavior

Portenta crashes

Additional information

Inspecting the generated files, they look identical to those generated for std_msgs

ROSIDL_GET_MSG_TYPE_SUPPORT(custom_msgs, msg, Float64) seems to be the culprit here

pablogs9 commented 2 years ago

Can you share the output of the micro-ROS agent using -v6 flag? How are you launching the micro-ROS agent? Is IPAddress agent_ip(192, 168, 1, 113) the IP of your agent?

tnagyzambo commented 2 years ago

The micro ros agent does not recognize that a client has connected, the portenta goes straight into the error loop.

Using the std_msgs the code works just fine and the agent responds correctly.

pablogs9 commented 2 years ago

Can you share the output of the micro-ROS agent using -v6 flag? How are you launching the micro-ROS agent? Is IPAddress agent_ip(192, 168, 1, 113) the IP of your agent?

tnagyzambo commented 2 years ago

micro-ROS-agent:

ros@33e4457a7ca1:/workspaces/rCTRL/ros2$ source install/setup.bash && ros2 run micro_ros_agent micro_ros_agent udp4 --port 8090 -v6
[1648730970.706597] info     | UDPv4AgentLinux.cpp | init                     | running...             | port: 8090
[1648730970.706959] info     | Root.cpp           | set_verbose_level        | logger setup           | verbose_level: 6

The example I posted above was slightly incorrect. This code reflects my actual networking setup

  byte arduino_mac[] = { 0xAA, 0xBB, 0xCC, 0xEE, 0xDD, 0xFF };
  IPAddress arduino_ip(192, 168, 2, 2);
  IPAddress agent_ip(192, 168, 2, 1);
  set_microros_native_ethernet_udp_transports(arduino_mac, arduino_ip, agent_ip, 8090);
pablogs9 commented 2 years ago

It seems that your code is failing in RCCHECK(rclc_support_init(&support, 0, NULL, &allocator));. This is not related to the created type because it should create a session and node before failing when creating the publisher.

Can you share the complete code (the one that you are building and flashing) with std_msgs and the modified one?

tnagyzambo commented 2 years ago

This works:

#include <micro_ros_arduino.h>

#include <stdio.h>
#include <rcl/rcl.h>
#include <rcl/error_handling.h>
#include <rclc/rclc.h>
#include <rclc/executor.h>
#include <std_msgs/msg/float64.h>

#if !defined(TARGET_STM32F4) && !defined(ARDUINO_TEENSY41) && !defined(TARGET_PORTENTA_H7_M7)
#error This example is only available for Arduino Portenta, Arduino Teensy41 and STM32F4
#endif

rcl_publisher_t publisher;
std_msgs__msg__Float64 msg;
rclc_support_t support;
rcl_allocator_t allocator;
rcl_node_t node;

#define LED_PIN 13

#define RCCHECK(fn) { rcl_ret_t temp_rc = fn; if((temp_rc != RCL_RET_OK)){error_loop();}}
#define RCSOFTCHECK(fn) { rcl_ret_t temp_rc = fn; if((temp_rc != RCL_RET_OK)){}}

void error_loop(){
  while(1){
    digitalWrite(LED_PIN, !digitalRead(LED_PIN));
    delay(100);
  }
}

void timer_callback(rcl_timer_t * timer, int64_t last_call_time)
{
  RCLC_UNUSED(last_call_time);
  if (timer != NULL) {
    RCSOFTCHECK(rcl_publish(&publisher, &msg, NULL));
    msg.data = msg.data + 1.0;
  }
}

void setup() {
  byte arduino_mac[] = { 0xAA, 0xBB, 0xCC, 0xEE, 0xDD, 0xFF };
  IPAddress arduino_ip(192, 168, 2, 2);
  IPAddress agent_ip(192, 168, 2, 1);
  set_microros_native_ethernet_udp_transports(arduino_mac, arduino_ip, agent_ip, 8090);

  pinMode(LED_PIN, OUTPUT);
  digitalWrite(LED_PIN, HIGH);

  delay(2000);

  allocator = rcl_get_default_allocator();

  //create init_options
  RCCHECK(rclc_support_init(&support, 0, NULL, &allocator));

  // create node
  RCCHECK(rclc_node_init_default(&node, "micro_ros_arduino_ethernet_node", "namespace", &support));

  // create publisher
  RCCHECK(rclc_publisher_init_best_effort(
    &publisher,
    &node,
    ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Float64),
    "topic_name"));

  msg.data = 0.0;
}

void loop() {
    RCSOFTCHECK(rcl_publish(&publisher, &msg, NULL));
    msg.data = msg.data + 1.0;
    delay(100);
}

This does not:

#include <micro_ros_arduino.h>

#include <stdio.h>
#include <rcl/rcl.h>
#include <rcl/error_handling.h>
#include <rclc/rclc.h>
#include <rclc/executor.h>
#include <custom_msgs/msg/float64.h>

#if !defined(TARGET_STM32F4) && !defined(ARDUINO_TEENSY41) && !defined(TARGET_PORTENTA_H7_M7)
#error This example is only available for Arduino Portenta, Arduino Teensy41 and STM32F4
#endif

rcl_publisher_t publisher;
custom_msgs__msg__Float64 msg;
rclc_support_t support;
rcl_allocator_t allocator;
rcl_node_t node;

#define LED_PIN 13

#define RCCHECK(fn) { rcl_ret_t temp_rc = fn; if((temp_rc != RCL_RET_OK)){error_loop();}}
#define RCSOFTCHECK(fn) { rcl_ret_t temp_rc = fn; if((temp_rc != RCL_RET_OK)){}}

void error_loop(){
  while(1){
    digitalWrite(LED_PIN, !digitalRead(LED_PIN));
    delay(100);
  }
}

void timer_callback(rcl_timer_t * timer, int64_t last_call_time)
{
  RCLC_UNUSED(last_call_time);
  if (timer != NULL) {
    RCSOFTCHECK(rcl_publish(&publisher, &msg, NULL));
    msg.data = msg.data + 1.0;
  }
}

void setup() {
  byte arduino_mac[] = { 0xAA, 0xBB, 0xCC, 0xEE, 0xDD, 0xFF };
  IPAddress arduino_ip(192, 168, 2, 2);
  IPAddress agent_ip(192, 168, 2, 1);
  set_microros_native_ethernet_udp_transports(arduino_mac, arduino_ip, agent_ip, 8090);

  pinMode(LED_PIN, OUTPUT);
  digitalWrite(LED_PIN, HIGH);

  delay(2000);

  allocator = rcl_get_default_allocator();

  //create init_options
  RCCHECK(rclc_support_init(&support, 0, NULL, &allocator));

  // create node
  RCCHECK(rclc_node_init_default(&node, "micro_ros_arduino_ethernet_node", "namespace", &support));

  // create publisher
  RCCHECK(rclc_publisher_init_best_effort(
    &publisher,
    &node,
    ROSIDL_GET_MSG_TYPE_SUPPORT(custom_msgs, msg, Float64),
    "topic_name"));

  msg.data = 0.0;
}

void loop() {
    RCSOFTCHECK(rcl_publish(&publisher, &msg, NULL));
    msg.data = msg.data + 1.0;
    delay(100);
}
pablogs9 commented 2 years ago

Can you share the Platform.IO project and build log? It can be related to different versions of the compiler between your PlatformIO and the version that we are using in the library building.

tnagyzambo commented 2 years ago

If this is not what you meant please let me know:

Processing portenta_h7_m7 (platform: ststm32; board: portenta_h7_m7; framework: arduino)
--------------------------------------------------------------------------------
Verbose mode can be enabled via `-v, --verbose` option
CONFIGURATION: https://docs.platformio.org/page/boards/ststm32/portenta_h7_m7.html
PLATFORM: ST STM32 (15.2.0) > Arduino Portenta H7 (M7 core)
HARDWARE: STM32H747XIH6 480MHz, 511.35KB RAM, 768KB Flash
DEBUG: Current (cmsis-dap) External (cmsis-dap, jlink, stlink)
PACKAGES: 
 - framework-arduino-mbed 2.6.1 
 - tool-dfuutil 1.9.200310 
 - tool-openocd 2.1100.211028 (11.0) 
 - tool-stm32duino 1.0.1 
 - toolchain-gccarmnoneeabi 1.70201.0 (7.2.1)
LDF: Library Dependency Finder -> https://bit.ly/configure-pio-ldf
LDF Modes: Finder ~ chain, Compatibility ~ soft
Found 32 compatible libraries
Scanning dependencies...
Dependency Graph
|-- <micro_ros_arduino> 2.0.4-galactic+sha.8e17d5b
|   |-- <Ethernet> 1.0.0
|   |   |-- <SocketWrapper> 1.0
|   |-- <SPI>
|   |-- <WiFi> 1.0
|   |   |-- <SocketWrapper> 1.0
Building in release mode
Checking size .pio/build/portenta_h7_m7/firmware.elf
Advanced Memory Usage is available via "PlatformIO Home > Project Inspect"
RAM:   [==        ]  17.3% (used 90672 bytes from 523624 bytes)
Flash: [====      ]  38.2% (used 300072 bytes from 786432 bytes)
Configuring upload protocol...
AVAILABLE: cmsis-dap, dfu, jlink, mbed, stlink
CURRENT: upload_protocol = dfu
Looking for upload port...
Processing portenta_h7_m7 (platform: ststm32; board: portenta_h7_m7; framework: arduino)
--------------------------------------------------------------------------------
Verbose mode can be enabled via `-v, --verbose` option
CONFIGURATION: https://docs.platformio.org/page/boards/ststm32/portenta_h7_m7.html
PLATFORM: ST STM32 (15.2.0) > Arduino Portenta H7 (M7 core)
HARDWARE: STM32H747XIH6 480MHz, 511.35KB RAM, 768KB Flash
DEBUG: Current (cmsis-dap) External (cmsis-dap, jlink, stlink)
PACKAGES: 
 - framework-arduino-mbed 2.6.1 
 - tool-dfuutil 1.9.200310 
 - tool-openocd 2.1100.211028 (11.0) 
 - tool-stm32duino 1.0.1 
 - toolchain-gccarmnoneeabi 1.70201.0 (7.2.1)
LDF: Library Dependency Finder -> https://bit.ly/configure-pio-ldf
LDF Modes: Finder ~ chain, Compatibility ~ soft
Found 32 compatible libraries
Scanning dependencies...
Dependency Graph
|-- <micro_ros_arduino> 2.0.4-galactic+sha.8e17d5b
|   |-- <Ethernet> 1.0.0
|   |   |-- <SocketWrapper> 1.0
|   |-- <SPI>
|   |-- <WiFi> 1.0
|   |   |-- <SocketWrapper> 1.0
Building in release mode
Checking size .pio/build/portenta_h7_m7/firmware.elf
Advanced Memory Usage is available via "PlatformIO Home > Project Inspect"
RAM:   [==        ]  17.3% (used 90672 bytes from 523624 bytes)
Flash: [====      ]  38.2% (used 300072 bytes from 786432 bytes)
Configuring upload protocol...
AVAILABLE: cmsis-dap, dfu, jlink, mbed, stlink
CURRENT: upload_protocol = dfu
Looking for upload port...
Auto-detected: /dev/cu.usbmodem14202
Forcing reset using 1200bps open/close on port /dev/cu.usbmodem14202
Waiting for the new upload port...
Uploading .pio/build/portenta_h7_m7/firmware.bin
dfu-util 0.9

Copyright 2005-2009 Weston Schmidt, Harald Welte and OpenMoko Inc.
Copyright 2010-2020 Tormod Volden and Stefan Schmidt
This program is Free Software and has ABSOLUTELY NO WARRANTY
Please report bugs to http://sourceforge.net/p/dfu-util/tickets/

Opening DFU capable USB device...
ID 2341:035b
Run-time device DFU version 011a
Claiming USB DFU Interface...
Setting Alternate Setting #0 ...
Determining device status: state = dfuIDLE, status = 0
dfuIDLE, continuing
DFU mode device DFU version 011a
Device returned transfer size 4096
DfuSe interface name: "Internal Flash   "
Downloading to address = 0x08040000, size = 301124

Erase       [                         ]   0%            0 bytes
Erase       [                         ]   0%            0 bytes
Erase       [=                        ]   4%        12288 bytes
Erase       [==                       ]   8%        24576 bytes
Erase       [===                      ]  12%        36864 bytes
Erase       [====                     ]  16%        49152 bytes
Erase       [=====                    ]  20%        61440 bytes
Erase       [======                   ]  24%        73728 bytes
Erase       [=======                  ]  28%        86016 bytes
Erase       [========                 ]  32%        98304 bytes
Erase       [=========                ]  36%       110592 bytes
Erase       [==========               ]  40%       122880 bytes
Erase       [==========               ]  43%       131072 bytes
Erase       [===========              ]  44%       135168 bytes
Erase       [============             ]  48%       147456 bytes
Erase       [=============            ]  53%       159744 bytes
Erase       [==============           ]  57%       172032 bytes
Erase       [===============          ]  61%       184320 bytes
Erase       [================         ]  65%       196608 bytes
Erase       [=================        ]  68%       204800 bytes
Erase       [==================       ]  72%       217088 bytes
Erase       [===================      ]  76%       229376 bytes
Erase       [====================     ]  80%       241664 bytes
Erase       [=====================    ]  84%       253952 bytes
Erase       [======================   ]  88%       266240 bytes
Erase       [=======================  ]  92%       278528 bytes
Erase       [======================== ]  96%       290816 bytes
Download    [                         ]   0%            0 bytes
Download    [                         ]   1%         4096 bytes
Download    [=                        ]   4%        12288 bytes
Download    [==                       ]   8%        24576 bytes
Download    [===                      ]  12%        36864 bytes
Download    [====                     ]  16%        49152 bytes
Download    [=====                    ]  20%        61440 bytes
Download    [======                   ]  24%        73728 bytes
Download    [=======                  ]  28%        86016 bytes
Download    [========                 ]  32%        98304 bytes
Download    [=========                ]  36%       110592 bytes
Download    [==========               ]  40%       122880 bytes
Download    [===========              ]  44%       135168 bytes
Download    [============             ]  48%       147456 bytes
Download    [=============            ]  53%       159744 bytes
Download    [==============           ]  57%       172032 bytes
Download    [===============          ]  61%       184320 bytes
Download    [================         ]  65%       196608 bytes
Download    [=================        ]  68%       204800 bytes
Download    [==================       ]  72%       217088 bytes
Download    [===================      ]  76%       229376 bytes
Download    [====================     ]  80%       241664 bytes
Download    [=====================    ]  84%       253952 bytes
Download    [======================   ]  88%       266240 bytes
Download    [=======================  ]  92%       278528 bytes
Download    [======================== ]  96%       290816 bytes
Download    [=========================] 100%       301124 bytes
Download done.
File downloaded successfully
Transitioning to dfuMANIFEST state
========================= [SUCCESS] Took 13.97 seconds =========================

platform.ini:

# There are issues using platformio link against the static H7 uros library
# REFERENCE: https://github.com/micro-ROS/micro_ros_arduino/issues/774

[env:portenta_h7_m7]
platform = ststm32
board = portenta_h7_m7
framework = arduino
extra_scripts = fix_linker.py
monitor_speed = 9600
build_flags =
    #-std=c++17
    -L ./.pio/libdeps/portenta_h7_m7/micro_ros_arduino/src/cortex-m7/fpv5-d16-softfp/
    -D TARGET_PORTENTA_H7_M7

lib_deps =
    https://github.com/micro-ROS/micro_ros_arduino
pablogs9 commented 2 years ago

Ok, we have replicated the problem. Give us some time to find a solution.

tnagyzambo commented 2 years ago

Thanks for the support, in the mean time should I be hunting down the compiler differences as you suggested?

pablogs9 commented 2 years ago

Try using the following CMake in your custom message package:

cmake_minimum_required(VERSION 3.8)
project(custom_msgs)

if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
  add_compile_options(-Wall -Wextra -Wpedantic)
endif()

# find dependencies
find_package(ament_cmake REQUIRED)
# uncomment the following section in order to fill in
# further dependencies manually.
# find_package(<dependency> REQUIRED)

if(BUILD_TESTING)
  find_package(ament_lint_auto REQUIRED)
  # the following line skips the linter which checks for copyrights
  # uncomment the line when a copyright and license is not present in all source files
  #set(ament_cmake_copyright_FOUND TRUE)
  # the following line skips cpplint (only works in a git repo)
  # uncomment the line when this package is not in a git repo
  #set(ament_cmake_cpplint_FOUND TRUE)
  ament_lint_auto_find_test_dependencies()
endif()

find_package(rosidl_default_generators REQUIRED)

rosidl_generate_interfaces(${PROJECT_NAME}
  "msg/Float64.msg"
 )

ament_package()
tnagyzambo commented 2 years ago

Changing the CMake and cleaning out the build directory solves the issue. Thanks for the help.

Its not clear to me what actually was the problem though.

pablogs9 commented 2 years ago

It seems something related to CMAKE_CXX_STANDARD, C++ flags and/or linking stage. We are not sure. Please reopen if you have further questions.