Explorer unable to find path to goal during exploration on robot.
nitin5 opened this issue · comments
Hello
I am in process of putting navigation_2d on robot.
Robot fails during exploration and it keep bumping into obstacles. Following console prints are received after some time.
[ INFO] [1482263403.132722137]: map size is 5994
[ INFO] [1482263403.132770053]: map resolution is 0.25
[ INFO] [1482263403.132855664]: Checked 154 cells.
[ INFO] [1482263403.132863249]: Exploration target has been set
[ INFO] [1482263403.132882130]: Map-Value of goal point is 0, lethal threshold is 19.
[ERROR] [1482263403.134084333]: No way between robot and goal!
[ WARN] [1482263403.134288453]: Exploration has failed!
Following warning is continuously reported
[ WARN] [1482264165.978509031]: Laser reading 0.003000 not between range_min 0.010000 and range_max 20.000000!
The rqt_graph is as follows.
The launch files are as follows:
>Main launch File
> <launch>
>
> <!-- Some general parameters -->
>
> <rosparam file="$(find nav2d_launch)/param/robot.yaml"/>
>
> <!-- Start the Operator to control the simulated robot -->
> <node name="Operator" pkg="nav2d_operator" type="operator" >
> <remap from="cmd_vel" to="/ros0xrobot/cmd_vel"/>
> <rosparam file="$(find nav2d_launch)/param/operator.yaml"/>
> <rosparam file="$(find nav2d_launch)/param/costmap.yaml" ns="local_map" />
> </node>
>
> <!-- Start Mapper to genreate map from laser scans -->
> <node name="Mapper" pkg="nav2d_karto" type="mapper" output="screen">
> <remap from="base_scan" to="scan"/>
>
> <rosparam file="$(find nav2d_launch)/param/mapper.yaml"/>
> </node>
>
> <!-- Start the Navigator to move the robot autonomously -->
> <node name="Navigator" pkg="nav2d_navigator" type="navigator" output="screen">
> <rosparam file="$(find nav2d_launch)/param/navigator.yaml"/>
> </node>
>
> <node name="GetMap" pkg="nav2d_navigator" type="get_map_client" />
> <node name="Explore" pkg="nav2d_navigator" type="explore_client" />
> <node name="SetGoal" pkg="nav2d_navigator" type="set_goal_client" />
>
>
> <!-- RVIZ to view the visualization
> <node name="RVIZ" pkg="rviz" type="rviz" args=" -d $(find nav2d_launch)/param/tutorial3.rviz" />-->
>
> </launch>
>
Robot.yaml
##TF frames
laser_frame: laser
robot_frame: base_link
odometry_frame: odom
offset_frame: offset
map_frame: map
### ROS topics ############################################
map_topic: map
laser_topic: scan
Mapper.yaml:
Mapper ################################################```
grid_resolution: 0.25
range_threshold: 20.0
map_update_rate: 2
publish_pose_graph: true
max_covariance: 0.05
transform_publish_period: 0.1
min_map_size: 20
Localizer
min_particles: 2500
max_particles: 10000
Karto
For a full list of available parameters and their
corresponding default values, see OpenKarto/OpenMapper.h
UseScanMatching: true
MinimumTravelDistance: 1.0
MinimumTravelHeading: 0.52
ScanBufferSize: 25
LoopSearchMaximumDistance: 10.0
###########################################################
Costmap.yaml
global_frame: odom
robot_base_frame: base_link
update_frequency: 5.0
publish_frequency: 1.0
#set if you want the voxel map published
publish_voxel_map: true
#set to true if you want to initialize the costmap from a static map
static_map: false
#begin - COMMENT these lines if you set static_map to true
rolling_window: true
width: 10.0
height: 10.0
resolution: 0.25
#end - COMMENT these lines if you set static_map to true
map_type: costmap
track_unknown_space: true
transform_tolerance: 0.3
obstacle_range: 10.0
min_obstacle_height: 0.0
max_obstacle_height: 2.0
raytrace_range: 12
footprint: [[-0.13, -0.20], [-0.13, 0.20], [0.20, 0.20], [0.20, -0.2]]
robot_radius: 0.40
inflation_radius: 0.25
cost_scaling_factor: 2.0
lethal_cost_threshold: 100
observation_sources: scan
scan: {data_type: LaserScan, expected_update_rate: 0.4,
observation_persistence: 0.0, marking: true, clearing: true, max_obstacle_height: 1.0, min_obstacle_height: 0.08}
Operator.yaml
Maximum velocity command the operator will send to the robot (in m/s)
max_velocity: 0.5
If set to true, the selected best direction is published as a GridCells-Message
publish_route: true
The way is assumed to be free when there is no obstacle within this range (in meter)
max_free_space: 1.0
safety_decay: 0.97
The importance weights of the 3 action values
distance_weight: 1
safety_weight: 2
conformance_weight: 5
continue_weight: 0
odometry_frame: odom
robot_frame: base_link
Rgds
Have you checked if the map that has been build so far is okay? I would always suggest to test map building and navigation separately before going into autonomous exploration. So first make sure that a good map is build when the robot is remote controlled. Then test if navigation is working by setting a goal manually in RVIZ. If both is working, continue with the exploration.
The warning with the laser ranges is probably caused by your laser driver. The LaserScan includes min and max range, which seems to be incorrect in your case. But I doubt that this is causing your problems.
Greetings,
Map appear correct on visual inspection. Although I have few doubts.
- What is intuition behind odometry_offset frame. It is paraent of odom frame. During the run of the mapper, both frames odometry_offset and odom drifts continuously.
- Mapper is unable to recover from the sudden wheel slip. I have experimented with different values of parameters as mentioned in openMapper.h . I have increased/decreased parameters to invoke loop closures. I have increased the ScanBufferMaximumScanDistance, ScanBufferSize , LoopSearchSpaceDimesion, LoopSearchMaximumDistance and reduced minimumTravelHeading. But it do not have much impact on final map.
Any suggestions on improvement of results will be welcome.
Thanks
-
This is the expected behaviour in a single robot scenario. The offset frame is relevant in a multi-robot scenario, where the robots have different starting points in the map.
-
This can be an issue if your wheel slip is too large, so that the scan matching algorithm (which has like all ICP variants a limited convergence radius) cannot compensate for it. If this is really the problem, I would suggest to reduce minimumTravelHeading and minimumTravelDistance. Maybe it's also possible to reduce wheel slip by driving slower?
Thanks for the reply.
I am still unable to understand how continuous drift of odom and offset frame is expected in single robot scenario. I expected odom frame to be fixed with map frame. Since I am setting odometry_offset frame to be same as odom frame, they should remain identicle as well. But with time both frame become different.
Please find a dataset at this link(138MB). I am attaching the launch file I used to run nav2d_karto as well (mapper_teb.yaml, mapper.launch).
The following parameters were used for karto:
Mapper
grid_resolution: 0.25
range_threshold: 18.0
map_update_rate: 2
publish_pose_graph: true
max_covariance: 0.01
transform_publish_period: 0.1
min_map_size: 20
laser_frame: laser
laser_topic: scan
odometry_frame: odom
offset_frame: odom
map_topic: map
map_frame: map
robot_frame: base_link
robot_id: 1
Localizer
min_particles: 2500
max_particles: 10000
Karto
UseScanMatching: true
MinimumTravelDistance: 1.0
MinimumTravelHeading: 0.34 # twenty degree
ScanBufferSize: 50
ScanBufferMaximumScanDistance: 20
UseResponseExpansion: true
LoopSearchMaximumDistance: 8.0
CorrelationSearchSpaceDimension: 2
LoopSearchSpaceDimension: 10
LoopSearchMaximumDistance: 8
###########################################################
is it possible to just nav2d_exploration with move_base, without nav2d_navigator.
Also do nav2d_karo publishes transform map to base_link? I can see pose being published, but not sure about tf part.
Thanks again.
Odom and offset will always stay identical in a single robot case. But odom and map will be different, because the odometry will drift over time, so the localization has to compensate for this. Only if you had a perfect odometry with no drift, map and odom would be identical. So the transformation from map to odom shows, how much error has been accumulated by the odometry, (Assuming that the localization is correct of course.)
The exploration-strategies are plug-ins for the navigator, so they cannot be used without it. As explained above, the transformation from map to base_link is available in tf via the following chain:
map -> offset -> odom -> base_link
But this is all handled internally by tf, you can just query tf for the map -> base_link transformation and it will return the correct matrices.
I used your bag-file to build a map, which looks mostly okay. In your yaml you mixed the ros.yaml and the mapper.yaml, which you can't. The frame-names and topic-names have to be on top level, not in the mapper namespace. Please check the examples from nav2d_tutorials.
After a minute or so your robot starts a 360° turn in the large room, which is not reflected in the odometry at all. (You can verify this by replaying the bag-file without the mapper and visualizing it with RViz.) This of course breaks the map completely.
Thank you for explaining the concepts.
Yes, I understood about relation between odom and map. I wrongly typed stating that I hoped odom and map to be same. I actually intended to say that odom and offset should be same for single robot experiments but in some experiments where odom error occurs, I observe sudden drift between odom and offset frame.
Thanks for pointing out mistake in parameter description in yaml file.
I have moved all the frame to ros.yaml as mentioned in tutorial1.launch file. But now I can not keep offset frame and odom frame to error as a TF_SELF_TRASFORM error was reported. Currently
Yes, I also get similar output for first 104 sec of data from bag. After that mapper start consuming a lot of cpu because of which it is not able to register data [ rviz stopped display of odometry tract and laser readings].
I am hoping that loop closure detection will detect failure in odometry and correct the map. I now understand that odometry should be good enough for mapper to work satisfactorily. But I believe mapper should be made robust with the inputs from loop closure detection and optimization in case of odom failures. In attached rosbag, I have teleoperated robot to move in loops in same room. I am expecting algorithm to pick the place as same area and correct in case of odometry drifts. Should I introduced some code to handle such failure cases?
I have also observed that at some places odometry is not captured. And, I observe slippage at places of decline ramps between the rooms. This kind of surface I can not avoid for the robot. I hope to correct these some how. How have you handled such errors [movements not reported by the odometry] in your experiments?
I was able to observe tf between map and base_link using tf_echo binary from tf package.
Thanks again
Regards
That was a misunderstanding: I mean that the transformation from offset to odom will always be an Identity in the single robot case. The frame names have to be different in every case!
Loop closing is implemented already within OpenKarto and will trigger automatically if detected. You can visualize the vertices and edges topics in RViz to see when loop closures happen. It can compensate drift that has accumulated over a long time, but it cannot compensate for plain wrong odometry. And if your odometry reports that the robot stands still while it actually turns 360°, this is as far off as it can be.