未分類

ESP32で/Joyトピックをサブスクライブする。

RMW_QOS_POLICY_DURABILITY

QoSを揃えてみたら行けるかと思って、ベストエフォートに変更。⇨変わらず。

https://micro.ros.org/docs/tutorials/programming_rcl_rclc/pub_sub/

https://micro.ros.org/docs/tutorials/programming_rcl_rclc/qos/

ライブラリーの場所

~/uros_ws/esp32/firmware/toolchain/esp-idf/components/freertos/include/
/opt/ros/foxy/include/

${workspaceFolder}/firmware/freertos_apps/microros_esp32_extensions/build/compile_commands.json

エラー①

けど、ノードにデータ送ると（↓）エラーでESP32が自動再起動してしまう。クラッシュしてるぽい。

yaki@ubu20l15:~/uros_ws/esp32$ ros2 topic info /joy -v

シリアルで吐き出されるデータ観ると、

Received: 0
Guru Meditation Error: Core 0 panic'ed (LoadProhibited). Exception was unhandled.
Core 0 register dump:
PC : 0x4015636a PS : 0x00060b30 A0 : 0x8014ff54 A1 : 0x3ffd1450
A2 : 0x00000000 A3 : 0x00000000 A4 : 0x00000000 A5 : 0x00000000
A6 : 0x3ffd1770 A7 : 0x00000004 A8 : 0x3ffae920 A9 : 0x3ffae920
A10 : 0x3ffae978 A11 : 0x3ffd1460 A12 : 0x000000ff A13 : 0x0000ff00
A14 : 0x00ff0000 A15 : 0xff000000 SAR : 0x0000000c EXCCAUSE: 0x0000001c
EXCVADDR: 0x00000000 LBEG : 0x400014fd LEND : 0x4000150d LCOUNT : 0xffffffff

ELF file SHA256: 8ad241366caf8f10

Backtrace: 0x40156367:0x3ffd1450 0x4014ff51:0x3ffd1760 0x400d5c0b:0x3ffd17b0 0x400d8fa5:0x3ffd17d0 0x400d91cd:0x3ffd1810 0x400d9648:0x3ffd1850 0x400d9699:0x3ffd1890 0x400d5cd7:0x3ffd18d0 0x400874dd:0x3ffd19e0

Rebooting...

「Guru Meditation Error: Core 0 panic'ed」でエラー。（Guru Meditation（Wiki））

参考１、参考２、

解決策①

データ型が間違ってたぽいので、修正。

sensor_msgs/Joy Messageの型を確認（公式）してみると、

std_msgs/Header header
　⇨uint32 seq
　　time stamp
⇨int32 sec
uint32 nanosec
　　string frame_id
float32[] axes
int32[] buttons

テスト①

デモのC言語プログラムを少しいじって、テスト。

yaki@ubu20l15:~/uros_ws/esp32$ ros2 topic info /joy -v
Type: sensor_msgs/msg/Joy

Publisher count: 0

Subscription count: 1

Node name: yaki_joy_subscriber
Node namespace: /
Topic type: sensor_msgs/msg/Joy
Endpoint type: SUBSCRIPTION
GID: 01.0f.82.bf.39.8c.00.00.01.00.00.00.00.00.01.04.00.00.00.00.00.00.00.00
QoS profile:
  Reliability: RMW_QOS_POLICY_RELIABILITY_RELIABLE
  Durability: RMW_QOS_POLICY_DURABILITY_VOLATILE
  Lifespan: 2147483651294967295 nanoseconds
  Deadline: 2147483651294967295 nanoseconds
  Liveliness: RMW_QOS_POLICY_LIVELINESS_AUTOMATIC
  Liveliness lease duration: 2147483651294967295 nanoseconds

無事にノードは動いた。（↓プログラム）

#include <rcl/rcl.h>
#include <rcl/error_handling.h>
#include <rclc/rclc.h>
#include <rclc/executor.h>
#include <stdio.h>
//メッセージ型を読み込み
#include <sensor_msgs/msg/joy.h>
//FreeRTOS読み込み
#include "freertos/FreeRTOS.h"

#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 subscriber;
//サブスクリプションをまるっと入れられるStructure
sensor_msgs__msg__Joy msg;
//センサーメッセージ型

void subscription_callback(const void *msgin)
{
    const sensor_msgs__msg__Joy *msg = (const sensor_msgs__msg__Joy *)msgin;

    printf("Received: %d\n", msg->header.stamp.sec);
    printf(msg->header.stamp.nanosec);
}

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

    // create subscriberサブスクライブのトピック名、トピックの型
    RCCHECK(rclc_subscription_init_default(
        &subscriber,
        &node,
        ROSIDL_GET_MSG_TYPE_SUPPORT(sensor_msgs, msg, Joy),
        "/joy"));
    //		ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Int32),
    //		"std_msgs_msg_Int32"));

    /*
    // create joy 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, &subscriber, &msg, &subscription_callback, ON_NEW_DATA));

    rclc_executor_spin(&executor);

    RCCHECK(rcl_subscription_fini(&subscriber, &node));
    RCCHECK(rcl_node_fini(&node));
}

メモ

~/uros_ws/esp32$ ros2 run micro_ros_setup flash_firmware.sh
Flashing firmware for freertos platform esp32

Found 1 serial ports
Serial port /dev/ttyUSB0
/dev/ttyUSB0 failed to connect: [Errno 16] could not open port /dev/ttyUSB0: [Errno 16] Device or resource busy: '/dev/ttyUSB0'

A fatal error occurred: Could not connect to an Espressif device on any of the 1 available serial ports.
CMake Error at run_cmd.cmake:14 (message):
esptool.py failed
Call Stack (most recent call first):
run_esptool.cmake:21 (include)

シリアルモニタ開いたままファムウェア書き込みしようとするとエラー。書き込み前にシリアルモニタ閉じるの毎回忘れる。

この記事タイトルとURLをコピー

-未分類