Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Adding a parameter server doesn't work ? #160

Open
l4es opened this issue Nov 10, 2024 · 8 comments
Open

Adding a parameter server doesn't work ? #160

l4es opened this issue Nov 10, 2024 · 8 comments

Comments

@l4es
Copy link

l4es commented Nov 10, 2024

Issue template

  • Hardware description: raspberry pi pico rp2040
  • RTOS: None
  • Installation type: platformio
  • Version or commit hash: humble

Steps to reproduce the issue

N/A

Expected behavior

  • Working with 2 publishers,1 subscription and 1 timer as previously,
  • Working with the additional parameter server of 2 parameters.

Actual behavior

Adding the parameter server causes multiple object deleted , repeated session closed / session established as soon as datawriter created and no more topic published :

[1731234790.341469] info     | ProxyClient.cpp    | create_datawriter        | datawriter created     | client_key: 0x63680BBD, datawriter_id: 0x002(5), publisher_id: 0x002(3)
[1731234790.341500] debug    | SerialAgentLinux.cpp | send_message             | [** <<SER>> **]        | client_key: 0x63680BBD, len: 14, data: 
0000: 81 80 0D 00 05 01 06 00 00 19 00 25 00 00
[1731234790.341515] debug    | SerialAgentLinux.cpp | send_message             | [** <<SER>> **]        | client_key: 0x63680BBD, len: 13, data: 
0000: 81 00 00 00 0A 01 05 00 0E 00 00 00 80
[1731234790.342558] debug    | SerialAgentLinux.cpp | recv_message             | [==>> SER <<==]        | client_key: 0x63680BBD, len: 13, data: 
0000: 81 00 00 00 0A 01 05 00 0E 00 00 00 80
[1731234790.347052] debug    | SerialAgentLinux.cpp | recv_message             | [==>> SER <<==]        | client_key: 0x63680BBD, len: 12, data: 
0000: 81 01 00 00 03 01 04 00 00 1A 00 02
[1731234790.347077] debug    | ProxyClient.cpp    | delete_object_unlock     | object deleted         | client_key: 0x63680BBD, object_id: 0x0000
[1731234790.347103] debug    | SerialAgentLinux.cpp | send_message             | [** <<SER>> **]        | client_key: 0x63680BBD, len: 14, data: 
0000: 81 01 00 00 05 01 06 00 00 1A 00 02 00 00
[1731234790.347480] debug    | SerialAgentLinux.cpp | recv_message             | [==>> SER <<==]        | client_key: 0x63680BBD, len: 20, data: 
0000: 81 01 01 00 03 01 04 00 00 1B 00 05 03 01 04 00 00 1C 00 03
[1731234790.347702] debug    | ProxyClient.cpp    | delete_object_unlock     | object deleted         | client_key: 0x63680BBD, object_id: 0x0000
[1731234790.347731] debug    | ProxyClient.cpp    | delete_object_unlock     | object deleted         | client_key: 0x63680BBD, object_id: 0x0000
[1731234790.347740] debug    | SerialAgentLinux.cpp | send_message             | [** <<SER>> **]        | client_key: 0x63680BBD, len: 14, data: 
0000: 81 01 01 00 05 01 06 00 00 1B 00 05 00 00
[1731234790.347805] debug    | SerialAgentLinux.cpp | send_message             | [** <<SER>> **]        | client_key: 0x63680BBD, len: 14, data: 
0000: 81 01 02 00 05 01 06 00 00 1C 00 03 00 00
[1731234790.347824] debug    | SerialAgentLinux.cpp | recv_message             | [==>> SER <<==]        | client_key: 0x63680BBD, len: 12, data: 
0000: 81 01 02 00 03 01 04 00 00 1D 00 12
[1731234790.347846] debug    | ProxyClient.cpp    | delete_object_unlock     | object deleted         | client_key: 0x63680BBD, object_id: 0x0001
[1731234790.347874] debug    | SerialAgentLinux.cpp | send_message             | [** <<SER>> **]        | client_key: 0x63680BBD, len: 14, data: 
0000: 81 01 03 00 05 01 06 00 00 1D 00 12 00 00
[1731234790.348236] debug    | SerialAgentLinux.cpp | recv_message             | [==>> SER <<==]        | client_key: 0x63680BBD, len: 20, data: 
0000: 81 01 03 00 03 01 04 00 00 1E 00 15 03 01 04 00 00 1F 00 13
[1731234790.348363] debug    | ProxyClient.cpp    | delete_object_unlock     | object deleted         | client_key: 0x63680BBD, object_id: 0x0001
[1731234790.348390] debug    | ProxyClient.cpp    | delete_object_unlock     | object deleted         | client_key: 0x63680BBD, object_id: 0x0001
[1731234790.348428] debug    | SerialAgentLinux.cpp | send_message             | [** <<SER>> **]        | client_key: 0x63680BBD, len: 14, data: 
0000: 81 01 04 00 05 01 06 00 00 1E 00 15 00 00
[1731234790.348462] debug    | SerialAgentLinux.cpp | send_message             | [** <<SER>> **]        | client_key: 0x63680BBD, len: 14, data: 
0000: 81 01 05 00 05 01 06 00 00 1F 00 13 00 00
[1731234790.348868] debug    | SerialAgentLinux.cpp | recv_message             | [==>> SER <<==]        | client_key: 0x63680BBD, len: 24, data: 
0000: 81 01 04 00 08 01 10 00 00 20 00 06 00 00 00 01 00 00 00 00 00 00 00 00
[1731234790.349181] debug    | SerialAgentLinux.cpp | recv_message             | [==>> SER <<==]        | client_key: 0x63680BBD, len: 12, data: 
0000: 81 01 05 00 03 01 04 00 00 21 00 22
[1731234790.349554] debug    | SerialAgentLinux.cpp | recv_message             | [==>> SER <<==]        | client_key: 0x63680BBD, len: 20, data: 
0000: 81 01 06 00 03 01 04 00 00 22 00 06 03 01 04 00 00 23 00 04
[1731234790.350119] debug    | SerialAgentLinux.cpp | recv_message             | [==>> SER <<==]        | client_key: 0x63680BBD, len: 24, data: 
0000: 81 01 07 00 08 01 10 00 00 24 00 08 00 00 00 01 00 00 00 00 00 00 00 00
[1731234790.350349] debug    | SerialAgentLinux.cpp | recv_message             | [==>> SER <<==]        | client_key: 0x63680BBD, len: 12, data: 
0000: 81 01 08 00 03 01 04 00 00 25 00 08
[1731234790.350650] debug    | SerialAgentLinux.cpp | recv_message             | [==>> SER <<==]        | client_key: 0x63680BBD, len: 12, data: 
0000: 81 01 09 00 03 01 04 00 00 26 00 32
[1731234790.350975] debug    | SerialAgentLinux.cpp | recv_message             | [==>> SER <<==]        | client_key: 0x63680BBD, len: 20, data: 
0000: 81 01 0A 00 03 01 04 00 00 27 00 25 03 01 04 00 00 28 00 23
[1731234790.352838] debug    | SerialAgentLinux.cpp | recv_message             | [==>> SER <<==]        | client_key: 0x63680BBD, len: 12, data: 
0000: 81 01 0B 00 03 01 04 00 00 29 00 01
[1731234790.353243] debug    | SerialAgentLinux.cpp | recv_message             | [==>> SER <<==]        | client_key: 0x63680BBD, len: 12, data: 
0000: 81 00 00 00 03 01 04 00 00 02 FF FE
[1731234790.354955] debug    | SerialAgentLinux.cpp | recv_message             | [==>> SER <<==]        | client_key: 0x63680BBD, len: 16, data: 
0000: 80 00 00 00 02 01 08 00 00 0A FF FD 02 00 00 00
[1731234790.431638] debug    | ProxyClient.cpp    | delete_object_unlock     | object deleted         | client_key: 0x63680BBD, object_id: 0x0002
[1731234790.431688] debug    | SerialAgentLinux.cpp | send_message             | [** <<SER>> **]        | client_key: 0x63680BBD, len: 14, data: 
0000: 81 01 06 00 05 01 06 00 00 21 00 22 00 00
[1731234790.531906] debug    | ProxyClient.cpp    | delete_object_unlock     | object deleted         | client_key: 0x63680BBD, object_id: 0x0000
[1731234790.531936] debug    | ProxyClient.cpp    | delete_object_unlock     | object deleted         | client_key: 0x63680BBD, object_id: 0x0000
[1731234790.531946] debug    | SerialAgentLinux.cpp | send_message             | [** <<SER>> **]        | client_key: 0x63680BBD, len: 14, data: 
0000: 81 01 07 00 05 01 06 00 00 22 00 06 00 00
[1731234790.531995] debug    | SerialAgentLinux.cpp | send_message             | [** <<SER>> **]        | client_key: 0x63680BBD, len: 14, data: 
0000: 81 01 08 00 05 01 06 00 00 23 00 04 00 00
[1731234790.542700] debug    | SerialAgentLinux.cpp | recv_message             | [==>> SER <<==]        | client_key: 0x63680BBD, len: 16, data: 
0000: 80 00 00 00 02 01 08 00 00 0A FF FD 02 00 00 00
[1731234790.637166] debug    | ProxyClient.cpp    | delete_object_unlock     | object deleted         | client_key: 0x63680BBD, object_id: 0x0000
[1731234790.637202] debug    | ProxyClient.cpp    | delete_object_unlock     | object deleted         | client_key: 0x63680BBD, object_id: 0x0003
[1731234790.637217] debug    | SerialAgentLinux.cpp | send_message             | [** <<SER>> **]        | client_key: 0x63680BBD, len: 14, data: 
0000: 81 01 09 00 05 01 06 00 00 25 00 08 00 00
[1731234790.637270] debug    | SerialAgentLinux.cpp | send_message             | [** <<SER>> **]        | client_key: 0x63680BBD, len: 14, data: 
0000: 81 01 0A 00 05 01 06 00 00 26 00 32 00 00
[1731234790.637407] debug    | ProxyClient.cpp    | delete_object_unlock     | object deleted         | client_key: 0x63680BBD, object_id: 0x0002
[1731234790.637437] debug    | SerialAgentLinux.cpp | send_message             | [** <<SER>> **]        | client_key: 0x63680BBD, len: 14, data: 
0000: 81 01 0B 00 05 01 06 00 00 27 00 25 00 00
[1731234790.637439] debug    | ProxyClient.cpp    | delete_object_unlock     | object deleted         | client_key: 0x63680BBD, object_id: 0x0002
[1731234790.637484] debug    | SerialAgentLinux.cpp | send_message             | [** <<SER>> **]        | client_key: 0x63680BBD, len: 14, data: 
0000: 81 01 0C 00 05 01 06 00 00 28 00 23 00 00
[1731234790.638773] debug    | ProxyClient.cpp    | delete_object_unlock     | object deleted         | client_key: 0x63680BBD, object_id: 0x0000
[1731234790.638825] info     | Root.cpp           | delete_client            | delete                 | client_key: 0x63680BBD
[1731234790.638845] info     | SessionManager.hpp | destroy_session          | session closed         | client_key: 0x63680BBD, address: 0
[1731234790.643167] info     | Root.cpp           | create_client            | create                 | client_key: 0x71ACE51E, session_id: 0x81
[1731234790.643184] info     | SessionManager.hpp | establish_session        | session established    | client_key: 0x71ACE51E, address: 0
[1731234790.643245] debug    | SerialAgentLinux.cpp | send_message             | [** <<SER>> **]        | client_key: 0x71ACE51E, len: 19, data: 
0000: 81 00 00 00 04 01 0B 00 00 00 58 52 43 45 01 00 01 0F 00
[1731234790.646187] debug    | SerialAgentLinux.cpp | recv_message             | [==>> SER <<==]        | client_key: 0x71ACE51E, len: 56, data: 
0000: 81 80 00 00 01 07 2E 00 00 0A 00 01 01 03 00 00 20 00 00 00 00 01 00 00 18 00 00 00 6D 69 63 72
0020: 6F 5F 72 6F 73 5F 66 69 72 6D 77 61 72 65 5F 6E 6F 64 65 00 00 00 67 73
[1731234790.648972] info     | ProxyClient.cpp    | create_participant       | participant created    | client_key: 0x71ACE51E, participant_id: 0x000(1)
[1731234790.649059] debug    | SerialAgentLinux.cpp | send_message             | [** <<SER>> **]        | client_key: 0x71ACE51E, len: 14, data: 
0000: 81 80 00 00 05 01 06 00 00 0A 00 01 00 00
[1731234790.649105] debug    | SerialAgentLinux.cpp | send_message             | [** <<SER>> **]        | client_key: 0x71ACE51E, len: 13, data: 
0000: 81 00 00 00 0A 01 05 00 01 00 00 00 80
[1731234790.650170] debug    | SerialAgentLinux.cpp | recv_message             | [==>> SER <<==]        | client_key: 0x71ACE51E, len: 13, data: 
0000: 81 00 00 00 0A 01 05 00 01 00 00 00 80
[1731234790.652268] debug    | SerialAgentLinux.cpp | recv_message             | [==>> SER <<==]        | client_key: 0x71ACE51E, len: 80, data: 

Additional information

platformio.ini :

[env]
framework = arduino
lib_deps = 
    https://github.com/micro-ROS/micro_ros_platformio
    Wire
    SPI
    robtillaart/MS5611@^0.4.0
    finani/ICM42688@^1.1.0
    paulstoffregen/Time@^1.6.1

board_microros_transport = serial
board_microros_distro = humble
board_microros_user_medata = project_colcon.meta

[env:pico]
platform = raspberrypi
board = pico
upload_protocol = picotool

; Flash Size: 2MB (no filesystem)
board_upload.maximum_size = 2097152

; CPU Speed: 133MHz
board_build.f_cpu =  133000000L

; Disable Debug and C++ Exceptions ; Optimize for size (Small OS)
build_unflags = -DDEBUG -fexceptions
build_flags = -DNDEBUG -fno-exceptions -Os -ffunction-sections -fdata-sections

project_colcon.meta

{
    "names": {
        "rmw_microxrcedds": {
            "cmake-args": [
                "-DRMW_UXRCE_MAX_NODES=1",
                "-DRMW_UXRCE_MAX_PUBLISHERS=20",
                "-DRMW_UXRCE_MAX_SUBSCRIPTIONS=20",
                "-DRMW_UXRCE_MAX_SERVICES=10",
                "-DRMW_UXRCE_MAX_CLIENTS=1",
                "-DRMW_UXRCE_MAX_HISTORY=2",
                "-DRMW_UXRCE_TRANSPORT=custom"
            ]
        }
    }
}

The code of adding the parameter server:

#include <rclc_parameter/rclc_parameter.h>

rcl_publisher_t imu_publisher;
rcl_publisher_t baro_publisher;
rcl_subscription_t subscriber;

rclc_executor_t executor;
rclc_support_t support;
rcl_allocator_t allocator;
rcl_node_t node;

rcl_timer_t timer;
rclc_parameter_server_t param_server;

const char * framerate_name  = "fps";
const char * exposure_name   = "exp";

int framerate = 20;
int exposure = 10000;

bool param_callback(const Parameter * old_param, const Parameter * new_param, void * context)
{
  // check if an existing parameter is updated
  if (old_param != NULL && new_param != NULL)
  {
    // set changed parameter value
    if(strcmp(new_param->name.data, framerate_name) == 0){ framerate = new_param->value.integer_value; }
    else if(strcmp(new_param->name.data, exposure_name) == 0){ exposure = new_param->value.integer_value; }

    return true;
  }
  else
    return false; // reject loading of new parameters or deletion of existing parameters
}

bool create_entities()
{
  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_firmware_node", "", &support));
  // create publisher
  RCCHECK(rclc_publisher_init_best_effort(
    &imu_publisher,
    &node,
    ROSIDL_GET_MSG_TYPE_SUPPORT(rtr_msgs, msg, TinyImu),
    "imu/tiny_data"));
  RCCHECK(rclc_publisher_init_best_effort(
    &baro_publisher,
    &node,
    ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Float32),
    "baro/pressure"));
    // create subscriber
    RCCHECK(rclc_subscription_init_default(
    &subscriber,
    &node,
    ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Int32),
    "trigger_pulse/duty"));

    // Create timer,
  RCCHECK(rclc_timer_init_default(
    &timer,
    &support,
    RCL_MS_TO_NS(timeout_ms),
    timer_callback));

   // Init custom parameter server
//   rclc_parameter_options_t param_options = {
//      .notify_changed_over_dds = true,
//      .max_params = 2,
//      .allow_undeclared_parameters = true,
//      .low_mem_mode = false};
//   RCCHECK(rclc_parameter_server_init_with_option(&param_server, &node, &param_options));

   // Init default parameter server
  RCCHECK(rclc_parameter_server_init_default(&param_server, &node));

  // create executor
  executor = rclc_executor_get_zero_initialized_executor();
  RCCHECK(rclc_executor_init(&executor, &support.context, RCLC_EXECUTOR_PARAMETER_SERVER_HANDLES + 1, &allocator));
  RCCHECK(rclc_executor_add_subscription(&executor, &subscriber, &trigger_duty_msg, &subscription_callback, ON_NEW_DATA));
  RCCHECK(rclc_executor_add_timer(&executor, &timer));
  RCCHECK(rclc_executor_add_parameter_server(&executor, &param_server, param_callback));

  // add parameters to the server
  RCCHECK(rclc_add_parameter(&param_server, framerate_name, RCLC_PARAMETER_INT));
  RCCHECK(rclc_add_parameter(&param_server, exposure_name, RCLC_PARAMETER_INT));

  // add parameter constraints
  RCCHECK(rclc_add_parameter_constraint_integer(&param_server, framerate_name, 0, 40, 10));
  RCCHECK(rclc_add_parameter_constraint_integer(&param_server, exposure_name, 20, 32000, 10));

  // set parameter default values
  RCCHECK(rclc_parameter_set_int(&param_server, framerate_name, framerate));
  RCCHECK(rclc_parameter_set_int(&param_server, exposure_name, exposure));

  return true;
}

void destroy_entities()
{
  rmw_context_t * rmw_context = rcl_context_get_rmw_context(&support.context);
  (void) rmw_uros_set_context_entity_destroy_session_timeout(rmw_context, 0);
  rcl_publisher_fini(&imu_publisher, &node);
  rcl_publisher_fini(&baro_publisher, &node);
  rcl_subscription_fini(&subscriber, &node);
  rcl_timer_fini(&timer);
  rclc_parameter_server_fini(&param_server, &node);
  rclc_executor_fini(&executor);
  rcl_node_fini(&node);
  rclc_support_fini(&support);
}

@hippo5329
Copy link
Contributor

hippo5329 commented Nov 10, 2024

Pico might have too limited ram. Try

https://github.com/hippo5329/micro-ROS-demos-platformio/tree/rolling/rclc/parameter_server

jazzy esp32 seems work, pico crashed

ubuntu@alive-pug:~$ ros2 topic list
/micro_ros_pub
/parameter_events
/rosout
ubuntu@alive-pug:~$ ros2 param list
/micro_ros_node:
  publish_rate_ms
  publish_toogle

@l4es
Copy link
Author

l4es commented Nov 11, 2024

Hi @hippo5329,

Thanks for your finding. Do you think that switching to use services seems to be a better solution by the way ?

@l4es
Copy link
Author

l4es commented Nov 11, 2024

Hi again @hippo5329,
To update, I was able to implement and test up to 2 services successfully together with 2 publishers, 1 subscription and 1 timer on the RP2040 Pico.

For limitation of RAM, maybe the RP2350 / Pico 2 would be better option if supported.

@hippo5329
Copy link
Contributor

I see RP2350/pico 2 is supported in platformio So micro-ROS port should work. We can try out. I just ordered an pico 2 to test.

https://github.com/maxgerhardt/platform-raspberrypi

@hippo5329
Copy link
Contributor

hippo5329 commented Nov 11, 2024

[env:pico2]
platform = https://github.com/maxgerhardt/platform-raspberrypi.git
board = rpipico2
framework = arduino
board_build.core = earlephilhower
lib_ldf_mode = chain+

@l4es
Copy link
Author

l4es commented Nov 23, 2024

Hi @hippo5329,

I tried above platformio.ini for the rp2350 but got the following issue :
Building micro-ROS library

Build mcu micro-ROS environment failed: 
--- stderr: rcutils
/root/ws/build/.pio/libdeps/pico2/micro_ros_platformio/build/mcu/src/rcutils/src/time_unix.c:48:4: warning: #warning before C23 is a GCC extension
   48 | #  warning no monotonic clock function available
      |    ^~~~~~~
/root/ws/build/.pio/libdeps/pico2/micro_ros_platformio/build/mcu/src/rcutils/src/time_unix.c:48:4: warning: #warning no monotonic clock function available [-Wcpp]
/root/ws/build/.pio/libdeps/pico2/micro_ros_platformio/build/mcu/src/rcutils/src/time_unix.c: In function 'rcutils_system_time_now':
/root/ws/build/.pio/libdeps/pico2/micro_ros_platformio/build/mcu/src/rcutils/src/time_unix.c:65:7: error: implicit declaration of function 'clock_gettime' [-Wimplicit-function-declaration]
   65 |   if (clock_gettime(CLOCK_REALTIME, &timespec_now) < 0) {
      |       ^~~~~~~~~~~~~
gmake[2]: *** [CMakeFiles/rcutils.dir/build.make:432: CMakeFiles/rcutils.dir/src/time_unix.c.obj] Error 1
gmake[1]: *** [CMakeFiles/Makefile2:192: CMakeFiles/rcutils.dir/all] Error 2
gmake: *** [Makefile:146: all] Error 2
---
Failed   <<< rcutils [5.53s, exited with code 2]

It looks similar to this : #153

@hippo5329
Copy link
Contributor

hippo5329 commented Nov 23, 2024

I see the same. This might be new toolchains issue of definition of _POSIX_TIMERS, we should report this to earle.

20276b0

@l4es
Copy link
Author

l4es commented Nov 26, 2024

Hi @hippo5329,

For micro_ros_platformio, I used the commit you shared :

lib_deps = 
    https://github.com/micro-ROS/micro_ros_platformio.git#20276b09f0b71131450f70a9acc53e05fbb9fda1


However I am still facing with the issue :

Building micro-ROS library
Build mcu micro-ROS environment failed: 
--- stderr: rcutils
/root/ws/build/.pio/libdeps/pico2/micro_ros_platformio/build/mcu/src/rcutils/src/time_unix.c:48:4: warning: #warning before C23 is a GCC extension
   48 | #  warning no monotonic clock function available
      |    ^~~~~~~
/root/ws/build/.pio/libdeps/pico2/micro_ros_platformio/build/mcu/src/rcutils/src/time_unix.c:48:4: warning: #warning no monotonic clock function available [-Wcpp]
/root/ws/build/.pio/libdeps/pico2/micro_ros_platformio/build/mcu/src/rcutils/src/time_unix.c: In function 'rcutils_system_time_now':
/root/ws/build/.pio/libdeps/pico2/micro_ros_platformio/build/mcu/src/rcutils/src/time_unix.c:65:7: error: implicit declaration of function 'clock_gettime' [-Wimplicit-function-declaration]
   65 |   if (clock_gettime(CLOCK_REALTIME, &timespec_now) < 0) {
      |       ^~~~~~~~~~~~~
gmake[2]: *** [CMakeFiles/rcutils.dir/build.make:432: CMakeFiles/rcutils.dir/src/time_unix.c.obj] Error 1
gmake[1]: *** [CMakeFiles/Makefile2:192: CMakeFiles/rcutils.dir/all] Error 2
gmake[1]: *** Waiting for unfinished jobs....
gmake: *** [Makefile:146: all] Error 2
---
Failed   <<< rcutils [1.28s, exited with code 2]

======================================================================================= [FAILED] Took 38.53 seconds =======================================================================================

Environment    Status    Duration
-------------  --------  ------------
pico2          FAILED    00:00:38.533

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
None yet
Projects
None yet
Development

No branches or pull requests

2 participants