9
Help us understand the problem. What are the problem?

More than 1 year has passed since last update.

posted at

updated at

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を設定する
の2つの対応が必要とのことです。

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

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使用するのは簡単で、私も使い始めてすぐに通信出来ました。
興味あれば試してみてください。

Why not register and get more from Qiita?
  1. We will deliver articles that match you
    By following users and tags, you can catch up information on technical fields that you are interested in as a whole
  2. you can read useful information later efficiently
    By "stocking" the articles you like, you can search right away
Sign upLogin
9
Help us understand the problem. What are the problem?