Hello,

I am using an Arduino Uno with an ultrasonic sensor. The us sensor is mounted on a servo which turns the sensor 90 degrees back and forth. Now I want to publish the coordinates of the detected obstacles using the sensor_msgs/PointCloud.

I had no problems with publishing the detected distances using float or range messages but when I try using PointClouds I always get error messages. I get two different error messages, which appear in no specific order (or at least I cannot see one). When I reupload the code to the arduino and try to start publishing with ROS it seems to be random which of the errors appears. I do not change anything in the setup or the code. I am using ROS and Rosserial both on Indigo.

I hope someone know a solution regarding this problem but as I am new to ROS I am also open to any other suggestions on how to publish the coordinates.

These are the different error messages:

~$ rosrun rosserial_python serial_node.py _port:=/dev/ttyACM1
[INFO] [WallTime: 1432029875.775153] ROS Serial Python Node
[INFO] [WallTime: 1432029875.778201] Connecting to /dev/ttyACM1 at 57600 baud
/opt/ros/indigo/lib/python2.7/dist-packages/rosserial_python/SerialClient.py:336: SyntaxWarning: The publisher should be created with an explicit keyword argument 'queue_size'. Please see http://wiki.ros.org/rospy/Overview/Publishers%20and%20Subscribers for more information.
  self.pub_diagnostics = rospy.Publisher('/diagnostics', diagnostic_msgs.msg.DiagnosticArray)
[INFO] [WallTime: 1432029878.258271] wrong checksum for msg length, length 0
[INFO] [WallTime: 1432029878.258495] chk is 0
[INFO] [WallTime: 1432029887.948041] wrong checksum for msg length, length 32
[INFO] [WallTime: 1432029887.948281] chk is 0
[ERROR] [WallTime: 1432029887.953545] Mismatched protocol version in packet: lost sync or rosserial_python is from different ros release than the rosserial client
[INFO] [WallTime: 1432029887.953721] Protocol version of client is unrecognized, expected Rev 1 (rosserial 0.5+)

---

~$ rosrun rosserial_python serial_node.py _port:=/dev/ttyACM1
[INFO] [WallTime: 1432031319.654376] ROS Serial Python Node
[INFO] [WallTime: 1432031319.657267] Connecting to /dev/ttyACM1 at 57600 baud
/opt/ros/indigo/lib/python2.7/dist-packages/rosserial_python/SerialClient.py:336: SyntaxWarning: The publisher should be created with an explicit keyword argument 'queue_size'. Please see http://wiki.ros.org/rospy/Overview/Publishers%20and%20Subscribers for more information.
  self.pub_diagnostics = rospy.Publisher('/diagnostics', diagnostic_msgs.msg.DiagnosticArray)
[ERROR] [WallTime: 1432031336.761629] Unable to sync with device; possible link problem or link software version mismatch such as hydro rosserial_python with groovy Arduino
[ERROR] [WallTime: 1432031351.762493] Unable to sync with device; possible link problem or link software version mismatch such as hydro rosserial_python with groovy Arduino
[ERROR] [WallTime: 1432031366.763851] Unable to sync with device; possible link problem or link software version mismatch such as hydro rosserial_python with groovy Arduino

---

This is my code:

#include <ros.h>
#include <ros/time.h>
#include <sensor_msgs/PointCloud.h>
#include <Wire.h>
#include <Servo.h> 
const float pi = 3.14159265359;

//Set up the ros node and publisher
sensor_msgs::PointCloud sonar_msg;
ros::Publisher pub_sonar("sonar", &sonar_msg);
ros::NodeHandle nh;
geometry_msgs::Point32 points_storage[11];

[...]

char frameid[] = "/sonar";
unsigned long range_timer;

void setup()
{
  [...]  
  sonar_msg.header.seq = 0;
  sonar_msg.header.frame_id =  frameid;

  sonar_msg.points = points_storage;
  sonar_msg.points[0].x = 1.23;
  sonar_msg.points[0].y = 4.56;
  sonar_msg.points_length = 11;

  nh.initNode();
  nh.advertise(pub_sonar);  
}

void loop() 
{
  // publish the range value every 250 milliseconds
  if ( (millis()-range_timer) > 250)
  {

  // start us sensor distance measurement
  getRange(); 

  //publish x and y with ros
  points_storage[i].x = reading*cos(angle);        //calculate x position of detected object
  points_storage[i].y = reading*sin(angle);         //calculate y position of detected object
  sonar_msg.points[i].x = points_storage[i].x;    
  sonar_msg.points[i].y = points_storage[i].y;    
  sonar_msg.header.stamp = nh.now();
  pub_sonar.publish(&sonar_msg);  

  // turn the servo one step ()
  i = oneStep();

  range_timer =  millis();
  }

  nh.spinOnce();
}

[...]

Thanks a lot, Marvin