arduino(rosserial)でJointState型のメッセージを受け取れない

[b130...@planet.kanazawa-it.ac.jp]

はじめまして、初めてこのグループを利用させていただきます。ROS歴1年未満の学生です。

PC側からJointState型のTopicをarduinoにパブリッシュして、arudinoがその値でサーボを制御する等を行いたいのですが、とある条件下でうまくいかなくなってしまいます。

JointStateには

std_msgs/Header header

  uint32 seq

  time stamp

  string frame_id

string[] name

float64[] position

float64[] velocity

float64[] effort

という構造になっているのですが、

パブリッシュするJointStateの情報にname、position、velocity、effortのうちの2項目以上の情報を持たせた状態だと、arduinoのコールバック関数が呼び出されません。

しかし、name、position、velocity、effortの1つのみの情報を持たせた状態ならば、正常に受けとることができます。

例えば、positiion[1.0, 2.0, 3.0]とvelocity[10.0, 20.0, 30.0]を含ませて送ると、コールバックが動きません

しかし、positiion[1.0, 2.0, 3.0]のみを含ませて送るとコールバックが動きます。

positiion[1.0, 2.0, 3.0]とheaderを含ませて送ってもコールバックが動きます。

arduinoからJointStateをPubすることは正常に行えました。Subのときだけ異常な動作をするようです。

rqt_graphで見ても、きちんとノードはTopicを通じてつながっています。

positionやvelocityの情報を含ませた状態でもコールバック関数が動くようにしたいのですが、原因が見当つきません。

知識が足りなくお恥ずかしい限りなのですが、どうかよろしくお願いします。

以下に詳細（プログラム等）を記載します。

############PC側のパブリッシャのプログラム(python)は以下です。###########

#!/usr/bin/env python

#coding: utf-8

import rospy

#rosからString型のメッセージの型を取得

from sensor_msgs.msg import JointState

#ノードを作成、ノード名はtalker

rospy.init_node('joint_state_pub')

#chatterという名前でString型のPublisherを作成、保持するバッファ数は10

pub = rospy.Publisher('joint_states', JointState, queue_size=10)

#周波数を１０Hzに設定

rate = rospy.Rate(1)

js = JointState() #string型のクラス作成

while not rospy.is_shutdown():

js.header.stamp = rospy.Time.now()

js.header.frame_id = "my_robo_py"

js.name = ["arm1", "arm2", "arm3", "arm4", "arm5", "arm6"]

js.position = [0.0, 0.1, 0.2, 0.3, 0.4, 0.5]

js.velocity = [1.0, 2., 3.0, 4.0, 5.0, 6.0]

pub.publish(js) #hello_str変数をPublishしている

rate.sleep() #rateで設定した周波数を実現するための時間を待機する

##########ここまでPC側のPub##########

############以下がarduino側のSubとなっています。############

#include <ros.h>

#include <stdlib.h>

#include <std_msgs/String.h>

//#include <std_msgs/Header.h>

#include <sensor_msgs/JointState.h>

#include <std_msgs/Int16.h>

//node

ros::NodeHandle node;     //ノードの作成

std_msgs::Int16 yy;

ros::Publisher num_pub("test_num", &yy);

void write_pos(const sensor_msgs::JointState &POS);

ros::Subscriber<sensor_msgs::JointState> kondo_pos_sub("joint_states",&write_pos);

int hoge=0;

int aaa=0;

//コールバック関数

void write_pos(const sensor_msgs::JointState &POS){

    if(aaa==0){

        digitalWrite(13,HIGH);

        aaa=1;

    } else{

        digitalWrite(13,LOW);

        aaa=0;

    }

    hoge++;

    yy.data=hoge;

    num_pub.publish(&yy);

}

void setup(){

    Serial1.begin(115200, SERIAL_8E1);

    pinMode(13,OUTPUT);

    node.initNode();    //nodeの初期化

    node.subscribe(kondo_pos_sub);    //subscriver_topicの作成

    node.advertise(num_pub);

}

void loop(){

    node.spinOnce();

}

##########arduino側のSubはここまで##########

環境情報：

arduino: Mega2560

OS:Ubuntu14.04

ROS:indigo