Fake LaserScan on Arduino 2560 – The LaserScan data seems to transfer correctly, except the program hangs up after ~ 11-18 successful data transfers.

edit

Fake LaserScan on Arduino 2560 – The LaserScan data seems to transfer correctly, except the program hangs up after ~ 11-18 successful data transfers.

Any suggestions?

Last successful data transfer looks like this – The data package is small to keep it simple.

---
header: 
  seq: 11
  stamp: 
    secs: 1437511609
    nsecs: 32560897
  frame_id: laser_frame
angle_min: -1.57000005245
angle_max: 1.57000005245
angle_increment: 0.785000026226
time_increment: 10.0
scan_time: 4.0
range_min: 0.10000000149
range_max: 30.0
ranges: [0.0, 1.0, 2.0, 3.0, 4.0]
intensities: [0.0, 1.0, 4.0, 9.0, 16.0]
---

Here is the code

#include "math.h"
#if defined(ARDUINO) && ARDUINO >= 100
#include "Arduino.h"
#else
#include "WProgram.h"
#endif

// ROS includes
#include <ros.h>
#include <ros/time.h>
#include <sensor_msgs/LaserScan.h>

sensor_msgs::LaserScan CDLaser_msg;

//
float f_angle_min;
float f_angle_max;
float f_angle_increment;
float f_time_increment;
float f_scan_time;
float f_range_min;
float f_range_max;
float f_ranges[5]; // max of 30 measurements
float f_intensities[5];

int publisher_timer;

ros:: Publisher pub_Laser("LaserData", &CDLaser_msg);
ros::NodeHandle nh;

void setup() 
{
  nh.initNode();
  nh.advertise(pub_Laser);

  f_angle_min = -1.57;
  f_angle_max = 1.57;
  f_angle_increment = 0.785;  // 3.14/4   - 5 measurement points
  f_time_increment = 10;
  f_scan_time = 4;
  f_range_min = 0.1;
  f_range_max = 30;

  CDLaser_msg.ranges_length = 5;
  CDLaser_msg.intensities_length = 5;

  // create the test data
  for (int z = 0 ; z<5; z++)
  {
    f_ranges[z] = z;
    f_intensities[z] = z*z;
  }
}

void loop() 
{
  if (millis() > publisher_timer)
  {
    CDLaser_msg.header.stamp = nh.now();
    CDLaser_msg.header.frame_id = "laser_frame";
    CDLaser_msg.angle_min = f_angle_min;
    CDLaser_msg.angle_max = f_angle_max;
    CDLaser_msg.angle_increment = f_angle_increment;
    CDLaser_msg.time_increment = f_time_increment;
    CDLaser_msg.scan_time = f_scan_time;
    CDLaser_msg.range_min = f_range_min;
    CDLaser_msg.range_max = f_range_max;

    for (int z = 0 ; z<5; z++)
    {
      CDLaser_msg.ranges[z] = f_ranges[z];
    }

    for (int z = 0 ; z<5; z++)
    {
      CDLaser_msg.intensities[z] = f_intensities[z];
    }

    publisher_timer = millis() + 1000;
    pub_Laser.publish(&CDLaser_msg);
  }
  nh.spinOnce();
  delay(500);
}