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

Passing a reference to the subscriber callback method #873

Open
liamhan0905 opened this issue Apr 25, 2023 · 9 comments
Open

Passing a reference to the subscriber callback method #873

liamhan0905 opened this issue Apr 25, 2023 · 9 comments

Comments

@liamhan0905
Copy link

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
Copy link
Member

@liamhan0905
Copy link
Author

liamhan0905 commented Apr 26, 2023

isn't this suitable for subscriber with additional argument?

@pablogs9
Copy link
Member

Not sure if lambdas can be used.

Any idea @JanStaschulat ?

@JanStaschulat
Copy link

JanStaschulat commented Apr 26, 2023

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
Copy link

JanStaschulat commented Apr 26, 2023

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
Copy link

JanStaschulat commented Apr 26, 2023

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
Copy link

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
Copy link

JanStaschulat commented Nov 11, 2024

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
Copy link

pedropiacenza commented Nov 13, 2024

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 :(

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

4 participants