How do I subscribe to a Twist message in micro-ROS

edit

I am using the ping_pong example as a starting point to create a subscriber for Twist messages.

I can't find any examples of how to do this, and being new to C I am getting a bit overwhelmed. So I'm hoping you can guide me and not get too frustrated with me.

I don't get any errors when I build the project, and I can successfully flash the ESP32, but none of the print() debug messages are shown when I run the monitor.

Any help would be very much appreciated.

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

// #include <std_msgs/msg/header.h>
#include <geometry_msgs/msg/twist.h>

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

#include "driver/gpio.h"

//#ifdef ESP_PLATFORM
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
//#endif

#define STRING_BUFFER_LEN 50

#define RCCHECK(fn) { rcl_ret_t temp_rc = fn; if((temp_rc != RCL_RET_OK)){printf("Failed status on line %d: %d. Aborting.\n",__LINE__,(int)temp_rc); vTaskDelete(NULL);}}
#define RCSOFTCHECK(fn) { rcl_ret_t temp_rc = fn; if((temp_rc != RCL_RET_OK)){printf("Failed status on line %d: %d. Continuing.\n",__LINE__,(int)temp_rc);}}

#define R_WHEEL_FORWARD_GPIO CONFIG_R_WHEEL_FORWARD_GPIO
#define R_WHEEL_BACKWARD_GPIO CONFIG_R_WHEEL_BACKWARD_GPIO
#define R_WHEEL_PWM_GPIO CONFIG_R_WHEEL_PWM_GPIO

#define L_WHEEL_FORWARD_GPIO CONFIG_L_WHEEL_FORWARD_GPIO
#define L_WHEEL_BACKWARD_GPIO CONFIG_L_WHEEL_BACKWARD_GPIO
#define L_WHEEL_PWM_GPIO CONFIG_L_WHEEL_PWM_GPIO


rcl_subscription_t cmd_vel_subscriber;


geometry_msgs__msg__Twist incoming_twist;

struct twist_message {
    double lin_x;
    double lin_y;
    double lin_z;
    double angle_x;
    double angle_y;
    double angle_z;
};

void twist_subscription_callback(const void * msgin)
{
    const geometry_msgs__msg__Twist * msg = (const geometry_msgs__msg__Twist *)msgin;

    //Deal with the twist message recieved here
    printf("I got a twist message");
}



void appMain(void *argument)
{
    printf("Starting appMain");
    printf("Hello Simon");
    rcl_allocator_t allocator = rcl_get_default_allocator();
    rclc_support_t support;

    // create init_options
    RCCHECK(rclc_support_init(&support, 0, NULL, &allocator));

    // create node
    rcl_node_t node;
    RCCHECK(rclc_node_init_default(&node, "robot_motor_node", "", &support));


    // Create a best effort cmd_vel subscriber
    RCCHECK(rclc_subscription_init_best_effort(&cmd_vel_subscriber, &node,
        ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, String), "/microROS/robot_cmd_vel"));


    // Create executor
    rclc_executor_t executor;
    RCCHECK(rclc_executor_init(&executor, &support.context, 3, &allocator));
    RCCHECK(rclc_executor_add_subscription(&executor, &cmd_vel_subscriber, &incoming_twist,
        &twist_subscription_callback, ON_NEW_DATA));

    // RCCHECK(rcl_subscription_fini(&cmd_vel_subscriber, &node));
    // RCCHECK(rcl_node_fini(&node));

    while(1){
        rclc_executor_spin_some(&executor, RCL_MS_TO_NS(10));
        usleep(10000);
    }

    RCCHECK(rcl_subscription_fini(&cmd_vel_subscriber, &node));
    RCCHECK(rcl_node_fini(&node));

}