ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question
1

Publish LaserScan from Arduino

asked 2018-02-24 15:44:37 -0500

mattMGN gravatar image

Hello,

I am using an arduino Mega to publish LaserScan message. But I have a weird result, when i run this code:

#include <ros.h>
#include <sensor_msgs/LaserScan.h>

ros::NodeHandle nh;

// Laser Scan
sensor_msgs::LaserScan lidar_msg;
ros::Publisher lidar_pub("/laser_scan", &lidar_msg);
float ranges[10] = {0};
float intensities[10] = {0};

void setup()
{  
  // Initialize ROS node handle, advertise and subscribe the topics
  nh.initNode();
  nh.getHardware()->setBaud(57600);
  nh.advertise(lidar_pub);

  // Set LaserScan Definition
  lidar_msg.header.frame_id = "lidar";
  lidar_msg.angle_min = 0.0;               // start angle of the scan [rad]
  lidar_msg.angle_max = 3.14*2;            // end angle of the scan [rad]
  lidar_msg.angle_increment = 3.14*2/360;  // angular distance between measurements [rad]
  lidar_msg.range_min = 0.3;               // minimum range value [m]
  lidar_msg.range_max = 50.0;                // maximum range value [m]
}


void loop(){

  // simple loop to generate values
  for (int i=0 ; i<10 ; ++i){
    ranges[i] = 1.0*i;
  }

  lidar_msg.ranges = ranges;
  lidar_msg.header.stamp = nh.now();
  lidar_pub.publish(&lidar_msg);
  nh.spinOnce();

}

Then, I launch rosserial node to start serial communication:

$ rosrun rosserial_python serial_node.py /dev/ttyUSB2
[INFO] [1519508591.451838]: ROS Serial Python Node
[INFO] [1519508591.468653]: Connecting to /dev/ttyUSB2 at 57600 baud
[INFO] [1519508593.624826]: Note: publish buffer size is 512 bytes
[INFO] [1519508593.625218]: Setup publisher on /laser_scan [sensor_msgs/LaserScan]

But when, i echo the topic, I didn't get any values in ranges:

---
header: 
  seq: 112
  stamp: 
    secs: 1519508602
    nsecs:  77765016
  frame_id: "lidar"
angle_min: 0.0
angle_max: 6.28000020981
angle_increment: 0.0174444448203
time_increment: 0.0
scan_time: 0.0
range_min: 0.300000011921
range_max: 50.0
ranges: []
intensities: []
---

Any idea ?

matt

edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
1

answered 2018-02-25 05:56:01 -0500

gvdhoorn gravatar image

Did you perhaps forget to set the ranges_length and intensities_length fields to the nr of elements in the arrays?

That is required with rosserial. See #q265037 and #q229621 for related questions.

According to wiki/rosserial/Overview/Messages:

Also be aware that the arrays in the messages are not defined as vector objects. Thus you have to predefine the array and then pass it as pointer to the message. To determine the end of the array each one has an auto-generated integer companion with the same name and the suffix _length.

And also wiki/rosserial/Overview - Limitations - Arrays.

edit flag offensive delete link more

Comments

ranges_length and intensities_length were the point ! Thanks you

mattMGN gravatar image mattMGN  ( 2018-02-25 07:19:59 -0500 )edit

hey can u show me where and how exactly did u add the ranges_length and intensities_length? Thank you.

kartiksoni gravatar image kartiksoni  ( 2022-09-02 14:57:18 -0500 )edit

Question Tools

1 follower

Stats

Asked: 2018-02-24 15:44:37 -0500

Seen: 1,394 times

Last updated: Feb 25 '18