Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

[WIP] Informed rrtstar #3

Open
wants to merge 27 commits into
base: main
Choose a base branch
from
Open

[WIP] Informed rrtstar #3

wants to merge 27 commits into from

Conversation

HiroIshida
Copy link
Owner

@HiroIshida HiroIshida commented May 6, 2022

ros2 porting作業

cost の変数が空なのなおす
hasFeasibleSolutionの復活
テストポーティング

pointer まわりなおす

startNodeでpushしたときに, queueはcomparisonがおきてしまい昔のpointerを参照してしまう。
ので初期化しないといけない.

h-ishida@stonet4:~/pilot-auto/build/freespace_planning_algorithms [(HEAD detached at 62a5ce98)]
$ ./freespace_planning_algorithms-test 
[==========] Running 2 tests from 1 test suite.
[----------] Global test environment set-up.
[----------] 2 tests from AstarSearchTestSuite
[ RUN      ] AstarSearchTestSuite.SingleCurvature
plan success : 182.221[msec]
=================================================================
==60867==ERROR: AddressSanitizer: heap-use-after-free on address 0x625001b5da60 at pc 0x7f57906db633 bp 0x7fffe63c2dc0 sp 0x7fffe63c2db0
READ of size 8 at 0x625001b5da60 thread T0
    #0 0x7f57906db632 in freespace_planning_algorithms::AstarNode::cost() const (/home/h-ishida/pilot-auto/install/freespace_planning_algorithms/lib/libfreespace_planning_algorithms.so+0x67632)
    #1 0x7f57906db693 in freespace_planning_algorithms::NodeComparison::operator()(freespace_planning_algorithms::AstarNode const*, freespace_planning_algorithms::AstarNode const*) (/home/h-ishida/pilot-auto/install/freespace_planning_algorithms/lib/libfreespace_planning_algorithms.so+0x67693)
    #2 0x7f57906e71d7 in bool __gnu_cxx::__ops::_Iter_comp_val<freespace_planning_algorithms::NodeComparison>::operator()<__gnu_cxx::__normal_iterator<freespace_planning_algorithms::AstarNode**, std::vector<freespace_planning_algorithms::AstarNode*, std::allocator<freespace_planning_algorithms::AstarNode*> > >, freespace_planning_algorithms::AstarNode*>(__gnu_cxx::__normal_iterator<freespace_planning_algorithms::AstarNode**, std::vector<freespace_planning_algorithms::AstarNode*, std::allocator<freespace_planning_algorithms::AstarNode*> > >, freespace_planning_algorithms::AstarNode*&) (/home/h-ishida/pilot-auto/install/freespace_planning_algorithms/lib/libfreespace_planning_algorithms.so+0x731d7)
    #3 0x7f57906e4678 in void std::__push_heap<__gnu_cxx::__normal_iterator<freespace_planning_algorithms::AstarNode**, std::vector<freespace_planning_algorithms::AstarNode*, std::allocator<freespace_planning_algorithms::AstarNode*> > >, long, freespace_planning_algorithms::AstarNode*, __gnu_cxx::__ops::_Iter_comp_val<freespace_planning_algorithms::NodeComparison> >(__gnu_cxx::__normal_iterator<freespace_planning_algorithms::AstarNode**, std::vector<freespace_planning_algorithms::AstarNode*, std::allocator<freespace_planning_algorithms::AstarNode*> > >, long, long, freespace_planning_algorithms::AstarNode*, __gnu_cxx::__ops::_Iter_comp_val<freespace_planning_algorithms::NodeComparison>&) (/home/h-ishida/pilot-auto/install/freespace_planning_algorithms/lib/libfreespace_planning_algorithms.so+0x70678)
    #4 0x7f57906e09ef in void std::push_heap<__gnu_cxx::__normal_iterator<freespace_planning_algorithms::AstarNode**, std::vector<freespace_planning_algorithms::AstarNode*, std::allocator<freespace_planning_algorithms::AstarNode*> > >, freespace_planning_algorithms::NodeComparison>(__gnu_cxx::__normal_iterator<freespace_planning_algorithms::AstarNode**, std::vector<freespace_planning_algorithms::AstarNode*, std::allocator<freespace_planning_algorithms::AstarNode*> > >, __gnu_cxx::__normal_iterator<freespace_planning_algorithms::AstarNode**, std::vector<freespace_planning_algorithms::AstarNode*, std::allocator<freespace_planning_algorithms::AstarNode*> > >, freespace_planning_algorithms::NodeComparison) (/home/h-ishida/pilot-auto/install/freespace_planning_algorithms/lib/libfreespace_planning_algorithms.so+0x6c9ef)
    #5 0x7f57906dd4eb in std::priority_queue<freespace_planning_algorithms::AstarNode*, std::vector<freespace_planning_algorithms::AstarNode*, std::allocator<freespace_planning_algorithms::AstarNode*> >, freespace_planning_algorithms::NodeComparison>::push(freespace_planning_algorithms::AstarNode* const&) (/home/h-ishida/pilot-auto/install/freespace_planning_algorithms/lib/libfreespace_planning_algorithms.so+0x694eb)
    #6 0x7f57906d6605 in freespace_planning_algorithms::AstarSearch::setStartNode() (/home/h-ishida/pilot-auto/install/freespace_planning_algorithms/lib/libfreespace_planning_algorithms.so+0x62605)
    #7 0x7f57906d60eb in freespace_planning_algorithms::AstarSearch::makePlan(geometry_msgs::msg::Pose_<std::allocator<void> > const&, geometry_msgs::msg::Pose_<std::allocator<void> > const&) (/home/h-ishida/pilot-auto/install/freespace_planning_algorithms/lib/libfreespace_planning_algorithms.so+0x620eb)
    #8 0x55ccada3c47b in test_astar(std::array<double, 3ul>, std::array<double, 3ul>, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >, double, int) (/home/h-ishida/pilot-auto/build/freespace_planning_algorithms/freespace_planning_algorithms-test+0x2447b)
    #9 0x55ccada3d856 in AstarSearchTestSuite_SingleCurvature_Test::TestBody() (/home/h-ishida/pilot-auto/build/freespace_planning_algorithms/freespace_planning_algorithms-test+0x25856)
    #10 0x55ccadac5bea in void testing::internal::HandleSehExceptionsInMethodIfSupported<testing::Test, void>(testing::Test*, void (testing::Test::*)(), char const*) (/home/h-ishida/pilot-auto/build/freespace_planning_algorithms/freespace_planning_algorithms-test+0xadbea)
    #11 0x55ccadab60e9 in void testing::internal::HandleExceptionsInMethodIfSupported<testing::Test, void>(testing::Test*, void (testing::Test::*)(), char const*) (/home/h-ishida/pilot-auto/build/freespace_planning_algorithms/freespace_planning_algorithms-test+0x9e0e9)
    #12 0x55ccada5d579 in testing::Test::Run() (/home/h-ishida/pilot-auto/build/freespace_planning_algorithms/freespace_planning_algorithms-test+0x45579)
    #13 0x55ccada5e9fe in testing::TestInfo::Run() (/home/h-ishida/pilot-auto/build/freespace_planning_algorithms/freespace_planning_algorithms-test+0x469fe)
    #14 0x55ccada5f733 in testing::TestSuite::Run() (/home/h-ishida/pilot-auto/build/freespace_planning_algorithms/freespace_planning_algorithms-test+0x47733)
    #15 0x55ccada7c217 in testing::internal::UnitTestImpl::RunAllTests() (/home/h-ishida/pilot-auto/build/freespace_planning_algorithms/freespace_planning_algorithms-test+0x64217)
    #16 0x55ccadac8afe in bool testing::internal::HandleSehExceptionsInMethodIfSupported<testing::internal::UnitTestImpl, bool>(testing::internal::UnitTestImpl*, bool (testing::internal::UnitTestImpl::*)(), char const*) (/home/h-ishida/pilot-auto/build/freespace_planning_algorithms/freespace_planning_algorithms-test+0xb0afe)
    #17 0x55ccadab8d28 in bool testing::internal::HandleExceptionsInMethodIfSupported<testing::internal::UnitTestImpl, bool>(testing::internal::UnitTestImpl*, bool (testing::internal::UnitTestImpl::*)(), char const*) (/home/h-ishida/pilot-auto/build/freespace_planning_algorithms/freespace_planning_algorithms-test+0xa0d28)
    #18 0x55ccada78cab in testing::UnitTest::Run() (/home/h-ishida/pilot-auto/build/freespace_planning_algorithms/freespace_planning_algorithms-test+0x60cab)
    #19 0x55ccada40faf in RUN_ALL_TESTS() (/home/h-ishida/pilot-auto/build/freespace_planning_algorithms/freespace_planning_algorithms-test+0x28faf)
    #20 0x55ccada3edc5 in main (/home/h-ishida/pilot-auto/build/freespace_planning_algorithms/freespace_planning_algorithms-test+0x26dc5)
    #21 0x7f578f8500b2 in __libc_start_main (/lib/x86_64-linux-gnu/libc.so.6+0x240b2)
    #22 0x55ccada3b13d in _start (/home/h-ishida/pilot-auto/build/freespace_planning_algorithms/freespace_planning_algorithms-test+0x2313d)

0x625001b5da60 is located 2400 bytes inside of 9216-byte region [0x625001b5d100,0x625001b5f500)
freed by thread T0 here:
    #0 0x7f579082151f in operator delete(void*) ../../../../src/libsanitizer/asan/asan_new_delete.cc:165
    #1 0x55ccada49599 in __gnu_cxx::new_allocator<freespace_planning_algorithms::AstarNode>::deallocate(freespace_planning_algorithms::AstarNode*, unsigned long) (/home/h-ishida/pilot-auto/build/freespace_planning_algorithms/freespace_planning_algorithms-test+0x31599)
    #2 0x55ccada4955f in std::allocator_traits<std::allocator<freespace_planning_algorithms::AstarNode> >::deallocate(std::allocator<freespace_planning_algorithms::AstarNode>&, freespace_planning_algorithms::AstarNode*, unsigned long) (/home/h-ishida/pilot-auto/build/freespace_planning_algorithms/freespace_planning_algorithms-test+0x3155f)
    #3 0x55ccada494f3 in std::_Vector_base<freespace_planning_algorithms::AstarNode, std::allocator<freespace_planning_algorithms::AstarNode> >::_M_deallocate(freespace_planning_algorithms::AstarNode*, unsigned long) (/home/h-ishida/pilot-auto/build/freespace_planning_algorithms/freespace_planning_algorithms-test+0x314f3)
    #4 0x55ccada4944d in std::_Vector_base<freespace_planning_algorithms::AstarNode, std::allocator<freespace_planning_algorithms::AstarNode> >::~_Vector_base() (/home/h-ishida/pilot-auto/build/freespace_planning_algorithms/freespace_planning_algorithms-test+0x3144d)
    #5 0x55ccada493a1 in std::vector<freespace_planning_algorithms::AstarNode, std::allocator<freespace_planning_algorithms::AstarNode> >::~vector() (/home/h-ishida/pilot-auto/build/freespace_planning_algorithms/freespace_planning_algorithms-test+0x313a1)
    #6 0x55ccada4930f in void std::_Destroy<std::vector<freespace_planning_algorithms::AstarNode, std::allocator<freespace_planning_algorithms::AstarNode> > >(std::vector<freespace_planning_algorithms::AstarNode, std::allocator<freespace_planning_algorithms::AstarNode> >*) (/home/h-ishida/pilot-auto/build/freespace_planning_algorithms/freespace_planning_algorithms-test+0x3130f)
    #7 0x55ccada4928a in void std::_Destroy_aux<false>::__destroy<std::vector<freespace_planning_algorithms::AstarNode, std::allocator<freespace_planning_algorithms::AstarNode> >*>(std::vector<freespace_planning_algorithms::AstarNode, std::allocator<freespace_planning_algorithms::AstarNode> >*, std::vector<freespace_planning_algorithms::AstarNode, std::allocator<freespace_planning_algorithms::AstarNode> >*) (/home/h-ishida/pilot-auto/build/freespace_planning_algorithms/freespace_planning_algorithms-test+0x3128a)
    #8 0x55ccada491bf in void std::_Destroy<std::vector<freespace_planning_algorithms::AstarNode, std::allocator<freespace_planning_algorithms::AstarNode> >*>(std::vector<freespace_planning_algorithms::AstarNode, std::allocator<freespace_planning_algorithms::AstarNode> >*, std::vector<freespace_planning_algorithms::AstarNode, std::allocator<freespace_planning_algorithms::AstarNode> >*) (/home/h-ishida/pilot-auto/build/freespace_planning_algorithms/freespace_planning_algorithms-test+0x311bf)
    #9 0x55ccada490b8 in void std::_Destroy<std::vector<freespace_planning_algorithms::AstarNode, std::allocator<freespace_planning_algorithms::AstarNode> >*, std::vector<freespace_planning_algorithms::AstarNode, std::allocator<freespace_planning_algorithms::AstarNode> > >(std::vector<freespace_planning_algorithms::AstarNode, std::allocator<freespace_planning_algorithms::AstarNode> >*, std::vector<freespace_planning_algorithms::AstarNode, std::allocator<freespace_planning_algorithms::AstarNode> >*, std::allocator<std::vector<freespace_planning_algorithms::AstarNode, std::allocator<freespace_planning_algorithms::AstarNode> > >&) (/home/h-ishida/pilot-auto/build/freespace_planning_algorithms/freespace_planning_algorithms-test+0x310b8)
    #10 0x55ccada48e9b in std::vector<std::vector<freespace_planning_algorithms::AstarNode, std::allocator<freespace_planning_algorithms::AstarNode> >, std::allocator<std::vector<freespace_planning_algorithms::AstarNode, std::allocator<freespace_planning_algorithms::AstarNode> > > >::~vector() (/home/h-ishida/pilot-auto/build/freespace_planning_algorithms/freespace_planning_algorithms-test+0x30e9b)
    #11 0x55ccada48d5d in void std::_Destroy<std::vector<std::vector<freespace_planning_algorithms::AstarNode, std::allocator<freespace_planning_algorithms::AstarNode> >, std::allocator<std::vector<freespace_planning_algorithms::AstarNode, std::allocator<freespace_planning_algorithms::AstarNode> > > > >(std::vector<std::vector<freespace_planning_algorithms::AstarNode, std::allocator<freespace_planning_algorithms::AstarNode> >, std::allocator<std::vector<freespace_planning_algorithms::AstarNode, std::allocator<freespace_planning_algorithms::AstarNode> > > >*) (/home/h-ishida/pilot-auto/build/freespace_planning_algorithms/freespace_planning_algorithms-test+0x30d5d)
    #12 0x55ccada48c52 in void std::_Destroy_aux<false>::__destroy<std::vector<std::vector<freespace_planning_algorithms::AstarNode, std::allocator<freespace_planning_algorithms::AstarNode> >, std::allocator<std::vector<freespace_planning_algorithms::AstarNode, std::allocator<freespace_planning_algorithms::AstarNode> > > >*>(std::vector<std::vector<freespace_planning_algorithms::AstarNode, std::allocator<freespace_planning_algorithms::AstarNode> >, std::allocator<std::vector<freespace_planning_algorithms::AstarNode, std::allocator<freespace_planning_algorithms::AstarNode> > > >*, std::vector<std::vector<freespace_planning_algorithms::AstarNode, std::allocator<freespace_planning_algorithms::AstarNode> >, std::allocator<std::vector<freespace_planning_algorithms::AstarNode, std::allocator<freespace_planning_algorithms::AstarNode> > > >*) (/home/h-ishida/pilot-auto/build/freespace_planning_algorithms/freespace_planning_algorithms-test+0x30c52)
    #13 0x55ccada48ad9 in void std::_Destroy<std::vector<std::vector<freespace_planning_algorithms::AstarNode, std::allocator<freespace_planning_algorithms::AstarNode> >, std::allocator<std::vector<freespace_planning_algorithms::AstarNode, std::allocator<freespace_planning_algorithms::AstarNode> > > >*>(std::vector<std::vector<freespace_planning_algorithms::AstarNode, std::allocator<freespace_planning_algorithms::AstarNode> >, std::allocator<std::vector<freespace_planning_algorithms::AstarNode, std::allocator<freespace_planning_algorithms::AstarNode> > > >*, std::vector<std::vector<freespace_planning_algorithms::AstarNode, std::allocator<freespace_planning_algorithms::AstarNode> >, std::allocator<std::vector<freespace_planning_algorithms::AstarNode, std::allocator<freespace_planning_algorithms::AstarNode> > > >*) (/home/h-ishida/pilot-auto/build/freespace_planning_algorithms/freespace_planning_algorithms-test+0x30ad9)
    #14 0x55ccada488da in void std::_Destroy<std::vector<std::vector<freespace_planning_algorithms::AstarNode, std::allocator<freespace_planning_algorithms::AstarNode> >, std::allocator<std::vector<freespace_planning_algorithms::AstarNode, std::allocator<freespace_planning_algorithms::AstarNode> > > >*, std::vector<std::vector<freespace_planning_algorithms::AstarNode, std::allocator<freespace_planning_algorithms::AstarNode> >, std::allocator<std::vector<freespace_planning_algorithms::AstarNode, std::allocator<freespace_planning_algorithms::AstarNode> > > > >(std::vector<std::vector<freespace_planning_algorithms::AstarNode, std::allocator<freespace_planning_algorithms::AstarNode> >, std::allocator<std::vector<freespace_planning_algorithms::AstarNode, std::allocator<freespace_planning_algorithms::AstarNode> > > >*, std::vector<std::vector<freespace_planning_algorithms::AstarNode, std::allocator<freespace_planning_algorithms::AstarNode> >, std::allocator<std::vector<freespace_planning_algorithms::AstarNode, std::allocator<freespace_planning_algorithms::AstarNode> > > >*, std::allocator<std::vector<std::vector<freespace_planning_algorithms::AstarNode, std::allocator<freespace_planning_algorithms::AstarNode> >, std::allocator<std::vector<freespace_planning_algorithms::AstarNode, std::allocator<freespace_planning_algorithms::AstarNode> > > > >&) (/home/h-ishida/pilot-auto/build/freespace_planning_algorithms/freespace_planning_algorithms-test+0x308da)
    #15 0x7f57906df596 in std::vector<std::vector<std::vector<freespace_planning_algorithms::AstarNode, std::allocator<freespace_planning_algorithms::AstarNode> >, std::allocator<std::vector<freespace_planning_algorithms::AstarNode, std::allocator<freespace_planning_algorithms::AstarNode> > > >, std::allocator<std::vector<std::vector<freespace_planning_algorithms::AstarNode, std::allocator<freespace_planning_algorithms::AstarNode> >, std::allocator<std::vector<freespace_planning_algorithms::AstarNode, std::allocator<freespace_planning_algorithms::AstarNode> > > > > >::_M_erase_at_end(std::vector<std::vector<freespace_planning_algorithms::AstarNode, std::allocator<freespace_planning_algorithms::AstarNode> >, std::allocator<std::vector<freespace_planning_algorithms::AstarNode, std::allocator<freespace_planning_algorithms::AstarNode> > > >*) (/home/h-ishida/pilot-auto/install/freespace_planning_algorithms/lib/libfreespace_planning_algorithms.so+0x6b596)
    #16 0x7f57906dd282 in std::vector<std::vector<std::vector<freespace_planning_algorithms::AstarNode, std::allocator<freespace_planning_algorithms::AstarNode> >, std::allocator<std::vector<freespace_planning_algorithms::AstarNode, std::allocator<freespace_planning_algorithms::AstarNode> > > >, std::allocator<std::vector<std::vector<freespace_planning_algorithms::AstarNode, std::allocator<freespace_planning_algorithms::AstarNode> >, std::allocator<std::vector<freespace_planning_algorithms::AstarNode, std::allocator<freespace_planning_algorithms::AstarNode> > > > > >::clear() (/home/h-ishida/pilot-auto/install/freespace_planning_algorithms/lib/libfreespace_planning_algorithms.so+0x69282)
    #17 0x7f57906d5e12 in freespace_planning_algorithms::AstarSearch::setMap(nav_msgs::msg::OccupancyGrid_<std::allocator<void> > const&) (/home/h-ishida/pilot-auto/install/freespace_planning_algorithms/lib/libfreespace_planning_algorithms.so+0x61e12)
    #18 0x55ccada3c235 in test_astar(std::array<double, 3ul>, std::array<double, 3ul>, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >, double, int) (/home/h-ishida/pilot-auto/build/freespace_planning_algorithms/freespace_planning_algorithms-test+0x24235)
    #19 0x55ccada3d856 in AstarSearchTestSuite_SingleCurvature_Test::TestBody() (/home/h-ishida/pilot-auto/build/freespace_planning_algorithms/freespace_planning_algorithms-test+0x25856)
    #20 0x55ccadac5bea in void testing::internal::HandleSehExceptionsInMethodIfSupported<testing::Test, void>(testing::Test*, void (testing::Test::*)(), char const*) (/home/h-ishida/pilot-auto/build/freespace_planning_algorithms/freespace_planning_algorithms-test+0xadbea)
    #21 0x55ccadab60e9 in void testing::internal::HandleExceptionsInMethodIfSupported<testing::Test, void>(testing::Test*, void (testing::Test::*)(), char const*) (/home/h-ishida/pilot-auto/build/freespace_planning_algorithms/freespace_planning_algorithms-test+0x9e0e9)
    #22 0x55ccada5d579 in testing::Test::Run() (/home/h-ishida/pilot-auto/build/freespace_planning_algorithms/freespace_planning_algorithms-test+0x45579)
    #23 0x55ccada5e9fe in testing::TestInfo::Run() (/home/h-ishida/pilot-auto/build/freespace_planning_algorithms/freespace_planning_algorithms-test+0x469fe)
    #24 0x55ccada5f733 in testing::TestSuite::Run() (/home/h-ishida/pilot-auto/build/freespace_planning_algorithms/freespace_planning_algorithms-test+0x47733)
    #25 0x55ccada7c217 in testing::internal::UnitTestImpl::RunAllTests() (/home/h-ishida/pilot-auto/build/freespace_planning_algorithms/freespace_planning_algorithms-test+0x64217)
    #26 0x55ccadac8afe in bool testing::internal::HandleSehExceptionsInMethodIfSupported<testing::internal::UnitTestImpl, bool>(testing::internal::UnitTestImpl*, bool (testing::internal::UnitTestImpl::*)(), char const*) (/home/h-ishida/pilot-auto/build/freespace_planning_algorithms/freespace_planning_algorithms-test+0xb0afe)
    #27 0x55ccadab8d28 in bool testing::internal::HandleExceptionsInMethodIfSupported<testing::internal::UnitTestImpl, bool>(testing::internal::UnitTestImpl*, bool (testing::internal::UnitTestImpl::*)(), char const*) (/home/h-ishida/pilot-auto/build/freespace_planning_algorithms/freespace_planning_algorithms-test+0xa0d28)
    #28 0x55ccada78cab in testing::UnitTest::Run() (/home/h-ishida/pilot-auto/build/freespace_planning_algorithms/freespace_planning_algorithms-test+0x60cab)
    #29 0x55ccada40faf in RUN_ALL_TESTS() (/home/h-ishida/pilot-auto/build/freespace_planning_algorithms/freespace_planning_algorithms-test+0x28faf)

previously allocated by thread T0 here:
    #0 0x7f5790820587 in operator new(unsigned long) ../../../../src/libsanitizer/asan/asan_new_delete.cc:104
    #1 0x7f57906e85ce in __gnu_cxx::new_allocator<freespace_planning_algorithms::AstarNode>::allocate(unsigned long, void const*) (/home/h-ishida/pilot-auto/install/freespace_planning_algorithms/lib/libfreespace_planning_algorithms.so+0x745ce)
    #2 0x7f57906e6a04 in std::allocator_traits<std::allocator<freespace_planning_algorithms::AstarNode> >::allocate(std::allocator<freespace_planning_algorithms::AstarNode>&, unsigned long) (/home/h-ishida/pilot-auto/install/freespace_planning_algorithms/lib/libfreespace_planning_algorithms.so+0x72a04)
    #3 0x7f57906e3aeb in std::_Vector_base<freespace_planning_algorithms::AstarNode, std::allocator<freespace_planning_algorithms::AstarNode> >::_M_allocate(unsigned long) (/home/h-ishida/pilot-auto/install/freespace_planning_algorithms/lib/libfreespace_planning_algorithms.so+0x6faeb)
    #4 0x7f57906e0196 in std::vector<freespace_planning_algorithms::AstarNode, std::allocator<freespace_planning_algorithms::AstarNode> >::_M_default_append(unsigned long) (/home/h-ishida/pilot-auto/install/freespace_planning_algorithms/lib/libfreespace_planning_algorithms.so+0x6c196)
    #5 0x7f57906dd43e in std::vector<freespace_planning_algorithms::AstarNode, std::allocator<freespace_planning_algorithms::AstarNode> >::resize(unsigned long) (/home/h-ishida/pilot-auto/install/freespace_planning_algorithms/lib/libfreespace_planning_algorithms.so+0x6943e)
    #6 0x7f57906d5ef8 in freespace_planning_algorithms::AstarSearch::setMap(nav_msgs::msg::OccupancyGrid_<std::allocator<void> > const&) (/home/h-ishida/pilot-auto/install/freespace_planning_algorithms/lib/libfreespace_planning_algorithms.so+0x61ef8)
    #7 0x55ccada3c235 in test_astar(std::array<double, 3ul>, std::array<double, 3ul>, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >, double, int) (/home/h-ishida/pilot-auto/build/freespace_planning_algorithms/freespace_planning_algorithms-test+0x24235)
    #8 0x55ccada3d856 in AstarSearchTestSuite_SingleCurvature_Test::TestBody() (/home/h-ishida/pilot-auto/build/freespace_planning_algorithms/freespace_planning_algorithms-test+0x25856)
    #9 0x55ccadac5bea in void testing::internal::HandleSehExceptionsInMethodIfSupported<testing::Test, void>(testing::Test*, void (testing::Test::*)(), char const*) (/home/h-ishida/pilot-auto/build/freespace_planning_algorithms/freespace_planning_algorithms-test+0xadbea)
    #10 0x55ccadab60e9 in void testing::internal::HandleExceptionsInMethodIfSupported<testing::Test, void>(testing::Test*, void (testing::Test::*)(), char const*) (/home/h-ishida/pilot-auto/build/freespace_planning_algorithms/freespace_planning_algorithms-test+0x9e0e9)
    #11 0x55ccada5d579 in testing::Test::Run() (/home/h-ishida/pilot-auto/build/freespace_planning_algorithms/freespace_planning_algorithms-test+0x45579)
    #12 0x55ccada5e9fe in testing::TestInfo::Run() (/home/h-ishida/pilot-auto/build/freespace_planning_algorithms/freespace_planning_algorithms-test+0x469fe)
    #13 0x55ccada5f733 in testing::TestSuite::Run() (/home/h-ishida/pilot-auto/build/freespace_planning_algorithms/freespace_planning_algorithms-test+0x47733)
    #14 0x55ccada7c217 in testing::internal::UnitTestImpl::RunAllTests() (/home/h-ishida/pilot-auto/build/freespace_planning_algorithms/freespace_planning_algorithms-test+0x64217)
    #15 0x55ccadac8afe in bool testing::internal::HandleSehExceptionsInMethodIfSupported<testing::internal::UnitTestImpl, bool>(testing::internal::UnitTestImpl*, bool (testing::internal::UnitTestImpl::*)(), char const*) (/home/h-ishida/pilot-auto/build/freespace_planning_algorithms/freespace_planning_algorithms-test+0xb0afe)
    #16 0x55ccadab8d28 in bool testing::internal::HandleExceptionsInMethodIfSupported<testing::internal::UnitTestImpl, bool>(testing::internal::UnitTestImpl*, bool (testing::internal::UnitTestImpl::*)(), char const*) (/home/h-ishida/pilot-auto/build/freespace_planning_algorithms/freespace_planning_algorithms-test+0xa0d28)
    #17 0x55ccada78cab in testing::UnitTest::Run() (/home/h-ishida/pilot-auto/build/freespace_planning_algorithms/freespace_planning_algorithms-test+0x60cab)
    #18 0x55ccada40faf in RUN_ALL_TESTS() (/home/h-ishida/pilot-auto/build/freespace_planning_algorithms/freespace_planning_algorithms-test+0x28faf)
    #19 0x55ccada3edc5 in main (/home/h-ishida/pilot-auto/build/freespace_planning_algorithms/freespace_planning_algorithms-test+0x26dc5)
    #20 0x7f578f8500b2 in __libc_start_main (/lib/x86_64-linux-gnu/libc.so.6+0x240b2)

SUMMARY: AddressSanitizer: heap-use-after-free (/home/h-ishida/pilot-auto/install/freespace_planning_algorithms/lib/libfreespace_planning_algorithms.so+0x67632) in freespace_planning_algorithms::AstarNode::cost() const
Shadow bytes around the buggy address:
  0x0c4a80363af0: fd fd fd fd fd fd fd fd fd fd fd fd fd fd fd fd
  0x0c4a80363b00: fd fd fd fd fd fd fd fd fd fd fd fd fd fd fd fd
  0x0c4a80363b10: fd fd fd fd fd fd fd fd fd fd fd fd fd fd fd fd
  0x0c4a80363b20: fd fd fd fd fd fd fd fd fd fd fd fd fd fd fd fd
  0x0c4a80363b30: fd fd fd fd fd fd fd fd fd fd fd fd fd fd fd fd
=>0x0c4a80363b40: fd fd fd fd fd fd fd fd fd fd fd fd[fd]fd fd fd
  0x0c4a80363b50: fd fd fd fd fd fd fd fd fd fd fd fd fd fd fd fd
  0x0c4a80363b60: fd fd fd fd fd fd fd fd fd fd fd fd fd fd fd fd
  0x0c4a80363b70: fd fd fd fd fd fd fd fd fd fd fd fd fd fd fd fd
  0x0c4a80363b80: fd fd fd fd fd fd fd fd fd fd fd fd fd fd fd fd
  0x0c4a80363b90: fd fd fd fd fd fd fd fd fd fd fd fd fd fd fd fd
Shadow byte legend (one shadow byte represents 8 application bytes):
  Addressable:           00
  Partially addressable: 01 02 03 04 05 06 07 
  Heap left redzone:       fa
  Freed heap region:       fd
  Stack left redzone:      f1
  Stack mid redzone:       f2
  Stack right redzone:     f3
  Stack after return:      f5
  Stack use after scope:   f8
  Global redzone:          f9
  Global init order:       f6
  Poisoned by user:        f7
  Container overflow:      fc
  Array cookie:            ac
  Intra object redzone:    bb
  ASan internal:           fe
  Left alloca redzone:     ca
  Right alloca redzone:    cb
  Shadow gap:              cc
==60867==ABORTING

@HiroIshida HiroIshida force-pushed the informed_rrtstar branch 2 times, most recently from d60dea3 to 1dceb98 Compare May 6, 2022 11:11
Signed-off-by: Hirokazu Ishida <h-ishida@jsk.imi.i.u-tokyo.ac.jp>
@HiroIshida HiroIshida force-pushed the informed_rrtstar branch 4 times, most recently from f462eea to ca6539b Compare May 14, 2022 12:07
HiroIshida and others added 11 commits May 15, 2022 03:23
Signed-off-by: Hirokazu Ishida <h-ishida@jsk.imi.i.u-tokyo.ac.jp>
…using raymarchig (autowarefoundation#527)

* Create pointcloud by raycasting from vehicle

Signed-off-by: Hirokazu Ishida <h-ishida@jsk.imi.i.u-tokyo.ac.jp>

* [after-review] Use vector of ObjectInfo

Signed-off-by: Hirokazu Ishida <h-ishida@jsk.imi.i.u-tokyo.ac.jp>

* [after-review] Implemented by strategy pattern

Signed-off-by: Hirokazu Ishida <h-ishida@jsk.imi.i.u-tokyo.ac.jp>

* [after-review] split files

Signed-off-by: Hirokazu Ishida <h-ishida@jsk.imi.i.u-tokyo.ac.jp>

* Use pcl raytracing

Tmp

Tmp

Tmp

Signed-off-by: Hirokazu Ishida <h-ishida@jsk.imi.i.u-tokyo.ac.jp>

* Add signed distance function lib

Signed-off-by: Hirokazu Ishida <h-ishida@jsk.imi.i.u-tokyo.ac.jp>

* Use sdf library

Signed-off-by: Hirokazu Ishida <h-ishida@jsk.imi.i.u-tokyo.ac.jp>

* Remove no longer used functions

Signed-off-by: Hirokazu Ishida <h-ishida@jsk.imi.i.u-tokyo.ac.jp>

* Refactor

Signed-off-by: Hirokazu Ishida <h-ishida@jsk.imi.i.u-tokyo.ac.jp>

* Simplify getPoint

Signed-off-by: Hirokazu Ishida <h-ishida@jsk.imi.i.u-tokyo.ac.jp>

* Raytracing considering inter object relation

Signed-off-by: Hirokazu Ishida <h-ishida@jsk.imi.i.u-tokyo.ac.jp>

* Add random noise

Signed-off-by: Hirokazu Ishida <h-ishida@jsk.imi.i.u-tokyo.ac.jp>

* Default is object centric

Signed-off-by: Hirokazu Ishida <h-ishida@jsk.imi.i.u-tokyo.ac.jp>

* Return if no objects are detected

Signed-off-by: Hirokazu Ishida <h-ishida@jsk.imi.i.u-tokyo.ac.jp>

* Change definition of tf_global_to_local (same as other autoware codes)

Signed-off-by: Hirokazu Ishida <h-ishida@jsk.imi.i.u-tokyo.ac.jp>

* Remove create method

Signed-off-by: Hirokazu Ishida <h-ishida@jsk.imi.i.u-tokyo.ac.jp>

* Reaname: VehicleCentric -> EgoCentric

Signed-off-by: Hirokazu Ishida <h-ishida@jsk.imi.i.u-tokyo.ac.jp>

* Refactor a bit

Signed-off-by: Hirokazu Ishida <h-ishida@jsk.imi.i.u-tokyo.ac.jp>

* Tune parameter

Signed-off-by: Hirokazu Ishida <h-ishida@jsk.imi.i.u-tokyo.ac.jp>

* Fix: Even if selected_idices is zero, pointclouds must be published

Signed-off-by: Hirokazu Ishida <h-ishida@jsk.imi.i.u-tokyo.ac.jp>

* Fix launch file

Signed-off-by: Hirokazu Ishida <h-ishida@jsk.imi.i.u-tokyo.ac.jp>

* Fix typo

Signed-off-by: Hirokazu Ishida <h-ishida@jsk.imi.i.u-tokyo.ac.jp>

* Fix: create merged pointcloud when no idx is selected

Signed-off-by: Hirokazu Ishida <h-ishida@jsk.imi.i.u-tokyo.ac.jp>

* Use ray-maching by default

Signed-off-by: Hirokazu Ishida <h-ishida@jsk.imi.i.u-tokyo.ac.jp>
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
None yet
Projects
None yet
Development

Successfully merging this pull request may close these issues.

1 participant