Hi,
I have a LIDAR connected to an AR drone, with drivers running on the drone to read the laser data, and send it to my ros master computer using netcat. I have written a ros driver in python that listens to the network and reads the incoming laser data, creates a LaserScan message, populates all the fields and then publishes this to /scan.
I can see /scan in rostopic list, and can see populated messages in rostopic echo /scan. However when I run my launch file which runs hector_mapping, no map is created. I know the launch file works as I have tested it with the LIDAR connected directly to the master computer using the official drivers and it builds a map fine.
So I think there must be a problem when I populate the LaserScan message and publish it. I have included one of my populated messages below, can anyone see anything wrong with it or suggest something else which I can try? I have noticed the seq fields in the header are always 0 but have read I shouldn't set that myself and leave ros to deal with it.
Node: /laserlistener
Time: 21:56:49.521363019 (2016-04-05)
Severity: Info
Published Topics: /rosout, /scan
header:
seq: 0
stamp:
secs: 1459889809
nsecs: 520699977
frame_id: laser
angle_min: 3.14159265359
angle_max: -3.14159265359
angle_increment: 0.0174532925199
time_increment: 0.000515921866295
scan_time: 0.18521595
range_min: 0.15
range_max: 6
ranges: [0.0, 0.0, 1274.25, ... ] omitted as lots of numbers, all look correct though
intensities: [ ... ] ditto
The angle_min and angle_max fields are meant to be like that (at least that is what the official driver spits out) I have also tried 0 to 2*pi
Launch file:
Publishing node:
#!/usr/bin/env python
import roslib;
roslib.load_manifest('laserdrone');
import rospy;
import math;
import tf;
from sensor_msgs.msg import LaserScan;
import socket;
import numpy as np;
import time;
import sys;
if __name__ == '__main__':
rospy.init_node('laserlistener');
pub = rospy.Publisher("scan", LaserScan, queue_size=10);
rate = rospy.Rate(10);
host = '';
port = 44444;
sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM);
sock.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1);
sock.bind((host, port));
sock.settimeout(15);
sock.setblocking(1);
sock.listen(0);
try:
print "Waiting for incoming data for 15 seconds"
request, addr = sock.accept();
except:
print "timed out"
sys.exit(0);
print "Connected"
angs = [];
dists = [];
Qs = [];
results = []
oldang = -1;
lastTime = rospy.Time.now();
while not rospy.is_shutdown():
data = request.recv(10);
while (data.find("\n")==-1):
data += request.recv(1);
#print data;
try:
line = data.split(","); #90.0, 1000.45, 12
if len(line) != 3:
continue;
ang = float(line[0].strip());
dist = float(line[1].strip());
Q = float(line[2].strip());
angs.append(ang);
dists.append(dist);
Qs.append(Q);
if(ang < oldang):
#publish here
print "Publishing now";
thisTime = rospy.Time.now();
duration = (thisTime - lastTime).to_sec();
lastTime = thisTime;
print thisTime.secs, thisTime.nsecs, duration;
# print thisTime, lastTime, thisTime-lastTime, thisTime.to_sec();
lasermsg = LaserScan();
lasermsg.header.stamp = lastTime;
lasermsg.header.frame_id = "laser";
lasermsg.scan_time = duration;
lasermsg.time_increment = duration / 359;
lasermsg.angle_min = math.pi;
lasermsg.angle_max = -math.pi;
lasermsg.angle_increment = math.pi / 180;
lasermsg.range_min = 0.15;
lasermsg.range_max = 6;
lasermsg.ranges = dists;
lasermsg.intensities = Qs;
rospy.loginfo(lasermsg);
pub.publish(lasermsg);
angs = [];
dists = [];
Qs = [];
oldang = ang;
except KeyboardInterrupt:
raise
except:
print "err, data:",data, ", ", sys.exc_info()[0];
rate.sleep();
Many thanks
↧