ArduinoのrosserialでArrayを扱うときの注意点

はじめに

小ネタですが、Arduinoでrosserialを使うときに、floatのArrayのデータがセット出来ずに少しハマった話です。

rosserialとは

rosserialは、Arduinoなどマイコン上でrosのtopicをpub/subするシリアル通信のライブラリです。

これを使うと簡単にArduinoで接続先のPCなどとrostopicのやりとりが出来ます。

導入方法はこちらの記事を参照ください。

ROSのrosserialを使って、M5stackとUSB通信してLチカする
ROS x Arduinoで遊んでみる

環境

• Ubuntu 16.04
• ROS kinetic
• Arduino IDE 1.8.9

float Arrayの扱い方

上記の記事を参照して、Arduinoからpubすることは出来たのですが、float array系のデータがセットすることが出来ませんでした。

例えばこんなやつです。

std_msgs/Float32MultiArray

std_msgs/Float32MultiArray.msg

# Please look at the MultiArrayLayout message definition for
# documentation on all multiarrays.

MultiArrayLayout  layout        # specification of data layout
float32[]         data          # array of data

通常のros上では、data.resize(9) などと設定出来ますがそれはArduinoでは対応していないようでした。

調べてみるとstackoverflowに解決法がありました。
https://stackoverflow.com/questions/42180378/request-for-member-resize-in-float64multiarray-which-is-of-non-class-type

つまりは、

• mallocなどで確保したメモリかstaticなメモリを代入する
• lengthを設定する
  の２つの対応が必要とのことです。

例えば下記のように書くことでデータ・セットできました。

vec3_pub.ino

#include <ros.h>
#include <std_msgs/Float32MultiArray.h>

ros::NodeHandle  nh;
std_msgs::Float32MultiArray vec3_msg;
ros::Publisher pub_vec("vec3", &vec3_msg);

void setup()
{
    nh.initNode();
    nh.advertise(pub_vec);
    vec3_msg.data = (float*)malloc(sizeof(float) * 9);
    vec3_msg.data_length = 9;
}

void loop()
{
    for (int i = 0; i < 9; ++i)
    {
        vec3_msg.data[i] = 0.1 * i;
    }
    pub_vec.publish(&vec3_msg);
    nh.spinOnce();
    delay(1000);
}

特にハマったのは、vec3_msg.data_length = 9;です。これが無いとデータ・セットされなかったです。

他の設定例：joystick

同じように、joystickのrostopic sensor_msgs/Joyも設定出来ます。

sensor_msgs/Joy.msg

# Reports the state of a joysticks axes and buttons.
Header header           # timestamp in the header is the time the data is received from the joystick
float32[] axes          # the axes measurements from a joystick
int32[] buttons         # the buttons measurements from a joystick 

これで例えばM5StackなどのJoystickの操作量をrostopicとしてpub出来ます。

joy_pub.ino

#include <ros.h>
#include <ros/time.h>
#include <sensor_msgs/Joy.h>

ros::NodeHandle  nh;
sensor_msgs::Joy joy_msg;
ros::Publisher pub_joy("joy", &joy_msg);
static float x = 0;
static float y = 0;

void setup()
{
    nh.initNode();
    nh.advertise(pub_joy);
    joy_msg.axes = (float*)malloc(sizeof(float) * 2);
    joy_msg.axes_length = 2;
}

void loop()
{
    //Joystickの操作量x,yは別の箇所で設定されている前提
    joy_msg.axes[0] = x;
    joy_msg.axes[1] = y;
    joy_msg.header.stamp = nh.now();

    pub_joy.publish(&joy_msg);
    nh.spinOnce();
    delay(100);
}

#おわりに
rosserial使用するのは簡単で、私も使い始めてすぐに通信出来ました。
興味あれば試してみてください。