micro-ROS/micro_ros_raspberrypi_pico_sdk

Couldn't use ros2 parameters

rahulswa08 opened this issue · 5 comments

rahulswa08 commented

Issue template

  • Hardware description: Raspberry Pi Pico to Ubuntu 20.04 with ROS2 Galactic
  • Version or commit hash: galactic and foxy

Steps to reproduce the issue

Replace pico_micro_ros_example.c with the following and build the code.
Dump the uf2 to pico and start the ros2 agent.

#include <stdio.h>
#include <unistd.h>

// Copyright (c) 2021 - for information on the respective copyright owner
// see the NOTICE file and/or the repository https://github.com/ros2/rclc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#include <stdio.h>
#include <unistd.h>

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

#include <rclc_parameter/rclc_parameter.h>
#include <rmw_microros/rmw_microros.h>

#include "pico/stdlib.h"
#include "pico_uart_transports.h"

const uint LED_PIN = 25;

rclc_parameter_server_t param_server;

void timer_callback(rcl_timer_t * timer, int64_t last_call_time)
{
  (void) timer;
  (void) last_call_time;

  int value;
  rclc_parameter_get_int(&param_server, "param2", &value);
  value++;
  rclc_parameter_set_int(&param_server, "param2", (int64_t) value);
}

void on_parameter_changed(Parameter * param)
{
  printf("Parameter %s modified.", param->name.data);
  switch (param->value.type) {
    case RCLC_PARAMETER_BOOL:
      printf(" New value: %d (bool)", param->value.bool_value);
      break;
    case RCLC_PARAMETER_INT:
      printf(" New value: %ld (int)", param->value.integer_value);
      break;
    case RCLC_PARAMETER_DOUBLE:
      printf(" New value: %f (double)", param->value.double_value);
      break;
    default:
      break;
  }
  printf("\n");
}

int main()
{

    rmw_uros_set_custom_transport(
		true,
		NULL,
		pico_serial_transport_open,
		pico_serial_transport_close,
		pico_serial_transport_write,
		pico_serial_transport_read
	);

    gpio_init(LED_PIN);
    gpio_set_dir(LED_PIN, GPIO_OUT);
  rcl_ret_t rc;
  rcl_allocator_t allocator = rcl_get_default_allocator();

    // Wait for agent successful ping for 2 minutes.
    const int timeout_ms = 1000; 
    const uint8_t attempts = 120;

    rcl_ret_t ret = rmw_uros_ping_agent(timeout_ms, attempts);


  // Create init_options
  rclc_support_t support;
  rclc_support_init(&support, 0, NULL, &allocator);

  // Create node
  rcl_node_t node;
  rclc_node_init_default(&node, "demo_param_node", "", &support);

  // Create parameter service
  rclc_parameter_server_init_default(&param_server, &node);

  // create timer,
  rcl_timer_t timer;
  rclc_timer_init_default(
    &timer,
    &support,
    RCL_MS_TO_NS(1000),
    timer_callback);

  // Create executor
  rclc_executor_t executor;
  rclc_executor_init(
    &executor, &support.context, RCLC_PARAMETER_EXECUTOR_HANDLES_NUMBER + 1,
    &allocator);
  rclc_executor_add_parameter_server(&executor, &param_server, on_parameter_changed);
  rclc_executor_add_timer(&executor, &timer);

  // Add parameters
  rclc_add_parameter(&param_server, "param1", RCLC_PARAMETER_BOOL);
  rclc_add_parameter(&param_server, "param2", RCLC_PARAMETER_INT);
  rclc_add_parameter(&param_server, "param3", RCLC_PARAMETER_DOUBLE);

  rclc_parameter_set_bool(&param_server, "param1", false);
  rclc_parameter_set_int(&param_server, "param2", 10);
  rclc_parameter_set_double(&param_server, "param3", 0.01);

  bool param1;
  int param2;
  double param3;

  rclc_parameter_get_bool(&param_server, "param1", &param1);
  rclc_parameter_get_int(&param_server, "param2", &param2);
  rclc_parameter_get_double(&param_server, "param3", &param3);

 
    // msg.data = 0;
    while (true)
    {
        rclc_executor_spin_some(&executor, RCL_MS_TO_NS(100));
    }
    return 0;
}

Expected behavior

When I run ros2 param list in agent side. I was expecting list of parameter.

Actual behavior

But I received Exception while calling service of node '/demo_param_node': None

Not sure why is this happening. @pablogs9 could you takelook into this.

Additional information

Also changed the colcon.meta file according to requirements mentioned in micro_ros_parameters tutorial : link

# colcon.meta example with memory requirements to use a parameter server
{
    "names": {
        "rmw_microxrcedds": {
            "cmake-args": [
                "-DRMW_UXRCE_MAX_NODES=1",
                "-DRMW_UXRCE_MAX_PUBLISHERS=1",
                "-DRMW_UXRCE_MAX_SUBSCRIPTIONS=0",
                "-DRMW_UXRCE_MAX_SERVICES=5",
                "-DRMW_UXRCE_MAX_CLIENTS=0"
            ]
        }
    }
}
Acuadros95 commented

Hi! What is the output log of the Agent?

rahulswa08 commented

Hi @Acuadros95 ,
This is the output of ros2 run micro_ros_agent micro_ros_agent serial --dev /dev/ttyACM0 -b 115200
Output:

[1688999708.765105] info     | TermiosAgentLinux.cpp | init                     | running...             | fd: 3
[1688999708.765309] info     | Root.cpp           | set_verbose_level        | logger setup           | verbose_level: 4
[1688999709.270147] info     | Root.cpp           | create_client            | create                 | client_key: 0x39BF781D, session_id: 0x81
[1688999709.270282] info     | SessionManager.hpp | establish_session        | session established    | client_key: 0x39BF781D, address: 0
[1688999709.288886] info     | ProxyClient.cpp    | create_participant       | participant created    | client_key: 0x39BF781D, participant_id: 0x000(1)
[1688999709.298349] info     | ProxyClient.cpp    | create_replier           | replier created        | client_key: 0x39BF781D, requester_id: 0x000(7), participant_id: 0x000(1)
[1688999709.309261] info     | ProxyClient.cpp    | create_replier           | replier created        | client_key: 0x39BF781D, requester_id: 0x001(7), participant_id: 0x000(1)
[1688999709.318876] info     | ProxyClient.cpp    | create_replier           | replier created        | client_key: 0x39BF781D, requester_id: 0x002(7), participant_id: 0x000(1)
[1688999709.328949] info     | ProxyClient.cpp    | create_replier           | replier created        | client_key: 0x39BF781D, requester_id: 0x003(7), participant_id: 0x000(1)
[1688999709.339673] info     | ProxyClient.cpp    | create_replier           | replier created        | client_key: 0x39BF781D, requester_id: 0x004(7), participant_id: 0x000(1)
[1688999709.344919] info     | ProxyClient.cpp    | create_topic             | topic created          | client_key: 0x39BF781D, topic_id: 0x000(2), participant_id: 0x000(1)
[1688999709.347345] info     | ProxyClient.cpp    | create_publisher         | publisher created      | client_key: 0x39BF781D, publisher_id: 0x000(3), participant_id: 0x000(1)
[1688999709.350572] info     | ProxyClient.cpp    | create_datawriter        | datawriter created     | client_key: 0x39BF781D, datawriter_id: 0x000(5), publisher_id: 0x000(3)
Acuadros95 commented

ROS2 Galactic Are you using Fast-DDS as your ROS 2 middleware?

Galactic uses cyclone dds as its default middleware, which does not offer full compatibility with micro-ROS.
Also, notice that micro-ROS galactic and foxy versions are deprecated, as their ROS 2 versions are also deprecated.

rahulswa08 commented

Thanks @Acuadros95 ,

That worked like magic. I was using Cyclonedds and as I changed to Fastdds it started working.

Would you suggest to change ROS version? If yes which version would be better? I'm currenly using Ubuntu 20.04

Acuadros95 commented

Would you suggest to change ROS version? If yes which version would be better? I'm currenly using Ubuntu 20.04

I think you are good for now, foxy just enter its end of life status a few months ago.
On the long term you should think about moving to Humble if possible, as it will be supported until 2027 (detail)

Closing!