microROSをRaspberry Pi Picoでつかってみる その４：PicoでROS2からデータを受け取る

実際のところ

10秒ごとにROS2からのサービスを読みとり、値が100以上の時に内蔵LED(25番に接続されている)を光らせる

#include <rcl/rcl.h>
#include <std_msgs/msg/int8.h>
#include "pico/stdlib.h"

#define LED_PIN 25

rcl_subscription_t subscription;
std_msgs__msg__Int8 msg;

void subscription_callback(const void * msgin) {
    const std_msgs__msg__Int8 * msg = (const std_msgs__msg__Int8 *)msgin;
    if (msg->data >= 100) {
        gpio_put(LED_PIN, 1);  // Turn on the LED
    } else {
        gpio_put(LED_PIN, 0);  // Turn off the LED
    }
}

void main(void) {
    // Initialize micro-ROS
    rcl_init_options_t init_options = rcl_get_zero_initialized_init_options();
    RCCHECK(rcl_init_options_init(&init_options, rclc_get_default_allocator()))
    rcl_context_t context = rcl_get_zero_initialized_context();
    RCCHECK(rcl_context_init(&context, &init_options, rclc_get_default_allocator()))
    rcl_node_t node = rcl_get_zero_initialized_node();
    rcl_node_options_t node_ops = rcl_node_get_default_options();
    RCCHECK(rcl_node_init(&node, "pico_node", "", &context, &node_ops))

    // Initialize subscriber
    rcl_subscription_options_t subscription_ops = rcl_subscription_get_default_options();
    const rosidl_message_type_support_t * type_support = ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Int8);
    RCCHECK(rcl_subscription_init(&subscription, &node, type_support, "/pico_int8_topic", &subscription_ops))

    // Initialize GPIO for the built-in LED
    gpio_init(LED_PIN);
    gpio_set_dir(LED_PIN, GPIO_OUT);
    gpio_put(LED_PIN, 0);  // Make sure the LED is off initially

    while (1) {
        rclc_executor_spin_some(&executor, RCL_MS_TO_NS(100));
    }
}