Ask Your Question
1

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

asked 2015-07-21 15:57:53 -0500

charlie gravatar image

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);
}
edit retag flag offensive close merge delete

2 Answers

Sort by » oldest newest most voted
1

answered 2017-02-06 08:02:24 -0500

charlie gravatar image

There are a lot of comments about the small amount of memory available in the arduino products. I still used the MEGA and decided to convert the Arduino program to a publisher that sends the laser data one measurement at a time via rosserial followed by an end of data code. Then when the end of data signal is reached the .cpp code on the PC converts all the previously recieved data to a proper Laser scan data format and publishes that data to the robot. I was able to get ~45 data points measured using a pulsed light sensor with a rotating motor and an encoder providing the angle information pushed up to the PC @ 57600 data rate. With this setup I get a scan about every 2 seconds. The scan rate is limited by the Arduino data transfer rate.

edit flag offensive delete link more
1

answered 2015-07-21 17:46:51 -0500

jseal gravatar image

updated 2015-07-21 23:03:58 -0500

It's probably the delay(500) at the bottom of your loop. This long of a delay (0.5 seconds) is probably causing it to lose sync and hang up.

Since you are using a timer, I would just remove it or delay(1).

Updated:

I upload your code (minus delay) to the arduino UNO (don't have an extra mega). I get a Low memory available, stability problems may occur. message. But you have a mega so you should be good.

But it runs good on the my UNO.

image description

Just in case it helps, I ran this launch file.

<launch>
  <node pkg="rosserial_python" type="serial_node.py" name="rosserial_lipo" output="screen">
  <param name="~port" value="/dev/ttyACM1" />
  </node>
</launch>

Then rostopic echo /LaserData

edit flag offensive delete link more

Comments

I changed the delay from 500 to 1, it made no difference.

charlie gravatar image charlie  ( 2015-07-21 17:59:35 -0500 )edit

I updated my answer, i don't know if it will help, but thought It would give you another data point.

jseal gravatar image jseal  ( 2015-07-21 21:07:06 -0500 )edit

Thanks for the info I tried removing the delay and ran the launch file ( I had to change the port to ACM0) it ran for 5 cycles then stopped.

charlie gravatar image charlie  ( 2015-07-26 20:42:35 -0500 )edit

Didn't work for me

أسامة الادريسي gravatar image أسامة الادريسي  ( 2017-11-04 04:50:55 -0500 )edit

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

2 followers

Stats

Asked: 2015-07-21 15:57:53 -0500

Seen: 3,573 times

Last updated: Feb 06 '17