hector_slam and hector_navigation

edit

I have been trying to achieve autonomous exploration using hector_slam, hector_navigation and an rplidar for 2 months now. hector_slam seems to be working fine and I can see the map it is generating.

The problem is with using hector_navigation, I have the nodes of hector_costmap, hector_elevation_mapping and hector_exploration_node running, I run simple_exploration_controller after and it starts generating cmd_vel commands. But those commands are always the same even if I change the robot's start position, it is like the exploration controller can't see the map.

This is hector_slam launch file :

<launch>

  <arg name="geotiff_map_file_path" default="$(find hector_geotiff)/maps"/>

  <param name="/use_sim_time" value="false"/>

  <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="rviz" type="rviz" name="rviz"
    args="-d $(find hector_slam_launch)/rviz_cfg/mapping_demo.rviz"/>

 <node pkg="tf" type="static_transform_publisher" name="base_frame_2_laser"
  args="0 0 0.05 0 0 0 1 /base_link /laser 50" />

  <include file="$(find hector_mapping)/launch/mapping_default.launch"/>

  <include file="$(find hector_geotiff)/launch/geotiff_mapper.launch">
    <arg name="trajectory_source_frame_name" value="scanmatcher_frame"/>
    <arg name="map_file_path" value="$(arg geotiff_map_file_path)"/>
  </include>

</launch>

and this is hector_navigation launch file :

<launch>
<node pkg="hector_costmap" type="hector_costmap" name="hector_costmap" output="screen" respawn="false">
    <param name="cost_map_topic" value="cost_map" />
    <param name="map_frame_id" value="map" />
    <param name="local_transform_frame_id" value="base_footprint" />
    <param name="initial_free_cells_radius" value="0.3" />
    <param name="update_radius" value="4.0"/>
    <param name="costmap_pub_freq" value="4.0" />
    <param name="sys_msg_topic" value="syscommand" />

    <param name="use_grid_map" value="true" />
    <param name="grid_map_topic" value="scanmatcher_map" />

    <param name="use_elevation_map" value="true" />
    <param name="elevation_map_topic" value="elevation_map_local" />

    <param name="use_cloud_map" value="false" />
    <param name="cloud_topic" value="openni/depth/points" />

<!-- Not necessary to set, set by dynamic reconfigure -->
    <!--param name="max_delta_elevation" value="0.08"/-->
    <!--param name="allow_elevation_map_to_clear_occupied_cells" value="true" /-->
    <!--param name="max_clear_size" value="4" /-->
    <!--param name="slize_min_height" value="0.3" /-->
    <!--param name="slize_max_height" value="0.4" /-->
<!-- Not necessary to set, set by scanmatcher -->
    <!--param name="map_resolution" value="0.05" /-->
    <!--param name="max_grid_size_x" value="1024" /-->
    <!--param name="max_grid_size_y" value="1024" /-->
<!-- Not necessary to set, set by elvation_mapping -->
    <!-- param name="elevation_resolution" value="0.01" / -->
</node>

<node pkg="hector_driving_aid_markers" type="hector_driving_aid_markers_node" name="hector_driving_aid_markers" output="screen">
    <param name="left_side_y_outer" value="0.205"/>
    <param name="left_side_y_inner" value="0.11"/>
    <param name="right_side_y_outer" value="-0.205"/>
    <param name="right_side_y_inner" value="-0.11"/>
</node>

<node pkg="nodelet" type="nodelet" name="ElevationMapping" args="load hector_elevation_mapping/ElevationMapping openni_nodelet_manager" output="screen">
    <param name="elevation_resolution" value="0.01" />
    <param name="elevation_zero_level" value="16384" />
    <param name="min_observable_elevation" value="-1.0" />
    <param name="max_observable_elevation" value="0.5" />
    <param name="max_observable_distance" value="4.0" /> 
    <param name="sensor_variance" value="0.001" />   
    <param name="map_frame_id" value="map" />
    <param name="local_map_frame_id" value="base_stabilized" />
    <param name="local_elevation_map_topic" value="elevation_map_local" />
    <param name="gloabl_elevation_map_topic" value="elevation_map_global" />
    <param name="point_cloud_topic" value="openni/depth/points"/>
    <param name="grid_map_topic" value="scanmatcher_map"/>
    <param name="sys_msg_topic" value="syscommand" />
    <param name="publish_poseupdate" value="false" />
    <param name="poseupdate_pub_period" value="1.0" />
    <param name="poseupdate_height_covariance" value="0.25" />
    <param name="poseupdate_used_pattern_size" value="3" />
<!-- Not necessary to set, set by scanmatcher -->
    <!--param name="map_resolution" value="0.05 ...