Open WilliamWoo45 opened 4 years ago
Hello :)
The default configurations of drl should be generic enough to be usable for your 3 DoF localization use case. By default, the global localization pipeline uses geometric features for finding the robot initial pose and then relies on point cloud matching algorithms for tracking the robot pose. You can customize the pipeline and its parameterization for your particular use case. The parameters are listed in yaml/schema/drl_configs.yaml. An overview of the pipeline is given in docs/perception-overview.pdf and described in my dissertation and associated papers. The discussion in issue #14 may also be helpful.
Let me known how drl performed in your use case.
P.S. Before testing drl, please do a git pull to get the last set of commits and then recompile.
Have a nice day :)
Good morning Carlos! Thanks for your speedy reply.
I've specified the critical parameters and implemented the algorithm following your suggestions.
I opened four command lines in the following order:
(1) roslaunch dynamic_robot_localization dynamic_robot_localization_system.launch
(2) rosrun map_server map_server /opt/ros/kinetic/share/husky_navigation/maps/mymap.yaml
(3) rosbag play --clock /home/williamwoo/Z_husky_S1-B4/husky-tf-scan-07-02-B-block-amcl-way1.bag
(4) rviz
However, there seems to be a problem with the missing TFs between [laser] and [odom]
and there is no robot showing in the Rviz.
Launch command line screenshots:
Rviz:
The tf_echo odom laser
:
The tf tree, modified launch file, .yaml file, .pgm file, and 3 DoF localization rosbag are uploaded for your reference.
Any idea for this problem?
Cheers :)
Best Regards, William
Good night :)
I looked at the files you uploaded and tested the rosbag. You were close, you just needed to set the sim time parameter and add a static transform for the missing TF before running the rosbag play:
roscore
rosparam set /use_sim_time true
rosrun tf2_ros static_transform_publisher 0 0 0 0 0 0 odom base_link
Then:
cd Carlos_Dynamic_Robot_Localization
rviz
rosrun map_server map_server mymap.yaml
roslaunch dynamic_robot_localization_system.launch
rosbag play --clock husky-tf-scan-07-02-B-block-amcl-way1.bag
drl uses the ros tf conventions for navigation, which state that:
In your case, if you do not have odometry information, you just specify an identity transformation with a static transform publisher.
Have a nice day :)
Hi Carlos! Thanks for your detailed explanation.
I followed your steps and there is no more warning about the tf between laser and odom
. However, I think there are still some problems because:
Rviz-LaserScan-Transform
For frame [laser]: No transform to fixed frame [map]. TF error: [Lookup would require extrapolation into the future. Requested time 1562070669.875814000 but the latest data is at time 1562070668.289503000, when looking up transform from frame [laser] to frame [map]]
EuclideanTransformationValidator rejected new pose at time 1562070670.157474292 due to root_mean_square_error/max_outliers_percentage
):
I try to fine tune the parameters following your suggestions about initial algorithms parameterization. However, the results seem no difference. I even manually set the ground truth initial 3DoF pose in the dynamic_robot_localization_system.launch
, but the result still looks no difference.
I also tried another repetitive environment and the problems are kind of the same:
So I'm wondering is there any parameter that need specific tuning? Or is it due to the reason that the experimental corridor environment is too repetitive/ambiguous for the robot to match the features?
Best Regards, William
Hello :)
drl publishes two topics with the inliers and outliers that you can view in rviz (inliers -> points that are considered correctly aligned, outliers -> sensor points that are too far from any map data). This can give you a good idea of how drl evaluates the overlap of the sensor data with the map.
For initial alignment, the critical parameters are the keypoint detector and descriptor, along with the sample_consensus_initial_alignment modules.
For tracking, the critical parameters are the correspondence distance, the maximum number of iterations and the time limit given to iterative closest point.
If you have a sensor with a lot of noise, you may need to increase the distance of the outlier detector.
If you expect that the robot will only see a small part of the map (due to occlusions, people walking around), you should increase the maximum allowed percentage of outliers (1 -> 100%, I do not recommend set it higher than 0.9 -> I think an overlap of at least 10% is required -> the confidence in any given map matching localization system is inversely proportional to the % of outliers -> the higher the overlap between sensor data and the map, the most confidant you can be that the localization system is working properly).
For testing the tracking only, you can give an initial alignment pose using rviz (in the map tf frame).
You can see which modules are being loaded in the dynamic_robot_localization_system.launch, which by default uses:
More information about the parameters is available in drl_configs.yaml.
You can adjust the logging level of drl and pcl using these parameters.
Have a nice day,
Good afternoon Carlos! Thanks again for your speedy reply.
I've looked through all your suggestions. Then I've recorded a new rosbag including the odometry topic as /husky_velocity_controller/odom
and fine tuned the parameters mentioned by you.
Besides, I also updated my roslaunch
file, especially the initial pose and the odometry topic.
However, the results seem no difference with the previous one. These is still warnings about root_mean_square_error
and Not enough correspondences found
. Besides, there is no tracking and laser scans showing in Rviz
.
So I'm wondering what could be the possible solutions for my case? Using the same rosbag, I've included a video of implementing the AMCL algorithm with the specified ground truth initial pose. I was hoping I could achieve something similiar with your algorithm, or even better than AMCL.
You can adjust the logging level of drl and pcl using these parameters
By the way, I'm wondering what is the function of the logging level? I tried to adjust them, but the results seem no difference.
Cheers! :)
Best Regards, William
Good night :)
drl follows the ros navigation recommendations and since it does not need the twist information present in nav_msgs/Odometry, it retrieves the 6 DoF pose of the odometry from the tf base_link -> odom As such, you need a node to subscribe to the odometry topic and publish the associated tf. You can use the pose_to_tf_publisher for that, example of launch file below:
<?xml version="1.0" encoding="UTF-8"?>
<launch>
<include file="$(find pose_to_tf_publisher)/launch/pose_to_tf_publisher.launch">
<arg name="odometry_topic" default="/husky_velocity_controller/odom"/>
<arg name="map_frame_id" default="odom"/>
<arg name="odom_frame_id" default=""/>
<arg name="base_link_frame_id" default="base_link"/>
<arg name="publish_rate" default="-1"/>
<arg name="publish_initial_pose" default="false"/>
<arg name="transform_pose_to_map_frame_id" default="false"/>
</include>
</launch>
The odometry parameter present in the dynamic_robot_localization_system.launch was for the laserscan_to_pointcloud assembler adjust how many scans it assembles for each point cloud it publishes (which is subscribed by drl). Since you have only 1 laser, you should configured it for assembling just 1 scan when the robot is moving and allowed it to merge several scans when the robot is stopped or moving slowly. If you do not want to use this feature, which is highly dependent on the accuracy of your odometry information, just disable it by setting the odometry topic to an empty string.
You must not set the odometry topic for the pose_to_tf_publisher. This node must only publish the tf associated with the msgs coming from the drl localization_pose or from the rviz \initialpose.
Below is a video showing how drl performed in the second bag file (drl stopped tracking when the robot reached the wall because the laser scans in the \scan topic were missing temporally for a few seconds). https://www.dropbox.com/s/u35jk0vqhnrzot7/2020-06-11-B4-test-drl.mp4?dl=0
You can see that drl is much more accurate and efficient than amcl (when using lookup tables, drl took less than 5 ms to process your laser data -> you can inspect this information in the localization_times topic). amcl uses a completely different approach to localization, namely, amcl relies on a particle filter and statistics / heuristics for choosing the best particle and then rearrange new particle candidates as the robot moves. It is also very tricky to configure its odometry coefficients., especially if your robot wheels have a lot of slippage. drl on the other hand uses point cloud matching algorithms that were built for accurate and efficient alignment of point clouds, but they require a rough alignment to converge to a good solution. That is why drl uses the iterative closest point algorithm (ICP) for accurate and efficient local tracking and feature matching for estimating a rough initial pose of the robot when the tracking is lost. For best accuracy the map / occupancy grid should have walls that are just 1 pixel thick. Your map has walls with several pixels thick. This may cause the matching algorithms to oscillate slightly, because the laser scan matching can slide within the pixels of the walls.
The logging levels allow you to control the level of detail of the messages that are shown in the terminal. Debug level will show you verbose output, giving you detailed information about drl operations in each module, while fatal level will pretty much not show you any output at all from drl (you can also set the logging level within the pcl code).
P.S. pose_to_tf_publisher discards poses in the past, and as such, before restarting the rosbag play, you need to restart the launch files of pose_to_tf_publisher (for the odometry tf) and drl.
Have a nice day :)
Hi Carlos! Thanks for your detailed guidance.
Following your suggestions, I've removed the odometry_topic in dynamic_robot_localization_system.launch
and configured the number of laser scans to one.
<?xml version="1.0" encoding="UTF-8"?>
<launch>
<include file="$(find pose_to_tf_publisher)/launch/pose_to_tf_publisher.launch">
<arg name="odometry_topic" default="/husky_velocity_controller/odom"/>
<arg name="map_frame_id" default="odom"/>
<arg name="odom_frame_id" default=""/>
<arg name="base_link_frame_id" default="base_link"/>
<arg name="publish_rate" default="-1"/>
<arg name="publish_initial_pose" default="false"/>
<arg name="transform_pose_to_map_frame_id" default="false"/>
</include>
</launch>
I was wondering, should it be <arg name="map_frame_id" default="map"/>
and <arg name="odom_frame_id" default="odom"/>
? Besides, I've put this into a new launch file named odom.launch under the route ~/catkin_ws_drl/src/pose_to_tf_publisher/launch
. Then I run the command lines in the following order:
roscore
rosparam set /use_sim_time true
rosrun tf2_ros static_transform_publisher 0 0 0 0 0 0 odom base_link
roslaunch pose_to_tf_publisher odom.launch
roslaunch dynamic_robot_localization dynamic_robot_localization_system.launch
rosrun map_server map_server /opt/ros/kinetic/share/husky_navigation/maps/mymap.yaml
rviz
rosbag play --clock /home/williamwoo/Documents/husky2-pose-_2020-06-11-B4-test1-new.bag
Now I can see the laser scans moving in the Rviz
.
However, the laser scans still do not match with the map, thus the localization is still considered to be failed. So I'm curious how did you tune the parameters, perform DRL on my rosbag file and obtain the good results as shown in your video https://www.dropbox.com/s/u35jk0vqhnrzot7/2020-06-11-B4-test-drl.mp4?dl=0 ?
Is it ok for you to share your launch files and .yaml files?
Besides, I'm also wondering where exactly should I set the initial pose? Is that I only need to set in dynamic_robot_localization_system.launch or should I also set it in pose_to_tf_publisher.launch? Or should I manually set it in the Rviz interface by using the 2D Pose Estimate
arrow?
Thanks a lot :)
Best Regards, William
Good night :)
The pose_to_tf_publisher package was initially developed for publishing the tf odom -> map from pose stamped msgs (which led to the names you see in the launch file), but it evolved to publish other things, namely tf from odometry, which should be between tf frames base_link -> odom I left the names of the arguments as map, odom and base_link because that is the typical use case (but they can be configured to other frames). For publishing the tf base_link -> odom from the odometry messages, please use the configurations that I specified earlier.
You also cannot have two sources of tf base_link -> odom You either use odom.launch or use the static_transform_publisher. If you use both, then the tf base_link -> odom will be oscillating between the identity pose and the cumulative pose from the wheel encoders. This will cause issues in tracking the robot pose. The odometry information must be stable and evolve over time in a continuous way, without sudden jumps.
I did not tune drl for your rosbags. I made the video with the configurations that I have used in the past for our mobile platforms, which are configured by default in the dynamic_robot_localization_system.launch
You can set the initial pose of the robot in its respective section in dynamic_robot_localization_system.launch, which will forward it to drl and pose_to_tf_publisher. While drl is running, you can also set it in rviz using the 2d pose estimate arrow (rviz must be in the map frame and publishing it to /initialpose).
Have a nice day :)
Hi Carlos:
Good Monday! Thanks for your patient explanations. Sorry for the delayed reply because of the closure of my lab on the weekend due to the COVID-19.
I've looked through your suggestions. I choose to use the odom.launch instead of the static_transform_publisher
. Hence, the command lines are adjusted and implemented in the following order:
roscore
rosparam set /use_sim_time true
roslaunch pose_to_tf_publisher odom.launch
roslaunch dynamic_robot_localization dynamic_robot_localization_system.launch
rosrun map_server map_server /opt/ros/kinetic/share/husky_navigation/maps/mymap.yaml
rviz
rosbag play --clock /home/williamwoo/Documents/husky2-pose-_2020-06-11-B4-test1-new.bag
However, I still can not get similar localization result as yours. I found the laser scans moving but sometimes oscillating. See below:
I'm still wondering what could be the possible reason for the different localization performances with the same rosbag on your side and on my side. I can not figure this point out. Am I lefting any configuration behind? I also tried to record a new rosbag. But the localization performance seems no difference with the previous one.
Have a nice day! :)
Best Regards, William
Good night :)
First, confirm that you have the latest version of drl:
roscd dynamic_robot_localization
git pull
catkin build
Then, be careful how you start the launch files. If you have the launch files in the Carlos_Dynamic_Robot_Localization folder, then you should:
cd Carlos_Dynamic_Robot_Localization
roslaunch odom.launch
roslaunch dynamic_robot_localization_system.launch
Otherwise you might not be running the launch files that you modified. I retested again in my laptop and it all seems to be working as expected and similar to the video I made previously.
The oscillations seen in the video I made (from 10s to 15s) were due to the laser field of view changing drastically because the wall in front of the robot temporary became invisible to the lidar, and since the robot was seeing just the two parallel corridor walls, one degree of freedom became unconstrained by the sensor data, allowing the matching algorithms to slide the sensor data along the corridor. You can see that in the beginning of the video the oscillations did not happen because the sensor data had several wall corners that constrained the 3 DoF pose of the robot, allowing stable matching. For minimizing the oscillations further, the map should have walls with 1 pixel thick, otherwise the matching algorithms may oscillate the sensor data within the walls.
You can set the ros_verbosity_level to DEBUG for seeing in the terminal what drl is doing and diagnose the problem and if necessary also set pcl_verbosity_level to DEBUG.
You should known that drl was preconfigured with timeouts for the registration algorithms:
If your PC is less powerful than mine, then you may need to increase these timeouts for giving more time to the algorithms to converge to better solutions and tune the convergence thresholds, such as:
Have a nice day :)
Good Morning Carlos!
First of all, sincerely thanks again for your detailed and patient explanations.
I followed and tried your suggestions. However, I still can not obtain the ideal localization results on my laptop with AMD FX-9830P RADEON R7 CPU
and 32GB memory
. I set both the ros_verbosity_level
and pcl_verbosity_level
to DEBUG to diagnose the possible problem, but the DEBUG results look quite ok without any serious warnings, screenshots as follows:
I've also recorded the whole DEBUG process, which can be seen in here
I think probably it is because of my laptop is not powerful enough to implement the DRL. Hence, I borrowed another laptop, installed and implemented DRL. It works!!! :)
Now I have another question. I found that if the initial pose is not given, it will be quite hard for the DRL to successfully localize the robot. I've recorded another two rosbag, namely the test2 and test3. The ground truth initial pose for these two rosbags are (44, 18.8, 0) and (1.5, 8, 0). If with initial pose, DRL can quickly and accurately localize the robot, as shown: test2:
test3:
If without initial pose, DRL fails to localize the robot in most cases (more than 90% will fail on my laptop), as shown: test2:
test3:
So I'm wondering, is it because of the feature matching technique that DRL uses that performs poorly in such repetitive and ambiguous environments? Could you run these two rosbags without giving the initial pose on your laptop and check the performance of DRL ?
Thanks again and cheers!
Best Regards, William
Hello :)
I made some improvements to drl and its default configurations. drl should now work better at feature matching for maps created using slam.
roscd dynamic_robot_localization
git pull
roscd pose_to_tf_publisher
git pull
catkin build
However, feature matching is more reliable for small maps with unique environment geometry. Large and repetitive environments are not ideal for feature matching because they have similar feature descriptors scattered throughout the environment. If you crop your map with gimp to the regions in which you have sensor data in the rosbags, will you see that drl reliability to estimate the robot initial pose will increase significantly.
While testing the new rosbags and using rviz to set the initial pose, I notice that you need to add to odom.launch the line below, otherwise the pose_to_tf_publisher that is sending the tf base_link -> odom will receive the initial pose from rviz and cause problems.
<arg name="pose_with_covariance_stamped_topic" default="" />
I also added the option to disable the discard of older poses. This way, you can restart the rosbags without needing to restart drl and odom.launch For that you need to add:
<arg name="discard_older_poses" default="false" />
to:
According to these benchmarks, my CPU is around 40% faster than yours.
But you can tune drl to use less CPU, for example:
/dynamic_robot_localization/reference_pointcloud
Have a nice day :)
Good Afternoon Carlos!
Thanks a lot for your step by step guidance and explanations.
Following your suggestions:
git pull
and updated the drl
with its configurations.<arg name="pose_with_covariance_stamped_topic" default="" />
and <arg name="discard_older_poses" default="false" />
to the corresponding launch files. Intel-Core-i7-6700HQ, 4 Cores, 8 Threads @2.6 GHz
), thus I no longer need to worry about the high CPU consumption problem. The drl
now works pretty nice on the borrowed laptop.As for the localization problem in repetitive environments, I agree that:
feature matching is more reliable for small maps with unique environment geometry. Large and repetitive environments are not ideal for feature matching.
However, I can not crop the map with gimp to the regions in which I have sensor data in the rosbags because in the real Global localization case no one knows where exactly is the robot (which is what I suppose the Global localization algorithm is doing).
As another way to test drl
, I did input the ground-truth initial pose to drl
and found that drl
works pretty nice in almost all the waypoints (i.e. where the robot starts the global localization) that I randomly selected, which demonstrated its perfect pose-tracking capability. But if without accurate initial pose, drl
only has about 20~30% successful localization cases. This is also the motivation for me to develop new global localization algorithm to cope with the repetitive environments.
Thanks again!
Best Regards, William
Hello :)
I developed and tested drl initially for ship building environments within the FP7 Carlos project. In the project use cases the robot would be operating inside a large room.
In your use case, you have the robot moving inside long corridors of a large building, so a different approach to global localization will likely be required, which means you have the opportunity to research in this area :)
As a quick test, you could try other algorithms for global localization, for example running amcl in the beginning until it converged to an acceptable solution and then send the initial pose of amcl to drl for achieving better tracking accuracy. If for some reason drl loses tracking after a while, you could revert to amcl for finding a new initial pose, which later on would be used by drl. This approach would be a supervisor for 2 localization systems (for alternating between the 2).
Have a nice day :)
Hi Carlos.
Sincerely thanks again for all your help and guidances!
The approach of combining amcl
and drl
seems doable. I'm willing to try it.
Take care and all the best :)
Best Regards, William
Hello :)
Let me know how drl performed when integrated with amcl or your own algorithms. It's always great to have feedback from the ros community.
All the best :)
Hi Carlos! @carlosmccosta
How are you recently? I hope everything's fine.
In recent days, I'm trying to implement other 2-D laser-based global localization methods apart from your DRL
, the AMCL
, Google Cartographer, MRPT localization . Do you have any recommendation for other open source algorithm?
Thanks a lot :)
Best Regards, William
Hello :)
A few links as a starting point:
If you find packages that work well for global localization, please let me known :)
Have a nice day :)
Dear Carlos,
Noted with thanks! I'll let you know if I find it.
Thanks and take care!
Best Regards, William
@WilliamWoo45 and @carlosmccosta
So I'm wondering, is it because of the feature matching technique that DRL uses that performs poorly in such repetitive and ambiguous environments? Could you run these two rosbags without giving the initial pose on your laptop and check the performance of DRL ?
Hello ,I am also facing a similar issue of repetitive and ambiguous environments and also featureless environments,it would be really gratful of you ,if by any chance you have come across any localization algorithm which performs well in such scenarios.if you could just provide a link ,it would be nice.(The above links mentioned earlier,do not really fit my case.) Thank you
Hi Carlos!
First of all, thanks for sharing your research work, which is quite fascinating.
I've successfully installed from the source and recently I'm looking through your codes. I try to implement your algorithm on my own 2-D laser data to accomplish 3 DoF (position_x, position_y, angle_yaw) localization tasks. However, I have a few questions which I hope you could clarify for me:
By the way, I wrongly posted the issue #17 and you can delete it. Sorry about this.
Thanks a lot and looking forward to your early reply!
Best Regards, William