search

leggedrobotics / se2_navigation

Pure Pursuit Control and SE(2) Planning
BSD 3-Clause "New" or "Revised" License
442 stars 84 forks source link

run planning_benchmark node failed. #5

Closed learnonroad closed 3 years ago

learnonroad commented 3 years ago

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.

jelavice commented 3 years ago

Hi learnedonroad, thanks for pointing this problem out. I will have a look into this next week and get back to you

learnonroad commented 3 years ago

thanks a lot

jelavice commented 3 years ago

Hi @learnonroad I just ran the planning_benchmark however, I cannot reproduce the issue. Could you push your branch in case you modified something? Or provide me with some instructions on how to reproduce the problem?

Which Eigen version are you using?

tomlankhorst commented 3 years ago

Looks like an out-of-bounds matrix read

jelavice commented 3 years ago

Can you reproduce it @tomlankhorst ?

learnonroad commented 3 years ago

I did not make any changes to the code.My software env is: ubuntu18.04 ros-melodic sudo apt-get install ros-melodic-desktop-full eigen3 3.3.4:sudo apt-get install libeigen3-dev gtest sudo apt-get install libgtest-dev boost ompl 1.4 sudo apt install ros-melodic-ompl when I ran the catkin build，there are some warnings

CMake Warning at /home/zk/workspace/se2_test/src/se2_navigation/se2_planning/CMakeLists.txt:113 (find_package):
  By not providing "Findcmake_clang_tools.cmake" in CMAKE_MODULE_PATH this
  project has asked CMake to find a package configuration file provided by
  "cmake_clang_tools", but CMake did not find one.

  Could not find a package configuration file provided by "cmake_clang_tools"
  with any of the following names:

    cmake_clang_toolsConfig.cmake
    cmake_clang_tools-config.cmake

  Add the installation prefix of "cmake_clang_tools" to CMAKE_PREFIX_PATH or
  set "cmake_clang_tools_DIR" to a directory containing one of the above
  files.  If "cmake_clang_tools" provides a separate development package or
  SDK, be sure it has been installed.

I did not konw how to fix it.

I don't konw if the above warnings will affect my running results. Maybe I should try to compile Eigen from source?Or can you tell me your operating env?

jelavice commented 3 years ago

I can reproduce the error. It'll be fixed by Thu. Don't worry about the warnings for now.

jelavice commented 3 years ago

Hi @learnonroad ,

Thank you for your patience. I added this pull request which should address the issue you opened. Could you please verify that your problem does no longer happen?

learnonroad commented 3 years ago

@jelavice It runs successfullt,thank you very much.

jelavice commented 3 years ago

For the future, you might want to build in release mode since the code runs much faster then. See README in se2_planning.