How do I subscribe to a Twist message in micro-ROS
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));
}