rosserial_arduino can't send a sensor_msgs/Imu msg
Hi,
I have a imu implemented with arduino that I'd like to connect to ros but I got stuck in the most simple thing, sending the Imu message. Sometimes it's it just hang sometimes I get an exception in the python client.
This is the program I'm trying:
#include <ros.h>
#include <sensor_msgs/Imu.h>
ros::NodeHandle nh;
sensor_msgs::Imu imu_msg;
ros::Publisher imu_pub("imu", &imu_msg);
void setup()
{
nh.initNode();
nh.advertise(imu_pub);
imu_msg.header.frame_id = 0;
imu_msg.orientation.x = 0.0;
imu_msg.orientation.y = 0.0;
imu_msg.orientation.z = 0.0;
imu_msg.orientation.w = 0.0;
}
void loop()
{
imu_msg.header.stamp = nh.now();
imu_pub.publish( &imu_msg );
nh.spinOnce();
delay(1000);
}
And this output of the python serial_node:
[INFO] [WallTime: 1327668877.112084] ROS Serial Python Node
[INFO] [WallTime: 1327668877.115963] Connected on /dev/ttyACM0 at 57600 baud
[INFO] [WallTime: 1327668880.075914] Note: publish buffer size is 512 bytes
[INFO] [WallTime: 1327668880.076318] Setup publisher on imu [sensor_msgs/Imu]
Traceback (most recent call last):
File "/home/dlobato/ros_workspace/rosserial/rosserial_python/nodes/serial_node.py", line 58, in <module>
client.run()
File "/home/dlobato/ros_workspace/rosserial/rosserial_python/src/rosserial_python/SerialClient.py", line 284, in run
self.callbacks[topic_id](msg)
File "/home/dlobato/ros_workspace/rosserial/rosserial_python/src/rosserial_python/SerialClient.py", line 95, in handlePacket
m.deserialize(data)
File "/opt/ros/electric/stacks/common_msgs/sensor_msgs/src/sensor_msgs/msg/_Imu.py", line 190, in deserialize
raise roslib.message.DeserializationError(e) #most likely buffer underfill
roslib.message.DeserializationError: unpack requires a string argument of length 72
If I just change the type of message it works fine, so I think either there's some kind of bug regarding the Imu message or I can't see the problem.
Any idea is welcome! Thanks! David.