whoenig / libMultiRobotPlanning

Library with search algorithms for task and path planning for multi robot/agent systems
MIT License
814 stars 218 forks source link

Error while adding a new action #27

Closed H-Bey closed 3 years ago

H-Bey commented 3 years ago

Hello! Thanks for sharing your work! I am trying to add a new action into the example/ECBS.cpp such that in addition to the available action set (up, down, left, and right). The agent can navigate to corner. The modification ended with successful path planning for most of the examples in the benchmark/. However, there are some yamls in the benchmark/ that show a segmentation fault, similar to below.

user:~/libMultiRobotPlanning-master/build$ ./ecbs -i ../benchmark/32x32_obst204/map_32by32_obst204_agents100_ex1.yaml -o output.yaml -w 1.3
Found conflict: 8: Vertex(20,10)
create child with id 1
  success. cost: 2305
create child with id 2
  success. cost: 2303
Found conflict: 8: Edge(20,11,19,11)
create child with id 3
  success. cost: 2305
create child with id 4
  success. cost: 2307
Found conflict: 11: Vertex(21,12)
create child with id 5
  success. cost: 2309
create child with id 6
Segmentation fault (core dumped)

It's important to keep in mind that all the cases showing a segmentation fault, while using the modified example/ECBS.cpp, e.g. ./ecbs -i ../benchmark/32x32_obst204/map_32by32_obst204_agents100_ex1.yaml -o output.yaml -w 1.3 could successfully generate a path by running them via valgrind e.g. valgrind ./ecbs -i ../benchmark/32x32_obst204/map_32by32_obst204_agents100_ex1.yaml -o output.yaml -w 1.3 Below I am providing the modified ecbs.cpp as a .txt ecbs_corner.txt Sharing any idea supports treating the shown fault would be highly appreciated.

whoenig commented 3 years ago

valgrind sometimes alters the execution significantly, because it replaces many low level function calls with the valgrind-internals. Could you try running it with gdb, i.e. gdb -ex run --args ./ecbs -i ../benchmark/32x32_obst204/map_32by32_obst204_agents100_ex1.yaml -o output.yaml -w 1.3? If it does 'find' the problem, you can look at the stacktrace using bt in the gdb prompt. Keep in mind that both valgrind and gdb will work best if compiled using `RelWithDebInfo' as build type.

H-Bey commented 3 years ago

Below I am providing the outcome of gdb -ex run --args ./ecbs -i ../benchmark/32x32_obst204/map_32by32_obst204_agents100_ex1.yaml -o output.yaml -w 1.3

user:~/libMultiRobotPlanning-master/build$ gdb -ex run --args ./ecbs -i ../benchmark/32x32_obst204/map_32by32_obst204_agents100_ex1.yaml -o output.yaml -w 1.3
GNU gdb (Ubuntu 8.1-0ubuntu3.2) 8.1.0.20180409-git
Copyright (C) 2018 Free Software Foundation, Inc.
License GPLv3+: GNU GPL version 3 or later <http://gnu.org/licenses/gpl.html>
This is free software: you are free to change and redistribute it.
There is NO WARRANTY, to the extent permitted by law.  Type "show copying"
and "show warranty" for details.
This GDB was configured as "x86_64-linux-gnu".
Type "show configuration" for configuration details.
For bug reporting instructions, please see:
<http://www.gnu.org/software/gdb/bugs/>.
Find the GDB manual and other documentation resources online at:
<http://www.gnu.org/software/gdb/documentation/>.
For help, type "help".
Type "apropos word" to search for commands related to "word"...
Reading symbols from ./ecbs...(no debugging symbols found)...done.
Starting program: /home/user/libMultiRobotPlanning-master/build/ecbs -i ../benchmark/32x32_obst204/map_32by32_obst204_agents100_ex1.yaml -o output.yaml -w 1.3
[Thread debugging using libthread_db enabled]
Using host libthread_db library "/lib/x86_64-linux-gnu/libthread_db.so.1".
Found conflict: 8: Vertex(20,10)
create child with id 1
  success. cost: 2305
create child with id 2
  success. cost: 2303
Found conflict: 8: Edge(20,11,19,11)
create child with id 3
  success. cost: 2305
create child with id 4
  success. cost: 2307
Found conflict: 11: Vertex(21,12)
create child with id 5
  success. cost: 2309
create child with id 6

Program received signal SIGSEGV, Segmentation fault.
0x00007ffff75f2f3a in std::__detail::_List_node_base::_M_unhook() ()
   from /usr/lib/x86_64-linux-gnu/libstdc++.so.6
(gdb) bt
#0  0x00007ffff75f2f3a in std::__detail::_List_node_base::_M_unhook() ()
   from /usr/lib/x86_64-linux-gnu/libstdc++.so.6
#1  0x000055555558d2be in std::__cxx11::list<std::pair<libMultiRobotPlanning::AStarEpsilon<State, Action, int, libMultiRobotPlanning::ECBS<State, Action, int, Conflict, Constraints, Environment>::LowLevelEnvironment, std::hash<State> >::Node, unsigned long>, std::allocator<std::pair<libMultiRobotPlanning::AStarEpsilon<State, Action, int, libMultiRobotPlanning::ECBS<State, Action, int, Conflict, Constraints, Environment>::LowLevelEnvironment, std::hash<State> >::Node, unsigned long> > >::_M_erase(std::_List_iterator<std::pair<libMultiRobotPlanning::AStarEpsilon<State, Action, int, libMultiRobotPlanning::ECBS<State, Action, int, Conflict, Constraints, Environment>::LowLevelEnvironment, std::hash<State> >::Node, unsigned long> >) ()
#2  0x000055555558796b in std::__cxx11::list<std::pair<libMultiRobotPlanning::AStarEpsilon<State, Action, int, libMultiRobotPlanning::ECBS<State, Action, int, Conflict, Constraints, Environment>::LowLevelEnvironment, std::hash<State> >::Node, unsigned long>, std::allocator<std::pair<libMultiRobotPlanning::AStarEpsilon<State, Action, int, libMultiRobotPlanning::ECBS<State, Action, int, Conflict, Constraints, Environment>::LowLevelEnvironment, std::hash<State> >::Node, unsigned long> > >::erase(std::_List_const_iterator<std::pair<libMultiRobotPlanning::AStarEpsilon<State, Action, int, libMultiRobotPlanning::ECBS<State, Action, int, Conflict, Constraints, Environment>::LowLevelEnvironment, std::hash<State> >::Node, unsigned long> >) ()
#3  0x0000555555581ca8 in boost::heap::detail::priority_queue_mutable_wrapper<bo---Type <return> to continue, or q <return> to quit---return 
ost::heap::detail::d_ary_heap<libMultiRobotPlanning::AStarEpsilon<State, Action, int, libMultiRobotPlanning::ECBS<State, Action, int, Conflict, Constraints, Environment>::LowLevelEnvironment, std::hash<State> >::Node, boost::parameter::aux::arg_list<boost::heap::mutable_<true>, boost::parameter::aux::arg_list<boost::heap::arity<2u>, boost::parameter::aux::empty_arg_list> >, boost::heap::detail::nop_index_updater> >::erase(boost::heap::detail::priority_queue_mutable_wrapper<boost::heap::detail::d_ary_heap<libMultiRobotPlanning::AStarEpsilon<State, Action, int, libMultiRobotPlanning::ECBS<State, Action, int, Conflict, Constraints, Environment>::LowLevelEnvironment, std::hash<State> >::Node, boost::parameter::aux::arg_list<boost::heap::mutable_<true>, boost::parameter::aux::arg_list<boost::heap::arity<2u>, boost::parameter::aux::empty_arg_list> >, boost::heap::detail::nop_index_updater> >::handle_type) ()
#4  0x000055555557a724 in boost::heap::d_ary_heap<libMultiRobotPlanning::AStarEpsilon<State, Action, int, libMultiRobotPlanning::ECBS<State, Action, int, Conflict, Constraints, Environment>::LowLevelEnvironment, std::hash<State> >::Node, boost::heap::arity<2u>, boost::heap::mutable_<true>, boost::parameter::void_, boost::parameter::void_, boost::parameter::void_, boost::parameter::void_>::erase(boost::heap::detail::priority_queue_mutable_wrapper<boost::heap::detail::d_ary_heap<libMultiRobotPlanning::AStarEpsilon<State, Action, int, libMultiRobotPlanning::ECBS<State, Action, int, Conflict, Constraints, Environment>::LowLevelEnvironment, std::hash<State> >::Node, boost::parameter::aux::arg_list<boost::heap::mutable_<true>, boost::parameter::aux::arg_list<boost::heap::arity<2u>, boost::parameter::aux::empty_arg_list> >, boost::heap::detail::nop_index_updater> >::handle_t---Type <return> to continue, or q <return> to quit---return 
ype) ()
#5  0x0000555555572a53 in libMultiRobotPlanning::AStarEpsilon<State, Action, int, libMultiRobotPlanning::ECBS<State, Action, int, Conflict, Constraints, Environment>::LowLevelEnvironment, std::hash<State> >::search(State const&, libMultiRobotPlanning::PlanResult<State, Action, int>&) ()
#6  0x000055555556bbc8 in libMultiRobotPlanning::ECBS<State, Action, int, Conflict, Constraints, Environment>::search(std::vector<State, std::allocator<State> > const&, std::vector<libMultiRobotPlanning::PlanResult<State, Action, int>, std::allocator<libMultiRobotPlanning::PlanResult<State, Action, int> > >&) ()
#7  0x0000555555561423 in main ()
(gdb) quit
A debugging session is active.

    Inferior 1 [process 17601] will be killed.

Quit anyway? (y or n) y
whoenig commented 3 years ago

I spend some time debugging this. Basically it crashes because it tries to expand some item in the focal list that doesn't exist, even though it should exist. This is caused by the fact that the focal list is by default updated incrementally, rather than recreated every time and some assumptions are violated. You can get rid of the crash by defining REBUILT_FOCAL_LIST. However, your solution will be still incorrect. The root cause of the crash is that your heuristic is no longer admissible, now that you allow a diagonal move. You would need to change the heuristic in https://github.com/whoenig/libMultiRobotPlanning/blob/b3289dce6d561da099a6be8ed5c1fc5f515db354/example/ecbs.cpp#L276-L279 to something admissable, e.g., the Euclidean distance.

H-Bey commented 3 years ago

Thanks for your support! Indeed, I tried to set the heuristic as the Euclidean distance, as follow:

int admissibleHeuristic(const State& s) {
    float q = std::sqrt(std::pow(std::abs(s.x - m_goals[m_agentIdx].x),2) +
           std::pow(std::abs(s.y - m_goals[m_agentIdx].y),2)); 
    return std::floor(q);  
}

However, still have a segmentation fault in some cases. Whereby, using the Chebyshev distance, as follows, treated the issue

int admissibleHeuristic(const State& s) {
    int q=std::abs(s.x - m_goals[m_agentIdx].x);
    int w=std::abs(s.y - m_goals[m_agentIdx].y);
    return std::max({q,w});
}
whoenig commented 3 years ago

For your Euclidean distance, you will need to round down properly (return value is an int), so use std::floor. You could also change the type to floating point, but this hasn't really been tested...

H-Bey commented 3 years ago

I have just updated my last comment, flooring the returned value in the Euclidean distance, but, still facing a segmentation fault. However, as I mentioned using the Chebyshev distance as an admissible heuristic treated the issue.

whoenig commented 3 years ago

Thanks for checking! The Euclidean distance is actually not admissible in your case, because your diagonal action cost is also just 1. The cost would need to be at least sqrt(2) for that to be admissible.

I guess it would be nice to have a checker, that is not just a crash.

H-Bey commented 3 years ago

I also agree with what you mentioned "The cost would need to be at least sqrt(2) for that to be admissible". Otherwise, Chebyshev distance wouldn't solve the seg fault, in the diagonal action case.

whoenig commented 3 years ago

Great that this got resolved - closing.