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.

[pedropiacenza]

How would I go about having the callback function be a method of a class using context? I'm working on a microros application to control dynamixel servos and want to organize the code running on my Teensy 4.1 as a class.
In such scenario, the rcl node, allocator, executor, subscriptions and publishers are all member variables of the main class.

During the initialization method of the class, I am initializing the subscriber like so:

// Create subscriber for "servo_command"
rclc_subscription_init_default(
    &servo_cmd_subscriber_,
    &node_,
    ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, UInt16MultiArray),
    "rh3_servo_pos_commands");

This is how I try to add the subscription to the executor using rclc_executor_add_subscription_with_context:

// Init executor
executor_ = rclc_executor_get_zero_initialized_executor();
rclc_executor_init(&executor_, &support_.context, 3, &allocator_);
rclc_executor_add_subscription_with_context(
    &executor_, 
    &servo_cmd_subscriber_, 
    &new_cmd_msg_, 
    &commandsCallback, 
    this,
    ON_NEW_DATA);

Here commandsCallback is a method of the main class, with the following header and implementation:

void commandsCallback(const void * msgin);

void RH3Controller::commandsCallback(const void * msgin){
  const std_msgs__msg__UInt16MultiArray * msg = (const std_msgs__msg__UInt16MultiArray *)msgin; 
  new_cmd_msg_.layout = msg->layout;
  new_cmd_msg_.data = msg->data;
  printNewCommand();
  service_servos_ = true;
}

Is it even possible to have the callback point to a method inside the class?? How do I do this?
This is the current error I get with the code shown above:

In file included from /home/pedro/code/Arduino/libraries/RH3_Controller/RH3_Controller.cpp:1:
/home/pedro/code/Arduino/libraries/RH3_Controller/RH3_Controller.cpp: In member function 'bool RH3Controller::initialize()':
/home/pedro/code/Arduino/libraries/RH3_Controller/RH3_Controller.cpp:107:6: warning: ISO C++ forbids taking the address of an unqualified or parenthesized non-static member function to form a pointer to member function.  Say '&RH3Controller::commandsCallback' [-fpermissive]
  107 |     &commandsCallback,

By the way, using &RH3Controller::commandsCallback as a parameter for rclc_executor_add_subscription_with_context instead of &commandsCallback also doesn't work.

[JanStaschulat]

Try with a static member function as in this example example_pingpong.cpp

Background: if you are using a (normal) C++ member function, then there is a hidden third parameter that points to the instance of that class. As the parameter in the rclc is a void pointer, and the conversion from C++ to C function creates an additional parameter, then the function definition is not correct. If a pointer to a static member function is used, there is no additional pointer to the class generated. This is the reason, why a pointer to normal C++ member function creates that error message.

See also the rclc-issue#126 Feature request: C++ support

[pedropiacenza]

Thank you very much for the answer, I managed to get things working. In case it helps anyone else (who might not be C++ experts), you only need the static definition on the header, but not on the cpp file. Also, you need to cast the context pointer as shown below:

void RH3Controller::commandsCallback(const void * msgin, void* context_void_ptr){
  // cast the context pointer into the appropriate type
  RH3Controller * context_ptr = (RH3Controller*) context_void_ptr;
  // cast the void msg pointer into the appropriate
  const std_msgs__msg__UInt16MultiArray * msg = (const std_msgs__msg__UInt16MultiArray *)msgin; 
  if (msg != NULL){
    context_ptr->new_cmd_msg_.layout = msg->layout;
    context_ptr->new_cmd_msg_.data = msg->data;
    context_ptr->printNewCommand();
    context_ptr->service_servos_ = true;
  }
}

One thing I did notice is that there is no equivalent for timer callbacks, is this correct? I could not find a way to add context to the timer callback function if we want this callback to be a method of our class.
All of this makes me think that maybe organizing my code as a class is more trouble than what its worth :(