Ask Your Question
0

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

asked 2021-01-12 17:32:27 -0600

IonCoder gravatar image

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

}
edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
1

answered 2021-01-13 05:54:51 -0600

Pablogs gravatar image

Hello, check this code. It is a full demo that controls a Kobuki robot with an ESP32 and micro-ROS, and it uses the Twist subscriber to move to micro-ROS robot.

You also can find some explanations about this demo here.

If you have more questions, please open an issue in one of our Github repos.

Hope that helps!

edit flag offensive delete link more

Comments

@Pablogs Thank you, I think that is probably just what I need to get me going.

I will look at it tonight and then come back and accept your answer.

Thanks again.

IonCoder gravatar image IonCoder  ( 2021-01-13 06:13:22 -0600 )edit

The code you pointed to worked well the first time. I took the publisher code out, so I can understand one thing at a time and I didn't break anything :-)

IonCoder gravatar image IonCoder  ( 2021-01-13 13:07:32 -0600 )edit

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

2 followers

Stats

Asked: 2021-01-12 15:04:57 -0600

Seen: 15 times

Last updated: Jan 13