rosserial arduino: tf::broadcaster contradicts using publisher
Hello together,
I am running rosserial on the arduino UNO with the "rosserial_server" packages node "serial_node" under ubuntu 14.04. The setup works in general, I just can not solve the following problem:
When using a tf::broadcaster and another publisherof arbitrary type at the same time, the arduino must run into some error. At least the serial_node either never gets anything back from the arduino or gives the error output:
[WARN] [WallTime: 1422869167.806875] Serial Port read returned short (expected 1 bytes, received 0 instead). [WARN] [WallTime: 1422869167.807257] Serial Port read failure:
I prepared a minimal example:
#include <ros.h>
#include <std_msgs/Int32.h>
#include <ros/time.h>
#include <tf/tf.h>
#include <math.h>
#include <tf/transform_broadcaster.h>
// Configuration hardware
const long BAUD_RATE = 115200;
int PUBLISH_HZ =10;
//time of last tf send
long int lastSendMillis=0;
//ros variables
const char base_link[] = "/base_link";
const char odom[] = "/odom";
ros::NodeHandle nh;
geometry_msgs::TransformStamped t;
tf::TransformBroadcaster broadcaster;
//std_msgs::Int32 iMsg;
//ros::Publisher intChatter("timeDifs", &iMsg);
void setup()
{
t.header.frame_id = base_link;
t.child_frame_id = odom;
nh.getHardware()->setBaud(BAUD_RATE);
nh.initNode();
broadcaster.init(nh);
// nh.advertise(intChatter);
}
void loop() {
int timeDiff=(millis() - lastSendMillis);
if (timeDiff >= 1000/PUBLISH_HZ) {
// send it now!
lastSendMillis=millis();
t.transform.translation.x = 1;
t.transform.translation.y = 1;
t.transform.rotation = tf::createQuaternionFromYaw(M_PI / 2.);
t.header.stamp = nh.now();
broadcaster.sendTransform(t);
// iMsg.data=timeDiff;
// intChatter.publish(&iMsg);
nh.spinOnce();
}
}
It is enough to comment in the message definition "std_msgs::Int32 iMsg;" to produce the error. I can not see how this is related.