No version for distro humble showing github. Known supported distros are highlighted in the buttons above.
Package symbol

motion_velocity_smoother package from autodrrt repo

autonomous_emergency_braking control_performance_analysis control_validator external_cmd_selector joy_controller lane_departure_checker mpc_lateral_controller obstacle_collision_checker operation_mode_transition_manager pid_longitudinal_controller predicted_path_checker pure_pursuit shift_decider trajectory_follower_base trajectory_follower_node vehicle_cmd_gate diagnostic_converter kinematic_evaluator localization_evaluator planning_evaluator ekf_localizer geo_pose_projector gyro_odometer ar_tag_based_localizer landmark_manager localization_error_monitor localization_gnss localization_util ndt_scan_matcher pose2twist pose_initializer pose_instability_detector stop_filter tree_structured_parzen_estimator twist2accel yabloc_common yabloc_image_processing yabloc_monitor yabloc_particle_filter yabloc_pose_initializer map_height_fitter map_loader map_projection_loader map_tf_generator lanelet2_map_preprocessor ros2_bevdet bytetrack cluster_merger compare_map_segmentation crosswalk_traffic_light_estimator detected_object_feature_remover detected_object_validation detection_by_tracker elevation_map_loader euclidean_cluster front_vehicle_velocity_estimator ground_segmentation heatmap_visualizer image_projection_based_fusion lidar_apollo_instance_segmentation lidar_apollo_segmentation_tvm lidar_apollo_segmentation_tvm_nodes lidar_centerpoint lidar_centerpoint_tvm map_based_prediction multi_object_tracker object_merger object_range_splitter object_velocity_splitter occupancy_grid_map_outlier_filter probabilistic_occupancy_grid_map radar_crossing_objects_noise_filter radar_fusion_to_detected_object radar_object_clustering radar_object_tracker radar_tracks_msgs_converter shape_estimation simple_object_merger tensorrt_classifier tensorrt_yolo tensorrt_yolox tracking_object_merger traffic_light_arbiter traffic_light_classifier traffic_light_fine_detector traffic_light_map_based_detector traffic_light_multi_camera_fusion traffic_light_occlusion_predictor traffic_light_ssd_fine_detector traffic_light_visualization behavior_path_avoidance_by_lane_change_module behavior_path_avoidance_module behavior_path_external_request_lane_change_module behavior_path_goal_planner_module behavior_path_lane_change_module behavior_path_planner behavior_path_planner_common behavior_path_side_shift_module behavior_path_start_planner_module behavior_velocity_blind_spot_module behavior_velocity_crosswalk_module behavior_velocity_detection_area_module behavior_velocity_intersection_module behavior_velocity_no_drivable_lane_module behavior_velocity_no_stopping_area_module behavior_velocity_occlusion_spot_module behavior_velocity_out_of_lane_module behavior_velocity_planner behavior_velocity_planner_common behavior_velocity_run_out_module behavior_velocity_speed_bump_module behavior_velocity_stop_line_module behavior_velocity_template_module behavior_velocity_traffic_light_module behavior_velocity_virtual_traffic_light_module behavior_velocity_walkway_module costmap_generator external_velocity_limit_selector freespace_planner freespace_planning_algorithms mission_planner motion_velocity_smoother objects_of_interest_marker_interface obstacle_avoidance_planner obstacle_cruise_planner obstacle_stop_planner obstacle_velocity_limiter path_smoother planning_debug_tools planning_test_utils planning_topic_converter planning_validator route_handler rtc_interface rtc_replayer bezier_sampler frenet_planner path_sampler sampler_common scenario_selector static_centerline_optimizer surround_obstacle_checker gnss_poser image_diagnostics image_transport_decompressor imu_corrector livox_tag_filter pointcloud_preprocessor radar_scan_to_pointcloud2 radar_static_pointcloud_filter radar_threshold_filter radar_tracks_noise_filter tier4_pcl_extensions vehicle_velocity_converter autoware_auto_msgs_adapter bluetooth_monitor component_state_monitor default_ad_api ad_api_adaptors ad_api_visualizers automatic_pose_initializer diagnostic_graph_aggregator dummy_diag_publisher dummy_infrastructure duplicated_node_checker emergency_handler mrm_comfortable_stop_operator mrm_emergency_stop_operator system_error_monitor system_monitor topic_state_monitor velodyne_monitor accel_brake_map_calibrator external_cmd_converter raw_vehicle_cmd_converter steer_offset_estimator vehicle_info_util launch loop_sim ssh_machine gpudirect-dds autoware_ad_api_specs autoware_adapi_v1_msgs autoware_adapi_version_msgs autoware_auto_common autoware_auto_geometry autoware_auto_control_msgs autoware_auto_geometry_msgs autoware_auto_mapping_msgs autoware_auto_msgs autoware_auto_perception_msgs autoware_auto_planning_msgs autoware_auto_system_msgs autoware_auto_vehicle_msgs autoware_auto_perception_rviz_plugin autoware_auto_tf2 autoware_cmake autoware_lint_common autoware_utils lanelet2_extension autoware_common_msgs autoware_control_msgs autoware_localization_msgs autoware_map_msgs autoware_perception_msgs autoware_planning_msgs autoware_sensing_msgs autoware_system_msgs autoware_vehicle_msgs autoware_point_types autoware_testing bag_time_manager_rviz_plugin component_interface_specs component_interface_tools component_interface_utils cuda_utils fake_test_node geography_utils global_parameter_loader glog_component goal_distance_calculator grid_map_utils interpolation kalman_filter motion_utils object_recognition_utils osqp_interface path_distance_calculator perception_utils polar_grid qp_interface rtc_manager_rviz_plugin signal_processing tensorrt_common tier4_adapi_rviz_plugin tier4_api_utils tier4_automatic_goal_rviz_plugin tier4_autoware_utils tier4_calibration_rviz_plugin tier4_camera_view_rviz_plugin tier4_control_rviz_plugin tier4_datetime_rviz_plugin tier4_debug_rviz_plugin tier4_debug_tools tier4_localization_rviz_plugin tier4_perception_rviz_plugin tier4_planning_rviz_plugin tier4_screen_capture_rviz_plugin tier4_simulated_clock_rviz_plugin tier4_state_rviz_plugin tier4_system_rviz_plugin tier4_target_object_type_rviz_plugin tier4_traffic_light_rviz_plugin tier4_vehicle_rviz_plugin time_utils simulator_compatibility_test traffic_light_recognition_marker_publisher traffic_light_utils tvm_utility dma_customer_msg dma_transfer eagleye_coordinate eagleye_navigation eagleye_msgs eagleye_rt eagleye_can_velocity_converter eagleye_fix2kml eagleye_geo_pose_converter eagleye_geo_pose_fusion eagleye_gnss_converter eagleye_tf llh_converter morai_msgs mussp ndt_omp orocos_kdl python_orocos_kdl pointcloud_to_laserscan rtklib_bridge rtklib_msgs autoware_external_api_msgs autoware_iv_external_api_adaptor autoware_iv_internal_api_adaptor awapi_awiv_adapter tier4_api_msgs tier4_auto_msgs_converter tier4_control_msgs tier4_debug_msgs tier4_external_api_msgs tier4_hmi_msgs tier4_localization_msgs tier4_map_msgs tier4_perception_msgs tier4_planning_msgs tier4_rtc_msgs tier4_simulation_msgs tier4_system_msgs tier4_v2x_msgs tier4_vehicle_msgs tier4_autoware_api_launch tier4_control_launch tier4_localization_launch tier4_map_launch tier4_perception_launch tier4_planning_launch tier4_sensing_launch tier4_simulator_launch tier4_system_launch tier4_vehicle_launch fastrtps cyclonedds lanelet2 lanelet2_core lanelet2_examples lanelet2_io lanelet2_maps lanelet2_matching lanelet2_projection lanelet2_python lanelet2_routing lanelet2_traffic_rules lanelet2_validation sophus angles behaviortree_cpp_v3 bond bond_core bondcpp bondpy smclib test_bond cudnn_cmake_module diagnostic_aggregator diagnostic_common_diagnostics diagnostic_updater diagnostics self_test filters geodesy geographic_info geographic_msgs grid_map grid_map_cmake_helpers grid_map_core grid_map_costmap_2d grid_map_cv grid_map_demos grid_map_filters grid_map_loader grid_map_msgs grid_map_octomap grid_map_pcl grid_map_ros grid_map_rviz_plugin grid_map_sdf grid_map_visualization mrt_cmake_modules nav2_amcl nav2_behavior_tree nav2_behaviors nav2_bringup nav2_bt_navigator nav2_collision_monitor nav2_common nav2_controller nav2_core nav2_costmap_2d costmap_queue dwb_core dwb_critics dwb_msgs dwb_plugins nav2_dwb_controller nav_2d_msgs nav_2d_utils nav2_lifecycle_manager nav2_map_server nav2_msgs nav2_navfn_planner nav2_planner nav2_regulated_pure_pursuit_controller nav2_rotation_shim_controller nav2_rviz_plugins nav2_simple_commander nav2_smac_planner nav2_smoother nav2_system_tests nav2_theta_star_planner nav2_util nav2_velocity_smoother nav2_voxel_grid nav2_waypoint_follower navigation2 dynamic_edt_3d octomap octovis octomap_msgs osqp_vendor pacmod3_msgs pcl_msgs pcl_conversions pcl_ros perception_pcl point_cloud_msg_wrapper radar_msgs can_msgs rqt_tf_tree tensorrt_cmake_module topic_tools topic_tools_interfaces tvm_vendor cv_bridge image_geometry opencv_tests vision_opencv xacro rviz2 rviz_assimp_vendor rviz_common rviz_default_plugins rviz_ogre_vendor rviz_rendering rviz_rendering_tests rviz_visual_testing_framework dummy_perception_publisher fault_injection simple_planning_simulator image_view v4l2_camera aion_interface carla_vehicle_can cgi430_can_driver cgi610_driver ARS408_driver common_sensor_launch kunyi_sensor_kit_description kunyi_sensor_kit_launch classformsg node_v2x data_format_dump data_preprocess_launch lidar_centerpoint_collect lidar_saver message_sync time_cal camera_calibration direct_visual_lidar_calibration multi_lidar_calibration

ROS Distro
github

Package Summary

Tags No category tags.
Version 0.1.0
License Apache License 2.0
Build type AMENT_CMAKE
Use RECOMMENDED

Repository Summary

Description
Checkout URI https://github.com/ieiauto/autodrrt.git
VCS Type git
VCS Version main
Last Updated 2025-05-30
Dev Status UNKNOWN
Released UNRELEASED
Tags No category tags.
Contributing Help Wanted (-)
Good First Issues (-)
Pull Requests to Review (-)

Package Description

The motion_velocity_smoother package

Additional Links

No additional links.

Maintainers

  • Fumiya Watanabe
  • Takamasa Horibe
  • Makoto Kurihara

Authors

  • Takamasa Horibe
  • Fumiya Watanabe
  • Yutaka Shimizu
  • Makoto Kurihara

Motion Velocity Smoother

Purpose

motion_velocity_smootherは目標軌道上の各点における望ましい車速を計画して出力するモジュールである。 このモジュールは、速度の最大化と乗り心地の良さを両立するために、事前に指定された制限速度、制限加速度および制限躍度の範囲で車速を計画する。 加速度や躍度の制限を与えることは車速の変化を滑らかにすることに対応するため、このモジュールをmotion_velocity_smootherと呼んでいる。

Inner-workings / Algorithms

Flow chart

motion_velocity_smoother_flow

Extract trajectory

自車後輪軸中心位置に最も近い参照経路上の点に対し、extract_behind_distだけ戻った点からextract_ahead_distだけ進んだ点までの参照経路を抜き出す。

Apply external velocity limit

モジュール外部から指定された速度制限を適用する。 ここで扱う外部の速度制限は/planning/scenario_planning/max_velocityの topic で渡されるもので、地図上で設定された速度制限など、参照経路にすでに設定されている制限速度とは別である。 外部から指定される速度制限は、パラメータで指定されている減速度および躍度の制限の範囲で減速可能な位置から速度制限を適用する。

Apply stop approaching velocity

停止点に近づいたときの速度を設定する。障害物近傍まで近づく場合や、正着精度向上などの目的に用いる。

Apply lateral acceleration limit

経路の曲率に応じて、指定された最大横加速度max_lateral_accelを超えない速度を制限速度として設定する。ただし、制限速度はmin_curve_velocityを下回らないように設定する。

Resample trajectory

指定された時間間隔で経路の点を再サンプルする。ただし、経路全体の長さはmin_trajectory_lengthからmax_trajectory_lengthの間となるように再サンプルを行い、点の間隔はmin_trajectory_interval_distanceより小さくならないようにする。 現在車速でresample_timeの間進む距離までは密にサンプリングし、それ以降は疎にサンプリングする。 この方法でサンプリングすることで、低速時は密に、高速時は疎にサンプルされるため、停止精度と計算負荷軽減の両立を図っている。

Calculate initial state

速度計画のための初期値を計算する。初期値は状況に応じて下表のように計算する。

状況 初期速度 初期加速度
最初の計算時 現在車速 0.0
発進時 engage_velocity engage_acceleration
現在車速と計画車速が乖離 現在車速 前回計画値
通常時 前回計画値 前回計画値

Smooth velocity

速度の計画を行う。速度計画のアルゴリズムはJerkFiltered, L2, Linfの 3 種類のうちからコンフィグで指定する。 最適化のソルバは OSQP[1]を利用する。

JerkFiltered

速度の 2 乗(最小化で表すため負値で表現)、制限速度逸脱量の 2 乗、制限加度逸脱量の 2 乗、制限ジャーク逸脱量の 2 乗、ジャークの 2 乗の総和を最小化する。

L2

速度の 2 乗(最小化で表すため負値で表現)、制限速度逸脱量の 2 乗、制限加度逸脱量の 2 乗、疑似ジャーク[2]の 2 乗の総和を最小化する。

Linf

速度の 2 乗(最小化で表すため負値で表現)、制限速度逸脱量の 2 乗、制限加度逸脱量の 2 乗の総和と疑似ジャーク[2]の絶対最大値の和の最小化する。

Post process

計画された軌道の後処理を行う。

  • 停止点より先の速度を 0 に設定
  • 速度がパラメータで与えられるmax_velocity以下となるように設定
  • 自車位置より手前の点における速度を設定
  • Trajectory の仕様に合わせてリサンプリング(post resampling)
  • デバッグデータの出力

最適化の計算が終わったあと、次のノードに経路を渡す前にpost resamplingと呼ばれるリサンプリングを行う。ここで再度リサンプリングを行っている理由としては、最適化前で必要な経路間隔と後段のモジュールに渡す経路間隔が必ずしも一致していないからであり、その差を埋めるために再度サンプリングを行っている。そのため、post resamplingでは後段モジュールの経路仕様を確認してパラメータを決める必要がある。なお、最適化アルゴリズムの計算負荷が高く、最初のリサンプリングで経路間隔が後段モジュールの経路仕様より疎になっている場合、post resamplingで経路を蜜にリサンプリングする。逆に最適化アルゴリズムの計算負荷が小さく、最初のリサンプリングで経路間隔が後段の経路仕様より蜜になっている場合は、post resamplingで経路をその仕様に合わせて疎にリサンプリングする。

Inputs / Outputs

Input

Name Type Description
~/input/trajectory autoware_auto_planning_msgs/Trajectory Reference trajectory
/planning/scenario_planning/max_velocity std_msgs/Float32 External velocity limit [m/s]
/localization/kinematic_state nav_msgs/Odometry Current odometry
/tf tf2_msgs/TFMessage TF
/tf_static tf2_msgs/TFMessage TF static

Output

Name Type Description
~/output/trajectory autoware_auto_planning_msgs/Trajectory Modified trajectory
/planning/scenario_planning/current_max_velocity std_msgs/Float32 Current external velocity limit [m/s]
~/closest_velocity std_msgs/Float32 Planned velocity closest to ego base_link (for debug)
~/closest_acceleration std_msgs/Float32 Planned acceleration closest to ego base_link (for debug)
~/closest_jerk std_msgs/Float32 Planned jerk closest to ego base_link (for debug)
~/debug/trajectory_raw autoware_auto_planning_msgs/Trajectory Extracted trajectory (for debug)

File truncated at 100 lines see the full file

CHANGELOG
No CHANGELOG found.

Launch files

  • launch/motion_velocity_smoother.launch.xml
      • common_param_path [default: $(find-pkg-share motion_velocity_smoother)/config/default_common.param.yaml]
      • nearest_search_param_path
      • input_trajectory [default: /planning/scenario_planning/scenario_selector/trajectory]
      • output_trajectory [default: /planning/scenario_planning/trajectory]
      • publish_debug_trajs [default: false]
      • velocity_smoother_type [default: JerkFiltered]
      • param_path [default: $(find-pkg-share motion_velocity_smoother)/config/default_motion_velocity_smoother.param.yaml]
      • velocity_smoother_param_path [default: $(find-pkg-share motion_velocity_smoother)/config/$(var velocity_smoother_type).param.yaml]

Messages

No message files found.

Services

No service files found

Plugins

No plugins found.

Recent questions tagged motion_velocity_smoother at Robotics Stack Exchange

No version for distro jazzy showing github. Known supported distros are highlighted in the buttons above.
Package symbol

motion_velocity_smoother package from autodrrt repo

autonomous_emergency_braking control_performance_analysis control_validator external_cmd_selector joy_controller lane_departure_checker mpc_lateral_controller obstacle_collision_checker operation_mode_transition_manager pid_longitudinal_controller predicted_path_checker pure_pursuit shift_decider trajectory_follower_base trajectory_follower_node vehicle_cmd_gate diagnostic_converter kinematic_evaluator localization_evaluator planning_evaluator ekf_localizer geo_pose_projector gyro_odometer ar_tag_based_localizer landmark_manager localization_error_monitor localization_gnss localization_util ndt_scan_matcher pose2twist pose_initializer pose_instability_detector stop_filter tree_structured_parzen_estimator twist2accel yabloc_common yabloc_image_processing yabloc_monitor yabloc_particle_filter yabloc_pose_initializer map_height_fitter map_loader map_projection_loader map_tf_generator lanelet2_map_preprocessor ros2_bevdet bytetrack cluster_merger compare_map_segmentation crosswalk_traffic_light_estimator detected_object_feature_remover detected_object_validation detection_by_tracker elevation_map_loader euclidean_cluster front_vehicle_velocity_estimator ground_segmentation heatmap_visualizer image_projection_based_fusion lidar_apollo_instance_segmentation lidar_apollo_segmentation_tvm lidar_apollo_segmentation_tvm_nodes lidar_centerpoint lidar_centerpoint_tvm map_based_prediction multi_object_tracker object_merger object_range_splitter object_velocity_splitter occupancy_grid_map_outlier_filter probabilistic_occupancy_grid_map radar_crossing_objects_noise_filter radar_fusion_to_detected_object radar_object_clustering radar_object_tracker radar_tracks_msgs_converter shape_estimation simple_object_merger tensorrt_classifier tensorrt_yolo tensorrt_yolox tracking_object_merger traffic_light_arbiter traffic_light_classifier traffic_light_fine_detector traffic_light_map_based_detector traffic_light_multi_camera_fusion traffic_light_occlusion_predictor traffic_light_ssd_fine_detector traffic_light_visualization behavior_path_avoidance_by_lane_change_module behavior_path_avoidance_module behavior_path_external_request_lane_change_module behavior_path_goal_planner_module behavior_path_lane_change_module behavior_path_planner behavior_path_planner_common behavior_path_side_shift_module behavior_path_start_planner_module behavior_velocity_blind_spot_module behavior_velocity_crosswalk_module behavior_velocity_detection_area_module behavior_velocity_intersection_module behavior_velocity_no_drivable_lane_module behavior_velocity_no_stopping_area_module behavior_velocity_occlusion_spot_module behavior_velocity_out_of_lane_module behavior_velocity_planner behavior_velocity_planner_common behavior_velocity_run_out_module behavior_velocity_speed_bump_module behavior_velocity_stop_line_module behavior_velocity_template_module behavior_velocity_traffic_light_module behavior_velocity_virtual_traffic_light_module behavior_velocity_walkway_module costmap_generator external_velocity_limit_selector freespace_planner freespace_planning_algorithms mission_planner motion_velocity_smoother objects_of_interest_marker_interface obstacle_avoidance_planner obstacle_cruise_planner obstacle_stop_planner obstacle_velocity_limiter path_smoother planning_debug_tools planning_test_utils planning_topic_converter planning_validator route_handler rtc_interface rtc_replayer bezier_sampler frenet_planner path_sampler sampler_common scenario_selector static_centerline_optimizer surround_obstacle_checker gnss_poser image_diagnostics image_transport_decompressor imu_corrector livox_tag_filter pointcloud_preprocessor radar_scan_to_pointcloud2 radar_static_pointcloud_filter radar_threshold_filter radar_tracks_noise_filter tier4_pcl_extensions vehicle_velocity_converter autoware_auto_msgs_adapter bluetooth_monitor component_state_monitor default_ad_api ad_api_adaptors ad_api_visualizers automatic_pose_initializer diagnostic_graph_aggregator dummy_diag_publisher dummy_infrastructure duplicated_node_checker emergency_handler mrm_comfortable_stop_operator mrm_emergency_stop_operator system_error_monitor system_monitor topic_state_monitor velodyne_monitor accel_brake_map_calibrator external_cmd_converter raw_vehicle_cmd_converter steer_offset_estimator vehicle_info_util launch loop_sim ssh_machine gpudirect-dds autoware_ad_api_specs autoware_adapi_v1_msgs autoware_adapi_version_msgs autoware_auto_common autoware_auto_geometry autoware_auto_control_msgs autoware_auto_geometry_msgs autoware_auto_mapping_msgs autoware_auto_msgs autoware_auto_perception_msgs autoware_auto_planning_msgs autoware_auto_system_msgs autoware_auto_vehicle_msgs autoware_auto_perception_rviz_plugin autoware_auto_tf2 autoware_cmake autoware_lint_common autoware_utils lanelet2_extension autoware_common_msgs autoware_control_msgs autoware_localization_msgs autoware_map_msgs autoware_perception_msgs autoware_planning_msgs autoware_sensing_msgs autoware_system_msgs autoware_vehicle_msgs autoware_point_types autoware_testing bag_time_manager_rviz_plugin component_interface_specs component_interface_tools component_interface_utils cuda_utils fake_test_node geography_utils global_parameter_loader glog_component goal_distance_calculator grid_map_utils interpolation kalman_filter motion_utils object_recognition_utils osqp_interface path_distance_calculator perception_utils polar_grid qp_interface rtc_manager_rviz_plugin signal_processing tensorrt_common tier4_adapi_rviz_plugin tier4_api_utils tier4_automatic_goal_rviz_plugin tier4_autoware_utils tier4_calibration_rviz_plugin tier4_camera_view_rviz_plugin tier4_control_rviz_plugin tier4_datetime_rviz_plugin tier4_debug_rviz_plugin tier4_debug_tools tier4_localization_rviz_plugin tier4_perception_rviz_plugin tier4_planning_rviz_plugin tier4_screen_capture_rviz_plugin tier4_simulated_clock_rviz_plugin tier4_state_rviz_plugin tier4_system_rviz_plugin tier4_target_object_type_rviz_plugin tier4_traffic_light_rviz_plugin tier4_vehicle_rviz_plugin time_utils simulator_compatibility_test traffic_light_recognition_marker_publisher traffic_light_utils tvm_utility dma_customer_msg dma_transfer eagleye_coordinate eagleye_navigation eagleye_msgs eagleye_rt eagleye_can_velocity_converter eagleye_fix2kml eagleye_geo_pose_converter eagleye_geo_pose_fusion eagleye_gnss_converter eagleye_tf llh_converter morai_msgs mussp ndt_omp orocos_kdl python_orocos_kdl pointcloud_to_laserscan rtklib_bridge rtklib_msgs autoware_external_api_msgs autoware_iv_external_api_adaptor autoware_iv_internal_api_adaptor awapi_awiv_adapter tier4_api_msgs tier4_auto_msgs_converter tier4_control_msgs tier4_debug_msgs tier4_external_api_msgs tier4_hmi_msgs tier4_localization_msgs tier4_map_msgs tier4_perception_msgs tier4_planning_msgs tier4_rtc_msgs tier4_simulation_msgs tier4_system_msgs tier4_v2x_msgs tier4_vehicle_msgs tier4_autoware_api_launch tier4_control_launch tier4_localization_launch tier4_map_launch tier4_perception_launch tier4_planning_launch tier4_sensing_launch tier4_simulator_launch tier4_system_launch tier4_vehicle_launch fastrtps cyclonedds lanelet2 lanelet2_core lanelet2_examples lanelet2_io lanelet2_maps lanelet2_matching lanelet2_projection lanelet2_python lanelet2_routing lanelet2_traffic_rules lanelet2_validation sophus angles behaviortree_cpp_v3 bond bond_core bondcpp bondpy smclib test_bond cudnn_cmake_module diagnostic_aggregator diagnostic_common_diagnostics diagnostic_updater diagnostics self_test filters geodesy geographic_info geographic_msgs grid_map grid_map_cmake_helpers grid_map_core grid_map_costmap_2d grid_map_cv grid_map_demos grid_map_filters grid_map_loader grid_map_msgs grid_map_octomap grid_map_pcl grid_map_ros grid_map_rviz_plugin grid_map_sdf grid_map_visualization mrt_cmake_modules nav2_amcl nav2_behavior_tree nav2_behaviors nav2_bringup nav2_bt_navigator nav2_collision_monitor nav2_common nav2_controller nav2_core nav2_costmap_2d costmap_queue dwb_core dwb_critics dwb_msgs dwb_plugins nav2_dwb_controller nav_2d_msgs nav_2d_utils nav2_lifecycle_manager nav2_map_server nav2_msgs nav2_navfn_planner nav2_planner nav2_regulated_pure_pursuit_controller nav2_rotation_shim_controller nav2_rviz_plugins nav2_simple_commander nav2_smac_planner nav2_smoother nav2_system_tests nav2_theta_star_planner nav2_util nav2_velocity_smoother nav2_voxel_grid nav2_waypoint_follower navigation2 dynamic_edt_3d octomap octovis octomap_msgs osqp_vendor pacmod3_msgs pcl_msgs pcl_conversions pcl_ros perception_pcl point_cloud_msg_wrapper radar_msgs can_msgs rqt_tf_tree tensorrt_cmake_module topic_tools topic_tools_interfaces tvm_vendor cv_bridge image_geometry opencv_tests vision_opencv xacro rviz2 rviz_assimp_vendor rviz_common rviz_default_plugins rviz_ogre_vendor rviz_rendering rviz_rendering_tests rviz_visual_testing_framework dummy_perception_publisher fault_injection simple_planning_simulator image_view v4l2_camera aion_interface carla_vehicle_can cgi430_can_driver cgi610_driver ARS408_driver common_sensor_launch kunyi_sensor_kit_description kunyi_sensor_kit_launch classformsg node_v2x data_format_dump data_preprocess_launch lidar_centerpoint_collect lidar_saver message_sync time_cal camera_calibration direct_visual_lidar_calibration multi_lidar_calibration

ROS Distro
github

Package Summary

Tags No category tags.
Version 0.1.0
License Apache License 2.0
Build type AMENT_CMAKE
Use RECOMMENDED

Repository Summary

Description
Checkout URI https://github.com/ieiauto/autodrrt.git
VCS Type git
VCS Version main
Last Updated 2025-05-30
Dev Status UNKNOWN
Released UNRELEASED
Tags No category tags.
Contributing Help Wanted (-)
Good First Issues (-)
Pull Requests to Review (-)

Package Description

The motion_velocity_smoother package

Additional Links

No additional links.

Maintainers

  • Fumiya Watanabe
  • Takamasa Horibe
  • Makoto Kurihara

Authors

  • Takamasa Horibe
  • Fumiya Watanabe
  • Yutaka Shimizu
  • Makoto Kurihara

Motion Velocity Smoother

Purpose

motion_velocity_smootherは目標軌道上の各点における望ましい車速を計画して出力するモジュールである。 このモジュールは、速度の最大化と乗り心地の良さを両立するために、事前に指定された制限速度、制限加速度および制限躍度の範囲で車速を計画する。 加速度や躍度の制限を与えることは車速の変化を滑らかにすることに対応するため、このモジュールをmotion_velocity_smootherと呼んでいる。

Inner-workings / Algorithms

Flow chart

motion_velocity_smoother_flow

Extract trajectory

自車後輪軸中心位置に最も近い参照経路上の点に対し、extract_behind_distだけ戻った点からextract_ahead_distだけ進んだ点までの参照経路を抜き出す。

Apply external velocity limit

モジュール外部から指定された速度制限を適用する。 ここで扱う外部の速度制限は/planning/scenario_planning/max_velocityの topic で渡されるもので、地図上で設定された速度制限など、参照経路にすでに設定されている制限速度とは別である。 外部から指定される速度制限は、パラメータで指定されている減速度および躍度の制限の範囲で減速可能な位置から速度制限を適用する。

Apply stop approaching velocity

停止点に近づいたときの速度を設定する。障害物近傍まで近づく場合や、正着精度向上などの目的に用いる。

Apply lateral acceleration limit

経路の曲率に応じて、指定された最大横加速度max_lateral_accelを超えない速度を制限速度として設定する。ただし、制限速度はmin_curve_velocityを下回らないように設定する。

Resample trajectory

指定された時間間隔で経路の点を再サンプルする。ただし、経路全体の長さはmin_trajectory_lengthからmax_trajectory_lengthの間となるように再サンプルを行い、点の間隔はmin_trajectory_interval_distanceより小さくならないようにする。 現在車速でresample_timeの間進む距離までは密にサンプリングし、それ以降は疎にサンプリングする。 この方法でサンプリングすることで、低速時は密に、高速時は疎にサンプルされるため、停止精度と計算負荷軽減の両立を図っている。

Calculate initial state

速度計画のための初期値を計算する。初期値は状況に応じて下表のように計算する。

状況 初期速度 初期加速度
最初の計算時 現在車速 0.0
発進時 engage_velocity engage_acceleration
現在車速と計画車速が乖離 現在車速 前回計画値
通常時 前回計画値 前回計画値

Smooth velocity

速度の計画を行う。速度計画のアルゴリズムはJerkFiltered, L2, Linfの 3 種類のうちからコンフィグで指定する。 最適化のソルバは OSQP[1]を利用する。

JerkFiltered

速度の 2 乗(最小化で表すため負値で表現)、制限速度逸脱量の 2 乗、制限加度逸脱量の 2 乗、制限ジャーク逸脱量の 2 乗、ジャークの 2 乗の総和を最小化する。

L2

速度の 2 乗(最小化で表すため負値で表現)、制限速度逸脱量の 2 乗、制限加度逸脱量の 2 乗、疑似ジャーク[2]の 2 乗の総和を最小化する。

Linf

速度の 2 乗(最小化で表すため負値で表現)、制限速度逸脱量の 2 乗、制限加度逸脱量の 2 乗の総和と疑似ジャーク[2]の絶対最大値の和の最小化する。

Post process

計画された軌道の後処理を行う。

  • 停止点より先の速度を 0 に設定
  • 速度がパラメータで与えられるmax_velocity以下となるように設定
  • 自車位置より手前の点における速度を設定
  • Trajectory の仕様に合わせてリサンプリング(post resampling)
  • デバッグデータの出力

最適化の計算が終わったあと、次のノードに経路を渡す前にpost resamplingと呼ばれるリサンプリングを行う。ここで再度リサンプリングを行っている理由としては、最適化前で必要な経路間隔と後段のモジュールに渡す経路間隔が必ずしも一致していないからであり、その差を埋めるために再度サンプリングを行っている。そのため、post resamplingでは後段モジュールの経路仕様を確認してパラメータを決める必要がある。なお、最適化アルゴリズムの計算負荷が高く、最初のリサンプリングで経路間隔が後段モジュールの経路仕様より疎になっている場合、post resamplingで経路を蜜にリサンプリングする。逆に最適化アルゴリズムの計算負荷が小さく、最初のリサンプリングで経路間隔が後段の経路仕様より蜜になっている場合は、post resamplingで経路をその仕様に合わせて疎にリサンプリングする。

Inputs / Outputs

Input

Name Type Description
~/input/trajectory autoware_auto_planning_msgs/Trajectory Reference trajectory
/planning/scenario_planning/max_velocity std_msgs/Float32 External velocity limit [m/s]
/localization/kinematic_state nav_msgs/Odometry Current odometry
/tf tf2_msgs/TFMessage TF
/tf_static tf2_msgs/TFMessage TF static

Output

Name Type Description
~/output/trajectory autoware_auto_planning_msgs/Trajectory Modified trajectory
/planning/scenario_planning/current_max_velocity std_msgs/Float32 Current external velocity limit [m/s]
~/closest_velocity std_msgs/Float32 Planned velocity closest to ego base_link (for debug)
~/closest_acceleration std_msgs/Float32 Planned acceleration closest to ego base_link (for debug)
~/closest_jerk std_msgs/Float32 Planned jerk closest to ego base_link (for debug)
~/debug/trajectory_raw autoware_auto_planning_msgs/Trajectory Extracted trajectory (for debug)

File truncated at 100 lines see the full file

CHANGELOG
No CHANGELOG found.

Launch files

  • launch/motion_velocity_smoother.launch.xml
      • common_param_path [default: $(find-pkg-share motion_velocity_smoother)/config/default_common.param.yaml]
      • nearest_search_param_path
      • input_trajectory [default: /planning/scenario_planning/scenario_selector/trajectory]
      • output_trajectory [default: /planning/scenario_planning/trajectory]
      • publish_debug_trajs [default: false]
      • velocity_smoother_type [default: JerkFiltered]
      • param_path [default: $(find-pkg-share motion_velocity_smoother)/config/default_motion_velocity_smoother.param.yaml]
      • velocity_smoother_param_path [default: $(find-pkg-share motion_velocity_smoother)/config/$(var velocity_smoother_type).param.yaml]

Messages

No message files found.

Services

No service files found

Plugins

No plugins found.

Recent questions tagged motion_velocity_smoother at Robotics Stack Exchange

No version for distro kilted showing github. Known supported distros are highlighted in the buttons above.
Package symbol

motion_velocity_smoother package from autodrrt repo

autonomous_emergency_braking control_performance_analysis control_validator external_cmd_selector joy_controller lane_departure_checker mpc_lateral_controller obstacle_collision_checker operation_mode_transition_manager pid_longitudinal_controller predicted_path_checker pure_pursuit shift_decider trajectory_follower_base trajectory_follower_node vehicle_cmd_gate diagnostic_converter kinematic_evaluator localization_evaluator planning_evaluator ekf_localizer geo_pose_projector gyro_odometer ar_tag_based_localizer landmark_manager localization_error_monitor localization_gnss localization_util ndt_scan_matcher pose2twist pose_initializer pose_instability_detector stop_filter tree_structured_parzen_estimator twist2accel yabloc_common yabloc_image_processing yabloc_monitor yabloc_particle_filter yabloc_pose_initializer map_height_fitter map_loader map_projection_loader map_tf_generator lanelet2_map_preprocessor ros2_bevdet bytetrack cluster_merger compare_map_segmentation crosswalk_traffic_light_estimator detected_object_feature_remover detected_object_validation detection_by_tracker elevation_map_loader euclidean_cluster front_vehicle_velocity_estimator ground_segmentation heatmap_visualizer image_projection_based_fusion lidar_apollo_instance_segmentation lidar_apollo_segmentation_tvm lidar_apollo_segmentation_tvm_nodes lidar_centerpoint lidar_centerpoint_tvm map_based_prediction multi_object_tracker object_merger object_range_splitter object_velocity_splitter occupancy_grid_map_outlier_filter probabilistic_occupancy_grid_map radar_crossing_objects_noise_filter radar_fusion_to_detected_object radar_object_clustering radar_object_tracker radar_tracks_msgs_converter shape_estimation simple_object_merger tensorrt_classifier tensorrt_yolo tensorrt_yolox tracking_object_merger traffic_light_arbiter traffic_light_classifier traffic_light_fine_detector traffic_light_map_based_detector traffic_light_multi_camera_fusion traffic_light_occlusion_predictor traffic_light_ssd_fine_detector traffic_light_visualization behavior_path_avoidance_by_lane_change_module behavior_path_avoidance_module behavior_path_external_request_lane_change_module behavior_path_goal_planner_module behavior_path_lane_change_module behavior_path_planner behavior_path_planner_common behavior_path_side_shift_module behavior_path_start_planner_module behavior_velocity_blind_spot_module behavior_velocity_crosswalk_module behavior_velocity_detection_area_module behavior_velocity_intersection_module behavior_velocity_no_drivable_lane_module behavior_velocity_no_stopping_area_module behavior_velocity_occlusion_spot_module behavior_velocity_out_of_lane_module behavior_velocity_planner behavior_velocity_planner_common behavior_velocity_run_out_module behavior_velocity_speed_bump_module behavior_velocity_stop_line_module behavior_velocity_template_module behavior_velocity_traffic_light_module behavior_velocity_virtual_traffic_light_module behavior_velocity_walkway_module costmap_generator external_velocity_limit_selector freespace_planner freespace_planning_algorithms mission_planner motion_velocity_smoother objects_of_interest_marker_interface obstacle_avoidance_planner obstacle_cruise_planner obstacle_stop_planner obstacle_velocity_limiter path_smoother planning_debug_tools planning_test_utils planning_topic_converter planning_validator route_handler rtc_interface rtc_replayer bezier_sampler frenet_planner path_sampler sampler_common scenario_selector static_centerline_optimizer surround_obstacle_checker gnss_poser image_diagnostics image_transport_decompressor imu_corrector livox_tag_filter pointcloud_preprocessor radar_scan_to_pointcloud2 radar_static_pointcloud_filter radar_threshold_filter radar_tracks_noise_filter tier4_pcl_extensions vehicle_velocity_converter autoware_auto_msgs_adapter bluetooth_monitor component_state_monitor default_ad_api ad_api_adaptors ad_api_visualizers automatic_pose_initializer diagnostic_graph_aggregator dummy_diag_publisher dummy_infrastructure duplicated_node_checker emergency_handler mrm_comfortable_stop_operator mrm_emergency_stop_operator system_error_monitor system_monitor topic_state_monitor velodyne_monitor accel_brake_map_calibrator external_cmd_converter raw_vehicle_cmd_converter steer_offset_estimator vehicle_info_util launch loop_sim ssh_machine gpudirect-dds autoware_ad_api_specs autoware_adapi_v1_msgs autoware_adapi_version_msgs autoware_auto_common autoware_auto_geometry autoware_auto_control_msgs autoware_auto_geometry_msgs autoware_auto_mapping_msgs autoware_auto_msgs autoware_auto_perception_msgs autoware_auto_planning_msgs autoware_auto_system_msgs autoware_auto_vehicle_msgs autoware_auto_perception_rviz_plugin autoware_auto_tf2 autoware_cmake autoware_lint_common autoware_utils lanelet2_extension autoware_common_msgs autoware_control_msgs autoware_localization_msgs autoware_map_msgs autoware_perception_msgs autoware_planning_msgs autoware_sensing_msgs autoware_system_msgs autoware_vehicle_msgs autoware_point_types autoware_testing bag_time_manager_rviz_plugin component_interface_specs component_interface_tools component_interface_utils cuda_utils fake_test_node geography_utils global_parameter_loader glog_component goal_distance_calculator grid_map_utils interpolation kalman_filter motion_utils object_recognition_utils osqp_interface path_distance_calculator perception_utils polar_grid qp_interface rtc_manager_rviz_plugin signal_processing tensorrt_common tier4_adapi_rviz_plugin tier4_api_utils tier4_automatic_goal_rviz_plugin tier4_autoware_utils tier4_calibration_rviz_plugin tier4_camera_view_rviz_plugin tier4_control_rviz_plugin tier4_datetime_rviz_plugin tier4_debug_rviz_plugin tier4_debug_tools tier4_localization_rviz_plugin tier4_perception_rviz_plugin tier4_planning_rviz_plugin tier4_screen_capture_rviz_plugin tier4_simulated_clock_rviz_plugin tier4_state_rviz_plugin tier4_system_rviz_plugin tier4_target_object_type_rviz_plugin tier4_traffic_light_rviz_plugin tier4_vehicle_rviz_plugin time_utils simulator_compatibility_test traffic_light_recognition_marker_publisher traffic_light_utils tvm_utility dma_customer_msg dma_transfer eagleye_coordinate eagleye_navigation eagleye_msgs eagleye_rt eagleye_can_velocity_converter eagleye_fix2kml eagleye_geo_pose_converter eagleye_geo_pose_fusion eagleye_gnss_converter eagleye_tf llh_converter morai_msgs mussp ndt_omp orocos_kdl python_orocos_kdl pointcloud_to_laserscan rtklib_bridge rtklib_msgs autoware_external_api_msgs autoware_iv_external_api_adaptor autoware_iv_internal_api_adaptor awapi_awiv_adapter tier4_api_msgs tier4_auto_msgs_converter tier4_control_msgs tier4_debug_msgs tier4_external_api_msgs tier4_hmi_msgs tier4_localization_msgs tier4_map_msgs tier4_perception_msgs tier4_planning_msgs tier4_rtc_msgs tier4_simulation_msgs tier4_system_msgs tier4_v2x_msgs tier4_vehicle_msgs tier4_autoware_api_launch tier4_control_launch tier4_localization_launch tier4_map_launch tier4_perception_launch tier4_planning_launch tier4_sensing_launch tier4_simulator_launch tier4_system_launch tier4_vehicle_launch fastrtps cyclonedds lanelet2 lanelet2_core lanelet2_examples lanelet2_io lanelet2_maps lanelet2_matching lanelet2_projection lanelet2_python lanelet2_routing lanelet2_traffic_rules lanelet2_validation sophus angles behaviortree_cpp_v3 bond bond_core bondcpp bondpy smclib test_bond cudnn_cmake_module diagnostic_aggregator diagnostic_common_diagnostics diagnostic_updater diagnostics self_test filters geodesy geographic_info geographic_msgs grid_map grid_map_cmake_helpers grid_map_core grid_map_costmap_2d grid_map_cv grid_map_demos grid_map_filters grid_map_loader grid_map_msgs grid_map_octomap grid_map_pcl grid_map_ros grid_map_rviz_plugin grid_map_sdf grid_map_visualization mrt_cmake_modules nav2_amcl nav2_behavior_tree nav2_behaviors nav2_bringup nav2_bt_navigator nav2_collision_monitor nav2_common nav2_controller nav2_core nav2_costmap_2d costmap_queue dwb_core dwb_critics dwb_msgs dwb_plugins nav2_dwb_controller nav_2d_msgs nav_2d_utils nav2_lifecycle_manager nav2_map_server nav2_msgs nav2_navfn_planner nav2_planner nav2_regulated_pure_pursuit_controller nav2_rotation_shim_controller nav2_rviz_plugins nav2_simple_commander nav2_smac_planner nav2_smoother nav2_system_tests nav2_theta_star_planner nav2_util nav2_velocity_smoother nav2_voxel_grid nav2_waypoint_follower navigation2 dynamic_edt_3d octomap octovis octomap_msgs osqp_vendor pacmod3_msgs pcl_msgs pcl_conversions pcl_ros perception_pcl point_cloud_msg_wrapper radar_msgs can_msgs rqt_tf_tree tensorrt_cmake_module topic_tools topic_tools_interfaces tvm_vendor cv_bridge image_geometry opencv_tests vision_opencv xacro rviz2 rviz_assimp_vendor rviz_common rviz_default_plugins rviz_ogre_vendor rviz_rendering rviz_rendering_tests rviz_visual_testing_framework dummy_perception_publisher fault_injection simple_planning_simulator image_view v4l2_camera aion_interface carla_vehicle_can cgi430_can_driver cgi610_driver ARS408_driver common_sensor_launch kunyi_sensor_kit_description kunyi_sensor_kit_launch classformsg node_v2x data_format_dump data_preprocess_launch lidar_centerpoint_collect lidar_saver message_sync time_cal camera_calibration direct_visual_lidar_calibration multi_lidar_calibration

ROS Distro
github

Package Summary

Tags No category tags.
Version 0.1.0
License Apache License 2.0
Build type AMENT_CMAKE
Use RECOMMENDED

Repository Summary

Description
Checkout URI https://github.com/ieiauto/autodrrt.git
VCS Type git
VCS Version main
Last Updated 2025-05-30
Dev Status UNKNOWN
Released UNRELEASED
Tags No category tags.
Contributing Help Wanted (-)
Good First Issues (-)
Pull Requests to Review (-)

Package Description

The motion_velocity_smoother package

Additional Links

No additional links.

Maintainers

  • Fumiya Watanabe
  • Takamasa Horibe
  • Makoto Kurihara

Authors

  • Takamasa Horibe
  • Fumiya Watanabe
  • Yutaka Shimizu
  • Makoto Kurihara

Motion Velocity Smoother

Purpose

motion_velocity_smootherは目標軌道上の各点における望ましい車速を計画して出力するモジュールである。 このモジュールは、速度の最大化と乗り心地の良さを両立するために、事前に指定された制限速度、制限加速度および制限躍度の範囲で車速を計画する。 加速度や躍度の制限を与えることは車速の変化を滑らかにすることに対応するため、このモジュールをmotion_velocity_smootherと呼んでいる。

Inner-workings / Algorithms

Flow chart

motion_velocity_smoother_flow

Extract trajectory

自車後輪軸中心位置に最も近い参照経路上の点に対し、extract_behind_distだけ戻った点からextract_ahead_distだけ進んだ点までの参照経路を抜き出す。

Apply external velocity limit

モジュール外部から指定された速度制限を適用する。 ここで扱う外部の速度制限は/planning/scenario_planning/max_velocityの topic で渡されるもので、地図上で設定された速度制限など、参照経路にすでに設定されている制限速度とは別である。 外部から指定される速度制限は、パラメータで指定されている減速度および躍度の制限の範囲で減速可能な位置から速度制限を適用する。

Apply stop approaching velocity

停止点に近づいたときの速度を設定する。障害物近傍まで近づく場合や、正着精度向上などの目的に用いる。

Apply lateral acceleration limit

経路の曲率に応じて、指定された最大横加速度max_lateral_accelを超えない速度を制限速度として設定する。ただし、制限速度はmin_curve_velocityを下回らないように設定する。

Resample trajectory

指定された時間間隔で経路の点を再サンプルする。ただし、経路全体の長さはmin_trajectory_lengthからmax_trajectory_lengthの間となるように再サンプルを行い、点の間隔はmin_trajectory_interval_distanceより小さくならないようにする。 現在車速でresample_timeの間進む距離までは密にサンプリングし、それ以降は疎にサンプリングする。 この方法でサンプリングすることで、低速時は密に、高速時は疎にサンプルされるため、停止精度と計算負荷軽減の両立を図っている。

Calculate initial state

速度計画のための初期値を計算する。初期値は状況に応じて下表のように計算する。

状況 初期速度 初期加速度
最初の計算時 現在車速 0.0
発進時 engage_velocity engage_acceleration
現在車速と計画車速が乖離 現在車速 前回計画値
通常時 前回計画値 前回計画値

Smooth velocity

速度の計画を行う。速度計画のアルゴリズムはJerkFiltered, L2, Linfの 3 種類のうちからコンフィグで指定する。 最適化のソルバは OSQP[1]を利用する。

JerkFiltered

速度の 2 乗(最小化で表すため負値で表現)、制限速度逸脱量の 2 乗、制限加度逸脱量の 2 乗、制限ジャーク逸脱量の 2 乗、ジャークの 2 乗の総和を最小化する。

L2

速度の 2 乗(最小化で表すため負値で表現)、制限速度逸脱量の 2 乗、制限加度逸脱量の 2 乗、疑似ジャーク[2]の 2 乗の総和を最小化する。

Linf

速度の 2 乗(最小化で表すため負値で表現)、制限速度逸脱量の 2 乗、制限加度逸脱量の 2 乗の総和と疑似ジャーク[2]の絶対最大値の和の最小化する。

Post process

計画された軌道の後処理を行う。

  • 停止点より先の速度を 0 に設定
  • 速度がパラメータで与えられるmax_velocity以下となるように設定
  • 自車位置より手前の点における速度を設定
  • Trajectory の仕様に合わせてリサンプリング(post resampling)
  • デバッグデータの出力

最適化の計算が終わったあと、次のノードに経路を渡す前にpost resamplingと呼ばれるリサンプリングを行う。ここで再度リサンプリングを行っている理由としては、最適化前で必要な経路間隔と後段のモジュールに渡す経路間隔が必ずしも一致していないからであり、その差を埋めるために再度サンプリングを行っている。そのため、post resamplingでは後段モジュールの経路仕様を確認してパラメータを決める必要がある。なお、最適化アルゴリズムの計算負荷が高く、最初のリサンプリングで経路間隔が後段モジュールの経路仕様より疎になっている場合、post resamplingで経路を蜜にリサンプリングする。逆に最適化アルゴリズムの計算負荷が小さく、最初のリサンプリングで経路間隔が後段の経路仕様より蜜になっている場合は、post resamplingで経路をその仕様に合わせて疎にリサンプリングする。

Inputs / Outputs

Input

Name Type Description
~/input/trajectory autoware_auto_planning_msgs/Trajectory Reference trajectory
/planning/scenario_planning/max_velocity std_msgs/Float32 External velocity limit [m/s]
/localization/kinematic_state nav_msgs/Odometry Current odometry
/tf tf2_msgs/TFMessage TF
/tf_static tf2_msgs/TFMessage TF static

Output

Name Type Description
~/output/trajectory autoware_auto_planning_msgs/Trajectory Modified trajectory
/planning/scenario_planning/current_max_velocity std_msgs/Float32 Current external velocity limit [m/s]
~/closest_velocity std_msgs/Float32 Planned velocity closest to ego base_link (for debug)
~/closest_acceleration std_msgs/Float32 Planned acceleration closest to ego base_link (for debug)
~/closest_jerk std_msgs/Float32 Planned jerk closest to ego base_link (for debug)
~/debug/trajectory_raw autoware_auto_planning_msgs/Trajectory Extracted trajectory (for debug)

File truncated at 100 lines see the full file

CHANGELOG
No CHANGELOG found.

Launch files

  • launch/motion_velocity_smoother.launch.xml
      • common_param_path [default: $(find-pkg-share motion_velocity_smoother)/config/default_common.param.yaml]
      • nearest_search_param_path
      • input_trajectory [default: /planning/scenario_planning/scenario_selector/trajectory]
      • output_trajectory [default: /planning/scenario_planning/trajectory]
      • publish_debug_trajs [default: false]
      • velocity_smoother_type [default: JerkFiltered]
      • param_path [default: $(find-pkg-share motion_velocity_smoother)/config/default_motion_velocity_smoother.param.yaml]
      • velocity_smoother_param_path [default: $(find-pkg-share motion_velocity_smoother)/config/$(var velocity_smoother_type).param.yaml]

Messages

No message files found.

Services

No service files found

Plugins

No plugins found.

Recent questions tagged motion_velocity_smoother at Robotics Stack Exchange

No version for distro rolling showing github. Known supported distros are highlighted in the buttons above.
Package symbol

motion_velocity_smoother package from autodrrt repo

autonomous_emergency_braking control_performance_analysis control_validator external_cmd_selector joy_controller lane_departure_checker mpc_lateral_controller obstacle_collision_checker operation_mode_transition_manager pid_longitudinal_controller predicted_path_checker pure_pursuit shift_decider trajectory_follower_base trajectory_follower_node vehicle_cmd_gate diagnostic_converter kinematic_evaluator localization_evaluator planning_evaluator ekf_localizer geo_pose_projector gyro_odometer ar_tag_based_localizer landmark_manager localization_error_monitor localization_gnss localization_util ndt_scan_matcher pose2twist pose_initializer pose_instability_detector stop_filter tree_structured_parzen_estimator twist2accel yabloc_common yabloc_image_processing yabloc_monitor yabloc_particle_filter yabloc_pose_initializer map_height_fitter map_loader map_projection_loader map_tf_generator lanelet2_map_preprocessor ros2_bevdet bytetrack cluster_merger compare_map_segmentation crosswalk_traffic_light_estimator detected_object_feature_remover detected_object_validation detection_by_tracker elevation_map_loader euclidean_cluster front_vehicle_velocity_estimator ground_segmentation heatmap_visualizer image_projection_based_fusion lidar_apollo_instance_segmentation lidar_apollo_segmentation_tvm lidar_apollo_segmentation_tvm_nodes lidar_centerpoint lidar_centerpoint_tvm map_based_prediction multi_object_tracker object_merger object_range_splitter object_velocity_splitter occupancy_grid_map_outlier_filter probabilistic_occupancy_grid_map radar_crossing_objects_noise_filter radar_fusion_to_detected_object radar_object_clustering radar_object_tracker radar_tracks_msgs_converter shape_estimation simple_object_merger tensorrt_classifier tensorrt_yolo tensorrt_yolox tracking_object_merger traffic_light_arbiter traffic_light_classifier traffic_light_fine_detector traffic_light_map_based_detector traffic_light_multi_camera_fusion traffic_light_occlusion_predictor traffic_light_ssd_fine_detector traffic_light_visualization behavior_path_avoidance_by_lane_change_module behavior_path_avoidance_module behavior_path_external_request_lane_change_module behavior_path_goal_planner_module behavior_path_lane_change_module behavior_path_planner behavior_path_planner_common behavior_path_side_shift_module behavior_path_start_planner_module behavior_velocity_blind_spot_module behavior_velocity_crosswalk_module behavior_velocity_detection_area_module behavior_velocity_intersection_module behavior_velocity_no_drivable_lane_module behavior_velocity_no_stopping_area_module behavior_velocity_occlusion_spot_module behavior_velocity_out_of_lane_module behavior_velocity_planner behavior_velocity_planner_common behavior_velocity_run_out_module behavior_velocity_speed_bump_module behavior_velocity_stop_line_module behavior_velocity_template_module behavior_velocity_traffic_light_module behavior_velocity_virtual_traffic_light_module behavior_velocity_walkway_module costmap_generator external_velocity_limit_selector freespace_planner freespace_planning_algorithms mission_planner motion_velocity_smoother objects_of_interest_marker_interface obstacle_avoidance_planner obstacle_cruise_planner obstacle_stop_planner obstacle_velocity_limiter path_smoother planning_debug_tools planning_test_utils planning_topic_converter planning_validator route_handler rtc_interface rtc_replayer bezier_sampler frenet_planner path_sampler sampler_common scenario_selector static_centerline_optimizer surround_obstacle_checker gnss_poser image_diagnostics image_transport_decompressor imu_corrector livox_tag_filter pointcloud_preprocessor radar_scan_to_pointcloud2 radar_static_pointcloud_filter radar_threshold_filter radar_tracks_noise_filter tier4_pcl_extensions vehicle_velocity_converter autoware_auto_msgs_adapter bluetooth_monitor component_state_monitor default_ad_api ad_api_adaptors ad_api_visualizers automatic_pose_initializer diagnostic_graph_aggregator dummy_diag_publisher dummy_infrastructure duplicated_node_checker emergency_handler mrm_comfortable_stop_operator mrm_emergency_stop_operator system_error_monitor system_monitor topic_state_monitor velodyne_monitor accel_brake_map_calibrator external_cmd_converter raw_vehicle_cmd_converter steer_offset_estimator vehicle_info_util launch loop_sim ssh_machine gpudirect-dds autoware_ad_api_specs autoware_adapi_v1_msgs autoware_adapi_version_msgs autoware_auto_common autoware_auto_geometry autoware_auto_control_msgs autoware_auto_geometry_msgs autoware_auto_mapping_msgs autoware_auto_msgs autoware_auto_perception_msgs autoware_auto_planning_msgs autoware_auto_system_msgs autoware_auto_vehicle_msgs autoware_auto_perception_rviz_plugin autoware_auto_tf2 autoware_cmake autoware_lint_common autoware_utils lanelet2_extension autoware_common_msgs autoware_control_msgs autoware_localization_msgs autoware_map_msgs autoware_perception_msgs autoware_planning_msgs autoware_sensing_msgs autoware_system_msgs autoware_vehicle_msgs autoware_point_types autoware_testing bag_time_manager_rviz_plugin component_interface_specs component_interface_tools component_interface_utils cuda_utils fake_test_node geography_utils global_parameter_loader glog_component goal_distance_calculator grid_map_utils interpolation kalman_filter motion_utils object_recognition_utils osqp_interface path_distance_calculator perception_utils polar_grid qp_interface rtc_manager_rviz_plugin signal_processing tensorrt_common tier4_adapi_rviz_plugin tier4_api_utils tier4_automatic_goal_rviz_plugin tier4_autoware_utils tier4_calibration_rviz_plugin tier4_camera_view_rviz_plugin tier4_control_rviz_plugin tier4_datetime_rviz_plugin tier4_debug_rviz_plugin tier4_debug_tools tier4_localization_rviz_plugin tier4_perception_rviz_plugin tier4_planning_rviz_plugin tier4_screen_capture_rviz_plugin tier4_simulated_clock_rviz_plugin tier4_state_rviz_plugin tier4_system_rviz_plugin tier4_target_object_type_rviz_plugin tier4_traffic_light_rviz_plugin tier4_vehicle_rviz_plugin time_utils simulator_compatibility_test traffic_light_recognition_marker_publisher traffic_light_utils tvm_utility dma_customer_msg dma_transfer eagleye_coordinate eagleye_navigation eagleye_msgs eagleye_rt eagleye_can_velocity_converter eagleye_fix2kml eagleye_geo_pose_converter eagleye_geo_pose_fusion eagleye_gnss_converter eagleye_tf llh_converter morai_msgs mussp ndt_omp orocos_kdl python_orocos_kdl pointcloud_to_laserscan rtklib_bridge rtklib_msgs autoware_external_api_msgs autoware_iv_external_api_adaptor autoware_iv_internal_api_adaptor awapi_awiv_adapter tier4_api_msgs tier4_auto_msgs_converter tier4_control_msgs tier4_debug_msgs tier4_external_api_msgs tier4_hmi_msgs tier4_localization_msgs tier4_map_msgs tier4_perception_msgs tier4_planning_msgs tier4_rtc_msgs tier4_simulation_msgs tier4_system_msgs tier4_v2x_msgs tier4_vehicle_msgs tier4_autoware_api_launch tier4_control_launch tier4_localization_launch tier4_map_launch tier4_perception_launch tier4_planning_launch tier4_sensing_launch tier4_simulator_launch tier4_system_launch tier4_vehicle_launch fastrtps cyclonedds lanelet2 lanelet2_core lanelet2_examples lanelet2_io lanelet2_maps lanelet2_matching lanelet2_projection lanelet2_python lanelet2_routing lanelet2_traffic_rules lanelet2_validation sophus angles behaviortree_cpp_v3 bond bond_core bondcpp bondpy smclib test_bond cudnn_cmake_module diagnostic_aggregator diagnostic_common_diagnostics diagnostic_updater diagnostics self_test filters geodesy geographic_info geographic_msgs grid_map grid_map_cmake_helpers grid_map_core grid_map_costmap_2d grid_map_cv grid_map_demos grid_map_filters grid_map_loader grid_map_msgs grid_map_octomap grid_map_pcl grid_map_ros grid_map_rviz_plugin grid_map_sdf grid_map_visualization mrt_cmake_modules nav2_amcl nav2_behavior_tree nav2_behaviors nav2_bringup nav2_bt_navigator nav2_collision_monitor nav2_common nav2_controller nav2_core nav2_costmap_2d costmap_queue dwb_core dwb_critics dwb_msgs dwb_plugins nav2_dwb_controller nav_2d_msgs nav_2d_utils nav2_lifecycle_manager nav2_map_server nav2_msgs nav2_navfn_planner nav2_planner nav2_regulated_pure_pursuit_controller nav2_rotation_shim_controller nav2_rviz_plugins nav2_simple_commander nav2_smac_planner nav2_smoother nav2_system_tests nav2_theta_star_planner nav2_util nav2_velocity_smoother nav2_voxel_grid nav2_waypoint_follower navigation2 dynamic_edt_3d octomap octovis octomap_msgs osqp_vendor pacmod3_msgs pcl_msgs pcl_conversions pcl_ros perception_pcl point_cloud_msg_wrapper radar_msgs can_msgs rqt_tf_tree tensorrt_cmake_module topic_tools topic_tools_interfaces tvm_vendor cv_bridge image_geometry opencv_tests vision_opencv xacro rviz2 rviz_assimp_vendor rviz_common rviz_default_plugins rviz_ogre_vendor rviz_rendering rviz_rendering_tests rviz_visual_testing_framework dummy_perception_publisher fault_injection simple_planning_simulator image_view v4l2_camera aion_interface carla_vehicle_can cgi430_can_driver cgi610_driver ARS408_driver common_sensor_launch kunyi_sensor_kit_description kunyi_sensor_kit_launch classformsg node_v2x data_format_dump data_preprocess_launch lidar_centerpoint_collect lidar_saver message_sync time_cal camera_calibration direct_visual_lidar_calibration multi_lidar_calibration

ROS Distro
github

Package Summary

Tags No category tags.
Version 0.1.0
License Apache License 2.0
Build type AMENT_CMAKE
Use RECOMMENDED

Repository Summary

Description
Checkout URI https://github.com/ieiauto/autodrrt.git
VCS Type git
VCS Version main
Last Updated 2025-05-30
Dev Status UNKNOWN
Released UNRELEASED
Tags No category tags.
Contributing Help Wanted (-)
Good First Issues (-)
Pull Requests to Review (-)

Package Description

The motion_velocity_smoother package

Additional Links

No additional links.

Maintainers

  • Fumiya Watanabe
  • Takamasa Horibe
  • Makoto Kurihara

Authors

  • Takamasa Horibe
  • Fumiya Watanabe
  • Yutaka Shimizu
  • Makoto Kurihara

Motion Velocity Smoother

Purpose

motion_velocity_smootherは目標軌道上の各点における望ましい車速を計画して出力するモジュールである。 このモジュールは、速度の最大化と乗り心地の良さを両立するために、事前に指定された制限速度、制限加速度および制限躍度の範囲で車速を計画する。 加速度や躍度の制限を与えることは車速の変化を滑らかにすることに対応するため、このモジュールをmotion_velocity_smootherと呼んでいる。

Inner-workings / Algorithms

Flow chart

motion_velocity_smoother_flow

Extract trajectory

自車後輪軸中心位置に最も近い参照経路上の点に対し、extract_behind_distだけ戻った点からextract_ahead_distだけ進んだ点までの参照経路を抜き出す。

Apply external velocity limit

モジュール外部から指定された速度制限を適用する。 ここで扱う外部の速度制限は/planning/scenario_planning/max_velocityの topic で渡されるもので、地図上で設定された速度制限など、参照経路にすでに設定されている制限速度とは別である。 外部から指定される速度制限は、パラメータで指定されている減速度および躍度の制限の範囲で減速可能な位置から速度制限を適用する。

Apply stop approaching velocity

停止点に近づいたときの速度を設定する。障害物近傍まで近づく場合や、正着精度向上などの目的に用いる。

Apply lateral acceleration limit

経路の曲率に応じて、指定された最大横加速度max_lateral_accelを超えない速度を制限速度として設定する。ただし、制限速度はmin_curve_velocityを下回らないように設定する。

Resample trajectory

指定された時間間隔で経路の点を再サンプルする。ただし、経路全体の長さはmin_trajectory_lengthからmax_trajectory_lengthの間となるように再サンプルを行い、点の間隔はmin_trajectory_interval_distanceより小さくならないようにする。 現在車速でresample_timeの間進む距離までは密にサンプリングし、それ以降は疎にサンプリングする。 この方法でサンプリングすることで、低速時は密に、高速時は疎にサンプルされるため、停止精度と計算負荷軽減の両立を図っている。

Calculate initial state

速度計画のための初期値を計算する。初期値は状況に応じて下表のように計算する。

状況 初期速度 初期加速度
最初の計算時 現在車速 0.0
発進時 engage_velocity engage_acceleration
現在車速と計画車速が乖離 現在車速 前回計画値
通常時 前回計画値 前回計画値

Smooth velocity

速度の計画を行う。速度計画のアルゴリズムはJerkFiltered, L2, Linfの 3 種類のうちからコンフィグで指定する。 最適化のソルバは OSQP[1]を利用する。

JerkFiltered

速度の 2 乗(最小化で表すため負値で表現)、制限速度逸脱量の 2 乗、制限加度逸脱量の 2 乗、制限ジャーク逸脱量の 2 乗、ジャークの 2 乗の総和を最小化する。

L2

速度の 2 乗(最小化で表すため負値で表現)、制限速度逸脱量の 2 乗、制限加度逸脱量の 2 乗、疑似ジャーク[2]の 2 乗の総和を最小化する。

Linf

速度の 2 乗(最小化で表すため負値で表現)、制限速度逸脱量の 2 乗、制限加度逸脱量の 2 乗の総和と疑似ジャーク[2]の絶対最大値の和の最小化する。

Post process

計画された軌道の後処理を行う。

  • 停止点より先の速度を 0 に設定
  • 速度がパラメータで与えられるmax_velocity以下となるように設定
  • 自車位置より手前の点における速度を設定
  • Trajectory の仕様に合わせてリサンプリング(post resampling)
  • デバッグデータの出力

最適化の計算が終わったあと、次のノードに経路を渡す前にpost resamplingと呼ばれるリサンプリングを行う。ここで再度リサンプリングを行っている理由としては、最適化前で必要な経路間隔と後段のモジュールに渡す経路間隔が必ずしも一致していないからであり、その差を埋めるために再度サンプリングを行っている。そのため、post resamplingでは後段モジュールの経路仕様を確認してパラメータを決める必要がある。なお、最適化アルゴリズムの計算負荷が高く、最初のリサンプリングで経路間隔が後段モジュールの経路仕様より疎になっている場合、post resamplingで経路を蜜にリサンプリングする。逆に最適化アルゴリズムの計算負荷が小さく、最初のリサンプリングで経路間隔が後段の経路仕様より蜜になっている場合は、post resamplingで経路をその仕様に合わせて疎にリサンプリングする。

Inputs / Outputs

Input

Name Type Description
~/input/trajectory autoware_auto_planning_msgs/Trajectory Reference trajectory
/planning/scenario_planning/max_velocity std_msgs/Float32 External velocity limit [m/s]
/localization/kinematic_state nav_msgs/Odometry Current odometry
/tf tf2_msgs/TFMessage TF
/tf_static tf2_msgs/TFMessage TF static

Output

Name Type Description
~/output/trajectory autoware_auto_planning_msgs/Trajectory Modified trajectory
/planning/scenario_planning/current_max_velocity std_msgs/Float32 Current external velocity limit [m/s]
~/closest_velocity std_msgs/Float32 Planned velocity closest to ego base_link (for debug)
~/closest_acceleration std_msgs/Float32 Planned acceleration closest to ego base_link (for debug)
~/closest_jerk std_msgs/Float32 Planned jerk closest to ego base_link (for debug)
~/debug/trajectory_raw autoware_auto_planning_msgs/Trajectory Extracted trajectory (for debug)

File truncated at 100 lines see the full file

CHANGELOG
No CHANGELOG found.

Launch files

  • launch/motion_velocity_smoother.launch.xml
      • common_param_path [default: $(find-pkg-share motion_velocity_smoother)/config/default_common.param.yaml]
      • nearest_search_param_path
      • input_trajectory [default: /planning/scenario_planning/scenario_selector/trajectory]
      • output_trajectory [default: /planning/scenario_planning/trajectory]
      • publish_debug_trajs [default: false]
      • velocity_smoother_type [default: JerkFiltered]
      • param_path [default: $(find-pkg-share motion_velocity_smoother)/config/default_motion_velocity_smoother.param.yaml]
      • velocity_smoother_param_path [default: $(find-pkg-share motion_velocity_smoother)/config/$(var velocity_smoother_type).param.yaml]

Messages

No message files found.

Services

No service files found

Plugins

No plugins found.

Recent questions tagged motion_velocity_smoother at Robotics Stack Exchange

Package symbol

motion_velocity_smoother package from autodrrt repo

autonomous_emergency_braking control_performance_analysis control_validator external_cmd_selector joy_controller lane_departure_checker mpc_lateral_controller obstacle_collision_checker operation_mode_transition_manager pid_longitudinal_controller predicted_path_checker pure_pursuit shift_decider trajectory_follower_base trajectory_follower_node vehicle_cmd_gate diagnostic_converter kinematic_evaluator localization_evaluator planning_evaluator ekf_localizer geo_pose_projector gyro_odometer ar_tag_based_localizer landmark_manager localization_error_monitor localization_gnss localization_util ndt_scan_matcher pose2twist pose_initializer pose_instability_detector stop_filter tree_structured_parzen_estimator twist2accel yabloc_common yabloc_image_processing yabloc_monitor yabloc_particle_filter yabloc_pose_initializer map_height_fitter map_loader map_projection_loader map_tf_generator lanelet2_map_preprocessor ros2_bevdet bytetrack cluster_merger compare_map_segmentation crosswalk_traffic_light_estimator detected_object_feature_remover detected_object_validation detection_by_tracker elevation_map_loader euclidean_cluster front_vehicle_velocity_estimator ground_segmentation heatmap_visualizer image_projection_based_fusion lidar_apollo_instance_segmentation lidar_apollo_segmentation_tvm lidar_apollo_segmentation_tvm_nodes lidar_centerpoint lidar_centerpoint_tvm map_based_prediction multi_object_tracker object_merger object_range_splitter object_velocity_splitter occupancy_grid_map_outlier_filter probabilistic_occupancy_grid_map radar_crossing_objects_noise_filter radar_fusion_to_detected_object radar_object_clustering radar_object_tracker radar_tracks_msgs_converter shape_estimation simple_object_merger tensorrt_classifier tensorrt_yolo tensorrt_yolox tracking_object_merger traffic_light_arbiter traffic_light_classifier traffic_light_fine_detector traffic_light_map_based_detector traffic_light_multi_camera_fusion traffic_light_occlusion_predictor traffic_light_ssd_fine_detector traffic_light_visualization behavior_path_avoidance_by_lane_change_module behavior_path_avoidance_module behavior_path_external_request_lane_change_module behavior_path_goal_planner_module behavior_path_lane_change_module behavior_path_planner behavior_path_planner_common behavior_path_side_shift_module behavior_path_start_planner_module behavior_velocity_blind_spot_module behavior_velocity_crosswalk_module behavior_velocity_detection_area_module behavior_velocity_intersection_module behavior_velocity_no_drivable_lane_module behavior_velocity_no_stopping_area_module behavior_velocity_occlusion_spot_module behavior_velocity_out_of_lane_module behavior_velocity_planner behavior_velocity_planner_common behavior_velocity_run_out_module behavior_velocity_speed_bump_module behavior_velocity_stop_line_module behavior_velocity_template_module behavior_velocity_traffic_light_module behavior_velocity_virtual_traffic_light_module behavior_velocity_walkway_module costmap_generator external_velocity_limit_selector freespace_planner freespace_planning_algorithms mission_planner motion_velocity_smoother objects_of_interest_marker_interface obstacle_avoidance_planner obstacle_cruise_planner obstacle_stop_planner obstacle_velocity_limiter path_smoother planning_debug_tools planning_test_utils planning_topic_converter planning_validator route_handler rtc_interface rtc_replayer bezier_sampler frenet_planner path_sampler sampler_common scenario_selector static_centerline_optimizer surround_obstacle_checker gnss_poser image_diagnostics image_transport_decompressor imu_corrector livox_tag_filter pointcloud_preprocessor radar_scan_to_pointcloud2 radar_static_pointcloud_filter radar_threshold_filter radar_tracks_noise_filter tier4_pcl_extensions vehicle_velocity_converter autoware_auto_msgs_adapter bluetooth_monitor component_state_monitor default_ad_api ad_api_adaptors ad_api_visualizers automatic_pose_initializer diagnostic_graph_aggregator dummy_diag_publisher dummy_infrastructure duplicated_node_checker emergency_handler mrm_comfortable_stop_operator mrm_emergency_stop_operator system_error_monitor system_monitor topic_state_monitor velodyne_monitor accel_brake_map_calibrator external_cmd_converter raw_vehicle_cmd_converter steer_offset_estimator vehicle_info_util launch loop_sim ssh_machine gpudirect-dds autoware_ad_api_specs autoware_adapi_v1_msgs autoware_adapi_version_msgs autoware_auto_common autoware_auto_geometry autoware_auto_control_msgs autoware_auto_geometry_msgs autoware_auto_mapping_msgs autoware_auto_msgs autoware_auto_perception_msgs autoware_auto_planning_msgs autoware_auto_system_msgs autoware_auto_vehicle_msgs autoware_auto_perception_rviz_plugin autoware_auto_tf2 autoware_cmake autoware_lint_common autoware_utils lanelet2_extension autoware_common_msgs autoware_control_msgs autoware_localization_msgs autoware_map_msgs autoware_perception_msgs autoware_planning_msgs autoware_sensing_msgs autoware_system_msgs autoware_vehicle_msgs autoware_point_types autoware_testing bag_time_manager_rviz_plugin component_interface_specs component_interface_tools component_interface_utils cuda_utils fake_test_node geography_utils global_parameter_loader glog_component goal_distance_calculator grid_map_utils interpolation kalman_filter motion_utils object_recognition_utils osqp_interface path_distance_calculator perception_utils polar_grid qp_interface rtc_manager_rviz_plugin signal_processing tensorrt_common tier4_adapi_rviz_plugin tier4_api_utils tier4_automatic_goal_rviz_plugin tier4_autoware_utils tier4_calibration_rviz_plugin tier4_camera_view_rviz_plugin tier4_control_rviz_plugin tier4_datetime_rviz_plugin tier4_debug_rviz_plugin tier4_debug_tools tier4_localization_rviz_plugin tier4_perception_rviz_plugin tier4_planning_rviz_plugin tier4_screen_capture_rviz_plugin tier4_simulated_clock_rviz_plugin tier4_state_rviz_plugin tier4_system_rviz_plugin tier4_target_object_type_rviz_plugin tier4_traffic_light_rviz_plugin tier4_vehicle_rviz_plugin time_utils simulator_compatibility_test traffic_light_recognition_marker_publisher traffic_light_utils tvm_utility dma_customer_msg dma_transfer eagleye_coordinate eagleye_navigation eagleye_msgs eagleye_rt eagleye_can_velocity_converter eagleye_fix2kml eagleye_geo_pose_converter eagleye_geo_pose_fusion eagleye_gnss_converter eagleye_tf llh_converter morai_msgs mussp ndt_omp orocos_kdl python_orocos_kdl pointcloud_to_laserscan rtklib_bridge rtklib_msgs autoware_external_api_msgs autoware_iv_external_api_adaptor autoware_iv_internal_api_adaptor awapi_awiv_adapter tier4_api_msgs tier4_auto_msgs_converter tier4_control_msgs tier4_debug_msgs tier4_external_api_msgs tier4_hmi_msgs tier4_localization_msgs tier4_map_msgs tier4_perception_msgs tier4_planning_msgs tier4_rtc_msgs tier4_simulation_msgs tier4_system_msgs tier4_v2x_msgs tier4_vehicle_msgs tier4_autoware_api_launch tier4_control_launch tier4_localization_launch tier4_map_launch tier4_perception_launch tier4_planning_launch tier4_sensing_launch tier4_simulator_launch tier4_system_launch tier4_vehicle_launch fastrtps cyclonedds lanelet2 lanelet2_core lanelet2_examples lanelet2_io lanelet2_maps lanelet2_matching lanelet2_projection lanelet2_python lanelet2_routing lanelet2_traffic_rules lanelet2_validation sophus angles behaviortree_cpp_v3 bond bond_core bondcpp bondpy smclib test_bond cudnn_cmake_module diagnostic_aggregator diagnostic_common_diagnostics diagnostic_updater diagnostics self_test filters geodesy geographic_info geographic_msgs grid_map grid_map_cmake_helpers grid_map_core grid_map_costmap_2d grid_map_cv grid_map_demos grid_map_filters grid_map_loader grid_map_msgs grid_map_octomap grid_map_pcl grid_map_ros grid_map_rviz_plugin grid_map_sdf grid_map_visualization mrt_cmake_modules nav2_amcl nav2_behavior_tree nav2_behaviors nav2_bringup nav2_bt_navigator nav2_collision_monitor nav2_common nav2_controller nav2_core nav2_costmap_2d costmap_queue dwb_core dwb_critics dwb_msgs dwb_plugins nav2_dwb_controller nav_2d_msgs nav_2d_utils nav2_lifecycle_manager nav2_map_server nav2_msgs nav2_navfn_planner nav2_planner nav2_regulated_pure_pursuit_controller nav2_rotation_shim_controller nav2_rviz_plugins nav2_simple_commander nav2_smac_planner nav2_smoother nav2_system_tests nav2_theta_star_planner nav2_util nav2_velocity_smoother nav2_voxel_grid nav2_waypoint_follower navigation2 dynamic_edt_3d octomap octovis octomap_msgs osqp_vendor pacmod3_msgs pcl_msgs pcl_conversions pcl_ros perception_pcl point_cloud_msg_wrapper radar_msgs can_msgs rqt_tf_tree tensorrt_cmake_module topic_tools topic_tools_interfaces tvm_vendor cv_bridge image_geometry opencv_tests vision_opencv xacro rviz2 rviz_assimp_vendor rviz_common rviz_default_plugins rviz_ogre_vendor rviz_rendering rviz_rendering_tests rviz_visual_testing_framework dummy_perception_publisher fault_injection simple_planning_simulator image_view v4l2_camera aion_interface carla_vehicle_can cgi430_can_driver cgi610_driver ARS408_driver common_sensor_launch kunyi_sensor_kit_description kunyi_sensor_kit_launch classformsg node_v2x data_format_dump data_preprocess_launch lidar_centerpoint_collect lidar_saver message_sync time_cal camera_calibration direct_visual_lidar_calibration multi_lidar_calibration

ROS Distro
github

Package Summary

Tags No category tags.
Version 0.1.0
License Apache License 2.0
Build type AMENT_CMAKE
Use RECOMMENDED

Repository Summary

Description
Checkout URI https://github.com/ieiauto/autodrrt.git
VCS Type git
VCS Version main
Last Updated 2025-05-30
Dev Status UNKNOWN
Released UNRELEASED
Tags No category tags.
Contributing Help Wanted (-)
Good First Issues (-)
Pull Requests to Review (-)

Package Description

The motion_velocity_smoother package

Additional Links

No additional links.

Maintainers

  • Fumiya Watanabe
  • Takamasa Horibe
  • Makoto Kurihara

Authors

  • Takamasa Horibe
  • Fumiya Watanabe
  • Yutaka Shimizu
  • Makoto Kurihara

Motion Velocity Smoother

Purpose

motion_velocity_smootherは目標軌道上の各点における望ましい車速を計画して出力するモジュールである。 このモジュールは、速度の最大化と乗り心地の良さを両立するために、事前に指定された制限速度、制限加速度および制限躍度の範囲で車速を計画する。 加速度や躍度の制限を与えることは車速の変化を滑らかにすることに対応するため、このモジュールをmotion_velocity_smootherと呼んでいる。

Inner-workings / Algorithms

Flow chart

motion_velocity_smoother_flow

Extract trajectory

自車後輪軸中心位置に最も近い参照経路上の点に対し、extract_behind_distだけ戻った点からextract_ahead_distだけ進んだ点までの参照経路を抜き出す。

Apply external velocity limit

モジュール外部から指定された速度制限を適用する。 ここで扱う外部の速度制限は/planning/scenario_planning/max_velocityの topic で渡されるもので、地図上で設定された速度制限など、参照経路にすでに設定されている制限速度とは別である。 外部から指定される速度制限は、パラメータで指定されている減速度および躍度の制限の範囲で減速可能な位置から速度制限を適用する。

Apply stop approaching velocity

停止点に近づいたときの速度を設定する。障害物近傍まで近づく場合や、正着精度向上などの目的に用いる。

Apply lateral acceleration limit

経路の曲率に応じて、指定された最大横加速度max_lateral_accelを超えない速度を制限速度として設定する。ただし、制限速度はmin_curve_velocityを下回らないように設定する。

Resample trajectory

指定された時間間隔で経路の点を再サンプルする。ただし、経路全体の長さはmin_trajectory_lengthからmax_trajectory_lengthの間となるように再サンプルを行い、点の間隔はmin_trajectory_interval_distanceより小さくならないようにする。 現在車速でresample_timeの間進む距離までは密にサンプリングし、それ以降は疎にサンプリングする。 この方法でサンプリングすることで、低速時は密に、高速時は疎にサンプルされるため、停止精度と計算負荷軽減の両立を図っている。

Calculate initial state

速度計画のための初期値を計算する。初期値は状況に応じて下表のように計算する。

状況 初期速度 初期加速度
最初の計算時 現在車速 0.0
発進時 engage_velocity engage_acceleration
現在車速と計画車速が乖離 現在車速 前回計画値
通常時 前回計画値 前回計画値

Smooth velocity

速度の計画を行う。速度計画のアルゴリズムはJerkFiltered, L2, Linfの 3 種類のうちからコンフィグで指定する。 最適化のソルバは OSQP[1]を利用する。

JerkFiltered

速度の 2 乗(最小化で表すため負値で表現)、制限速度逸脱量の 2 乗、制限加度逸脱量の 2 乗、制限ジャーク逸脱量の 2 乗、ジャークの 2 乗の総和を最小化する。

L2

速度の 2 乗(最小化で表すため負値で表現)、制限速度逸脱量の 2 乗、制限加度逸脱量の 2 乗、疑似ジャーク[2]の 2 乗の総和を最小化する。

Linf

速度の 2 乗(最小化で表すため負値で表現)、制限速度逸脱量の 2 乗、制限加度逸脱量の 2 乗の総和と疑似ジャーク[2]の絶対最大値の和の最小化する。

Post process

計画された軌道の後処理を行う。

  • 停止点より先の速度を 0 に設定
  • 速度がパラメータで与えられるmax_velocity以下となるように設定
  • 自車位置より手前の点における速度を設定
  • Trajectory の仕様に合わせてリサンプリング(post resampling)
  • デバッグデータの出力

最適化の計算が終わったあと、次のノードに経路を渡す前にpost resamplingと呼ばれるリサンプリングを行う。ここで再度リサンプリングを行っている理由としては、最適化前で必要な経路間隔と後段のモジュールに渡す経路間隔が必ずしも一致していないからであり、その差を埋めるために再度サンプリングを行っている。そのため、post resamplingでは後段モジュールの経路仕様を確認してパラメータを決める必要がある。なお、最適化アルゴリズムの計算負荷が高く、最初のリサンプリングで経路間隔が後段モジュールの経路仕様より疎になっている場合、post resamplingで経路を蜜にリサンプリングする。逆に最適化アルゴリズムの計算負荷が小さく、最初のリサンプリングで経路間隔が後段の経路仕様より蜜になっている場合は、post resamplingで経路をその仕様に合わせて疎にリサンプリングする。

Inputs / Outputs

Input

Name Type Description
~/input/trajectory autoware_auto_planning_msgs/Trajectory Reference trajectory
/planning/scenario_planning/max_velocity std_msgs/Float32 External velocity limit [m/s]
/localization/kinematic_state nav_msgs/Odometry Current odometry
/tf tf2_msgs/TFMessage TF
/tf_static tf2_msgs/TFMessage TF static

Output

Name Type Description
~/output/trajectory autoware_auto_planning_msgs/Trajectory Modified trajectory
/planning/scenario_planning/current_max_velocity std_msgs/Float32 Current external velocity limit [m/s]
~/closest_velocity std_msgs/Float32 Planned velocity closest to ego base_link (for debug)
~/closest_acceleration std_msgs/Float32 Planned acceleration closest to ego base_link (for debug)
~/closest_jerk std_msgs/Float32 Planned jerk closest to ego base_link (for debug)
~/debug/trajectory_raw autoware_auto_planning_msgs/Trajectory Extracted trajectory (for debug)

File truncated at 100 lines see the full file

CHANGELOG
No CHANGELOG found.

Launch files

  • launch/motion_velocity_smoother.launch.xml
      • common_param_path [default: $(find-pkg-share motion_velocity_smoother)/config/default_common.param.yaml]
      • nearest_search_param_path
      • input_trajectory [default: /planning/scenario_planning/scenario_selector/trajectory]
      • output_trajectory [default: /planning/scenario_planning/trajectory]
      • publish_debug_trajs [default: false]
      • velocity_smoother_type [default: JerkFiltered]
      • param_path [default: $(find-pkg-share motion_velocity_smoother)/config/default_motion_velocity_smoother.param.yaml]
      • velocity_smoother_param_path [default: $(find-pkg-share motion_velocity_smoother)/config/$(var velocity_smoother_type).param.yaml]

Messages

No message files found.

Services

No service files found

Plugins

No plugins found.

Recent questions tagged motion_velocity_smoother at Robotics Stack Exchange

No version for distro galactic showing github. Known supported distros are highlighted in the buttons above.
Package symbol

motion_velocity_smoother package from autodrrt repo

autonomous_emergency_braking control_performance_analysis control_validator external_cmd_selector joy_controller lane_departure_checker mpc_lateral_controller obstacle_collision_checker operation_mode_transition_manager pid_longitudinal_controller predicted_path_checker pure_pursuit shift_decider trajectory_follower_base trajectory_follower_node vehicle_cmd_gate diagnostic_converter kinematic_evaluator localization_evaluator planning_evaluator ekf_localizer geo_pose_projector gyro_odometer ar_tag_based_localizer landmark_manager localization_error_monitor localization_gnss localization_util ndt_scan_matcher pose2twist pose_initializer pose_instability_detector stop_filter tree_structured_parzen_estimator twist2accel yabloc_common yabloc_image_processing yabloc_monitor yabloc_particle_filter yabloc_pose_initializer map_height_fitter map_loader map_projection_loader map_tf_generator lanelet2_map_preprocessor ros2_bevdet bytetrack cluster_merger compare_map_segmentation crosswalk_traffic_light_estimator detected_object_feature_remover detected_object_validation detection_by_tracker elevation_map_loader euclidean_cluster front_vehicle_velocity_estimator ground_segmentation heatmap_visualizer image_projection_based_fusion lidar_apollo_instance_segmentation lidar_apollo_segmentation_tvm lidar_apollo_segmentation_tvm_nodes lidar_centerpoint lidar_centerpoint_tvm map_based_prediction multi_object_tracker object_merger object_range_splitter object_velocity_splitter occupancy_grid_map_outlier_filter probabilistic_occupancy_grid_map radar_crossing_objects_noise_filter radar_fusion_to_detected_object radar_object_clustering radar_object_tracker radar_tracks_msgs_converter shape_estimation simple_object_merger tensorrt_classifier tensorrt_yolo tensorrt_yolox tracking_object_merger traffic_light_arbiter traffic_light_classifier traffic_light_fine_detector traffic_light_map_based_detector traffic_light_multi_camera_fusion traffic_light_occlusion_predictor traffic_light_ssd_fine_detector traffic_light_visualization behavior_path_avoidance_by_lane_change_module behavior_path_avoidance_module behavior_path_external_request_lane_change_module behavior_path_goal_planner_module behavior_path_lane_change_module behavior_path_planner behavior_path_planner_common behavior_path_side_shift_module behavior_path_start_planner_module behavior_velocity_blind_spot_module behavior_velocity_crosswalk_module behavior_velocity_detection_area_module behavior_velocity_intersection_module behavior_velocity_no_drivable_lane_module behavior_velocity_no_stopping_area_module behavior_velocity_occlusion_spot_module behavior_velocity_out_of_lane_module behavior_velocity_planner behavior_velocity_planner_common behavior_velocity_run_out_module behavior_velocity_speed_bump_module behavior_velocity_stop_line_module behavior_velocity_template_module behavior_velocity_traffic_light_module behavior_velocity_virtual_traffic_light_module behavior_velocity_walkway_module costmap_generator external_velocity_limit_selector freespace_planner freespace_planning_algorithms mission_planner motion_velocity_smoother objects_of_interest_marker_interface obstacle_avoidance_planner obstacle_cruise_planner obstacle_stop_planner obstacle_velocity_limiter path_smoother planning_debug_tools planning_test_utils planning_topic_converter planning_validator route_handler rtc_interface rtc_replayer bezier_sampler frenet_planner path_sampler sampler_common scenario_selector static_centerline_optimizer surround_obstacle_checker gnss_poser image_diagnostics image_transport_decompressor imu_corrector livox_tag_filter pointcloud_preprocessor radar_scan_to_pointcloud2 radar_static_pointcloud_filter radar_threshold_filter radar_tracks_noise_filter tier4_pcl_extensions vehicle_velocity_converter autoware_auto_msgs_adapter bluetooth_monitor component_state_monitor default_ad_api ad_api_adaptors ad_api_visualizers automatic_pose_initializer diagnostic_graph_aggregator dummy_diag_publisher dummy_infrastructure duplicated_node_checker emergency_handler mrm_comfortable_stop_operator mrm_emergency_stop_operator system_error_monitor system_monitor topic_state_monitor velodyne_monitor accel_brake_map_calibrator external_cmd_converter raw_vehicle_cmd_converter steer_offset_estimator vehicle_info_util launch loop_sim ssh_machine gpudirect-dds autoware_ad_api_specs autoware_adapi_v1_msgs autoware_adapi_version_msgs autoware_auto_common autoware_auto_geometry autoware_auto_control_msgs autoware_auto_geometry_msgs autoware_auto_mapping_msgs autoware_auto_msgs autoware_auto_perception_msgs autoware_auto_planning_msgs autoware_auto_system_msgs autoware_auto_vehicle_msgs autoware_auto_perception_rviz_plugin autoware_auto_tf2 autoware_cmake autoware_lint_common autoware_utils lanelet2_extension autoware_common_msgs autoware_control_msgs autoware_localization_msgs autoware_map_msgs autoware_perception_msgs autoware_planning_msgs autoware_sensing_msgs autoware_system_msgs autoware_vehicle_msgs autoware_point_types autoware_testing bag_time_manager_rviz_plugin component_interface_specs component_interface_tools component_interface_utils cuda_utils fake_test_node geography_utils global_parameter_loader glog_component goal_distance_calculator grid_map_utils interpolation kalman_filter motion_utils object_recognition_utils osqp_interface path_distance_calculator perception_utils polar_grid qp_interface rtc_manager_rviz_plugin signal_processing tensorrt_common tier4_adapi_rviz_plugin tier4_api_utils tier4_automatic_goal_rviz_plugin tier4_autoware_utils tier4_calibration_rviz_plugin tier4_camera_view_rviz_plugin tier4_control_rviz_plugin tier4_datetime_rviz_plugin tier4_debug_rviz_plugin tier4_debug_tools tier4_localization_rviz_plugin tier4_perception_rviz_plugin tier4_planning_rviz_plugin tier4_screen_capture_rviz_plugin tier4_simulated_clock_rviz_plugin tier4_state_rviz_plugin tier4_system_rviz_plugin tier4_target_object_type_rviz_plugin tier4_traffic_light_rviz_plugin tier4_vehicle_rviz_plugin time_utils simulator_compatibility_test traffic_light_recognition_marker_publisher traffic_light_utils tvm_utility dma_customer_msg dma_transfer eagleye_coordinate eagleye_navigation eagleye_msgs eagleye_rt eagleye_can_velocity_converter eagleye_fix2kml eagleye_geo_pose_converter eagleye_geo_pose_fusion eagleye_gnss_converter eagleye_tf llh_converter morai_msgs mussp ndt_omp orocos_kdl python_orocos_kdl pointcloud_to_laserscan rtklib_bridge rtklib_msgs autoware_external_api_msgs autoware_iv_external_api_adaptor autoware_iv_internal_api_adaptor awapi_awiv_adapter tier4_api_msgs tier4_auto_msgs_converter tier4_control_msgs tier4_debug_msgs tier4_external_api_msgs tier4_hmi_msgs tier4_localization_msgs tier4_map_msgs tier4_perception_msgs tier4_planning_msgs tier4_rtc_msgs tier4_simulation_msgs tier4_system_msgs tier4_v2x_msgs tier4_vehicle_msgs tier4_autoware_api_launch tier4_control_launch tier4_localization_launch tier4_map_launch tier4_perception_launch tier4_planning_launch tier4_sensing_launch tier4_simulator_launch tier4_system_launch tier4_vehicle_launch fastrtps cyclonedds lanelet2 lanelet2_core lanelet2_examples lanelet2_io lanelet2_maps lanelet2_matching lanelet2_projection lanelet2_python lanelet2_routing lanelet2_traffic_rules lanelet2_validation sophus angles behaviortree_cpp_v3 bond bond_core bondcpp bondpy smclib test_bond cudnn_cmake_module diagnostic_aggregator diagnostic_common_diagnostics diagnostic_updater diagnostics self_test filters geodesy geographic_info geographic_msgs grid_map grid_map_cmake_helpers grid_map_core grid_map_costmap_2d grid_map_cv grid_map_demos grid_map_filters grid_map_loader grid_map_msgs grid_map_octomap grid_map_pcl grid_map_ros grid_map_rviz_plugin grid_map_sdf grid_map_visualization mrt_cmake_modules nav2_amcl nav2_behavior_tree nav2_behaviors nav2_bringup nav2_bt_navigator nav2_collision_monitor nav2_common nav2_controller nav2_core nav2_costmap_2d costmap_queue dwb_core dwb_critics dwb_msgs dwb_plugins nav2_dwb_controller nav_2d_msgs nav_2d_utils nav2_lifecycle_manager nav2_map_server nav2_msgs nav2_navfn_planner nav2_planner nav2_regulated_pure_pursuit_controller nav2_rotation_shim_controller nav2_rviz_plugins nav2_simple_commander nav2_smac_planner nav2_smoother nav2_system_tests nav2_theta_star_planner nav2_util nav2_velocity_smoother nav2_voxel_grid nav2_waypoint_follower navigation2 dynamic_edt_3d octomap octovis octomap_msgs osqp_vendor pacmod3_msgs pcl_msgs pcl_conversions pcl_ros perception_pcl point_cloud_msg_wrapper radar_msgs can_msgs rqt_tf_tree tensorrt_cmake_module topic_tools topic_tools_interfaces tvm_vendor cv_bridge image_geometry opencv_tests vision_opencv xacro rviz2 rviz_assimp_vendor rviz_common rviz_default_plugins rviz_ogre_vendor rviz_rendering rviz_rendering_tests rviz_visual_testing_framework dummy_perception_publisher fault_injection simple_planning_simulator image_view v4l2_camera aion_interface carla_vehicle_can cgi430_can_driver cgi610_driver ARS408_driver common_sensor_launch kunyi_sensor_kit_description kunyi_sensor_kit_launch classformsg node_v2x data_format_dump data_preprocess_launch lidar_centerpoint_collect lidar_saver message_sync time_cal camera_calibration direct_visual_lidar_calibration multi_lidar_calibration

ROS Distro
github

Package Summary

Tags No category tags.
Version 0.1.0
License Apache License 2.0
Build type AMENT_CMAKE
Use RECOMMENDED

Repository Summary

Description
Checkout URI https://github.com/ieiauto/autodrrt.git
VCS Type git
VCS Version main
Last Updated 2025-05-30
Dev Status UNKNOWN
Released UNRELEASED
Tags No category tags.
Contributing Help Wanted (-)
Good First Issues (-)
Pull Requests to Review (-)

Package Description

The motion_velocity_smoother package

Additional Links

No additional links.

Maintainers

  • Fumiya Watanabe
  • Takamasa Horibe
  • Makoto Kurihara

Authors

  • Takamasa Horibe
  • Fumiya Watanabe
  • Yutaka Shimizu
  • Makoto Kurihara

Motion Velocity Smoother

Purpose

motion_velocity_smootherは目標軌道上の各点における望ましい車速を計画して出力するモジュールである。 このモジュールは、速度の最大化と乗り心地の良さを両立するために、事前に指定された制限速度、制限加速度および制限躍度の範囲で車速を計画する。 加速度や躍度の制限を与えることは車速の変化を滑らかにすることに対応するため、このモジュールをmotion_velocity_smootherと呼んでいる。

Inner-workings / Algorithms

Flow chart

motion_velocity_smoother_flow

Extract trajectory

自車後輪軸中心位置に最も近い参照経路上の点に対し、extract_behind_distだけ戻った点からextract_ahead_distだけ進んだ点までの参照経路を抜き出す。

Apply external velocity limit

モジュール外部から指定された速度制限を適用する。 ここで扱う外部の速度制限は/planning/scenario_planning/max_velocityの topic で渡されるもので、地図上で設定された速度制限など、参照経路にすでに設定されている制限速度とは別である。 外部から指定される速度制限は、パラメータで指定されている減速度および躍度の制限の範囲で減速可能な位置から速度制限を適用する。

Apply stop approaching velocity

停止点に近づいたときの速度を設定する。障害物近傍まで近づく場合や、正着精度向上などの目的に用いる。

Apply lateral acceleration limit

経路の曲率に応じて、指定された最大横加速度max_lateral_accelを超えない速度を制限速度として設定する。ただし、制限速度はmin_curve_velocityを下回らないように設定する。

Resample trajectory

指定された時間間隔で経路の点を再サンプルする。ただし、経路全体の長さはmin_trajectory_lengthからmax_trajectory_lengthの間となるように再サンプルを行い、点の間隔はmin_trajectory_interval_distanceより小さくならないようにする。 現在車速でresample_timeの間進む距離までは密にサンプリングし、それ以降は疎にサンプリングする。 この方法でサンプリングすることで、低速時は密に、高速時は疎にサンプルされるため、停止精度と計算負荷軽減の両立を図っている。

Calculate initial state

速度計画のための初期値を計算する。初期値は状況に応じて下表のように計算する。

状況 初期速度 初期加速度
最初の計算時 現在車速 0.0
発進時 engage_velocity engage_acceleration
現在車速と計画車速が乖離 現在車速 前回計画値
通常時 前回計画値 前回計画値

Smooth velocity

速度の計画を行う。速度計画のアルゴリズムはJerkFiltered, L2, Linfの 3 種類のうちからコンフィグで指定する。 最適化のソルバは OSQP[1]を利用する。

JerkFiltered

速度の 2 乗(最小化で表すため負値で表現)、制限速度逸脱量の 2 乗、制限加度逸脱量の 2 乗、制限ジャーク逸脱量の 2 乗、ジャークの 2 乗の総和を最小化する。

L2

速度の 2 乗(最小化で表すため負値で表現)、制限速度逸脱量の 2 乗、制限加度逸脱量の 2 乗、疑似ジャーク[2]の 2 乗の総和を最小化する。

Linf

速度の 2 乗(最小化で表すため負値で表現)、制限速度逸脱量の 2 乗、制限加度逸脱量の 2 乗の総和と疑似ジャーク[2]の絶対最大値の和の最小化する。

Post process

計画された軌道の後処理を行う。

  • 停止点より先の速度を 0 に設定
  • 速度がパラメータで与えられるmax_velocity以下となるように設定
  • 自車位置より手前の点における速度を設定
  • Trajectory の仕様に合わせてリサンプリング(post resampling)
  • デバッグデータの出力

最適化の計算が終わったあと、次のノードに経路を渡す前にpost resamplingと呼ばれるリサンプリングを行う。ここで再度リサンプリングを行っている理由としては、最適化前で必要な経路間隔と後段のモジュールに渡す経路間隔が必ずしも一致していないからであり、その差を埋めるために再度サンプリングを行っている。そのため、post resamplingでは後段モジュールの経路仕様を確認してパラメータを決める必要がある。なお、最適化アルゴリズムの計算負荷が高く、最初のリサンプリングで経路間隔が後段モジュールの経路仕様より疎になっている場合、post resamplingで経路を蜜にリサンプリングする。逆に最適化アルゴリズムの計算負荷が小さく、最初のリサンプリングで経路間隔が後段の経路仕様より蜜になっている場合は、post resamplingで経路をその仕様に合わせて疎にリサンプリングする。

Inputs / Outputs

Input

Name Type Description
~/input/trajectory autoware_auto_planning_msgs/Trajectory Reference trajectory
/planning/scenario_planning/max_velocity std_msgs/Float32 External velocity limit [m/s]
/localization/kinematic_state nav_msgs/Odometry Current odometry
/tf tf2_msgs/TFMessage TF
/tf_static tf2_msgs/TFMessage TF static

Output

Name Type Description
~/output/trajectory autoware_auto_planning_msgs/Trajectory Modified trajectory
/planning/scenario_planning/current_max_velocity std_msgs/Float32 Current external velocity limit [m/s]
~/closest_velocity std_msgs/Float32 Planned velocity closest to ego base_link (for debug)
~/closest_acceleration std_msgs/Float32 Planned acceleration closest to ego base_link (for debug)
~/closest_jerk std_msgs/Float32 Planned jerk closest to ego base_link (for debug)
~/debug/trajectory_raw autoware_auto_planning_msgs/Trajectory Extracted trajectory (for debug)

File truncated at 100 lines see the full file

CHANGELOG
No CHANGELOG found.

Launch files

  • launch/motion_velocity_smoother.launch.xml
      • common_param_path [default: $(find-pkg-share motion_velocity_smoother)/config/default_common.param.yaml]
      • nearest_search_param_path
      • input_trajectory [default: /planning/scenario_planning/scenario_selector/trajectory]
      • output_trajectory [default: /planning/scenario_planning/trajectory]
      • publish_debug_trajs [default: false]
      • velocity_smoother_type [default: JerkFiltered]
      • param_path [default: $(find-pkg-share motion_velocity_smoother)/config/default_motion_velocity_smoother.param.yaml]
      • velocity_smoother_param_path [default: $(find-pkg-share motion_velocity_smoother)/config/$(var velocity_smoother_type).param.yaml]

Messages

No message files found.

Services

No service files found

Plugins

No plugins found.

Recent questions tagged motion_velocity_smoother at Robotics Stack Exchange

No version for distro iron showing github. Known supported distros are highlighted in the buttons above.
Package symbol

motion_velocity_smoother package from autodrrt repo

autonomous_emergency_braking control_performance_analysis control_validator external_cmd_selector joy_controller lane_departure_checker mpc_lateral_controller obstacle_collision_checker operation_mode_transition_manager pid_longitudinal_controller predicted_path_checker pure_pursuit shift_decider trajectory_follower_base trajectory_follower_node vehicle_cmd_gate diagnostic_converter kinematic_evaluator localization_evaluator planning_evaluator ekf_localizer geo_pose_projector gyro_odometer ar_tag_based_localizer landmark_manager localization_error_monitor localization_gnss localization_util ndt_scan_matcher pose2twist pose_initializer pose_instability_detector stop_filter tree_structured_parzen_estimator twist2accel yabloc_common yabloc_image_processing yabloc_monitor yabloc_particle_filter yabloc_pose_initializer map_height_fitter map_loader map_projection_loader map_tf_generator lanelet2_map_preprocessor ros2_bevdet bytetrack cluster_merger compare_map_segmentation crosswalk_traffic_light_estimator detected_object_feature_remover detected_object_validation detection_by_tracker elevation_map_loader euclidean_cluster front_vehicle_velocity_estimator ground_segmentation heatmap_visualizer image_projection_based_fusion lidar_apollo_instance_segmentation lidar_apollo_segmentation_tvm lidar_apollo_segmentation_tvm_nodes lidar_centerpoint lidar_centerpoint_tvm map_based_prediction multi_object_tracker object_merger object_range_splitter object_velocity_splitter occupancy_grid_map_outlier_filter probabilistic_occupancy_grid_map radar_crossing_objects_noise_filter radar_fusion_to_detected_object radar_object_clustering radar_object_tracker radar_tracks_msgs_converter shape_estimation simple_object_merger tensorrt_classifier tensorrt_yolo tensorrt_yolox tracking_object_merger traffic_light_arbiter traffic_light_classifier traffic_light_fine_detector traffic_light_map_based_detector traffic_light_multi_camera_fusion traffic_light_occlusion_predictor traffic_light_ssd_fine_detector traffic_light_visualization behavior_path_avoidance_by_lane_change_module behavior_path_avoidance_module behavior_path_external_request_lane_change_module behavior_path_goal_planner_module behavior_path_lane_change_module behavior_path_planner behavior_path_planner_common behavior_path_side_shift_module behavior_path_start_planner_module behavior_velocity_blind_spot_module behavior_velocity_crosswalk_module behavior_velocity_detection_area_module behavior_velocity_intersection_module behavior_velocity_no_drivable_lane_module behavior_velocity_no_stopping_area_module behavior_velocity_occlusion_spot_module behavior_velocity_out_of_lane_module behavior_velocity_planner behavior_velocity_planner_common behavior_velocity_run_out_module behavior_velocity_speed_bump_module behavior_velocity_stop_line_module behavior_velocity_template_module behavior_velocity_traffic_light_module behavior_velocity_virtual_traffic_light_module behavior_velocity_walkway_module costmap_generator external_velocity_limit_selector freespace_planner freespace_planning_algorithms mission_planner motion_velocity_smoother objects_of_interest_marker_interface obstacle_avoidance_planner obstacle_cruise_planner obstacle_stop_planner obstacle_velocity_limiter path_smoother planning_debug_tools planning_test_utils planning_topic_converter planning_validator route_handler rtc_interface rtc_replayer bezier_sampler frenet_planner path_sampler sampler_common scenario_selector static_centerline_optimizer surround_obstacle_checker gnss_poser image_diagnostics image_transport_decompressor imu_corrector livox_tag_filter pointcloud_preprocessor radar_scan_to_pointcloud2 radar_static_pointcloud_filter radar_threshold_filter radar_tracks_noise_filter tier4_pcl_extensions vehicle_velocity_converter autoware_auto_msgs_adapter bluetooth_monitor component_state_monitor default_ad_api ad_api_adaptors ad_api_visualizers automatic_pose_initializer diagnostic_graph_aggregator dummy_diag_publisher dummy_infrastructure duplicated_node_checker emergency_handler mrm_comfortable_stop_operator mrm_emergency_stop_operator system_error_monitor system_monitor topic_state_monitor velodyne_monitor accel_brake_map_calibrator external_cmd_converter raw_vehicle_cmd_converter steer_offset_estimator vehicle_info_util launch loop_sim ssh_machine gpudirect-dds autoware_ad_api_specs autoware_adapi_v1_msgs autoware_adapi_version_msgs autoware_auto_common autoware_auto_geometry autoware_auto_control_msgs autoware_auto_geometry_msgs autoware_auto_mapping_msgs autoware_auto_msgs autoware_auto_perception_msgs autoware_auto_planning_msgs autoware_auto_system_msgs autoware_auto_vehicle_msgs autoware_auto_perception_rviz_plugin autoware_auto_tf2 autoware_cmake autoware_lint_common autoware_utils lanelet2_extension autoware_common_msgs autoware_control_msgs autoware_localization_msgs autoware_map_msgs autoware_perception_msgs autoware_planning_msgs autoware_sensing_msgs autoware_system_msgs autoware_vehicle_msgs autoware_point_types autoware_testing bag_time_manager_rviz_plugin component_interface_specs component_interface_tools component_interface_utils cuda_utils fake_test_node geography_utils global_parameter_loader glog_component goal_distance_calculator grid_map_utils interpolation kalman_filter motion_utils object_recognition_utils osqp_interface path_distance_calculator perception_utils polar_grid qp_interface rtc_manager_rviz_plugin signal_processing tensorrt_common tier4_adapi_rviz_plugin tier4_api_utils tier4_automatic_goal_rviz_plugin tier4_autoware_utils tier4_calibration_rviz_plugin tier4_camera_view_rviz_plugin tier4_control_rviz_plugin tier4_datetime_rviz_plugin tier4_debug_rviz_plugin tier4_debug_tools tier4_localization_rviz_plugin tier4_perception_rviz_plugin tier4_planning_rviz_plugin tier4_screen_capture_rviz_plugin tier4_simulated_clock_rviz_plugin tier4_state_rviz_plugin tier4_system_rviz_plugin tier4_target_object_type_rviz_plugin tier4_traffic_light_rviz_plugin tier4_vehicle_rviz_plugin time_utils simulator_compatibility_test traffic_light_recognition_marker_publisher traffic_light_utils tvm_utility dma_customer_msg dma_transfer eagleye_coordinate eagleye_navigation eagleye_msgs eagleye_rt eagleye_can_velocity_converter eagleye_fix2kml eagleye_geo_pose_converter eagleye_geo_pose_fusion eagleye_gnss_converter eagleye_tf llh_converter morai_msgs mussp ndt_omp orocos_kdl python_orocos_kdl pointcloud_to_laserscan rtklib_bridge rtklib_msgs autoware_external_api_msgs autoware_iv_external_api_adaptor autoware_iv_internal_api_adaptor awapi_awiv_adapter tier4_api_msgs tier4_auto_msgs_converter tier4_control_msgs tier4_debug_msgs tier4_external_api_msgs tier4_hmi_msgs tier4_localization_msgs tier4_map_msgs tier4_perception_msgs tier4_planning_msgs tier4_rtc_msgs tier4_simulation_msgs tier4_system_msgs tier4_v2x_msgs tier4_vehicle_msgs tier4_autoware_api_launch tier4_control_launch tier4_localization_launch tier4_map_launch tier4_perception_launch tier4_planning_launch tier4_sensing_launch tier4_simulator_launch tier4_system_launch tier4_vehicle_launch fastrtps cyclonedds lanelet2 lanelet2_core lanelet2_examples lanelet2_io lanelet2_maps lanelet2_matching lanelet2_projection lanelet2_python lanelet2_routing lanelet2_traffic_rules lanelet2_validation sophus angles behaviortree_cpp_v3 bond bond_core bondcpp bondpy smclib test_bond cudnn_cmake_module diagnostic_aggregator diagnostic_common_diagnostics diagnostic_updater diagnostics self_test filters geodesy geographic_info geographic_msgs grid_map grid_map_cmake_helpers grid_map_core grid_map_costmap_2d grid_map_cv grid_map_demos grid_map_filters grid_map_loader grid_map_msgs grid_map_octomap grid_map_pcl grid_map_ros grid_map_rviz_plugin grid_map_sdf grid_map_visualization mrt_cmake_modules nav2_amcl nav2_behavior_tree nav2_behaviors nav2_bringup nav2_bt_navigator nav2_collision_monitor nav2_common nav2_controller nav2_core nav2_costmap_2d costmap_queue dwb_core dwb_critics dwb_msgs dwb_plugins nav2_dwb_controller nav_2d_msgs nav_2d_utils nav2_lifecycle_manager nav2_map_server nav2_msgs nav2_navfn_planner nav2_planner nav2_regulated_pure_pursuit_controller nav2_rotation_shim_controller nav2_rviz_plugins nav2_simple_commander nav2_smac_planner nav2_smoother nav2_system_tests nav2_theta_star_planner nav2_util nav2_velocity_smoother nav2_voxel_grid nav2_waypoint_follower navigation2 dynamic_edt_3d octomap octovis octomap_msgs osqp_vendor pacmod3_msgs pcl_msgs pcl_conversions pcl_ros perception_pcl point_cloud_msg_wrapper radar_msgs can_msgs rqt_tf_tree tensorrt_cmake_module topic_tools topic_tools_interfaces tvm_vendor cv_bridge image_geometry opencv_tests vision_opencv xacro rviz2 rviz_assimp_vendor rviz_common rviz_default_plugins rviz_ogre_vendor rviz_rendering rviz_rendering_tests rviz_visual_testing_framework dummy_perception_publisher fault_injection simple_planning_simulator image_view v4l2_camera aion_interface carla_vehicle_can cgi430_can_driver cgi610_driver ARS408_driver common_sensor_launch kunyi_sensor_kit_description kunyi_sensor_kit_launch classformsg node_v2x data_format_dump data_preprocess_launch lidar_centerpoint_collect lidar_saver message_sync time_cal camera_calibration direct_visual_lidar_calibration multi_lidar_calibration

ROS Distro
github

Package Summary

Tags No category tags.
Version 0.1.0
License Apache License 2.0
Build type AMENT_CMAKE
Use RECOMMENDED

Repository Summary

Description
Checkout URI https://github.com/ieiauto/autodrrt.git
VCS Type git
VCS Version main
Last Updated 2025-05-30
Dev Status UNKNOWN
Released UNRELEASED
Tags No category tags.
Contributing Help Wanted (-)
Good First Issues (-)
Pull Requests to Review (-)

Package Description

The motion_velocity_smoother package

Additional Links

No additional links.

Maintainers

  • Fumiya Watanabe
  • Takamasa Horibe
  • Makoto Kurihara

Authors

  • Takamasa Horibe
  • Fumiya Watanabe
  • Yutaka Shimizu
  • Makoto Kurihara

Motion Velocity Smoother

Purpose

motion_velocity_smootherは目標軌道上の各点における望ましい車速を計画して出力するモジュールである。 このモジュールは、速度の最大化と乗り心地の良さを両立するために、事前に指定された制限速度、制限加速度および制限躍度の範囲で車速を計画する。 加速度や躍度の制限を与えることは車速の変化を滑らかにすることに対応するため、このモジュールをmotion_velocity_smootherと呼んでいる。

Inner-workings / Algorithms

Flow chart

motion_velocity_smoother_flow

Extract trajectory

自車後輪軸中心位置に最も近い参照経路上の点に対し、extract_behind_distだけ戻った点からextract_ahead_distだけ進んだ点までの参照経路を抜き出す。

Apply external velocity limit

モジュール外部から指定された速度制限を適用する。 ここで扱う外部の速度制限は/planning/scenario_planning/max_velocityの topic で渡されるもので、地図上で設定された速度制限など、参照経路にすでに設定されている制限速度とは別である。 外部から指定される速度制限は、パラメータで指定されている減速度および躍度の制限の範囲で減速可能な位置から速度制限を適用する。

Apply stop approaching velocity

停止点に近づいたときの速度を設定する。障害物近傍まで近づく場合や、正着精度向上などの目的に用いる。

Apply lateral acceleration limit

経路の曲率に応じて、指定された最大横加速度max_lateral_accelを超えない速度を制限速度として設定する。ただし、制限速度はmin_curve_velocityを下回らないように設定する。

Resample trajectory

指定された時間間隔で経路の点を再サンプルする。ただし、経路全体の長さはmin_trajectory_lengthからmax_trajectory_lengthの間となるように再サンプルを行い、点の間隔はmin_trajectory_interval_distanceより小さくならないようにする。 現在車速でresample_timeの間進む距離までは密にサンプリングし、それ以降は疎にサンプリングする。 この方法でサンプリングすることで、低速時は密に、高速時は疎にサンプルされるため、停止精度と計算負荷軽減の両立を図っている。

Calculate initial state

速度計画のための初期値を計算する。初期値は状況に応じて下表のように計算する。

状況 初期速度 初期加速度
最初の計算時 現在車速 0.0
発進時 engage_velocity engage_acceleration
現在車速と計画車速が乖離 現在車速 前回計画値
通常時 前回計画値 前回計画値

Smooth velocity

速度の計画を行う。速度計画のアルゴリズムはJerkFiltered, L2, Linfの 3 種類のうちからコンフィグで指定する。 最適化のソルバは OSQP[1]を利用する。

JerkFiltered

速度の 2 乗(最小化で表すため負値で表現)、制限速度逸脱量の 2 乗、制限加度逸脱量の 2 乗、制限ジャーク逸脱量の 2 乗、ジャークの 2 乗の総和を最小化する。

L2

速度の 2 乗(最小化で表すため負値で表現)、制限速度逸脱量の 2 乗、制限加度逸脱量の 2 乗、疑似ジャーク[2]の 2 乗の総和を最小化する。

Linf

速度の 2 乗(最小化で表すため負値で表現)、制限速度逸脱量の 2 乗、制限加度逸脱量の 2 乗の総和と疑似ジャーク[2]の絶対最大値の和の最小化する。

Post process

計画された軌道の後処理を行う。

  • 停止点より先の速度を 0 に設定
  • 速度がパラメータで与えられるmax_velocity以下となるように設定
  • 自車位置より手前の点における速度を設定
  • Trajectory の仕様に合わせてリサンプリング(post resampling)
  • デバッグデータの出力

最適化の計算が終わったあと、次のノードに経路を渡す前にpost resamplingと呼ばれるリサンプリングを行う。ここで再度リサンプリングを行っている理由としては、最適化前で必要な経路間隔と後段のモジュールに渡す経路間隔が必ずしも一致していないからであり、その差を埋めるために再度サンプリングを行っている。そのため、post resamplingでは後段モジュールの経路仕様を確認してパラメータを決める必要がある。なお、最適化アルゴリズムの計算負荷が高く、最初のリサンプリングで経路間隔が後段モジュールの経路仕様より疎になっている場合、post resamplingで経路を蜜にリサンプリングする。逆に最適化アルゴリズムの計算負荷が小さく、最初のリサンプリングで経路間隔が後段の経路仕様より蜜になっている場合は、post resamplingで経路をその仕様に合わせて疎にリサンプリングする。

Inputs / Outputs

Input

Name Type Description
~/input/trajectory autoware_auto_planning_msgs/Trajectory Reference trajectory
/planning/scenario_planning/max_velocity std_msgs/Float32 External velocity limit [m/s]
/localization/kinematic_state nav_msgs/Odometry Current odometry
/tf tf2_msgs/TFMessage TF
/tf_static tf2_msgs/TFMessage TF static

Output

Name Type Description
~/output/trajectory autoware_auto_planning_msgs/Trajectory Modified trajectory
/planning/scenario_planning/current_max_velocity std_msgs/Float32 Current external velocity limit [m/s]
~/closest_velocity std_msgs/Float32 Planned velocity closest to ego base_link (for debug)
~/closest_acceleration std_msgs/Float32 Planned acceleration closest to ego base_link (for debug)
~/closest_jerk std_msgs/Float32 Planned jerk closest to ego base_link (for debug)
~/debug/trajectory_raw autoware_auto_planning_msgs/Trajectory Extracted trajectory (for debug)

File truncated at 100 lines see the full file

CHANGELOG
No CHANGELOG found.

Launch files

  • launch/motion_velocity_smoother.launch.xml
      • common_param_path [default: $(find-pkg-share motion_velocity_smoother)/config/default_common.param.yaml]
      • nearest_search_param_path
      • input_trajectory [default: /planning/scenario_planning/scenario_selector/trajectory]
      • output_trajectory [default: /planning/scenario_planning/trajectory]
      • publish_debug_trajs [default: false]
      • velocity_smoother_type [default: JerkFiltered]
      • param_path [default: $(find-pkg-share motion_velocity_smoother)/config/default_motion_velocity_smoother.param.yaml]
      • velocity_smoother_param_path [default: $(find-pkg-share motion_velocity_smoother)/config/$(var velocity_smoother_type).param.yaml]

Messages

No message files found.

Services

No service files found

Plugins

No plugins found.

Recent questions tagged motion_velocity_smoother at Robotics Stack Exchange

No version for distro melodic showing github. Known supported distros are highlighted in the buttons above.
Package symbol

motion_velocity_smoother package from autodrrt repo

autonomous_emergency_braking control_performance_analysis control_validator external_cmd_selector joy_controller lane_departure_checker mpc_lateral_controller obstacle_collision_checker operation_mode_transition_manager pid_longitudinal_controller predicted_path_checker pure_pursuit shift_decider trajectory_follower_base trajectory_follower_node vehicle_cmd_gate diagnostic_converter kinematic_evaluator localization_evaluator planning_evaluator ekf_localizer geo_pose_projector gyro_odometer ar_tag_based_localizer landmark_manager localization_error_monitor localization_gnss localization_util ndt_scan_matcher pose2twist pose_initializer pose_instability_detector stop_filter tree_structured_parzen_estimator twist2accel yabloc_common yabloc_image_processing yabloc_monitor yabloc_particle_filter yabloc_pose_initializer map_height_fitter map_loader map_projection_loader map_tf_generator lanelet2_map_preprocessor ros2_bevdet bytetrack cluster_merger compare_map_segmentation crosswalk_traffic_light_estimator detected_object_feature_remover detected_object_validation detection_by_tracker elevation_map_loader euclidean_cluster front_vehicle_velocity_estimator ground_segmentation heatmap_visualizer image_projection_based_fusion lidar_apollo_instance_segmentation lidar_apollo_segmentation_tvm lidar_apollo_segmentation_tvm_nodes lidar_centerpoint lidar_centerpoint_tvm map_based_prediction multi_object_tracker object_merger object_range_splitter object_velocity_splitter occupancy_grid_map_outlier_filter probabilistic_occupancy_grid_map radar_crossing_objects_noise_filter radar_fusion_to_detected_object radar_object_clustering radar_object_tracker radar_tracks_msgs_converter shape_estimation simple_object_merger tensorrt_classifier tensorrt_yolo tensorrt_yolox tracking_object_merger traffic_light_arbiter traffic_light_classifier traffic_light_fine_detector traffic_light_map_based_detector traffic_light_multi_camera_fusion traffic_light_occlusion_predictor traffic_light_ssd_fine_detector traffic_light_visualization behavior_path_avoidance_by_lane_change_module behavior_path_avoidance_module behavior_path_external_request_lane_change_module behavior_path_goal_planner_module behavior_path_lane_change_module behavior_path_planner behavior_path_planner_common behavior_path_side_shift_module behavior_path_start_planner_module behavior_velocity_blind_spot_module behavior_velocity_crosswalk_module behavior_velocity_detection_area_module behavior_velocity_intersection_module behavior_velocity_no_drivable_lane_module behavior_velocity_no_stopping_area_module behavior_velocity_occlusion_spot_module behavior_velocity_out_of_lane_module behavior_velocity_planner behavior_velocity_planner_common behavior_velocity_run_out_module behavior_velocity_speed_bump_module behavior_velocity_stop_line_module behavior_velocity_template_module behavior_velocity_traffic_light_module behavior_velocity_virtual_traffic_light_module behavior_velocity_walkway_module costmap_generator external_velocity_limit_selector freespace_planner freespace_planning_algorithms mission_planner motion_velocity_smoother objects_of_interest_marker_interface obstacle_avoidance_planner obstacle_cruise_planner obstacle_stop_planner obstacle_velocity_limiter path_smoother planning_debug_tools planning_test_utils planning_topic_converter planning_validator route_handler rtc_interface rtc_replayer bezier_sampler frenet_planner path_sampler sampler_common scenario_selector static_centerline_optimizer surround_obstacle_checker gnss_poser image_diagnostics image_transport_decompressor imu_corrector livox_tag_filter pointcloud_preprocessor radar_scan_to_pointcloud2 radar_static_pointcloud_filter radar_threshold_filter radar_tracks_noise_filter tier4_pcl_extensions vehicle_velocity_converter autoware_auto_msgs_adapter bluetooth_monitor component_state_monitor default_ad_api ad_api_adaptors ad_api_visualizers automatic_pose_initializer diagnostic_graph_aggregator dummy_diag_publisher dummy_infrastructure duplicated_node_checker emergency_handler mrm_comfortable_stop_operator mrm_emergency_stop_operator system_error_monitor system_monitor topic_state_monitor velodyne_monitor accel_brake_map_calibrator external_cmd_converter raw_vehicle_cmd_converter steer_offset_estimator vehicle_info_util launch loop_sim ssh_machine gpudirect-dds autoware_ad_api_specs autoware_adapi_v1_msgs autoware_adapi_version_msgs autoware_auto_common autoware_auto_geometry autoware_auto_control_msgs autoware_auto_geometry_msgs autoware_auto_mapping_msgs autoware_auto_msgs autoware_auto_perception_msgs autoware_auto_planning_msgs autoware_auto_system_msgs autoware_auto_vehicle_msgs autoware_auto_perception_rviz_plugin autoware_auto_tf2 autoware_cmake autoware_lint_common autoware_utils lanelet2_extension autoware_common_msgs autoware_control_msgs autoware_localization_msgs autoware_map_msgs autoware_perception_msgs autoware_planning_msgs autoware_sensing_msgs autoware_system_msgs autoware_vehicle_msgs autoware_point_types autoware_testing bag_time_manager_rviz_plugin component_interface_specs component_interface_tools component_interface_utils cuda_utils fake_test_node geography_utils global_parameter_loader glog_component goal_distance_calculator grid_map_utils interpolation kalman_filter motion_utils object_recognition_utils osqp_interface path_distance_calculator perception_utils polar_grid qp_interface rtc_manager_rviz_plugin signal_processing tensorrt_common tier4_adapi_rviz_plugin tier4_api_utils tier4_automatic_goal_rviz_plugin tier4_autoware_utils tier4_calibration_rviz_plugin tier4_camera_view_rviz_plugin tier4_control_rviz_plugin tier4_datetime_rviz_plugin tier4_debug_rviz_plugin tier4_debug_tools tier4_localization_rviz_plugin tier4_perception_rviz_plugin tier4_planning_rviz_plugin tier4_screen_capture_rviz_plugin tier4_simulated_clock_rviz_plugin tier4_state_rviz_plugin tier4_system_rviz_plugin tier4_target_object_type_rviz_plugin tier4_traffic_light_rviz_plugin tier4_vehicle_rviz_plugin time_utils simulator_compatibility_test traffic_light_recognition_marker_publisher traffic_light_utils tvm_utility dma_customer_msg dma_transfer eagleye_coordinate eagleye_navigation eagleye_msgs eagleye_rt eagleye_can_velocity_converter eagleye_fix2kml eagleye_geo_pose_converter eagleye_geo_pose_fusion eagleye_gnss_converter eagleye_tf llh_converter morai_msgs mussp ndt_omp orocos_kdl python_orocos_kdl pointcloud_to_laserscan rtklib_bridge rtklib_msgs autoware_external_api_msgs autoware_iv_external_api_adaptor autoware_iv_internal_api_adaptor awapi_awiv_adapter tier4_api_msgs tier4_auto_msgs_converter tier4_control_msgs tier4_debug_msgs tier4_external_api_msgs tier4_hmi_msgs tier4_localization_msgs tier4_map_msgs tier4_perception_msgs tier4_planning_msgs tier4_rtc_msgs tier4_simulation_msgs tier4_system_msgs tier4_v2x_msgs tier4_vehicle_msgs tier4_autoware_api_launch tier4_control_launch tier4_localization_launch tier4_map_launch tier4_perception_launch tier4_planning_launch tier4_sensing_launch tier4_simulator_launch tier4_system_launch tier4_vehicle_launch fastrtps cyclonedds lanelet2 lanelet2_core lanelet2_examples lanelet2_io lanelet2_maps lanelet2_matching lanelet2_projection lanelet2_python lanelet2_routing lanelet2_traffic_rules lanelet2_validation sophus angles behaviortree_cpp_v3 bond bond_core bondcpp bondpy smclib test_bond cudnn_cmake_module diagnostic_aggregator diagnostic_common_diagnostics diagnostic_updater diagnostics self_test filters geodesy geographic_info geographic_msgs grid_map grid_map_cmake_helpers grid_map_core grid_map_costmap_2d grid_map_cv grid_map_demos grid_map_filters grid_map_loader grid_map_msgs grid_map_octomap grid_map_pcl grid_map_ros grid_map_rviz_plugin grid_map_sdf grid_map_visualization mrt_cmake_modules nav2_amcl nav2_behavior_tree nav2_behaviors nav2_bringup nav2_bt_navigator nav2_collision_monitor nav2_common nav2_controller nav2_core nav2_costmap_2d costmap_queue dwb_core dwb_critics dwb_msgs dwb_plugins nav2_dwb_controller nav_2d_msgs nav_2d_utils nav2_lifecycle_manager nav2_map_server nav2_msgs nav2_navfn_planner nav2_planner nav2_regulated_pure_pursuit_controller nav2_rotation_shim_controller nav2_rviz_plugins nav2_simple_commander nav2_smac_planner nav2_smoother nav2_system_tests nav2_theta_star_planner nav2_util nav2_velocity_smoother nav2_voxel_grid nav2_waypoint_follower navigation2 dynamic_edt_3d octomap octovis octomap_msgs osqp_vendor pacmod3_msgs pcl_msgs pcl_conversions pcl_ros perception_pcl point_cloud_msg_wrapper radar_msgs can_msgs rqt_tf_tree tensorrt_cmake_module topic_tools topic_tools_interfaces tvm_vendor cv_bridge image_geometry opencv_tests vision_opencv xacro rviz2 rviz_assimp_vendor rviz_common rviz_default_plugins rviz_ogre_vendor rviz_rendering rviz_rendering_tests rviz_visual_testing_framework dummy_perception_publisher fault_injection simple_planning_simulator image_view v4l2_camera aion_interface carla_vehicle_can cgi430_can_driver cgi610_driver ARS408_driver common_sensor_launch kunyi_sensor_kit_description kunyi_sensor_kit_launch classformsg node_v2x data_format_dump data_preprocess_launch lidar_centerpoint_collect lidar_saver message_sync time_cal camera_calibration direct_visual_lidar_calibration multi_lidar_calibration

ROS Distro
github

Package Summary

Tags No category tags.
Version 0.1.0
License Apache License 2.0
Build type AMENT_CMAKE
Use RECOMMENDED

Repository Summary

Description
Checkout URI https://github.com/ieiauto/autodrrt.git
VCS Type git
VCS Version main
Last Updated 2025-05-30
Dev Status UNKNOWN
Released UNRELEASED
Tags No category tags.
Contributing Help Wanted (-)
Good First Issues (-)
Pull Requests to Review (-)

Package Description

The motion_velocity_smoother package

Additional Links

No additional links.

Maintainers

  • Fumiya Watanabe
  • Takamasa Horibe
  • Makoto Kurihara

Authors

  • Takamasa Horibe
  • Fumiya Watanabe
  • Yutaka Shimizu
  • Makoto Kurihara

Motion Velocity Smoother

Purpose

motion_velocity_smootherは目標軌道上の各点における望ましい車速を計画して出力するモジュールである。 このモジュールは、速度の最大化と乗り心地の良さを両立するために、事前に指定された制限速度、制限加速度および制限躍度の範囲で車速を計画する。 加速度や躍度の制限を与えることは車速の変化を滑らかにすることに対応するため、このモジュールをmotion_velocity_smootherと呼んでいる。

Inner-workings / Algorithms

Flow chart

motion_velocity_smoother_flow

Extract trajectory

自車後輪軸中心位置に最も近い参照経路上の点に対し、extract_behind_distだけ戻った点からextract_ahead_distだけ進んだ点までの参照経路を抜き出す。

Apply external velocity limit

モジュール外部から指定された速度制限を適用する。 ここで扱う外部の速度制限は/planning/scenario_planning/max_velocityの topic で渡されるもので、地図上で設定された速度制限など、参照経路にすでに設定されている制限速度とは別である。 外部から指定される速度制限は、パラメータで指定されている減速度および躍度の制限の範囲で減速可能な位置から速度制限を適用する。

Apply stop approaching velocity

停止点に近づいたときの速度を設定する。障害物近傍まで近づく場合や、正着精度向上などの目的に用いる。

Apply lateral acceleration limit

経路の曲率に応じて、指定された最大横加速度max_lateral_accelを超えない速度を制限速度として設定する。ただし、制限速度はmin_curve_velocityを下回らないように設定する。

Resample trajectory

指定された時間間隔で経路の点を再サンプルする。ただし、経路全体の長さはmin_trajectory_lengthからmax_trajectory_lengthの間となるように再サンプルを行い、点の間隔はmin_trajectory_interval_distanceより小さくならないようにする。 現在車速でresample_timeの間進む距離までは密にサンプリングし、それ以降は疎にサンプリングする。 この方法でサンプリングすることで、低速時は密に、高速時は疎にサンプルされるため、停止精度と計算負荷軽減の両立を図っている。

Calculate initial state

速度計画のための初期値を計算する。初期値は状況に応じて下表のように計算する。

状況 初期速度 初期加速度
最初の計算時 現在車速 0.0
発進時 engage_velocity engage_acceleration
現在車速と計画車速が乖離 現在車速 前回計画値
通常時 前回計画値 前回計画値

Smooth velocity

速度の計画を行う。速度計画のアルゴリズムはJerkFiltered, L2, Linfの 3 種類のうちからコンフィグで指定する。 最適化のソルバは OSQP[1]を利用する。

JerkFiltered

速度の 2 乗(最小化で表すため負値で表現)、制限速度逸脱量の 2 乗、制限加度逸脱量の 2 乗、制限ジャーク逸脱量の 2 乗、ジャークの 2 乗の総和を最小化する。

L2

速度の 2 乗(最小化で表すため負値で表現)、制限速度逸脱量の 2 乗、制限加度逸脱量の 2 乗、疑似ジャーク[2]の 2 乗の総和を最小化する。

Linf

速度の 2 乗(最小化で表すため負値で表現)、制限速度逸脱量の 2 乗、制限加度逸脱量の 2 乗の総和と疑似ジャーク[2]の絶対最大値の和の最小化する。

Post process

計画された軌道の後処理を行う。

  • 停止点より先の速度を 0 に設定
  • 速度がパラメータで与えられるmax_velocity以下となるように設定
  • 自車位置より手前の点における速度を設定
  • Trajectory の仕様に合わせてリサンプリング(post resampling)
  • デバッグデータの出力

最適化の計算が終わったあと、次のノードに経路を渡す前にpost resamplingと呼ばれるリサンプリングを行う。ここで再度リサンプリングを行っている理由としては、最適化前で必要な経路間隔と後段のモジュールに渡す経路間隔が必ずしも一致していないからであり、その差を埋めるために再度サンプリングを行っている。そのため、post resamplingでは後段モジュールの経路仕様を確認してパラメータを決める必要がある。なお、最適化アルゴリズムの計算負荷が高く、最初のリサンプリングで経路間隔が後段モジュールの経路仕様より疎になっている場合、post resamplingで経路を蜜にリサンプリングする。逆に最適化アルゴリズムの計算負荷が小さく、最初のリサンプリングで経路間隔が後段の経路仕様より蜜になっている場合は、post resamplingで経路をその仕様に合わせて疎にリサンプリングする。

Inputs / Outputs

Input

Name Type Description
~/input/trajectory autoware_auto_planning_msgs/Trajectory Reference trajectory
/planning/scenario_planning/max_velocity std_msgs/Float32 External velocity limit [m/s]
/localization/kinematic_state nav_msgs/Odometry Current odometry
/tf tf2_msgs/TFMessage TF
/tf_static tf2_msgs/TFMessage TF static

Output

Name Type Description
~/output/trajectory autoware_auto_planning_msgs/Trajectory Modified trajectory
/planning/scenario_planning/current_max_velocity std_msgs/Float32 Current external velocity limit [m/s]
~/closest_velocity std_msgs/Float32 Planned velocity closest to ego base_link (for debug)
~/closest_acceleration std_msgs/Float32 Planned acceleration closest to ego base_link (for debug)
~/closest_jerk std_msgs/Float32 Planned jerk closest to ego base_link (for debug)
~/debug/trajectory_raw autoware_auto_planning_msgs/Trajectory Extracted trajectory (for debug)

File truncated at 100 lines see the full file

CHANGELOG
No CHANGELOG found.

Launch files

  • launch/motion_velocity_smoother.launch.xml
      • common_param_path [default: $(find-pkg-share motion_velocity_smoother)/config/default_common.param.yaml]
      • nearest_search_param_path
      • input_trajectory [default: /planning/scenario_planning/scenario_selector/trajectory]
      • output_trajectory [default: /planning/scenario_planning/trajectory]
      • publish_debug_trajs [default: false]
      • velocity_smoother_type [default: JerkFiltered]
      • param_path [default: $(find-pkg-share motion_velocity_smoother)/config/default_motion_velocity_smoother.param.yaml]
      • velocity_smoother_param_path [default: $(find-pkg-share motion_velocity_smoother)/config/$(var velocity_smoother_type).param.yaml]

Messages

No message files found.

Services

No service files found

Plugins

No plugins found.

Recent questions tagged motion_velocity_smoother at Robotics Stack Exchange

No version for distro noetic showing github. Known supported distros are highlighted in the buttons above.
Package symbol

motion_velocity_smoother package from autodrrt repo

autonomous_emergency_braking control_performance_analysis control_validator external_cmd_selector joy_controller lane_departure_checker mpc_lateral_controller obstacle_collision_checker operation_mode_transition_manager pid_longitudinal_controller predicted_path_checker pure_pursuit shift_decider trajectory_follower_base trajectory_follower_node vehicle_cmd_gate diagnostic_converter kinematic_evaluator localization_evaluator planning_evaluator ekf_localizer geo_pose_projector gyro_odometer ar_tag_based_localizer landmark_manager localization_error_monitor localization_gnss localization_util ndt_scan_matcher pose2twist pose_initializer pose_instability_detector stop_filter tree_structured_parzen_estimator twist2accel yabloc_common yabloc_image_processing yabloc_monitor yabloc_particle_filter yabloc_pose_initializer map_height_fitter map_loader map_projection_loader map_tf_generator lanelet2_map_preprocessor ros2_bevdet bytetrack cluster_merger compare_map_segmentation crosswalk_traffic_light_estimator detected_object_feature_remover detected_object_validation detection_by_tracker elevation_map_loader euclidean_cluster front_vehicle_velocity_estimator ground_segmentation heatmap_visualizer image_projection_based_fusion lidar_apollo_instance_segmentation lidar_apollo_segmentation_tvm lidar_apollo_segmentation_tvm_nodes lidar_centerpoint lidar_centerpoint_tvm map_based_prediction multi_object_tracker object_merger object_range_splitter object_velocity_splitter occupancy_grid_map_outlier_filter probabilistic_occupancy_grid_map radar_crossing_objects_noise_filter radar_fusion_to_detected_object radar_object_clustering radar_object_tracker radar_tracks_msgs_converter shape_estimation simple_object_merger tensorrt_classifier tensorrt_yolo tensorrt_yolox tracking_object_merger traffic_light_arbiter traffic_light_classifier traffic_light_fine_detector traffic_light_map_based_detector traffic_light_multi_camera_fusion traffic_light_occlusion_predictor traffic_light_ssd_fine_detector traffic_light_visualization behavior_path_avoidance_by_lane_change_module behavior_path_avoidance_module behavior_path_external_request_lane_change_module behavior_path_goal_planner_module behavior_path_lane_change_module behavior_path_planner behavior_path_planner_common behavior_path_side_shift_module behavior_path_start_planner_module behavior_velocity_blind_spot_module behavior_velocity_crosswalk_module behavior_velocity_detection_area_module behavior_velocity_intersection_module behavior_velocity_no_drivable_lane_module behavior_velocity_no_stopping_area_module behavior_velocity_occlusion_spot_module behavior_velocity_out_of_lane_module behavior_velocity_planner behavior_velocity_planner_common behavior_velocity_run_out_module behavior_velocity_speed_bump_module behavior_velocity_stop_line_module behavior_velocity_template_module behavior_velocity_traffic_light_module behavior_velocity_virtual_traffic_light_module behavior_velocity_walkway_module costmap_generator external_velocity_limit_selector freespace_planner freespace_planning_algorithms mission_planner motion_velocity_smoother objects_of_interest_marker_interface obstacle_avoidance_planner obstacle_cruise_planner obstacle_stop_planner obstacle_velocity_limiter path_smoother planning_debug_tools planning_test_utils planning_topic_converter planning_validator route_handler rtc_interface rtc_replayer bezier_sampler frenet_planner path_sampler sampler_common scenario_selector static_centerline_optimizer surround_obstacle_checker gnss_poser image_diagnostics image_transport_decompressor imu_corrector livox_tag_filter pointcloud_preprocessor radar_scan_to_pointcloud2 radar_static_pointcloud_filter radar_threshold_filter radar_tracks_noise_filter tier4_pcl_extensions vehicle_velocity_converter autoware_auto_msgs_adapter bluetooth_monitor component_state_monitor default_ad_api ad_api_adaptors ad_api_visualizers automatic_pose_initializer diagnostic_graph_aggregator dummy_diag_publisher dummy_infrastructure duplicated_node_checker emergency_handler mrm_comfortable_stop_operator mrm_emergency_stop_operator system_error_monitor system_monitor topic_state_monitor velodyne_monitor accel_brake_map_calibrator external_cmd_converter raw_vehicle_cmd_converter steer_offset_estimator vehicle_info_util launch loop_sim ssh_machine gpudirect-dds autoware_ad_api_specs autoware_adapi_v1_msgs autoware_adapi_version_msgs autoware_auto_common autoware_auto_geometry autoware_auto_control_msgs autoware_auto_geometry_msgs autoware_auto_mapping_msgs autoware_auto_msgs autoware_auto_perception_msgs autoware_auto_planning_msgs autoware_auto_system_msgs autoware_auto_vehicle_msgs autoware_auto_perception_rviz_plugin autoware_auto_tf2 autoware_cmake autoware_lint_common autoware_utils lanelet2_extension autoware_common_msgs autoware_control_msgs autoware_localization_msgs autoware_map_msgs autoware_perception_msgs autoware_planning_msgs autoware_sensing_msgs autoware_system_msgs autoware_vehicle_msgs autoware_point_types autoware_testing bag_time_manager_rviz_plugin component_interface_specs component_interface_tools component_interface_utils cuda_utils fake_test_node geography_utils global_parameter_loader glog_component goal_distance_calculator grid_map_utils interpolation kalman_filter motion_utils object_recognition_utils osqp_interface path_distance_calculator perception_utils polar_grid qp_interface rtc_manager_rviz_plugin signal_processing tensorrt_common tier4_adapi_rviz_plugin tier4_api_utils tier4_automatic_goal_rviz_plugin tier4_autoware_utils tier4_calibration_rviz_plugin tier4_camera_view_rviz_plugin tier4_control_rviz_plugin tier4_datetime_rviz_plugin tier4_debug_rviz_plugin tier4_debug_tools tier4_localization_rviz_plugin tier4_perception_rviz_plugin tier4_planning_rviz_plugin tier4_screen_capture_rviz_plugin tier4_simulated_clock_rviz_plugin tier4_state_rviz_plugin tier4_system_rviz_plugin tier4_target_object_type_rviz_plugin tier4_traffic_light_rviz_plugin tier4_vehicle_rviz_plugin time_utils simulator_compatibility_test traffic_light_recognition_marker_publisher traffic_light_utils tvm_utility dma_customer_msg dma_transfer eagleye_coordinate eagleye_navigation eagleye_msgs eagleye_rt eagleye_can_velocity_converter eagleye_fix2kml eagleye_geo_pose_converter eagleye_geo_pose_fusion eagleye_gnss_converter eagleye_tf llh_converter morai_msgs mussp ndt_omp orocos_kdl python_orocos_kdl pointcloud_to_laserscan rtklib_bridge rtklib_msgs autoware_external_api_msgs autoware_iv_external_api_adaptor autoware_iv_internal_api_adaptor awapi_awiv_adapter tier4_api_msgs tier4_auto_msgs_converter tier4_control_msgs tier4_debug_msgs tier4_external_api_msgs tier4_hmi_msgs tier4_localization_msgs tier4_map_msgs tier4_perception_msgs tier4_planning_msgs tier4_rtc_msgs tier4_simulation_msgs tier4_system_msgs tier4_v2x_msgs tier4_vehicle_msgs tier4_autoware_api_launch tier4_control_launch tier4_localization_launch tier4_map_launch tier4_perception_launch tier4_planning_launch tier4_sensing_launch tier4_simulator_launch tier4_system_launch tier4_vehicle_launch fastrtps cyclonedds lanelet2 lanelet2_core lanelet2_examples lanelet2_io lanelet2_maps lanelet2_matching lanelet2_projection lanelet2_python lanelet2_routing lanelet2_traffic_rules lanelet2_validation sophus angles behaviortree_cpp_v3 bond bond_core bondcpp bondpy smclib test_bond cudnn_cmake_module diagnostic_aggregator diagnostic_common_diagnostics diagnostic_updater diagnostics self_test filters geodesy geographic_info geographic_msgs grid_map grid_map_cmake_helpers grid_map_core grid_map_costmap_2d grid_map_cv grid_map_demos grid_map_filters grid_map_loader grid_map_msgs grid_map_octomap grid_map_pcl grid_map_ros grid_map_rviz_plugin grid_map_sdf grid_map_visualization mrt_cmake_modules nav2_amcl nav2_behavior_tree nav2_behaviors nav2_bringup nav2_bt_navigator nav2_collision_monitor nav2_common nav2_controller nav2_core nav2_costmap_2d costmap_queue dwb_core dwb_critics dwb_msgs dwb_plugins nav2_dwb_controller nav_2d_msgs nav_2d_utils nav2_lifecycle_manager nav2_map_server nav2_msgs nav2_navfn_planner nav2_planner nav2_regulated_pure_pursuit_controller nav2_rotation_shim_controller nav2_rviz_plugins nav2_simple_commander nav2_smac_planner nav2_smoother nav2_system_tests nav2_theta_star_planner nav2_util nav2_velocity_smoother nav2_voxel_grid nav2_waypoint_follower navigation2 dynamic_edt_3d octomap octovis octomap_msgs osqp_vendor pacmod3_msgs pcl_msgs pcl_conversions pcl_ros perception_pcl point_cloud_msg_wrapper radar_msgs can_msgs rqt_tf_tree tensorrt_cmake_module topic_tools topic_tools_interfaces tvm_vendor cv_bridge image_geometry opencv_tests vision_opencv xacro rviz2 rviz_assimp_vendor rviz_common rviz_default_plugins rviz_ogre_vendor rviz_rendering rviz_rendering_tests rviz_visual_testing_framework dummy_perception_publisher fault_injection simple_planning_simulator image_view v4l2_camera aion_interface carla_vehicle_can cgi430_can_driver cgi610_driver ARS408_driver common_sensor_launch kunyi_sensor_kit_description kunyi_sensor_kit_launch classformsg node_v2x data_format_dump data_preprocess_launch lidar_centerpoint_collect lidar_saver message_sync time_cal camera_calibration direct_visual_lidar_calibration multi_lidar_calibration

ROS Distro
github

Package Summary

Tags No category tags.
Version 0.1.0
License Apache License 2.0
Build type AMENT_CMAKE
Use RECOMMENDED

Repository Summary

Description
Checkout URI https://github.com/ieiauto/autodrrt.git
VCS Type git
VCS Version main
Last Updated 2025-05-30
Dev Status UNKNOWN
Released UNRELEASED
Tags No category tags.
Contributing Help Wanted (-)
Good First Issues (-)
Pull Requests to Review (-)

Package Description

The motion_velocity_smoother package

Additional Links

No additional links.

Maintainers

  • Fumiya Watanabe
  • Takamasa Horibe
  • Makoto Kurihara

Authors

  • Takamasa Horibe
  • Fumiya Watanabe
  • Yutaka Shimizu
  • Makoto Kurihara

Motion Velocity Smoother

Purpose

motion_velocity_smootherは目標軌道上の各点における望ましい車速を計画して出力するモジュールである。 このモジュールは、速度の最大化と乗り心地の良さを両立するために、事前に指定された制限速度、制限加速度および制限躍度の範囲で車速を計画する。 加速度や躍度の制限を与えることは車速の変化を滑らかにすることに対応するため、このモジュールをmotion_velocity_smootherと呼んでいる。

Inner-workings / Algorithms

Flow chart

motion_velocity_smoother_flow

Extract trajectory

自車後輪軸中心位置に最も近い参照経路上の点に対し、extract_behind_distだけ戻った点からextract_ahead_distだけ進んだ点までの参照経路を抜き出す。

Apply external velocity limit

モジュール外部から指定された速度制限を適用する。 ここで扱う外部の速度制限は/planning/scenario_planning/max_velocityの topic で渡されるもので、地図上で設定された速度制限など、参照経路にすでに設定されている制限速度とは別である。 外部から指定される速度制限は、パラメータで指定されている減速度および躍度の制限の範囲で減速可能な位置から速度制限を適用する。

Apply stop approaching velocity

停止点に近づいたときの速度を設定する。障害物近傍まで近づく場合や、正着精度向上などの目的に用いる。

Apply lateral acceleration limit

経路の曲率に応じて、指定された最大横加速度max_lateral_accelを超えない速度を制限速度として設定する。ただし、制限速度はmin_curve_velocityを下回らないように設定する。

Resample trajectory

指定された時間間隔で経路の点を再サンプルする。ただし、経路全体の長さはmin_trajectory_lengthからmax_trajectory_lengthの間となるように再サンプルを行い、点の間隔はmin_trajectory_interval_distanceより小さくならないようにする。 現在車速でresample_timeの間進む距離までは密にサンプリングし、それ以降は疎にサンプリングする。 この方法でサンプリングすることで、低速時は密に、高速時は疎にサンプルされるため、停止精度と計算負荷軽減の両立を図っている。

Calculate initial state

速度計画のための初期値を計算する。初期値は状況に応じて下表のように計算する。

状況 初期速度 初期加速度
最初の計算時 現在車速 0.0
発進時 engage_velocity engage_acceleration
現在車速と計画車速が乖離 現在車速 前回計画値
通常時 前回計画値 前回計画値

Smooth velocity

速度の計画を行う。速度計画のアルゴリズムはJerkFiltered, L2, Linfの 3 種類のうちからコンフィグで指定する。 最適化のソルバは OSQP[1]を利用する。

JerkFiltered

速度の 2 乗(最小化で表すため負値で表現)、制限速度逸脱量の 2 乗、制限加度逸脱量の 2 乗、制限ジャーク逸脱量の 2 乗、ジャークの 2 乗の総和を最小化する。

L2

速度の 2 乗(最小化で表すため負値で表現)、制限速度逸脱量の 2 乗、制限加度逸脱量の 2 乗、疑似ジャーク[2]の 2 乗の総和を最小化する。

Linf

速度の 2 乗(最小化で表すため負値で表現)、制限速度逸脱量の 2 乗、制限加度逸脱量の 2 乗の総和と疑似ジャーク[2]の絶対最大値の和の最小化する。

Post process

計画された軌道の後処理を行う。

  • 停止点より先の速度を 0 に設定
  • 速度がパラメータで与えられるmax_velocity以下となるように設定
  • 自車位置より手前の点における速度を設定
  • Trajectory の仕様に合わせてリサンプリング(post resampling)
  • デバッグデータの出力

最適化の計算が終わったあと、次のノードに経路を渡す前にpost resamplingと呼ばれるリサンプリングを行う。ここで再度リサンプリングを行っている理由としては、最適化前で必要な経路間隔と後段のモジュールに渡す経路間隔が必ずしも一致していないからであり、その差を埋めるために再度サンプリングを行っている。そのため、post resamplingでは後段モジュールの経路仕様を確認してパラメータを決める必要がある。なお、最適化アルゴリズムの計算負荷が高く、最初のリサンプリングで経路間隔が後段モジュールの経路仕様より疎になっている場合、post resamplingで経路を蜜にリサンプリングする。逆に最適化アルゴリズムの計算負荷が小さく、最初のリサンプリングで経路間隔が後段の経路仕様より蜜になっている場合は、post resamplingで経路をその仕様に合わせて疎にリサンプリングする。

Inputs / Outputs

Input

Name Type Description
~/input/trajectory autoware_auto_planning_msgs/Trajectory Reference trajectory
/planning/scenario_planning/max_velocity std_msgs/Float32 External velocity limit [m/s]
/localization/kinematic_state nav_msgs/Odometry Current odometry
/tf tf2_msgs/TFMessage TF
/tf_static tf2_msgs/TFMessage TF static

Output

Name Type Description
~/output/trajectory autoware_auto_planning_msgs/Trajectory Modified trajectory
/planning/scenario_planning/current_max_velocity std_msgs/Float32 Current external velocity limit [m/s]
~/closest_velocity std_msgs/Float32 Planned velocity closest to ego base_link (for debug)
~/closest_acceleration std_msgs/Float32 Planned acceleration closest to ego base_link (for debug)
~/closest_jerk std_msgs/Float32 Planned jerk closest to ego base_link (for debug)
~/debug/trajectory_raw autoware_auto_planning_msgs/Trajectory Extracted trajectory (for debug)

File truncated at 100 lines see the full file

CHANGELOG
No CHANGELOG found.

Launch files

  • launch/motion_velocity_smoother.launch.xml
      • common_param_path [default: $(find-pkg-share motion_velocity_smoother)/config/default_common.param.yaml]
      • nearest_search_param_path
      • input_trajectory [default: /planning/scenario_planning/scenario_selector/trajectory]
      • output_trajectory [default: /planning/scenario_planning/trajectory]
      • publish_debug_trajs [default: false]
      • velocity_smoother_type [default: JerkFiltered]
      • param_path [default: $(find-pkg-share motion_velocity_smoother)/config/default_motion_velocity_smoother.param.yaml]
      • velocity_smoother_param_path [default: $(find-pkg-share motion_velocity_smoother)/config/$(var velocity_smoother_type).param.yaml]

Messages

No message files found.

Services

No service files found

Plugins

No plugins found.

Recent questions tagged motion_velocity_smoother at Robotics Stack Exchange