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:
<launch>
<node pkg="tf" type="static_transform_publisher" name="map2odom" args="0 0 0 0 0 0 map odom 100" />
<node pkg="tf" type="static_transform_publisher" name="odom2base_frame" args="0 0 0 0 0 0 odom base_frame 100" />
<node pkg="tf" type="static_transform_publisher" name="base_frame2base_footprint" args="0 0 0 0 0 0 base_frame base_footprint 100" />
<node pkg="tf" type="static_transform_publisher" name="base_footprint2base_stabalized" args="0 0 0 0 0 0 base_footprint base_stabalized 100" />
<node pkg="tf" type="static_transform_publisher" name="base_stabalized2base_link" args="0 0 0 0 0 0 base_stabalized base_link 100" />
<node pkg="tf" type="static_transform_publisher" name="base_link2laser" args="0 0 0 0 0 0 base_link laser 100" />
<node pkg="tf" type="static_transform_publisher" name="map2scanmatcher_frame" args="0 0 0 0 0 0 map scanmatcher_frame 100" />
<include file="$(find hector_slam_launch)/launch/tutorial.launch"/>
<param name="use_sim_time" value="false" />
<param name="hector_mapping/pub_map_odom_transform" value="false"/>
<!-- Map size / start point -->
<param name="hector_mapping/map_resolution" value="0.050"/>
<param name="hector_mapping/map_size" value="2048"/>
<param name="hector_mapping/map_start_x" value="0.5"/>
<param name="hector_mapping/map_start_y" value="0.5" />
<param name="hector_mapping/map_multi_res_levels" value="5" />
<!-- Map update parameters -->
<param name="hector_mapping/update_factor_free" value="0.4"/>
<param name="hector_mapping/update_factor_occupied" value="0.9" />
<param name="hector_mapping/map_update_distance_thresh" value="0.01"/>
<param name="hector_mapping/map_update_angle_thresh" value="0.06" />
<param name="hector_mapping/laser_z_min_value" value = "-1.0" />
<param name="hector_mapping/laser_z_max_value" value = "1.0" />
<param name="hector_mapping/laser_min_dist" value="0.4" />
<param name="hector_mapping/laser_max_dist" value="4.0" />
<param name="hector_trajectory_server/trajectory_update_rate" value="4" />
<param name="hector_trajectory_server/trajectory_publish_rate" value="4" />
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
Originally posted by andythepandy93 on ROS Answers with karma: 55 on 2016-04-05
Post score: 1
Original comments
Comment by YingHua on 2016-04-05:
Could you also upload your launch file? I guess it may cause by launch file connect.
Comment by andythepandy93 on 2016-04-06:
Knew I'd forgotten something! Added launch file and he publishing node
Comment by DavidN on 2016-04-06:
Can you visualize the scan via rviz? Also, I saw you set the max range to be 6m but i also saw the laser scan data 1274.25. Are you sure all the laser scans have correct distances. In Hector slam, the scan will be filtered by the laser_max_dist set in launch file (4m in your case)
Comment by andythepandy93 on 2016-04-07:
I cannot visualise with rviz, it counts up the messages coming in so is definitely receiving them. Very true, when I run it with the actual drivers all the numbers are much smaller. I will check the code and post back
Comment by andythepandy93 on 2016-04-07:
Yes that was it! The numbers were in millimetres instead of meters. Thanks for the pointer
Comment by andythepandy93 on 2016-04-07:
Don't know how to mark this as answered either as these are only comments to the original question
Comment by DavidN on 2016-04-07:
That s great. You can write your own answer of how you fixed it in the answer session.
Comment by andythepandy93 on 2016-04-08:
I've added it, but says I need >10 points to accept my own answer