vorpalblade / quickmcl Goto Github PK

It might be worth it to move scan sub-sampling earlier.

Currently it happens in MapLikelihood::update_importance, but at this point the point cloud has already been transformed via TF into a new coordinate frame.

Look into the possibility of doing this sub-sampling much earlier. It would require a rewrite of the laser_handler.cpp code, since currently this happens very early (in the call projector.transformLaserScanToPointCloud in LaserSub<sensor_msgs::LaserScan>::laser_callback, or externally for point clouds).

Hi,

I'm a maintainer / developer on ROS2 Navigation [1] and was curious about this. I saw you posted on Navigation and in my glancing around I found your blog on it. There's definitely some work to be done on it, but I think its a good starting point to be a candidate for replacement of AMCL in ROS2 Navigation stack, if you're interested. I would be open to helping you finish it up to be feature parity and move it into the later stack.

The big things questions I'd have are:

• You mention not testing KLD sampling, but I see it implemented. Can you explain more about why you didn't test against it? Just want to make sure feature-wise it works, regardless of the specific determinism of the memory/CPU benchmarks.
• Did you do localization accuracy testing in simulation (or ground truth hardware)? I'd want to know that it works on the order as well. Did you test that your implementation correctly was able to recover from delocalization?
• Other than motion models/beam skipping, anything else missing? Those are things I can do pretty fast.

Finally, why GPL-3? I admit, I'm a fan of relatively restrictive licenses for my application-level code [2] but since this is lower-level and people need to build on top of it, that's going to be quite an up-hill battle to get people to use it. I use LGPL-2.1 when building very difficult application-level code because its the most restrictive (give back if you improve it) license that companies will actually accept. However if it wasn't some new technique or non-application level I'm usually running Apache/BSD. GPL-3 for many companies is a non-starter [3]. It would be monumentally easier of a sell if this was available as Apache-2.0 (which still leaves IP + ability to patent should there be something here) or BSD (very very permissive, good baseline if you don't actually care). Given you're the only author, if this interests you, the process is trivial.

[1] https://github.com/ros-planning/navigation2
[2] https://github.com/SteveMacenski/slam_toolbox
[3] https://www.cnet.com/news/torvalds-no-gpl-3-for-linux/

Clean up private benchmark code & create a new repository for the code in question.
Also put a link to it in the README.

Add some test that starts up the entire node and performs some basic testing.

It might be useful (to be a drop in replacement for AMCL) to be able to subscribe to /scan directly as an option. Would also make comparing CPU and memory usage fairer.

Check if using laser_geometry for this is suitable or if it is better to hand roll a custom solution like AMCL is doing.

https://github.com/VorpalBlade/ros_spatial_utils has code split out from this project. QuickMCL should use that code, and the duplication should be removed.

Main issue is to figure out how to make this work with the CI system. Maybe it would be better to have it as multiple projects in the same repository from that point of view?

Sometimes it only takes effect on every other message. May be related to the fix for #5.

Currently it is assumed in LaserSub<sensor_msgs::PointCloud2>::laser_callback in laser_handler.cpp that the point cloud is already in the proper reference frame (base_link). It would be nice to support transforming it with TF2.

CI appears to have broken during the last year. Build fails with the following, which doesn't make much sense:

quickmcl: Cannot locate rosdep definition for [rosunit]

Apparently CircleCI runs out of memory when compiling. Look into a way to reduce the -j option to make, or switch to a different CI.

The motion looks smoother in rviz with AMCL. Figure out why and fix it.

Implement dynamic reconfiguration for parameters it is feasible for.

Put up a page on the ROS wiki about QuickMCL.

Would be good to add support (similar to AMCL) for initial pose via parameters. Maybe also write back current estimate to the parameters (similar to AMCL) to facilitate restart.

Set up CI, code coverage, code quality analysis etc.

Hello, I wanted to replace AMCL with this package, but I had a problem.In the process of robot moving, each laser processing takes a lot of time, which leads to positioning delay. But I put likelihood_ num_ When beams is turned on, the delay problem will be alleviated, but it still can't meet the requirements, which is completely different from AMCL. Is this normal, please? What can I do to change that? Hope to get your help ，thanks. @VorpalBlade

[ INFO] [1622604936.906454767]: Sensor model took 515 ms
[ INFO] [1622604936.909437357]: Laser cloud processing took 521 ms
[ INFO] [1622604936.913094327]: Laser cloud processing took 3 ms
[ INFO] [1622604936.916572128]: Laser cloud processing took 3 ms
[ INFO] [1622604936.919894154]: Laser cloud processing took 3 ms
[ INFO] [1622604936.923857023]: Laser cloud processing took 3 ms
[ INFO] [1622604936.927586915]: Laser cloud processing took 3 ms
[ INFO] [1622604937.000761051]: Laser cloud processing took 12 ms
[ INFO] [1622604937.090635198]: Laser cloud processing took 3 ms
[ INFO] [1622604937.201118132]: Laser cloud processing took 13 ms
[ INFO] [1622604937.290968898]: Laser cloud processing took 3 ms
[ INFO] [1622604937.400704508]: Laser cloud processing took 12 ms
[ INFO] [1622604937.490612819]: Laser cloud processing took 3 ms
[ INFO] [1622604937.599952569]: Laser cloud processing took 11 ms
[ INFO] [1622604937.699796460]: Laser cloud processing took 11 ms
[ INFO] [1622604937.800352997]: Laser cloud processing took 12 ms
[ INFO] [1622604938.355880810]: Sensor model took 467 ms
[ INFO] [1622604938.359256935]: Resampling took 3 ms