Passing a reference to the subscriber callback method

[liamhan0905]

I'd like to pass an object into the subscriber_callback method via reference. I've tried couple different trials but couldn't figure out. Does microros_raspberrypi_pico_sdk support this feature?

#ifdef __cplusplus
extern "C" {
#endif

#include <stdio.h>
#include <rcl/rcl.h>
#include <rcl/error_handling.h>
#include <rclc/rclc.h>
#include <rclc/executor.h>
#include <std_msgs/msg/int32.h>
#include <geometry_msgs/msg/twist.h>
#include <rmw_microros/rmw_microros.h>
#include "pico/stdlib.h"
#include "pico_uart_transports.h"

// pwm control
#include "hardware/pwm.h"
#include "hardware/gpio.h"

#ifdef __cplusplus
}
#endif
// include custom files
#include "robotControl.hpp"

#include <vector>
#include <memory>

rcl_publisher_t publisher;
rcl_subscription_t subscriber;
std_msgs__msg__Int32 send_msg;
std_msgs__msg__Int32 recv_msg;
geometry_msgs__msg__Twist twist_msg;

void subscription_callback(const void * msgin, RobotControl& robot)
{
}

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_timer_t timer;
    rcl_node_t node;
    rcl_allocator_t allocator;
    rclc_support_t support;
    rclc_executor_t executor;

    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);

    if (ret != RCL_RET_OK)
    {
        // Unreachable agent, exiting program.
        return ret;
    }

    rclc_support_init(&support, 0, NULL, &allocator);

    // node init
    rclc_node_init_default(&node, "pico_node", "", &support);

    // create publisher
    rclc_publisher_init_default(
        &publisher,
        &node,
        ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Int32),
        "pub");

    // create subscriber
    rclc_subscription_init_default(
        &subscriber,
        &node,
        ROSIDL_GET_MSG_TYPE_SUPPORT(geometry_msgs, msg, Twist),
        "robot/cmd_vel");

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

    send_msg.data = 0;
    recv_msg.data = 0;

    MotorControl motor1 {0,1,2};
    MotorControl motor2 {3,4,5};
    MotorControl motor3 {6,7,8};
    MotorControl motor4 {9,10,11};
    RobotControl robot {motor1, motor2, motor3, motor4};

    rclc_executor_init(&executor, &support.context, 2, &allocator);

   // rclc_executor_add_subscription(&executor, &subscriber, &recv_msg, &subscription_callback, ON_NEW_DATA); 
   
rclc_executor_add_subscription(&executor, &subscriber, &recv_msg,
  [&robot](const void * msg) { subscription_callback(msg, robot); }, ON_NEW_DATA);

    while (true)
    {
        rclc_executor_spin_some(&executor, RCL_MS_TO_NS(100));
    }
    return 0;
}

I get the following error

/home/ubuntu/nav-robot/src/micro_ros_raspberrypi_pico_sdk/main.cpp:193:81: error: cannot convert 'main()::<lambda(const void*)>' to 'rclc_subscription_callback_t' {aka 'void (*)(const void*)'}
  193 |  msg) { subscription_callback(msg, robot); }, ON_NEW_DATA);
      |                                                          ^
      |                                                          |
      |                                                          main()::<lambda(const void*)>

In file included from /home/ubuntu/nav-robot/src/micro_ros_raspberrypi_pico_sdk/main.cpp:9:
/home/ubuntu/nav-robot/src/micro_ros_raspberrypi_pico_sdk/libmicroros/include/rclc/executor.h:247:32: note:   initializing argument 4 of 'rcl_ret_t rclc_executor_add_subscription(rclc_executor_t*, rcl_subscription_t*, void*, rclc_subscription_callback_t, rclc_executor_handle_invocation_t)'
  247 |   rclc_subscription_callback_t callback,
      |   ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~^~~~~~~~
make[2]: *** [CMakeFiles/main.dir/build.make:63: CMakeFiles/main.dir/main.cpp.obj] Error 1
make[1]: *** [CMakeFiles/Makefile2:1748: CMakeFiles/main.dir/all] Error 2
make: *** [Makefile:84: all] Error 2

[pablogs9]

Hello @liamhan0905 , the callback shall follow the following signature: https://github.com/ros2/rclc/blob/442c4e84bf9ce9c819aa371b2d01e0c4e0835e06/rclc/include/rclc/executor_handle.h#L63

[liamhan0905]

isn't this suitable for subscriber with additional argument?

[pablogs9]

Not sure if lambdas can be used.

Any idea @JanStaschulat ?

[JanStaschulat]

HI @liamhan0905 , see here an example with subscription with context

replace:

void subscription_callback(const void * msgin, RobotControl& robot)

void subscription_callback(const void * msgin, void * context_ptr) { RobotControl * robot = (RobotControl *) context_ptr; robot->function_call(); }

In your subscription callback you are not using are void pointer as second argument.

[JanStaschulat]

If you are using C++, you can also refer to this example. There a static member function is used as subscription callback. The lambda function with state is a problem, because this state would be a second parameter to a C function. But the subscription function signature expects only one parameter, the const void * , but in the lambda-expression you are implicitly creating two: robot as state and msg as parameter.

For the same reason, normal class methods won't work as subscription callback functions, because the conversion from C++ to C does not know how to handle the implicit (this) pointer (which refers to the current class instantiation).

Therefore the current work-around is to use subscripiton_with_context. The context can be any pointer an object (e.g. class).

[JanStaschulat]

isn't this suitable for subscriber with additional argument?

@liamhan0905 Yes, this function type declaration is used in the function rclc_executor_add_subscription_with_context, in which the callbackFunction -parameter expects two parameters. But this function type is not used in the default function rclc_executor_add_subscription, in which the callbackFunction-parameter expects only one parameter. In your source code above, the default rclc_executor_add_subscription is used.