leggedrobotics / se2_navigation Goto Github PK

View Code? Open in Web Editor NEW

402.0 14.0 81.0 61.74 MB

Pure Pursuit Control and SE(2) Planning

License: BSD 3-Clause "New" or "Revised" License

CMake 6.24% C++ 92.52% Python 1.24%

planning car vehicle pure-pursuit reeds-shepp control mobile-robots navigation ros se2-planning

se2_navigation's Introduction

Planning and control for car-like vehicles

Overview

Pure pursuit controller and Reeds-Shepp sampling based planner for navigation in SE(2) space. This package has been mainly intended for car-like vehicles, although it can be used with mobile bases too.

How is this package different from other SE(2) planning/control packages?

Provides planning and control for car like vehicles which are non-holonomic
Correctly handles both forward and reverse driving
Allows joint path and approach pose planning when we only know approximate target location
Comes with a minimal set of dependencies
Core algorithmics is separated from ros dependent code
Tested on real-hardware
Comes with a car example
Integrated with grid_map package
Features visualizatios and rviz planning interface
Easily extensible
Supports dynamic reconfigure for controller tuning

Warning: At the moment, this planning and control framework uses geometric planners which makes it unsuitable for high-speed driving. It is meant to be used for slow maneuvers, e.g. parking maneuvers.

Released under BSD 3-Clause license.

Author: Edo Jelavic

Maintainer: Edo Jelavic, [email protected]

Navigation with prius	Navigation with Menzi Muck M545

Navigation with Spacebok

Publications

The code inside this repo has been developed as a part of our research on autonomous precision harvesting. The paper can be found here. If you are using packages from se2_navigation, please add the following citation to your publication:

Jelavic, E., Jud, D., Egli, P. and Hutter, M., 2021. Towards Autonomous Robotic Precision Harvesting. arXiv preprint arXiv:2104.10110.

@article{jelavic2021towards,  
   title   = {Towards Autonomous Robotic Precision Harvesting},  
   author  = {Jelavic, Edo and Jud, Dominic and Egli, Pascal and Hutter, Marco},  
   journal = {arXiv preprint arXiv:2104.10110},  
   year    = {2021}
}

Documentation

This package is split into smaller units each of which features it's own README. Follow these links for more info:

Installation

Refer to car_demo for the details. Refer to approach_pose_planner if you are interested in approach pose planning.

Usage

Run the main demo with:
roslaunch car_demo demo_autonomous.launch
See car_demo for the details.

Coming soon

Mobile base demo

se2_navigation's People

Contributors

Stargazers

Watchers

Forkers

daoran tuandle soorajanilkumar min-chuir-park meychr ericwang1hitsz walid19 jdr-neu forrest-z collector-m ymhzzz 5730289021-nn huangvictorauto qqsskk wang-zheng-jiu ramonpereira movebot iefiac zeta1999 swxind shike1239 toutou205 fxprime casiarobot 13304490790 vega4 jediofgever lijian8 navigationcn zhangnage123 mutuducc jcmayoral ali-robot mgou123 oygx210 chengwei920412 metavai zsyxs0926 enginbozkurt yangkang411 hitxiaomi tongliuzhu bingyingl wzr6009 zdkhit fangzheng81 hi-mrchen gongyue666 michaelfyang mfkiwl zhuoligetu123 zhurenjiek25 idate96 runningchauncey barral-robotica-e-automacao roni-permana-saputra yinflight jiangchenxu liushuchun zheli1531913 zhangl2auto yulon-wang mpheng666 yinghaojia98 motionplanner yangbo18416 wangfy1 rainbowaurora rvsz-jiangfangfei jianglingxin gary-robotics hanruihua cnlancheng tshiamor ballonballon kinggreat24 xzealjumping wjpneu abendabrise arricle dtbinh

se2_navigation's Issues

PluginlibFactory

Fails to compute lookahead point

I noticed that the computation of the lookahead point fails, if the waypoints are too far spread out. Introducing intermediate points mitigates the problem.

Below are two images to illustrate the problem. They contain the illustrations as provided by this package (path waypoints in green, lookahead/anchor point in yellow and the robot state as a yellow arrow).

The error message for the failure case is:

[ INFO] [1600849981.064074339, 10.961000000]: [ExcavationStateMachine::run] Excavation state: driving
Going to driving state (received plan)
AckermannSteeringController: Failed to compute lookahead point.
[ERROR] [1600849981.064469624, 10.961000000]: [PurePursuitController::run] Failed to advance path tracker.

[BUILD FAILS] Error: ompl/geometric/planners/informedtrees/BITstar.h: No such file or directory

System Configuration
ROS Melodic on Ubuntu 18.04

Error

Errors     << se2_planning:make /home/adbidwai/catkin_ws/logs/se2_planning/build.make.002.log       
In file included from /opt/ros/melodic/include/ompl-1.4/ompl/base/ProjectionEvaluator.h:43:0,
                 from /opt/ros/melodic/include/ompl-1.4/ompl/base/StateSpace.h:43,
                 from /opt/ros/melodic/include/ompl-1.4/ompl/base/SpaceInformation.h:43,
                 from /opt/ros/melodic/include/ompl-1.4/ompl/base/Planner.h:40,
                 from /home/adbidwai/catkin_ws/src/se2_navigation/se2_planning/include/se2_planning/ompl_planner_creators.hpp:10,
                 from /home/adbidwai/catkin_ws/src/se2_navigation/se2_planning/src/ompl_planner_creators.cpp:8:
/opt/ros/melodic/include/ompl-1.4/ompl/base/GenericParam.h:152:104: warning: ISO C++11 requires at least one argument for the "..." in a variadic macro
                     OMPL_ERROR("At least one setter or getter function must be specified for parameter");
                                                                                                        ^
/home/adbidwai/catkin_ws/src/se2_navigation/se2_planning/src/ompl_planner_creators.cpp:9:10: fatal error: ompl/geometric/planners/informedtrees/BITstar.h: No such file or directory
 #include <ompl/geometric/planners/informedtrees/BITstar.h>
          ^~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
compilation terminated.
make[2]: *** [CMakeFiles/se2_planning.dir/src/ompl_planner_creators.cpp.o] Error 1
make[1]: *** [CMakeFiles/se2_planning.dir/all] Error 2
make: *** [all] Error 2
cd /home/adbidwai/catkin_ws/build/se2_planning; catkin build --get-env se2_planning | catkin env -si  /usr/bin/make --jobserver-fds=6,7 -j; cd -
....................................................................................................
Failed     << se2_planning:make                          [ Exited with code 2 ]                     
Failed    <<< se2_planning                               [ 1.7 seconds ]                            
Abandoned <<< approach_pose_planner                      [ Unrelated job failed ]                   
Abandoned <<< se2_planning_ros                           [ Unrelated job failed ]                   
Abandoned <<< car_demo                                   [ Unrelated job failed ]                   
Abandoned <<< approach_pose_planner_ros                  [ Unrelated job failed ]                   
[build] Summary: 10 of 15 packages succeeded.                                                       
[build]   Ignored:   None.                                                                          
[build]   Warnings:  None.                                                                          
[build]   Abandoned: 4 packages were abandoned.                                                     
[build]   Failed:    1 packages failed.                                                             
[build] Runtime: 2.5 seconds total.

run planning_benchmark node failed.

i had run the planning_benchmark node ,but it not run well.

benchmarking standard validator 
Done, success rate: 500/500
total duration: 45981 miliseconds
per planning query: 91.962 miliseconds

===================
foot print size:4  foot print points size:100
benchmarking lazy validator 
planning_benchmark: /usr/include/eigen3/Eigen/src/Core/DenseCoeffsBase.h:118: Eigen::DenseCoeffsBase<Derived, 0>::CoeffReturnType Eigen::DenseCoeffsBase<Derived, 0>::operator()(Eigen::Index, Eigen::Index) const [with Derived = Eigen::Matrix<float, -1, -1>; Eigen::DenseCoeffsBase<Derived, 0>::CoeffReturnType = const float&; Eigen::Index = long int]: Assertion `row >= 0 && row < rows() && col >= 0 && col < cols()' failed. core dumped.

Then i run it in gdb:

#0  __GI_raise (sig=sig@entry=6) at ../sysdeps/unix/sysv/linux/raise.c:51
#1  0x00007ffff657e921 in __GI_abort () at abort.c:79
#2  0x00007ffff656e48a in __assert_fail_base (fmt=0x7ffff66f5750 "%s%s%s:%u: %s%sAssertion `%s' failed.\n%n", assertion=assertion@entry=0x7ffff6f7b360 "row >= 0 && row < rows() && col >= 0 && col < cols()", 
    file=file@entry=0x7ffff6f7b2f8 "/usr/include/eigen3/Eigen/src/Core/DenseCoeffsBase.h", line=line@entry=118, 
    function=function@entry=0x7ffff6f7b9a0 <Eigen::DenseCoeffsBase<Eigen::Matrix<float, -1, -1, 0, -1, -1>, 0>::operator()(long, long) const::__PRETTY_FUNCTION__> "Eigen::DenseCoeffsBase<Derived, 0>::CoeffReturnType Eigen::DenseCoeffsBase<Derived, 0>::operator()(Eigen::Index, Eigen::Index) const [with Derived = Eigen::Matrix<float, -1, -1>; Eigen::DenseCoeffsBas"...) at assert.c:92
#3  0x00007ffff656e502 in __GI___assert_fail (assertion=0x7ffff6f7b360 "row >= 0 && row < rows() && col >= 0 && col < cols()", file=0x7ffff6f7b2f8 "/usr/include/eigen3/Eigen/src/Core/DenseCoeffsBase.h", 
    line=118, 
    function=0x7ffff6f7b9a0 <Eigen::DenseCoeffsBase<Eigen::Matrix<float, -1, -1, 0, -1, -1>, 0>::operator()(long, long) const::__PRETTY_FUNCTION__> "Eigen::DenseCoeffsBase<Derived, 0>::CoeffReturnType Eigen::DenseCoeffsBase<Derived, 0>::operator()(Eigen::Index, Eigen::Index) const [with Derived = Eigen::Matrix<float, -1, -1>; Eigen::DenseCoeffsBas"...) at assert.c:101
#4  0x00007ffff6f61aa5 in Eigen::DenseCoeffsBase<Eigen::Matrix<float, -1, -1, 0, -1, -1>, 0>::operator() (this=0x55555585e618, row=-1, col=390) at /usr/include/eigen3/Eigen/src/Core/DenseCoeffsBase.h:117
#5  0x00007ffff6f609f4 in se2_planning::isInCollision (state=..., footprint=std::vector of length 100, capacity 160000 = {...}, gridMap=..., obstacleLayer="occupancy")
    at /home/zk/workspace/se2_nav_ws/src/se2_navigation/se2_planning/src/GridMapLazyStateValidator.cpp:106
#6  0x00007ffff6f603db in se2_planning::GridMapLazyStateValidator::isStateValid (this=0x5555558344f0, state=...)
    at /home/zk/workspace/se2_nav_ws/src/se2_navigation/se2_planning/src/GridMapLazyStateValidator.cpp:67
#7  0x00007ffff6f6ea3b in se2_planning::OmplReedsSheppPlanner::isStateValid (this=0x7fffffffd840, si=0x5555557850d0, state=0x55555583ee60)
    at /home/zk/workspace/se2_nav_ws/src/se2_navigation/se2_planning/src/OmplReedsSheppPlanner.cpp:78
#8  0x00007ffff6f6bf78 in se2_planning::OmplPlanner::<lambda(const ompl::base::State*)>::operator()(const ompl::base::State *) const (__closure=0x555555784250, state=0x55555583ee60)
    at /home/zk/workspace/se2_nav_ws/src/se2_navigation/se2_planning/src/OmplPlanner.cpp:62
#9  0x00007ffff6f6c904 in std::_Function_handler<bool(const ompl::base::State*), se2_planning::OmplPlanner::initialize()::<lambda(const ompl::base::State*)> >::_M_invoke(const std::_Any_data &, const ompl::base::State *&&) (__functor=..., __args#0=@0x7fffffffd140: 0x55555583ee60) at /usr/include/c++/7/bits/std_function.h:302
#10 0x00007ffff765085c in ?? () from /opt/ros/melodic/lib/libompl.so.15
#11 0x00007ffff75ffbf5 in ompl::base::ReedsSheppMotionValidator::checkMotion(ompl::base::State const*, ompl::base::State const*) const () from /opt/ros/melodic/lib/libompl.so.15
#12 0x00007ffff77bc628 in ompl::geometric::RRTstar::solve(ompl::base::PlannerTerminationCondition const&) () from /opt/ros/melodic/lib/libompl.so.15
#13 0x00007ffff76174b2 in ompl::base::Planner::solve(double) () from /opt/ros/melodic/lib/libompl.so.15
#14 0x00007ffff7820446 in ompl::geometric::SimpleSetup::solve(double) () from /opt/ros/melodic/lib/libompl.so.15
#15 0x00007ffff6f6bde8 in se2_planning::OmplPlanner::plan (this=0x7fffffffd840) at /home/zk/workspace/se2_nav_ws/src/se2_navigation/se2_planning/src/OmplPlanner.cpp:38
#16 0x00007ffff6f6e7d2 in se2_planning::OmplReedsSheppPlanner::plan (this=0x7fffffffd840) at /home/zk/workspace/se2_nav_ws/src/se2_navigation/se2_planning/src/OmplReedsSheppPlanner.cpp:56
#17 0x00005555555635d2 in runPlanner (planner=...) at /home/zk/workspace/se2_nav_ws/src/se2_navigation/se2_planning/src/planning_benchmark.cpp:45
#18 0x0000555555563a0a in runLazyValidator (planner=..., gridMap=...) at /home/zk/workspace/se2_nav_ws/src/se2_navigation/se2_planning/src/planning_benchmark.cpp:71
#19 0x000055555556409f in main (argc=1, argv=0x7fffffffdaf8) at /home/zk/workspace/se2_nav_ws/src/se2_navigation/se2_planning/src/planning_benchmark.cpp:119

I teied to solve this problem,but it took a long time.Thank you very much for helping me solve this problem.

[Nav2] Potential interest in homologating with Nav2?

Hi,

I've seen this around and thought now would be a good time to start a dialog. We in Nav2 have been hard at work to support many new types of robots that previously did not get first-class support in ROS's flagship navigation project.

Since maybe the last time you took a look, we now support a highly-optimized Hybrid A* implementation supporting forward-only and forward-backward motion, as well very soon to have a Pure Pursuit implementation that also has a bunch of new bells and whistles.

It looks like this work is well aligned with our interests as well in supporting legged robots and ackermann cars and might be worth joining forces to take the best parts of both of our work. In the second half of 2020 I'd like to also work on publishing a follow up paper on the nav2 intro paper showing full-fledged quadruped support and examples of behavior trees for quadruped applications.

Steve