How to get whole waypoints to the destination

[Kim-mins]

Hi team,

I'm currently trying to get waypoints from the initial position to the target position.

For this, I ran planning simulation after modifying the value forward_path_length in the file
autoware/src/launcher/autoware_launch/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/behavior_path_planner.param.yaml from 300.0 to 5000.0, and get message from the topic /planning/scenario_planning/lane_driving/behavior_planning/path.

It works well with short paths, but for longer paths, the error occurs with the message below:

[component_container_mt-32] terminate called after throwing an instance of 'std::length_error'
[component_container_mt-32]   what():  Exceeded upper bound
[component_container_mt-32] *** Aborted at 1729247892 (unix time) try "date -d @1729247892" if you are using GNU date ***
[component_container_mt-32] PC: @                0x0 (unknown)
[component_container_mt-32] *** SIGABRT (@0x3e80000479e) received by PID 18334 (TID 0x7f92a3fff640) from PID 18334; stack trace: ***
[component_container_mt-32]     @     0x7f9240213a86 google::(anonymous namespace)::FailureSignalHandler()
[component_container_mt-32]     @     0x7f92cf716520 (unknown)
[component_container_mt-32]     @     0x7f92cf76a9fc pthread_kill
[component_container_mt-32]     @     0x7f92cf716476 raise
[component_container_mt-32]     @     0x7f92cf6fc7f3 abort
[component_container_mt-32]     @     0x7f92cf9bfb9e (unknown)
[component_container_mt-32]     @     0x7f92cf9cb20c (unknown)
[component_container_mt-32]     @     0x7f92cf9cb277 std::terminate()
[component_container_mt-32]     @     0x7f92cf9cb4d8 __cxa_throw
[component_container_mt-32]     @     0x7f9297d62af1 _ZN12motion_utils18resampleTrajectoryERKN27autoware_auto_planning_msgs3msg11Trajectory_ISaIvEEERKSt6vectorIdSaIdEEbbb.cold
[component_container_mt-32]     @     0x7f929f10ef96 motion_velocity_smoother::AnalyticalJerkConstrainedSmoother::applyLateralAccelerationFilter()
[component_container_mt-32]     @     0x7f929f1999d5 behavior_velocity_planner::smoothPath()
[component_container_mt-32]     @     0x7f92407b6af6 behavior_velocity_planner::IntersectionModule::calcIntersectionPassingTime()
[component_container_mt-32]     @     0x7f9240723f00 behavior_velocity_planner::IntersectionModule::modifyPathVelocityDetail()
[component_container_mt-32]     @     0x7f9240726ba7 behavior_velocity_planner::IntersectionModule::modifyPathVelocity()
[component_container_mt-32]     @     0x7f929f173bd1 behavior_velocity_planner::SceneModuleManagerInterface::modifyPathVelocity()
[component_container_mt-32]     @     0x7f929f16bc82 behavior_velocity_planner::SceneModuleManagerInterfaceWithRTC::plan()
[component_container_mt-32]     @     0x7f929f5368fe autoware::behavior_velocity_planner::BehaviorVelocityPlannerManager::planPathVelocity()
[component_container_mt-32]     @     0x7f929f44ca1e autoware::behavior_velocity_planner::BehaviorVelocityPlannerNode::generatePath()
[component_container_mt-32]     @     0x7f929f44d08b autoware::behavior_velocity_planner::BehaviorVelocityPlannerNode::onTrigger()
[component_container_mt-32]     @     0x7f929f4984c7 std::_Function_handler<>::_M_invoke()
[component_container_mt-32]     @     0x7f929f494b82 _ZNSt8__detail9__variant17__gen_vtable_implINS0_12_Multi_arrayIPFNS0_21__deduce_visit_resultIvEEOZN6rclcpp23AnySubscriptionCallbackIN27autoware_auto_planning_msgs3msg15PathWithLaneId_ISaIvEEESA_E8dispatchESt10shared_ptrISB_ERKNS5_11MessageInfoEEUlOT_E_RSt7variantIJSt8functionIFvRKSB_EESN_IFvSP_SH_EESN_IFvRKNS5_17SerializedMessageEEESN_IFvSW_SH_EESN_IFvSt10unique_ptrISB_St14default_deleteISB_EEEESN_IFvS14_SH_EESN_IFvS11_ISU_S12_ISU_EEEESN_IFvS1A_SH_EESN_IFvSD_ISO_EEESN_IFvS1F_SH_EESN_IFvSD_ISV_EEESN_IFvS1K_SH_EESN_IFvRKS1F_EESN_IFvS1Q_SH_EESN_IFvRKS1K_EESN_IFvS1W_SH_EESN_IFvSE_EESN_IFvSE_SH_EESN_IFvSD_ISU_EEESN_IFvS25_SH_EEEEEJEEESt16integer_sequenceImJLm8EEEE14__visit_invokeESL_S2B_
[component_container_mt-32]     @     0x7f929f51eba5 rclcpp::Subscription<>::handle_message()
[component_container_mt-32]     @     0x7f92cfc6f7bc rclcpp::Executor::execute_subscription()
[component_container_mt-32]     @     0x7f92cfc6ffbf rclcpp::Executor::execute_any_executable()
[component_container_mt-32]     @     0x7f92cfc7727a rclcpp::executors::MultiThreadedExecutor::run()
[component_container_mt-32]     @     0x7f92cf9f9253 (unknown)
[component_container_mt-32]     @     0x7f92cf768ac3 (unknown)
[component_container_mt-32]     @     0x7f92cf7fa850 (unknown)
[component_container_mt-32]     @                0x0 (unknown)
[planning_validator_node-35] [ERROR] [1729247892.228917764] [planning.planning_validator]: Caution! Invalid Trajectory published.
[planning_validator_node-35] [WARN] [1729247892.229021154] [planning.planning_validator]: planning trajectory is too far from ego!!
[planning_validator_node-35] [WARN] [1729247892.229034524] [planning.planning_validator]: planning trajectory is too far from ego in longitudinal direction!!
[ERROR] [component_container_mt-32]: process has died [pid 18334, exit code -6, cmd '/opt/ros/humble/lib/rclcpp_components/component_container_mt --ros-args -r __node:=behavior_planning_container -r __ns:=/planning/scenario_planning/lane_driving/behavior_planning -p use_sim_time:=False -p wheel_radius:=0.383 -p wheel_width:=0.235 -p wheel_base:=2.79 -p wheel_tread:=1.64 -p front_overhang:=1.0 -p rear_overhang:=1.1 -p left_overhang:=0.128 -p right_overhang:=0.128 -p vehicle_height:=2.5 -p max_steer_angle:=0.7'].

I guess the error occurs due to the modification of forward_path_length, but I need longer path, so I cannot resolve this issue.

Can anyone please tell me how to get whole waypoints from the start to the destination?

Thanks!

[maxime-clem]

I think you are using a slightly older version of Autoware where the Trajectory message has a maximum capacity of 10000 points (https://github.com/tier4/autoware_auto_msgs/blob/tier4/main/autoware_auto_planning_msgs/msg/Trajectory.idl#L14).
A quick solution is to remove this limit by editing the message definition and rebuilding Autoware.

[Kim-mins]

Thank you for the kind response @maxime-clem!
I'll try your solution. Thanks!