SLAM with LIDAR
I've been posting similar questions on open topics, but that may have not been the proper way to address the forum.
This may seen like a data dump, but I'm trying to be as complete as possible here so that one can see as much about what I have going on as possible. Of course, I will provide if more information if more is needed.
Thank you guys in advance for any help or leads you can provide.
Using ros indigo
I've seen several examples of real-time and post-processed slam with RPLIDAR. Just a few: https://www.youtube.com/watch?v=uZDeNKbcnuE
https://www.youtube.com/watch?v=pCF7P7u8pDk
https://youtu.be/gy9cDuaNW1w?t=48s
Now I've been fiddling with launch files for a couple months now on my free time with little luck as I know very little about how ros is structured and how to tie one tool to another. I've been basically making a Frankenstein Monster launch file and crossing my fingers.
I'm not receiving any errors and RVIS launches with out any problems and I can see the realtime data plotted on the 2D plane as red squares. I set the Global Fixed Frame to "map" and the RPLidarLaserScan display (really just LaserScan) Topic to "/scan". Then I add the "Map" display and set the topic to "/map". And press "Reset" at the bottom. The 2D plane on the display changes color to the "map" color, but I still do not see an occupancy grid, just the same red squares.
I feel like I'm missing something simple, like I'm not listening in on the right published topic. I've tried all the permuations of topic combinations but I cannot get a nice slam occupancy grid or map.
It might be easier to make a .bag file and post process it through hector mapping. Does anyone
Here is my current launch file:
<!--
Used for visualising rplidar in action.
It requires rplidar.launch.
-->
<launch>
<include file="$(find rplidar_ros)/launch/rplidar.launch" />
<node name="rviz" pkg="rviz" type="rviz" args="-d $(find rplidar_ros)/rviz/rplidar.rviz" />
<node pkg="hector_mapping" type="hector_mapping" name="hector_mapping" output="screen">
<!-- Frame Names -->
<param name="base_frame" value="base_link" />
<!-- param name="base_frame" value="/laser" -->
<param name="map_frame" value="map" />
<param name="odom_frame" value="base-link" />
<!-- param name="odom_frame" value="/laser" -->
<!-- Tf use -->
<param name="use_tf_scan_transformation" value="true"/>
<param name="use_tf_pose_start_estimate" value="false"/>
<param name="pub_map_odom_transform" value="true"/>
<!-- Map size / start point -->
<param name="map_resolution" value="0.050"/>
<param name="map_size" value="2048"/>
<param name="map_start_x" value="0.5"/>
<param name="map_start_y" value="0.5" />
<param name="map_multi_res_levels" value="2" />
<!-- Map update parameters -->
<param name="update_factor_free" value="0.4"/>
<param name="update_factor_occupied" value="0.9" />
<param name="map_update_distance_thresh" value="0.4"/>
<param name="map_update_angle_thresh" value="0.06" />
<param name="laser_z_min_value" value = "-.5" />
<param name="laser_z_max_value" value = "0.5" />
<!-- Advertising config -->
<param name="advertise_map_service" value="true"/>
<param name="scan_subscriber_queue_size" value="5"/>
<param name="scan_topic" value="/laser/scan"/>
<!-- param name="scan_topic" value="/laser/scan" -->
<param name="tf_map_scanmatch_transform_frame_name" value="scanmatcher_frame" />
</node>
<node pkg="tf" type="static_transform_publisher" name="base_to_laser_broadcaster" args="0 0 0 0 0 0 /base_link /laser 50" />
<node pkg="tf" type="static_transform_publisher" name="laser_link" args="0.031 0.0 0.251 0.0 0.0 0.0 /base_frame /laser 100" />
<node pkg="tf" type="static_transform_publisher" name="map_2_base_footprint" args="0 0 0 0 0 0 /map /base_footprint 100"/>
<node pkg="tf" type="static_transform_publisher" name="base_footprint_2_base_link" args="0 0 0 0 0 0 /base_footprint /base_link 100"/>
<node pkg="tf" type="static_transform_publisher" name="base_link_2_base_stabilized_link" args="0 0 0 0 0 0 /base_link /base_stabilized 100"/>
<node pkg="tf" type="static_transform_publisher" name="base_stablized_2_base_frame" args="0 0 0 0 0 0 /base_stabilized /base_frame 100"/>
<node pkg="tf" type="static_transform_publisher" name="base_2_nav_link" args="0 0 0 0 0 0 /base_frame /nav 100"/>
<node pkg="tf" type="static_transform_publisher" name="base_frame_2_laser_link" args="0 0 0 0 0 0 /base_frame /laser_link 100"/>
<!-- cnode pkg="tf" type="static_transform_publisher" name="imu_link" args="-0.049 0.0 0.180 0.0 0.0 0.0 /base_link /imu 50" -->
<!--
<node pkg="hector_trajectory_server" type="hector_trajectory_server" name="hector_trajectory_server" output="screen">
<param name="target_frame_name" type="string" value="/map" />
<param name="source_frame_name" type="string" value="laser" />
<param name="trajectory_update_rate" type="double" value="4" />
<param name="trajectory_publish_rate" type="double" value="0.25" />
</node>
<node pkg="hector_geotiff" type="geotiff_node" name="hector_geotiff_node" output="screen" launch-prefix="nice -n 15">
<remap from="map" to="/dynamic_map" />
<param name="map_file_path" type="string" value="$(find hector_geotiff)/maps" />
<param name="map_file_base_name" type="string" value="hector_slam_map" />
<param name="geotiff_save_period" type="double" value="10" />
<param name="draw_background_checkerboard" type="bool" value="true" />
<param name="draw_free_space_grid" type="bool" value="true" />
<param name="plugins" type="string" value="hector_geotiff_plugins/TrajectoryMapWriter" />
</node>
-->
</launch>
And here is rplidar.launch:
<launch>
<node name="rplidarNode" pkg="rplidar_ros" type="rplidarNode" output="screen">
<param name="serial_port" type="string" value="/dev/ttyUSB0"/>
<param name="serial_baudrate" type="int" value="115200"/>
<param name="frame_id" type="string" value="laser"/>
<param name="inverted" type="bool" value="true"/>
<param name="angle_compensate" type="bool" value="true"/>
</node>
</launch>
Here is the output on the cmd prompt after running ROS with the following Launch file:
$ cd catkin_ws/
$ source devel/setup.bash
$ roslaunch rplidar_ros view_rplidar.launch
... logging to /home/[username]/.ros/log/24efe2b2-3ae9-11e5-aead-28b2bd43e86a/roslaunch-[username]-5670.log
Checking log directory for disk usage. This may take awhile.
Press Ctrl-C to interrupt
Done checking log file disk usage. Usage is <1GB.
started roslaunch server http://[username]:43425/
SUMMARY
========
PARAMETERS
* /hector_mapping/advertise_map_service: True
* /hector_mapping/base_frame: base_link
* /hector_mapping/laser_z_max_value: 1.0
* /hector_mapping/laser_z_min_value: -1.0
* /hector_mapping/map_frame: map
* /hector_mapping/map_multi_res_levels: 2
* /hector_mapping/map_resolution: 0.05
* /hector_mapping/map_size: 2048
* /hector_mapping/map_start_x: 0.5
* /hector_mapping/map_start_y: 0.5
* /hector_mapping/map_update_angle_thresh: 0.06
* /hector_mapping/map_update_distance_thresh: 0.4
* /hector_mapping/odom_frame: base-link
* /hector_mapping/pub_map_odom_transform: True
* /hector_mapping/scan_subscriber_queue_size: 5
* /hector_mapping/scan_topic: /laser/scan
* /hector_mapping/tf_map_scanmatch_transform_frame_name: scanmatcher_frame
* /hector_mapping/update_factor_free: 0.4
* /hector_mapping/update_factor_occupied: 0.9
* /hector_mapping/use_tf_pose_start_estimate: False
* /hector_mapping/use_tf_scan_transformation: True
* /rosdistro: indigo
* /rosversion: 1.11.13
* /rplidarNode/angle_compensate: True
* /rplidarNode/frame_id: laser
* /rplidarNode/inverted: True
* /rplidarNode/serial_baudrate: 115200
* /rplidarNode/serial_port: /dev/ttyUSB0
NODES
/
base_2_nav_link (tf/static_transform_publisher)
base_footprint_2_base_link (tf/static_transform_publisher)
base_frame_2_laser_link (tf/static_transform_publisher)
base_link_2_base_stabilized_link (tf/static_transform_publisher)
base_stablized_2_base_frame (tf/static_transform_publisher)
base_to_laser_broadcaster (tf/static_transform_publisher)
hector_mapping (hector_mapping/hector_mapping)
laser_link (tf/static_transform_publisher)
map_2_base_footprint (tf/static_transform_publisher)
rplidarNode (rplidar_ros/rplidarNode)
rviz (rviz/rviz)
ROS_MASTER_URI=http://localhost:11311
core service [/rosout] found
process[rplidarNode-1]: started with pid [5688]
RPLidar health status : 0
process[rviz-2]: started with pid [5710]
process[hector_mapping-3]: started with pid [5711]
process[base_to_laser_broadcaster-4]: started with pid [5744]
process[laser_link-5]: started with pid [5795]
HectorSM map lvl 0: cellLength: 0.05 res x:2048 res y: 2048
HectorSM map lvl 1: cellLength: 0.1 res x:1024 res y: 1024
[ INFO] [1438720899.451507703]: HectorSM p_base_frame_: base_link
[ INFO] [1438720899.451589413]: HectorSM p_map_frame_: map
[ INFO] [1438720899.451663601]: HectorSM p_odom_frame_: base-link
[ INFO] [1438720899.451700905]: HectorSM p_scan_topic_: /laser/scan
[ INFO] [1438720899.451825522]: HectorSM p_use_tf_scan_transformation_: true
[ INFO] [1438720899.451861594]: HectorSM p_pub_map_odom_transform_: true
[ INFO] [1438720899.451895145]: HectorSM p_scan_subscriber_queue_size_: 5
[ INFO] [1438720899.451932493]: HectorSM p_map_pub_period_: 2.000000
[ INFO] [1438720899.451969605]: HectorSM p_update_factor_free_: 0.400000
[ INFO] [1438720899.452001562]: HectorSM p_update_factor_occupied_: 0.900000
[ INFO] [1438720899.452032465]: HectorSM p_map_update_distance_threshold_: 0.400000
[ INFO] [1438720899.452064037]: HectorSM p_map_update_angle_threshold_: 0.060000
[ INFO] [1438720899.452096293]: HectorSM p_laser_z_min_value_: -1.000000
[ INFO] [1438720899.452127195]: HectorSM p_laser_z_max_value_: 1.000000
process[map_2_base_footprint-6]: started with pid [5835]
process[base_footprint_2_base_link-7]: started with pid [5857]
process[base_link_2_base_stabilized_link-8]: started with pid [5868]
process[base_stablized_2_base_frame-9]: started with pid [5879]
process[base_2_nav_link-10]: started with pid [5894]
process[base_frame_2_laser_link-11]: started with pid [5905]
Here's the rosout.log:
1438730777.038540646 Node Startup
1438730777.344989855 INFO [/tmp/buildd/ros-indigo-hector-mapping-0.3.3-0trusty-20150523-1227/src/HectorMappingRos.cpp:166(HectorMappingRos) [topics: /rosout, /map, /map_metadata] HectorSM p_base_frame_: base_link
1438730777.345337655 INFO [/tmp/buildd/ros-indigo-hector-mapping-0.3.3-0trusty-20150523-1227/src/HectorMappingRos.cpp:167(HectorMappingRos) [topics: /rosout, /map, /map_metadata] HectorSM p_map_frame_: map
1438730777.345531977 INFO [/tmp/buildd/ros-indigo-hector-mapping-0.3.3-0trusty-20150523-1227/src/HectorMappingRos.cpp:168(HectorMappingRos) [topics: /rosout, /map, /map_metadata] HectorSM p_odom_frame_: base-link
1438730777.345711115 INFO [/tmp/buildd/ros-indigo-hector-mapping-0.3.3-0trusty-20150523-1227/src/HectorMappingRos.cpp:169(HectorMappingRos) [topics: /rosout, /map, /map_metadata] HectorSM p_scan_topic_: /laser/scan
1438730777.345875667 INFO [/tmp/buildd/ros-indigo-hector-mapping-0.3.3-0trusty-20150523-1227/src/HectorMappingRos.cpp:170(HectorMappingRos) [topics: /rosout, /map, /map_metadata] HectorSM p_use_tf_scan_transformation_: true
1438730777.346040560 INFO [/tmp/buildd/ros-indigo-hector-mapping-0.3.3-0trusty-20150523-1227/src/HectorMappingRos.cpp:171(HectorMappingRos) [topics: /rosout, /map, /map_metadata] HectorSM p_pub_map_odom_transform_: true
1438730777.346207613 INFO [/tmp/buildd/ros-indigo-hector-mapping-0.3.3-0trusty-20150523-1227/src/HectorMappingRos.cpp:172(HectorMappingRos) [topics: /rosout, /map, /map_metadata] HectorSM p_scan_subscriber_queue_size_: 5
1438730777.346363004 INFO [/tmp/buildd/ros-indigo-hector-mapping-0.3.3-0trusty-20150523-1227/src/HectorMappingRos.cpp:173(HectorMappingRos) [topics: /rosout, /map, /map_metadata] HectorSM p_map_pub_period_: 2.000000
1438730777.346505343 INFO [/tmp/buildd/ros-indigo-hector-mapping-0.3.3-0trusty-20150523-1227/src/HectorMappingRos.cpp:174(HectorMappingRos) [topics: /rosout, /map, /map_metadata] HectorSM p_update_factor_free_: 0.400000
1438730777.346647567 INFO [/tmp/buildd/ros-indigo-hector-mapping-0.3.3-0trusty-20150523-1227/src/HectorMappingRos.cpp:175(HectorMappingRos) [topics: /rosout, /map, /map_metadata] HectorSM p_update_factor_occupied_: 0.900000
1438730777.346796391 INFO [/tmp/buildd/ros-indigo-hector-mapping-0.3.3-0trusty-20150523-1227/src/HectorMappingRos.cpp:176(HectorMappingRos) [topics: /rosout, /map, /map_metadata] HectorSM p_map_update_distance_threshold_: 0.400000
1438730777.346951000 INFO [/tmp/buildd/ros-indigo-hector-mapping-0.3.3-0trusty-20150523-1227/src/HectorMappingRos.cpp:177(HectorMappingRos) [topics: /rosout, /map, /map_metadata] HectorSM p_map_update_angle_threshold_: 0.060000
1438730777.347056452 INFO [/tmp/buildd/ros-indigo-hector-mapping-0.3.3-0trusty-20150523-1227/src/HectorMappingRos.cpp:178(HectorMappingRos) [topics: /rosout, /map, /map_metadata] HectorSM p_laser_z_min_value_: -0.500000
1438730777.347137277 INFO [/tmp/buildd/ros-indigo-hector-mapping-0.3.3-0trusty-20150523-1227/src/HectorMappingRos.cpp:179(HectorMappingRos) [topics: /rosout, /map, /map_metadata] HectorSM p_laser_z_max_value_: 0.500000
1438730777.426748894 INFO [/tmp/buildd/ros-indigo-rviz-1.11.7-0trusty-20150523-0922/src/rviz/ogre_helpers/render_system.cpp:200(RenderSystem::detectGlVersion) [topics: /rosout] OpenGl version: 3 (GLSL 1.3).
Here is an image of my RVIS window showing the TF tree.
Here is the output from $rostopic list -v
Published topics:
* /map_metadata [nav_msgs/MapMetaData] 1 publisher
* /move_base_simple/goal [geometry_msgs/PoseStamped] 1 publisher
* /slam_cloud [sensor_msgs/PointCloud] 1 publisher
* /rosout [rosgraph_msgs/Log] 11 publishers
* /tf [tf2_msgs/TFMessage] 9 publishers
* /initialpose [geometry_msgs/PoseWithCovarianceStamped] 1 publisher
* /poseupdate [geometry_msgs/PoseWithCovarianceStamped] 1 publisher
* /scan [sensor_msgs/LaserScan] 1 publisher
* /rosout_agg [rosgraph_msgs/Log] 1 publisher
* /map [nav_msgs/OccupancyGrid] 1 publisher
* /slam_out_pose [geometry_msgs/PoseStamped] 1 publisher
Subscribed topics:
* /rosout [rosgraph_msgs/Log] 1 subscriber
* /syscommand [std_msgs/String] 1 subscriber
* /laser/scan [sensor_msgs/LaserScan] 1 subscriber
* /tf_static [tf2_msgs/TFMessage] 2 subscribers
* /initialpose [geometry_msgs/PoseWithCovarianceStamped] 1 subscriber
* /map_updates [map_msgs/OccupancyGridUpdate] 1 subscriber
* /tf [tf2_msgs/TFMessage] 2 subscribers
* /map [nav_msgs/OccupancyGrid] 1 subscriber
* /scan [sensor_msgs/LaserScan] 1 subscriber
AND my output from $rosbag record -a but I don't know what to do with the bag file, but i would much rather have both the bag AND a realtime map with geotiff or dotcloud info.
[ INFO] [1438732560.844018230]: Recording to 2015-08-04-16-56-00.bag.
[ INFO] [1438732560.845369112]: Subscribing to /map_metadata
[ INFO] [1438732560.848899880]: Subscribing to /move_base_simple/goal
[ INFO] [1438732560.851750135]: Subscribing to /slam_cloud
[ INFO] [1438732560.854621094]: Subscribing to /rosout
[ INFO] [1438732560.858674790]: Subscribing to /tf
[ INFO] [1438732560.863254613]: Subscribing to /initialpose
[ INFO] [1438732560.866927436]: Subscribing to /poseupdate
[ INFO] [1438732560.870107602]: Subscribing to /scan
[ INFO] [1438732560.874706166]: Subscribing to /rosout_agg
[ INFO] [1438732560.878961002]: Subscribing to /map
[ INFO] [1438732560.885161275]: Subscribing to /slam_out_pose
Asked by casper911ca on 2015-08-04 19:12:20 UTC
Answers
You are making a mistake here:
param name="base_frame" value="base_link"
param name="odom_frame" value="base-link"
It should be the same "base_link" for both.
I don't understand very well your launch file. I did it writing 3 launch files. The first one to setup the RVIZ, launch the hector_mapping.launch and the geotiff_mapper.launch. The second one to setup the hector_mapping parameters and also to launch the urg node. And the third one to setup the geotiff parameters. Those are mi files. I hope it helps.
slam.launch
<launch>
<param name="/use_sim_time" value="false"/>ros
<node pkg="rviz" type="rviz" name="rviz"
args="-d $(find hector_slam_launch)/rviz_cfg/mapping_demo.rviz"/>
<include file="$(find laser_prueba)/hector_mapping.launch"/>
<include file="$(find laser_prueba)/geotiff_mapper.launch">
<arg name="trajectory_source_frame_name" value="scanmatcher_frame"/>
</include>
</launch>
hector_mapping.launch
<launch>
<node pkg="urg_node" type="urg_node" name="laser_scan">
<param name="port" value="/dev/ttyACM0"/>
<param name="frame_id" value="laser"/>
<param name="calibrate_time" type="bool" value="true"/>
<param name="intensity" type="bool" value="false"/>
<param name="min_ang" value="-2.356194437"/>
<param name="max_ang" value="2.35619443"/>
<param name="cluster" value="1"/>
</node>
<node pkg="hector_mapping" type="hector_mapping" name="hector_mapping" output="screen">
<param name="pub_map_odom_transform" value="true"/>
<param name="map_frame" value="map" />
<param name="base_frame" value="base_link" />
<param name="odom_frame" value="base_link" />
<!-- Map size / start point -->
<param name="map_resolution" value="0.025"/>
<param name="map_size" value="2048"/>
<param name="map_start_x" value="0.5"/>
<param name="map_start_y" value="0.5" />
<param name="laser_z_min_value" value="-2.5" />
<param name="laser_z_max_value" value="7.5" />
<!-- Map update parameters -->
<param name="update_factor_free" value="0.4"/>
<param name="update_factor_occupied" value="0.7" />
<param name="map_update_distance_thresh" value="0.2"/>
<param name="map_update_angle_thresh" value="0.06" />
</node>
<node pkg="tf" type="static_transform_publisher" name="base_to_laser_broadcaster" args="0 0 0 0 0 0 /base_link /laser 100" />
</launch>
geotiff_mapper.launch
<launch>
<arg name="trajectory_source_frame_name" default="/base_link"/>
<arg name="trajectory_update_rate" default="4"/>
<arg name="trajectory_publish_rate" default="0.25"/>
<node pkg="hector_trajectory_server" type="hector_trajectory_server" name="hector_trajectory_server" output="screen">
<param name="target_frame_name" type="string" value="/map" />
<param name="source_frame_name" type="string" value="$(arg trajectory_source_frame_name)" />
<param name="trajectory_update_rate" type="double" value="$(arg trajectory_update_rate)" />
<param name="trajectory_publish_rate" type="double" value="$(arg trajectory_publish_rate)" />
</node>
<node pkg="hector_geotiff" type="geotiff_node" name="hector_geotiff_node" output="screen" launch-prefix="nice -n 15">
<remap from="map" to="/dynamic_map" />
<param name="map_file_path" type="string" value="$(find hector_geotiff)/maps" />
<param name="map_file_base_name" type="string" value="uprobotics" />
<param name="geotiff_save_period" type="double" value="0" />
<param name="draw_background_checkerboard" type="bool" value="true" />
<param name="draw_free_space_grid" type="bool" value="true" />
</node>
</launch>
Asked by Jhair on 2015-09-01 10:35:30 UTC
Comments