查看: 10555|回复: 7

[讨论] ROS和arduino连接掉线，Lost sync with device, restarting...

我的Arduino板子发布nav_msgs/Odometry类型主题以及tf变换，每当我打开节点时会出现一个错误，错误如下：
[ERROR] [WallTime: 1478676771.744104] Lost sync with device, restarting...
有趣的是，我的程序中去掉这一句“odom_pub.publish(&odom);”就不会出现这个错误了，我的程序如下，请各位朋友帮我分析以下，怎么解决，谢谢.
/*
* rosserial Planar Odometry Example
*/

#include <ros.h>
#include <ros/time.h>
#include <tf/tf.h>
#include <tf/transform_broadcaster.h>
#include <nav_msgs/Odometry.h>

ros::NodeHandle  nh;

geometry_msgs::TransformStamped t;
tf::TransformBroadcaster broadcaster;
nav_msgs::Odometry odom;
ros::Publisher odom_pub("/odom",&odom);
ros::Time current_time;
ros::Time last_time;

double x = 0.0;
double y = 0.0;
double theta = 0.0;
double vx=0.0;
double vy=0.0;
double vth=0.0;

char base_link[] = "/base_link";
char base_odom[] = "/odom";

void setup()
{
  nh.initNode();
  broadcaster.init(nh);
  nh.advertise(odom_pub);
  last_time = nh.now();
}

void loop()
{
  current_time = nh.now();
  double dt=current_time.toSec()-last_time.toSec();
  double delta_x = (vx*cos(theta)-vy*sin(theta))*dt;
  double delta_y = (vx*sin(theta)+vy*cos(theta))*dt;
  double delta_th = vth*dt;

  x+=delta_x;
  y+=delta_y;
  theta+=delta_th;

  // tf odom->base_link
  t.header.frame_id = base_odom;
  t.child_frame_id = base_link;
  t.transform.translation.x = x;
  t.transform.translation.y = y;
  t.transform.rotation = tf::createQuaternionFromYaw(theta);
  t.header.stamp = current_time;
//odometry odom->base_link
  odom.header.stamp=current_time;
  odom.header.frame_id="odom";
  odom.child_frame_id="base_link";

  odom.pose.pose.position.x=x;
  odom.pose.pose.position.y=y;
  odom.pose.pose.position.z=0;
  odom.pose.pose.orientation = tf::createQuaternionFromYaw(theta);

  odom.twist.twist.linear.x=vx;
  odom.twist.twist.linear.y=0;
  odom.twist.twist.linear.z=0;
  odom.twist.twist.angular.z=vth;
  odom.twist.twist.angular.x=0;
  odom.twist.twist.angular.y=0;
  last_time=current_time;
  broadcaster.sendTransform(t);
  odom_pub.publish(&odom);

  nh.spinOnce();
  delay(10);
}

使用道具举报