[Openplanner 2.5+] Failed to achieve lane change with errors in op_trajectory_evaluator
Hi all, I am trying to achieve lane change using autoware1.13 + OpenPlanner2.5 from hatem's github. Ubuntu 18.04 + ros melodic.
You can check this video to see my steps. The problem occurs at 3:10 when I tried to enable the op_trajectory_evaluator, with error message in rviz:
\#\#\# Waiting until nodes are synchronized, Generated rollouts = 27, But they should = 9
As the video shows, I enabled the op_global_planner to generate lane-change global path. Then I enabled op_common_params, op_trajectory_generator, op_motion_predictor, and I could visualize the generated roll-outs in rviz. But when I trie to launch op_trajectory_evaluator, there occured the error above about the number of generated roll-outs mismatching.
In my configurations, I set rollouts number to be 8. As the number of global paths was 3, there were totally 3*(8+1) = 27 rollouts generated by op_trajectory_generator, I think this could not pass the check in op_trajectory_evaluator_core.cpp line 234:
if(msg->lanes.size() != (m_PlanningParams.rollOutNumber + 1))
{
std::cout << " ### Waiting until nodes are synchronized, Generated rollouts = " << msg->lanes.size() << ", But They should = " << m_PlanningParams.rollOutNumber + 1 << std::endl;
return;
}
I am wondering how to solve this problem of generated rollouts size mismatch. Could you offer me some advice? @Hatem
Thank you!
Good observation, Have you tried to disable this condition or update it to:
if(msg->lanes.size() != m_GlobalPaths.size()*(m_PlanningParams.rollOutNumber + 1))
Hope this will solve this issue, if so please send and MR.
Regards,
@Hatem
I try to update the condition as you advised and it works now. Check my new video of lane change demo. I get two questions now:
Is this a common issue? I see others could have op_trajectory_evaluator work correctly without updating the condition, such as in this video
I tried this solution on autoware1.15+openplanner2.5, and the op_trajectory_evaluator worked correctly. But the pure_pursuit node seemed not receiving /current_velocity and /current_pose, even though I had enabled wf_simulator and vel_pose_connect. (Sorry that I didn't record a video for this problem, if you need I will update one)
That is different issue. you need to get the op_car_simulator to work. The other lane's planes won't be updated unless the vehicle can't switch to the goal lane.
I don't understand you question
@Hatem Thank you for your reply.
The 2nd question is about pure_pursuit in 1.15, it is not publishing /twist_raw by default. I solved this by setting output_interface_ = "all" in pure_pursuit_core.cpp. (It is a question about using autoware1.15+openplanner2.5 with LGSVL, because LGSVL needs /twist_raw).
Finally, updating the condition in op_trajectory_evaluator_core.cpp works perfectly for me on autoware1.13+op planner2.5 to enable lane change. Thanks again for your advice!
Great,
Can you add your solution as an answer to this question so others can benefit from it.