印刷用メモ２

/uros_ws/esp32/firmware/freertos/apps/joint_states_subscriber

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

#include <sensor_msgs/msg/joint_state.h>
#include <stdio.h>

#include "freertos/FreeRTOS.h"

#define ARRAY_LEN 200
#define JOINT_DOUBLE_LEN 20

#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); \
            return 1;                                                                    \
        }                                                                                \
    }
#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); \
        }                                                                                  \
    }

rcl_subscription_t joint_states_subscriber;
sensor_msgs__msg__JointState joint_states_msg;

void subscription_joint_state_callback(const void *msgin)
{
    const sensor_msgs__msg__JointState *msg = (const sensor_msgs__msg__JointState *)msgin;
    //
    int joy_btn_num = 0;
    joy_btn_num = sizeof(msg->position.data);
    
    int joy_val[joy_btn_num];
    for (int i = 0; i < joy_btn_num; i++)
    {
        joy_val[i] = msg->position.data[i];
    }
    printf("I get joint_state topic msg.pos:");
    for (int l = 0; l < joy_btn_num; l++)
    {
        printf(" %d", joy_val[l]);
    }
    printf("\r\n");
    // %d \r\n", (int)(msg->position.data[i] * 100));
    //
}
/*
          joy_msg.header.stamp = ros::Time().now();




*/
/*---
header:
  stamp:
    sec: 0
    nanosec: 0
  frame_id: ''
axes: []
buttons: []
---*/
int appMain(void *argument)
{

    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, "string_node", "", &support));

    // create joint_state subscriber
    RCCHECK(rclc_subscription_init_default(
        &joint_states_subscriber,
        &node,
        ROSIDL_GET_MSG_TYPE_SUPPORT(sensor_msgs, msg, Joy),
        "/joy"));

    // create executor
    rclc_executor_t executor = rclc_executor_get_zero_initialized_executor();
    RCCHECK(rclc_executor_init(&executor, &support.context, 1, &allocator));
    RCCHECK(rclc_executor_add_subscription(&executor, &joint_states_subscriber, &joint_states_msg, &subscription_joint_state_callback, ON_NEW_DATA));

    char joint_states_msg_buffer[ARRAY_LEN];
    joint_states_msg.header.frame_id.data = joint_states_msg_buffer;
    joint_states_msg.header.frame_id.size = 20;
    joint_states_msg.header.frame_id.capacity = ARRAY_LEN;

    rosidl_runtime_c__String string_buffer[JOINT_DOUBLE_LEN];
    joint_states_msg.name.data = string_buffer;
    joint_states_msg.name.size = 0;
    joint_states_msg.name.capacity = JOINT_DOUBLE_LEN;

    for (int i = 0; i < JOINT_DOUBLE_LEN; i++)
    {
        joint_states_msg.name.data[i].data = (char *)malloc(ARRAY_LEN);
        joint_states_msg.name.data[i].size = 0;
        joint_states_msg.name.data[i].capacity = ARRAY_LEN;
    }

    double joint_states_position_buffer[JOINT_DOUBLE_LEN];
    joint_states_msg.position.data = joint_states_position_buffer;
    joint_states_msg.position.size = 7;
    joint_states_msg.position.capacity = JOINT_DOUBLE_LEN;

    double joint_states_velocity_buffer[JOINT_DOUBLE_LEN];
    joint_states_msg.velocity.data = joint_states_velocity_buffer;
    joint_states_msg.velocity.size = 7;
    joint_states_msg.velocity.capacity = JOINT_DOUBLE_LEN;

    double joint_states_effort_buffer[JOINT_DOUBLE_LEN];
    joint_states_msg.effort.data = joint_states_effort_buffer;
    joint_states_msg.effort.size = 7;
    joint_states_msg.effort.capacity = JOINT_DOUBLE_LEN;

    rclc_executor_spin(&executor);

    RCCHECK(rcl_subscription_fini(&joint_states_subscriber, &node));
    RCCHECK(rcl_node_fini(&node));
    return 0;
}

micro-ROS-demos/rclcpp/string_subscriber/main.cpp

#include <std_msgs/msg/string.hpp>
#include <rclcpp/rclcpp.hpp>
#include <memory>
#include <cstdio>
using std::placeholders::_1;

class string_subscriber_cpp_node : public rclcpp::Node
{
public:
  string_subscriber_cpp_node()
  : Node("string_subscriber_cpp")
  {
    rclcpp::QoS qos(10);
    subscription_ = this->create_subscription<std_msgs::msg::String>("std_msgs_msg_String", qos,
        std::bind(&string_subscriber_cpp_node::topic_callback, this, _1));
  }

private:
  rclcpp::Subscription<std_msgs::msg::String>::SharedPtr subscription_;

  void topic_callback(const std_msgs::msg::String::SharedPtr msg)
  {
    std::cout << "I heard: " << msg->data << std::endl;
  }
};

int main(int argc, char * argv[])
{
  rclcpp::init(argc, argv);
  rclcpp::spin(std::make_shared<string_subscriber_cpp_node>());
  rclcpp::shutdown();
  return 0;
}

ESP32（Micro-ROS）ーPublisherSample

~$ cat uros_ws/esp32/firmware/freertos_apps/apps/int32_publisher/app.c

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

#include <rcl/rcl.h>
#include <rcl/error_handling.h>
#include <std_msgs/msg/int32.h>

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

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

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

rcl_publisher_t publisher;
std_msgs__msg__Int32 msg;

void timer_callback(rcl_timer_t * timer, int64_t last_call_time)
{
    RCLC_UNUSED(last_call_time);
    if (timer != NULL) {
        RCSOFTCHECK(rcl_publish(&publisher, &msg, NULL));
        msg.data++;
    }
}

void appMain(void * arg)
{
    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, "freertos_int32_publisher", "", &support));

    // create publisher
    RCCHECK(rclc_publisher_init_default(
        &publisher,
        &node,
        ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Int32),
        "freertos_int32_publisher"));

    // create timer,
    rcl_timer_t timer;
    const unsigned int timer_timeout = 1000;
    RCCHECK(rclc_timer_init_default(
        &timer,
        &support,
        RCL_MS_TO_NS(timer_timeout),
        timer_callback));

    // create executor
    rclc_executor_t executor;
    RCCHECK(rclc_executor_init(&executor, &support.context, 1, &allocator));
    RCCHECK(rclc_executor_add_timer(&executor, &timer));

    msg.data = 0;

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

    // free resources
    RCCHECK(rcl_publisher_fini(&publisher, &node))
    RCCHECK(rcl_node_fini(&node))

  	vTaskDelete(NULL);
}