MainThread [0x7f527df57e48]
121: while (true) {
122: current_start_time = system_clock::now();
123: next_start_time = current_start_time + interval;
> 124: PlanCycleCallback();
125: std::this_thread::sleep_until(next_start_time);
126: }
127: }
#10 Source "/home/zz/open_learn_code/demo1_auto_drive_ws/src/Dftpav/src/Plan/traj_planner/src/traj_server_ros.cpp", line 168, in PlanCycleCallback [0x7f527df57d3c]
165: fsm_num++;
166: if (fsm_num > 100000||CheckReplan()) {
167: if (next_traj_ == nullptr) {
> 168: Replan();
169: // return;
170: }
171: if (next_traj_ !=nullptr) {
#9 | Source "/home/zz/open_learn_code/demo1_auto_drive_ws/src/Dftpav/src/Plan/traj_planner/src/traj_server_ros.cpp", line 417, in operator()
| 415: if(executing_traj_ ==nullptr){
| 416: p_planner_->set_initial_state(desired_state);
| > 417: if (trajplan()!= kSuccess) {
| 418: Display();
| 419: return kWrongStatus;
Source "/usr/include/c++/7/bits/std_function.h", line 706, in Replan [0x7f527df570d1]
703: {
704: if (_M_empty())
705: __throw_bad_function_call();
> 706: return _M_invoker(_M_functor, std::forward<ArgTypes>(args)...);
707: }
708:
709: #if cpp_rtti
#8 Source "/home/zz/open_learn_code/demo1_auto_drive_ws/src/Dftpav/src/Plan/traj_planner/src/traj_manager.cpp", line 203, in RunOnceParking [0x7f527a14a299]
200: }
201:
202: double frontendt1 = ros::Time::now().toSec();
> 203: if (getKinoPath(parking_end) != kSuccess){
204: LOG(ERROR) << "[PolyTrajManager Parking] fail to get the front-end.\n";
205: return kWrongStatus;
206: }
#7 Source "/home/zz/open_learn_code/demo1_auto_drive_ws/src/Dftpav/src/Plan/traj_planner/src/traj_manager.cpp", line 85, in getKinoPath [0x7f527a140562]
82: // start_state << -26.3909, 20.7379 ,1.57702 , 0; end_state << -45.4141, 10.3171 ,0.0600779 , 0.01;
83: std::cout<<"start state: "<<start_state.transpose()<<" end_state: "<<end_state.transpose()<<std::endl;
84: std::cout<<"init ctrl: "<<init_ctrl.transpose()<<std::endl;
> 85: int status = kino_path_finder->search(start_state, init_ctrl, end_state, true);
86: double searcht2 = ros::Time::now().toSec();
87: if (status == path_searching::KinoAstar::NO_PATH)
88: {
#6 Source "/home/zz/open_learn_code/demo1_auto_drive_ws/src/Dftpav/src/Plan/traj_planner/src/kino_astar.cpp", line 92, in search [0x7f527a16661d]
89: double t1 = ros::Time::now().toSec();
90: if((cur_node->state.head(2) - end_state.head(2)).norm()<15.0&& initsearch){
91:
> 92: is_shot_sucess(cur_node->state,end_state.head(3));
93: }
94: double t2 = ros::Time::now().toSec();
95: // std::cout<<"one-shot time: "<<(t2-t1)*1000<<" ms"<<std::endl;
#5 Source "/home/zz/open_learn_code/demo1_auto_drive_ws/src/Dftpav/src/Plan/traj_planner/src/kino_astar.cpp", line 309, in is_shot_sucess [0x7f527a161d13]
306: std::vectorEigen::Vector3d path_list;
307: double len;
308: double ct1 = ros::Time::now().toSec();
> 309: computeShotTraj(state1,state2,path_list,len);
310: double ct2 = ros::Time::now().toSec();
311: // std::cout<<"compute shot traj time: "<<(ct2-ct1)*1000.0<<" ms"<<std::endl;
312: bool is_occ = false;
#4 Source "/home/zz/open_learn_code/demo1_auto_drive_ws/src/Dftpav/src/Plan/traj_planner/src/kino_astar.cpp", line 332, in computeShotTraj [0x7f527a1618c8]
329: double& len){
330: namespace ob = ompl::base;
331: namespace og = ompl::geometric;
> 332: ob::ScopedState<> from(shotptr), to(shotptr), s(shotptr);
333: from[0] = state1[0]; from[1] = state1[1]; from[2] = state1[2];
334: to[0] = state2[0]; to[1] = state2[1]; to[2] = state2[2];
335: std::vector reals;
#3 Source "/opt/ros/melodic/include/ompl/base/ScopedState.h", line 86, in ScopedState [0x7f527a16884e]
83: allocate a state. */
84: explicit ScopedState(StateSpacePtr space) : space(std::move(space))
85: {
> 86: State *s = space->allocState();
87:
88: // ideally, this should be a dynamic_cast and we
89: // should throw an exception in case of
#2 Object "/usr/lib/x86_64-linux-gnu/libompl.so.1.2.1", at 0x7f5277537ac6, in ompl::base::CompoundStateSpace::printState(ompl::base::State const*, std::ostream&) const
#1 Object "/usr/lib/x86_64-linux-gnu/libstdc++.so.6.0.25", at 0x7f527c7c2a77, in std::basic_ostream<char, std::char_traits >& std::__ostream_insert<char, std::char_traits >(std::basic_ostream<char, std::char_traits >&, char const*, long)
#0 Object "/usr/lib/x86_64-linux-gnu/libstdc++.so.6.0.25", at 0x7f527c7c23f6, in std::ostream::sentry::sentry(std::ostream&)
Segmentation fault (Signal sent by the kernel [(nil)])
[park_0-1] process has died [pid 30974, exit code -11, cmd /home/zz/open_learn_code/demo1_auto_drive_ws/devel/lib/planning_integrated/park ~arena_info_static:=/arena_info_static ~arena_info_dynamic:=/arena_info_dynamic ~ctrl:=/ctrl/agent_0 __name:=park_0 __log:=/home/zz/.ros/log/88641df6-4357-11ef-9389-50eb71d1e115/park_0-1.log].
log file: /home/zz/.ros/log/88641df6-4357-11ef-9389-50eb71d1e115/park_0-1*.log
React
Typescript
Django
Laravel
Facebook
Microsoft
Google
Alibaba
D3
Tencent