hector_slam with LIDAR not updating map in RVIZ
Hello! I am trying to use ROS Kinetic and a Hokuyo URG LIDAR unit in conjunction with hector_slam in order to produce a map of a room as the robot navigates around it. However, I am running in to some issues.
After running the example launch file, everything seems to be fine. No errors in RVIZ or terminal, and a map is generated based on LIDAR data, like so:

However, as soon as the robot moves, it becomes immediately clear that all is not well after all. Here, we moved the robot physically forward. We can see that the LIDAR input is still updating, as the white dots symbolizing the scanned wall creep closer. However, the map does not update at all, and, even more oddly, seems to be parented to the robot's mobile coordinate frame.
Frankly, I find this behavior very perplexing. I suspect the error may have something to do with this warning, hidden away in the hector launch terminal:
Waiting for tf transform data between frames /map and /base_link to become available
Unfortunately, I have no idea how to make such a transform. FOr reference, however, here is how our transforms are set up by default from hector_slam:

The launch file I am calling looks like this:
<?xml version="1.0"?>
<launch>
<param name="pub_map_odom_transform" value="true"/>
<param name="map_frame" value="map"/>
<param name="base_frame" value="base_frame"/>
<param name="odom_frame" value="base_frame"/>
<node pkg="urg_node" type="urg_node" name="urg_node"/>
<node pkg="tf" type="static_transform_publisher" name="map_2_odom" args="0 0 0 0 0 0 /map /odom 100"/>
<node pkg="tf" type="static_transform_publisher" name="odom_2_base_footprint" args="0 0 0 0 0 0 /odom /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_frame_2_laser_link" args="0 0 0 0 0 0 /base_frame /laser 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="rviz" type="rviz" name="rviz" args="-d $(find hector_slam_example)/launch/rviz_cfg.rviz"/>
<include file="$(find hector_slam_example)/launch/default_mapping.launch"/>
<include file="$(find hector_geotiff)/launch/geotiff_mapper.launch"/>
</launch>
This file, as you can see, calls two other launch files, the first of which is default_mapping.launch:
<?xml version="1.0"?>
<launch>
<arg name="tf_map_scanmatch_transform_frame_name" default="scanmatcher_frame"/>
<arg name="base_frame" default="base_footprint"/>
<arg name="odom_frame" default="nav"/>
<arg name="pub_map_odom_transform" default="true"/>
<arg name="scan_subscriber_queue_size" default="5"/>
<arg name="scan_topic" default="scan"/>
<arg name="map_size" default="2048"/>
<node pkg="hector_mapping" type="hector_mapping" name="hector_mapping" output="screen">
<!-- Frame names -->
<param name="map_frame" value="map" />
<param name="base_frame" value="$(arg base_frame)" />
<param name="odom_frame" value="$(arg base_frame)" />
<!-- Tf use -->
<param name="use_tf_scan_transformation" value="true"/>
<param name="use_tf_pose_start_estimate" value="true"/>
<param name="pub_map_odom_transform" value="$(arg pub_map_odom_transform)"/>
<!-- Map size / start point -->
<param name="map_resolution" value="0.050"/>
<param name="map_size" value="$(arg map_size)"/>
<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 = "-1.0" />
<param name="laser_z_max_value" value = "1.0" />
<!-- Advertising config -->
<param name="advertise_map_service" value="true"/>
<param name="scan_subscriber_queue_size" value="$(arg scan_subscriber_queue_size)"/>
<param name="scan_topic" value="$(arg scan_topic)"/>
<!-- Debug parameters -->
<!--
<param name="output_timing" value="false"/>
<param name="pub_drawings" value="true"/>
<param name="pub_debug_output" value="true"/>
-->
<param name="tf_map_scanmatch_transform_frame_name" value="$(arg tf_map_scanmatch_transform_frame_name)" />
</node>
</launch>
The second launch file is geotiff: <?xml version="1.0"?>
<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"/>
<arg name="map_file_path" default="$(find hector_geotiff)/maps"/>
<arg name="map_file_base_name" default="hector_slam_map"/>
<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="$(arg map_file_path)" />
<param name="map_file_base_name" type="string" value="$(arg map_file_base_name)" />
<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" />
<param name="plugins" type="string" value="hector_geotiff_plugins/TrajectoryMapWriter" />
</node>
</launch>
Please let me know if any additional information would be of use, and thank you very much for taking the time to look this over.
Asked by FomTarro on 2017-03-24 15:37:01 UTC
Comments