From ece38099fe94d959e98ce582bc04be49c6c1f12b Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E5=BF=83=E5=88=9A?= <90366790+liuXinGangChina@users.noreply.github.com> Date: Thu, 27 Feb 2025 04:24:49 +0800 Subject: [PATCH] feat(autoware_utils): replace autoware_universe_utils with autoware_utils (#10191) Signed-off-by: liuXinGangChina Signed-off-by: Ryohsuke Mitsudome --- .../goal_distance_calculator.hpp | 4 +- .../goal_distance_calculator_node.hpp | 12 +- .../package.xml | 2 +- .../src/goal_distance_calculator.cpp | 2 +- .../src/goal_distance_calculator_node.cpp | 10 +- common/autoware_grid_map_utils/package.xml | 2 +- .../test/benchmark.cpp | 4 +- .../test/test_polygon_iterator.cpp | 2 +- .../object_recognition_utils/matching.hpp | 24 +- .../predicted_path_utils.hpp | 2 +- .../package.xml | 2 +- .../src/predicted_path_utils.cpp | 4 +- .../test/src/test_conversion.cpp | 2 +- .../test/src/test_matching.cpp | 8 +- .../test/src/test_predicted_path_utils.cpp | 40 +- .../Readme.md | 6 +- .../package.xml | 4 +- .../src/path_distance_calculator.cpp | 10 +- .../src/path_distance_calculator.hpp | 14 +- .../package.xml | 2 +- .../autonomous_emergency_braking/node.hpp | 28 +- .../autonomous_emergency_braking/utils.hpp | 6 +- .../package.xml | 2 +- .../src/node.cpp | 229 ++++++------ .../src/utils.cpp | 16 +- .../test/test.cpp | 8 +- .../test/test.hpp | 2 +- .../autoware/collision_detector/node.hpp | 13 +- .../autoware_collision_detector/package.xml | 1 - .../autoware_collision_detector/src/node.cpp | 18 +- .../control_performance_analysis_node.hpp | 2 +- .../package.xml | 2 +- .../src/control_performance_analysis_core.cpp | 17 +- .../control_validator/control_validator.hpp | 10 +- .../autoware_control_validator/package.xml | 2 +- .../src/control_validator.cpp | 8 +- .../src/debug_marker.cpp | 10 +- .../autoware_control_validator/src/utils.cpp | 4 +- .../joy_controller/joy_controller.hpp | 7 +- control/autoware_joy_controller/package.xml | 2 +- .../src/joy_controller_node.cpp | 4 +- .../lane_departure_checker.hpp | 24 +- .../lane_departure_checker_node.hpp | 29 +- .../lane_departure_checker/parameters.hpp | 8 +- .../autoware/lane_departure_checker/utils.hpp | 10 +- .../package.xml | 2 +- .../lane_departure_checker.cpp | 65 ++-- .../lane_departure_checker_node.cpp | 64 ++-- .../parameters.cpp | 48 +-- .../src/lane_departure_checker_node/utils.cpp | 20 +- .../test/test_calc_trajectory_deviation.cpp | 2 +- .../test/test_create_vehicle_footprints.cpp | 2 +- .../test_create_vehicle_passing_areas.cpp | 6 +- .../mpc_lateral_controller/mpc_trajectory.hpp | 4 +- .../package.xml | 2 +- .../src/mpc.cpp | 12 +- .../src/mpc_lateral_controller.cpp | 4 +- .../src/mpc_utils.cpp | 35 +- .../obstacle_collision_checker.hpp | 4 +- .../obstacle_collision_checker_node.hpp | 18 +- .../package.xml | 2 +- .../src/debug.cpp | 28 +- .../src/obstacle_collision_checker.cpp | 26 +- .../src/obstacle_collision_checker_node.cpp | 35 +- .../test/test_obstacle_collision_checker.cpp | 8 +- .../package.xml | 2 +- .../src/node.cpp | 4 +- .../src/node.hpp | 10 +- .../src/state.cpp | 12 +- .../pid_longitudinal_controller.hpp | 2 +- .../package.xml | 2 +- .../src/longitudinal_controller_utils.cpp | 10 +- .../src/pid_longitudinal_controller.cpp | 4 +- .../collision_checker.hpp | 10 +- .../predicted_path_checker/debug_marker.hpp | 6 +- .../predicted_path_checker_node.hpp | 10 +- .../autoware/predicted_path_checker/utils.hpp | 6 +- .../package.xml | 2 +- .../collision_checker.cpp | 10 +- .../debug_marker.cpp | 100 ++--- .../predicted_path_checker_node.cpp | 30 +- .../src/predicted_path_checker_node/utils.cpp | 64 ++-- .../autoware_pure_pursuit_node.hpp | 12 +- control/autoware_pure_pursuit/package.xml | 2 +- ...toware_pure_pursuit_lateral_controller.cpp | 12 +- .../autoware_pure_pursuit_node.cpp | 4 +- .../shift_decider/autoware_shift_decider.hpp | 8 +- control/autoware_shift_decider/package.xml | 2 +- .../src/autoware_shift_decider.cpp | 6 +- .../package.xml | 2 +- .../package.xml | 2 +- .../controller_node.hpp | 26 +- .../simple_trajectory_follower.hpp | 6 +- .../package.xml | 2 +- .../src/controller_node.cpp | 15 +- .../src/simple_trajectory_follower.cpp | 12 +- control/autoware_vehicle_cmd_gate/package.xml | 2 +- .../src/vehicle_cmd_gate.cpp | 85 +++-- .../src/vehicle_cmd_gate.hpp | 34 +- .../control_evaluator_node.hpp | 34 +- .../metrics/metrics_utils.hpp | 6 +- .../autoware_control_evaluator/package.xml | 2 +- .../src/control_evaluator_node.cpp | 22 +- .../src/metrics/deviation_metrics.cpp | 16 +- .../src/metrics/metrics_utils.cpp | 5 +- .../kinematic_evaluator_node.hpp | 4 +- .../metrics/kinematic_metrics.hpp | 4 +- .../metrics_calculator.hpp | 4 +- .../autoware_kinematic_evaluator/package.xml | 2 +- .../src/metrics/kinematic_metrics.cpp | 6 +- .../localization_evaluator_node.hpp | 4 +- .../metrics/localization_metrics.hpp | 4 +- .../metrics_calculator.hpp | 4 +- .../package.xml | 2 +- .../metrics/metric.hpp | 4 +- .../perception_online_evaluator_node.hpp | 4 +- .../utils/marker_utils.hpp | 4 +- .../package.xml | 2 +- .../src/metrics/detection_count.cpp | 8 +- .../src/metrics/deviation_metrics.cpp | 8 +- .../src/metrics_calculator.cpp | 49 ++- .../src/perception_online_evaluator_node.cpp | 101 ++--- .../src/utils/marker_utils.cpp | 70 ++-- .../test_perception_online_evaluator_node.cpp | 10 +- .../autoware_planning_evaluator/README.md | 2 +- .../metrics/deviation_metrics.hpp | 4 +- .../metrics/obstacle_metrics.hpp | 4 +- .../metrics/stability_metrics.hpp | 4 +- .../metrics/trajectory_metrics.hpp | 4 +- .../planning_evaluator/metrics_calculator.hpp | 4 +- .../motion_evaluator_node.hpp | 4 +- .../planning_evaluator_node.hpp | 30 +- .../autoware_planning_evaluator/package.xml | 2 +- .../src/metrics/deviation_metrics.cpp | 16 +- .../src/metrics/metrics_utils.cpp | 14 +- .../src/metrics/obstacle_metrics.cpp | 10 +- .../src/metrics/stability_metrics.cpp | 19 +- .../src/metrics/trajectory_metrics.cpp | 20 +- .../src/metrics_calculator.cpp | 2 +- .../src/planning_evaluator_node.cpp | 18 +- .../autoware_gyro_odometer/package.xml | 2 +- .../src/gyro_odometer_core.cpp | 14 +- .../src/gyro_odometer_core.hpp | 16 +- .../src/ar_tag_based_localizer.cpp | 7 +- .../autoware_landmark_manager/package.xml | 2 +- .../src/landmark_manager.cpp | 6 +- .../package.xml | 2 +- .../src/lidar_marker_localizer.cpp | 17 +- .../src/lidar_marker_localizer.hpp | 4 +- .../package.xml | 2 +- .../src/localization_error_monitor.cpp | 4 +- .../src/localization_error_monitor.hpp | 8 +- .../ndt_scan_matcher/map_update_module.hpp | 8 +- .../ndt_scan_matcher_core.hpp | 6 +- .../autoware_ndt_scan_matcher/package.xml | 2 +- .../src/ndt_scan_matcher_core.cpp | 22 +- .../src/switch_rule/pcd_map_based_rule.cpp | 6 +- .../package.xml | 2 +- .../src/pose_estimator_arbiter.hpp | 4 +- .../src/pose_estimator_arbiter_core.cpp | 2 +- .../autoware_pose_initializer/package.xml | 2 +- .../src/pose_initializer_core.cpp | 6 +- .../src/pose_initializer_core.hpp | 8 +- .../package.xml | 2 +- .../src/pose_instability_detector.cpp | 4 +- localization/yabloc/yabloc_common/package.xml | 2 +- .../ll2_decomposer/ll2_decomposer_core.cpp | 6 +- .../yabloc_image_processing/package.xml | 2 +- .../src/graph_segment/graph_segment_core.cpp | 4 +- .../line_segment_detector_core.cpp | 4 +- .../src/undistort/undistort_node.cpp | 6 +- .../yabloc/yabloc_particle_filter/package.xml | 2 +- .../camera_particle_corrector_core.cpp | 8 +- .../ll2_cost_map/hierarchical_cost_map.cpp | 4 +- .../autoware_cluster_merger/package.xml | 2 +- .../src/cluster_merger_node.hpp | 2 +- .../CMakeLists.txt | 2 +- .../package.xml | 2 +- .../node.cpp | 8 +- .../node.cpp | 8 +- .../voxel_based_compare_map_filter/node.cpp | 8 +- .../node.cpp | 8 +- .../node.hpp | 8 +- .../package.xml | 2 +- .../package.xml | 2 +- .../detected_object_feature_remover_node.cpp | 3 +- .../detected_object_feature_remover_node.hpp | 4 +- .../package.xml | 2 +- .../src/lanelet_filter/lanelet_filter.cpp | 22 +- .../src/lanelet_filter/lanelet_filter.hpp | 20 +- .../obstacle_pointcloud_validator.cpp | 19 +- .../obstacle_pointcloud_validator.hpp | 8 +- .../occupancy_grid_map_validator.cpp | 11 +- .../occupancy_grid_map_validator.hpp | 4 +- .../src/position_filter/position_filter.cpp | 3 +- .../src/position_filter/position_filter.hpp | 6 +- .../autoware_detection_by_tracker/package.xml | 2 +- .../src/debugger/debugger.hpp | 13 +- .../src/detection_by_tracker_node.cpp | 13 +- .../src/detection_by_tracker_node.hpp | 4 +- .../src/tracker/tracker_handler.cpp | 8 +- .../autoware_elevation_map_loader/package.xml | 2 +- .../src/elevation_map_loader_node.cpp | 2 +- .../src/elevation_map_loader_node.hpp | 2 +- .../autoware_euclidean_cluster/package.xml | 2 +- .../src/euclidean_cluster_node.cpp | 6 +- .../src/euclidean_cluster_node.hpp | 8 +- ...oxel_grid_based_euclidean_cluster_node.cpp | 7 +- ...oxel_grid_based_euclidean_cluster_node.hpp | 8 +- .../autoware_ground_segmentation/package.xml | 2 +- .../src/ransac_ground_filter/node.cpp | 12 +- .../src/ransac_ground_filter/node.hpp | 10 +- .../src/ray_ground_filter/node.cpp | 8 +- .../src/ray_ground_filter/node.hpp | 6 +- .../src/scan_ground_filter/grid.hpp | 12 +- .../scan_ground_filter/grid_ground_filter.hpp | 6 +- .../src/scan_ground_filter/node.cpp | 32 +- .../src/scan_ground_filter/node.hpp | 11 +- .../fusion_node.hpp | 16 +- .../pointpainting_fusion/node.hpp | 4 +- .../roi_detected_object_fusion/node.hpp | 2 +- .../utils/utils.hpp | 2 +- .../package.xml | 2 +- .../src/fusion_node.cpp | 16 +- .../src/pointpainting_fusion/node.cpp | 10 +- .../pointpainting_trt.cpp | 2 +- .../src/roi_cluster_fusion/node.cpp | 4 +- .../src/roi_detected_object_fusion/node.cpp | 4 +- .../src/roi_pointcloud_fusion/node.cpp | 4 +- .../segmentation_pointcloud_fusion/node.cpp | 4 +- .../src/utils/utils.cpp | 2 +- .../CMakeLists.txt | 4 +- .../detector.hpp | 2 +- .../node.hpp | 8 +- .../package.xml | 2 +- .../src/detector.cpp | 5 +- .../src/node.cpp | 8 +- .../autoware_lidar_centerpoint/README.md | 10 +- .../autoware/lidar_centerpoint/node.hpp | 17 +- .../lib/centerpoint_trt.cpp | 4 +- .../postprocess/non_maximum_suppression.cpp | 4 +- .../lib/ros_utils.cpp | 16 +- .../autoware_lidar_centerpoint/package.xml | 2 +- .../autoware_lidar_centerpoint/src/node.cpp | 15 +- .../autoware_lidar_transfusion/README.md | 18 +- .../lidar_transfusion_node.hpp | 13 +- .../lidar_transfusion/transfusion_trt.hpp | 5 +- .../postprocess/non_maximum_suppression.cpp | 4 +- .../lib/ros_utils.cpp | 10 +- .../lib/transfusion_trt.cpp | 5 +- .../autoware_lidar_transfusion/package.xml | 2 +- .../src/lidar_transfusion_node.cpp | 15 +- .../map_based_prediction/data_structure.hpp | 4 +- .../map_based_prediction_node.hpp | 46 +-- .../map_based_prediction/path_generator.hpp | 6 +- .../map_based_prediction/predictor_vru.hpp | 6 +- .../include/map_based_prediction/utils.hpp | 4 +- .../autoware_map_based_prediction/package.xml | 2 +- .../src/debug.cpp | 6 +- .../src/map_based_prediction_node.cpp | 72 ++-- .../src/path_generator.cpp | 33 +- .../src/predictor_vru.cpp | 37 +- .../src/utils.cpp | 18 +- .../lib/association/association.cpp | 9 +- .../lib/object_model/shapes.cpp | 27 +- .../lib/tracker/model/bicycle_tracker.cpp | 12 +- .../tracker/model/pass_through_tracker.cpp | 4 +- .../lib/tracker/model/pedestrian_tracker.cpp | 12 +- .../lib/tracker/model/unknown_tracker.cpp | 20 +- .../lib/tracker/model/vehicle_tracker.cpp | 14 +- .../motion_model/bicycle_motion_model.cpp | 10 +- .../motion_model/ctrv_motion_model.cpp | 10 +- .../tracker/motion_model/cv_motion_model.cpp | 8 +- .../lib/uncertainty/uncertainty_processor.cpp | 2 +- .../autoware_multi_object_tracker/package.xml | 2 +- .../src/debugger/debug_object.hpp | 2 +- .../src/debugger/debugger.cpp | 2 +- .../src/debugger/debugger.hpp | 6 +- .../src/multi_object_tracker_node.cpp | 3 +- .../src/multi_object_tracker_node.hpp | 2 +- .../object_association_merger_node.hpp | 12 +- perception/autoware_object_merger/package.xml | 2 +- .../src/association/data_association.cpp | 8 +- .../src/object_association_merger_node.cpp | 12 +- .../package.xml | 2 +- .../src/object_velocity_splitter_node.cpp | 6 +- .../package.xml | 2 +- ...occupancy_grid_map_outlier_filter_node.cpp | 20 +- ...occupancy_grid_map_outlier_filter_node.hpp | 14 +- .../costmap_2d/occupancy_grid_map_base.hpp | 8 +- .../costmap_2d/occupancy_grid_map_fixed.cpp | 2 +- .../occupancy_grid_map_projective.cpp | 2 +- .../lib/utils/utils.cpp | 8 +- .../package.xml | 2 +- .../synchronized_grid_map_fusion_node.cpp | 12 +- .../synchronized_grid_map_fusion_node.hpp | 14 +- ...aserscan_based_occupancy_grid_map_node.cpp | 8 +- ...aserscan_based_occupancy_grid_map_node.hpp | 6 +- ...intcloud_based_occupancy_grid_map_node.cpp | 16 +- ...intcloud_based_occupancy_grid_map_node.hpp | 14 +- .../package.xml | 2 +- ...dar_crossing_objects_noise_filter_node.cpp | 10 +- ...radar_crossing_objects_filter_is_noise.cpp | 28 +- .../package.xml | 2 +- .../radar_fusion_to_detected_object.hpp | 6 +- .../src/radar_fusion_to_detected_object.cpp | 28 +- .../package.xml | 2 +- .../src/radar_object_clustering_node.cpp | 8 +- .../utils/radar_object_tracker_utils.hpp | 4 +- .../autoware_radar_object_tracker/package.xml | 2 +- .../src/association/data_association.cpp | 9 +- .../constant_turn_rate_motion_tracker.cpp | 14 +- .../tracker/model/linear_motion_tracker.cpp | 14 +- .../src/utils/radar_object_tracker_utils.cpp | 8 +- .../package.xml | 2 +- .../src/radar_tracks_msgs_converter_node.cpp | 28 +- .../src/radar_tracks_msgs_converter_node.hpp | 4 +- .../package.xml | 2 +- .../src/low_intensity_cluster_filter_node.cpp | 10 +- .../src/low_intensity_cluster_filter_node.hpp | 9 +- .../lib/corrector/utils.cpp | 6 +- .../autoware_shape_estimation/package.xml | 2 +- .../src/shape_estimation_node.cpp | 12 +- .../src/shape_estimation_node.hpp | 12 +- .../autoware_simple_object_merger/package.xml | 2 +- .../src/simple_object_merger_node.cpp | 4 +- .../src/simple_object_merger_node.hpp | 4 +- .../tensorrt_yolox/tensorrt_yolox_node.hpp | 8 +- .../src/tensorrt_yolox_node.cpp | 6 +- .../decorative_tracker_merger_node.hpp | 12 +- .../tracking_object_merger/utils/utils.hpp | 2 +- .../package.xml | 2 +- .../src/association/data_association.cpp | 8 +- .../src/decorative_tracker_merger_node.cpp | 10 +- .../src/utils/utils.cpp | 2 +- .../package.xml | 2 +- .../traffic_light_category_merger_node.hpp | 2 +- .../package.xml | 2 +- .../traffic_light_map_based_detector_node.cpp | 14 +- .../package.xml | 2 +- .../src/node.cpp | 2 +- .../src/occlusion_predictor.cpp | 2 +- .../src/occlusion_predictor.hpp | 4 +- .../package.xml | 2 +- .../src/traffic_light_selector_node.cpp | 12 +- .../src/traffic_light_selector_node.hpp | 10 +- .../costmap_generator/costmap_generator.hpp | 23 +- .../autoware_costmap_generator/package.xml | 2 +- .../src/costmap_generator.cpp | 20 +- .../test/test_object_map_utils.cpp | 2 +- .../test/test_objects_to_costmap.cpp | 18 +- .../freespace_planner_node.hpp | 14 +- .../autoware_freespace_planner/package.xml | 2 +- .../freespace_planner_node.cpp | 16 +- .../src/autoware_freespace_planner/utils.cpp | 8 +- .../test/test_freespace_planner_utils.cpp | 4 +- .../abstract_algorithm.hpp | 8 +- .../kinematic_bicycle_model.hpp | 4 +- .../package.xml | 2 +- .../src/abstract_algorithm.cpp | 12 +- .../src/astar_search.cpp | 21 +- .../package.xml | 2 +- .../src/lanelet2_plugins/default_planner.cpp | 58 ++- .../src/lanelet2_plugins/default_planner.hpp | 6 +- .../lanelet2_plugins/utility_functions.cpp | 7 +- .../lanelet2_plugins/utility_functions.hpp | 5 +- .../src/mission_planner/arrival_checker.cpp | 12 +- .../src/mission_planner/mission_planner.cpp | 8 +- .../src/mission_planner/mission_planner.hpp | 15 +- .../src/mission_planner/route_selector.cpp | 2 +- .../src/mission_planner/route_selector.hpp | 5 +- .../src/mission_planner/service_utils.hpp | 4 +- .../test_lanelet2_plugins_default_planner.cpp | 26 +- .../test/test_utility_functions.cpp | 10 +- .../common_structs.hpp | 44 +-- .../autoware/obstacle_cruise_planner/node.hpp | 23 +- .../planner_interface.hpp | 30 +- .../obstacle_cruise_planner/polygon_utils.hpp | 6 +- .../obstacle_cruise_planner/type_alias.hpp | 6 +- .../obstacle_cruise_planner/utils.hpp | 8 +- .../package.xml | 2 +- .../src/node.cpp | 197 +++++----- .../optimization_based_planner.cpp | 8 +- .../pid_based_planner/pid_based_planner.cpp | 53 ++- .../src/planner_interface.cpp | 31 +- .../src/polygon_utils.cpp | 16 +- .../src/utils.cpp | 8 +- .../package.xml | 2 +- .../src/adaptive_cruise_control.cpp | 7 +- .../src/debug_marker.cpp | 146 ++++---- .../src/debug_marker.hpp | 6 +- .../src/node.cpp | 43 ++- .../src/node.hpp | 20 +- .../src/planner_utils.cpp | 79 ++-- .../src/planner_utils.hpp | 6 +- .../path_optimizer/common_structs.hpp | 14 +- .../autoware/path_optimizer/debug_marker.hpp | 4 +- .../autoware/path_optimizer/mpt_optimizer.hpp | 12 +- .../include/autoware/path_optimizer/node.hpp | 23 +- .../state_equation_generator.hpp | 6 +- planning/autoware_path_optimizer/package.xml | 2 +- .../src/debug_marker.cpp | 120 +++--- .../src/mpt_optimizer.cpp | 141 ++++--- planning/autoware_path_optimizer/src/node.cpp | 51 ++- .../src/replan_checker.cpp | 18 +- .../src/state_equation_generator.cpp | 2 +- .../src/utils/geometry_utils.cpp | 16 +- .../autoware/path_smoother/common_structs.hpp | 18 +- .../path_smoother/elastic_band_smoother.hpp | 13 +- .../path_smoother/utils/geometry_utils.hpp | 6 +- .../path_smoother/utils/trajectory_utils.hpp | 12 +- planning/autoware_path_smoother/package.xml | 2 +- .../src/elastic_band.cpp | 31 +- .../src/elastic_band_smoother.cpp | 9 +- .../src/replan_checker.cpp | 24 +- .../src/utils/trajectory_utils.cpp | 2 +- .../package.xml | 2 +- .../package.xml | 2 +- .../src/path_to_trajectory.cpp | 4 +- .../planning_validator/planning_validator.hpp | 16 +- .../autoware_planning_validator/package.xml | 2 +- .../src/debug_marker.cpp | 22 +- .../src/planning_validator.cpp | 15 +- .../autoware_planning_validator/src/utils.cpp | 31 +- .../src/test_planning_validator_helper.cpp | 10 +- .../package.xml | 2 +- ...emaining_distance_time_calculator_node.cpp | 2 +- planning/autoware_rtc_interface/README.md | 2 +- planning/autoware_rtc_interface/package.xml | 2 +- .../test/test_rtc_interface.cpp | 4 +- .../autoware/scenario_selector/node.hpp | 18 +- .../autoware_scenario_selector/package.xml | 2 +- .../autoware_scenario_selector/src/node.cpp | 11 +- .../package.xml | 2 +- .../src/debug_marker.cpp | 20 +- .../src/node.cpp | 39 +- .../src/node.hpp | 14 +- .../autoware/velocity_smoother/node.hpp | 41 +-- .../analytical_jerk_constrained_smoother.hpp | 6 +- .../smoother/jerk_filtered_smoother.hpp | 6 +- .../smoother/l2_pseudo_jerk_smoother.hpp | 6 +- .../smoother/linf_pseudo_jerk_smoother.hpp | 6 +- .../smoother/smoother_base.hpp | 6 +- .../autoware_velocity_smoother/package.xml | 2 +- .../autoware_velocity_smoother/src/node.cpp | 47 ++- .../src/resample.cpp | 6 +- .../analytical_jerk_constrained_smoother.cpp | 15 +- .../velocity_planning_utils.cpp | 6 +- .../src/smoother/jerk_filtered_smoother.cpp | 20 +- .../src/smoother/l2_pseudo_jerk_smoother.cpp | 2 +- .../smoother/linf_pseudo_jerk_smoother.cpp | 2 +- .../src/smoother/smoother_base.cpp | 15 +- .../src/trajectory_utils.cpp | 28 +- .../package.xml | 2 +- .../src/manager.cpp | 89 ++--- .../src/scene.cpp | 20 +- .../scene.hpp | 16 +- .../package.xml | 2 +- .../src/manager.cpp | 106 +++--- .../src/scene.cpp | 153 ++++---- .../package.xml | 2 +- .../examples/plot_map_case1.cpp | 19 +- .../examples/plot_map_case2.cpp | 19 +- .../decision_state.hpp | 4 +- .../fixed_goal_planner_base.hpp | 2 +- .../goal_planner_module.hpp | 4 +- .../goal_searcher.hpp | 4 +- .../pull_over_planner_base.hpp | 2 +- .../thread_data.hpp | 13 +- .../util.hpp | 6 +- .../package.xml | 2 +- .../src/decision_state.cpp | 4 +- .../src/default_fixed_goal_planner.cpp | 2 +- .../src/goal_planner_module.cpp | 132 ++++--- .../src/goal_searcher.cpp | 41 ++- .../src/manager.cpp | 251 ++++++------- .../pull_over_planner/bezier_pull_over.cpp | 6 +- .../pull_over_planner/freespace_pull_over.cpp | 4 +- .../src/pull_over_planner/shift_pull_over.cpp | 4 +- .../src/util.cpp | 110 +++--- .../test/test_util.cpp | 4 +- .../base_class.hpp | 12 +- .../interface.hpp | 4 +- .../structs/data.hpp | 4 +- .../structs/parameters.hpp | 4 +- .../utils/utils.hpp | 6 +- .../package.xml | 2 +- .../src/interface.cpp | 18 +- .../src/manager.cpp | 345 +++++++++--------- .../src/scene.cpp | 42 +-- .../src/utils/calculation.cpp | 14 +- .../src/utils/markers.cpp | 38 +- .../src/utils/path.cpp | 37 +- .../src/utils/utils.cpp | 37 +- .../test/test_lane_change_utils.cpp | 18 +- .../behavior_path_planner_node.hpp | 47 ++- .../behavior_path_planner/planner_manager.hpp | 10 +- .../package.xml | 2 +- .../src/behavior_path_planner_node.cpp | 117 +++--- .../src/planner_manager.cpp | 10 +- .../data_manager.hpp | 2 +- .../interface/scene_module_interface.hpp | 26 +- .../scene_module_manager_interface.hpp | 20 +- .../marker_utils/colors.hpp | 26 +- .../marker_utils/utils.hpp | 2 +- .../turn_signal_decider.hpp | 12 +- .../drivable_area_expansion/footprints.hpp | 2 +- .../utils/drivable_area_expansion/types.hpp | 16 +- ...ccupancy_grid_based_collision_detector.hpp | 2 +- .../path_safety_checker/objects_filtering.hpp | 2 +- .../path_safety_checker_parameters.hpp | 8 +- .../path_safety_checker/safety_check.hpp | 12 +- .../utils/path_shifter/path_shifter.hpp | 6 +- .../utils/utils.hpp | 10 +- .../package.xml | 2 +- .../scene_module_manager_interface.cpp | 18 +- .../src/marker_utils/utils.cpp | 143 ++++---- .../src/turn_signal_decider.cpp | 23 +- .../drivable_area_expansion.cpp | 29 +- .../drivable_area_expansion/footprints.cpp | 7 +- .../static_drivable_area.cpp | 123 +++---- ...ccupancy_grid_based_collision_detector.cpp | 16 +- .../geometric_parallel_parking.cpp | 66 ++-- .../path_safety_checker/objects_filtering.cpp | 18 +- .../path_safety_checker/safety_check.cpp | 108 +++--- .../src/utils/path_utils.cpp | 22 +- .../src/utils/traffic_light_utils.cpp | 3 +- .../src/utils/utils.cpp | 95 +++-- .../test/test_footprints.cpp | 4 +- .../test/test_objects_filtering.cpp | 42 +-- .../test/test_parking_departure_utils.cpp | 2 +- .../test/test_path_shifter.cpp | 6 +- .../test/test_safety_check.cpp | 55 ++- .../test/test_traffic_light_utils.cpp | 4 +- .../test/test_turn_signal.cpp | 102 +++--- .../test/test_utils.cpp | 10 +- .../sampling_planner_module.hpp | 16 +- .../sampling_planner_parameters.hpp | 10 +- .../util.hpp | 6 +- .../package.xml | 2 +- .../src/manager.cpp | 40 +- .../src/sampling_planner_module.cpp | 14 +- .../package.xml | 2 +- .../src/manager.cpp | 6 +- .../src/scene.cpp | 15 +- .../freespace_pull_out.hpp | 2 +- .../geometric_pull_out.hpp | 6 +- .../pull_out_planner_base.hpp | 10 +- .../shift_pull_out.hpp | 6 +- .../package.xml | 2 +- .../src/data_structs.cpp | 261 ++++++------- .../src/freespace_pull_out.cpp | 2 +- .../src/geometric_pull_out.cpp | 8 +- .../src/manager.cpp | 260 ++++++------- .../src/pull_out_planner_base.cpp | 2 +- .../src/shift_pull_out.cpp | 16 +- .../src/start_planner_module.cpp | 102 +++--- .../src/util.cpp | 4 +- .../helper.hpp | 2 +- .../parameter_helper.hpp | 253 ++++++------- .../scene.hpp | 6 +- .../type_alias.hpp | 42 +-- .../package.xml | 2 +- .../src/debug.cpp | 178 ++++----- .../src/manager.cpp | 154 ++++---- .../src/scene.cpp | 94 ++--- .../src/shift_line_generator.cpp | 32 +- .../src/utils.cpp | 68 ++-- .../test/test_utils.cpp | 252 ++++++------- .../scene.hpp | 2 +- .../util.hpp | 4 +- .../package.xml | 2 +- .../src/debug.cpp | 20 +- .../src/manager.cpp | 2 +- .../src/parameter.cpp | 25 +- .../src/scene.cpp | 4 +- .../src/util.cpp | 6 +- .../package.xml | 2 +- .../src/debug.cpp | 106 +++--- .../src/manager.cpp | 125 ++++--- .../src/occluded_crosswalk.cpp | 4 +- .../src/scene_crosswalk.cpp | 65 ++-- .../src/scene_crosswalk.hpp | 13 +- .../src/util.cpp | 26 +- .../package.xml | 2 +- .../src/debug.cpp | 40 +- .../src/manager.cpp | 22 +- .../src/scene.cpp | 2 +- .../src/scene.hpp | 2 +- .../src/utils.cpp | 6 +- .../src/utils.hpp | 4 +- .../src/debug.cpp | 80 ++-- .../src/intersection_lanelets.cpp | 2 +- .../src/intersection_lanelets.hpp | 4 +- .../src/manager.cpp | 192 +++++----- .../src/object_manager.cpp | 24 +- .../src/scene_intersection.cpp | 10 +- .../src/scene_intersection.hpp | 2 +- .../src/scene_intersection_collision.cpp | 49 ++- .../src/scene_intersection_occlusion.cpp | 22 +- .../src/scene_intersection_prepare_data.cpp | 13 +- .../src/scene_intersection_stuck.cpp | 4 +- .../src/scene_merge_from_private_road.cpp | 10 +- .../src/scene_merge_from_private_road.hpp | 2 +- .../src/util.cpp | 18 +- .../src/util.hpp | 8 +- .../package.xml | 2 +- .../src/debug.cpp | 30 +- .../src/manager.cpp | 8 +- .../src/scene.cpp | 12 +- .../src/scene.hpp | 2 +- .../src/util.cpp | 16 +- .../package.xml | 2 +- .../src/debug.cpp | 40 +- .../src/manager.cpp | 20 +- .../src/scene_no_stopping_area.cpp | 8 +- .../src/scene_no_stopping_area.hpp | 2 +- .../src/utils.cpp | 10 +- .../src/utils.hpp | 8 +- .../test/test_utils.cpp | 12 +- .../package.xml | 2 +- .../src/debug.cpp | 66 ++-- .../src/grid_utils.cpp | 20 +- .../src/grid_utils.hpp | 10 +- .../src/manager.cpp | 67 ++-- .../src/occlusion_spot_utils.cpp | 21 +- .../src/scene_occlusion_spot.cpp | 2 +- .../src/scene_occlusion_spot.hpp | 6 +- .../test/src/test_grid_utils.cpp | 4 +- .../behavior_velocity_planner/node.hpp | 39 +- .../package.xml | 2 +- .../src/node.cpp | 23 +- .../scene_module_interface.hpp | 28 +- .../utilization/arc_lane_util.hpp | 27 +- .../utilization/boost_geometry_helper.hpp | 8 +- .../utilization/util.hpp | 8 +- .../package.xml | 2 +- .../src/scene_module_interface.cpp | 4 +- .../src/utilization/arc_lane_util.cpp | 2 +- .../src/utilization/debug.cpp | 38 +- .../src/utilization/path_utilization.cpp | 4 +- .../src/utilization/util.cpp | 32 +- .../test/src/test_trajectory_utils.cpp | 2 +- .../scene_module_interface_with_rtc.hpp | 14 +- .../package.xml | 2 +- .../src/scene_module_interface_with_rtc.cpp | 8 +- .../package.xml | 2 +- .../src/debug.cpp | 80 ++-- .../src/dynamic_obstacle.cpp | 55 ++- .../src/manager.cpp | 96 ++--- .../src/scene.cpp | 74 ++-- .../src/scene.hpp | 2 +- .../src/utils.cpp | 27 +- .../src/utils.hpp | 10 +- .../test/test_dynamic_obstacle.cpp | 25 +- .../test/test_path_utils.cpp | 20 +- .../test/test_state_machine.cpp | 2 +- .../test/test_utils.cpp | 10 +- .../package.xml | 2 +- .../src/debug.cpp | 52 +-- .../src/manager.cpp | 19 +- .../src/scene.cpp | 8 +- .../src/scene.hpp | 2 +- .../src/util.cpp | 16 +- .../src/debug.cpp | 4 +- .../src/manager.cpp | 10 +- .../src/scene.cpp | 2 +- .../src/scene.hpp | 2 +- .../test/test_scene.cpp | 2 +- .../package.xml | 2 +- .../src/manager.cpp | 6 +- .../src/scene.cpp | 3 +- .../src/scene.hpp | 2 +- .../package.xml | 2 +- .../src/debug.cpp | 8 +- .../src/manager.cpp | 20 +- .../src/scene.cpp | 2 +- .../src/scene.hpp | 2 +- .../test/test_utils.cpp | 14 +- .../package.xml | 2 +- .../src/debug.cpp | 50 +-- .../src/manager.cpp | 31 +- .../src/manager.hpp | 4 +- .../src/scene.cpp | 2 +- .../src/scene.hpp | 14 +- .../src/utils.cpp | 22 +- .../src/utils.hpp | 21 +- .../test/test_utils.cpp | 18 +- .../package.xml | 2 +- .../src/debug.cpp | 34 +- .../src/manager.cpp | 8 +- .../src/scene_walkway.cpp | 8 +- .../src/scene_walkway.hpp | 2 +- .../package.xml | 2 +- .../src/collision.cpp | 16 +- .../src/collision.hpp | 4 +- .../src/debug.cpp | 13 +- .../src/debug.hpp | 5 +- .../src/dynamic_obstacle_stop_module.cpp | 53 +-- .../src/footprint.cpp | 24 +- .../src/footprint.hpp | 11 +- .../src/object_filtering.cpp | 12 +- .../src/types.hpp | 10 +- .../test/test_object_filtering.cpp | 18 +- .../package.xml | 2 +- .../src/obstacle_cruise_module.cpp | 48 +-- .../src/obstacle_cruise_module.hpp | 11 +- .../optimization_based_planner.cpp | 6 +- .../src/parameters.hpp | 68 ++-- .../pid_based_planner/pid_based_planner.cpp | 53 ++- .../src/type_alias.hpp | 4 +- .../package.xml | 2 +- .../src/obstacle_slow_down_module.cpp | 73 ++-- .../src/obstacle_slow_down_module.hpp | 11 +- .../src/parameters.hpp | 75 ++-- .../src/type_alias.hpp | 4 +- .../package.xml | 2 +- .../src/obstacle_stop_module.cpp | 104 +++--- .../src/obstacle_stop_module.hpp | 11 +- .../src/parameters.hpp | 92 ++--- .../src/type_alias.hpp | 4 +- .../package.xml | 2 +- .../src/obstacle_velocity_limiter.cpp | 2 +- .../src/obstacle_velocity_limiter_module.cpp | 10 +- .../src/obstacle_velocity_limiter_module.hpp | 6 +- .../src/obstacles.hpp | 2 +- .../src/pointcloud_utils.hpp | 2 +- .../src/trajectory_preprocessing.cpp | 11 +- .../src/types.hpp | 16 +- .../test/test_collision_distance.cpp | 6 +- .../test/test_forward_projection.cpp | 2 +- .../package.xml | 2 +- .../src/calculate_slowdown_points.cpp | 6 +- .../src/calculate_slowdown_points.hpp | 6 +- .../src/debug.cpp | 39 +- .../src/filter_predicted_objects.cpp | 16 +- .../src/filter_predicted_objects.hpp | 4 +- .../src/footprint.cpp | 14 +- .../src/footprint.hpp | 6 +- .../src/lanelets_selection.cpp | 16 +- .../src/lanelets_selection.hpp | 4 +- .../src/out_of_lane_collisions.cpp | 12 +- .../src/out_of_lane_module.cpp | 101 ++--- .../src/types.hpp | 12 +- .../test/test_filter_predicted_objects.cpp | 2 +- .../collision_checker.hpp | 10 +- .../planner_data.hpp | 4 +- .../plugin_module_interface.hpp | 4 +- .../polygon_utils.hpp | 6 +- .../utils.hpp | 12 +- .../package.xml | 2 +- .../src/collision_checker.cpp | 34 +- .../src/planner_data.cpp | 9 +- .../src/polygon_utils.cpp | 49 ++- .../src/utils.cpp | 12 +- .../test/test_collision_checker.cpp | 12 +- .../package.xml | 2 +- .../src/node.cpp | 50 +-- .../src/node.hpp | 32 +- .../frenet_planner.hpp | 2 +- .../src/frenet_planner/frenet_planner.cpp | 5 +- .../autoware_path_sampler/common_structs.hpp | 22 +- .../include/autoware_path_sampler/node.hpp | 7 +- .../utils/geometry_utils.hpp | 6 +- .../utils/trajectory_utils.hpp | 12 +- .../autoware_path_sampler/package.xml | 2 +- .../autoware_path_sampler/src/node.cpp | 81 ++-- .../src/prepare_inputs.cpp | 4 +- .../autoware_sampler_common/structures.hpp | 14 +- .../autoware_sampler_common/package.xml | 2 +- .../autoware_image_diagnostics/package.xml | 2 +- sensing/autoware_imu_corrector/package.xml | 2 +- .../src/gyro_bias_estimation_module.cpp | 12 +- .../src/gyro_bias_estimator.cpp | 14 +- .../src/gyro_bias_estimator.hpp | 4 +- .../src/imu_corrector_core.cpp | 6 +- .../src/imu_corrector_core.hpp | 8 +- .../CMakeLists.txt | 2 +- .../combine_cloud_handler.hpp | 6 +- .../concatenate_and_time_sync_node.hpp | 8 +- .../concatenate_pointclouds.hpp | 12 +- .../distortion_corrector.hpp | 6 +- .../distortion_corrector_node.hpp | 20 +- .../pointcloud_preprocessor/filter.hpp | 16 +- .../time_synchronizer_node.hpp | 12 +- .../lanelet2_map_filter_node.hpp | 8 +- .../vector_map_inside_area_filter_node.hpp | 4 +- .../package.xml | 2 +- .../combine_cloud_handler.cpp | 6 +- .../concatenate_and_time_sync_node.cpp | 4 +- .../concatenate_pointclouds.cpp | 8 +- .../crop_box_filter/crop_box_filter_node.cpp | 4 +- .../distortion_corrector.cpp | 44 ++- .../distortion_corrector_node.cpp | 16 +- ...ased_voxel_grid_downsample_filter_node.cpp | 4 +- .../src/filter.cpp | 13 +- .../ring_outlier_filter_node.cpp | 4 +- .../time_synchronizer_node.cpp | 10 +- .../vector_map_inside_area_filter_node.cpp | 7 +- .../test/test_distortion_corrector_node.cpp | 36 +- .../package.xml | 2 +- .../radar_static_pointcloud_filter_node.cpp | 4 +- .../radar_static_pointcloud_filter_node.hpp | 4 +- .../package.xml | 2 +- .../src/node.cpp | 5 +- .../autoware_fault_injection/package.xml | 2 +- .../fault_injection_node.cpp | 2 +- .../package.xml | 2 +- .../simple_planning_simulator_core.cpp | 26 +- .../package.xml | 2 +- .../src/converter.cpp | 2 +- .../src/converter.hpp | 5 +- .../package.xml | 2 +- .../mrm_comfortable_stop_operator_core.cpp | 10 +- .../package.xml | 2 +- .../mrm_emergency_stop_operator_core.cpp | 8 +- .../autoware/mrm_handler/mrm_handler_core.hpp | 18 +- system/autoware_mrm_handler/package.xml | 2 +- .../src/mrm_handler/mrm_handler_core.cpp | 16 +- .../package.xml | 2 +- .../src/processing_time_checker.hpp | 4 +- system/autoware_system_monitor/package.xml | 2 +- .../src/ntp_monitor/ntp_monitor.cpp | 4 +- .../src/process_monitor/process_monitor.cpp | 4 +- tools/reaction_analyzer/include/utils.hpp | 4 +- tools/reaction_analyzer/package.xml | 2 +- .../src/reaction_analyzer_node.cpp | 10 +- tools/reaction_analyzer/src/utils.cpp | 29 +- .../accel_brake_map_calibrator_node.hpp | 23 +- .../package.xml | 2 +- .../src/accel_brake_map_calibrator_node.cpp | 14 +- .../autoware_external_cmd_converter/node.hpp | 10 +- .../src/node.cpp | 6 +- .../node.hpp | 15 +- .../package.xml | 2 +- .../src/node.cpp | 12 +- .../package.xml | 2 +- .../package.xml | 2 +- .../src/planning_factor_rviz_plugin.cpp | 32 +- .../path/display.hpp | 10 +- .../path/display_base.hpp | 26 +- .../tier4_planning_rviz_plugin/utils.hpp | 16 +- .../tier4_planning_rviz_plugin/package.xml | 2 +- .../tier4_system_rviz_plugin/package.xml | 2 +- .../tier4_vehicle_rviz_plugin/package.xml | 2 +- .../src/tools/acceleration_meter.cpp | 8 +- .../src/tools/acceleration_meter.hpp | 6 +- .../src/tools/console_meter.cpp | 8 +- .../src/tools/console_meter.hpp | 12 +- 849 files changed, 7327 insertions(+), 7571 deletions(-) diff --git a/common/autoware_goal_distance_calculator/include/autoware/goal_distance_calculator/goal_distance_calculator.hpp b/common/autoware_goal_distance_calculator/include/autoware/goal_distance_calculator/goal_distance_calculator.hpp index 99bc9bd73d140..5375ec743d9f9 100644 --- a/common/autoware_goal_distance_calculator/include/autoware/goal_distance_calculator/goal_distance_calculator.hpp +++ b/common/autoware_goal_distance_calculator/include/autoware/goal_distance_calculator/goal_distance_calculator.hpp @@ -15,7 +15,7 @@ #ifndef AUTOWARE__GOAL_DISTANCE_CALCULATOR__GOAL_DISTANCE_CALCULATOR_HPP_ #define AUTOWARE__GOAL_DISTANCE_CALCULATOR__GOAL_DISTANCE_CALCULATOR_HPP_ -#include +#include #include #include @@ -25,7 +25,7 @@ namespace autoware::goal_distance_calculator { -using autoware::universe_utils::PoseDeviation; +using autoware_utils::PoseDeviation; struct Param { diff --git a/common/autoware_goal_distance_calculator/include/autoware/goal_distance_calculator/goal_distance_calculator_node.hpp b/common/autoware_goal_distance_calculator/include/autoware/goal_distance_calculator/goal_distance_calculator_node.hpp index 60d33b6ddc7af..a4cd9e6a3ba41 100644 --- a/common/autoware_goal_distance_calculator/include/autoware/goal_distance_calculator/goal_distance_calculator_node.hpp +++ b/common/autoware_goal_distance_calculator/include/autoware/goal_distance_calculator/goal_distance_calculator_node.hpp @@ -17,9 +17,9 @@ #include "autoware/goal_distance_calculator/goal_distance_calculator.hpp" -#include -#include -#include +#include +#include +#include #include #include @@ -45,12 +45,12 @@ class GoalDistanceCalculatorNode : public rclcpp::Node private: // Subscriber - autoware::universe_utils::SelfPoseListener self_pose_listener_; - autoware::universe_utils::InterProcessPollingSubscriber + autoware_utils::SelfPoseListener self_pose_listener_; + autoware_utils::InterProcessPollingSubscriber sub_route_{this, "/planning/mission_planning/route"}; // Publisher - autoware::universe_utils::DebugPublisher debug_publisher_; + autoware_utils::DebugPublisher debug_publisher_; // Timer rclcpp::TimerBase::SharedPtr timer_; diff --git a/common/autoware_goal_distance_calculator/package.xml b/common/autoware_goal_distance_calculator/package.xml index 60de6e02e7f45..c4b68fe3ec560 100644 --- a/common/autoware_goal_distance_calculator/package.xml +++ b/common/autoware_goal_distance_calculator/package.xml @@ -12,7 +12,7 @@ autoware_internal_debug_msgs autoware_planning_msgs - autoware_universe_utils + autoware_utils geometry_msgs rclcpp rclcpp_components diff --git a/common/autoware_goal_distance_calculator/src/goal_distance_calculator.cpp b/common/autoware_goal_distance_calculator/src/goal_distance_calculator.cpp index 0bf1d5d5f54bc..decc93dc9dc53 100755 --- a/common/autoware_goal_distance_calculator/src/goal_distance_calculator.cpp +++ b/common/autoware_goal_distance_calculator/src/goal_distance_calculator.cpp @@ -21,7 +21,7 @@ Output GoalDistanceCalculator::update(const Input & input) Output output{}; output.goal_deviation = - autoware::universe_utils::calcPoseDeviation(input.route->goal_pose, input.current_pose->pose); + autoware_utils::calc_pose_deviation(input.route->goal_pose, input.current_pose->pose); return output; } diff --git a/common/autoware_goal_distance_calculator/src/goal_distance_calculator_node.cpp b/common/autoware_goal_distance_calculator/src/goal_distance_calculator_node.cpp index 0e38dfc170f2e..051874fe3561d 100644 --- a/common/autoware_goal_distance_calculator/src/goal_distance_calculator_node.cpp +++ b/common/autoware_goal_distance_calculator/src/goal_distance_calculator_node.cpp @@ -14,7 +14,7 @@ #include "autoware/goal_distance_calculator/goal_distance_calculator_node.hpp" -#include +#include #include #include @@ -42,7 +42,7 @@ GoalDistanceCalculatorNode::GoalDistanceCalculatorNode(const rclcpp::NodeOptions goal_distance_calculator_->setParam(param_); // Wait for first self pose - self_pose_listener_.waitForFirstPose(); + self_pose_listener_.wait_for_first_pose(); // Timer const auto period_ns = rclcpp::Rate(node_param_.update_rate).period(); @@ -54,7 +54,7 @@ GoalDistanceCalculatorNode::GoalDistanceCalculatorNode(const rclcpp::NodeOptions bool GoalDistanceCalculatorNode::tryGetCurrentPose( geometry_msgs::msg::PoseStamped::ConstSharedPtr current_pose) { - auto current_pose_tmp = self_pose_listener_.getCurrentPose(); + auto current_pose_tmp = self_pose_listener_.get_current_pose(); if (!current_pose_tmp) return false; current_pose = current_pose_tmp; return true; @@ -63,7 +63,7 @@ bool GoalDistanceCalculatorNode::tryGetCurrentPose( bool GoalDistanceCalculatorNode::tryGetRoute( autoware_planning_msgs::msg::LaneletRoute::ConstSharedPtr route) { - auto route_tmp = sub_route_.takeData(); + auto route_tmp = sub_route_.take_data(); if (!route_tmp) return false; route = route_tmp; return true; @@ -97,7 +97,7 @@ void GoalDistanceCalculatorNode::onTimer() Output output = goal_distance_calculator_->update(input); { - using autoware::universe_utils::rad2deg; + using autoware_utils::rad2deg; const auto & deviation = output.goal_deviation; debug_publisher_.publish( diff --git a/common/autoware_grid_map_utils/package.xml b/common/autoware_grid_map_utils/package.xml index d745d007f8b37..cdba5b9adb6c5 100644 --- a/common/autoware_grid_map_utils/package.xml +++ b/common/autoware_grid_map_utils/package.xml @@ -10,7 +10,7 @@ autoware_cmake eigen3_cmake_module - autoware_universe_utils + autoware_utils grid_map_core grid_map_cv libopencv-dev diff --git a/common/autoware_grid_map_utils/test/benchmark.cpp b/common/autoware_grid_map_utils/test/benchmark.cpp index 1498b1f7f4e0b..1c33f75a6db24 100644 --- a/common/autoware_grid_map_utils/test/benchmark.cpp +++ b/common/autoware_grid_map_utils/test/benchmark.cpp @@ -17,7 +17,7 @@ #include "grid_map_cv/GridMapCvConverter.hpp" #include "grid_map_cv/GridMapCvProcessing.hpp" -#include +#include #include #include #include @@ -46,7 +46,7 @@ int main(int argc, char * argv[]) result_file << "#Size PolygonVertices PolygonIndexes grid_map_utils_constructor grid_map_utils_iteration " "grid_map_constructor grid_map_iteration\n"; - autoware::universe_utils::StopWatch stopwatch; + autoware_utils::StopWatch stopwatch; constexpr auto nb_iterations = 10; constexpr auto polygon_side_vertices = diff --git a/common/autoware_grid_map_utils/test/test_polygon_iterator.cpp b/common/autoware_grid_map_utils/test/test_polygon_iterator.cpp index 36cff60fc4abe..01aee5298207c 100644 --- a/common/autoware_grid_map_utils/test/test_polygon_iterator.cpp +++ b/common/autoware_grid_map_utils/test/test_polygon_iterator.cpp @@ -14,7 +14,7 @@ #include "autoware/grid_map_utils/polygon_iterator.hpp" -#include +#include #include // gtest diff --git a/common/autoware_object_recognition_utils/include/autoware/object_recognition_utils/matching.hpp b/common/autoware_object_recognition_utils/include/autoware/object_recognition_utils/matching.hpp index 59c01c6d873e7..afeb56c1e6a5d 100644 --- a/common/autoware_object_recognition_utils/include/autoware/object_recognition_utils/matching.hpp +++ b/common/autoware_object_recognition_utils/include/autoware/object_recognition_utils/matching.hpp @@ -16,8 +16,8 @@ #define AUTOWARE__OBJECT_RECOGNITION_UTILS__MATCHING_HPP_ #include "autoware/object_recognition_utils/geometry.hpp" -#include "autoware/universe_utils/geometry/boost_geometry.hpp" -#include "autoware/universe_utils/geometry/boost_polygon_utils.hpp" +#include "autoware_utils/geometry/boost_geometry.hpp" +#include "autoware_utils/geometry/boost_polygon_utils.hpp" #include @@ -28,7 +28,7 @@ namespace autoware::object_recognition_utils { -using autoware::universe_utils::Polygon2d; +using autoware_utils::Polygon2d; // minimum area to avoid division by zero static const double MIN_AREA = 1e-6; @@ -37,7 +37,7 @@ inline double getConvexShapeArea(const Polygon2d & source_polygon, const Polygon boost::geometry::model::multi_polygon union_polygons; boost::geometry::union_(source_polygon, target_polygon, union_polygons); - autoware::universe_utils::Polygon2d hull; + autoware_utils::Polygon2d hull; boost::geometry::convex_hull(union_polygons, hull); return boost::geometry::area(hull); } @@ -67,9 +67,9 @@ inline double getUnionArea(const Polygon2d & source_polygon, const Polygon2d & t template double get2dIoU(const T1 source_object, const T2 target_object, const double min_union_area = 0.01) { - const auto source_polygon = autoware::universe_utils::toPolygon2d(source_object); + const auto source_polygon = autoware_utils::to_polygon2d(source_object); if (boost::geometry::area(source_polygon) < MIN_AREA) return 0.0; - const auto target_polygon = autoware::universe_utils::toPolygon2d(target_object); + const auto target_polygon = autoware_utils::to_polygon2d(target_object); if (boost::geometry::area(target_polygon) < MIN_AREA) return 0.0; const double intersection_area = getIntersectionArea(source_polygon, target_polygon); @@ -84,9 +84,9 @@ double get2dIoU(const T1 source_object, const T2 target_object, const double min template double get2dGeneralizedIoU(const T1 & source_object, const T2 & target_object) { - const auto source_polygon = autoware::universe_utils::toPolygon2d(source_object); + const auto source_polygon = autoware_utils::to_polygon2d(source_object); if (boost::geometry::area(source_polygon) < MIN_AREA) return 0.0; - const auto target_polygon = autoware::universe_utils::toPolygon2d(target_object); + const auto target_polygon = autoware_utils::to_polygon2d(target_object); if (boost::geometry::area(target_polygon) < MIN_AREA) return 0.0; const double intersection_area = getIntersectionArea(source_polygon, target_polygon); @@ -100,10 +100,10 @@ double get2dGeneralizedIoU(const T1 & source_object, const T2 & target_object) template double get2dPrecision(const T1 source_object, const T2 target_object) { - const auto source_polygon = autoware::universe_utils::toPolygon2d(source_object); + const auto source_polygon = autoware_utils::to_polygon2d(source_object); const double source_area = boost::geometry::area(source_polygon); if (source_area < MIN_AREA) return 0.0; - const auto target_polygon = autoware::universe_utils::toPolygon2d(target_object); + const auto target_polygon = autoware_utils::to_polygon2d(target_object); if (boost::geometry::area(target_polygon) < MIN_AREA) return 0.0; const double intersection_area = getIntersectionArea(source_polygon, target_polygon); @@ -115,9 +115,9 @@ double get2dPrecision(const T1 source_object, const T2 target_object) template double get2dRecall(const T1 source_object, const T2 target_object) { - const auto source_polygon = autoware::universe_utils::toPolygon2d(source_object); + const auto source_polygon = autoware_utils::to_polygon2d(source_object); if (boost::geometry::area(source_polygon) < MIN_AREA) return 0.0; - const auto target_polygon = autoware::universe_utils::toPolygon2d(target_object); + const auto target_polygon = autoware_utils::to_polygon2d(target_object); const double target_area = boost::geometry::area(target_polygon); if (target_area < MIN_AREA) return 0.0; diff --git a/common/autoware_object_recognition_utils/include/autoware/object_recognition_utils/predicted_path_utils.hpp b/common/autoware_object_recognition_utils/include/autoware/object_recognition_utils/predicted_path_utils.hpp index 836b5296497e8..46a5676da7ca2 100644 --- a/common/autoware_object_recognition_utils/include/autoware/object_recognition_utils/predicted_path_utils.hpp +++ b/common/autoware_object_recognition_utils/include/autoware/object_recognition_utils/predicted_path_utils.hpp @@ -15,7 +15,7 @@ #ifndef AUTOWARE__OBJECT_RECOGNITION_UTILS__PREDICTED_PATH_UTILS_HPP_ #define AUTOWARE__OBJECT_RECOGNITION_UTILS__PREDICTED_PATH_UTILS_HPP_ -#include "autoware/universe_utils/geometry/geometry.hpp" +#include "autoware_utils/geometry/geometry.hpp" #include #include diff --git a/common/autoware_object_recognition_utils/package.xml b/common/autoware_object_recognition_utils/package.xml index 37b334c0e1bfb..9f3058f9c3872 100644 --- a/common/autoware_object_recognition_utils/package.xml +++ b/common/autoware_object_recognition_utils/package.xml @@ -13,7 +13,7 @@ autoware_interpolation autoware_perception_msgs - autoware_universe_utils + autoware_utils geometry_msgs libboost-dev pcl_conversions diff --git a/common/autoware_object_recognition_utils/src/predicted_path_utils.cpp b/common/autoware_object_recognition_utils/src/predicted_path_utils.cpp index 55ac4e2a370c3..102c0e7e2b487 100644 --- a/common/autoware_object_recognition_utils/src/predicted_path_utils.cpp +++ b/common/autoware_object_recognition_utils/src/predicted_path_utils.cpp @@ -39,7 +39,7 @@ boost::optional calcInterpolatedPose( if (relative_time - epsilon < time_step * path_idx) { const double offset = relative_time - time_step * (path_idx - 1); const double ratio = std::clamp(offset / time_step, 0.0, 1.0); - return autoware::universe_utils::calcInterpolatedPose(prev_pt, pt, ratio, false); + return autoware_utils::calc_interpolated_pose(prev_pt, pt, ratio, false); } } @@ -91,7 +91,7 @@ autoware_perception_msgs::msg::PredictedPath resamplePredictedPath( // Set Position for (size_t i = 0; i < resampled_size; ++i) { - const auto p = autoware::universe_utils::createPoint( + const auto p = autoware_utils::create_point( interpolated_x.at(i), interpolated_y.at(i), interpolated_z.at(i)); resampled_path.path.at(i).position = p; resampled_path.path.at(i).orientation = interpolated_quat.at(i); diff --git a/common/autoware_object_recognition_utils/test/src/test_conversion.cpp b/common/autoware_object_recognition_utils/test/src/test_conversion.cpp index 6a05771149ad2..5692c4c42f20d 100644 --- a/common/autoware_object_recognition_utils/test/src/test_conversion.cpp +++ b/common/autoware_object_recognition_utils/test/src/test_conversion.cpp @@ -13,7 +13,7 @@ // limitations under the License. #include "autoware/object_recognition_utils/conversion.hpp" -#include "autoware/universe_utils/geometry/geometry.hpp" +#include "autoware_utils/geometry/geometry.hpp" #include diff --git a/common/autoware_object_recognition_utils/test/src/test_matching.cpp b/common/autoware_object_recognition_utils/test/src/test_matching.cpp index aefc6251c6276..270a23ff786e1 100644 --- a/common/autoware_object_recognition_utils/test/src/test_matching.cpp +++ b/common/autoware_object_recognition_utils/test/src/test_matching.cpp @@ -13,14 +13,14 @@ // limitations under the License. #include "autoware/object_recognition_utils/matching.hpp" -#include "autoware/universe_utils/geometry/geometry.hpp" +#include "autoware_utils/geometry/geometry.hpp" #include #include -using autoware::universe_utils::Point2d; -using autoware::universe_utils::Point3d; +using autoware_utils::Point2d; +using autoware_utils::Point3d; constexpr double epsilon = 1e-06; @@ -30,7 +30,7 @@ geometry_msgs::msg::Pose createPose(const double x, const double y, const double { geometry_msgs::msg::Pose p; p.position = geometry_msgs::build().x(x).y(y).z(0.0); - p.orientation = autoware::universe_utils::createQuaternionFromYaw(yaw); + p.orientation = autoware_utils::create_quaternion_from_yaw(yaw); return p; } } // namespace diff --git a/common/autoware_object_recognition_utils/test/src/test_predicted_path_utils.cpp b/common/autoware_object_recognition_utils/test/src/test_predicted_path_utils.cpp index 1f434d3362b86..1386f3c460145 100644 --- a/common/autoware_object_recognition_utils/test/src/test_predicted_path_utils.cpp +++ b/common/autoware_object_recognition_utils/test/src/test_predicted_path_utils.cpp @@ -13,8 +13,8 @@ // limitations under the License. #include "autoware/object_recognition_utils/predicted_path_utils.hpp" -#include "autoware/universe_utils/geometry/geometry.hpp" -#include "autoware/universe_utils/math/unit_conversion.hpp" +#include "autoware_utils/geometry/geometry.hpp" +#include "autoware_utils/math/unit_conversion.hpp" #include @@ -22,24 +22,24 @@ #include -using autoware::universe_utils::Point2d; -using autoware::universe_utils::Point3d; +using autoware_utils::Point2d; +using autoware_utils::Point3d; constexpr double epsilon = 1e-06; namespace { -using autoware::universe_utils::createPoint; -using autoware::universe_utils::createQuaternionFromRPY; -using autoware::universe_utils::transformPoint; using autoware_perception_msgs::msg::PredictedPath; +using autoware_utils::create_point; +using autoware_utils::create_quaternion_from_rpy; +using autoware_utils::transform_point; geometry_msgs::msg::Pose createPose( double x, double y, double z, double roll, double pitch, double yaw) { geometry_msgs::msg::Pose p; - p.position = createPoint(x, y, z); - p.orientation = createQuaternionFromRPY(roll, pitch, yaw); + p.position = create_point(x, y, z); + p.orientation = create_quaternion_from_rpy(roll, pitch, yaw); return p; } @@ -67,15 +67,15 @@ PredictedPath createTestPredictedPath( TEST(predicted_path_utils, testCalcInterpolatedPose) { using autoware::object_recognition_utils::calcInterpolatedPose; - using autoware::universe_utils::createQuaternionFromRPY; - using autoware::universe_utils::createQuaternionFromYaw; - using autoware::universe_utils::deg2rad; + using autoware_utils::create_quaternion_from_rpy; + using autoware_utils::create_quaternion_from_yaw; + using autoware_utils::deg2rad; const auto path = createTestPredictedPath(100, 0.1, 1.0); // Normal Case (same point as the original point) { - const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); + const auto ans_quat = create_quaternion_from_rpy(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); for (double t = 0.0; t < 9.0 + 1e-6; t += 1.0) { const auto p = calcInterpolatedPose(path, t); @@ -92,7 +92,7 @@ TEST(predicted_path_utils, testCalcInterpolatedPose) // Normal Case (random case) { - const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); + const auto ans_quat = create_quaternion_from_rpy(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); for (double t = 0.0; t < 9.0; t += 0.3) { const auto p = calcInterpolatedPose(path, t); @@ -133,9 +133,9 @@ TEST(predicted_path_utils, testCalcInterpolatedPose) TEST(predicted_path_utils, resamplePredictedPath_by_vector) { using autoware::object_recognition_utils::resamplePredictedPath; - using autoware::universe_utils::createQuaternionFromRPY; - using autoware::universe_utils::createQuaternionFromYaw; - using autoware::universe_utils::deg2rad; + using autoware_utils::create_quaternion_from_rpy; + using autoware_utils::create_quaternion_from_yaw; + using autoware_utils::deg2rad; const auto path = createTestPredictedPath(10, 1.0, 1.0); @@ -210,9 +210,9 @@ TEST(predicted_path_utils, resamplePredictedPath_by_vector) TEST(predicted_path_utils, resamplePredictedPath_by_sampling_time) { using autoware::object_recognition_utils::resamplePredictedPath; - using autoware::universe_utils::createQuaternionFromRPY; - using autoware::universe_utils::createQuaternionFromYaw; - using autoware::universe_utils::deg2rad; + using autoware_utils::create_quaternion_from_rpy; + using autoware_utils::create_quaternion_from_yaw; + using autoware_utils::deg2rad; const auto path = createTestPredictedPath(10, 1.0, 1.0); diff --git a/common/autoware_path_distance_calculator/Readme.md b/common/autoware_path_distance_calculator/Readme.md index c6389c930f62a..917ba0ff2a50a 100644 --- a/common/autoware_path_distance_calculator/Readme.md +++ b/common/autoware_path_distance_calculator/Readme.md @@ -18,9 +18,9 @@ Note that the distance means the arc-length along the path, not the Euclidean di ### Output -| Name | Type | Description | -| ------------ | --------------------------------------- | ----------------------------------------------------------------------------------------------------- | -| `~/distance` | `tier4_debug_msgs::msg::Float64Stamped` | Publish a distance from the closest path point from the self-position to the end point of the path[m] | +| Name | Type | Description | +| ------------ | --------------------------------------------------- | ----------------------------------------------------------------------------------------------------- | +| `~/distance` | `autoware_internal_debug_msgs::msg::Float64Stamped` | Publish a distance from the closest path point from the self-position to the end point of the path[m] | ## Parameters diff --git a/common/autoware_path_distance_calculator/package.xml b/common/autoware_path_distance_calculator/package.xml index 150ca10754e24..15cedb7b3377c 100644 --- a/common/autoware_path_distance_calculator/package.xml +++ b/common/autoware_path_distance_calculator/package.xml @@ -10,12 +10,12 @@ ament_cmake autoware_cmake + autoware_internal_debug_msgs autoware_motion_utils autoware_planning_msgs - autoware_universe_utils + autoware_utils rclcpp rclcpp_components - tier4_debug_msgs ament_lint_auto autoware_lint_common diff --git a/common/autoware_path_distance_calculator/src/path_distance_calculator.cpp b/common/autoware_path_distance_calculator/src/path_distance_calculator.cpp index b91752e547196..fa24229eb5d7c 100644 --- a/common/autoware_path_distance_calculator/src/path_distance_calculator.cpp +++ b/common/autoware_path_distance_calculator/src/path_distance_calculator.cpp @@ -27,13 +27,13 @@ namespace autoware::path_distance_calculator PathDistanceCalculator::PathDistanceCalculator(const rclcpp::NodeOptions & options) : Node("path_distance_calculator", options), self_pose_listener_(this) { - pub_dist_ = - create_publisher("~/output/distance", rclcpp::QoS(1)); + pub_dist_ = create_publisher( + "~/output/distance", rclcpp::QoS(1)); using std::chrono_literals::operator""s; timer_ = rclcpp::create_timer(this, get_clock(), 1s, [this]() { - const auto path = sub_path_.takeData(); - const auto pose = self_pose_listener_.getCurrentPose(); + const auto path = sub_path_.take_data(); + const auto pose = self_pose_listener_.get_current_pose(); if (!pose) { RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 5000, "no pose"); return; @@ -50,7 +50,7 @@ PathDistanceCalculator::PathDistanceCalculator(const rclcpp::NodeOptions & optio const double distance = autoware::motion_utils::calcSignedArcLength( path->points, pose->pose.position, path->points.size() - 1); - tier4_debug_msgs::msg::Float64Stamped msg; + autoware_internal_debug_msgs::msg::Float64Stamped msg; msg.stamp = pose->header.stamp; msg.data = distance; pub_dist_->publish(msg); diff --git a/common/autoware_path_distance_calculator/src/path_distance_calculator.hpp b/common/autoware_path_distance_calculator/src/path_distance_calculator.hpp index ff83483676467..36a905f67edad 100644 --- a/common/autoware_path_distance_calculator/src/path_distance_calculator.hpp +++ b/common/autoware_path_distance_calculator/src/path_distance_calculator.hpp @@ -15,12 +15,12 @@ #ifndef PATH_DISTANCE_CALCULATOR_HPP_ #define PATH_DISTANCE_CALCULATOR_HPP_ -#include -#include +#include +#include #include +#include #include -#include namespace autoware::path_distance_calculator { @@ -31,11 +31,11 @@ class PathDistanceCalculator : public rclcpp::Node explicit PathDistanceCalculator(const rclcpp::NodeOptions & options); private: - autoware::universe_utils::InterProcessPollingSubscriber - sub_path_{this, "~/input/path"}; - rclcpp::Publisher::SharedPtr pub_dist_; + autoware_utils::InterProcessPollingSubscriber sub_path_{ + this, "~/input/path"}; + rclcpp::Publisher::SharedPtr pub_dist_; rclcpp::TimerBase::SharedPtr timer_; - autoware::universe_utils::SelfPoseListener self_pose_listener_; + autoware_utils::SelfPoseListener self_pose_listener_; }; } // namespace autoware::path_distance_calculator diff --git a/common/autoware_traffic_light_recognition_marker_publisher/package.xml b/common/autoware_traffic_light_recognition_marker_publisher/package.xml index b3f240b4b1ac3..8c4714c5ccc1b 100644 --- a/common/autoware_traffic_light_recognition_marker_publisher/package.xml +++ b/common/autoware_traffic_light_recognition_marker_publisher/package.xml @@ -15,7 +15,7 @@ autoware_lanelet2_extension autoware_map_msgs autoware_perception_msgs - autoware_universe_utils + autoware_utils geometry_msgs rclcpp rclcpp_components diff --git a/control/autoware_autonomous_emergency_braking/include/autoware/autonomous_emergency_braking/node.hpp b/control/autoware_autonomous_emergency_braking/include/autoware/autonomous_emergency_braking/node.hpp index 3de8a52c70643..828965b834541 100644 --- a/control/autoware_autonomous_emergency_braking/include/autoware/autonomous_emergency_braking/node.hpp +++ b/control/autoware_autonomous_emergency_braking/include/autoware/autonomous_emergency_braking/node.hpp @@ -15,11 +15,11 @@ #ifndef AUTOWARE__AUTONOMOUS_EMERGENCY_BRAKING__NODE_HPP_ #define AUTOWARE__AUTONOMOUS_EMERGENCY_BRAKING__NODE_HPP_ -#include "autoware/universe_utils/system/time_keeper.hpp" +#include "autoware_utils/system/time_keeper.hpp" #include -#include -#include +#include +#include #include #include #include @@ -67,9 +67,9 @@ using nav_msgs::msg::Odometry; using sensor_msgs::msg::Imu; using sensor_msgs::msg::PointCloud2; using PointCloud = pcl::PointCloud; -using autoware::universe_utils::Polygon2d; -using autoware::universe_utils::Polygon3d; using autoware::vehicle_info_utils::VehicleInfo; +using autoware_utils::Polygon2d; +using autoware_utils::Polygon3d; using diagnostic_updater::DiagnosticStatusWrapper; using diagnostic_updater::Updater; using visualization_msgs::msg::Marker; @@ -331,28 +331,28 @@ class AEB : public rclcpp::Node explicit AEB(const rclcpp::NodeOptions & node_options); // subscriber - autoware::universe_utils::InterProcessPollingSubscriber sub_point_cloud_{ - this, "~/input/pointcloud", autoware::universe_utils::SingleDepthSensorQoS()}; - autoware::universe_utils::InterProcessPollingSubscriber sub_velocity_{ + autoware_utils::InterProcessPollingSubscriber sub_point_cloud_{ + this, "~/input/pointcloud", autoware_utils::single_depth_sensor_qos()}; + autoware_utils::InterProcessPollingSubscriber sub_velocity_{ this, "~/input/velocity"}; - autoware::universe_utils::InterProcessPollingSubscriber sub_imu_{this, "~/input/imu"}; - autoware::universe_utils::InterProcessPollingSubscriber sub_predicted_traj_{ + autoware_utils::InterProcessPollingSubscriber sub_imu_{this, "~/input/imu"}; + autoware_utils::InterProcessPollingSubscriber sub_predicted_traj_{ this, "~/input/predicted_trajectory"}; - autoware::universe_utils::InterProcessPollingSubscriber predicted_objects_sub_{ + autoware_utils::InterProcessPollingSubscriber predicted_objects_sub_{ this, "~/input/objects"}; - autoware::universe_utils::InterProcessPollingSubscriber sub_autoware_state_{ + autoware_utils::InterProcessPollingSubscriber sub_autoware_state_{ this, "/autoware/state"}; // publisher rclcpp::Publisher::SharedPtr pub_obstacle_pointcloud_; rclcpp::Publisher::SharedPtr debug_marker_publisher_; rclcpp::Publisher::SharedPtr virtual_wall_publisher_; - rclcpp::Publisher::SharedPtr + rclcpp::Publisher::SharedPtr debug_processing_time_detail_pub_; rclcpp::Publisher::SharedPtr debug_rss_distance_publisher_; rclcpp::Publisher::SharedPtr metrics_pub_; // timer rclcpp::TimerBase::SharedPtr timer_; - mutable std::shared_ptr time_keeper_{nullptr}; + mutable std::shared_ptr time_keeper_{nullptr}; // callback /** diff --git a/control/autoware_autonomous_emergency_braking/include/autoware/autonomous_emergency_braking/utils.hpp b/control/autoware_autonomous_emergency_braking/include/autoware/autonomous_emergency_braking/utils.hpp index 3b1994481c8b1..20098386c002a 100644 --- a/control/autoware_autonomous_emergency_braking/include/autoware/autonomous_emergency_braking/utils.hpp +++ b/control/autoware_autonomous_emergency_braking/include/autoware/autonomous_emergency_braking/utils.hpp @@ -17,7 +17,7 @@ #include "autoware/autonomous_emergency_braking/node.hpp" -#include +#include #include #include @@ -44,10 +44,10 @@ namespace autoware::motion::control::autonomous_emergency_braking::utils { -using autoware::universe_utils::Polygon2d; -using autoware::universe_utils::Polygon3d; using autoware_perception_msgs::msg::PredictedObject; using autoware_perception_msgs::msg::PredictedObjects; +using autoware_utils::Polygon2d; +using autoware_utils::Polygon3d; using geometry_msgs::msg::Point; using geometry_msgs::msg::Pose; using geometry_msgs::msg::TransformStamped; diff --git a/control/autoware_autonomous_emergency_braking/package.xml b/control/autoware_autonomous_emergency_braking/package.xml index ce7f8cce1233e..0fa9493e6096a 100644 --- a/control/autoware_autonomous_emergency_braking/package.xml +++ b/control/autoware_autonomous_emergency_braking/package.xml @@ -26,7 +26,7 @@ autoware_planning_msgs autoware_system_msgs autoware_test_utils - autoware_universe_utils + autoware_utils autoware_vehicle_info_utils autoware_vehicle_msgs diagnostic_updater diff --git a/control/autoware_autonomous_emergency_braking/src/node.cpp b/control/autoware_autonomous_emergency_braking/src/node.cpp index eb0884e4f344e..2b2700220cbdb 100644 --- a/control/autoware_autonomous_emergency_braking/src/node.cpp +++ b/control/autoware_autonomous_emergency_braking/src/node.cpp @@ -16,12 +16,12 @@ #include #include #include -#include -#include -#include -#include -#include #include +#include +#include +#include +#include +#include #include #include @@ -73,8 +73,8 @@ constexpr colorTuple MPC_PATH_COLOR = {0.0 / 256.0, 100.0 / 256.0, 0.0 / 256.0, namespace autoware::motion::control::autonomous_emergency_braking { using autoware::motion::control::autonomous_emergency_braking::utils::convertObjToPolygon; -using autoware::universe_utils::Point2d; -using autoware::universe_utils::Point3d; +using autoware_utils::Point2d; +using autoware_utils::Point3d; using diagnostic_msgs::msg::DiagnosticStatus; namespace bg = boost::geometry; @@ -108,34 +108,27 @@ Polygon2d createPolygon( const double rear_overhang = vehicle_info.rear_overhang_m; appendPointToPolygon( - polygon, - autoware::universe_utils::calcOffsetPose(base_pose, longitudinal_offset, width, 0.0).position); + polygon, autoware_utils::calc_offset_pose(base_pose, longitudinal_offset, width, 0.0).position); appendPointToPolygon( polygon, - autoware::universe_utils::calcOffsetPose(base_pose, longitudinal_offset, -width, 0.0).position); + autoware_utils::calc_offset_pose(base_pose, longitudinal_offset, -width, 0.0).position); appendPointToPolygon( - polygon, - autoware::universe_utils::calcOffsetPose(base_pose, -rear_overhang, -width, 0.0).position); + polygon, autoware_utils::calc_offset_pose(base_pose, -rear_overhang, -width, 0.0).position); appendPointToPolygon( - polygon, - autoware::universe_utils::calcOffsetPose(base_pose, -rear_overhang, width, 0.0).position); + polygon, autoware_utils::calc_offset_pose(base_pose, -rear_overhang, width, 0.0).position); appendPointToPolygon( - polygon, - autoware::universe_utils::calcOffsetPose(next_pose, longitudinal_offset, width, 0.0).position); + polygon, autoware_utils::calc_offset_pose(next_pose, longitudinal_offset, width, 0.0).position); appendPointToPolygon( polygon, - autoware::universe_utils::calcOffsetPose(next_pose, longitudinal_offset, -width, 0.0).position); + autoware_utils::calc_offset_pose(next_pose, longitudinal_offset, -width, 0.0).position); appendPointToPolygon( - polygon, - autoware::universe_utils::calcOffsetPose(next_pose, -rear_overhang, -width, 0.0).position); + polygon, autoware_utils::calc_offset_pose(next_pose, -rear_overhang, -width, 0.0).position); appendPointToPolygon( - polygon, - autoware::universe_utils::calcOffsetPose(next_pose, -rear_overhang, width, 0.0).position); + polygon, autoware_utils::calc_offset_pose(next_pose, -rear_overhang, width, 0.0).position); - polygon = autoware::universe_utils::isClockwise(polygon) - ? polygon - : autoware::universe_utils::inverseClockwise(polygon); + polygon = + autoware_utils::is_clockwise(polygon) ? polygon : autoware_utils::inverse_clockwise(polygon); Polygon2d hull_polygon; bg::convex_hull(polygon, hull_polygon); @@ -219,59 +212,57 @@ AEB::AEB(const rclcpp::NodeOptions & node_options) timer_ = rclcpp::create_timer(this, this->get_clock(), period_ns, std::bind(&AEB::onTimer, this)); debug_processing_time_detail_pub_ = - create_publisher( - "~/debug/processing_time_detail_ms", 1); - time_keeper_ = - std::make_shared(debug_processing_time_detail_pub_); + create_publisher("~/debug/processing_time_detail_ms", 1); + time_keeper_ = std::make_shared(debug_processing_time_detail_pub_); } rcl_interfaces::msg::SetParametersResult AEB::onParameter( const std::vector & parameters) { - using autoware::universe_utils::updateParam; - updateParam(parameters, "publish_debug_pointcloud", publish_debug_pointcloud_); - updateParam(parameters, "publish_debug_markers", publish_debug_markers_); - updateParam(parameters, "use_predicted_trajectory", use_predicted_trajectory_); - updateParam(parameters, "use_imu_path", use_imu_path_); - updateParam(parameters, "limit_imu_path_lat_dev", limit_imu_path_lat_dev_); - updateParam(parameters, "limit_imu_path_length", limit_imu_path_length_); - updateParam(parameters, "use_pointcloud_data", use_pointcloud_data_); - updateParam(parameters, "use_predicted_object_data", use_predicted_object_data_); - updateParam( + using autoware_utils::update_param; + update_param(parameters, "publish_debug_pointcloud", publish_debug_pointcloud_); + update_param(parameters, "publish_debug_markers", publish_debug_markers_); + update_param(parameters, "use_predicted_trajectory", use_predicted_trajectory_); + update_param(parameters, "use_imu_path", use_imu_path_); + update_param(parameters, "limit_imu_path_lat_dev", limit_imu_path_lat_dev_); + update_param(parameters, "limit_imu_path_length", limit_imu_path_length_); + update_param(parameters, "use_pointcloud_data", use_pointcloud_data_); + update_param(parameters, "use_predicted_object_data", use_predicted_object_data_); + update_param( parameters, "use_object_velocity_calculation", use_object_velocity_calculation_); - updateParam(parameters, "check_autoware_state", check_autoware_state_); - updateParam(parameters, "path_footprint_extra_margin", path_footprint_extra_margin_); - updateParam(parameters, "imu_path_lat_dev_threshold", imu_path_lat_dev_threshold_); - updateParam( + update_param(parameters, "check_autoware_state", check_autoware_state_); + update_param(parameters, "path_footprint_extra_margin", path_footprint_extra_margin_); + update_param(parameters, "imu_path_lat_dev_threshold", imu_path_lat_dev_threshold_); + update_param( parameters, "speed_calculation_expansion_margin", speed_calculation_expansion_margin_); - updateParam(parameters, "detection_range_min_height", detection_range_min_height_); - updateParam( + update_param(parameters, "detection_range_min_height", detection_range_min_height_); + update_param( parameters, "detection_range_max_height_margin", detection_range_max_height_margin_); - updateParam(parameters, "voxel_grid_x", voxel_grid_x_); - updateParam(parameters, "voxel_grid_y", voxel_grid_y_); - updateParam(parameters, "voxel_grid_z", voxel_grid_z_); - updateParam(parameters, "min_generated_imu_path_length", min_generated_imu_path_length_); - updateParam(parameters, "max_generated_imu_path_length", max_generated_imu_path_length_); - updateParam(parameters, "expand_width", expand_width_); - updateParam(parameters, "longitudinal_offset_margin", longitudinal_offset_margin_); - updateParam(parameters, "t_response", t_response_); - updateParam(parameters, "a_ego_min", a_ego_min_); - updateParam(parameters, "a_obj_min", a_obj_min_); - - updateParam(parameters, "cluster_tolerance", cluster_tolerance_); - updateParam(parameters, "cluster_minimum_height", cluster_minimum_height_); - updateParam(parameters, "minimum_cluster_size", minimum_cluster_size_); - updateParam(parameters, "maximum_cluster_size", maximum_cluster_size_); - - updateParam(parameters, "imu_prediction_time_horizon", imu_prediction_time_horizon_); - updateParam(parameters, "imu_prediction_time_interval", imu_prediction_time_interval_); - updateParam(parameters, "mpc_prediction_time_horizon", mpc_prediction_time_horizon_); - updateParam(parameters, "mpc_prediction_time_interval", mpc_prediction_time_interval_); + update_param(parameters, "voxel_grid_x", voxel_grid_x_); + update_param(parameters, "voxel_grid_y", voxel_grid_y_); + update_param(parameters, "voxel_grid_z", voxel_grid_z_); + update_param(parameters, "min_generated_imu_path_length", min_generated_imu_path_length_); + update_param(parameters, "max_generated_imu_path_length", max_generated_imu_path_length_); + update_param(parameters, "expand_width", expand_width_); + update_param(parameters, "longitudinal_offset_margin", longitudinal_offset_margin_); + update_param(parameters, "t_response", t_response_); + update_param(parameters, "a_ego_min", a_ego_min_); + update_param(parameters, "a_obj_min", a_obj_min_); + + update_param(parameters, "cluster_tolerance", cluster_tolerance_); + update_param(parameters, "cluster_minimum_height", cluster_minimum_height_); + update_param(parameters, "minimum_cluster_size", minimum_cluster_size_); + update_param(parameters, "maximum_cluster_size", maximum_cluster_size_); + + update_param(parameters, "imu_prediction_time_horizon", imu_prediction_time_horizon_); + update_param(parameters, "imu_prediction_time_interval", imu_prediction_time_interval_); + update_param(parameters, "mpc_prediction_time_horizon", mpc_prediction_time_horizon_); + update_param(parameters, "mpc_prediction_time_interval", mpc_prediction_time_interval_); { // Object history data keeper setup auto [previous_obstacle_keep_time, collision_keeping_sec] = collision_data_keeper_.getTimeout(); - updateParam(parameters, "previous_obstacle_keep_time", previous_obstacle_keep_time); - updateParam(parameters, "collision_keeping_sec", collision_keeping_sec); + update_param(parameters, "previous_obstacle_keep_time", previous_obstacle_keep_time); + update_param(parameters, "collision_keeping_sec", collision_keeping_sec); collision_data_keeper_.setTimeout(collision_keeping_sec, previous_obstacle_keep_time); } @@ -300,7 +291,7 @@ void AEB::onImu(const Imu::ConstSharedPtr input_msg) void AEB::onPointCloud(const PointCloud2::ConstSharedPtr input_msg) { - autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + autoware_utils::ScopedTimeTrack st(__func__, *time_keeper_); PointCloud::Ptr pointcloud_ptr(new PointCloud); pcl::fromROSMsg(*input_msg, *pointcloud_ptr); @@ -351,13 +342,13 @@ bool AEB::fetchLatestData() return false; }; - current_velocity_ptr_ = sub_velocity_.takeData(); + current_velocity_ptr_ = sub_velocity_.take_data(); if (!current_velocity_ptr_) { return missing("ego velocity"); } if (use_pointcloud_data_) { - const auto pointcloud_ptr = sub_point_cloud_.takeData(); + const auto pointcloud_ptr = sub_point_cloud_.take_data(); if (!pointcloud_ptr) { return missing("object pointcloud message"); } @@ -371,7 +362,7 @@ bool AEB::fetchLatestData() } if (use_predicted_object_data_) { - predicted_objects_ptr_ = predicted_objects_sub_.takeData(); + predicted_objects_ptr_ = predicted_objects_sub_.take_data(); if (!predicted_objects_ptr_) { return missing("predicted objects"); } @@ -385,7 +376,7 @@ bool AEB::fetchLatestData() const bool has_imu_path = std::invoke([&]() { if (!use_imu_path_) return false; - const auto imu_ptr = sub_imu_.takeData(); + const auto imu_ptr = sub_imu_.take_data(); if (!imu_ptr) { return missing("imu message"); } @@ -398,7 +389,7 @@ bool AEB::fetchLatestData() if (!use_predicted_trajectory_) { return false; } - predicted_traj_ptr_ = sub_predicted_traj_.takeData(); + predicted_traj_ptr_ = sub_predicted_traj_.take_data(); return (!predicted_traj_ptr_) ? missing("control predicted trajectory") : true; }); @@ -409,7 +400,7 @@ bool AEB::fetchLatestData() return false; } - autoware_state_ = sub_autoware_state_.takeData(); + autoware_state_ = sub_autoware_state_.take_data(); if (check_autoware_state_ && !autoware_state_) { return missing("autoware_state"); } @@ -462,7 +453,7 @@ void AEB::onCheckCollision(DiagnosticStatusWrapper & stat) bool AEB::checkCollision(MarkerArray & debug_markers) { - autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + autoware_utils::ScopedTimeTrack st(__func__, *time_keeper_); // step1. check data if (!fetchLatestData()) { @@ -634,7 +625,7 @@ bool AEB::checkCollision(MarkerArray & debug_markers) bool AEB::hasCollision(const double current_v, const ObjectData & closest_object) { - autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + autoware_utils::ScopedTimeTrack st(__func__, *time_keeper_); const double rss_dist = std::invoke([&]() { const double & obj_v = closest_object.velocity; const double & t = t_response_; @@ -663,7 +654,7 @@ bool AEB::hasCollision(const double current_v, const ObjectData & closest_object Path AEB::generateEgoPath(const double curr_v, const double curr_w) { - autoware::universe_utils::ScopedTimeTrack st(std::string(__func__) + "(IMU)", *time_keeper_); + autoware_utils::ScopedTimeTrack st(std::string(__func__) + "(IMU)", *time_keeper_); const double & dt = imu_prediction_time_interval_; const double distance_between_points = std::abs(curr_v) * dt; constexpr double minimum_distance_between_points{1e-2}; @@ -675,8 +666,8 @@ Path AEB::generateEgoPath(const double curr_v, const double curr_w) // The initial pose is always aligned with the local reference frame. geometry_msgs::msg::Pose initial_pose; - initial_pose.position = autoware::universe_utils::createPoint(0.0, 0.0, 0.0); - initial_pose.orientation = autoware::universe_utils::createQuaternionFromYaw(0.0); + initial_pose.position = autoware_utils::create_point(0.0, 0.0, 0.0); + initial_pose.orientation = autoware_utils::create_quaternion_from_yaw(0.0); const double horizon = imu_prediction_time_horizon_; const double base_link_to_front_offset = vehicle_info_.max_longitudinal_offset_m; @@ -701,13 +692,13 @@ Path AEB::generateEgoPath(const double curr_v, const double curr_w) curr_y = curr_y + curr_v * std::sin(curr_yaw) * dt; curr_yaw = curr_yaw + curr_w * dt; geometry_msgs::msg::Pose current_pose = - autoware::universe_utils::calcOffsetPose(initial_pose, curr_x, curr_y, 0.0, curr_yaw); + autoware_utils::calc_offset_pose(initial_pose, curr_x, curr_y, 0.0, curr_yaw); t += dt; path_arc_length += distance_between_points; - const auto edge_of_ego_vehicle = autoware::universe_utils::calcOffsetPose( - current_pose, longitudinal_offset, lateral_offset, 0.0) - .position; + const auto edge_of_ego_vehicle = + autoware_utils::calc_offset_pose(current_pose, longitudinal_offset, lateral_offset, 0.0) + .position; const bool basic_path_conditions_satisfied = (t > horizon) && (path_arc_length > min_generated_imu_path_length_); @@ -729,7 +720,7 @@ Path AEB::generateEgoPath(const double curr_v, const double curr_w) std::optional AEB::generateEgoPath(const Trajectory & predicted_traj) { - autoware::universe_utils::ScopedTimeTrack st(std::string(__func__) + "(MPC)", *time_keeper_); + autoware_utils::ScopedTimeTrack st(std::string(__func__) + "(MPC)", *time_keeper_); if (predicted_traj.points.empty()) { return std::nullopt; } @@ -747,9 +738,7 @@ std::optional AEB::generateEgoPath(const Trajectory & predicted_traj) tf2::doTransform(predicted_traj.points.at(i).pose, map_pose, transform_stamped.value()); // skip points that are too close to the last point in the path - if ( - autoware::universe_utils::calcDistance2d(path.back(), map_pose) < - minimum_distance_between_points) { + if (autoware_utils::calc_distance2d(path.back(), map_pose) < minimum_distance_between_points) { continue; } @@ -766,7 +755,7 @@ std::optional AEB::generateEgoPath(const Trajectory & predicted_traj) void AEB::generatePathFootprint( const Path & path, const double extra_width_margin, std::vector & polygons) { - autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + autoware_utils::ScopedTimeTrack st(__func__, *time_keeper_); if (path.empty()) { return; } @@ -779,7 +768,7 @@ void AEB::generatePathFootprint( std::vector AEB::generatePathFootprint( const Path & path, const double extra_width_margin) { - autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + autoware_utils::ScopedTimeTrack st(__func__, *time_keeper_); if (path.empty()) { return {}; } @@ -795,7 +784,7 @@ void AEB::createObjectDataUsingPredictedObjects( const Path & ego_path, const std::vector & ego_polys, std::vector & object_data_vector) { - autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + autoware_utils::ScopedTimeTrack st(__func__, *time_keeper_); if (predicted_objects_ptr_->objects.empty() || ego_polys.empty()) return; const double current_ego_speed = current_velocity_ptr_->longitudinal_velocity; @@ -806,7 +795,7 @@ void AEB::createObjectDataUsingPredictedObjects( const auto current_p = [&]() { const auto & first_point_of_path = ego_path.front(); const auto & p = first_point_of_path.position; - return autoware::universe_utils::createPoint(p.x, p.y, p.z); + return autoware_utils::create_point(p.x, p.y, p.z); }(); auto get_object_tangent_velocity = @@ -854,7 +843,7 @@ void AEB::createObjectDataUsingPredictedObjects( bool collision_points_added{false}; for (const auto & collision_point : collision_points_bg) { const auto obj_position = - autoware::universe_utils::createPoint(collision_point.x(), collision_point.y(), 0.0); + autoware_utils::create_point(collision_point.x(), collision_point.y(), 0.0); const double obj_arc_length = autoware::motion_utils::calcSignedArcLength(ego_path, current_p, obj_position); if (std::isnan(obj_arc_length)) continue; @@ -883,7 +872,7 @@ void AEB::getPointsBelongingToClusterHulls( const PointCloud::Ptr obstacle_points_ptr, const PointCloud::Ptr points_belonging_to_cluster_hulls, MarkerArray & debug_markers) { - autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + autoware_utils::ScopedTimeTrack st(__func__, *time_keeper_); // eliminate noisy points by only considering points belonging to clusters of at least a certain // size if (obstacle_points_ptr->empty()) return; @@ -923,7 +912,7 @@ void AEB::getPointsBelongingToClusterHulls( for (const auto & p : *surface_hull) { points_belonging_to_cluster_hulls->push_back(p); if (publish_debug_markers_) { - const auto geom_point = autoware::universe_utils::createPoint(p.x, p.y, p.z); + const auto geom_point = autoware_utils::create_point(p.x, p.y, p.z); appendPointToPolygon(hull_polygon, geom_point); } } @@ -939,7 +928,7 @@ void AEB::getClosestObjectsOnPath( const Path & ego_path, const rclcpp::Time & stamp, const PointCloud::Ptr points_belonging_to_cluster_hulls, std::vector & objects) { - autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + autoware_utils::ScopedTimeTrack st(__func__, *time_keeper_); // check if the predicted path has a valid number of points if (ego_path.size() < 2 || points_belonging_to_cluster_hulls->empty()) { return; @@ -954,7 +943,7 @@ void AEB::getClosestObjectsOnPath( const auto path_width = vehicle_info_.vehicle_width_m / 2.0 + expand_width_; // select points inside the ego footprint path for (const auto & p : *points_belonging_to_cluster_hulls) { - const auto obj_position = autoware::universe_utils::createPoint(p.x, p.y, p.z); + const auto obj_position = autoware_utils::create_point(p.x, p.y, p.z); auto obj_data_opt = utils::getObjectOnPathData( ego_path, obj_position, stamp, path_length, path_width, speed_calculation_expansion_margin_, longitudinal_offset, 0.0); @@ -965,7 +954,7 @@ void AEB::getClosestObjectsOnPath( void AEB::cropPointCloudWithEgoFootprintPath( const std::vector & ego_polys, PointCloud::Ptr filtered_objects) { - autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + autoware_utils::ScopedTimeTrack st(__func__, *time_keeper_); if (ego_polys.empty()) { return; } @@ -1000,13 +989,13 @@ void AEB::addClusterHullMarkers( const rclcpp::Time & current_time, const std::vector & hulls, const colorTuple & debug_colors, const std::string & ns, MarkerArray & debug_markers) { - autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + autoware_utils::ScopedTimeTrack st(__func__, *time_keeper_); const auto [color_r, color_g, color_b, color_a] = debug_colors; - auto hull_marker = autoware::universe_utils::createDefaultMarker( + auto hull_marker = autoware_utils::create_default_marker( "base_link", current_time, ns, 0, Marker::LINE_LIST, - autoware::universe_utils::createMarkerScale(0.03, 0.0, 0.0), - autoware::universe_utils::createMarkerColor(color_r, color_g, color_b, color_a)); + autoware_utils::create_marker_scale(0.03, 0.0, 0.0), + autoware_utils::create_marker_color(color_r, color_g, color_b, color_a)); utils::fillMarkerFromPolygon(hulls, hull_marker); debug_markers.markers.push_back(hull_marker); } @@ -1016,30 +1005,30 @@ void AEB::addMarker( const std::vector & objects, const std::optional & closest_object, const colorTuple & debug_colors, const std::string & ns, MarkerArray & debug_markers) { - autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + autoware_utils::ScopedTimeTrack st(__func__, *time_keeper_); const auto [color_r, color_g, color_b, color_a] = debug_colors; - auto path_marker = autoware::universe_utils::createDefaultMarker( + auto path_marker = autoware_utils::create_default_marker( "base_link", current_time, ns + "_path", 0L, Marker::LINE_STRIP, - autoware::universe_utils::createMarkerScale(0.2, 0.2, 0.2), - autoware::universe_utils::createMarkerColor(color_r, color_g, color_b, color_a)); + autoware_utils::create_marker_scale(0.2, 0.2, 0.2), + autoware_utils::create_marker_color(color_r, color_g, color_b, color_a)); path_marker.points.reserve(path.size()); for (const auto & p : path) { path_marker.points.push_back(p.position); } debug_markers.markers.push_back(path_marker); - auto polygon_marker = autoware::universe_utils::createDefaultMarker( + auto polygon_marker = autoware_utils::create_default_marker( "base_link", current_time, ns + "_polygon", 0, Marker::LINE_LIST, - autoware::universe_utils::createMarkerScale(0.03, 0.0, 0.0), - autoware::universe_utils::createMarkerColor(color_r, color_g, color_b, color_a)); + autoware_utils::create_marker_scale(0.03, 0.0, 0.0), + autoware_utils::create_marker_color(color_r, color_g, color_b, color_a)); utils::fillMarkerFromPolygon(polygons, polygon_marker); debug_markers.markers.push_back(polygon_marker); - auto object_data_marker = autoware::universe_utils::createDefaultMarker( + auto object_data_marker = autoware_utils::create_default_marker( "base_link", current_time, ns + "_objects", 0, Marker::SPHERE_LIST, - autoware::universe_utils::createMarkerScale(0.5, 0.5, 0.5), - autoware::universe_utils::createMarkerColor(color_r, color_g, color_b, color_a)); + autoware_utils::create_marker_scale(0.5, 0.5, 0.5), + autoware_utils::create_marker_color(color_r, color_g, color_b, color_a)); for (const auto & e : objects) { object_data_marker.points.push_back(e.position); } @@ -1048,11 +1037,11 @@ void AEB::addMarker( // Visualize planner type text if (closest_object.has_value()) { const auto & obj = closest_object.value(); - const auto color = autoware::universe_utils::createMarkerColor(0.95, 0.95, 0.95, 0.999); - auto closest_object_velocity_marker_array = autoware::universe_utils::createDefaultMarker( + const auto color = autoware_utils::create_marker_color(0.95, 0.95, 0.95, 0.999); + auto closest_object_velocity_marker_array = autoware_utils::create_default_marker( "base_link", obj.stamp, ns + "_closest_object_velocity", 0, visualization_msgs::msg::Marker::TEXT_VIEW_FACING, - autoware::universe_utils::createMarkerScale(0.0, 0.0, 0.7), color); + autoware_utils::create_marker_scale(0.0, 0.0, 0.7), color); closest_object_velocity_marker_array.pose.position = obj.position; const auto ego_velocity = current_velocity_ptr_->longitudinal_velocity; closest_object_velocity_marker_array.text = @@ -1070,7 +1059,7 @@ void AEB::addMarker( void AEB::addVirtualStopWallMarker(MarkerArray & markers) { - autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + autoware_utils::ScopedTimeTrack st(__func__, *time_keeper_); const auto ego_map_pose = std::invoke([this]() -> std::optional { const auto logger = get_logger(); const auto tf_current_pose = utils::getTransform("map", "base_link", tf_buffer_, logger); @@ -1086,21 +1075,21 @@ void AEB::addVirtualStopWallMarker(MarkerArray & markers) if (ego_map_pose.has_value()) { const double base_link_to_front_offset = vehicle_info_.max_longitudinal_offset_m; - const auto ego_front_pose = autoware::universe_utils::calcOffsetPose( + const auto ego_front_pose = autoware_utils::calc_offset_pose( ego_map_pose.value(), base_link_to_front_offset, 0.0, 0.0, 0.0); const auto virtual_stop_wall = autoware::motion_utils::createStopVirtualWallMarker( ego_front_pose, "autonomous_emergency_braking", this->now(), 0); - autoware::universe_utils::appendMarkerArray(virtual_stop_wall, &markers); + autoware_utils::append_marker_array(virtual_stop_wall, &markers); } } void AEB::addCollisionMarker(const ObjectData & data, MarkerArray & debug_markers) { - autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); - auto point_marker = autoware::universe_utils::createDefaultMarker( + autoware_utils::ScopedTimeTrack st(__func__, *time_keeper_); + auto point_marker = autoware_utils::create_default_marker( "base_link", data.stamp, "collision_point", 0, Marker::SPHERE, - autoware::universe_utils::createMarkerScale(0.3, 0.3, 0.3), - autoware::universe_utils::createMarkerColor(1.0, 0.0, 0.0, 0.3)); + autoware_utils::create_marker_scale(0.3, 0.3, 0.3), + autoware_utils::create_marker_color(1.0, 0.0, 0.0, 0.3)); point_marker.pose.position = data.position; debug_markers.markers.push_back(point_marker); } diff --git a/control/autoware_autonomous_emergency_braking/src/utils.cpp b/control/autoware_autonomous_emergency_braking/src/utils.cpp index abd203dee2a02..e2b7691c2e8e8 100644 --- a/control/autoware_autonomous_emergency_braking/src/utils.cpp +++ b/control/autoware_autonomous_emergency_braking/src/utils.cpp @@ -14,7 +14,7 @@ #include #include -#include +#include #include #include @@ -22,9 +22,9 @@ namespace autoware::motion::control::autonomous_emergency_braking::utils { -using autoware::universe_utils::Polygon2d; using autoware_perception_msgs::msg::PredictedObject; using autoware_perception_msgs::msg::PredictedObjects; +using autoware_utils::Polygon2d; using geometry_msgs::msg::Point; using geometry_msgs::msg::Pose; using geometry_msgs::msg::TransformStamped; @@ -37,7 +37,7 @@ std::optional getObjectOnPathData( { const auto current_p = [&]() { const auto & p = ego_path.front().position; - return autoware::universe_utils::createPoint(p.x, p.y, p.z); + return autoware_utils::create_point(p.x, p.y, p.z); }(); const double obj_arc_length = autoware::motion_utils::calcSignedArcLength(ego_path, current_p, obj_position); @@ -228,10 +228,8 @@ void fillMarkerFromPolygon( for (size_t dp_idx = 0; dp_idx < poly.outer().size(); ++dp_idx) { const auto & boost_cp = poly.outer().at(dp_idx); const auto & boost_np = poly.outer().at((dp_idx + 1) % poly.outer().size()); - const auto curr_point = - autoware::universe_utils::createPoint(boost_cp.x(), boost_cp.y(), 0.0); - const auto next_point = - autoware::universe_utils::createPoint(boost_np.x(), boost_np.y(), 0.0); + const auto curr_point = autoware_utils::create_point(boost_cp.x(), boost_cp.y(), 0.0); + const auto next_point = autoware_utils::create_point(boost_np.x(), boost_np.y(), 0.0); polygon_marker.points.push_back(curr_point); polygon_marker.points.push_back(next_point); } @@ -246,9 +244,9 @@ void fillMarkerFromPolygon( const auto & boost_cp = poly.outer().at(dp_idx); const auto & boost_np = poly.outer().at((dp_idx + 1) % poly.outer().size()); const auto curr_point = - autoware::universe_utils::createPoint(boost_cp.x(), boost_cp.y(), boost_cp.z()); + autoware_utils::create_point(boost_cp.x(), boost_cp.y(), boost_cp.z()); const auto next_point = - autoware::universe_utils::createPoint(boost_np.x(), boost_np.y(), boost_np.z()); + autoware_utils::create_point(boost_np.x(), boost_np.y(), boost_np.z()); polygon_marker.points.push_back(curr_point); polygon_marker.points.push_back(next_point); } diff --git a/control/autoware_autonomous_emergency_braking/test/test.cpp b/control/autoware_autonomous_emergency_braking/test/test.cpp index 1845ab4074fad..f8678b184bd55 100644 --- a/control/autoware_autonomous_emergency_braking/test/test.cpp +++ b/control/autoware_autonomous_emergency_braking/test/test.cpp @@ -15,7 +15,7 @@ #include "test.hpp" #include "autoware/autonomous_emergency_braking/node.hpp" -#include "autoware/universe_utils/geometry/geometry.hpp" +#include "autoware_utils/geometry/geometry.hpp" #include #include @@ -35,9 +35,9 @@ namespace autoware::motion::control::autonomous_emergency_braking::test { -using autoware::universe_utils::Polygon2d; using autoware_perception_msgs::msg::PredictedObject; using autoware_perception_msgs::msg::PredictedObjects; +using autoware_utils::Polygon2d; using geometry_msgs::msg::Point; using geometry_msgs::msg::Pose; using geometry_msgs::msg::TransformStamped; @@ -58,7 +58,7 @@ Imu make_imu_message( { Imu imu_msg; imu_msg.header = header; - imu_msg.orientation = autoware::universe_utils::createQuaternionFromYaw(yaw); + imu_msg.orientation = autoware_utils::create_quaternion_from_yaw(yaw); imu_msg.angular_velocity.z = angular_velocity_z; imu_msg.linear_acceleration.x = ax; imu_msg.linear_acceleration.y = ay; @@ -327,7 +327,7 @@ TEST_F(TestAEB, checkConvertObjectToPolygon) geometry_msgs::msg::Transform transform; constexpr double yaw{0.0}; - transform.rotation = autoware::universe_utils::createQuaternionFromYaw(yaw); + transform.rotation = autoware_utils::create_quaternion_from_yaw(yaw); geometry_msgs::msg::Vector3 translation; translation.x = 1.0; translation.y = 0.0; diff --git a/control/autoware_autonomous_emergency_braking/test/test.hpp b/control/autoware_autonomous_emergency_braking/test/test.hpp index 86edbf73a2a12..fd34729c1d3b6 100644 --- a/control/autoware_autonomous_emergency_braking/test/test.hpp +++ b/control/autoware_autonomous_emergency_braking/test/test.hpp @@ -38,8 +38,8 @@ using nav_msgs::msg::Odometry; using sensor_msgs::msg::Imu; using sensor_msgs::msg::PointCloud2; using PointCloud = pcl::PointCloud; -using autoware::universe_utils::Polygon2d; using autoware::vehicle_info_utils::VehicleInfo; +using autoware_utils::Polygon2d; using diagnostic_updater::DiagnosticStatusWrapper; using diagnostic_updater::Updater; using visualization_msgs::msg::Marker; diff --git a/control/autoware_collision_detector/include/autoware/collision_detector/node.hpp b/control/autoware_collision_detector/include/autoware/collision_detector/node.hpp index a45f70fc2612e..727d5b5faeb3f 100644 --- a/control/autoware_collision_detector/include/autoware/collision_detector/node.hpp +++ b/control/autoware_collision_detector/include/autoware/collision_detector/node.hpp @@ -15,7 +15,7 @@ #ifndef AUTOWARE__COLLISION_DETECTOR__NODE_HPP_ #define AUTOWARE__COLLISION_DETECTOR__NODE_HPP_ -#include +#include #include #include #include @@ -108,14 +108,13 @@ class CollisionDetectorNode : public rclcpp::Node rclcpp::TimerBase::SharedPtr timer_; // publisher and subscriber - autoware::universe_utils::InterProcessPollingSubscriber sub_odometry_{ + autoware_utils::InterProcessPollingSubscriber sub_odometry_{ this, "~/input/odometry"}; - autoware::universe_utils::InterProcessPollingSubscriber - sub_pointcloud_{this, "~/input/pointcloud", autoware::universe_utils::SingleDepthSensorQoS()}; - autoware::universe_utils::InterProcessPollingSubscriber sub_dynamic_objects_{ + autoware_utils::InterProcessPollingSubscriber sub_pointcloud_{ + this, "~/input/pointcloud", autoware_utils::single_depth_sensor_qos()}; + autoware_utils::InterProcessPollingSubscriber sub_dynamic_objects_{ this, "~/input/objects"}; - autoware::universe_utils::InterProcessPollingSubscriber< - autoware_adapi_v1_msgs::msg::OperationModeState> + autoware_utils::InterProcessPollingSubscriber sub_operation_mode_{this, "/api/operation_mode/state", rclcpp::QoS{1}.transient_local()}; // parameter diff --git a/control/autoware_collision_detector/package.xml b/control/autoware_collision_detector/package.xml index e6f01ce964161..c573b811027fb 100644 --- a/control/autoware_collision_detector/package.xml +++ b/control/autoware_collision_detector/package.xml @@ -20,7 +20,6 @@ autoware_adapi_v1_msgs autoware_perception_msgs - autoware_universe_utils autoware_utils autoware_vehicle_info_utils diagnostic_msgs diff --git a/control/autoware_collision_detector/src/node.cpp b/control/autoware_collision_detector/src/node.cpp index 77d6d76c08b0a..b720f2731f6e9 100644 --- a/control/autoware_collision_detector/src/node.cpp +++ b/control/autoware_collision_detector/src/node.cpp @@ -14,8 +14,8 @@ #include "autoware/collision_detector/node.hpp" -#include -#include +#include +#include #include #include @@ -44,8 +44,8 @@ namespace autoware::collision_detector namespace bg = boost::geometry; using Point2d = bg::model::d2::point_xy; using Polygon2d = bg::model::polygon; -using autoware::universe_utils::createPoint; -using autoware::universe_utils::pose2transform; +using autoware_utils::create_point; +using autoware_utils::pose2transform; namespace { @@ -318,7 +318,7 @@ bool CollisionDetectorNode::shouldBeExcluded( void CollisionDetectorNode::checkCollision(diagnostic_updater::DiagnosticStatusWrapper & stat) { - odometry_ptr_ = sub_odometry_.takeData(); + odometry_ptr_ = sub_odometry_.take_data(); if (!odometry_ptr_) { RCLCPP_INFO_THROTTLE( @@ -326,9 +326,9 @@ void CollisionDetectorNode::checkCollision(diagnostic_updater::DiagnosticStatusW return; } - pointcloud_ptr_ = sub_pointcloud_.takeData(); - object_ptr_ = sub_dynamic_objects_.takeData(); - operation_mode_ptr_ = sub_operation_mode_.takeData(); + pointcloud_ptr_ = sub_pointcloud_.take_data(); + object_ptr_ = sub_dynamic_objects_.take_data(); + operation_mode_ptr_ = sub_operation_mode_.take_data(); if (node_param_.use_pointcloud && !pointcloud_ptr_) { RCLCPP_WARN_THROTTLE( @@ -431,7 +431,7 @@ boost::optional CollisionDetectorNode::getNearestObstacleByPointCloud( const auto distance_to_object = bg::distance(ego_polygon, boost_point); if (distance_to_object < minimum_distance) { - nearest_point = createPoint(p.x, p.y, p.z); + nearest_point = create_point(p.x, p.y, p.z); minimum_distance = distance_to_object; } } diff --git a/control/autoware_control_performance_analysis/include/autoware/control_performance_analysis/control_performance_analysis_node.hpp b/control/autoware_control_performance_analysis/include/autoware/control_performance_analysis/control_performance_analysis_node.hpp index 973574d47c50f..ab8a734b72efa 100644 --- a/control/autoware_control_performance_analysis/include/autoware/control_performance_analysis/control_performance_analysis_node.hpp +++ b/control/autoware_control_performance_analysis/include/autoware/control_performance_analysis/control_performance_analysis_node.hpp @@ -20,7 +20,7 @@ #include "autoware_control_performance_analysis/msg/error_stamped.hpp" #include -#include +#include #include #include diff --git a/control/autoware_control_performance_analysis/package.xml b/control/autoware_control_performance_analysis/package.xml index a77a740130861..426ffbb97ed90 100644 --- a/control/autoware_control_performance_analysis/package.xml +++ b/control/autoware_control_performance_analysis/package.xml @@ -30,7 +30,7 @@ autoware_motion_utils autoware_planning_msgs autoware_signal_processing - autoware_universe_utils + autoware_utils autoware_vehicle_info_utils autoware_vehicle_msgs geometry_msgs diff --git a/control/autoware_control_performance_analysis/src/control_performance_analysis_core.cpp b/control/autoware_control_performance_analysis/src/control_performance_analysis_core.cpp index ee4b4ad816a7f..388c63de3c2ac 100644 --- a/control/autoware_control_performance_analysis/src/control_performance_analysis_core.cpp +++ b/control/autoware_control_performance_analysis/src/control_performance_analysis_core.cpp @@ -15,8 +15,8 @@ #include "autoware/control_performance_analysis/control_performance_analysis_core.hpp" #include "autoware/motion_utils/trajectory/interpolation.hpp" -#include "autoware/universe_utils/geometry/geometry.hpp" -#include "autoware/universe_utils/geometry/pose_deviation.hpp" +#include "autoware_utils/geometry/geometry.hpp" +#include "autoware_utils/geometry/pose_deviation.hpp" #include #include @@ -162,7 +162,7 @@ bool ControlPerformanceAnalysisCore::calculateErrorVars() // Compute the yaw angle error. const double heading_yaw_error = - autoware::universe_utils::calcYawDeviation(pose_interp_wp, *current_vec_pose_ptr_); + autoware_utils::calc_yaw_deviation(pose_interp_wp, *current_vec_pose_ptr_); // Set the values of ErrorMsgVars. @@ -178,7 +178,7 @@ bool ControlPerformanceAnalysisCore::calculateErrorVars() const double Vx = odom_history_ptr_->at(odom_size - 2).twist.twist.linear.x; // Current acceleration calculation - const auto ds = autoware::universe_utils::calcDistance2d( + const auto ds = autoware_utils::calc_distance2d( odom_history_ptr_->at(odom_size - 1).pose.pose, odom_history_ptr_->at(odom_size - 2).pose.pose); const double vel_mean = (odom_history_ptr_->at(odom_size - 1).twist.twist.linear.x + @@ -377,8 +377,7 @@ std::optional ControlPerformanceAnalysisCore::findCurveRefIdx() } auto fun_distance_cond = [this](auto point_t) { - const double dist = - autoware::universe_utils::calcDistance2d(point_t.pose, *interpolated_pose_ptr_); + const double dist = autoware_utils::calc_distance2d(point_t.pose, *interpolated_pose_ptr_); return dist > p_.wheelbase_; }; @@ -449,7 +448,7 @@ double ControlPerformanceAnalysisCore::estimateCurvature() // Compute arc-length ds between 2 points. const double ds_arc_length = - autoware::universe_utils::calcDistance2d(front_axleWP_pose_prev, front_axleWP_pose); + autoware_utils::calc_distance2d(front_axleWP_pose_prev, front_axleWP_pose); // Define waypoints 10 meters behind the rear axle if exist. // If not exist, we will take the first point of the @@ -468,7 +467,7 @@ double ControlPerformanceAnalysisCore::estimateCurvature() // We compute a curvature estimate from these points. double estimated_curvature = 0.0; try { - estimated_curvature = autoware::universe_utils::calcCurvature( + estimated_curvature = autoware_utils::calc_curvature( points.at(loc_of_back_idx).pose.position, points.at(idx_curve_ref_wp).pose.position, points.at(loc_of_forward_idx).pose.position); } catch (...) { @@ -493,7 +492,7 @@ double ControlPerformanceAnalysisCore::estimatePurePursuitCurvature() const double look_ahead_distance_pp = std::max(p_.wheelbase_, 2 * Vx); auto fun_distance_cond = [this, &look_ahead_distance_pp](auto point_t) { - const double dist = autoware::universe_utils::calcDistance2d(point_t, *interpolated_pose_ptr_); + const double dist = autoware_utils::calc_distance2d(point_t, *interpolated_pose_ptr_); return dist > look_ahead_distance_pp; }; diff --git a/control/autoware_control_validator/include/autoware/control_validator/control_validator.hpp b/control/autoware_control_validator/include/autoware/control_validator/control_validator.hpp index b828c8d07ac49..88d116490065d 100644 --- a/control/autoware_control_validator/include/autoware/control_validator/control_validator.hpp +++ b/control/autoware_control_validator/include/autoware/control_validator/control_validator.hpp @@ -16,13 +16,13 @@ #define AUTOWARE__CONTROL_VALIDATOR__CONTROL_VALIDATOR_HPP_ #include "autoware/control_validator/debug_marker.hpp" -#include "autoware/universe_utils/ros/polling_subscriber.hpp" +#include "autoware_utils/ros/polling_subscriber.hpp" #include "autoware_vehicle_info_utils/vehicle_info.hpp" #include "diagnostic_updater/diagnostic_updater.hpp" #include -#include #include +#include #include #include @@ -135,8 +135,8 @@ class ControlValidator : public rclcpp::Node // Subscribers and publishers rclcpp::Subscription::SharedPtr sub_predicted_traj_; - universe_utils::InterProcessPollingSubscriber::SharedPtr sub_kinematics_; - universe_utils::InterProcessPollingSubscriber::SharedPtr sub_reference_traj_; + autoware_utils::InterProcessPollingSubscriber::SharedPtr sub_kinematics_; + autoware_utils::InterProcessPollingSubscriber::SharedPtr sub_reference_traj_; rclcpp::Publisher::SharedPtr pub_status_; rclcpp::Publisher::SharedPtr pub_markers_; rclcpp::Publisher::SharedPtr @@ -168,7 +168,7 @@ class ControlValidator : public rclcpp::Node Odometry::ConstSharedPtr current_kinematics_; - autoware::universe_utils::StopWatch stop_watch; + autoware_utils::StopWatch stop_watch; std::shared_ptr debug_pose_publisher_; }; diff --git a/control/autoware_control_validator/package.xml b/control/autoware_control_validator/package.xml index fbd0d4d332fe8..ecfef2a9046ed 100644 --- a/control/autoware_control_validator/package.xml +++ b/control/autoware_control_validator/package.xml @@ -23,7 +23,7 @@ autoware_motion_utils autoware_planning_msgs autoware_signal_processing - autoware_universe_utils + autoware_utils autoware_vehicle_info_utils diagnostic_updater geometry_msgs diff --git a/control/autoware_control_validator/src/control_validator.cpp b/control/autoware_control_validator/src/control_validator.cpp index e0b9c7135e5f0..0f2763a53f0c9 100644 --- a/control/autoware_control_validator/src/control_validator.cpp +++ b/control/autoware_control_validator/src/control_validator.cpp @@ -38,10 +38,10 @@ ControlValidator::ControlValidator(const rclcpp::NodeOptions & options) "~/input/predicted_trajectory", 1, std::bind(&ControlValidator::on_predicted_trajectory, this, _1)); sub_kinematics_ = - universe_utils::InterProcessPollingSubscriber::create_subscription( + autoware_utils::InterProcessPollingSubscriber::create_subscription( this, "~/input/kinematics", 1); sub_reference_traj_ = - autoware::universe_utils::InterProcessPollingSubscriber::create_subscription( + autoware_utils::InterProcessPollingSubscriber::create_subscription( this, "~/input/reference_trajectory", 1); pub_status_ = create_publisher("~/output/validation_status", 1); @@ -152,8 +152,8 @@ void ControlValidator::on_predicted_trajectory(const Trajectory::ConstSharedPtr stop_watch.tic(); current_predicted_trajectory_ = msg; - current_reference_trajectory_ = sub_reference_traj_->takeData(); - current_kinematics_ = sub_kinematics_->takeData(); + current_reference_trajectory_ = sub_reference_traj_->take_data(); + current_kinematics_ = sub_kinematics_->take_data(); if (!is_data_ready()) return; diff --git a/control/autoware_control_validator/src/debug_marker.cpp b/control/autoware_control_validator/src/debug_marker.cpp index dfd073884ce89..21ec27a4e25c7 100644 --- a/control/autoware_control_validator/src/debug_marker.cpp +++ b/control/autoware_control_validator/src/debug_marker.cpp @@ -15,7 +15,7 @@ #include "autoware/control_validator/debug_marker.hpp" #include -#include +#include #include @@ -40,10 +40,10 @@ void ControlValidatorDebugMarkerPublisher::clear_markers() void ControlValidatorDebugMarkerPublisher::push_warning_msg( const geometry_msgs::msg::Pose & pose, const std::string & msg) { - visualization_msgs::msg::Marker marker = autoware::universe_utils::createDefaultMarker( + visualization_msgs::msg::Marker marker = autoware_utils::create_default_marker( "map", node_->get_clock()->now(), "warning_msg", 0, Marker::TEXT_VIEW_FACING, - autoware::universe_utils::createMarkerScale(0.0, 0.0, 1.0), - autoware::universe_utils::createMarkerColor(1.0, 0.1, 0.1, 0.999)); + autoware_utils::create_marker_scale(0.0, 0.0, 1.0), + autoware_utils::create_marker_color(1.0, 0.1, 0.1, 0.999)); marker.lifetime = rclcpp::Duration::from_seconds(0.2); marker.pose = pose; marker.text = msg; @@ -55,7 +55,7 @@ void ControlValidatorDebugMarkerPublisher::push_virtual_wall(const geometry_msgs const auto now = node_->get_clock()->now(); const auto stop_wall_marker = autoware::motion_utils::createStopVirtualWallMarker(pose, "control_validator", now, 0); - autoware::universe_utils::appendMarkerArray(stop_wall_marker, &marker_array_virtual_wall_, now); + autoware_utils::append_marker_array(stop_wall_marker, &marker_array_virtual_wall_, now); } void ControlValidatorDebugMarkerPublisher::publish() diff --git a/control/autoware_control_validator/src/utils.cpp b/control/autoware_control_validator/src/utils.cpp index 12f4ea70f2ca1..101af21cd83ad 100644 --- a/control/autoware_control_validator/src/utils.cpp +++ b/control/autoware_control_validator/src/utils.cpp @@ -17,7 +17,7 @@ #include "autoware/motion_utils/trajectory/conversion.hpp" #include "autoware/motion_utils/trajectory/interpolation.hpp" #include "autoware/motion_utils/trajectory/trajectory.hpp" -#include "autoware/universe_utils/geometry/geometry.hpp" +#include "autoware_utils/geometry/geometry.hpp" #include @@ -161,7 +161,7 @@ double calc_max_lateral_distance( align_trajectory_with_reference_trajectory(reference_trajectory, predicted_trajectory); double max_dist = 0; for (const auto & point : alined_predicted_trajectory.points) { - const auto p0 = autoware::universe_utils::getPoint(point); + const auto p0 = autoware_utils::get_point(point); // find nearest segment const size_t nearest_segment_idx = autoware::motion_utils::findNearestSegmentIndex(reference_trajectory.points, p0); diff --git a/control/autoware_joy_controller/include/autoware/joy_controller/joy_controller.hpp b/control/autoware_joy_controller/include/autoware/joy_controller/joy_controller.hpp index a4dcbc841696e..47d9c351454a1 100644 --- a/control/autoware_joy_controller/include/autoware/joy_controller/joy_controller.hpp +++ b/control/autoware_joy_controller/include/autoware/joy_controller/joy_controller.hpp @@ -17,7 +17,7 @@ #include "autoware/joy_controller/joy_converter/joy_converter_base.hpp" -#include +#include #include #include @@ -69,9 +69,8 @@ class AutowareJoyControllerNode : public rclcpp::Node rclcpp::CallbackGroup::SharedPtr callback_group_services_; // Subscriber - autoware::universe_utils::InterProcessPollingSubscriber sub_joy_{ - this, "input/joy"}; - autoware::universe_utils::InterProcessPollingSubscriber sub_odom_{ + autoware_utils::InterProcessPollingSubscriber sub_joy_{this, "input/joy"}; + autoware_utils::InterProcessPollingSubscriber sub_odom_{ this, "input/odometry"}; rclcpp::Time last_joy_received_time_; diff --git a/control/autoware_joy_controller/package.xml b/control/autoware_joy_controller/package.xml index b0a128be10ddb..f9f6d397f8ed6 100644 --- a/control/autoware_joy_controller/package.xml +++ b/control/autoware_joy_controller/package.xml @@ -17,7 +17,7 @@ autoware_cmake autoware_control_msgs - autoware_universe_utils + autoware_utils autoware_vehicle_msgs geometry_msgs joy diff --git a/control/autoware_joy_controller/src/joy_controller_node.cpp b/control/autoware_joy_controller/src/joy_controller_node.cpp index 378109eacc574..ed6a6a3524ea2 100644 --- a/control/autoware_joy_controller/src/joy_controller_node.cpp +++ b/control/autoware_joy_controller/src/joy_controller_node.cpp @@ -149,7 +149,7 @@ namespace autoware::joy_controller { void AutowareJoyControllerNode::onJoy() { - const auto msg = sub_joy_.takeData(); + const auto msg = sub_joy_.take_data(); if (!msg) { return; } @@ -200,7 +200,7 @@ void AutowareJoyControllerNode::onOdometry() return; } - const auto msg = sub_odom_.takeData(); + const auto msg = sub_odom_.take_data(); if (!msg) { return; } diff --git a/control/autoware_lane_departure_checker/include/autoware/lane_departure_checker/lane_departure_checker.hpp b/control/autoware_lane_departure_checker/include/autoware/lane_departure_checker/lane_departure_checker.hpp index 99aa8623de6c7..30afc92ce8e20 100644 --- a/control/autoware_lane_departure_checker/include/autoware/lane_departure_checker/lane_departure_checker.hpp +++ b/control/autoware_lane_departure_checker/include/autoware/lane_departure_checker/lane_departure_checker.hpp @@ -17,7 +17,7 @@ #include "autoware/lane_departure_checker/parameters.hpp" -#include +#include #include #include @@ -45,24 +45,24 @@ namespace autoware::lane_departure_checker { -using autoware::universe_utils::Segment2d; using autoware_internal_planning_msgs::msg::PathWithLaneId; +using autoware_utils::Segment2d; typedef boost::geometry::index::rtree> SegmentRtree; class LaneDepartureChecker { public: explicit LaneDepartureChecker( - std::shared_ptr time_keeper = - std::make_shared()) + std::shared_ptr time_keeper = + std::make_shared()) : time_keeper_(time_keeper) { } LaneDepartureChecker( const Param & param, const autoware::vehicle_info_utils::VehicleInfo & vehicle_info, - std::shared_ptr time_keeper = - std::make_shared()) + std::shared_ptr time_keeper = + std::make_shared()) : param_(param), vehicle_info_ptr_(std::make_shared(vehicle_info)), time_keeper_(time_keeper) @@ -78,13 +78,13 @@ class LaneDepartureChecker std::vector> getLaneletsFromPath( const lanelet::LaneletMapPtr lanelet_map_ptr, const PathWithLaneId & path) const; - std::optional getFusedLaneletPolygonForPath( + std::optional getFusedLaneletPolygonForPath( const lanelet::LaneletMapPtr lanelet_map_ptr, const PathWithLaneId & path) const; bool updateFusedLaneletPolygonForPath( const lanelet::LaneletMapPtr lanelet_map_ptr, const PathWithLaneId & path, std::vector & fused_lanelets_id, - std::optional & fused_lanelets_polygon) const; + std::optional & fused_lanelets_polygon) const; bool checkPathWillLeaveLane( const lanelet::LaneletMapPtr lanelet_map_ptr, const PathWithLaneId & path) const; @@ -92,12 +92,12 @@ class LaneDepartureChecker bool checkPathWillLeaveLane( const lanelet::LaneletMapPtr lanelet_map_ptr, const PathWithLaneId & path, std::vector & fused_lanelets_id, - std::optional & fused_lanelets_polygon) const; + std::optional & fused_lanelets_polygon) const; PathWithLaneId cropPointsOutsideOfLanes( const lanelet::LaneletMapPtr lanelet_map_ptr, const PathWithLaneId & path, const size_t end_index, std::vector & fused_lanelets_id, - std::optional & fused_lanelets_polygon); + std::optional & fused_lanelets_polygon); static bool isOutOfLane( const lanelet::ConstLanelets & candidate_lanelets, const LinearRing2d & vehicle_footprint); @@ -119,9 +119,9 @@ class LaneDepartureChecker const SegmentRtree & uncrossable_segments) const; lanelet::BasicPolygon2d toBasicPolygon2D(const LinearRing2d & footprint_hull) const; - autoware::universe_utils::Polygon2d toPolygon2D(const lanelet::BasicPolygon2d & poly) const; + autoware_utils::Polygon2d toPolygon2D(const lanelet::BasicPolygon2d & poly) const; - mutable std::shared_ptr time_keeper_; + mutable std::shared_ptr time_keeper_; }; } // namespace autoware::lane_departure_checker diff --git a/control/autoware_lane_departure_checker/include/autoware/lane_departure_checker/lane_departure_checker_node.hpp b/control/autoware_lane_departure_checker/include/autoware/lane_departure_checker/lane_departure_checker_node.hpp index 7aaf4816708bd..913dc94088be8 100644 --- a/control/autoware_lane_departure_checker/include/autoware/lane_departure_checker/lane_departure_checker_node.hpp +++ b/control/autoware_lane_departure_checker/include/autoware/lane_departure_checker/lane_departure_checker_node.hpp @@ -17,10 +17,10 @@ #include "autoware/lane_departure_checker/lane_departure_checker.hpp" #include "autoware/lane_departure_checker/parameters.hpp" -#include "autoware/universe_utils/ros/polling_subscriber.hpp" +#include "autoware_utils/ros/polling_subscriber.hpp" -#include -#include +#include +#include #include #include @@ -55,22 +55,19 @@ class LaneDepartureCheckerNode : public rclcpp::Node private: // Subscriber - autoware::universe_utils::InterProcessPollingSubscriber sub_odom_{ + autoware_utils::InterProcessPollingSubscriber sub_odom_{ this, "~/input/odometry"}; - autoware::universe_utils::InterProcessPollingSubscriber< - LaneletMapBin, autoware::universe_utils::polling_policy::Newest> + autoware_utils::InterProcessPollingSubscriber< + LaneletMapBin, autoware_utils::polling_policy::Newest> sub_lanelet_map_bin_{this, "~/input/lanelet_map_bin", rclcpp::QoS{1}.transient_local()}; - autoware::universe_utils::InterProcessPollingSubscriber sub_route_{ - this, "~/input/route"}; - autoware::universe_utils::InterProcessPollingSubscriber sub_reference_trajectory_{ + autoware_utils::InterProcessPollingSubscriber sub_route_{this, "~/input/route"}; + autoware_utils::InterProcessPollingSubscriber sub_reference_trajectory_{ this, "~/input/reference_trajectory"}; - autoware::universe_utils::InterProcessPollingSubscriber sub_predicted_trajectory_{ + autoware_utils::InterProcessPollingSubscriber sub_predicted_trajectory_{ this, "~/input/predicted_trajectory"}; - autoware::universe_utils::InterProcessPollingSubscriber< - autoware_adapi_v1_msgs::msg::OperationModeState> + autoware_utils::InterProcessPollingSubscriber sub_operation_mode_{this, "/api/operation_mode/state"}; - autoware::universe_utils::InterProcessPollingSubscriber< - autoware_vehicle_msgs::msg::ControlModeReport> + autoware_utils::InterProcessPollingSubscriber sub_control_mode_{this, "/vehicle/status/control_mode"}; // Data Buffer @@ -98,8 +95,8 @@ class LaneDepartureCheckerNode : public rclcpp::Node void onControlMode(const autoware_vehicle_msgs::msg::ControlModeReport::ConstSharedPtr msg); // Publisher - autoware::universe_utils::DebugPublisher debug_publisher_{this, "~/debug"}; - autoware::universe_utils::ProcessingTimePublisher processing_diag_publisher_{ + autoware_utils::DebugPublisher debug_publisher_{this, "~/debug"}; + autoware_utils::ProcessingTimePublisher processing_diag_publisher_{ this, "~/debug/processing_time_ms_diag"}; rclcpp::Publisher::SharedPtr processing_time_publisher_; diff --git a/control/autoware_lane_departure_checker/include/autoware/lane_departure_checker/parameters.hpp b/control/autoware_lane_departure_checker/include/autoware/lane_departure_checker/parameters.hpp index 425b032af425f..77d592a2574cd 100644 --- a/control/autoware_lane_departure_checker/include/autoware/lane_departure_checker/parameters.hpp +++ b/control/autoware_lane_departure_checker/include/autoware/lane_departure_checker/parameters.hpp @@ -15,8 +15,8 @@ #ifndef AUTOWARE__LANE_DEPARTURE_CHECKER__PARAMETERS_HPP_ #define AUTOWARE__LANE_DEPARTURE_CHECKER__PARAMETERS_HPP_ -#include -#include +#include +#include #include #include @@ -32,12 +32,12 @@ namespace autoware::lane_departure_checker { -using autoware::universe_utils::PoseDeviation; using autoware_planning_msgs::msg::LaneletRoute; using autoware_planning_msgs::msg::Trajectory; using autoware_planning_msgs::msg::TrajectoryPoint; +using autoware_utils::PoseDeviation; using TrajectoryPoints = std::vector; -using autoware::universe_utils::LinearRing2d; +using autoware_utils::LinearRing2d; struct Param { diff --git a/control/autoware_lane_departure_checker/include/autoware/lane_departure_checker/utils.hpp b/control/autoware_lane_departure_checker/include/autoware/lane_departure_checker/utils.hpp index 1f0a0ed6ff666..581bf4be497d3 100644 --- a/control/autoware_lane_departure_checker/include/autoware/lane_departure_checker/utils.hpp +++ b/control/autoware_lane_departure_checker/include/autoware/lane_departure_checker/utils.hpp @@ -15,8 +15,8 @@ #ifndef AUTOWARE__LANE_DEPARTURE_CHECKER__UTILS_HPP_ #define AUTOWARE__LANE_DEPARTURE_CHECKER__UTILS_HPP_ -#include -#include +#include +#include #include #include @@ -32,12 +32,12 @@ namespace autoware::lane_departure_checker::utils { -using autoware::universe_utils::LinearRing2d; -using autoware::universe_utils::MultiPoint2d; -using autoware::universe_utils::PoseDeviation; using autoware_internal_planning_msgs::msg::PathWithLaneId; using autoware_planning_msgs::msg::Trajectory; using autoware_planning_msgs::msg::TrajectoryPoint; +using autoware_utils::LinearRing2d; +using autoware_utils::MultiPoint2d; +using autoware_utils::PoseDeviation; using TrajectoryPoints = std::vector; /** diff --git a/control/autoware_lane_departure_checker/package.xml b/control/autoware_lane_departure_checker/package.xml index 5c06c43ffc0c8..4ef54ab4cf4ba 100644 --- a/control/autoware_lane_departure_checker/package.xml +++ b/control/autoware_lane_departure_checker/package.xml @@ -19,7 +19,7 @@ autoware_map_msgs autoware_motion_utils autoware_planning_msgs - autoware_universe_utils + autoware_utils autoware_vehicle_info_utils autoware_vehicle_msgs boost diff --git a/control/autoware_lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker.cpp b/control/autoware_lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker.cpp index 5d50ac0e2951b..01bbcb54d42d1 100644 --- a/control/autoware_lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker.cpp +++ b/control/autoware_lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker.cpp @@ -16,9 +16,9 @@ #include "autoware/lane_departure_checker/utils.hpp" -#include -#include -#include +#include +#include +#include #include @@ -29,11 +29,11 @@ #include #include -using autoware::universe_utils::LinearRing2d; -using autoware::universe_utils::LineString2d; -using autoware::universe_utils::MultiPoint2d; -using autoware::universe_utils::MultiPolygon2d; -using autoware::universe_utils::Point2d; +using autoware_utils::LinearRing2d; +using autoware_utils::LineString2d; +using autoware_utils::MultiPoint2d; +using autoware_utils::MultiPolygon2d; +using autoware_utils::Point2d; namespace { @@ -67,7 +67,7 @@ Output LaneDepartureChecker::update(const Input & input) { Output output{}; - autoware::universe_utils::StopWatch stop_watch; + autoware_utils::StopWatch stop_watch; output.trajectory_deviation = utils::calcTrajectoryDeviation( *input.reference_trajectory, input.current_odom->pose.pose, param_.ego_nearest_dist_threshold, @@ -127,7 +127,7 @@ Output LaneDepartureChecker::update(const Input & input) bool LaneDepartureChecker::checkPathWillLeaveLane( const lanelet::ConstLanelets & lanelets, const PathWithLaneId & path) const { - universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + autoware_utils::ScopedTimeTrack st(__func__, *time_keeper_); std::vector vehicle_footprints = utils::createVehicleFootprints(path, *vehicle_info_ptr_, param_.footprint_extra_margin); @@ -140,7 +140,7 @@ bool LaneDepartureChecker::willLeaveLane( const lanelet::ConstLanelets & candidate_lanelets, const std::vector & vehicle_footprints) const { - universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + autoware_utils::ScopedTimeTrack st(__func__, *time_keeper_); for (const auto & vehicle_footprint : vehicle_footprints) { if (isOutOfLane(candidate_lanelets, vehicle_footprint)) { @@ -154,7 +154,7 @@ bool LaneDepartureChecker::willLeaveLane( std::vector> LaneDepartureChecker::getLaneletsFromPath( const lanelet::LaneletMapPtr lanelet_map_ptr, const PathWithLaneId & path) const { - universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + autoware_utils::ScopedTimeTrack st(__func__, *time_keeper_); // Get Footprint Hull basic polygon std::vector vehicle_footprints = @@ -168,23 +168,22 @@ std::vector> LaneDepartureChecker::getLanele lanelet_map_ptr->laneletLayer, footprint_hull_basic_polygon, 0.0); } -std::optional -LaneDepartureChecker::getFusedLaneletPolygonForPath( +std::optional LaneDepartureChecker::getFusedLaneletPolygonForPath( const lanelet::LaneletMapPtr lanelet_map_ptr, const PathWithLaneId & path) const { - universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + autoware_utils::ScopedTimeTrack st(__func__, *time_keeper_); const auto lanelets_distance_pair = getLaneletsFromPath(lanelet_map_ptr, path); if (lanelets_distance_pair.empty()) return std::nullopt; // Fuse lanelets into a single BasicPolygon2d - autoware::universe_utils::MultiPolygon2d lanelet_unions; - autoware::universe_utils::MultiPolygon2d result; + autoware_utils::MultiPolygon2d lanelet_unions; + autoware_utils::MultiPolygon2d result; for (size_t i = 0; i < lanelets_distance_pair.size(); ++i) { const auto & route_lanelet = lanelets_distance_pair.at(i).second; const auto & p = route_lanelet.polygon2d().basicPolygon(); - autoware::universe_utils::Polygon2d poly = toPolygon2D(p); + autoware_utils::Polygon2d poly = toPolygon2D(p); boost::geometry::union_(lanelet_unions, poly, result); lanelet_unions = result; result.clear(); @@ -197,15 +196,15 @@ LaneDepartureChecker::getFusedLaneletPolygonForPath( bool LaneDepartureChecker::updateFusedLaneletPolygonForPath( const lanelet::LaneletMapPtr lanelet_map_ptr, const PathWithLaneId & path, std::vector & fused_lanelets_id, - std::optional & fused_lanelets_polygon) const + std::optional & fused_lanelets_polygon) const { - universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + autoware_utils::ScopedTimeTrack st(__func__, *time_keeper_); const auto lanelets_distance_pair = getLaneletsFromPath(lanelet_map_ptr, path); if (lanelets_distance_pair.empty()) return false; - autoware::universe_utils::MultiPolygon2d lanelet_unions; - autoware::universe_utils::MultiPolygon2d result; + autoware_utils::MultiPolygon2d lanelet_unions; + autoware_utils::MultiPolygon2d result; if (fused_lanelets_polygon) lanelet_unions.push_back(fused_lanelets_polygon.value()); @@ -218,7 +217,7 @@ bool LaneDepartureChecker::updateFusedLaneletPolygonForPath( if (id_exist) continue; const auto & p = route_lanelet.polygon2d().basicPolygon(); - autoware::universe_utils::Polygon2d poly = toPolygon2D(p); + autoware_utils::Polygon2d poly = toPolygon2D(p); boost::geometry::union_(lanelet_unions, poly, result); lanelet_unions = result; result.clear(); @@ -237,7 +236,7 @@ bool LaneDepartureChecker::updateFusedLaneletPolygonForPath( bool LaneDepartureChecker::checkPathWillLeaveLane( const lanelet::LaneletMapPtr lanelet_map_ptr, const PathWithLaneId & path) const { - universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + autoware_utils::ScopedTimeTrack st(__func__, *time_keeper_); // check if the footprint is not fully contained within the fused lanelets polygon const std::vector vehicle_footprints = @@ -254,9 +253,9 @@ bool LaneDepartureChecker::checkPathWillLeaveLane( bool LaneDepartureChecker::checkPathWillLeaveLane( const lanelet::LaneletMapPtr lanelet_map_ptr, const PathWithLaneId & path, std::vector & fused_lanelets_id, - std::optional & fused_lanelets_polygon) const + std::optional & fused_lanelets_polygon) const { - universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + autoware_utils::ScopedTimeTrack st(__func__, *time_keeper_); const std::vector vehicle_footprints = utils::createVehicleFootprints(path, *vehicle_info_ptr_, param_.footprint_extra_margin); @@ -286,9 +285,9 @@ bool LaneDepartureChecker::checkPathWillLeaveLane( PathWithLaneId LaneDepartureChecker::cropPointsOutsideOfLanes( const lanelet::LaneletMapPtr lanelet_map_ptr, const PathWithLaneId & path, const size_t end_index, std::vector & fused_lanelets_id, - std::optional & fused_lanelets_polygon) + std::optional & fused_lanelets_polygon) { - universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + autoware_utils::ScopedTimeTrack st(__func__, *time_keeper_); PathWithLaneId temp_path; @@ -303,7 +302,7 @@ PathWithLaneId LaneDepartureChecker::cropPointsOutsideOfLanes( utils::createVehicleFootprints(path, *vehicle_info_ptr_, param_.footprint_extra_margin); { - universe_utils::ScopedTimeTrack st2( + autoware_utils::ScopedTimeTrack st2( "check if footprint is within fused_lanelets_polygon", *time_keeper_); size_t idx = 0; @@ -365,7 +364,7 @@ bool LaneDepartureChecker::willCrossBoundary( const std::vector & vehicle_footprints, const SegmentRtree & uncrossable_segments) const { - universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + autoware_utils::ScopedTimeTrack st(__func__, *time_keeper_); for (const auto & footprint : vehicle_footprints) { std::vector intersection_result; @@ -389,14 +388,14 @@ lanelet::BasicPolygon2d LaneDepartureChecker::toBasicPolygon2D( return basic_polygon; } -autoware::universe_utils::Polygon2d LaneDepartureChecker::toPolygon2D( +autoware_utils::Polygon2d LaneDepartureChecker::toPolygon2D( const lanelet::BasicPolygon2d & poly) const { - autoware::universe_utils::Polygon2d polygon; + autoware_utils::Polygon2d polygon; auto & outer = polygon.outer(); for (const auto & p : poly) { - autoware::universe_utils::Point2d p2d(p.x(), p.y()); + autoware_utils::Point2d p2d(p.x(), p.y()); outer.push_back(p2d); } boost::geometry::correct(polygon); diff --git a/control/autoware_lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker_node.cpp b/control/autoware_lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker_node.cpp index 70ddabb0107ce..8b37aa890ba5e 100644 --- a/control/autoware_lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker_node.cpp +++ b/control/autoware_lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker_node.cpp @@ -14,14 +14,14 @@ #include "autoware/lane_departure_checker/lane_departure_checker_node.hpp" -#include -#include -#include #include #include #include #include #include +#include +#include +#include #include @@ -31,7 +31,7 @@ #include #include -using autoware::universe_utils::rad2deg; +using autoware_utils::rad2deg; namespace { @@ -234,17 +234,17 @@ bool LaneDepartureCheckerNode::isDataValid() void LaneDepartureCheckerNode::onTimer() { std::map processing_time_map; - autoware::universe_utils::StopWatch stop_watch; + autoware_utils::StopWatch stop_watch; stop_watch.tic("Total"); - current_odom_ = sub_odom_.takeData(); - route_ = sub_route_.takeData(); - reference_trajectory_ = sub_reference_trajectory_.takeData(); - predicted_trajectory_ = sub_predicted_trajectory_.takeData(); - operation_mode_ = sub_operation_mode_.takeData(); - control_mode_ = sub_control_mode_.takeData(); + current_odom_ = sub_odom_.take_data(); + route_ = sub_route_.take_data(); + reference_trajectory_ = sub_reference_trajectory_.take_data(); + predicted_trajectory_ = sub_predicted_trajectory_.take_data(); + operation_mode_ = sub_operation_mode_.take_data(); + control_mode_ = sub_control_mode_.take_data(); - const auto lanelet_map_bin_msg = sub_lanelet_map_bin_.takeData(); + const auto lanelet_map_bin_msg = sub_lanelet_map_bin_.take_data(); if (lanelet_map_bin_msg) { lanelet_map_ = std::make_shared(); lanelet::utils::conversion::fromBinMsg( @@ -450,9 +450,9 @@ void LaneDepartureCheckerNode::checkTrajectoryDeviation( visualization_msgs::msg::MarkerArray LaneDepartureCheckerNode::createMarkerArray() const { - using autoware::universe_utils::createDefaultMarker; - using autoware::universe_utils::createMarkerColor; - using autoware::universe_utils::createMarkerScale; + using autoware_utils::create_default_marker; + using autoware_utils::create_marker_color; + using autoware_utils::create_marker_scale; visualization_msgs::msg::MarkerArray marker_array; @@ -461,9 +461,9 @@ visualization_msgs::msg::MarkerArray LaneDepartureCheckerNode::createMarkerArray if (node_param_.visualize_lanelet) { // Route Lanelets { - auto marker = createDefaultMarker( + auto marker = create_default_marker( "map", this->now(), "route_lanelets", 0, visualization_msgs::msg::Marker::TRIANGLE_LIST, - createMarkerScale(1.0, 1.0, 1.0), createMarkerColor(0.0, 0.5, 0.5, 0.5)); + create_marker_scale(1.0, 1.0, 1.0), create_marker_color(0.0, 0.5, 0.5, 0.5)); for (const auto & lanelet : input_.route_lanelets) { std::vector triangles; @@ -482,9 +482,9 @@ visualization_msgs::msg::MarkerArray LaneDepartureCheckerNode::createMarkerArray // Candidate Lanelets { - auto marker = createDefaultMarker( + auto marker = create_default_marker( "map", this->now(), "candidate_lanelets", 0, visualization_msgs::msg::Marker::TRIANGLE_LIST, - createMarkerScale(1.0, 1.0, 1.0), createMarkerColor(1.0, 1.0, 0.0, 0.5)); + create_marker_scale(1.0, 1.0, 1.0), create_marker_color(1.0, 1.0, 0.0, 0.5)); for (const auto & lanelet : output_.candidate_lanelets) { std::vector triangles; @@ -505,10 +505,10 @@ visualization_msgs::msg::MarkerArray LaneDepartureCheckerNode::createMarkerArray if (output_.resampled_trajectory.size() >= 2) { // Line of resampled_trajectory { - auto marker = createDefaultMarker( + auto marker = create_default_marker( "map", this->now(), "resampled_trajectory_line", 0, - visualization_msgs::msg::Marker::LINE_STRIP, createMarkerScale(0.05, 0, 0), - createMarkerColor(1.0, 1.0, 1.0, 0.999)); + visualization_msgs::msg::Marker::LINE_STRIP, create_marker_scale(0.05, 0, 0), + create_marker_color(1.0, 1.0, 1.0, 0.999)); for (const auto & p : output_.resampled_trajectory) { marker.points.push_back(p.pose.position); @@ -520,10 +520,10 @@ visualization_msgs::msg::MarkerArray LaneDepartureCheckerNode::createMarkerArray // Points of resampled_trajectory { - auto marker = createDefaultMarker( + auto marker = create_default_marker( "map", this->now(), "resampled_trajectory_points", 0, - visualization_msgs::msg::Marker::SPHERE_LIST, createMarkerScale(0.1, 0.1, 0.1), - createMarkerColor(0.0, 1.0, 0.0, 0.999)); + visualization_msgs::msg::Marker::SPHERE_LIST, create_marker_scale(0.1, 0.1, 0.1), + create_marker_color(0.0, 1.0, 0.0, 0.999)); for (const auto & p : output_.resampled_trajectory) { marker.points.push_back(p.pose.position); @@ -536,9 +536,9 @@ visualization_msgs::msg::MarkerArray LaneDepartureCheckerNode::createMarkerArray // Vehicle Footprints { - const auto color_ok = createMarkerColor(0.0, 1.0, 0.0, 0.5); - const auto color_will_leave_lane = createMarkerColor(0.5, 0.5, 0.0, 0.5); - const auto color_is_out_of_lane = createMarkerColor(1.0, 0.0, 0.0, 0.5); + const auto color_ok = create_marker_color(0.0, 1.0, 0.0, 0.5); + const auto color_will_leave_lane = create_marker_color(0.5, 0.5, 0.0, 0.5); + const auto color_is_out_of_lane = create_marker_color(1.0, 0.0, 0.0, 0.5); auto color = color_ok; if (output_.will_leave_lane) { @@ -548,17 +548,17 @@ visualization_msgs::msg::MarkerArray LaneDepartureCheckerNode::createMarkerArray color = color_is_out_of_lane; } - auto marker = createDefaultMarker( + auto marker = create_default_marker( "map", this->now(), "vehicle_footprints", 0, visualization_msgs::msg::Marker::LINE_LIST, - createMarkerScale(0.05, 0, 0), color); + create_marker_scale(0.05, 0, 0), color); for (const auto & vehicle_footprint : output_.vehicle_footprints) { for (size_t i = 0; i < vehicle_footprint.size() - 1; ++i) { const auto p1 = vehicle_footprint.at(i); const auto p2 = vehicle_footprint.at(i + 1); - marker.points.push_back(autoware::universe_utils::toMsg(p1.to_3d(base_link_z))); - marker.points.push_back(autoware::universe_utils::toMsg(p2.to_3d(base_link_z))); + marker.points.push_back(autoware_utils::to_msg(p1.to_3d(base_link_z))); + marker.points.push_back(autoware_utils::to_msg(p2.to_3d(base_link_z))); } } diff --git a/control/autoware_lane_departure_checker/src/lane_departure_checker_node/parameters.cpp b/control/autoware_lane_departure_checker/src/lane_departure_checker_node/parameters.cpp index f3aa23275e35c..4f815499872d7 100644 --- a/control/autoware_lane_departure_checker/src/lane_departure_checker_node/parameters.cpp +++ b/control/autoware_lane_departure_checker/src/lane_departure_checker_node/parameters.cpp @@ -14,46 +14,48 @@ #include "autoware/lane_departure_checker/parameters.hpp" -#include +#include #include #include namespace autoware::lane_departure_checker { -using autoware::universe_utils::getOrDeclareParameter; +using autoware_utils::get_or_declare_parameter; Param Param::init(rclcpp::Node & node) { Param p; - p.footprint_margin_scale = getOrDeclareParameter(node, "footprint_margin_scale"); - p.footprint_extra_margin = getOrDeclareParameter(node, "footprint_extra_margin"); - p.resample_interval = getOrDeclareParameter(node, "resample_interval"); - p.max_deceleration = getOrDeclareParameter(node, "max_deceleration"); - p.delay_time = getOrDeclareParameter(node, "delay_time"); - p.max_lateral_deviation = getOrDeclareParameter(node, "max_lateral_deviation"); - p.max_longitudinal_deviation = getOrDeclareParameter(node, "max_longitudinal_deviation"); - p.max_yaw_deviation_deg = getOrDeclareParameter(node, "max_yaw_deviation_deg"); - p.min_braking_distance = getOrDeclareParameter(node, "min_braking_distance"); - p.ego_nearest_dist_threshold = getOrDeclareParameter(node, "ego_nearest_dist_threshold"); - p.ego_nearest_yaw_threshold = getOrDeclareParameter(node, "ego_nearest_yaw_threshold"); + p.footprint_margin_scale = get_or_declare_parameter(node, "footprint_margin_scale"); + p.footprint_extra_margin = get_or_declare_parameter(node, "footprint_extra_margin"); + p.resample_interval = get_or_declare_parameter(node, "resample_interval"); + p.max_deceleration = get_or_declare_parameter(node, "max_deceleration"); + p.delay_time = get_or_declare_parameter(node, "delay_time"); + p.max_lateral_deviation = get_or_declare_parameter(node, "max_lateral_deviation"); + p.max_longitudinal_deviation = + get_or_declare_parameter(node, "max_longitudinal_deviation"); + p.max_yaw_deviation_deg = get_or_declare_parameter(node, "max_yaw_deviation_deg"); + p.min_braking_distance = get_or_declare_parameter(node, "min_braking_distance"); + p.ego_nearest_dist_threshold = + get_or_declare_parameter(node, "ego_nearest_dist_threshold"); + p.ego_nearest_yaw_threshold = get_or_declare_parameter(node, "ego_nearest_yaw_threshold"); return p; } NodeParam NodeParam::init(rclcpp::Node & node) { NodeParam p; - p.will_out_of_lane_checker = getOrDeclareParameter(node, "will_out_of_lane_checker"); - p.out_of_lane_checker = getOrDeclareParameter(node, "out_of_lane_checker"); - p.boundary_departure_checker = getOrDeclareParameter(node, "boundary_departure_checker"); - p.update_rate = getOrDeclareParameter(node, "update_rate"); - p.visualize_lanelet = getOrDeclareParameter(node, "visualize_lanelet"); - p.include_right_lanes = getOrDeclareParameter(node, "include_right_lanes"); - p.include_left_lanes = getOrDeclareParameter(node, "include_left_lanes"); - p.include_opposite_lanes = getOrDeclareParameter(node, "include_opposite_lanes"); - p.include_conflicting_lanes = getOrDeclareParameter(node, "include_conflicting_lanes"); + p.will_out_of_lane_checker = get_or_declare_parameter(node, "will_out_of_lane_checker"); + p.out_of_lane_checker = get_or_declare_parameter(node, "out_of_lane_checker"); + p.boundary_departure_checker = get_or_declare_parameter(node, "boundary_departure_checker"); + p.update_rate = get_or_declare_parameter(node, "update_rate"); + p.visualize_lanelet = get_or_declare_parameter(node, "visualize_lanelet"); + p.include_right_lanes = get_or_declare_parameter(node, "include_right_lanes"); + p.include_left_lanes = get_or_declare_parameter(node, "include_left_lanes"); + p.include_opposite_lanes = get_or_declare_parameter(node, "include_opposite_lanes"); + p.include_conflicting_lanes = get_or_declare_parameter(node, "include_conflicting_lanes"); p.boundary_types_to_detect = - getOrDeclareParameter>(node, "boundary_types_to_detect"); + get_or_declare_parameter>(node, "boundary_types_to_detect"); return p; } } // namespace autoware::lane_departure_checker diff --git a/control/autoware_lane_departure_checker/src/lane_departure_checker_node/utils.cpp b/control/autoware_lane_departure_checker/src/lane_departure_checker_node/utils.cpp index 394d07833c261..2df7a69de90bc 100644 --- a/control/autoware_lane_departure_checker/src/lane_departure_checker_node/utils.cpp +++ b/control/autoware_lane_departure_checker/src/lane_departure_checker_node/utils.cpp @@ -15,7 +15,7 @@ #include "autoware/lane_departure_checker/utils.hpp" #include -#include +#include #include @@ -66,7 +66,7 @@ TrajectoryPoints cutTrajectory(const TrajectoryPoints & trajectory, const double TrajectoryPoints cut; double total_length = 0.0; - auto last_point = autoware::universe_utils::fromMsg(trajectory.front().pose.position); + auto last_point = autoware_utils::from_msg(trajectory.front().pose.position); auto end_it = std::next(trajectory.cbegin()); for (; end_it != trajectory.cend(); ++end_it) { const auto remain_distance = length - total_length; @@ -77,7 +77,7 @@ TrajectoryPoints cutTrajectory(const TrajectoryPoints & trajectory, const double } const auto & new_pose = end_it->pose; - const auto new_point = autoware::universe_utils::fromMsg(new_pose.position); + const auto new_point = autoware_utils::from_msg(new_pose.position); const auto points_distance = boost::geometry::distance(last_point.to_2d(), new_point.to_2d()); // Require interpolation @@ -112,11 +112,11 @@ TrajectoryPoints resampleTrajectory(const Trajectory & trajectory, const double TrajectoryPoints resampled; resampled.push_back(trajectory.points.front()); - auto prev_point = autoware::universe_utils::fromMsg(trajectory.points.front().pose.position); + auto prev_point = autoware_utils::from_msg(trajectory.points.front().pose.position); for (size_t i = 1; i < trajectory.points.size() - 1; ++i) { const auto & traj_point = trajectory.points.at(i); - const auto next_point = autoware::universe_utils::fromMsg(traj_point.pose.position); + const auto next_point = autoware_utils::from_msg(traj_point.pose.position); if (boost::geometry::distance(prev_point.to_2d(), next_point.to_2d()) >= interval) { resampled.push_back(traj_point); @@ -143,8 +143,8 @@ std::vector createVehicleFootprints( // Create vehicle footprint on each TrajectoryPoint std::vector vehicle_footprints; for (const auto & p : trajectory) { - vehicle_footprints.push_back(autoware::universe_utils::transformVector( - local_vehicle_footprint, autoware::universe_utils::pose2transform(p.pose))); + vehicle_footprints.push_back(autoware_utils::transform_vector( + local_vehicle_footprint, autoware_utils::pose2transform(p.pose))); } return vehicle_footprints; @@ -160,8 +160,8 @@ std::vector createVehicleFootprints( // Create vehicle footprint on each Path point std::vector vehicle_footprints; for (const auto & p : path.points) { - vehicle_footprints.push_back(autoware::universe_utils::transformVector( - local_vehicle_footprint, autoware::universe_utils::pose2transform(p.point.pose))); + vehicle_footprints.push_back(autoware_utils::transform_vector( + local_vehicle_footprint, autoware_utils::pose2transform(p.point.pose))); } return vehicle_footprints; @@ -230,7 +230,7 @@ PoseDeviation calcTrajectoryDeviation( { const auto nearest_idx = autoware::motion_utils::findFirstNearestIndexWithSoftConstraints( trajectory.points, pose, dist_threshold, yaw_threshold); - return autoware::universe_utils::calcPoseDeviation(trajectory.points.at(nearest_idx).pose, pose); + return autoware_utils::calc_pose_deviation(trajectory.points.at(nearest_idx).pose, pose); } double calcMaxSearchLengthForBoundaries( diff --git a/control/autoware_lane_departure_checker/test/test_calc_trajectory_deviation.cpp b/control/autoware_lane_departure_checker/test/test_calc_trajectory_deviation.cpp index ccb85d1a0fec1..df011c9e26c44 100644 --- a/control/autoware_lane_departure_checker/test/test_calc_trajectory_deviation.cpp +++ b/control/autoware_lane_departure_checker/test/test_calc_trajectory_deviation.cpp @@ -21,9 +21,9 @@ #include #include -using autoware::universe_utils::PoseDeviation; using autoware_planning_msgs::msg::Trajectory; using autoware_planning_msgs::msg::TrajectoryPoint; +using autoware_utils::PoseDeviation; namespace { diff --git a/control/autoware_lane_departure_checker/test/test_create_vehicle_footprints.cpp b/control/autoware_lane_departure_checker/test/test_create_vehicle_footprints.cpp index 6f5e9a2c3d444..3c567f7b714fc 100644 --- a/control/autoware_lane_departure_checker/test/test_create_vehicle_footprints.cpp +++ b/control/autoware_lane_departure_checker/test/test_create_vehicle_footprints.cpp @@ -21,10 +21,10 @@ #include #include -using autoware::universe_utils::LinearRing2d; using autoware_internal_planning_msgs::msg::PathPointWithLaneId; using autoware_internal_planning_msgs::msg::PathWithLaneId; using autoware_planning_msgs::msg::TrajectoryPoint; +using autoware_utils::LinearRing2d; using geometry_msgs::msg::PoseWithCovariance; using TrajectoryPoints = std::vector; diff --git a/control/autoware_lane_departure_checker/test/test_create_vehicle_passing_areas.cpp b/control/autoware_lane_departure_checker/test/test_create_vehicle_passing_areas.cpp index 40a4db4024094..0c7ec0176d188 100644 --- a/control/autoware_lane_departure_checker/test/test_create_vehicle_passing_areas.cpp +++ b/control/autoware_lane_departure_checker/test/test_create_vehicle_passing_areas.cpp @@ -15,7 +15,7 @@ #include "autoware/lane_departure_checker/utils.hpp" #include -#include +#include #include #include @@ -27,8 +27,8 @@ #include using autoware::lane_departure_checker::utils::createVehiclePassingAreas; -using autoware::universe_utils::LinearRing2d; -using autoware::universe_utils::Point2d; +using autoware_utils::LinearRing2d; +using autoware_utils::Point2d; class CreateVehiclePassingAreasTest : public ::testing::Test { diff --git a/control/autoware_mpc_lateral_controller/include/autoware/mpc_lateral_controller/mpc_trajectory.hpp b/control/autoware_mpc_lateral_controller/include/autoware/mpc_lateral_controller/mpc_trajectory.hpp index b761aa9f19d23..9df22cf020c2a 100644 --- a/control/autoware_mpc_lateral_controller/include/autoware/mpc_lateral_controller/mpc_trajectory.hpp +++ b/control/autoware_mpc_lateral_controller/include/autoware/mpc_lateral_controller/mpc_trajectory.hpp @@ -15,7 +15,7 @@ #ifndef AUTOWARE__MPC_LATERAL_CONTROLLER__MPC_TRAJECTORY_HPP_ #define AUTOWARE__MPC_LATERAL_CONTROLLER__MPC_TRAJECTORY_HPP_ -#include "autoware/universe_utils/geometry/geometry.hpp" +#include "autoware_utils/geometry/geometry.hpp" #include "geometry_msgs/msg/point.hpp" @@ -117,7 +117,7 @@ class MPCTrajectory point.pose.position.x = x.at(i); point.pose.position.y = y.at(i); point.pose.position.z = z.at(i); - point.pose.orientation = autoware::universe_utils::createQuaternionFromYaw(yaw.at(i)); + point.pose.orientation = autoware_utils::create_quaternion_from_yaw(yaw.at(i)); point.longitudinal_velocity_mps = vx.at(i); points.push_back(point); } diff --git a/control/autoware_mpc_lateral_controller/package.xml b/control/autoware_mpc_lateral_controller/package.xml index b818b24786a16..55ac4feb7d2a0 100644 --- a/control/autoware_mpc_lateral_controller/package.xml +++ b/control/autoware_mpc_lateral_controller/package.xml @@ -25,7 +25,7 @@ autoware_osqp_interface autoware_planning_msgs autoware_trajectory_follower_base - autoware_universe_utils + autoware_utils autoware_vehicle_info_utils autoware_vehicle_msgs diagnostic_msgs diff --git a/control/autoware_mpc_lateral_controller/src/mpc.cpp b/control/autoware_mpc_lateral_controller/src/mpc.cpp index 49bf90b89bc90..5692cf504448b 100644 --- a/control/autoware_mpc_lateral_controller/src/mpc.cpp +++ b/control/autoware_mpc_lateral_controller/src/mpc.cpp @@ -17,7 +17,7 @@ #include "autoware/interpolation/linear_interpolation.hpp" #include "autoware/motion_utils/trajectory/trajectory.hpp" #include "autoware/mpc_lateral_controller/mpc_utils.hpp" -#include "autoware/universe_utils/math/unit_conversion.hpp" +#include "autoware_utils/math/unit_conversion.hpp" #include "rclcpp/rclcpp.hpp" #include @@ -31,9 +31,9 @@ namespace autoware::motion::control::mpc_lateral_controller { -using autoware::universe_utils::calcDistance2d; -using autoware::universe_utils::normalizeRadian; -using autoware::universe_utils::rad2deg; +using autoware_utils::calc_distance2d; +using autoware_utils::normalize_radian; +using autoware_utils::rad2deg; MPC::MPC(rclcpp::Node & node) { @@ -301,7 +301,7 @@ std::pair MPC::getData( // get data data.steer = static_cast(current_steer.steering_tire_angle); data.lateral_err = MPCUtils::calcLateralError(current_pose, data.nearest_pose); - data.yaw_err = normalizeRadian( + data.yaw_err = normalize_radian( tf2::getYaw(current_pose.orientation) - tf2::getYaw(data.nearest_pose.orientation)); // get predicted steer @@ -526,7 +526,7 @@ MPCMatrix MPC::generateMPCMatrix( // get reference input (feed-forward) m_vehicle_model_ptr->setCurvature(ref_smooth_k); m_vehicle_model_ptr->calculateReferenceInput(Uref); - if (std::fabs(Uref(0, 0)) < autoware::universe_utils::deg2rad(m_param.zero_ff_steer_deg)) { + if (std::fabs(Uref(0, 0)) < autoware_utils::deg2rad(m_param.zero_ff_steer_deg)) { Uref(0, 0) = 0.0; // ignore curvature noise } m.Uref_ex.block(i * DIM_U, 0, DIM_U, 1) = Uref; diff --git a/control/autoware_mpc_lateral_controller/src/mpc_lateral_controller.cpp b/control/autoware_mpc_lateral_controller/src/mpc_lateral_controller.cpp index 86d0c0143ed34..947db6ebb0b7e 100644 --- a/control/autoware_mpc_lateral_controller/src/mpc_lateral_controller.cpp +++ b/control/autoware_mpc_lateral_controller/src/mpc_lateral_controller.cpp @@ -461,7 +461,7 @@ bool MpcLateralController::isStoppedState() const auto covered_distance = 0.0; for (auto i = nearest + 1; i < m_current_trajectory.points.size(); ++i) { min_vel = std::min(min_vel, m_current_trajectory.points.at(i).longitudinal_velocity_mps); - covered_distance += autoware::universe_utils::calcDistance2d( + covered_distance += autoware_utils::calc_distance2d( m_current_trajectory.points.at(i - 1).pose, m_current_trajectory.points.at(i).pose); if (covered_distance > distance_margin) break; } @@ -670,7 +670,7 @@ bool MpcLateralController::isTrajectoryShapeChanged() const // TODO(Horibe): update implementation to check trajectory shape around ego vehicle. // Now temporally check the goal position. for (const auto & trajectory : m_trajectory_buffer) { - const auto change_distance = autoware::universe_utils::calcDistance2d( + const auto change_distance = autoware_utils::calc_distance2d( trajectory.points.back().pose, m_current_trajectory.points.back().pose); if (change_distance > m_new_traj_end_dist) { return true; diff --git a/control/autoware_mpc_lateral_controller/src/mpc_utils.cpp b/control/autoware_mpc_lateral_controller/src/mpc_utils.cpp index e8bc981a8ade8..f46d8e3fcbedf 100644 --- a/control/autoware_mpc_lateral_controller/src/mpc_utils.cpp +++ b/control/autoware_mpc_lateral_controller/src/mpc_utils.cpp @@ -17,8 +17,8 @@ #include "autoware/interpolation/linear_interpolation.hpp" #include "autoware/interpolation/spline_interpolation.hpp" #include "autoware/motion_utils/trajectory/trajectory.hpp" -#include "autoware/universe_utils/geometry/geometry.hpp" -#include "autoware/universe_utils/math/normalization.hpp" +#include "autoware_utils/geometry/geometry.hpp" +#include "autoware_utils/math/normalization.hpp" #include #include @@ -44,9 +44,9 @@ double calcLongitudinalOffset( namespace MPCUtils { -using autoware::universe_utils::calcDistance2d; -using autoware::universe_utils::createQuaternionFromYaw; -using autoware::universe_utils::normalizeRadian; +using autoware_utils::calc_distance2d; +using autoware_utils::create_quaternion_from_yaw; +using autoware_utils::normalize_radian; double calcDistance2d(const MPCTrajectory & trajectory, const size_t idx1, const size_t idx2) { @@ -67,7 +67,7 @@ void convertEulerAngleToMonotonic(std::vector & angle_vector) { for (uint i = 1; i < angle_vector.size(); ++i) { const double da = angle_vector.at(i) - angle_vector.at(i - 1); - angle_vector.at(i) = angle_vector.at(i - 1) + normalizeRadian(da); + angle_vector.at(i) = angle_vector.at(i - 1) + normalize_radian(da); } } @@ -242,7 +242,7 @@ std::vector calcTrajectoryCurvature( p2.y = traj.y.at(curr_idx); p3.y = traj.y.at(next_idx); try { - curvature_vec.at(curr_idx) = autoware::universe_utils::calcCurvature(p1, p2, p3); + curvature_vec.at(curr_idx) = autoware_utils::calc_curvature(p1, p2, p3); } catch (...) { std::cerr << "[MPC] 2 points are too close to calculate curvature." << std::endl; curvature_vec.at(curr_idx) = 0.0; @@ -283,7 +283,7 @@ Trajectory convertToAutowareTrajectory(const MPCTrajectory & input) p.pose.position.x = input.x.at(i); p.pose.position.y = input.y.at(i); p.pose.position.z = input.z.at(i); - p.pose.orientation = autoware::universe_utils::createQuaternionFromYaw(input.yaw.at(i)); + p.pose.orientation = autoware_utils::create_quaternion_from_yaw(input.yaw.at(i)); p.longitudinal_velocity_mps = static_cast(input.vx.at(i)); output.points.push_back(p); @@ -355,7 +355,7 @@ bool calcNearestPoseInterp( if (traj.size() == 1) { nearest_pose->position.x = traj.x.at(*nearest_index); nearest_pose->position.y = traj.y.at(*nearest_index); - nearest_pose->orientation = createQuaternionFromYaw(traj.yaw.at(*nearest_index)); + nearest_pose->orientation = create_quaternion_from_yaw(traj.yaw.at(*nearest_index)); *nearest_time = traj.relative_time.at(*nearest_index); return true; } @@ -390,13 +390,12 @@ bool calcNearestPoseInterp( geometry_msgs::msg::Point prev_traj_point; prev_traj_point.x = traj.x.at(prev); prev_traj_point.y = traj.y.at(prev); - const double traj_seg_length = - autoware::universe_utils::calcDistance2d(prev_traj_point, next_traj_point); + const double traj_seg_length = autoware_utils::calc_distance2d(prev_traj_point, next_traj_point); /* if distance between two points are too close */ if (traj_seg_length < 1.0E-5) { nearest_pose->position.x = traj.x.at(*nearest_index); nearest_pose->position.y = traj.y.at(*nearest_index); - nearest_pose->orientation = createQuaternionFromYaw(traj.yaw.at(*nearest_index)); + nearest_pose->orientation = create_quaternion_from_yaw(traj.yaw.at(*nearest_index)); *nearest_time = traj.relative_time.at(*nearest_index); return true; } @@ -407,9 +406,9 @@ bool calcNearestPoseInterp( 0.0, 1.0); nearest_pose->position.x = (1 - ratio) * traj.x.at(prev) + ratio * traj.x.at(next); nearest_pose->position.y = (1 - ratio) * traj.y.at(prev) + ratio * traj.y.at(next); - const double tmp_yaw_err = normalizeRadian(traj.yaw.at(prev) - traj.yaw.at(next)); - const double nearest_yaw = normalizeRadian(traj.yaw.at(next) + (1 - ratio) * tmp_yaw_err); - nearest_pose->orientation = createQuaternionFromYaw(nearest_yaw); + const double tmp_yaw_err = normalize_radian(traj.yaw.at(prev) - traj.yaw.at(next)); + const double nearest_yaw = normalize_radian(traj.yaw.at(next) + (1 - ratio) * tmp_yaw_err); + nearest_pose->orientation = create_quaternion_from_yaw(nearest_yaw); *nearest_time = (1 - ratio) * traj.relative_time.at(prev) + ratio * traj.relative_time.at(next); return true; } @@ -426,7 +425,7 @@ double calcStopDistance(const Trajectory & current_trajectory, const int origin) for (int i = origin + 1; i < static_cast(current_trajectory.points.size()) - 1; ++i) { const auto & p0 = current_trajectory.points.at(i); const auto & p1 = current_trajectory.points.at(i - 1); - stop_dist += calcDistance2d(p0, p1); + stop_dist += calc_distance2d(p0, p1); if (std::fabs(p0.longitudinal_velocity_mps) < zero_velocity) { break; } @@ -441,7 +440,7 @@ double calcStopDistance(const Trajectory & current_trajectory, const int origin) if (std::fabs(p0.longitudinal_velocity_mps) > zero_velocity) { break; } - stop_dist -= calcDistance2d(p0, p1); + stop_dist -= calc_distance2d(p0, p1); } return stop_dist; } @@ -462,7 +461,7 @@ void extendTrajectoryInYawDirection( const double dt = interval / extend_vel; const size_t num_extended_point = static_cast(extend_dist / interval); for (size_t i = 0; i < num_extended_point; ++i) { - extended_pose = autoware::universe_utils::calcOffsetPose(extended_pose, x_offset, 0.0, 0.0); + extended_pose = autoware_utils::calc_offset_pose(extended_pose, x_offset, 0.0, 0.0); traj.push_back( extended_pose.position.x, extended_pose.position.y, extended_pose.position.z, traj.yaw.back(), extend_vel, traj.k.back(), traj.smooth_k.back(), traj.relative_time.back() + dt); diff --git a/control/autoware_obstacle_collision_checker/include/autoware/obstacle_collision_checker/obstacle_collision_checker.hpp b/control/autoware_obstacle_collision_checker/include/autoware/obstacle_collision_checker/obstacle_collision_checker.hpp index ac1caacfaf9b1..def21750dcbd7 100644 --- a/control/autoware_obstacle_collision_checker/include/autoware/obstacle_collision_checker/obstacle_collision_checker.hpp +++ b/control/autoware_obstacle_collision_checker/include/autoware/obstacle_collision_checker/obstacle_collision_checker.hpp @@ -15,7 +15,7 @@ #ifndef AUTOWARE__OBSTACLE_COLLISION_CHECKER__OBSTACLE_COLLISION_CHECKER_HPP_ #define AUTOWARE__OBSTACLE_COLLISION_CHECKER__OBSTACLE_COLLISION_CHECKER_HPP_ -#include +#include #include #include @@ -33,7 +33,7 @@ namespace autoware::obstacle_collision_checker { -using autoware::universe_utils::LinearRing2d; +using autoware_utils::LinearRing2d; struct Param { diff --git a/control/autoware_obstacle_collision_checker/include/autoware/obstacle_collision_checker/obstacle_collision_checker_node.hpp b/control/autoware_obstacle_collision_checker/include/autoware/obstacle_collision_checker/obstacle_collision_checker_node.hpp index 86fe8e06872b9..81e7e0c353868 100644 --- a/control/autoware_obstacle_collision_checker/include/autoware/obstacle_collision_checker/obstacle_collision_checker_node.hpp +++ b/control/autoware_obstacle_collision_checker/include/autoware/obstacle_collision_checker/obstacle_collision_checker_node.hpp @@ -17,11 +17,11 @@ #include "autoware/obstacle_collision_checker/obstacle_collision_checker.hpp" -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include #include #include #include @@ -48,8 +48,8 @@ class ObstacleCollisionCheckerNode : public rclcpp::Node private: // Subscriber - std::shared_ptr self_pose_listener_; - std::shared_ptr transform_listener_; + std::shared_ptr self_pose_listener_; + std::shared_ptr transform_listener_; rclcpp::Subscription::SharedPtr sub_obstacle_pointcloud_; rclcpp::Subscription::SharedPtr sub_reference_trajectory_; @@ -73,8 +73,8 @@ class ObstacleCollisionCheckerNode : public rclcpp::Node void on_odom(const nav_msgs::msg::Odometry::SharedPtr msg); // Publisher - std::shared_ptr debug_publisher_; - std::shared_ptr time_publisher_; + std::shared_ptr debug_publisher_; + std::shared_ptr time_publisher_; // Timer rclcpp::TimerBase::SharedPtr timer_; diff --git a/control/autoware_obstacle_collision_checker/package.xml b/control/autoware_obstacle_collision_checker/package.xml index 3472882c92982..e81cdc162be23 100644 --- a/control/autoware_obstacle_collision_checker/package.xml +++ b/control/autoware_obstacle_collision_checker/package.xml @@ -19,7 +19,7 @@ autoware_cmake autoware_planning_msgs - autoware_universe_utils + autoware_utils autoware_vehicle_info_utils boost diagnostic_updater diff --git a/control/autoware_obstacle_collision_checker/src/debug.cpp b/control/autoware_obstacle_collision_checker/src/debug.cpp index 20ef534939649..4cf92c033d7f1 100644 --- a/control/autoware_obstacle_collision_checker/src/debug.cpp +++ b/control/autoware_obstacle_collision_checker/src/debug.cpp @@ -14,25 +14,25 @@ #include "autoware/obstacle_collision_checker/debug.hpp" -#include +#include namespace autoware::obstacle_collision_checker { visualization_msgs::msg::MarkerArray create_marker_array( const Output & output, const double base_link_z, const rclcpp::Time & now) { - using autoware::universe_utils::createDefaultMarker; - using autoware::universe_utils::createMarkerColor; - using autoware::universe_utils::createMarkerScale; + using autoware_utils::create_default_marker; + using autoware_utils::create_marker_color; + using autoware_utils::create_marker_scale; visualization_msgs::msg::MarkerArray marker_array; if (output.resampled_trajectory.points.size() >= 2) { // Line of resampled_trajectory { - auto marker = createDefaultMarker( + auto marker = create_default_marker( "map", now, "resampled_trajectory_line", 0, visualization_msgs::msg::Marker::LINE_STRIP, - createMarkerScale(0.05, 0, 0), createMarkerColor(1.0, 1.0, 1.0, 0.999)); + create_marker_scale(0.05, 0, 0), create_marker_color(1.0, 1.0, 1.0, 0.999)); for (const auto & p : output.resampled_trajectory.points) { marker.points.push_back(p.pose.position); @@ -44,9 +44,9 @@ visualization_msgs::msg::MarkerArray create_marker_array( // Points of resampled_trajectory { - auto marker = createDefaultMarker( + auto marker = create_default_marker( "map", now, "resampled_trajectory_points", 0, visualization_msgs::msg::Marker::SPHERE_LIST, - createMarkerScale(0.1, 0.1, 0.1), createMarkerColor(0.0, 1.0, 0.0, 0.999)); + create_marker_scale(0.1, 0.1, 0.1), create_marker_color(0.0, 1.0, 0.0, 0.999)); for (const auto & p : output.resampled_trajectory.points) { marker.points.push_back(p.pose.position); @@ -59,25 +59,25 @@ visualization_msgs::msg::MarkerArray create_marker_array( // Vehicle Footprints { - const auto color_ok = createMarkerColor(0.0, 1.0, 0.0, 0.5); - const auto color_will_collide = createMarkerColor(1.0, 0.0, 0.0, 0.5); + const auto color_ok = create_marker_color(0.0, 1.0, 0.0, 0.5); + const auto color_will_collide = create_marker_color(1.0, 0.0, 0.0, 0.5); auto color = color_ok; if (output.will_collide) { color = color_will_collide; } - auto marker = createDefaultMarker( + auto marker = create_default_marker( "map", now, "vehicle_footprints", 0, visualization_msgs::msg::Marker::LINE_LIST, - createMarkerScale(0.05, 0, 0), color); + create_marker_scale(0.05, 0, 0), color); for (const auto & vehicle_footprint : output.vehicle_footprints) { for (size_t i = 0; i < vehicle_footprint.size() - 1; ++i) { const auto & p1 = vehicle_footprint.at(i); const auto & p2 = vehicle_footprint.at(i + 1); - marker.points.push_back(autoware::universe_utils::toMsg(p1.to_3d(base_link_z))); - marker.points.push_back(autoware::universe_utils::toMsg(p2.to_3d(base_link_z))); + marker.points.push_back(autoware_utils::to_msg(p1.to_3d(base_link_z))); + marker.points.push_back(autoware_utils::to_msg(p2.to_3d(base_link_z))); } } diff --git a/control/autoware_obstacle_collision_checker/src/obstacle_collision_checker.cpp b/control/autoware_obstacle_collision_checker/src/obstacle_collision_checker.cpp index bc8cb373a60df..997ce4661153d 100644 --- a/control/autoware_obstacle_collision_checker/src/obstacle_collision_checker.cpp +++ b/control/autoware_obstacle_collision_checker/src/obstacle_collision_checker.cpp @@ -14,10 +14,10 @@ #include "autoware/obstacle_collision_checker/obstacle_collision_checker.hpp" -#include -#include -#include -#include +#include +#include +#include +#include #include #include #include @@ -79,7 +79,7 @@ namespace autoware::obstacle_collision_checker Output check_for_collisions(const Input & input) { Output output; - autoware::universe_utils::StopWatch stop_watch; + autoware_utils::StopWatch stop_watch; // resample trajectory by braking distance constexpr double min_velocity = 0.01; @@ -122,7 +122,7 @@ autoware_planning_msgs::msg::Trajectory resample_trajectory( const auto & point = trajectory.points.at(i); const auto distance = - autoware::universe_utils::calcDistance2d(resampled.points.back(), point.pose.position); + autoware_utils::calc_distance2d(resampled.points.back(), point.pose.position); if (distance > interval) { resampled.points.push_back(point); } @@ -143,8 +143,8 @@ autoware_planning_msgs::msg::Trajectory cut_trajectory( for (size_t i = 1; i < trajectory.points.size(); ++i) { const auto & point = trajectory.points.at(i); - const auto p1 = autoware::universe_utils::fromMsg(cut.points.back().pose.position); - const auto p2 = autoware::universe_utils::fromMsg(point.pose.position); + const auto p1 = autoware_utils::from_msg(cut.points.back().pose.position); + const auto p2 = autoware_utils::from_msg(point.pose.position); const auto points_distance = boost::geometry::distance(p1, p2); const auto remain_distance = length - total_length; @@ -185,9 +185,8 @@ std::vector create_vehicle_footprints( // Create vehicle footprint on each TrajectoryPoint std::vector vehicle_footprints; for (const auto & p : trajectory.points) { - vehicle_footprints.push_back( - autoware::universe_utils::transformVector( - local_vehicle_footprint, autoware::universe_utils::pose2transform(p.pose))); + vehicle_footprints.push_back(autoware_utils::transform_vector( + local_vehicle_footprint, autoware_utils::pose2transform(p.pose))); } return vehicle_footprints; @@ -209,7 +208,7 @@ std::vector create_vehicle_passing_areas( LinearRing2d create_hull_from_footprints(const LinearRing2d & area1, const LinearRing2d & area2) { - autoware::universe_utils::MultiPoint2d combined; + autoware_utils::MultiPoint2d combined; for (const auto & p : area1) { combined.push_back(p); } @@ -242,8 +241,7 @@ bool has_collision( const LinearRing2d & vehicle_footprint) { for (const auto & point : obstacle_pointcloud.points) { - if (boost::geometry::within( - autoware::universe_utils::Point2d{point.x, point.y}, vehicle_footprint)) { + if (boost::geometry::within(autoware_utils::Point2d{point.x, point.y}, vehicle_footprint)) { RCLCPP_WARN( rclcpp::get_logger("obstacle_collision_checker"), "Collide to Point x: %f y: %f", point.x, point.y); diff --git a/control/autoware_obstacle_collision_checker/src/obstacle_collision_checker_node.cpp b/control/autoware_obstacle_collision_checker/src/obstacle_collision_checker_node.cpp index 598ae592f4c8d..7167de8db259d 100644 --- a/control/autoware_obstacle_collision_checker/src/obstacle_collision_checker_node.cpp +++ b/control/autoware_obstacle_collision_checker/src/obstacle_collision_checker_node.cpp @@ -16,9 +16,9 @@ #include "autoware/obstacle_collision_checker/debug.hpp" -#include -#include -#include +#include +#include +#include #include #include @@ -49,8 +49,8 @@ ObstacleCollisionCheckerNode::ObstacleCollisionCheckerNode(const rclcpp::NodeOpt &autoware::obstacle_collision_checker::ObstacleCollisionCheckerNode::param_callback, this, _1)); // Subscriber - self_pose_listener_ = std::make_shared(this); - transform_listener_ = std::make_shared(this); + self_pose_listener_ = std::make_shared(this); + transform_listener_ = std::make_shared(this); sub_obstacle_pointcloud_ = create_subscription( "input/obstacle_pointcloud", rclcpp::SensorDataQoS(), @@ -65,9 +65,8 @@ ObstacleCollisionCheckerNode::ObstacleCollisionCheckerNode(const rclcpp::NodeOpt "input/odometry", 1, std::bind(&ObstacleCollisionCheckerNode::on_odom, this, _1)); // Publisher - debug_publisher_ = - std::make_shared(this, "debug/marker"); - time_publisher_ = std::make_shared(this); + debug_publisher_ = std::make_shared(this, "debug/marker"); + time_publisher_ = std::make_shared(this); // Diagnostic Updater updater_.setHardwareID("obstacle_collision_checker"); @@ -76,7 +75,7 @@ ObstacleCollisionCheckerNode::ObstacleCollisionCheckerNode(const rclcpp::NodeOpt "obstacle_collision_checker", this, &ObstacleCollisionCheckerNode::check_lane_departure); // Wait for first self pose - self_pose_listener_->waitForFirstPose(); + self_pose_listener_->wait_for_first_pose(); // Timer init_timer(1.0 / node_param_.update_rate); @@ -174,11 +173,11 @@ bool ObstacleCollisionCheckerNode::is_data_timeout() void ObstacleCollisionCheckerNode::on_timer() { - current_pose_ = self_pose_listener_->getCurrentPose(); + current_pose_ = self_pose_listener_->get_current_pose(); if (obstacle_pointcloud_) { const auto & header = obstacle_pointcloud_->header; try { - obstacle_transform_ = transform_listener_->getTransform( + obstacle_transform_ = transform_listener_->get_transform( "map", header.frame_id, header.stamp, rclcpp::Duration::from_seconds(0.01)); } catch (tf2::TransformException & ex) { RCLCPP_INFO( @@ -222,22 +221,22 @@ rcl_interfaces::msg::SetParametersResult ObstacleCollisionCheckerNode::param_cal result.reason = "success"; try { - using autoware::universe_utils::updateParam; + using autoware_utils::update_param; // Node Parameter { auto & p = node_param_; // Update params - updateParam(parameters, "update_rate", p.update_rate); + update_param(parameters, "update_rate", p.update_rate); } auto & p = input_.param; - updateParam(parameters, "delay_time", p.delay_time); - updateParam(parameters, "footprint_margin", p.footprint_margin); - updateParam(parameters, "max_deceleration", p.max_deceleration); - updateParam(parameters, "resample_interval", p.resample_interval); - updateParam(parameters, "search_radius", p.search_radius); + update_param(parameters, "delay_time", p.delay_time); + update_param(parameters, "footprint_margin", p.footprint_margin); + update_param(parameters, "max_deceleration", p.max_deceleration); + update_param(parameters, "resample_interval", p.resample_interval); + update_param(parameters, "search_radius", p.search_radius); } catch (const rclcpp::exceptions::InvalidParameterTypeException & e) { result.successful = false; result.reason = e.what(); diff --git a/control/autoware_obstacle_collision_checker/test/test_obstacle_collision_checker.cpp b/control/autoware_obstacle_collision_checker/test/test_obstacle_collision_checker.cpp index 789d5602b2414..56ec2db0c8845 100644 --- a/control/autoware_obstacle_collision_checker/test/test_obstacle_collision_checker.cpp +++ b/control/autoware_obstacle_collision_checker/test/test_obstacle_collision_checker.cpp @@ -15,7 +15,7 @@ #include "../src/obstacle_collision_checker.cpp" // NOLINT #include "gtest/gtest.h" -#include +#include #include #include @@ -126,7 +126,7 @@ TEST(test_obstacle_collision_checker, getTransformedPointCloud) } { // rotation geometry_msgs::msg::Transform transform; - transform.rotation = autoware::universe_utils::createQuaternionFromRPY(0.0, 0.0, M_PI); + transform.rotation = autoware_utils::create_quaternion_from_rpy(0.0, 0.0, M_PI); const auto transformed_pcd = get_transformed_point_cloud(pcd_msg, transform); EXPECT_EQ(pcl_pcd.size(), transformed_pcd.size()); EXPECT_TRUE(point_in_pcl_pointcloud(pcl_point(0.0, 0.0), transformed_pcd)); @@ -137,7 +137,7 @@ TEST(test_obstacle_collision_checker, getTransformedPointCloud) geometry_msgs::msg::Transform transform; transform.translation.x = 0.5; transform.translation.y = -0.5; - transform.rotation = autoware::universe_utils::createQuaternionFromRPY(0.0, 0.0, M_PI); + transform.rotation = autoware_utils::create_quaternion_from_rpy(0.0, 0.0, M_PI); const auto transformed_pcd = get_transformed_point_cloud(pcd_msg, transform); EXPECT_EQ(pcl_pcd.size(), transformed_pcd.size()); EXPECT_TRUE(point_in_pcl_pointcloud(pcl_point(0.5, -0.5), transformed_pcd)); @@ -213,7 +213,7 @@ TEST(test_obstacle_collision_checker, check_for_collisions) EXPECT_FALSE(output.will_collide); // 1s * 1m/s = 1m for the delay, then 1->0m/s at 1m/s² = 0.5m -> 1.5m braking distance EXPECT_DOUBLE_EQ( - autoware::universe_utils::calcDistance2d( + autoware_utils::calc_distance2d( output.resampled_trajectory.points.front(), output.resampled_trajectory.points.back()), 1.5); } diff --git a/control/autoware_operation_mode_transition_manager/package.xml b/control/autoware_operation_mode_transition_manager/package.xml index 52e663a4a2103..d16452e06c5c2 100644 --- a/control/autoware_operation_mode_transition_manager/package.xml +++ b/control/autoware_operation_mode_transition_manager/package.xml @@ -16,7 +16,7 @@ autoware_component_interface_utils autoware_control_msgs autoware_motion_utils - autoware_universe_utils + autoware_utils autoware_vehicle_info_utils autoware_vehicle_msgs geometry_msgs diff --git a/control/autoware_operation_mode_transition_manager/src/node.cpp b/control/autoware_operation_mode_transition_manager/src/node.cpp index 25c048b0540fa..e851fa3432a03 100644 --- a/control/autoware_operation_mode_transition_manager/src/node.cpp +++ b/control/autoware_operation_mode_transition_manager/src/node.cpp @@ -210,11 +210,11 @@ void OperationModeTransitionManager::processTransition() void OperationModeTransitionManager::onTimer() { - const auto control_mode_report_ptr = sub_control_mode_report_.takeData(); + const auto control_mode_report_ptr = sub_control_mode_report_.take_data(); if (!control_mode_report_ptr) { return; } - const auto gate_operation_mode_ptr = sub_gate_operation_mode_.takeData(); + const auto gate_operation_mode_ptr = sub_gate_operation_mode_.take_data(); if (!gate_operation_mode_ptr) { return; } diff --git a/control/autoware_operation_mode_transition_manager/src/node.hpp b/control/autoware_operation_mode_transition_manager/src/node.hpp index ffef5f5fe999d..5ec7884da1afa 100644 --- a/control/autoware_operation_mode_transition_manager/src/node.hpp +++ b/control/autoware_operation_mode_transition_manager/src/node.hpp @@ -20,7 +20,7 @@ #include #include -#include +#include #include #include @@ -55,10 +55,10 @@ class OperationModeTransitionManager : public rclcpp::Node const ChangeOperationModeAPI::Service::Response::SharedPtr response); using ControlModeCommandType = ControlModeCommand::Request::_mode_type; - autoware::universe_utils::InterProcessPollingSubscriber - sub_control_mode_report_{this, "control_mode_report"}; - autoware::universe_utils::InterProcessPollingSubscriber - sub_gate_operation_mode_{this, "gate_operation_mode"}; + autoware_utils::InterProcessPollingSubscriber sub_control_mode_report_{ + this, "control_mode_report"}; + autoware_utils::InterProcessPollingSubscriber sub_gate_operation_mode_{ + this, "gate_operation_mode"}; rclcpp::Client::SharedPtr cli_control_mode_; rclcpp::Publisher::SharedPtr pub_debug_info_; rclcpp::TimerBase::SharedPtr timer_; diff --git a/control/autoware_operation_mode_transition_manager/src/state.cpp b/control/autoware_operation_mode_transition_manager/src/state.cpp index 5969e6b9fb03d..747dcfd3b8c5a 100644 --- a/control/autoware_operation_mode_transition_manager/src/state.cpp +++ b/control/autoware_operation_mode_transition_manager/src/state.cpp @@ -17,8 +17,8 @@ #include "util.hpp" #include -#include -#include +#include +#include #include #include @@ -29,8 +29,8 @@ namespace autoware::operation_mode_transition_manager { using autoware::motion_utils::findNearestIndex; -using autoware::universe_utils::calcDistance2d; -using autoware::universe_utils::calcYawDeviation; +using autoware_utils::calc_distance2d; +using autoware_utils::calc_yaw_deviation; AutonomousMode::AutonomousMode(rclcpp::Node * node) : logger_(node->get_logger()), clock_(node->get_clock()) @@ -252,11 +252,11 @@ bool AutonomousMode::isModeChangeAvailable() debug_info_.trajectory_available_ok = true; // No engagement is lateral control error is large - const auto lateral_deviation = calcDistance2d(closest_point.pose, kinematics_.pose.pose); + const auto lateral_deviation = calc_distance2d(closest_point.pose, kinematics_.pose.pose); const bool lateral_deviation_ok = lateral_deviation < param.dist_threshold; // No engagement is yaw control error is large - const auto yaw_deviation = calcYawDeviation(closest_point.pose, kinematics_.pose.pose); + const auto yaw_deviation = calc_yaw_deviation(closest_point.pose, kinematics_.pose.pose); const bool yaw_deviation_ok = yaw_deviation < param.yaw_threshold; // No engagement if speed control error is large diff --git a/control/autoware_pid_longitudinal_controller/include/autoware/pid_longitudinal_controller/pid_longitudinal_controller.hpp b/control/autoware_pid_longitudinal_controller/include/autoware/pid_longitudinal_controller/pid_longitudinal_controller.hpp index db5fa09b5eeee..d7aa93a5b4f08 100644 --- a/control/autoware_pid_longitudinal_controller/include/autoware/pid_longitudinal_controller/pid_longitudinal_controller.hpp +++ b/control/autoware_pid_longitudinal_controller/include/autoware/pid_longitudinal_controller/pid_longitudinal_controller.hpp @@ -21,7 +21,7 @@ #include "autoware/pid_longitudinal_controller/pid.hpp" #include "autoware/pid_longitudinal_controller/smooth_stop.hpp" #include "autoware/trajectory_follower_base/longitudinal_controller_base.hpp" -#include "autoware/universe_utils/ros/marker_helper.hpp" +#include "autoware_utils/ros/marker_helper.hpp" #include "autoware_vehicle_info_utils/vehicle_info_utils.hpp" #include "diagnostic_updater/diagnostic_updater.hpp" #include "rclcpp/rclcpp.hpp" diff --git a/control/autoware_pid_longitudinal_controller/package.xml b/control/autoware_pid_longitudinal_controller/package.xml index b685e424e8aa9..5b843ed9c194f 100644 --- a/control/autoware_pid_longitudinal_controller/package.xml +++ b/control/autoware_pid_longitudinal_controller/package.xml @@ -26,7 +26,7 @@ autoware_motion_utils autoware_planning_msgs autoware_trajectory_follower_base - autoware_universe_utils + autoware_utils autoware_vehicle_info_utils autoware_vehicle_msgs diagnostic_msgs diff --git a/control/autoware_pid_longitudinal_controller/src/longitudinal_controller_utils.cpp b/control/autoware_pid_longitudinal_controller/src/longitudinal_controller_utils.cpp index 9dc9e71ba25bc..35adb377fdfb3 100644 --- a/control/autoware_pid_longitudinal_controller/src/longitudinal_controller_utils.cpp +++ b/control/autoware_pid_longitudinal_controller/src/longitudinal_controller_utils.cpp @@ -14,7 +14,7 @@ #include "autoware/pid_longitudinal_controller/longitudinal_controller_utils.hpp" -#include "autoware/universe_utils/geometry/geometry.hpp" +#include "autoware_utils/geometry/geometry.hpp" #include "tf2/LinearMath/Matrix3x3.h" #include "tf2/LinearMath/Quaternion.h" @@ -96,8 +96,8 @@ double getPitchByTraj( const auto [prev_idx, next_idx] = [&]() { for (size_t i = start_idx + 1; i < trajectory.points.size(); ++i) { - const double dist = autoware::universe_utils::calcDistance3d( - trajectory.points.at(start_idx), trajectory.points.at(i)); + const double dist = + autoware_utils::calc_distance3d(trajectory.points.at(start_idx), trajectory.points.at(i)); if (dist > wheel_base) { // calculate pitch from trajectory between rear wheel (nearest) and front center (i) return std::make_pair(start_idx, i); @@ -108,7 +108,7 @@ double getPitchByTraj( std::min(start_idx, trajectory.points.size() - 2), trajectory.points.size() - 1); }(); - return autoware::universe_utils::calcElevationAngle( + return autoware_utils::calc_elevation_angle( trajectory.points.at(prev_idx).pose.position, trajectory.points.at(next_idx).pose.position); } @@ -169,7 +169,7 @@ geometry_msgs::msg::Pose findTrajectoryPoseAfterDistance( double remain_dist = distance; geometry_msgs::msg::Pose p = trajectory.points.back().pose; for (size_t i = src_idx; i < trajectory.points.size() - 1; ++i) { - const double dist = autoware::universe_utils::calcDistance3d( + const double dist = autoware_utils::calc_distance3d( trajectory.points.at(i).pose, trajectory.points.at(i + 1).pose); if (remain_dist < dist) { if (remain_dist <= 0.0) { diff --git a/control/autoware_pid_longitudinal_controller/src/pid_longitudinal_controller.cpp b/control/autoware_pid_longitudinal_controller/src/pid_longitudinal_controller.cpp index c87e936de3e40..e9e4b6679adde 100644 --- a/control/autoware_pid_longitudinal_controller/src/pid_longitudinal_controller.cpp +++ b/control/autoware_pid_longitudinal_controller/src/pid_longitudinal_controller.cpp @@ -16,8 +16,8 @@ #include "autoware/motion_utils/marker/marker_helper.hpp" #include "autoware/motion_utils/trajectory/trajectory.hpp" -#include "autoware/universe_utils/geometry/geometry.hpp" -#include "autoware/universe_utils/math/normalization.hpp" +#include "autoware_utils/geometry/geometry.hpp" +#include "autoware_utils/math/normalization.hpp" #include diff --git a/control/autoware_predicted_path_checker/include/autoware/predicted_path_checker/collision_checker.hpp b/control/autoware_predicted_path_checker/include/autoware/predicted_path_checker/collision_checker.hpp index bed95f4d8987c..d144ab7d55d5f 100644 --- a/control/autoware_predicted_path_checker/include/autoware/predicted_path_checker/collision_checker.hpp +++ b/control/autoware_predicted_path_checker/include/autoware/predicted_path_checker/collision_checker.hpp @@ -19,9 +19,9 @@ #include #include #include -#include -#include -#include +#include +#include +#include #include #include @@ -45,10 +45,10 @@ namespace autoware::predicted_path_checker using autoware_planning_msgs::msg::Trajectory; using autoware_planning_msgs::msg::TrajectoryPoint; using TrajectoryPoints = std::vector; -using autoware::universe_utils::Point2d; -using autoware::universe_utils::Polygon2d; using autoware_perception_msgs::msg::PredictedObject; using autoware_perception_msgs::msg::PredictedObjects; +using autoware_utils::Point2d; +using autoware_utils::Polygon2d; using geometry_msgs::msg::Pose; using geometry_msgs::msg::TransformStamped; using PointArray = std::vector; diff --git a/control/autoware_predicted_path_checker/include/autoware/predicted_path_checker/debug_marker.hpp b/control/autoware_predicted_path_checker/include/autoware/predicted_path_checker/debug_marker.hpp index f9dff3347272a..898bac4a045c1 100644 --- a/control/autoware_predicted_path_checker/include/autoware/predicted_path_checker/debug_marker.hpp +++ b/control/autoware_predicted_path_checker/include/autoware/predicted_path_checker/debug_marker.hpp @@ -31,7 +31,7 @@ #define EIGEN_MPL2_ONLY -#include +#include #include #include @@ -52,12 +52,12 @@ class PredictedPathCheckerDebugNode ~PredictedPathCheckerDebugNode() {} bool pushPolygon( - const autoware::universe_utils::Polygon2d & polygon, const double z, const PolygonType & type); + const autoware_utils::Polygon2d & polygon, const double z, const PolygonType & type); bool pushPolygon(const std::vector & polygon, const PolygonType & type); bool pushPolyhedron( - const autoware::universe_utils::Polygon2d & polyhedron, const double z_min, const double z_max, + const autoware_utils::Polygon2d & polyhedron, const double z_min, const double z_max, const PolygonType & type); bool pushPolyhedron(const std::vector & polyhedron, const PolygonType & type); diff --git a/control/autoware_predicted_path_checker/include/autoware/predicted_path_checker/predicted_path_checker_node.hpp b/control/autoware_predicted_path_checker/include/autoware/predicted_path_checker/predicted_path_checker_node.hpp index 33e0cb13efdca..bb1033666f8f8 100644 --- a/control/autoware_predicted_path_checker/include/autoware/predicted_path_checker/predicted_path_checker_node.hpp +++ b/control/autoware_predicted_path_checker/include/autoware/predicted_path_checker/predicted_path_checker_node.hpp @@ -21,8 +21,8 @@ #include #include #include -#include -#include +#include +#include #include #include #include @@ -50,9 +50,9 @@ namespace autoware::predicted_path_checker using autoware_planning_msgs::msg::Trajectory; using autoware_planning_msgs::msg::TrajectoryPoint; using TrajectoryPoints = std::vector; -using autoware::universe_utils::Point2d; -using autoware::universe_utils::Polygon2d; using autoware_perception_msgs::msg::PredictedObjects; +using autoware_utils::Point2d; +using autoware_utils::Polygon2d; using geometry_msgs::msg::Pose; using geometry_msgs::msg::TransformStamped; @@ -88,7 +88,7 @@ class PredictedPathCheckerNode : public rclcpp::Node rclcpp::CallbackGroup::SharedPtr group_cli_; // Subscriber - std::shared_ptr self_pose_listener_; + std::shared_ptr self_pose_listener_; rclcpp::Subscription::SharedPtr sub_dynamic_objects_; rclcpp::Subscription::SharedPtr sub_reference_trajectory_; diff --git a/control/autoware_predicted_path_checker/include/autoware/predicted_path_checker/utils.hpp b/control/autoware_predicted_path_checker/include/autoware/predicted_path_checker/utils.hpp index bf46025242401..d1706ec874f2d 100644 --- a/control/autoware_predicted_path_checker/include/autoware/predicted_path_checker/utils.hpp +++ b/control/autoware_predicted_path_checker/include/autoware/predicted_path_checker/utils.hpp @@ -17,7 +17,7 @@ #include #include -#include +#include #include #include @@ -39,12 +39,12 @@ namespace autoware::predicted_path_checker { -using autoware::universe_utils::Point2d; -using autoware::universe_utils::Polygon2d; using autoware::vehicle_info_utils::VehicleInfo; using autoware_perception_msgs::msg::PredictedObject; using autoware_perception_msgs::msg::PredictedObjects; using autoware_planning_msgs::msg::TrajectoryPoint; +using autoware_utils::Point2d; +using autoware_utils::Polygon2d; using geometry_msgs::msg::Point; using geometry_msgs::msg::Pose; using geometry_msgs::msg::TransformStamped; diff --git a/control/autoware_predicted_path_checker/package.xml b/control/autoware_predicted_path_checker/package.xml index 521d34455f465..4ec74a3937ac1 100644 --- a/control/autoware_predicted_path_checker/package.xml +++ b/control/autoware_predicted_path_checker/package.xml @@ -18,7 +18,7 @@ autoware_motion_utils autoware_perception_msgs autoware_planning_msgs - autoware_universe_utils + autoware_utils autoware_vehicle_info_utils boost diagnostic_updater diff --git a/control/autoware_predicted_path_checker/src/predicted_path_checker_node/collision_checker.cpp b/control/autoware_predicted_path_checker/src/predicted_path_checker_node/collision_checker.cpp index d21766f36e202..0a2ce810ef26d 100644 --- a/control/autoware_predicted_path_checker/src/predicted_path_checker_node/collision_checker.cpp +++ b/control/autoware_predicted_path_checker/src/predicted_path_checker_node/collision_checker.cpp @@ -14,7 +14,7 @@ #include "autoware/predicted_path_checker/collision_checker.hpp" -#include +#include #include #include @@ -81,12 +81,12 @@ CollisionChecker::checkTrajectoryForCollision( double distance_to_current = std::numeric_limits::max(); double distance_to_history = std::numeric_limits::max(); if (found_collision_at_dynamic_objects) { - distance_to_current = autoware::universe_utils::calcDistance2d( - p_front, found_collision_at_dynamic_objects.get().first); + distance_to_current = + autoware_utils::calc_distance2d(p_front, found_collision_at_dynamic_objects.get().first); } if (found_collision_at_history) { distance_to_history = - autoware::universe_utils::calcDistance2d(p_front, found_collision_at_history.get().first); + autoware_utils::calc_distance2d(p_front, found_collision_at_history.get().first); } else { predicted_object_history_.clear(); } @@ -141,7 +141,7 @@ CollisionChecker::checkObstacleHistory( bool is_init = false; std::pair nearest_collision_object_with_point; for (const auto & p : collision_points_in_history) { - double norm = autoware::universe_utils::calcDistance2d(p.first, base_pose); + double norm = autoware_utils::calc_distance2d(p.first, base_pose); if (norm < min_norm || !is_init) { min_norm = norm; nearest_collision_object_with_point = p; diff --git a/control/autoware_predicted_path_checker/src/predicted_path_checker_node/debug_marker.cpp b/control/autoware_predicted_path_checker/src/predicted_path_checker_node/debug_marker.cpp index 61125b88484f0..4cd9e1c82719f 100644 --- a/control/autoware_predicted_path_checker/src/predicted_path_checker_node/debug_marker.cpp +++ b/control/autoware_predicted_path_checker/src/predicted_path_checker_node/debug_marker.cpp @@ -15,28 +15,28 @@ #include "autoware/predicted_path_checker/debug_marker.hpp" #include -#include -#include +#include +#include #ifdef ROS_DISTRO_GALACTIC #include #else #include #endif -#include +#include #include #include using autoware::motion_utils::createDeletedStopVirtualWallMarker; using autoware::motion_utils::createStopVirtualWallMarker; -using autoware::universe_utils::appendMarkerArray; -using autoware::universe_utils::calcOffsetPose; -using autoware::universe_utils::createDefaultMarker; -using autoware::universe_utils::createMarkerColor; -using autoware::universe_utils::createMarkerOrientation; -using autoware::universe_utils::createMarkerScale; -using autoware::universe_utils::createPoint; +using autoware_utils::append_marker_array; +using autoware_utils::calc_offset_pose; +using autoware_utils::create_default_marker; +using autoware_utils::create_marker_color; +using autoware_utils::create_marker_orientation; +using autoware_utils::create_marker_scale; +using autoware_utils::create_point; namespace autoware::predicted_path_checker { @@ -51,7 +51,7 @@ PredictedPathCheckerDebugNode::PredictedPathCheckerDebugNode( } bool PredictedPathCheckerDebugNode::pushPolygon( - const autoware::universe_utils::Polygon2d & polygon, const double z, const PolygonType & type) + const autoware_utils::Polygon2d & polygon, const double z, const PolygonType & type) { std::vector eigen_polygon; for (const auto & point : polygon.outer()) { @@ -82,7 +82,7 @@ bool PredictedPathCheckerDebugNode::pushPolygon( } bool PredictedPathCheckerDebugNode::pushPolyhedron( - const autoware::universe_utils::Polygon2d & polyhedron, const double z_min, const double z_max, + const autoware_utils::Polygon2d & polyhedron, const double z_min, const double z_max, const PolygonType & type) { std::vector eigen_polyhedron; @@ -166,22 +166,22 @@ visualization_msgs::msg::MarkerArray PredictedPathCheckerDebugNode::makeVirtualW rclcpp::Time current_time = node_->now(); if (stop_pose_ptr_ != nullptr) { - const auto p = calcOffsetPose(*stop_pose_ptr_, base_link2front_, 0.0, 0.0); + const auto p = calc_offset_pose(*stop_pose_ptr_, base_link2front_, 0.0, 0.0); const auto markers = createStopVirtualWallMarker(p, "obstacle on predicted path", current_time, 0); - appendMarkerArray(markers, &msg); + append_marker_array(markers, &msg); } else { const auto markers = createDeletedStopVirtualWallMarker(current_time, 0); - appendMarkerArray(markers, &msg); + append_marker_array(markers, &msg); } if (collision_pose_ptr_ != nullptr) { const auto markers = createStopVirtualWallMarker(*collision_pose_ptr_, "collision_point", current_time, 1); - appendMarkerArray(markers, &msg); + append_marker_array(markers, &msg); } else { const auto markers = createDeletedStopVirtualWallMarker(current_time, 1); - appendMarkerArray(markers, &msg); + append_marker_array(markers, &msg); } return msg; @@ -194,64 +194,64 @@ visualization_msgs::msg::MarkerArray PredictedPathCheckerDebugNode::makeVisualiz // cube if (!vehicle_polyhedrons_.empty()) { - auto marker = createDefaultMarker( + auto marker = create_default_marker( "map", current_time, "detection_cubes", 0, visualization_msgs::msg::Marker::LINE_LIST, - createMarkerScale(0.01, 0.0, 0.0), createMarkerColor(0.0, 1.0, 0.0, 0.999)); + create_marker_scale(0.01, 0.0, 0.0), create_marker_color(0.0, 1.0, 0.0, 0.999)); for (size_t i = 0; i < vehicle_polyhedrons_.size(); ++i) { for (size_t j = 0; j < vehicle_polyhedrons_.at(i).size(); ++j) { const auto & p = vehicle_polyhedrons_.at(i).at(j); - marker.points.push_back(createPoint(p.x(), p.y(), p.z())); + marker.points.push_back(create_point(p.x(), p.y(), p.z())); } } for (size_t i = 0; i < vehicle_polyhedrons_.size(); ++i) { for (size_t j = 0; j + 2 < vehicle_polyhedrons_.at(i).size(); ++j) { const auto & p = vehicle_polyhedrons_.at(i).at(j); - marker.points.push_back(createPoint(p.x(), p.y(), p.z())); + marker.points.push_back(create_point(p.x(), p.y(), p.z())); const auto & p1 = vehicle_polyhedrons_.at(i).at(j + 2); - marker.points.push_back(createPoint(p1.x(), p1.y(), p1.z())); + marker.points.push_back(create_point(p1.x(), p1.y(), p1.z())); } const auto & p = vehicle_polyhedrons_.at(i).at(1); - marker.points.push_back(createPoint(p.x(), p.y(), p.z())); + marker.points.push_back(create_point(p.x(), p.y(), p.z())); const auto & p1 = vehicle_polyhedrons_.at(i).at(vehicle_polyhedrons_.at(i).size() - 1); - marker.points.push_back(createPoint(p1.x(), p1.y(), p1.z())); + marker.points.push_back(create_point(p1.x(), p1.y(), p1.z())); const auto & p2 = vehicle_polyhedrons_.at(i).at(0); - marker.points.push_back(createPoint(p2.x(), p2.y(), p2.z())); + marker.points.push_back(create_point(p2.x(), p2.y(), p2.z())); const auto & p3 = vehicle_polyhedrons_.at(i).at(vehicle_polyhedrons_.at(i).size() - 2); - marker.points.push_back(createPoint(p3.x(), p3.y(), p3.z())); + marker.points.push_back(create_point(p3.x(), p3.y(), p3.z())); } msg.markers.push_back(marker); } if (!collision_polyhedrons_.empty()) { - auto marker = createDefaultMarker( + auto marker = create_default_marker( "map", current_time, "collision_cubes", 0, visualization_msgs::msg::Marker::LINE_LIST, - createMarkerScale(0.05, 0.0, 0.0), createMarkerColor(1.0, 0.0, 0.0, 0.999)); + create_marker_scale(0.05, 0.0, 0.0), create_marker_color(1.0, 0.0, 0.0, 0.999)); for (size_t i = 0; i < collision_polyhedrons_.size(); ++i) { for (size_t j = 0; j < collision_polyhedrons_.at(i).size(); ++j) { const auto & p = collision_polyhedrons_.at(i).at(j); - marker.points.push_back(createPoint(p.x(), p.y(), p.z())); + marker.points.push_back(create_point(p.x(), p.y(), p.z())); } } for (size_t i = 0; i < collision_polyhedrons_.size(); ++i) { for (size_t j = 0; j + 2 < collision_polyhedrons_.at(i).size(); ++j) { const auto & p = collision_polyhedrons_.at(i).at(j); - marker.points.push_back(createPoint(p.x(), p.y(), p.z())); + marker.points.push_back(create_point(p.x(), p.y(), p.z())); const auto & p1 = collision_polyhedrons_.at(i).at(j + 2); - marker.points.push_back(createPoint(p1.x(), p1.y(), p1.z())); + marker.points.push_back(create_point(p1.x(), p1.y(), p1.z())); } const auto & p = collision_polyhedrons_.at(i).at(1); - marker.points.push_back(createPoint(p.x(), p.y(), p.z())); + marker.points.push_back(create_point(p.x(), p.y(), p.z())); const auto & p1 = collision_polyhedrons_.at(i).at(collision_polyhedrons_.at(i).size() - 1); - marker.points.push_back(createPoint(p1.x(), p1.y(), p1.z())); + marker.points.push_back(create_point(p1.x(), p1.y(), p1.z())); const auto & p2 = collision_polyhedrons_.at(i).at(0); - marker.points.push_back(createPoint(p2.x(), p2.y(), p2.z())); + marker.points.push_back(create_point(p2.x(), p2.y(), p2.z())); const auto & p3 = collision_polyhedrons_.at(i).at(collision_polyhedrons_.at(i).size() - 2); - marker.points.push_back(createPoint(p3.x(), p3.y(), p3.z())); + marker.points.push_back(create_point(p3.x(), p3.y(), p3.z())); } msg.markers.push_back(marker); @@ -259,22 +259,22 @@ visualization_msgs::msg::MarkerArray PredictedPathCheckerDebugNode::makeVisualiz // polygon if (!vehicle_polygons_.empty()) { - auto marker = createDefaultMarker( + auto marker = create_default_marker( "map", current_time, "detection_polygons", 0, visualization_msgs::msg::Marker::LINE_LIST, - createMarkerScale(0.01, 0.0, 0.0), createMarkerColor(0.0, 1.0, 0.0, 0.999)); + create_marker_scale(0.01, 0.0, 0.0), create_marker_color(0.0, 1.0, 0.0, 0.999)); for (size_t i = 0; i < vehicle_polygons_.size(); ++i) { for (size_t j = 0; j < vehicle_polygons_.at(i).size(); ++j) { { const auto & p = vehicle_polygons_.at(i).at(j); - marker.points.push_back(createPoint(p.x(), p.y(), p.z())); + marker.points.push_back(create_point(p.x(), p.y(), p.z())); } if (j + 1 == vehicle_polygons_.at(i).size()) { const auto & p = vehicle_polygons_.at(i).at(0); - marker.points.push_back(createPoint(p.x(), p.y(), p.z())); + marker.points.push_back(create_point(p.x(), p.y(), p.z())); } else { const auto & p = vehicle_polygons_.at(i).at(j + 1); - marker.points.push_back(createPoint(p.x(), p.y(), p.z())); + marker.points.push_back(create_point(p.x(), p.y(), p.z())); } } } @@ -282,22 +282,22 @@ visualization_msgs::msg::MarkerArray PredictedPathCheckerDebugNode::makeVisualiz } if (!collision_polygons_.empty()) { - auto marker = createDefaultMarker( + auto marker = create_default_marker( "map", current_time, "collision_polygons", 0, visualization_msgs::msg::Marker::LINE_LIST, - createMarkerScale(0.05, 0.0, 0.0), createMarkerColor(1.0, 0.0, 0.0, 0.999)); + create_marker_scale(0.05, 0.0, 0.0), create_marker_color(1.0, 0.0, 0.0, 0.999)); for (size_t i = 0; i < collision_polygons_.size(); ++i) { for (size_t j = 0; j < collision_polygons_.at(i).size(); ++j) { { const auto & p = collision_polygons_.at(i).at(j); - marker.points.push_back(createPoint(p.x(), p.y(), p.z())); + marker.points.push_back(create_point(p.x(), p.y(), p.z())); } if (j + 1 == collision_polygons_.at(i).size()) { const auto & p = collision_polygons_.at(i).at(0); - marker.points.push_back(createPoint(p.x(), p.y(), p.z())); + marker.points.push_back(create_point(p.x(), p.y(), p.z())); } else { const auto & p = collision_polygons_.at(i).at(j + 1); - marker.points.push_back(createPoint(p.x(), p.y(), p.z())); + marker.points.push_back(create_point(p.x(), p.y(), p.z())); } } } @@ -305,18 +305,18 @@ visualization_msgs::msg::MarkerArray PredictedPathCheckerDebugNode::makeVisualiz } if (stop_obstacle_point_ptr_ != nullptr) { - auto marker = createDefaultMarker( + auto marker = create_default_marker( "map", current_time, "stop_obstacle_point", 0, visualization_msgs::msg::Marker::SPHERE, - createMarkerScale(0.25, 0.25, 0.25), createMarkerColor(1.0, 0.0, 0.0, 0.999)); + create_marker_scale(0.25, 0.25, 0.25), create_marker_color(1.0, 0.0, 0.0, 0.999)); marker.pose.position = *stop_obstacle_point_ptr_; msg.markers.push_back(marker); } if (stop_obstacle_point_ptr_ != nullptr) { - auto marker = createDefaultMarker( + auto marker = create_default_marker( "map", current_time, "stop_obstacle_text", 0, - visualization_msgs::msg::Marker::TEXT_VIEW_FACING, createMarkerScale(0.0, 0.0, 1.0), - createMarkerColor(1.0, 1.0, 1.0, 0.999)); + visualization_msgs::msg::Marker::TEXT_VIEW_FACING, create_marker_scale(0.0, 0.0, 1.0), + create_marker_color(1.0, 1.0, 1.0, 0.999)); marker.pose.position = *stop_obstacle_point_ptr_; marker.pose.position.z += 2.0; marker.text = "!"; diff --git a/control/autoware_predicted_path_checker/src/predicted_path_checker_node/predicted_path_checker_node.cpp b/control/autoware_predicted_path_checker/src/predicted_path_checker_node/predicted_path_checker_node.cpp index 559e19fe8e0ea..82fe4ef09a244 100644 --- a/control/autoware_predicted_path_checker/src/predicted_path_checker_node/predicted_path_checker_node.cpp +++ b/control/autoware_predicted_path_checker/src/predicted_path_checker_node/predicted_path_checker_node.cpp @@ -16,9 +16,9 @@ #include #include -#include -#include -#include +#include +#include +#include #include #include @@ -68,7 +68,7 @@ PredictedPathCheckerNode::PredictedPathCheckerNode(const rclcpp::NodeOptions & n declare_parameter("collision_checker_params.chattering_threshold", 0.2); // Subscriber - self_pose_listener_ = std::make_shared(this); + self_pose_listener_ = std::make_shared(this); sub_dynamic_objects_ = create_subscription( "~/input/objects", rclcpp::SensorDataQoS(), @@ -98,7 +98,7 @@ PredictedPathCheckerNode::PredictedPathCheckerNode(const rclcpp::NodeOptions & n updater_.add("predicted_path_checker", this, &PredictedPathCheckerNode::checkVehicleState); // Wait for first self pose - self_pose_listener_->waitForFirstPose(); + self_pose_listener_->wait_for_first_pose(); // Timer initTimer(1.0 / node_param_.update_rate); @@ -225,7 +225,7 @@ bool PredictedPathCheckerNode::isDataTimeout() void PredictedPathCheckerNode::onTimer() { - current_pose_ = self_pose_listener_->getCurrentPose(); + current_pose_ = self_pose_listener_->get_current_pose(); if (!isDataReady()) { return; @@ -435,13 +435,13 @@ bool PredictedPathCheckerNode::isItDiscretePoint( const auto nearest_point = calcInterpolatedPoint( reference_trajectory, collision_point.pose.position, *nearest_segment, false); - const auto distance = autoware::universe_utils::calcDistance2d( - nearest_point.pose.position, collision_point.pose.position); + const auto distance = + autoware_utils::calc_distance2d(nearest_point.pose.position, collision_point.pose.position); const auto yaw_diff = - std::abs(autoware::universe_utils::calcYawDeviation(nearest_point.pose, collision_point.pose)); + std::abs(autoware_utils::calc_yaw_deviation(nearest_point.pose, collision_point.pose)); return distance >= node_param_.distinct_point_distance_threshold || - yaw_diff >= autoware::universe_utils::deg2rad(node_param_.distinct_point_yaw_threshold); + yaw_diff >= autoware_utils::deg2rad(node_param_.distinct_point_yaw_threshold); } Trajectory PredictedPathCheckerNode::cutTrajectory( @@ -457,8 +457,8 @@ Trajectory PredictedPathCheckerNode::cutTrajectory( for (size_t i = 1; i < trajectory.points.size(); ++i) { const auto & point = trajectory.points.at(i); - const auto p1 = autoware::universe_utils::fromMsg(cut.points.back().pose.position); - const auto p2 = autoware::universe_utils::fromMsg(point.pose.position); + const auto p1 = autoware_utils::from_msg(cut.points.back().pose.position); + const auto p2 = autoware_utils::from_msg(point.pose.position); const auto points_distance = boost::geometry::distance(p1.to_2d(), p2.to_2d()); const auto remain_distance = length - total_length; @@ -531,7 +531,7 @@ std::pair PredictedPathCheckerNode::calculateProjectedVelAndAcc( const auto velocity_obj = object.kinematics.initial_twist_with_covariance.twist.linear.x; const auto acceleration_obj = object.kinematics.initial_acceleration_with_covariance.accel.linear.x; - const auto k = std::cos(autoware::universe_utils::normalizeRadian( + const auto k = std::cos(autoware_utils::normalize_radian( tf2::getYaw(orientation_obj) - tf2::getYaw(orientation_stop_point))); const auto projected_velocity = velocity_obj * k; const auto projected_acceleration = acceleration_obj * k; @@ -559,8 +559,8 @@ void PredictedPathCheckerNode::filterObstacles( const double max_length = calcObstacleMaxLength(object.shape); const size_t seg_idx = autoware::motion_utils::findNearestSegmentIndex( traj, object.kinematics.initial_pose_with_covariance.pose.position); - const auto p_front = autoware::universe_utils::getPoint(traj.at(seg_idx)); - const auto p_back = autoware::universe_utils::getPoint(traj.at(seg_idx + 1)); + const auto p_front = autoware_utils::get_point(traj.at(seg_idx)); + const auto p_back = autoware_utils::get_point(traj.at(seg_idx + 1)); const auto & p_target = object.kinematics.initial_pose_with_covariance.pose.position; const Eigen::Vector3d segment_vec{p_back.x - p_front.x, p_back.y - p_front.y, 0.0}; const Eigen::Vector3d target_vec{p_target.x - p_front.x, p_target.y - p_front.y, 0.0}; diff --git a/control/autoware_predicted_path_checker/src/predicted_path_checker_node/utils.cpp b/control/autoware_predicted_path_checker/src/predicted_path_checker_node/utils.cpp index a5b1b08d32c1d..80813131820b4 100644 --- a/control/autoware_predicted_path_checker/src/predicted_path_checker_node/utils.cpp +++ b/control/autoware_predicted_path_checker/src/predicted_path_checker_node/utils.cpp @@ -14,7 +14,7 @@ #include "autoware/predicted_path_checker/utils.hpp" -#include +#include #include #include @@ -27,8 +27,8 @@ namespace autoware::predicted_path_checker using autoware::motion_utils::findFirstNearestIndexWithSoftConstraints; using autoware::motion_utils::findFirstNearestSegmentIndexWithSoftConstraints; -using autoware::universe_utils::calcDistance2d; -using autoware::universe_utils::getRPY; +using autoware_utils::calc_distance2d; +using autoware_utils::get_rpy; // Utils Functions Polygon2d createOneStepPolygon( @@ -44,40 +44,35 @@ Polygon2d createOneStepPolygon( { // base step appendPointToPolygon( polygon, - autoware::universe_utils::calcOffsetPose(base_step_pose, longitudinal_offset, width, 0.0) - .position); + autoware_utils::calc_offset_pose(base_step_pose, longitudinal_offset, width, 0.0).position); appendPointToPolygon( polygon, - autoware::universe_utils::calcOffsetPose(base_step_pose, longitudinal_offset, -width, 0.0) - .position); + autoware_utils::calc_offset_pose(base_step_pose, longitudinal_offset, -width, 0.0).position); appendPointToPolygon( - polygon, autoware::universe_utils::calcOffsetPose(base_step_pose, -rear_overhang, -width, 0.0) - .position); + polygon, + autoware_utils::calc_offset_pose(base_step_pose, -rear_overhang, -width, 0.0).position); appendPointToPolygon( - polygon, autoware::universe_utils::calcOffsetPose(base_step_pose, -rear_overhang, width, 0.0) - .position); + polygon, + autoware_utils::calc_offset_pose(base_step_pose, -rear_overhang, width, 0.0).position); } { // next step appendPointToPolygon( polygon, - autoware::universe_utils::calcOffsetPose(next_step_pose, longitudinal_offset, width, 0.0) - .position); + autoware_utils::calc_offset_pose(next_step_pose, longitudinal_offset, width, 0.0).position); appendPointToPolygon( polygon, - autoware::universe_utils::calcOffsetPose(next_step_pose, longitudinal_offset, -width, 0.0) - .position); + autoware_utils::calc_offset_pose(next_step_pose, longitudinal_offset, -width, 0.0).position); appendPointToPolygon( - polygon, autoware::universe_utils::calcOffsetPose(next_step_pose, -rear_overhang, -width, 0.0) - .position); + polygon, + autoware_utils::calc_offset_pose(next_step_pose, -rear_overhang, -width, 0.0).position); appendPointToPolygon( - polygon, autoware::universe_utils::calcOffsetPose(next_step_pose, -rear_overhang, width, 0.0) - .position); + polygon, + autoware_utils::calc_offset_pose(next_step_pose, -rear_overhang, width, 0.0).position); } - polygon = autoware::universe_utils::isClockwise(polygon) - ? polygon - : autoware::universe_utils::inverseClockwise(polygon); + polygon = + autoware_utils::is_clockwise(polygon) ? polygon : autoware_utils::inverse_clockwise(polygon); Polygon2d hull_polygon; boost::geometry::convex_hull(polygon, hull_polygon); @@ -101,8 +96,8 @@ TrajectoryPoint calcInterpolatedPoint( // Calculate interpolation ratio const auto & curr_pt = trajectory.at(segment_idx); const auto & next_pt = trajectory.at(segment_idx + 1); - const auto v1 = autoware::universe_utils::point2tfVector(curr_pt, next_pt); - const auto v2 = autoware::universe_utils::point2tfVector(curr_pt, target_point); + const auto v1 = autoware_utils::point_2_tf_vector(curr_pt, next_pt); + const auto v2 = autoware_utils::point_2_tf_vector(curr_pt, target_point); if (v1.length2() < 1e-3) { return curr_pt; } @@ -114,8 +109,7 @@ TrajectoryPoint calcInterpolatedPoint( TrajectoryPoint interpolated_point{}; // pose interpolation - interpolated_point.pose = - autoware::universe_utils::calcInterpolatedPose(curr_pt, next_pt, clamped_ratio); + interpolated_point.pose = autoware_utils::calc_interpolated_pose(curr_pt, next_pt, clamped_ratio); // twist interpolation if (use_zero_order_hold_for_twist) { @@ -180,8 +174,8 @@ std::pair findStopPoint( base_point.pose.position.x - next_point.pose.position.x, base_point.pose.position.y - next_point.pose.position.y)); - geometry_msgs::msg::Pose interpolated_pose = autoware::universe_utils::calcInterpolatedPose( - base_point.pose, next_point.pose, ratio, false); + geometry_msgs::msg::Pose interpolated_pose = + autoware_utils::calc_interpolated_pose(base_point.pose, next_point.pose, ratio, false); TrajectoryPoint output; output.set__pose(interpolated_pose); return std::make_pair(stop_segment_idx, output); @@ -253,7 +247,7 @@ double getNearestPointAndDistanceForPredictedObject( bool is_init = false; for (const auto & p : points) { - double norm = autoware::universe_utils::calcDistance2d(p, base_pose); + double norm = autoware_utils::calc_distance2d(p, base_pose); if (norm < min_norm || !is_init) { min_norm = norm; *nearest_collision_point = p; @@ -375,7 +369,7 @@ Polygon2d convertObjToPolygon(const PredictedObject & obj) bool isFrontObstacle(const Pose & ego_pose, const geometry_msgs::msg::Point & obstacle_pos) { - const auto yaw = autoware::universe_utils::getRPY(ego_pose).z; + const auto yaw = autoware_utils::get_rpy(ego_pose).z; const Eigen::Vector2d base_pose_vec(std::cos(yaw), std::sin(yaw)); const Eigen::Vector2d obstacle_vec( obstacle_pos.x - ego_pose.position.x, obstacle_pos.y - ego_pose.position.y); @@ -407,7 +401,7 @@ void getCurrentObjectPose( PredictedObject & predicted_object, const rclcpp::Time & obj_base_time, const rclcpp::Time & current_time) { - const double yaw = autoware::universe_utils::getRPY( + const double yaw = autoware_utils::get_rpy( predicted_object.kinematics.initial_pose_with_covariance.pose.orientation) .z; const double vx = predicted_object.kinematics.initial_twist_with_covariance.twist.linear.x; @@ -418,8 +412,8 @@ void getCurrentObjectPose( const double delta_yaw = predicted_object.kinematics.initial_twist_with_covariance.twist.angular.z * dt; geometry_msgs::msg::Transform transform; - transform.translation = autoware::universe_utils::createTranslation(ds, 0.0, 0.0); - transform.rotation = autoware::universe_utils::createQuaternionFromRPY(0.0, 0.0, yaw); + transform.translation = autoware_utils::create_translation(ds, 0.0, 0.0); + transform.rotation = autoware_utils::create_quaternion_from_rpy(0.0, 0.0, yaw); tf2::Transform tf_pose; tf2::Transform tf_offset; @@ -428,8 +422,8 @@ void getCurrentObjectPose( tf2::toMsg(tf_pose * tf_offset, predicted_object.kinematics.initial_pose_with_covariance.pose); predicted_object.kinematics.initial_twist_with_covariance.twist.linear.x += ax * dt; predicted_object.kinematics.initial_pose_with_covariance.pose.orientation = - autoware::universe_utils::createQuaternionFromRPY( - 0.0, 0.0, autoware::universe_utils::normalizeRadian(yaw + delta_yaw)); + autoware_utils::create_quaternion_from_rpy( + 0.0, 0.0, autoware_utils::normalize_radian(yaw + delta_yaw)); } } // namespace autoware::predicted_path_checker diff --git a/control/autoware_pure_pursuit/include/autoware/pure_pursuit/autoware_pure_pursuit_node.hpp b/control/autoware_pure_pursuit/include/autoware/pure_pursuit/autoware_pure_pursuit_node.hpp index 58ce03a0dbf26..519334d5becf6 100644 --- a/control/autoware_pure_pursuit/include/autoware/pure_pursuit/autoware_pure_pursuit_node.hpp +++ b/control/autoware_pure_pursuit/include/autoware/pure_pursuit/autoware_pure_pursuit_node.hpp @@ -33,8 +33,8 @@ #include "autoware/pure_pursuit/autoware_pure_pursuit.hpp" #include "autoware/pure_pursuit/autoware_pure_pursuit_viz.hpp" -#include -#include +#include +#include #include #include @@ -78,11 +78,11 @@ class PurePursuitNode : public rclcpp::Node private: // Subscriber - autoware::universe_utils::SelfPoseListener self_pose_listener_{this}; - autoware::universe_utils::InterProcessPollingSubscriber + autoware_utils::SelfPoseListener self_pose_listener_{this}; + autoware_utils::InterProcessPollingSubscriber sub_trajectory_{this, "input/reference_trajectory"}; - autoware::universe_utils::InterProcessPollingSubscriber - sub_current_odometry_{this, "input/current_odometry"}; + autoware_utils::InterProcessPollingSubscriber sub_current_odometry_{ + this, "input/current_odometry"}; autoware_planning_msgs::msg::Trajectory::ConstSharedPtr trajectory_; nav_msgs::msg::Odometry::ConstSharedPtr current_odometry_; diff --git a/control/autoware_pure_pursuit/package.xml b/control/autoware_pure_pursuit/package.xml index 98f1e71f26cf6..6b349ef931773 100644 --- a/control/autoware_pure_pursuit/package.xml +++ b/control/autoware_pure_pursuit/package.xml @@ -19,7 +19,7 @@ autoware_motion_utils autoware_planning_msgs autoware_trajectory_follower_base - autoware_universe_utils + autoware_utils autoware_vehicle_info_utils boost geometry_msgs diff --git a/control/autoware_pure_pursuit/src/autoware_pure_pursuit/autoware_pure_pursuit_lateral_controller.cpp b/control/autoware_pure_pursuit/src/autoware_pure_pursuit/autoware_pure_pursuit_lateral_controller.cpp index d2aba8a2f1d01..025be4ea9dab9 100644 --- a/control/autoware_pure_pursuit/src/autoware_pure_pursuit/autoware_pure_pursuit_lateral_controller.cpp +++ b/control/autoware_pure_pursuit/src/autoware_pure_pursuit/autoware_pure_pursuit_lateral_controller.cpp @@ -34,7 +34,7 @@ #include "autoware/pure_pursuit/util/planning_utils.hpp" #include "autoware/pure_pursuit/util/tf_utils.hpp" -#include +#include #include #include @@ -144,7 +144,7 @@ TrajectoryPoint PurePursuitLateralController::calcNextPose( const double ds, TrajectoryPoint & point, Lateral cmd) const { geometry_msgs::msg::Transform transform; - transform.translation = autoware::universe_utils::createTranslation(ds, 0.0, 0.0); + transform.translation = autoware_utils::create_translation(ds, 0.0, 0.0); transform.rotation = planning_utils::getQuaternionFromYaw(((tan(cmd.steering_tire_angle) * ds) / param_.wheel_base)); TrajectoryPoint output_p; @@ -205,10 +205,10 @@ double PurePursuitLateralController::calcCurvature(const size_t closest_idx) double current_curvature = 0.0; try { - current_curvature = autoware::universe_utils::calcCurvature( - autoware::universe_utils::getPoint(trajectory_resampled_->points.at(prev_idx)), - autoware::universe_utils::getPoint(trajectory_resampled_->points.at(closest_idx)), - autoware::universe_utils::getPoint(trajectory_resampled_->points.at(next_idx))); + current_curvature = autoware_utils::calc_curvature( + autoware_utils::get_point(trajectory_resampled_->points.at(prev_idx)), + autoware_utils::get_point(trajectory_resampled_->points.at(closest_idx)), + autoware_utils::get_point(trajectory_resampled_->points.at(next_idx))); } catch (std::exception const & e) { // ...code that handles the error... RCLCPP_WARN(rclcpp::get_logger("pure_pursuit"), "%s", e.what()); diff --git a/control/autoware_pure_pursuit/src/autoware_pure_pursuit/autoware_pure_pursuit_node.cpp b/control/autoware_pure_pursuit/src/autoware_pure_pursuit/autoware_pure_pursuit_node.cpp index 9a3b90b86b98e..d74632fcc4bb1 100644 --- a/control/autoware_pure_pursuit/src/autoware_pure_pursuit/autoware_pure_pursuit_node.cpp +++ b/control/autoware_pure_pursuit/src/autoware_pure_pursuit/autoware_pure_pursuit_node.cpp @@ -115,8 +115,8 @@ void PurePursuitNode::onTimer() { current_pose_ = self_pose_listener_.getCurrentPose(); - current_odometry_ = sub_current_odometry_.takeData(); - trajectory_ = sub_trajectory_.takeData(); + current_odometry_ = sub_current_odometry_.take_data(); + trajectory_ = sub_trajectory_.take_data(); if (!isDataReady()) { return; } diff --git a/control/autoware_shift_decider/include/autoware/shift_decider/autoware_shift_decider.hpp b/control/autoware_shift_decider/include/autoware/shift_decider/autoware_shift_decider.hpp index c7ce822d1ac18..cac5bcb55fb09 100644 --- a/control/autoware_shift_decider/include/autoware/shift_decider/autoware_shift_decider.hpp +++ b/control/autoware_shift_decider/include/autoware/shift_decider/autoware_shift_decider.hpp @@ -15,7 +15,7 @@ #ifndef AUTOWARE__SHIFT_DECIDER__AUTOWARE_SHIFT_DECIDER_HPP_ #define AUTOWARE__SHIFT_DECIDER__AUTOWARE_SHIFT_DECIDER_HPP_ -#include "autoware/universe_utils/ros/polling_subscriber.hpp" +#include "autoware_utils/ros/polling_subscriber.hpp" #include @@ -43,11 +43,11 @@ class ShiftDecider : public rclcpp::Node void initTimer(double period_s); rclcpp::Publisher::SharedPtr pub_shift_cmd_; - autoware::universe_utils::InterProcessPollingSubscriber + autoware_utils::InterProcessPollingSubscriber sub_control_cmd_{this, "input/control_cmd"}; - autoware::universe_utils::InterProcessPollingSubscriber + autoware_utils::InterProcessPollingSubscriber sub_autoware_state_{this, "input/state"}; - autoware::universe_utils::InterProcessPollingSubscriber + autoware_utils::InterProcessPollingSubscriber sub_current_gear_{this, "input/current_gear"}; rclcpp::TimerBase::SharedPtr timer_; diff --git a/control/autoware_shift_decider/package.xml b/control/autoware_shift_decider/package.xml index 6ae72a6ade360..a6dd7be0f9675 100644 --- a/control/autoware_shift_decider/package.xml +++ b/control/autoware_shift_decider/package.xml @@ -15,7 +15,7 @@ autoware_control_msgs autoware_system_msgs - autoware_universe_utils + autoware_utils autoware_vehicle_msgs rclcpp rclcpp_components diff --git a/control/autoware_shift_decider/src/autoware_shift_decider.cpp b/control/autoware_shift_decider/src/autoware_shift_decider.cpp index 846eaabee4b88..9ee927a7ee85c 100644 --- a/control/autoware_shift_decider/src/autoware_shift_decider.cpp +++ b/control/autoware_shift_decider/src/autoware_shift_decider.cpp @@ -41,9 +41,9 @@ ShiftDecider::ShiftDecider(const rclcpp::NodeOptions & node_options) void ShiftDecider::onTimer() { - control_cmd_ = sub_control_cmd_.takeData(); - autoware_state_ = sub_autoware_state_.takeData(); - current_gear_ptr_ = sub_current_gear_.takeData(); + control_cmd_ = sub_control_cmd_.take_data(); + autoware_state_ = sub_autoware_state_.take_data(); + current_gear_ptr_ = sub_current_gear_.take_data(); if (!autoware_state_ || !control_cmd_ || !current_gear_ptr_) { return; } diff --git a/control/autoware_smart_mpc_trajectory_follower/package.xml b/control/autoware_smart_mpc_trajectory_follower/package.xml index a5dff3d73da63..20b18e417a020 100644 --- a/control/autoware_smart_mpc_trajectory_follower/package.xml +++ b/control/autoware_smart_mpc_trajectory_follower/package.xml @@ -24,7 +24,7 @@ autoware_motion_utils autoware_planning_msgs autoware_system_msgs - autoware_universe_utils + autoware_utils autoware_vehicle_cmd_gate autoware_vehicle_msgs pybind11_vendor diff --git a/control/autoware_trajectory_follower_base/package.xml b/control/autoware_trajectory_follower_base/package.xml index e7d4bd9599625..20cc45725e83f 100644 --- a/control/autoware_trajectory_follower_base/package.xml +++ b/control/autoware_trajectory_follower_base/package.xml @@ -26,7 +26,7 @@ autoware_motion_utils autoware_osqp_interface autoware_planning_msgs - autoware_universe_utils + autoware_utils autoware_vehicle_info_utils autoware_vehicle_msgs diagnostic_msgs diff --git a/control/autoware_trajectory_follower_node/include/autoware/trajectory_follower_node/controller_node.hpp b/control/autoware_trajectory_follower_node/include/autoware/trajectory_follower_node/controller_node.hpp index c84355cc202fa..994fa19fd3694 100644 --- a/control/autoware_trajectory_follower_node/include/autoware/trajectory_follower_node/controller_node.hpp +++ b/control/autoware_trajectory_follower_node/include/autoware/trajectory_follower_node/controller_node.hpp @@ -19,9 +19,9 @@ #include "autoware/trajectory_follower_base/lateral_controller_base.hpp" #include "autoware/trajectory_follower_base/longitudinal_controller_base.hpp" #include "autoware/trajectory_follower_node/visibility_control.hpp" -#include "autoware/universe_utils/ros/logger_level_configure.hpp" -#include "autoware/universe_utils/ros/polling_subscriber.hpp" -#include "autoware/universe_utils/system/stop_watch.hpp" +#include "autoware_utils/ros/logger_level_configure.hpp" +#include "autoware_utils/ros/polling_subscriber.hpp" +#include "autoware_utils/system/stop_watch.hpp" #include "autoware_vehicle_info_utils/vehicle_info_utils.hpp" #include "rclcpp/rclcpp.hpp" #include "tf2/utils.h" @@ -30,7 +30,7 @@ #include #include -#include +#include #include #include "autoware_control_msgs/msg/control.hpp" @@ -60,10 +60,10 @@ using trajectory_follower::LongitudinalOutput; namespace trajectory_follower_node { -using autoware::universe_utils::StopWatch; using autoware_adapi_v1_msgs::msg::OperationModeState; using autoware_control_msgs::msg::ControlHorizon; using autoware_internal_debug_msgs::msg::Float64Stamped; +using autoware_utils::StopWatch; namespace trajectory_follower = ::autoware::motion::control::trajectory_follower; @@ -89,21 +89,19 @@ class TRAJECTORY_FOLLOWER_PUBLIC Controller : public rclcpp::Node std::shared_ptr lateral_controller_; // Subscribers - autoware::universe_utils::InterProcessPollingSubscriber + autoware_utils::InterProcessPollingSubscriber sub_ref_path_{this, "~/input/reference_trajectory"}; - autoware::universe_utils::InterProcessPollingSubscriber sub_odometry_{ + autoware_utils::InterProcessPollingSubscriber sub_odometry_{ this, "~/input/current_odometry"}; - autoware::universe_utils::InterProcessPollingSubscriber< - autoware_vehicle_msgs::msg::SteeringReport> + autoware_utils::InterProcessPollingSubscriber sub_steering_{this, "~/input/current_steering"}; - autoware::universe_utils::InterProcessPollingSubscriber< - geometry_msgs::msg::AccelWithCovarianceStamped> + autoware_utils::InterProcessPollingSubscriber sub_accel_{this, "~/input/current_accel"}; - autoware::universe_utils::InterProcessPollingSubscriber sub_operation_mode_{ + autoware_utils::InterProcessPollingSubscriber sub_operation_mode_{ this, "~/input/current_operation_mode"}; // Publishers @@ -156,9 +154,9 @@ class TRAJECTORY_FOLLOWER_PUBLIC Controller : public rclcpp::Node const LateralHorizon & lateral_horizon, const LongitudinalHorizon & longitudinal_horizon, const rclcpp::Time & stamp); - std::unique_ptr logger_configure_; + std::unique_ptr logger_configure_; - std::unique_ptr published_time_publisher_; + std::unique_ptr published_time_publisher_; void publishProcessingTime( const double t_ms, const rclcpp::Publisher::SharedPtr pub); diff --git a/control/autoware_trajectory_follower_node/include/autoware/trajectory_follower_node/simple_trajectory_follower.hpp b/control/autoware_trajectory_follower_node/include/autoware/trajectory_follower_node/simple_trajectory_follower.hpp index c23128ebfb695..284f67f6ff382 100644 --- a/control/autoware_trajectory_follower_node/include/autoware/trajectory_follower_node/simple_trajectory_follower.hpp +++ b/control/autoware_trajectory_follower_node/include/autoware/trajectory_follower_node/simple_trajectory_follower.hpp @@ -15,7 +15,7 @@ #ifndef AUTOWARE__TRAJECTORY_FOLLOWER_NODE__SIMPLE_TRAJECTORY_FOLLOWER_HPP_ #define AUTOWARE__TRAJECTORY_FOLLOWER_NODE__SIMPLE_TRAJECTORY_FOLLOWER_HPP_ -#include "autoware/universe_utils/ros/polling_subscriber.hpp" +#include "autoware_utils/ros/polling_subscriber.hpp" #include @@ -44,9 +44,9 @@ class SimpleTrajectoryFollower : public rclcpp::Node ~SimpleTrajectoryFollower() = default; private: - autoware::universe_utils::InterProcessPollingSubscriber sub_kinematics_{ + autoware_utils::InterProcessPollingSubscriber sub_kinematics_{ this, "~/input/kinematics"}; - autoware::universe_utils::InterProcessPollingSubscriber sub_trajectory_{ + autoware_utils::InterProcessPollingSubscriber sub_trajectory_{ this, "~/input/trajectory"}; rclcpp::Publisher::SharedPtr pub_cmd_; rclcpp::TimerBase::SharedPtr timer_; diff --git a/control/autoware_trajectory_follower_node/package.xml b/control/autoware_trajectory_follower_node/package.xml index e12cda7d0de1b..c573ca8f0a62c 100644 --- a/control/autoware_trajectory_follower_node/package.xml +++ b/control/autoware_trajectory_follower_node/package.xml @@ -27,7 +27,7 @@ autoware_planning_msgs autoware_pure_pursuit autoware_trajectory_follower_base - autoware_universe_utils + autoware_utils autoware_vehicle_info_utils autoware_vehicle_msgs rclcpp diff --git a/control/autoware_trajectory_follower_node/src/controller_node.cpp b/control/autoware_trajectory_follower_node/src/controller_node.cpp index d3fc1ba52c95c..fab0f329f0d95 100644 --- a/control/autoware_trajectory_follower_node/src/controller_node.cpp +++ b/control/autoware_trajectory_follower_node/src/controller_node.cpp @@ -17,7 +17,7 @@ #include "autoware/mpc_lateral_controller/mpc_lateral_controller.hpp" #include "autoware/pid_longitudinal_controller/pid_longitudinal_controller.hpp" #include "autoware/pure_pursuit/autoware_pure_pursuit_lateral_controller.hpp" -#include "autoware/universe_utils/ros/marker_helper.hpp" +#include "autoware_utils/ros/marker_helper.hpp" #include @@ -116,10 +116,9 @@ Controller::Controller(const rclcpp::NodeOptions & node_options) : Node("control this, get_clock(), period_ns, std::bind(&Controller::callbackTimerControl, this)); } - logger_configure_ = std::make_unique(this); + logger_configure_ = std::make_unique(this); - published_time_publisher_ = - std::make_unique(this); + published_time_publisher_ = std::make_unique(this); } Controller::LateralControllerMode Controller::getLateralControllerMode( @@ -149,7 +148,7 @@ bool Controller::processData(rclcpp::Clock & clock) }; const auto & getData = [&logData](auto & dest, auto & sub, const std::string & data_type = "") { - const auto temp = sub.takeData(); + const auto temp = sub.take_data(); if (temp) { dest = temp; return true; @@ -268,10 +267,10 @@ void Controller::publishDebugMarker( // steer converged marker { - auto marker = autoware::universe_utils::createDefaultMarker( + auto marker = autoware_utils::create_default_marker( "map", this->now(), "steer_converged", 0, visualization_msgs::msg::Marker::TEXT_VIEW_FACING, - autoware::universe_utils::createMarkerScale(0.0, 0.0, 1.0), - autoware::universe_utils::createMarkerColor(1.0, 1.0, 1.0, 0.99)); + autoware_utils::create_marker_scale(0.0, 0.0, 1.0), + autoware_utils::create_marker_color(1.0, 1.0, 1.0, 0.99)); marker.pose = input_data.current_odometry.pose.pose; std::stringstream ss; diff --git a/control/autoware_trajectory_follower_node/src/simple_trajectory_follower.cpp b/control/autoware_trajectory_follower_node/src/simple_trajectory_follower.cpp index 95082e608d50f..1975264e1b658 100644 --- a/control/autoware_trajectory_follower_node/src/simple_trajectory_follower.cpp +++ b/control/autoware_trajectory_follower_node/src/simple_trajectory_follower.cpp @@ -15,7 +15,7 @@ #include "autoware/trajectory_follower_node/simple_trajectory_follower.hpp" #include -#include +#include #include @@ -23,8 +23,8 @@ namespace simple_trajectory_follower { using autoware::motion_utils::findNearestIndex; -using autoware::universe_utils::calcLateralDeviation; -using autoware::universe_utils::calcYawDeviation; +using autoware_utils::calc_lateral_deviation; +using autoware_utils::calc_yaw_deviation; SimpleTrajectoryFollower::SimpleTrajectoryFollower(const rclcpp::NodeOptions & options) : Node("simple_trajectory_follower", options) @@ -68,9 +68,9 @@ void SimpleTrajectoryFollower::updateClosest() double SimpleTrajectoryFollower::calcSteerCmd() { const auto lat_err = - calcLateralDeviation(closest_traj_point_.pose, odometry_->pose.pose.position) - + calc_lateral_deviation(closest_traj_point_.pose, odometry_->pose.pose.position) - lateral_deviation_; - const auto yaw_err = calcYawDeviation(closest_traj_point_.pose, odometry_->pose.pose); + const auto yaw_err = calc_yaw_deviation(closest_traj_point_.pose, odometry_->pose.pose); // linearized pure_pursuit control constexpr auto wheel_base = 4.0; @@ -109,7 +109,7 @@ bool SimpleTrajectoryFollower::processData() { bool is_ready = true; const auto & getData = [](auto & dest, auto & sub) { - const auto temp = sub.takeData(); + const auto temp = sub.take_data(); if (!temp) return false; dest = temp; return true; diff --git a/control/autoware_vehicle_cmd_gate/package.xml b/control/autoware_vehicle_cmd_gate/package.xml index ba1f31c194946..8410623605917 100644 --- a/control/autoware_vehicle_cmd_gate/package.xml +++ b/control/autoware_vehicle_cmd_gate/package.xml @@ -21,7 +21,7 @@ autoware_control_msgs autoware_internal_debug_msgs autoware_motion_utils - autoware_universe_utils + autoware_utils autoware_vehicle_info_utils autoware_vehicle_msgs diagnostic_updater diff --git a/control/autoware_vehicle_cmd_gate/src/vehicle_cmd_gate.cpp b/control/autoware_vehicle_cmd_gate/src/vehicle_cmd_gate.cpp index a4838ddcdc1f5..2ca8234274802 100644 --- a/control/autoware_vehicle_cmd_gate/src/vehicle_cmd_gate.cpp +++ b/control/autoware_vehicle_cmd_gate/src/vehicle_cmd_gate.cpp @@ -14,7 +14,7 @@ #include "vehicle_cmd_gate.hpp" -#include "autoware/universe_utils/ros/update_param.hpp" +#include "autoware_utils/ros/update_param.hpp" #include "marker_helper.hpp" #include @@ -216,10 +216,9 @@ VehicleCmdGate::VehicleCmdGate(const rclcpp::NodeOptions & node_options) timer_pub_status_ = rclcpp::create_timer( this, get_clock(), period_ns, std::bind(&VehicleCmdGate::publishStatus, this)); - logger_configure_ = std::make_unique(this); + logger_configure_ = std::make_unique(this); - published_time_publisher_ = - std::make_unique(this); + published_time_publisher_ = std::make_unique(this); // Parameter Callback set_param_res_ = @@ -229,25 +228,25 @@ VehicleCmdGate::VehicleCmdGate(const rclcpp::NodeOptions & node_options) rcl_interfaces::msg::SetParametersResult VehicleCmdGate::onParameter( const std::vector & parameters) { - using autoware::universe_utils::updateParam; + using autoware_utils::update_param; // Parameter - updateParam(parameters, "use_emergency_handling", use_emergency_handling_); - updateParam( + update_param(parameters, "use_emergency_handling", use_emergency_handling_); + update_param( parameters, "check_external_emergency_heartbeat", check_external_emergency_heartbeat_); - updateParam( + update_param( parameters, "system_emergency_heartbeat_timeout", system_emergency_heartbeat_timeout_); - updateParam( + update_param( parameters, "external_emergency_stop_heartbeat_timeout", external_emergency_stop_heartbeat_timeout_); - updateParam(parameters, "stop_hold_acceleration", stop_hold_acceleration_); - updateParam(parameters, "emergency_acceleration", emergency_acceleration_); - updateParam( + update_param(parameters, "stop_hold_acceleration", stop_hold_acceleration_); + update_param(parameters, "emergency_acceleration", emergency_acceleration_); + update_param( parameters, "moderate_stop_service_acceleration", moderate_stop_service_acceleration_); - updateParam(parameters, "stop_check_duration", stop_check_duration_); - updateParam(parameters, "enable_cmd_limit_filter", enable_cmd_limit_filter_); - updateParam( + update_param(parameters, "stop_check_duration", stop_check_duration_); + update_param(parameters, "enable_cmd_limit_filter", enable_cmd_limit_filter_); + update_param( parameters, "filter_activated_count_threshold", filter_activated_count_threshold_); - updateParam( + update_param( parameters, "filter_activated_velocity_threshold", filter_activated_velocity_threshold_); // Vehicle Parameter @@ -255,16 +254,16 @@ rcl_interfaces::msg::SetParametersResult VehicleCmdGate::onParameter( { VehicleCmdFilterParam p = filter_.getParam(); p.wheel_base = vehicle_info.wheel_base_m; - updateParam(parameters, "nominal.vel_lim", p.vel_lim); - updateParam>( + update_param(parameters, "nominal.vel_lim", p.vel_lim); + update_param>( parameters, "nominal.reference_speed_points", p.reference_speed_points); - updateParam>(parameters, "nominal.steer_lim", p.steer_lim); - updateParam>(parameters, "nominal.steer_rate_lim", p.steer_rate_lim); - updateParam>(parameters, "nominal.lon_acc_lim", p.lon_acc_lim); - updateParam>(parameters, "nominal.lon_jerk_lim", p.lon_jerk_lim); - updateParam>(parameters, "nominal.lat_acc_lim", p.lat_acc_lim); - updateParam>(parameters, "nominal.lat_jerk_lim", p.lat_jerk_lim); - updateParam>( + update_param>(parameters, "nominal.steer_lim", p.steer_lim); + update_param>(parameters, "nominal.steer_rate_lim", p.steer_rate_lim); + update_param>(parameters, "nominal.lon_acc_lim", p.lon_acc_lim); + update_param>(parameters, "nominal.lon_jerk_lim", p.lon_jerk_lim); + update_param>(parameters, "nominal.lat_acc_lim", p.lat_acc_lim); + update_param>(parameters, "nominal.lat_jerk_lim", p.lat_jerk_lim); + update_param>( parameters, "nominal.actual_steer_diff_lim", p.actual_steer_diff_lim); filter_.setParam(p); } @@ -272,16 +271,16 @@ rcl_interfaces::msg::SetParametersResult VehicleCmdGate::onParameter( { VehicleCmdFilterParam p = filter_on_transition_.getParam(); p.wheel_base = vehicle_info.wheel_base_m; - updateParam(parameters, "on_transition.vel_lim", p.vel_lim); - updateParam>( + update_param(parameters, "on_transition.vel_lim", p.vel_lim); + update_param>( parameters, "on_transition.reference_speed_points", p.reference_speed_points); - updateParam>(parameters, "on_transition.steer_lim", p.steer_lim); - updateParam>(parameters, "on_transition.steer_rate_lim", p.steer_rate_lim); - updateParam>(parameters, "on_transition.lon_acc_lim", p.lon_acc_lim); - updateParam>(parameters, "on_transition.lon_jerk_lim", p.lon_jerk_lim); - updateParam>(parameters, "on_transition.lat_acc_lim", p.lat_acc_lim); - updateParam>(parameters, "on_transition.lat_jerk_lim", p.lat_jerk_lim); - updateParam>( + update_param>(parameters, "on_transition.steer_lim", p.steer_lim); + update_param>(parameters, "on_transition.steer_rate_lim", p.steer_rate_lim); + update_param>(parameters, "on_transition.lon_acc_lim", p.lon_acc_lim); + update_param>(parameters, "on_transition.lon_jerk_lim", p.lon_jerk_lim); + update_param>(parameters, "on_transition.lat_acc_lim", p.lat_acc_lim); + update_param>(parameters, "on_transition.lat_jerk_lim", p.lat_jerk_lim); + update_param>( parameters, "on_transition.actual_steer_diff_lim", p.actual_steer_diff_lim); filter_on_transition_.setParam(p); } @@ -377,37 +376,37 @@ T VehicleCmdGate::getContinuousTopic( void VehicleCmdGate::onTimer() { - autoware::universe_utils::StopWatch stop_watch; + autoware_utils::StopWatch stop_watch; // Subscriber for auto - const auto msg_auto_command_turn_indicator = auto_turn_indicator_cmd_sub_.takeData(); + const auto msg_auto_command_turn_indicator = auto_turn_indicator_cmd_sub_.take_data(); if (msg_auto_command_turn_indicator) auto_commands_.turn_indicator = *msg_auto_command_turn_indicator; - const auto msg_auto_command_hazard_light = auto_hazard_light_cmd_sub_.takeData(); + const auto msg_auto_command_hazard_light = auto_hazard_light_cmd_sub_.take_data(); if (msg_auto_command_hazard_light) auto_commands_.hazard_light = *msg_auto_command_hazard_light; - const auto msg_auto_command_gear = auto_gear_cmd_sub_.takeData(); + const auto msg_auto_command_gear = auto_gear_cmd_sub_.take_data(); if (msg_auto_command_gear) auto_commands_.gear = *msg_auto_command_gear; // Subscribe for external - const auto msg_remote_command_turn_indicator = remote_turn_indicator_cmd_sub_.takeData(); + const auto msg_remote_command_turn_indicator = remote_turn_indicator_cmd_sub_.take_data(); if (msg_remote_command_turn_indicator) remote_commands_.turn_indicator = *msg_remote_command_turn_indicator; - const auto msg_remote_command_hazard_light = remote_hazard_light_cmd_sub_.takeData(); + const auto msg_remote_command_hazard_light = remote_hazard_light_cmd_sub_.take_data(); if (msg_remote_command_hazard_light) remote_commands_.hazard_light = *msg_remote_command_hazard_light; - const auto msg_remote_command_gear = remote_gear_cmd_sub_.takeData(); + const auto msg_remote_command_gear = remote_gear_cmd_sub_.take_data(); if (msg_remote_command_gear) remote_commands_.gear = *msg_remote_command_gear; // Subscribe for emergency - const auto msg_emergency_command_hazard_light = emergency_hazard_light_cmd_sub_.takeData(); + const auto msg_emergency_command_hazard_light = emergency_hazard_light_cmd_sub_.take_data(); if (msg_emergency_command_hazard_light) emergency_commands_.hazard_light = *msg_emergency_command_hazard_light; - const auto msg_emergency_command_gear = emergency_gear_cmd_sub_.takeData(); + const auto msg_emergency_command_gear = emergency_gear_cmd_sub_.take_data(); if (msg_emergency_command_gear) emergency_commands_.gear = *msg_emergency_command_gear; updater_.force_update(); diff --git a/control/autoware_vehicle_cmd_gate/src/vehicle_cmd_gate.hpp b/control/autoware_vehicle_cmd_gate/src/vehicle_cmd_gate.hpp index 094f114abb0a5..ab7d02aaff215 100644 --- a/control/autoware_vehicle_cmd_gate/src/vehicle_cmd_gate.hpp +++ b/control/autoware_vehicle_cmd_gate/src/vehicle_cmd_gate.hpp @@ -16,14 +16,14 @@ #define VEHICLE_CMD_GATE_HPP_ #include "adapi_pause_interface.hpp" -#include "autoware/universe_utils/ros/logger_level_configure.hpp" +#include "autoware_utils/ros/logger_level_configure.hpp" #include "moderate_stop_interface.hpp" #include "vehicle_cmd_filter.hpp" #include -#include -#include -#include +#include +#include +#include #include #include #include @@ -159,31 +159,31 @@ class VehicleCmdGate : public rclcpp::Node // Subscriber for auto Commands auto_commands_; rclcpp::Subscription::SharedPtr auto_control_cmd_sub_; - autoware::universe_utils::InterProcessPollingSubscriber - auto_turn_indicator_cmd_sub_{this, "input/auto/turn_indicators_cmd"}; - autoware::universe_utils::InterProcessPollingSubscriber - auto_hazard_light_cmd_sub_{this, "input/auto/hazard_lights_cmd"}; - autoware::universe_utils::InterProcessPollingSubscriber auto_gear_cmd_sub_{ + autoware_utils::InterProcessPollingSubscriber auto_turn_indicator_cmd_sub_{ + this, "input/auto/turn_indicators_cmd"}; + autoware_utils::InterProcessPollingSubscriber auto_hazard_light_cmd_sub_{ + this, "input/auto/hazard_lights_cmd"}; + autoware_utils::InterProcessPollingSubscriber auto_gear_cmd_sub_{ this, "input/auto/gear_cmd"}; void onAutoCtrlCmd(Control::ConstSharedPtr msg); // Subscription for external Commands remote_commands_; rclcpp::Subscription::SharedPtr remote_control_cmd_sub_; - autoware::universe_utils::InterProcessPollingSubscriber + autoware_utils::InterProcessPollingSubscriber remote_turn_indicator_cmd_sub_{this, "input/external/turn_indicators_cmd"}; - autoware::universe_utils::InterProcessPollingSubscriber - remote_hazard_light_cmd_sub_{this, "input/external/hazard_lights_cmd"}; - autoware::universe_utils::InterProcessPollingSubscriber remote_gear_cmd_sub_{ + autoware_utils::InterProcessPollingSubscriber remote_hazard_light_cmd_sub_{ + this, "input/external/hazard_lights_cmd"}; + autoware_utils::InterProcessPollingSubscriber remote_gear_cmd_sub_{ this, "input/external/gear_cmd"}; void onRemoteCtrlCmd(Control::ConstSharedPtr msg); // Subscription for emergency Commands emergency_commands_; rclcpp::Subscription::SharedPtr emergency_control_cmd_sub_; - autoware::universe_utils::InterProcessPollingSubscriber + autoware_utils::InterProcessPollingSubscriber emergency_hazard_light_cmd_sub_{this, "input/emergency/hazard_lights_cmd"}; - autoware::universe_utils::InterProcessPollingSubscriber emergency_gear_cmd_sub_{ + autoware_utils::InterProcessPollingSubscriber emergency_gear_cmd_sub_{ this, "input/emergency/gear_cmd"}; void onEmergencyCtrlCmd(Control::ConstSharedPtr msg); @@ -275,9 +275,9 @@ class VehicleCmdGate : public rclcpp::Node MarkerArray createMarkerArray(const IsFilterActivated & filter_activated); void publishMarkers(const IsFilterActivated & filter_activated); - std::unique_ptr logger_configure_; + std::unique_ptr logger_configure_; - std::unique_ptr published_time_publisher_; + std::unique_ptr published_time_publisher_; }; } // namespace autoware::vehicle_cmd_gate diff --git a/evaluator/autoware_control_evaluator/include/autoware/control_evaluator/control_evaluator_node.hpp b/evaluator/autoware_control_evaluator/include/autoware/control_evaluator/control_evaluator_node.hpp index c84005ed97a18..6646ce5fd30c9 100644 --- a/evaluator/autoware_control_evaluator/include/autoware/control_evaluator/control_evaluator_node.hpp +++ b/evaluator/autoware_control_evaluator/include/autoware/control_evaluator/control_evaluator_node.hpp @@ -17,12 +17,12 @@ #include "autoware/control_evaluator/metrics/deviation_metrics.hpp" #include "autoware/control_evaluator/metrics/metric.hpp" -#include "autoware/universe_utils/math/accumulator.hpp" +#include "autoware_utils/math/accumulator.hpp" #include -#include -#include -#include +#include +#include +#include #include #include @@ -42,11 +42,11 @@ namespace control_diagnostics { -using autoware::universe_utils::Accumulator; -using autoware::universe_utils::LineString2d; -using autoware::universe_utils::Point2d; using autoware::vehicle_info_utils::VehicleInfo; using autoware_planning_msgs::msg::Trajectory; +using autoware_utils::Accumulator; +using autoware_utils::LineString2d; +using autoware_utils::Point2d; using autoware_vehicle_msgs::msg::SteeringReport; using geometry_msgs::msg::Point; using geometry_msgs::msg::Pose; @@ -83,21 +83,19 @@ class ControlEvaluatorNode : public rclcpp::Node void onTimer(); private: - autoware::universe_utils::InterProcessPollingSubscriber odometry_sub_{ - this, "~/input/odometry"}; - autoware::universe_utils::InterProcessPollingSubscriber accel_sub_{ + autoware_utils::InterProcessPollingSubscriber odometry_sub_{this, "~/input/odometry"}; + autoware_utils::InterProcessPollingSubscriber accel_sub_{ this, "~/input/acceleration"}; - autoware::universe_utils::InterProcessPollingSubscriber traj_sub_{ - this, "~/input/trajectory"}; - autoware::universe_utils::InterProcessPollingSubscriber< - LaneletRoute, autoware::universe_utils::polling_policy::Newest> + autoware_utils::InterProcessPollingSubscriber traj_sub_{this, "~/input/trajectory"}; + autoware_utils::InterProcessPollingSubscriber< + LaneletRoute, autoware_utils::polling_policy::Newest> route_subscriber_{this, "~/input/route", rclcpp::QoS{1}.transient_local()}; - autoware::universe_utils::InterProcessPollingSubscriber< - LaneletMapBin, autoware::universe_utils::polling_policy::Newest> + autoware_utils::InterProcessPollingSubscriber< + LaneletMapBin, autoware_utils::polling_policy::Newest> vector_map_subscriber_{this, "~/input/vector_map", rclcpp::QoS{1}.transient_local()}; - autoware::universe_utils::InterProcessPollingSubscriber behavior_path_subscriber_{ + autoware_utils::InterProcessPollingSubscriber behavior_path_subscriber_{ this, "~/input/behavior_path"}; - autoware::universe_utils::InterProcessPollingSubscriber steering_sub_{ + autoware_utils::InterProcessPollingSubscriber steering_sub_{ this, "~/input/steering_status"}; rclcpp::Publisher::SharedPtr diff --git a/evaluator/autoware_control_evaluator/include/autoware/control_evaluator/metrics/metrics_utils.hpp b/evaluator/autoware_control_evaluator/include/autoware/control_evaluator/metrics/metrics_utils.hpp index 9527b00aa3b9d..1245975026532 100644 --- a/evaluator/autoware_control_evaluator/include/autoware/control_evaluator/metrics/metrics_utils.hpp +++ b/evaluator/autoware_control_evaluator/include/autoware/control_evaluator/metrics/metrics_utils.hpp @@ -16,7 +16,7 @@ #define AUTOWARE__CONTROL_EVALUATOR__METRICS__METRICS_UTILS_HPP_ #include -#include +#include #include namespace control_diagnostics @@ -45,8 +45,8 @@ lanelet::ConstLanelets get_current_lanes(const RouteHandler & route_handler, con *with the line **/ double calc_distance_to_line( - const autoware::universe_utils::LinearRing2d & vehicle_footprint, - const autoware::universe_utils::LineString2d & line); + const autoware_utils::LinearRing2d & vehicle_footprint, + const autoware_utils::LineString2d & line); /** * @brief Check if the point is on the left side of the line. diff --git a/evaluator/autoware_control_evaluator/package.xml b/evaluator/autoware_control_evaluator/package.xml index 8d32c2cf76711..80a96fddaae33 100644 --- a/evaluator/autoware_control_evaluator/package.xml +++ b/evaluator/autoware_control_evaluator/package.xml @@ -20,7 +20,7 @@ autoware_planning_msgs autoware_route_handler autoware_test_utils - autoware_universe_utils + autoware_utils autoware_vehicle_info_utils autoware_vehicle_msgs nav_msgs diff --git a/evaluator/autoware_control_evaluator/src/control_evaluator_node.cpp b/evaluator/autoware_control_evaluator/src/control_evaluator_node.cpp index 205649aba62f2..2d58a1eb02b37 100644 --- a/evaluator/autoware_control_evaluator/src/control_evaluator_node.cpp +++ b/evaluator/autoware_control_evaluator/src/control_evaluator_node.cpp @@ -16,9 +16,9 @@ #include "autoware/control_evaluator/metrics/metrics_utils.hpp" -#include #include #include +#include #include #include @@ -109,7 +109,7 @@ void ControlEvaluatorNode::getRouteData() { // route { - const auto msg = route_subscriber_.takeData(); + const auto msg = route_subscriber_.take_data(); if (msg) { if (msg->segments.empty()) { RCLCPP_ERROR(get_logger(), "input route is empty. ignored"); @@ -121,7 +121,7 @@ void ControlEvaluatorNode::getRouteData() // map { - const auto msg = vector_map_subscriber_.takeData(); + const auto msg = vector_map_subscriber_.take_data(); if (msg) { route_handler_.setMap(*msg); } @@ -176,8 +176,8 @@ void ControlEvaluatorNode::AddBoundaryDistanceMetricMsg( lanelet::ConstLanelet current_lane; lanelet::utils::query::getClosestLanelet(current_lanelets, ego_pose, ¤t_lane); const auto local_vehicle_footprint = vehicle_info_.createFootprint(); - const auto current_vehicle_footprint = autoware::universe_utils::transformVector( - local_vehicle_footprint, autoware::universe_utils::pose2transform(ego_pose)); + const auto current_vehicle_footprint = autoware_utils::transform_vector( + local_vehicle_footprint, autoware_utils::pose2transform(ego_pose)); if (behavior_path.left_bound.size() >= 1) { LineString2d left_boundary; @@ -330,12 +330,12 @@ void ControlEvaluatorNode::AddGoalYawDeviationMetricMsg(const Pose & ego_pose) void ControlEvaluatorNode::onTimer() { - autoware::universe_utils::StopWatch stop_watch; - const auto traj = traj_sub_.takeData(); - const auto odom = odometry_sub_.takeData(); - const auto acc = accel_sub_.takeData(); - const auto behavior_path = behavior_path_subscriber_.takeData(); - const auto steering_status = steering_sub_.takeData(); + autoware_utils::StopWatch stop_watch; + const auto traj = traj_sub_.take_data(); + const auto odom = odometry_sub_.take_data(); + const auto acc = accel_sub_.take_data(); + const auto behavior_path = behavior_path_subscriber_.take_data(); + const auto steering_status = steering_sub_.take_data(); // calculate deviation metrics if (odom && traj && !traj->points.empty()) { diff --git a/evaluator/autoware_control_evaluator/src/metrics/deviation_metrics.cpp b/evaluator/autoware_control_evaluator/src/metrics/deviation_metrics.cpp index aa5b47ad31a4b..65025b3c2e96a 100644 --- a/evaluator/autoware_control_evaluator/src/metrics/deviation_metrics.cpp +++ b/evaluator/autoware_control_evaluator/src/metrics/deviation_metrics.cpp @@ -15,8 +15,8 @@ #include "autoware/control_evaluator/metrics/deviation_metrics.hpp" #include "autoware/motion_utils/trajectory/trajectory.hpp" -#include "autoware/universe_utils/geometry/geometry.hpp" -#include "autoware/universe_utils/geometry/pose_deviation.hpp" +#include "autoware_utils/geometry/geometry.hpp" +#include "autoware_utils/geometry/pose_deviation.hpp" namespace control_diagnostics { @@ -27,30 +27,28 @@ using autoware_planning_msgs::msg::Trajectory; double calcLateralDeviation(const Trajectory & traj, const Point & point) { const size_t nearest_index = autoware::motion_utils::findNearestIndex(traj.points, point); - return std::abs( - autoware::universe_utils::calcLateralDeviation(traj.points[nearest_index].pose, point)); + return std::abs(autoware_utils::calc_lateral_deviation(traj.points[nearest_index].pose, point)); } double calcYawDeviation(const Trajectory & traj, const Pose & pose) { const size_t nearest_index = autoware::motion_utils::findNearestIndex(traj.points, pose.position); - return std::abs( - autoware::universe_utils::calcYawDeviation(traj.points[nearest_index].pose, pose)); + return std::abs(autoware_utils::calc_yaw_deviation(traj.points[nearest_index].pose, pose)); } double calcLongitudinalDeviation(const Pose & base_pose, const Point & target_point) { - return std::abs(autoware::universe_utils::calcLongitudinalDeviation(base_pose, target_point)); + return std::abs(autoware_utils::calc_longitudinal_deviation(base_pose, target_point)); } double calcLateralDeviation(const Pose & base_pose, const Point & target_point) { - return std::abs(autoware::universe_utils::calcLateralDeviation(base_pose, target_point)); + return std::abs(autoware_utils::calc_lateral_deviation(base_pose, target_point)); } double calcYawDeviation(const Pose & base_pose, const Pose & target_pose) { - return std::abs(autoware::universe_utils::calcYawDeviation(base_pose, target_pose)); + return std::abs(autoware_utils::calc_yaw_deviation(base_pose, target_pose)); } } // namespace metrics diff --git a/evaluator/autoware_control_evaluator/src/metrics/metrics_utils.cpp b/evaluator/autoware_control_evaluator/src/metrics/metrics_utils.cpp index 3fe95d4decb8d..013fd7c24abef 100644 --- a/evaluator/autoware_control_evaluator/src/metrics/metrics_utils.cpp +++ b/evaluator/autoware_control_evaluator/src/metrics/metrics_utils.cpp @@ -16,10 +16,10 @@ #include "autoware/motion_utils/trajectory/trajectory.hpp" -#include #include #include #include +#include #include // #include @@ -50,8 +50,7 @@ lanelet::ConstLanelets get_current_lanes(const RouteHandler & route_handler, con } double calc_distance_to_line( - const autoware::universe_utils::LinearRing2d & vehicle_footprint, - const autoware::universe_utils::LineString2d & line) + const autoware_utils::LinearRing2d & vehicle_footprint, const autoware_utils::LineString2d & line) { return boost::geometry::distance(vehicle_footprint, line); } diff --git a/evaluator/autoware_kinematic_evaluator/include/autoware/kinematic_evaluator/kinematic_evaluator_node.hpp b/evaluator/autoware_kinematic_evaluator/include/autoware/kinematic_evaluator/kinematic_evaluator_node.hpp index 053fbfc11f192..148b0498c2b3f 100644 --- a/evaluator/autoware_kinematic_evaluator/include/autoware/kinematic_evaluator/kinematic_evaluator_node.hpp +++ b/evaluator/autoware_kinematic_evaluator/include/autoware/kinematic_evaluator/kinematic_evaluator_node.hpp @@ -16,7 +16,7 @@ #define AUTOWARE__KINEMATIC_EVALUATOR__KINEMATIC_EVALUATOR_NODE_HPP_ #include "autoware/kinematic_evaluator/metrics_calculator.hpp" -#include "autoware/universe_utils/math/accumulator.hpp" +#include "autoware_utils/math/accumulator.hpp" #include "rclcpp/rclcpp.hpp" #include "tf2_ros/buffer.h" #include "tf2_ros/transform_listener.h" @@ -33,7 +33,7 @@ namespace autoware::kinematic_diagnostics { -using autoware::universe_utils::Accumulator; +using autoware_utils::Accumulator; using diagnostic_msgs::msg::DiagnosticArray; using diagnostic_msgs::msg::DiagnosticStatus; diff --git a/evaluator/autoware_kinematic_evaluator/include/autoware/kinematic_evaluator/metrics/kinematic_metrics.hpp b/evaluator/autoware_kinematic_evaluator/include/autoware/kinematic_evaluator/metrics/kinematic_metrics.hpp index 2ae1f60902de7..9f6bd79a4cd16 100644 --- a/evaluator/autoware_kinematic_evaluator/include/autoware/kinematic_evaluator/metrics/kinematic_metrics.hpp +++ b/evaluator/autoware_kinematic_evaluator/include/autoware/kinematic_evaluator/metrics/kinematic_metrics.hpp @@ -15,7 +15,7 @@ #ifndef AUTOWARE__KINEMATIC_EVALUATOR__METRICS__KINEMATIC_METRICS_HPP_ #define AUTOWARE__KINEMATIC_EVALUATOR__METRICS__KINEMATIC_METRICS_HPP_ -#include "autoware/universe_utils/math/accumulator.hpp" +#include "autoware_utils/math/accumulator.hpp" #include @@ -23,7 +23,7 @@ namespace autoware::kinematic_diagnostics { namespace metrics { -using autoware::universe_utils::Accumulator; +using autoware_utils::Accumulator; using nav_msgs::msg::Odometry; /** diff --git a/evaluator/autoware_kinematic_evaluator/include/autoware/kinematic_evaluator/metrics_calculator.hpp b/evaluator/autoware_kinematic_evaluator/include/autoware/kinematic_evaluator/metrics_calculator.hpp index 0a70d6477de1a..498793dabf6f3 100644 --- a/evaluator/autoware_kinematic_evaluator/include/autoware/kinematic_evaluator/metrics_calculator.hpp +++ b/evaluator/autoware_kinematic_evaluator/include/autoware/kinematic_evaluator/metrics_calculator.hpp @@ -17,14 +17,14 @@ #include "autoware/kinematic_evaluator/metrics/metric.hpp" #include "autoware/kinematic_evaluator/parameters.hpp" -#include "autoware/universe_utils/math/accumulator.hpp" +#include "autoware_utils/math/accumulator.hpp" #include "geometry_msgs/msg/pose.hpp" #include namespace autoware::kinematic_diagnostics { -using autoware::universe_utils::Accumulator; +using autoware_utils::Accumulator; using nav_msgs::msg::Odometry; class MetricsCalculator diff --git a/evaluator/autoware_kinematic_evaluator/package.xml b/evaluator/autoware_kinematic_evaluator/package.xml index 7118702290f27..49fc1a0b36f63 100644 --- a/evaluator/autoware_kinematic_evaluator/package.xml +++ b/evaluator/autoware_kinematic_evaluator/package.xml @@ -19,7 +19,7 @@ autoware_cmake autoware_planning_msgs - autoware_universe_utils + autoware_utils diagnostic_msgs eigen geometry_msgs diff --git a/evaluator/autoware_kinematic_evaluator/src/metrics/kinematic_metrics.cpp b/evaluator/autoware_kinematic_evaluator/src/metrics/kinematic_metrics.cpp index c686a60436803..8721ebf5e0531 100644 --- a/evaluator/autoware_kinematic_evaluator/src/metrics/kinematic_metrics.cpp +++ b/evaluator/autoware_kinematic_evaluator/src/metrics/kinematic_metrics.cpp @@ -19,10 +19,10 @@ namespace autoware::kinematic_diagnostics namespace metrics { -autoware::universe_utils::Accumulator updateVelocityStats( - const double & value, const autoware::universe_utils::Accumulator stat_prev) +autoware_utils::Accumulator updateVelocityStats( + const double & value, const autoware_utils::Accumulator stat_prev) { - autoware::universe_utils::Accumulator stat(stat_prev); + autoware_utils::Accumulator stat(stat_prev); stat.add(value); return stat; } diff --git a/evaluator/autoware_localization_evaluator/include/autoware/localization_evaluator/localization_evaluator_node.hpp b/evaluator/autoware_localization_evaluator/include/autoware/localization_evaluator/localization_evaluator_node.hpp index 99f38df4d35e1..a25a2645e3935 100644 --- a/evaluator/autoware_localization_evaluator/include/autoware/localization_evaluator/localization_evaluator_node.hpp +++ b/evaluator/autoware_localization_evaluator/include/autoware/localization_evaluator/localization_evaluator_node.hpp @@ -16,7 +16,7 @@ #define AUTOWARE__LOCALIZATION_EVALUATOR__LOCALIZATION_EVALUATOR_NODE_HPP_ #include "autoware/localization_evaluator/metrics_calculator.hpp" -#include "autoware/universe_utils/math/accumulator.hpp" +#include "autoware_utils/math/accumulator.hpp" #include "rclcpp/rclcpp.hpp" #include "tf2_ros/buffer.h" #include "tf2_ros/transform_listener.h" @@ -38,7 +38,7 @@ namespace autoware::localization_diagnostics { -using autoware::universe_utils::Accumulator; +using autoware_utils::Accumulator; using diagnostic_msgs::msg::DiagnosticArray; using diagnostic_msgs::msg::DiagnosticStatus; using geometry_msgs::msg::PoseWithCovarianceStamped; diff --git a/evaluator/autoware_localization_evaluator/include/autoware/localization_evaluator/metrics/localization_metrics.hpp b/evaluator/autoware_localization_evaluator/include/autoware/localization_evaluator/metrics/localization_metrics.hpp index 78b9e446f7cce..7685d4502a2d7 100644 --- a/evaluator/autoware_localization_evaluator/include/autoware/localization_evaluator/metrics/localization_metrics.hpp +++ b/evaluator/autoware_localization_evaluator/include/autoware/localization_evaluator/metrics/localization_metrics.hpp @@ -15,13 +15,13 @@ #ifndef AUTOWARE__LOCALIZATION_EVALUATOR__METRICS__LOCALIZATION_METRICS_HPP_ #define AUTOWARE__LOCALIZATION_EVALUATOR__METRICS__LOCALIZATION_METRICS_HPP_ -#include "autoware/universe_utils/math/accumulator.hpp" +#include "autoware_utils/math/accumulator.hpp" #include namespace autoware::localization_diagnostics { -using autoware::universe_utils::Accumulator; +using autoware_utils::Accumulator; namespace metrics { /** diff --git a/evaluator/autoware_localization_evaluator/include/autoware/localization_evaluator/metrics_calculator.hpp b/evaluator/autoware_localization_evaluator/include/autoware/localization_evaluator/metrics_calculator.hpp index 6fb86dc9e8d7b..a5cc89c12cf4e 100644 --- a/evaluator/autoware_localization_evaluator/include/autoware/localization_evaluator/metrics_calculator.hpp +++ b/evaluator/autoware_localization_evaluator/include/autoware/localization_evaluator/metrics_calculator.hpp @@ -17,14 +17,14 @@ #include "autoware/localization_evaluator/metrics/metric.hpp" #include "autoware/localization_evaluator/parameters.hpp" -#include "autoware/universe_utils/math/accumulator.hpp" +#include "autoware_utils/math/accumulator.hpp" #include "geometry_msgs/msg/pose.hpp" #include namespace autoware::localization_diagnostics { -using autoware::universe_utils::Accumulator; +using autoware_utils::Accumulator; class MetricsCalculator { public: diff --git a/evaluator/autoware_localization_evaluator/package.xml b/evaluator/autoware_localization_evaluator/package.xml index 69f4b4eeb818c..8a0a50e2e96b6 100644 --- a/evaluator/autoware_localization_evaluator/package.xml +++ b/evaluator/autoware_localization_evaluator/package.xml @@ -19,7 +19,7 @@ autoware_cmake autoware_planning_msgs - autoware_universe_utils + autoware_utils diagnostic_msgs eigen geometry_msgs diff --git a/evaluator/autoware_perception_online_evaluator/include/autoware/perception_online_evaluator/metrics/metric.hpp b/evaluator/autoware_perception_online_evaluator/include/autoware/perception_online_evaluator/metrics/metric.hpp index c190af8162ebf..4143e84e756bb 100644 --- a/evaluator/autoware_perception_online_evaluator/include/autoware/perception_online_evaluator/metrics/metric.hpp +++ b/evaluator/autoware_perception_online_evaluator/include/autoware/perception_online_evaluator/metrics/metric.hpp @@ -15,7 +15,7 @@ #ifndef AUTOWARE__PERCEPTION_ONLINE_EVALUATOR__METRICS__METRIC_HPP_ #define AUTOWARE__PERCEPTION_ONLINE_EVALUATOR__METRICS__METRIC_HPP_ -#include "autoware/universe_utils/math/accumulator.hpp" +#include "autoware_utils/math/accumulator.hpp" #include #include @@ -39,7 +39,7 @@ enum class Metric { // Each metric has a different return type. (statistic or just a one value etc). // To handle them all in the MetricsCalculator::calculate function, define MetricsMap as a variant -using autoware::universe_utils::Accumulator; +using autoware_utils::Accumulator; using MetricStatMap = std::unordered_map>; using MetricValueMap = std::unordered_map; using MetricsMap = std::variant; diff --git a/evaluator/autoware_perception_online_evaluator/include/autoware/perception_online_evaluator/perception_online_evaluator_node.hpp b/evaluator/autoware_perception_online_evaluator/include/autoware/perception_online_evaluator/perception_online_evaluator_node.hpp index 62e4d14af63e0..7da60624aee1b 100644 --- a/evaluator/autoware_perception_online_evaluator/include/autoware/perception_online_evaluator/perception_online_evaluator_node.hpp +++ b/evaluator/autoware_perception_online_evaluator/include/autoware/perception_online_evaluator/perception_online_evaluator_node.hpp @@ -17,7 +17,7 @@ #include "autoware/perception_online_evaluator/metrics_calculator.hpp" #include "autoware/perception_online_evaluator/parameters.hpp" -#include "autoware/universe_utils/math/accumulator.hpp" +#include "autoware_utils/math/accumulator.hpp" #include "rclcpp/rclcpp.hpp" #include "tf2_ros/buffer.h" #include "tf2_ros/transform_listener.h" @@ -36,9 +36,9 @@ namespace autoware::perception_diagnostics { -using autoware::universe_utils::Accumulator; using autoware_perception_msgs::msg::ObjectClassification; using autoware_perception_msgs::msg::PredictedObjects; +using autoware_utils::Accumulator; using nav_msgs::msg::Odometry; using TFMessage = tf2_msgs::msg::TFMessage; diff --git a/evaluator/autoware_perception_online_evaluator/include/autoware/perception_online_evaluator/utils/marker_utils.hpp b/evaluator/autoware_perception_online_evaluator/include/autoware/perception_online_evaluator/utils/marker_utils.hpp index 90afa8cd99ffb..b6ecac871e281 100644 --- a/evaluator/autoware_perception_online_evaluator/include/autoware/perception_online_evaluator/utils/marker_utils.hpp +++ b/evaluator/autoware_perception_online_evaluator/include/autoware/perception_online_evaluator/utils/marker_utils.hpp @@ -15,7 +15,7 @@ #ifndef AUTOWARE__PERCEPTION_ONLINE_EVALUATOR__UTILS__MARKER_UTILS_HPP_ #define AUTOWARE__PERCEPTION_ONLINE_EVALUATOR__UTILS__MARKER_UTILS_HPP_ -#include +#include #include #include "autoware_perception_msgs/msg/predicted_objects.hpp" @@ -31,8 +31,8 @@ namespace autoware::perception_diagnostics::marker_utils { -using autoware::universe_utils::Polygon2d; using autoware_perception_msgs::msg::PredictedObject; +using autoware_utils::Polygon2d; using geometry_msgs::msg::Point; using geometry_msgs::msg::Pose; using std_msgs::msg::ColorRGBA; diff --git a/evaluator/autoware_perception_online_evaluator/package.xml b/evaluator/autoware_perception_online_evaluator/package.xml index eb2779bf70228..06fcf04b1c103 100644 --- a/evaluator/autoware_perception_online_evaluator/package.xml +++ b/evaluator/autoware_perception_online_evaluator/package.xml @@ -21,7 +21,7 @@ autoware_motion_utils autoware_object_recognition_utils autoware_perception_msgs - autoware_universe_utils + autoware_utils autoware_vehicle_info_utils eigen geometry_msgs diff --git a/evaluator/autoware_perception_online_evaluator/src/metrics/detection_count.cpp b/evaluator/autoware_perception_online_evaluator/src/metrics/detection_count.cpp index 3245a80dd7ff5..105f9fb6d2c07 100644 --- a/evaluator/autoware_perception_online_evaluator/src/metrics/detection_count.cpp +++ b/evaluator/autoware_perception_online_evaluator/src/metrics/detection_count.cpp @@ -16,9 +16,9 @@ #include "autoware/object_recognition_utils/object_recognition_utils.hpp" #include "autoware/perception_online_evaluator/utils/objects_filtering.hpp" -#include "autoware/universe_utils/geometry/geometry.hpp" +#include "autoware_utils/geometry/geometry.hpp" -#include +#include #include #include @@ -29,7 +29,7 @@ namespace autoware::perception_diagnostics { namespace metrics { -using autoware::universe_utils::toHexString; +using autoware_utils::to_hex_string; bool isCountObject( const std::uint8_t classification, const std::unordered_map & params) @@ -76,7 +76,7 @@ void DetectionCounter::addObjects( unique_timestamps_.insert(timestamp); for (const auto & object : objects.objects) { - const auto uuid = toHexString(object.object_id); + const auto uuid = to_hex_string(object.object_id); const auto label = autoware::object_recognition_utils::getHighestProbLabel(object.classification); if (!isCountObject(label, parameters_->object_parameters)) { diff --git a/evaluator/autoware_perception_online_evaluator/src/metrics/deviation_metrics.cpp b/evaluator/autoware_perception_online_evaluator/src/metrics/deviation_metrics.cpp index 9aa8632ec7f51..6c34a4d4e1d41 100644 --- a/evaluator/autoware_perception_online_evaluator/src/metrics/deviation_metrics.cpp +++ b/evaluator/autoware_perception_online_evaluator/src/metrics/deviation_metrics.cpp @@ -14,8 +14,8 @@ #include "autoware/perception_online_evaluator/metrics/deviation_metrics.hpp" -#include "autoware/universe_utils/geometry/geometry.hpp" -#include "autoware/universe_utils/geometry/pose_deviation.hpp" +#include "autoware_utils/geometry/geometry.hpp" +#include "autoware_utils/geometry/pose_deviation.hpp" #include @@ -35,7 +35,7 @@ double calcLateralDeviation(const std::vector & ref_path, const Pose & tar const size_t nearest_index = autoware::motion_utils::findNearestIndex(ref_path, target_pose.position); return std::abs( - autoware::universe_utils::calcLateralDeviation(ref_path[nearest_index], target_pose.position)); + autoware_utils::calc_lateral_deviation(ref_path[nearest_index], target_pose.position)); } double calcYawDeviation(const std::vector & ref_path, const Pose & target_pose) @@ -46,7 +46,7 @@ double calcYawDeviation(const std::vector & ref_path, const Pose & target_ const size_t nearest_index = autoware::motion_utils::findNearestIndex(ref_path, target_pose.position); - return std::abs(autoware::universe_utils::calcYawDeviation(ref_path[nearest_index], target_pose)); + return std::abs(autoware_utils::calc_yaw_deviation(ref_path[nearest_index], target_pose)); } } // namespace metrics diff --git a/evaluator/autoware_perception_online_evaluator/src/metrics_calculator.cpp b/evaluator/autoware_perception_online_evaluator/src/metrics_calculator.cpp index 0243e0c7edbbb..1b8f06166edee 100644 --- a/evaluator/autoware_perception_online_evaluator/src/metrics_calculator.cpp +++ b/evaluator/autoware_perception_online_evaluator/src/metrics_calculator.cpp @@ -17,14 +17,14 @@ #include "autoware/motion_utils/trajectory/trajectory.hpp" #include "autoware/object_recognition_utils/object_classification.hpp" #include "autoware/object_recognition_utils/object_recognition_utils.hpp" -#include "autoware/universe_utils/geometry/geometry.hpp" +#include "autoware_utils/geometry/geometry.hpp" -#include +#include namespace autoware::perception_diagnostics { using autoware::object_recognition_utils::convertLabelToString; -using autoware::universe_utils::inverseTransformPoint; +using autoware_utils::inverse_transform_point; std::optional MetricsCalculator::calculate(const Metric & metric) const { @@ -238,7 +238,7 @@ MetricStatMap MetricsCalculator::calcLateralDeviationMetrics( Accumulator stat{}; const auto stamp = rclcpp::Time(objects.header.stamp); for (const auto & object : objects.objects) { - const auto uuid = autoware::universe_utils::toHexString(object.object_id); + const auto uuid = autoware_utils::to_hex_string(object.object_id); if (!hasPassedTime(uuid, stamp)) { continue; } @@ -267,7 +267,7 @@ MetricStatMap MetricsCalculator::calcYawDeviationMetrics( Accumulator stat{}; const auto stamp = rclcpp::Time(objects.header.stamp); for (const auto & object : objects.objects) { - const auto uuid = autoware::universe_utils::toHexString(object.object_id); + const auto uuid = autoware_utils::to_hex_string(object.object_id); if (!hasPassedTime(uuid, stamp)) { continue; } @@ -326,7 +326,7 @@ PredictedPathDeviationMetrics MetricsCalculator::calcPredictedPathDeviationMetri const auto stamp = objects.header.stamp; for (const auto & object : objects.objects) { - const auto uuid = autoware::universe_utils::toHexString(object.object_id); + const auto uuid = autoware_utils::to_hex_string(object.object_id); const auto predicted_paths = object.kinematics.predicted_paths; for (size_t i = 0; i < predicted_paths.size(); i++) { const auto predicted_path = predicted_paths[i]; @@ -349,8 +349,7 @@ PredictedPathDeviationMetrics MetricsCalculator::calcPredictedPathDeviationMetri const auto history_object = history_object_opt.value(); const auto history_pose = history_object.kinematics.initial_pose_with_covariance.pose; const Pose & p = predicted_path.path[j]; - const double distance = - autoware::universe_utils::calcDistance2d(p.position, history_pose.position); + const double distance = autoware_utils::calc_distance2d(p.position, history_pose.position); deviation_map_for_paths[uuid][i].push_back(distance); // Save debug information @@ -424,7 +423,7 @@ MetricStatMap MetricsCalculator::calcYawRateMetrics(const ClassObjectsMap & clas const auto stamp = rclcpp::Time(objects.header.stamp); for (const auto & object : objects.objects) { - const auto uuid = autoware::universe_utils::toHexString(object.object_id); + const auto uuid = autoware_utils::to_hex_string(object.object_id); if (!hasPassedTime(uuid, stamp)) { continue; } @@ -444,7 +443,7 @@ MetricStatMap MetricsCalculator::calcYawRateMetrics(const ClassObjectsMap & clas tf2::getYaw(previous_object.kinematics.initial_pose_with_covariance.pose.orientation); // Calculate the absolute difference between current_yaw and previous_yaw const double yaw_diff = - std::abs(autoware::universe_utils::normalizeRadian(current_yaw - previous_yaw)); + std::abs(autoware_utils::normalize_radian(current_yaw - previous_yaw)); // The yaw difference is flipped if the angle is larger than 90 degrees const double yaw_diff_flip_fixed = std::min(yaw_diff, M_PI - yaw_diff); const double yaw_rate = yaw_diff_flip_fixed / time_diff; @@ -494,9 +493,9 @@ void MetricsCalculator::setPredictedObjects( // store objects to check deviation { - using autoware::universe_utils::toHexString; + using autoware_utils::to_hex_string; for (const auto & object : objects.objects) { - std::string uuid = toHexString(object.object_id); + std::string uuid = to_hex_string(object.object_id); updateObjects(uuid, current_stamp_, object); } deleteOldObjects(current_stamp_); @@ -557,8 +556,7 @@ void MetricsCalculator::updateHistoryPath() const auto current_pose = object.kinematics.initial_pose_with_covariance.pose; const auto prev_pose = prev_object.kinematics.initial_pose_with_covariance.pose; const auto velocity = - autoware::universe_utils::calcDistance2d(current_pose.position, prev_pose.position) / - time_diff; + autoware_utils::calc_distance2d(current_pose.position, prev_pose.position) / time_diff; if (velocity < parameters_->stopped_velocity_threshold) { continue; } @@ -603,9 +601,9 @@ std::vector MetricsCalculator::generateHistoryPathWithPrev( std::vector MetricsCalculator::averageFilterPath( const std::vector & path, const size_t window_size) const { - using autoware::universe_utils::calcAzimuthAngle; - using autoware::universe_utils::calcDistance2d; - using autoware::universe_utils::createQuaternionFromYaw; + using autoware_utils::calc_azimuth_angle; + using autoware_utils::calc_distance2d; + using autoware_utils::create_quaternion_from_yaw; std::vector filtered_path; filtered_path.reserve(path.size()); // Reserve space to avoid reallocations @@ -637,15 +635,15 @@ std::vector MetricsCalculator::averageFilterPath( // skip if the points are too close if ( filtered_path.size() > 0 && - calcDistance2d(filtered_path.back().position, average_pose.position) < 0.5) { + calc_distance2d(filtered_path.back().position, average_pose.position) < 0.5) { continue; } // skip if the difference between the current orientation and the azimuth angle is large if (i > 0) { - const double azimuth = calcAzimuthAngle(path.at(i - 1).position, path.at(i).position); + const double azimuth = calc_azimuth_angle(path.at(i - 1).position, path.at(i).position); const double yaw = tf2::getYaw(path.at(i).orientation); - if (autoware::universe_utils::normalizeRadian(yaw - azimuth) > M_PI_2) { + if (autoware_utils::normalize_radian(yaw - azimuth) > M_PI_2) { continue; } } @@ -659,11 +657,10 @@ std::vector MetricsCalculator::averageFilterPath( if (filtered_path.size() > 2) { auto it = filtered_path.begin() + 2; while (it != filtered_path.end()) { - const double azimuth_to_prev = calcAzimuthAngle((it - 2)->position, (it - 1)->position); - const double azimuth_to_current = calcAzimuthAngle((it - 1)->position, it->position); + const double azimuth_to_prev = calc_azimuth_angle((it - 2)->position, (it - 1)->position); + const double azimuth_to_current = calc_azimuth_angle((it - 1)->position, it->position); if ( - std::abs(autoware::universe_utils::normalizeRadian(azimuth_to_prev - azimuth_to_current)) > - M_PI_2) { + std::abs(autoware_utils::normalize_radian(azimuth_to_prev - azimuth_to_current)) > M_PI_2) { it = filtered_path.erase(it); continue; } @@ -675,8 +672,8 @@ std::vector MetricsCalculator::averageFilterPath( for (size_t i = 0; i < filtered_path.size(); ++i) { Pose & p = filtered_path[i]; if (i < filtered_path.size() - 1) { - const double azimuth_to_next = calcAzimuthAngle(p.position, filtered_path[i + 1].position); - filtered_path[i].orientation = createQuaternionFromYaw(azimuth_to_next); + const double azimuth_to_next = calc_azimuth_angle(p.position, filtered_path[i + 1].position); + filtered_path[i].orientation = create_quaternion_from_yaw(azimuth_to_next); } else if (filtered_path.size() > 1) { // For the last point, use the orientation of the second-to-last point p.orientation = filtered_path[i - 1].orientation; diff --git a/evaluator/autoware_perception_online_evaluator/src/perception_online_evaluator_node.cpp b/evaluator/autoware_perception_online_evaluator/src/perception_online_evaluator_node.cpp index 4a02745b6ad99..9cb5ee80223b8 100644 --- a/evaluator/autoware_perception_online_evaluator/src/perception_online_evaluator_node.cpp +++ b/evaluator/autoware_perception_online_evaluator/src/perception_online_evaluator_node.cpp @@ -15,11 +15,11 @@ #include "autoware/perception_online_evaluator/perception_online_evaluator_node.hpp" #include "autoware/perception_online_evaluator/utils/marker_utils.hpp" -#include "autoware/universe_utils/ros/marker_helper.hpp" -#include "autoware/universe_utils/ros/parameter.hpp" -#include "autoware/universe_utils/ros/update_param.hpp" +#include "autoware_utils/ros/marker_helper.hpp" +#include "autoware_utils/ros/parameter.hpp" +#include "autoware_utils/ros/update_param.hpp" -#include +#include #include "boost/lexical_cast.hpp" @@ -152,7 +152,7 @@ void PerceptionOnlineEvaluatorNode::publishDebugMarker() for (auto & marker : added.markers) { marker.lifetime = rclcpp::Duration::from_seconds(1.5); } - autoware::universe_utils::appendMarkerArray(added, &marker); + autoware_utils::append_marker_array(added, &marker); }; const auto & p = parameters_->debug_marker_parameters; @@ -232,31 +232,32 @@ void PerceptionOnlineEvaluatorNode::publishDebugMarker() rcl_interfaces::msg::SetParametersResult PerceptionOnlineEvaluatorNode::onParameter( const std::vector & parameters) { - using autoware::universe_utils::updateParam; + using autoware_utils::update_param; auto & p = parameters_; - updateParam(parameters, "smoothing_window_size", p->smoothing_window_size); - updateParam(parameters, "stopped_velocity_threshold", p->stopped_velocity_threshold); - updateParam( + update_param(parameters, "smoothing_window_size", p->smoothing_window_size); + update_param(parameters, "stopped_velocity_threshold", p->stopped_velocity_threshold); + update_param( parameters, "detection_count_purge_seconds", p->detection_count_purge_seconds); - updateParam(parameters, "objects_count_window_seconds", p->objects_count_window_seconds); + update_param(parameters, "objects_count_window_seconds", p->objects_count_window_seconds); // update parameters for each object class { const auto update_object_param = [&p, ¶meters]( const auto & semantic, const std::string & ns) { auto & config = p->object_parameters.at(semantic); - updateParam(parameters, ns + "check_lateral_deviation", config.check_lateral_deviation); - updateParam(parameters, ns + "check_yaw_deviation", config.check_yaw_deviation); - updateParam( + update_param( + parameters, ns + "check_lateral_deviation", config.check_lateral_deviation); + update_param(parameters, ns + "check_yaw_deviation", config.check_yaw_deviation); + update_param( parameters, ns + "check_predicted_path_deviation", config.check_predicted_path_deviation); - updateParam(parameters, ns + "check_yaw_rate", config.check_yaw_rate); - updateParam( + update_param(parameters, ns + "check_yaw_rate", config.check_yaw_rate); + update_param( parameters, ns + "check_total_objects_count", config.check_total_objects_count); - updateParam( + update_param( parameters, ns + "check_average_objects_count", config.check_average_objects_count); - updateParam( + update_param( parameters, ns + "check_interval_average_objects_count", config.check_interval_average_objects_count); }; @@ -273,23 +274,23 @@ rcl_interfaces::msg::SetParametersResult PerceptionOnlineEvaluatorNode::onParame // update debug marker parameters { const std::string ns = "debug_marker."; - updateParam( + update_param( parameters, ns + "history_path", p->debug_marker_parameters.show_history_path); - updateParam( + update_param( parameters, ns + "history_path_arrows", p->debug_marker_parameters.show_history_path_arrows); - updateParam( + update_param( parameters, ns + "smoothed_history_path", p->debug_marker_parameters.show_smoothed_history_path); - updateParam( + update_param( parameters, ns + "smoothed_history_path_arrows", p->debug_marker_parameters.show_smoothed_history_path_arrows); - updateParam( + update_param( parameters, ns + "predicted_path", p->debug_marker_parameters.show_predicted_path); - updateParam( + update_param( parameters, ns + "predicted_path_gt", p->debug_marker_parameters.show_predicted_path_gt); - updateParam( + update_param( parameters, ns + "deviation_lines", p->debug_marker_parameters.show_deviation_lines); - updateParam( + update_param( parameters, ns + "object_polygon", p->debug_marker_parameters.show_object_polygon); } @@ -302,28 +303,28 @@ rcl_interfaces::msg::SetParametersResult PerceptionOnlineEvaluatorNode::onParame void PerceptionOnlineEvaluatorNode::initParameter() { - using autoware::universe_utils::getOrDeclareParameter; - using autoware::universe_utils::updateParam; + using autoware_utils::get_or_declare_parameter; + using autoware_utils::update_param; auto & p = parameters_; - p->smoothing_window_size = getOrDeclareParameter(*this, "smoothing_window_size"); + p->smoothing_window_size = get_or_declare_parameter(*this, "smoothing_window_size"); p->prediction_time_horizons = - getOrDeclareParameter>(*this, "prediction_time_horizons"); + get_or_declare_parameter>(*this, "prediction_time_horizons"); p->stopped_velocity_threshold = - getOrDeclareParameter(*this, "stopped_velocity_threshold"); + get_or_declare_parameter(*this, "stopped_velocity_threshold"); p->detection_radius_list = - getOrDeclareParameter>(*this, "detection_radius_list"); + get_or_declare_parameter>(*this, "detection_radius_list"); p->detection_height_list = - getOrDeclareParameter>(*this, "detection_height_list"); + get_or_declare_parameter>(*this, "detection_height_list"); p->detection_count_purge_seconds = - getOrDeclareParameter(*this, "detection_count_purge_seconds"); + get_or_declare_parameter(*this, "detection_count_purge_seconds"); p->objects_count_window_seconds = - getOrDeclareParameter(*this, "objects_count_window_seconds"); + get_or_declare_parameter(*this, "objects_count_window_seconds"); // set metrics const auto selected_metrics = - getOrDeclareParameter>(*this, "selected_metrics"); + get_or_declare_parameter>(*this, "selected_metrics"); for (const std::string & selected_metric : selected_metrics) { const Metric metric = str_to_metric.at(selected_metric); parameters_->metrics.push_back(metric); @@ -334,17 +335,17 @@ void PerceptionOnlineEvaluatorNode::initParameter() const auto get_object_param = [&](std::string && ns) -> ObjectParameter { ObjectParameter param{}; param.check_lateral_deviation = - getOrDeclareParameter(*this, ns + "check_lateral_deviation"); - param.check_yaw_deviation = getOrDeclareParameter(*this, ns + "check_yaw_deviation"); + get_or_declare_parameter(*this, ns + "check_lateral_deviation"); + param.check_yaw_deviation = get_or_declare_parameter(*this, ns + "check_yaw_deviation"); param.check_predicted_path_deviation = - getOrDeclareParameter(*this, ns + "check_predicted_path_deviation"); - param.check_yaw_rate = getOrDeclareParameter(*this, ns + "check_yaw_rate"); + get_or_declare_parameter(*this, ns + "check_predicted_path_deviation"); + param.check_yaw_rate = get_or_declare_parameter(*this, ns + "check_yaw_rate"); param.check_total_objects_count = - getOrDeclareParameter(*this, ns + "check_total_objects_count"); + get_or_declare_parameter(*this, ns + "check_total_objects_count"); param.check_average_objects_count = - getOrDeclareParameter(*this, ns + "check_average_objects_count"); + get_or_declare_parameter(*this, ns + "check_average_objects_count"); param.check_interval_average_objects_count = - getOrDeclareParameter(*this, ns + "check_interval_average_objects_count"); + get_or_declare_parameter(*this, ns + "check_interval_average_objects_count"); return param; }; @@ -365,21 +366,21 @@ void PerceptionOnlineEvaluatorNode::initParameter() { const std::string ns = "debug_marker."; p->debug_marker_parameters.show_history_path = - getOrDeclareParameter(*this, ns + "history_path"); + get_or_declare_parameter(*this, ns + "history_path"); p->debug_marker_parameters.show_history_path_arrows = - getOrDeclareParameter(*this, ns + "history_path_arrows"); + get_or_declare_parameter(*this, ns + "history_path_arrows"); p->debug_marker_parameters.show_smoothed_history_path = - getOrDeclareParameter(*this, ns + "smoothed_history_path"); + get_or_declare_parameter(*this, ns + "smoothed_history_path"); p->debug_marker_parameters.show_smoothed_history_path_arrows = - getOrDeclareParameter(*this, ns + "smoothed_history_path_arrows"); + get_or_declare_parameter(*this, ns + "smoothed_history_path_arrows"); p->debug_marker_parameters.show_predicted_path = - getOrDeclareParameter(*this, ns + "predicted_path"); + get_or_declare_parameter(*this, ns + "predicted_path"); p->debug_marker_parameters.show_predicted_path_gt = - getOrDeclareParameter(*this, ns + "predicted_path_gt"); + get_or_declare_parameter(*this, ns + "predicted_path_gt"); p->debug_marker_parameters.show_deviation_lines = - getOrDeclareParameter(*this, ns + "deviation_lines"); + get_or_declare_parameter(*this, ns + "deviation_lines"); p->debug_marker_parameters.show_object_polygon = - getOrDeclareParameter(*this, ns + "object_polygon"); + get_or_declare_parameter(*this, ns + "object_polygon"); } } } // namespace autoware::perception_diagnostics diff --git a/evaluator/autoware_perception_online_evaluator/src/utils/marker_utils.cpp b/evaluator/autoware_perception_online_evaluator/src/utils/marker_utils.cpp index f38b0dee51af1..f908dec2fd5b5 100644 --- a/evaluator/autoware_perception_online_evaluator/src/utils/marker_utils.cpp +++ b/evaluator/autoware_perception_online_evaluator/src/utils/marker_utils.cpp @@ -14,11 +14,11 @@ #include "autoware/perception_online_evaluator/utils/marker_utils.hpp" -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include #include #include @@ -31,12 +31,12 @@ namespace autoware::perception_diagnostics::marker_utils { -using autoware::universe_utils::calcOffsetPose; -using autoware::universe_utils::createDefaultMarker; -using autoware::universe_utils::createMarkerColor; -using autoware::universe_utils::createMarkerOrientation; -using autoware::universe_utils::createMarkerScale; -using autoware::universe_utils::createPoint; +using autoware_utils::calc_offset_pose; +using autoware_utils::create_default_marker; +using autoware_utils::create_marker_color; +using autoware_utils::create_marker_orientation; +using autoware_utils::create_marker_scale; +using autoware_utils::create_point; using std_msgs::msg::ColorRGBA; using visualization_msgs::msg::Marker; @@ -49,13 +49,13 @@ void addFootprintMarker( const double base_to_rear = vehicle_info.rear_overhang_m; marker.points.push_back( - autoware::universe_utils::calcOffsetPose(pose, base_to_front, -half_width, 0.0).position); + autoware_utils::calc_offset_pose(pose, base_to_front, -half_width, 0.0).position); marker.points.push_back( - autoware::universe_utils::calcOffsetPose(pose, base_to_front, half_width, 0.0).position); + autoware_utils::calc_offset_pose(pose, base_to_front, half_width, 0.0).position); marker.points.push_back( - autoware::universe_utils::calcOffsetPose(pose, -base_to_rear, half_width, 0.0).position); + autoware_utils::calc_offset_pose(pose, -base_to_rear, half_width, 0.0).position); marker.points.push_back( - autoware::universe_utils::calcOffsetPose(pose, -base_to_rear, -half_width, 0.0).position); + autoware_utils::calc_offset_pose(pose, -base_to_rear, -half_width, 0.0).position); marker.points.push_back(marker.points.front()); } @@ -66,9 +66,9 @@ MarkerArray createFootprintMarkerArray( const auto current_time = rclcpp::Clock{RCL_ROS_TIME}.now(); MarkerArray msg; - Marker marker = createDefaultMarker( - "map", current_time, ns, id, Marker::LINE_STRIP, createMarkerScale(0.2, 0.2, 0.2), - createMarkerColor(r, g, b, 0.999)); + Marker marker = create_default_marker( + "map", current_time, ns, id, Marker::LINE_STRIP, create_marker_scale(0.2, 0.2, 0.2), + create_marker_color(r, g, b, 0.999)); marker.lifetime = rclcpp::Duration::from_seconds(1.5); MarkerArray marker_array; @@ -82,9 +82,9 @@ MarkerArray createPointsMarkerArray( const std::vector & points, const std::string & ns, const int32_t id, const float r, const float g, const float b) { - auto marker = createDefaultMarker( + auto marker = create_default_marker( "map", rclcpp::Clock{RCL_ROS_TIME}.now(), ns, id, Marker::LINE_STRIP, - createMarkerScale(0.05, 0.0, 0.0), createMarkerColor(r, g, b, 0.999)); + create_marker_scale(0.05, 0.0, 0.0), create_marker_color(r, g, b, 0.999)); for (const auto & point : points) { marker.points.push_back(point); @@ -99,9 +99,9 @@ MarkerArray createPointsMarkerArray( const std::vector & poses, const std::string & ns, const int32_t id, const float r, const float g, const float b) { - auto marker = createDefaultMarker( + auto marker = create_default_marker( "map", rclcpp::Clock{RCL_ROS_TIME}.now(), ns, id, Marker::LINE_STRIP, - createMarkerScale(0.05, 0.0, 0.0), createMarkerColor(r, g, b, 0.999)); + create_marker_scale(0.05, 0.0, 0.0), create_marker_color(r, g, b, 0.999)); for (const auto & pose : poses) { marker.points.push_back(pose.position); @@ -120,9 +120,9 @@ MarkerArray createDeviationLines( const size_t max_idx = std::min(poses1.size(), poses2.size()); for (size_t i = 0; i < max_idx; ++i) { - auto marker = createDefaultMarker( + auto marker = create_default_marker( "map", rclcpp::Clock{RCL_ROS_TIME}.now(), ns, first_id + i, Marker::LINE_STRIP, - createMarkerScale(0.01, 0.0, 0.0), createMarkerColor(r, g, b, 0.5)); + create_marker_scale(0.01, 0.0, 0.0), create_marker_color(r, g, b, 0.5)); marker.points.push_back(poses1.at(i).position); marker.points.push_back(poses2.at(i).position); msg.markers.push_back(marker); @@ -137,9 +137,9 @@ MarkerArray createPoseMarkerArray( { MarkerArray msg; - Marker marker = createDefaultMarker( + Marker marker = create_default_marker( "map", rclcpp::Clock{RCL_ROS_TIME}.now(), ns, id, Marker::ARROW, - createMarkerScale(0.7, 0.3, 0.3), createMarkerColor(r, g, b, 0.999)); + create_marker_scale(0.7, 0.3, 0.3), create_marker_color(r, g, b, 0.999)); marker.pose = pose; msg.markers.push_back(marker); @@ -154,9 +154,9 @@ MarkerArray createPosesMarkerArray( MarkerArray msg; for (size_t i = 0; i < poses.size(); ++i) { - Marker marker = createDefaultMarker( + Marker marker = create_default_marker( "map", rclcpp::Clock{RCL_ROS_TIME}.now(), ns, first_id + i, Marker::ARROW, - createMarkerScale(x_scale, y_scale, z_scale), createMarkerColor(r, g, b, 0.5)); + create_marker_scale(x_scale, y_scale, z_scale), create_marker_color(r, g, b, 0.5)); marker.pose = poses.at(i); msg.markers.push_back(marker); } @@ -170,7 +170,7 @@ std_msgs::msg::ColorRGBA createColorFromString(const std::string & str) const auto r = (hash & 0xFF) / 255.0; const auto g = ((hash >> 8) & 0xFF) / 255.0; const auto b = ((hash >> 16) & 0xFF) / 255.0; - return autoware::universe_utils::createMarkerColor(r, g, b, 0.5); + return autoware_utils::create_marker_color(r, g, b, 0.5); } MarkerArray createObjectPolygonMarkerArray( @@ -179,17 +179,17 @@ MarkerArray createObjectPolygonMarkerArray( { MarkerArray msg; - auto marker = createDefaultMarker( + auto marker = create_default_marker( "map", rclcpp::Clock{RCL_ROS_TIME}.now(), ns, id, Marker::LINE_STRIP, - createMarkerScale(0.05, 0.0, 0.0), createMarkerColor(r, g, b, 0.999)); + create_marker_scale(0.05, 0.0, 0.0), create_marker_color(r, g, b, 0.999)); const double z = object.kinematics.initial_pose_with_covariance.pose.position.z; const double height = object.shape.dimensions.z; - const auto polygon = autoware::universe_utils::toPolygon2d( - object.kinematics.initial_pose_with_covariance.pose, object.shape); + const auto polygon = + autoware_utils::to_polygon2d(object.kinematics.initial_pose_with_covariance.pose, object.shape); for (const auto & p : polygon.outer()) { - marker.points.push_back(createPoint(p.x(), p.y(), z - height / 2)); - marker.points.push_back(createPoint(p.x(), p.y(), z + height / 2)); + marker.points.push_back(create_point(p.x(), p.y(), z - height / 2)); + marker.points.push_back(create_point(p.x(), p.y(), z + height / 2)); } marker.id = id; msg.markers.push_back(marker); diff --git a/evaluator/autoware_perception_online_evaluator/test/test_perception_online_evaluator_node.cpp b/evaluator/autoware_perception_online_evaluator/test/test_perception_online_evaluator_node.cpp index 9faf680767db8..8ac750b264634 100644 --- a/evaluator/autoware_perception_online_evaluator/test/test_perception_online_evaluator_node.cpp +++ b/evaluator/autoware_perception_online_evaluator/test/test_perception_online_evaluator_node.cpp @@ -17,7 +17,7 @@ #include #include -#include +#include #include #include @@ -44,7 +44,7 @@ using ObjectClassification = autoware_perception_msgs::msg::ObjectClassification using nav_msgs::msg::Odometry; using TFMessage = tf2_msgs::msg::TFMessage; -using autoware::universe_utils::generateUUID; +using autoware_utils::generate_uuid; constexpr double epsilon = 1e-6; @@ -93,7 +93,7 @@ class EvalTest : public ::testing::Test dummy_node, "/perception_online_evaluator/input/objects", 1); tf_pub_ = rclcpp::create_publisher(dummy_node, "/tf", 1); - uuid_ = generateUUID(); + uuid_ = generate_uuid(); } ~EvalTest() override @@ -1169,14 +1169,14 @@ TEST_F(EvalTest, testTotalObjectsCount_DifferentCAR) for (double time = 0; time < time_delay_; time += time_step_) { publishEgoTF(time); publishObjects( - makeStraightPredictedObjects(time, ObjectClassification::CAR, velocity, generateUUID())); + makeStraightPredictedObjects(time, ObjectClassification::CAR, velocity, generate_uuid())); } const double num_objects = 11.0; publishEgoTF(time_delay_); EXPECT_NEAR( publishObjectsAndGetMetric(makeStraightPredictedObjects( - time_delay_, ObjectClassification::CAR, velocity, generateUUID())), + time_delay_, ObjectClassification::CAR, velocity, generate_uuid())), num_objects, epsilon); } diff --git a/evaluator/autoware_planning_evaluator/README.md b/evaluator/autoware_planning_evaluator/README.md index b9dd3201a2541..82b8f51a63bdf 100644 --- a/evaluator/autoware_planning_evaluator/README.md +++ b/evaluator/autoware_planning_evaluator/README.md @@ -21,7 +21,7 @@ which is also responsible for calculating metrics. ### Stat -Each metric is calculated using a `autoware::universe_utils::Accumulator` instance which contains +Each metric is calculated using a `autoware_utils::Accumulator` instance which contains the minimum, maximum, and mean values calculated for the metric as well as the number of values measured. diff --git a/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/metrics/deviation_metrics.hpp b/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/metrics/deviation_metrics.hpp index 2341ad2bb6ba3..ecdb1b4824922 100644 --- a/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/metrics/deviation_metrics.hpp +++ b/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/metrics/deviation_metrics.hpp @@ -15,7 +15,7 @@ #ifndef AUTOWARE__PLANNING_EVALUATOR__METRICS__DEVIATION_METRICS_HPP_ #define AUTOWARE__PLANNING_EVALUATOR__METRICS__DEVIATION_METRICS_HPP_ -#include "autoware/universe_utils/math/accumulator.hpp" +#include "autoware_utils/math/accumulator.hpp" #include "autoware_planning_msgs/msg/trajectory.hpp" #include "autoware_planning_msgs/msg/trajectory_point.hpp" @@ -24,9 +24,9 @@ namespace planning_diagnostics { namespace metrics { -using autoware::universe_utils::Accumulator; using autoware_planning_msgs::msg::Trajectory; using autoware_planning_msgs::msg::TrajectoryPoint; +using autoware_utils::Accumulator; using geometry_msgs::msg::Point; using geometry_msgs::msg::Pose; diff --git a/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/metrics/obstacle_metrics.hpp b/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/metrics/obstacle_metrics.hpp index 55e46bf710450..64c8c747715b5 100644 --- a/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/metrics/obstacle_metrics.hpp +++ b/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/metrics/obstacle_metrics.hpp @@ -15,7 +15,7 @@ #ifndef AUTOWARE__PLANNING_EVALUATOR__METRICS__OBSTACLE_METRICS_HPP_ #define AUTOWARE__PLANNING_EVALUATOR__METRICS__OBSTACLE_METRICS_HPP_ -#include "autoware/universe_utils/math/accumulator.hpp" +#include "autoware_utils/math/accumulator.hpp" #include "autoware_perception_msgs/msg/predicted_objects.hpp" #include "autoware_planning_msgs/msg/trajectory.hpp" @@ -24,9 +24,9 @@ namespace planning_diagnostics { namespace metrics { -using autoware::universe_utils::Accumulator; using autoware_perception_msgs::msg::PredictedObjects; using autoware_planning_msgs::msg::Trajectory; +using autoware_utils::Accumulator; /** * @brief calculate the distance to the closest obstacle at each point of the trajectory diff --git a/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/metrics/stability_metrics.hpp b/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/metrics/stability_metrics.hpp index 1b46fbddfb297..98e0930b1508a 100644 --- a/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/metrics/stability_metrics.hpp +++ b/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/metrics/stability_metrics.hpp @@ -15,7 +15,7 @@ #ifndef AUTOWARE__PLANNING_EVALUATOR__METRICS__STABILITY_METRICS_HPP_ #define AUTOWARE__PLANNING_EVALUATOR__METRICS__STABILITY_METRICS_HPP_ -#include "autoware/universe_utils/math/accumulator.hpp" +#include "autoware_utils/math/accumulator.hpp" #include "autoware_planning_msgs/msg/trajectory.hpp" #include @@ -24,8 +24,8 @@ namespace planning_diagnostics { namespace metrics { -using autoware::universe_utils::Accumulator; using autoware_planning_msgs::msg::Trajectory; +using autoware_utils::Accumulator; /** * @brief calculate the discrete Frechet distance between two trajectories diff --git a/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/metrics/trajectory_metrics.hpp b/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/metrics/trajectory_metrics.hpp index d9784452a343e..df19aa9925e65 100644 --- a/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/metrics/trajectory_metrics.hpp +++ b/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/metrics/trajectory_metrics.hpp @@ -18,7 +18,7 @@ #include "autoware/motion_utils/resample/resample.hpp" #include "autoware/motion_utils/trajectory/conversion.hpp" #include "autoware/motion_utils/trajectory/trajectory.hpp" -#include "autoware/universe_utils/math/accumulator.hpp" +#include "autoware_utils/math/accumulator.hpp" #include "autoware_planning_msgs/msg/trajectory.hpp" #include "autoware_planning_msgs/msg/trajectory_point.hpp" @@ -27,9 +27,9 @@ namespace planning_diagnostics { namespace metrics { -using autoware::universe_utils::Accumulator; using autoware_planning_msgs::msg::Trajectory; using autoware_planning_msgs::msg::TrajectoryPoint; +using autoware_utils::Accumulator; /** * @brief calculate relative angle metric (angle between successive points) diff --git a/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/metrics_calculator.hpp b/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/metrics_calculator.hpp index 55626bd65b19c..379fd488d0538 100644 --- a/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/metrics_calculator.hpp +++ b/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/metrics_calculator.hpp @@ -16,7 +16,7 @@ #define AUTOWARE__PLANNING_EVALUATOR__METRICS_CALCULATOR_HPP_ #include "autoware/planning_evaluator/metrics/metric.hpp" #include "autoware/planning_evaluator/parameters.hpp" -#include "autoware/universe_utils/math/accumulator.hpp" +#include "autoware_utils/math/accumulator.hpp" #include "autoware_perception_msgs/msg/predicted_objects.hpp" #include "autoware_planning_msgs/msg/pose_with_uuid_stamped.hpp" @@ -29,11 +29,11 @@ namespace planning_diagnostics { -using autoware::universe_utils::Accumulator; using autoware_perception_msgs::msg::PredictedObjects; using autoware_planning_msgs::msg::PoseWithUuidStamped; using autoware_planning_msgs::msg::Trajectory; using autoware_planning_msgs::msg::TrajectoryPoint; +using autoware_utils::Accumulator; using geometry_msgs::msg::Point; using geometry_msgs::msg::Pose; diff --git a/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/motion_evaluator_node.hpp b/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/motion_evaluator_node.hpp index 4c5d6c8ced09c..6cc9f9f2b853c 100644 --- a/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/motion_evaluator_node.hpp +++ b/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/motion_evaluator_node.hpp @@ -16,7 +16,7 @@ #define AUTOWARE__PLANNING_EVALUATOR__MOTION_EVALUATOR_NODE_HPP_ #include "autoware/planning_evaluator/metrics_calculator.hpp" -#include "autoware/universe_utils/math/accumulator.hpp" +#include "autoware_utils/math/accumulator.hpp" #include "rclcpp/rclcpp.hpp" #include "tf2_ros/buffer.h" #include "tf2_ros/transform_listener.h" @@ -35,10 +35,10 @@ namespace planning_diagnostics { -using autoware::universe_utils::Accumulator; using autoware::vehicle_info_utils::VehicleInfo; using autoware_planning_msgs::msg::Trajectory; using autoware_planning_msgs::msg::TrajectoryPoint; +using autoware_utils::Accumulator; /** * @brief Node for planning evaluation diff --git a/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/planning_evaluator_node.hpp b/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/planning_evaluator_node.hpp index 2568ea19fdb32..658b86f90e3c9 100644 --- a/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/planning_evaluator_node.hpp +++ b/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/planning_evaluator_node.hpp @@ -16,14 +16,14 @@ #define AUTOWARE__PLANNING_EVALUATOR__PLANNING_EVALUATOR_NODE_HPP_ #include "autoware/planning_evaluator/metrics_calculator.hpp" -#include "autoware/universe_utils/math/accumulator.hpp" +#include "autoware_utils/math/accumulator.hpp" #include "rclcpp/rclcpp.hpp" #include "tf2_ros/buffer.h" #include "tf2_ros/transform_listener.h" #include -#include -#include +#include +#include #include #include "autoware_perception_msgs/msg/predicted_objects.hpp" @@ -44,12 +44,12 @@ #include namespace planning_diagnostics { -using autoware::universe_utils::Accumulator; using autoware::vehicle_info_utils::VehicleInfo; using autoware_perception_msgs::msg::PredictedObjects; using autoware_planning_msgs::msg::PoseWithUuidStamped; using autoware_planning_msgs::msg::Trajectory; using autoware_planning_msgs::msg::TrajectoryPoint; +using autoware_utils::Accumulator; using MetricMsg = tier4_metric_msgs::msg::Metric; using MetricArrayMsg = tier4_metric_msgs::msg::MetricArray; using nav_msgs::msg::Odometry; @@ -128,23 +128,21 @@ class PlanningEvaluatorNode : public rclcpp::Node void onTimer(); // ROS - autoware::universe_utils::InterProcessPollingSubscriber traj_sub_{ - this, "~/input/trajectory"}; - autoware::universe_utils::InterProcessPollingSubscriber ref_sub_{ + autoware_utils::InterProcessPollingSubscriber traj_sub_{this, "~/input/trajectory"}; + autoware_utils::InterProcessPollingSubscriber ref_sub_{ this, "~/input/reference_trajectory"}; - autoware::universe_utils::InterProcessPollingSubscriber objects_sub_{ + autoware_utils::InterProcessPollingSubscriber objects_sub_{ this, "~/input/objects"}; - autoware::universe_utils::InterProcessPollingSubscriber modified_goal_sub_{ + autoware_utils::InterProcessPollingSubscriber modified_goal_sub_{ this, "~/input/modified_goal"}; - autoware::universe_utils::InterProcessPollingSubscriber odometry_sub_{ - this, "~/input/odometry"}; - autoware::universe_utils::InterProcessPollingSubscriber< - LaneletRoute, autoware::universe_utils::polling_policy::Newest> + autoware_utils::InterProcessPollingSubscriber odometry_sub_{this, "~/input/odometry"}; + autoware_utils::InterProcessPollingSubscriber< + LaneletRoute, autoware_utils::polling_policy::Newest> route_subscriber_{this, "~/input/route", rclcpp::QoS{1}.transient_local()}; - autoware::universe_utils::InterProcessPollingSubscriber< - LaneletMapBin, autoware::universe_utils::polling_policy::Newest> + autoware_utils::InterProcessPollingSubscriber< + LaneletMapBin, autoware_utils::polling_policy::Newest> vector_map_subscriber_{this, "~/input/vector_map", rclcpp::QoS{1}.transient_local()}; - autoware::universe_utils::InterProcessPollingSubscriber accel_sub_{ + autoware_utils::InterProcessPollingSubscriber accel_sub_{ this, "~/input/acceleration"}; rclcpp::Publisher::SharedPtr diff --git a/evaluator/autoware_planning_evaluator/package.xml b/evaluator/autoware_planning_evaluator/package.xml index f8d960479085d..4cbab6a798825 100644 --- a/evaluator/autoware_planning_evaluator/package.xml +++ b/evaluator/autoware_planning_evaluator/package.xml @@ -18,7 +18,7 @@ autoware_perception_msgs autoware_planning_msgs autoware_route_handler - autoware_universe_utils + autoware_utils autoware_vehicle_info_utils eigen geometry_msgs diff --git a/evaluator/autoware_planning_evaluator/src/metrics/deviation_metrics.cpp b/evaluator/autoware_planning_evaluator/src/metrics/deviation_metrics.cpp index 9b5959948f8cb..0e665fbe45cc9 100644 --- a/evaluator/autoware_planning_evaluator/src/metrics/deviation_metrics.cpp +++ b/evaluator/autoware_planning_evaluator/src/metrics/deviation_metrics.cpp @@ -15,8 +15,8 @@ #include "autoware/planning_evaluator/metrics/deviation_metrics.hpp" #include "autoware/motion_utils/trajectory/trajectory.hpp" -#include "autoware/universe_utils/geometry/geometry.hpp" -#include "autoware/universe_utils/geometry/pose_deviation.hpp" +#include "autoware_utils/geometry/geometry.hpp" +#include "autoware_utils/geometry/pose_deviation.hpp" namespace planning_diagnostics { @@ -39,8 +39,8 @@ Accumulator calcLateralDeviation(const Trajectory & ref, const Trajector for (TrajectoryPoint p : traj.points) { const size_t nearest_index = autoware::motion_utils::findNearestIndex(ref.points, p.pose.position); - stat.add(autoware::universe_utils::calcLateralDeviation( - ref.points[nearest_index].pose, p.pose.position)); + stat.add( + autoware_utils::calc_lateral_deviation(ref.points[nearest_index].pose, p.pose.position)); } return stat; } @@ -78,7 +78,7 @@ Accumulator calcYawDeviation(const Trajectory & ref, const Trajectory & for (TrajectoryPoint p : traj.points) { const size_t nearest_index = autoware::motion_utils::findNearestIndex(ref.points, p.pose.position); - stat.add(autoware::universe_utils::calcYawDeviation(ref.points[nearest_index].pose, p.pose)); + stat.add(autoware_utils::calc_yaw_deviation(ref.points[nearest_index].pose, p.pose)); } return stat; } @@ -103,21 +103,21 @@ Accumulator calcVelocityDeviation(const Trajectory & ref, const Trajecto Accumulator calcLongitudinalDeviation(const Pose & base_pose, const Point & target_point) { Accumulator stat; - stat.add(std::abs(autoware::universe_utils::calcLongitudinalDeviation(base_pose, target_point))); + stat.add(std::abs(autoware_utils::calc_longitudinal_deviation(base_pose, target_point))); return stat; } Accumulator calcLateralDeviation(const Pose & base_pose, const Point & target_point) { Accumulator stat; - stat.add(std::abs(autoware::universe_utils::calcLateralDeviation(base_pose, target_point))); + stat.add(std::abs(autoware_utils::calc_lateral_deviation(base_pose, target_point))); return stat; } Accumulator calcYawDeviation(const Pose & base_pose, const Pose & target_pose) { Accumulator stat; - stat.add(std::abs(autoware::universe_utils::calcYawDeviation(base_pose, target_pose))); + stat.add(std::abs(autoware_utils::calc_yaw_deviation(base_pose, target_pose))); return stat; } } // namespace metrics diff --git a/evaluator/autoware_planning_evaluator/src/metrics/metrics_utils.cpp b/evaluator/autoware_planning_evaluator/src/metrics/metrics_utils.cpp index 7451264f037a6..16c2cae527ae1 100644 --- a/evaluator/autoware_planning_evaluator/src/metrics/metrics_utils.cpp +++ b/evaluator/autoware_planning_evaluator/src/metrics/metrics_utils.cpp @@ -14,7 +14,7 @@ #include "autoware/motion_utils/trajectory/trajectory.hpp" #include "autoware/planning_evaluator/metrics/trajectory_metrics.hpp" -#include "autoware/universe_utils/geometry/geometry.hpp" +#include "autoware_utils/geometry/geometry.hpp" namespace planning_diagnostics { @@ -22,9 +22,9 @@ namespace metrics { namespace utils { -using autoware::universe_utils::calcDistance2d; using autoware_planning_msgs::msg::Trajectory; using autoware_planning_msgs::msg::TrajectoryPoint; +using autoware_utils::calc_distance2d; using geometry_msgs::msg::Pose; size_t getIndexAfterDistance(const Trajectory & traj, const size_t curr_id, const double distance) @@ -34,7 +34,7 @@ size_t getIndexAfterDistance(const Trajectory & traj, const size_t curr_id, cons size_t target_id = curr_id; for (size_t traj_id = curr_id + 1; traj_id < traj.points.size(); ++traj_id) { - double current_distance = calcDistance2d(traj.points.at(traj_id), curr_p); + double current_distance = calc_distance2d(traj.points.at(traj_id), curr_p); if (current_distance >= distance) { target_id = traj_id; break; @@ -61,8 +61,8 @@ Trajectory get_lookahead_trajectory( auto prev_point_it = curr_point_it; while (curr_point_it != traj.points.end() && dist <= max_dist_m && time <= max_time_s) { lookahead_traj.points.push_back(*curr_point_it); - const auto d = autoware::universe_utils::calcDistance2d( - prev_point_it->pose.position, curr_point_it->pose.position); + const auto d = + autoware_utils::calc_distance2d(prev_point_it->pose.position, curr_point_it->pose.position); dist += d; if (prev_point_it->longitudinal_velocity_mps != 0.0) { time += d / std::abs(prev_point_it->longitudinal_velocity_mps); @@ -81,8 +81,8 @@ double calc_lookahead_trajectory_distance(const Trajectory & traj, const Pose & auto curr_point_it = std::next(traj.points.begin(), ego_index); auto prev_point_it = curr_point_it; for (size_t i = 0; i < traj.points.size(); ++i) { - const auto d = autoware::universe_utils::calcDistance2d( - prev_point_it->pose.position, curr_point_it->pose.position); + const auto d = + autoware_utils::calc_distance2d(prev_point_it->pose.position, curr_point_it->pose.position); dist += d; prev_point_it = curr_point_it; ++curr_point_it; diff --git a/evaluator/autoware_planning_evaluator/src/metrics/obstacle_metrics.cpp b/evaluator/autoware_planning_evaluator/src/metrics/obstacle_metrics.cpp index 0afc8d982a7e9..0387aa952d51f 100644 --- a/evaluator/autoware_planning_evaluator/src/metrics/obstacle_metrics.cpp +++ b/evaluator/autoware_planning_evaluator/src/metrics/obstacle_metrics.cpp @@ -14,7 +14,7 @@ #include "autoware/planning_evaluator/metrics/obstacle_metrics.hpp" -#include "autoware/universe_utils/geometry/geometry.hpp" +#include "autoware_utils/geometry/geometry.hpp" #include @@ -27,8 +27,8 @@ namespace planning_diagnostics { namespace metrics { -using autoware::universe_utils::calcDistance2d; using autoware_planning_msgs::msg::TrajectoryPoint; +using autoware_utils::calc_distance2d; Accumulator calcDistanceToObstacle( const PredictedObjects & obstacles, const Trajectory & traj) @@ -38,7 +38,7 @@ Accumulator calcDistanceToObstacle( double min_dist = std::numeric_limits::max(); for (const auto & object : obstacles.objects) { // TODO(Maxime CLEMENT): take into account the shape, not only the centroid - const auto dist = calcDistance2d(object.kinematics.initial_pose_with_covariance.pose, p); + const auto dist = calc_distance2d(object.kinematics.initial_pose_with_covariance.pose, p); min_dist = std::min(min_dist, dist); } stat.add(min_dist); @@ -60,13 +60,13 @@ Accumulator calcTimeToCollision( double t = 0.0; // [s] time from start of trajectory for (const TrajectoryPoint & p : traj.points) { - const double traj_dist = calcDistance2d(p0, p); + const double traj_dist = calc_distance2d(p0, p); if (p0.longitudinal_velocity_mps != 0) { const double dt = traj_dist / std::abs(p0.longitudinal_velocity_mps); t += dt; for (auto obstacle : obstacles.objects) { const double obstacle_dist = - calcDistance2d(p, obstacle.kinematics.initial_pose_with_covariance.pose); + calc_distance2d(p, obstacle.kinematics.initial_pose_with_covariance.pose); // TODO(Maxime CLEMENT): take shape into consideration if (obstacle_dist <= distance_threshold) { stat.add(t); diff --git a/evaluator/autoware_planning_evaluator/src/metrics/stability_metrics.cpp b/evaluator/autoware_planning_evaluator/src/metrics/stability_metrics.cpp index 61e18a6ad0b63..2e80b8f3c425b 100644 --- a/evaluator/autoware_planning_evaluator/src/metrics/stability_metrics.cpp +++ b/evaluator/autoware_planning_evaluator/src/metrics/stability_metrics.cpp @@ -17,7 +17,7 @@ #include "autoware/motion_utils/resample/resample.hpp" #include "autoware/motion_utils/trajectory/trajectory.hpp" #include "autoware/planning_evaluator/metrics/metrics_utils.hpp" -#include "autoware/universe_utils/geometry/geometry.hpp" +#include "autoware_utils/geometry/geometry.hpp" #include @@ -43,8 +43,7 @@ Accumulator calcFrechetDistance(const Trajectory & traj1, const Trajecto for (size_t i = 0; i < traj1.points.size(); ++i) { for (size_t j = 0; j < traj2.points.size(); ++j) { - const double dist = - autoware::universe_utils::calcDistance2d(traj1.points[i], traj2.points[j]); + const double dist = autoware_utils::calc_distance2d(traj1.points[i], traj2.points[j]); if (i > 0 && j > 0) { ca(i, j) = std::max(std::min(ca(i - 1, j), std::min(ca(i - 1, j - 1), ca(i, j - 1))), dist); } else if (i > 0 /*&& j == 0*/) { @@ -67,7 +66,7 @@ Accumulator calcLateralDistance(const Trajectory & traj1, const Trajecto return stat; } for (const auto & point : traj2.points) { - const auto p0 = autoware::universe_utils::getPoint(point); + const auto p0 = autoware_utils::get_point(point); // find nearest segment const size_t nearest_segment_idx = autoware::motion_utils::findNearestSegmentIndex(traj1.points, p0); @@ -77,19 +76,19 @@ Accumulator calcLateralDistance(const Trajectory & traj1, const Trajecto nearest_segment_idx == traj1.points.size() - 2 && autoware::motion_utils::calcLongitudinalOffsetToSegment( traj1.points, nearest_segment_idx, p0) > - autoware::universe_utils::calcDistance2d( + autoware_utils::calc_distance2d( traj1.points[nearest_segment_idx], traj1.points[nearest_segment_idx + 1])) { // distance to last point - dist = autoware::universe_utils::calcDistance2d(traj1.points.back(), p0); + dist = autoware_utils::calc_distance2d(traj1.points.back(), p0); } else if ( // NOLINT nearest_segment_idx == 0 && autoware::motion_utils::calcLongitudinalOffsetToSegment( traj1.points, nearest_segment_idx, p0) <= 0) { // distance to first point - dist = autoware::universe_utils::calcDistance2d(traj1.points.front(), p0); + dist = autoware_utils::calc_distance2d(traj1.points.front(), p0); } else { // orthogonal distance - const auto p1 = autoware::universe_utils::getPoint(traj1.points[nearest_segment_idx]); - const auto p2 = autoware::universe_utils::getPoint(traj1.points[nearest_segment_idx + 1]); + const auto p1 = autoware_utils::get_point(traj1.points[nearest_segment_idx]); + const auto p2 = autoware_utils::get_point(traj1.points[nearest_segment_idx + 1]); dist = std::abs((p2.x - p1.x) * (p1.y - p0.y) - (p1.x - p0.x) * (p2.y - p1.y)) / std::sqrt((p2.x - p1.x) * (p2.x - p1.x) + (p2.y - p1.y) * (p2.y - p1.y)); } @@ -133,7 +132,7 @@ Accumulator calcLookaheadLateralTrajectoryDisplacement( interval); for (const auto & point : resampled_traj1.points) { - const auto p0 = autoware::universe_utils::getPoint(point); + const auto p0 = autoware_utils::get_point(point); const double dist = autoware::motion_utils::calcLateralOffset(traj2.points, p0); stat.add(std::abs(dist)); } diff --git a/evaluator/autoware_planning_evaluator/src/metrics/trajectory_metrics.cpp b/evaluator/autoware_planning_evaluator/src/metrics/trajectory_metrics.cpp index 9943318490c12..20b1c6c5dad4e 100644 --- a/evaluator/autoware_planning_evaluator/src/metrics/trajectory_metrics.cpp +++ b/evaluator/autoware_planning_evaluator/src/metrics/trajectory_metrics.cpp @@ -15,7 +15,7 @@ #include "autoware/planning_evaluator/metrics/trajectory_metrics.hpp" #include "autoware/planning_evaluator/metrics/metrics_utils.hpp" -#include "autoware/universe_utils/geometry/geometry.hpp" +#include "autoware_utils/geometry/geometry.hpp" #include @@ -23,14 +23,14 @@ namespace planning_diagnostics { namespace metrics { -using autoware::universe_utils::calcCurvature; -using autoware::universe_utils::calcDistance2d; +using autoware_utils::calc_curvature; +using autoware_utils::calc_distance2d; Accumulator calcTrajectoryInterval(const Trajectory & traj) { Accumulator stat; for (size_t i = 1; i < traj.points.size(); ++i) { - stat.add(calcDistance2d(traj.points.at(i), traj.points.at(i - 1))); + stat.add(calc_distance2d(traj.points.at(i), traj.points.at(i - 1))); } return stat; } @@ -103,9 +103,9 @@ Accumulator calcTrajectoryResampledRelativeAngle( // Calc diff yaw between base pose yaw and target pose yaw const double front_diff_yaw = - std::abs(autoware::universe_utils::normalizeRadian(front_target_yaw - base_yaw)); + std::abs(autoware_utils::normalize_radian(front_target_yaw - base_yaw)); const double back_diff_yaw = - std::abs(autoware::universe_utils::normalizeRadian(back_target_yaw - base_yaw)); + std::abs(autoware_utils::normalize_radian(back_target_yaw - base_yaw)); stat.add(std::max(front_diff_yaw, back_diff_yaw)); break; @@ -142,7 +142,7 @@ Accumulator calcTrajectoryCurvature(const Trajectory & traj) break; } - stat.add(calcCurvature(p1, p2, p3)); + stat.add(calc_curvature(p1, p2, p3)); } return stat; } @@ -151,7 +151,7 @@ Accumulator calcTrajectoryLength(const Trajectory & traj) { double length = 0.0; for (size_t i = 1; i < traj.points.size(); ++i) { - length += calcDistance2d(traj.points.at(i), traj.points.at(i - 1)); + length += calc_distance2d(traj.points.at(i), traj.points.at(i - 1)); } Accumulator stat; stat.add(length); @@ -162,7 +162,7 @@ Accumulator calcTrajectoryDuration(const Trajectory & traj) { double duration = 0.0; for (size_t i = 0; i + 1 < traj.points.size(); ++i) { - const double length = calcDistance2d(traj.points.at(i), traj.points.at(i + 1)); + const double length = calc_distance2d(traj.points.at(i), traj.points.at(i + 1)); const double velocity = traj.points.at(i).longitudinal_velocity_mps; if (velocity != 0) { duration += length / std::abs(velocity); @@ -198,7 +198,7 @@ Accumulator calcTrajectoryJerk(const Trajectory & traj) const double vel = traj.points.at(i).longitudinal_velocity_mps; if (vel != 0) { const double duration = - calcDistance2d(traj.points.at(i), traj.points.at(i + 1)) / std::abs(vel); + calc_distance2d(traj.points.at(i), traj.points.at(i + 1)) / std::abs(vel); if (duration != 0) { const double start_accel = traj.points.at(i).acceleration_mps2; const double end_accel = traj.points.at(i + 1).acceleration_mps2; diff --git a/evaluator/autoware_planning_evaluator/src/metrics_calculator.cpp b/evaluator/autoware_planning_evaluator/src/metrics_calculator.cpp index 0a7033877c30e..b97a26cceaf7a 100644 --- a/evaluator/autoware_planning_evaluator/src/metrics_calculator.cpp +++ b/evaluator/autoware_planning_evaluator/src/metrics_calculator.cpp @@ -20,7 +20,7 @@ #include "autoware/planning_evaluator/metrics/obstacle_metrics.hpp" #include "autoware/planning_evaluator/metrics/stability_metrics.hpp" #include "autoware/planning_evaluator/metrics/trajectory_metrics.hpp" -#include "autoware/universe_utils/geometry/geometry.hpp" +#include "autoware_utils/geometry/geometry.hpp" namespace planning_diagnostics { std::optional> MetricsCalculator::calculate( diff --git a/evaluator/autoware_planning_evaluator/src/planning_evaluator_node.cpp b/evaluator/autoware_planning_evaluator/src/planning_evaluator_node.cpp index 848b0cb8091b4..b4af8c14f1c3b 100644 --- a/evaluator/autoware_planning_evaluator/src/planning_evaluator_node.cpp +++ b/evaluator/autoware_planning_evaluator/src/planning_evaluator_node.cpp @@ -130,7 +130,7 @@ void PlanningEvaluatorNode::getRouteData() { // route { - const auto msg = route_subscriber_.takeData(); + const auto msg = route_subscriber_.take_data(); if (msg) { if (msg->segments.empty()) { RCLCPP_ERROR(get_logger(), "input route is empty. ignored"); @@ -142,7 +142,7 @@ void PlanningEvaluatorNode::getRouteData() // map { - const auto msg = vector_map_subscriber_.takeData(); + const auto msg = vector_map_subscriber_.take_data(); if (msg) { route_handler_.setMap(*msg); } @@ -254,26 +254,26 @@ void PlanningEvaluatorNode::AddMetricMsg( void PlanningEvaluatorNode::onTimer() { - autoware::universe_utils::StopWatch stop_watch; + autoware_utils::StopWatch stop_watch; - const auto ego_state_ptr = odometry_sub_.takeData(); + const auto ego_state_ptr = odometry_sub_.take_data(); onOdometry(ego_state_ptr); { - const auto objects_msg = objects_sub_.takeData(); + const auto objects_msg = objects_sub_.take_data(); onObjects(objects_msg); } { - const auto ref_traj_msg = ref_sub_.takeData(); + const auto ref_traj_msg = ref_sub_.take_data(); onReferenceTrajectory(ref_traj_msg); } { - const auto traj_msg = traj_sub_.takeData(); + const auto traj_msg = traj_sub_.take_data(); onTrajectory(traj_msg, ego_state_ptr); } { - const auto modified_goal_msg = modified_goal_sub_.takeData(); + const auto modified_goal_msg = modified_goal_sub_.take_data(); onModifiedGoal(modified_goal_msg, ego_state_ptr); } @@ -361,7 +361,7 @@ void PlanningEvaluatorNode::onOdometry(const Odometry::ConstSharedPtr odometry_m AddLaneletMetricMsg(odometry_msg); } - const auto acc_msg = accel_sub_.takeData(); + const auto acc_msg = accel_sub_.take_data(); if (acc_msg && odometry_msg) { AddKinematicStateMetricMsg(*acc_msg, odometry_msg); } diff --git a/localization/autoware_gyro_odometer/package.xml b/localization/autoware_gyro_odometer/package.xml index c05867de34aaa..13871951777c6 100644 --- a/localization/autoware_gyro_odometer/package.xml +++ b/localization/autoware_gyro_odometer/package.xml @@ -18,7 +18,7 @@ autoware_cmake autoware_localization_util - autoware_universe_utils + autoware_utils diagnostic_msgs fmt geometry_msgs diff --git a/localization/autoware_gyro_odometer/src/gyro_odometer_core.cpp b/localization/autoware_gyro_odometer/src/gyro_odometer_core.cpp index 0a85f75b74ad7..fc39012a1ede6 100644 --- a/localization/autoware_gyro_odometer/src/gyro_odometer_core.cpp +++ b/localization/autoware_gyro_odometer/src/gyro_odometer_core.cpp @@ -34,7 +34,7 @@ namespace autoware::gyro_odometer std::array transform_covariance(const std::array & cov) { - using COV_IDX = autoware::universe_utils::xyz_covariance_index::XYZ_COV_IDX; + using COV_IDX = autoware_utils::xyz_covariance_index::XYZ_COV_IDX; double max_cov = std::max({cov[COV_IDX::X_X], cov[COV_IDX::Y_Y], cov[COV_IDX::Z_Z]}); @@ -53,8 +53,8 @@ GyroOdometerNode::GyroOdometerNode(const rclcpp::NodeOptions & node_options) vehicle_twist_arrived_(false), imu_arrived_(false) { - transform_listener_ = std::make_shared(this); - logger_configure_ = std::make_unique(this); + transform_listener_ = std::make_shared(this); + logger_configure_ = std::make_unique(this); vehicle_twist_sub_ = create_subscription( "vehicle/twist_with_covariance", rclcpp::QoS{100}, @@ -73,7 +73,7 @@ GyroOdometerNode::GyroOdometerNode(const rclcpp::NodeOptions & node_options) "twist_with_covariance", rclcpp::QoS{10}); diagnostics_ = - std::make_unique(this, "gyro_odometer_status"); + std::make_unique(this, "gyro_odometer_status"); // TODO(YamatoAndo) createTimer } @@ -178,7 +178,7 @@ void GyroOdometerNode::concat_gyro_and_odometer() // get transformation geometry_msgs::msg::TransformStamped::ConstSharedPtr tf_imu2base_ptr = - transform_listener_->getLatestTransform(gyro_queue_.front().header.frame_id, output_frame_); + transform_listener_->get_latest_transform(gyro_queue_.front().header.frame_id, output_frame_); const bool is_succeed_transform_imu = (tf_imu2base_ptr != nullptr); diagnostics_->add_key_value("is_succeed_transform_imu", is_succeed_transform_imu); @@ -210,8 +210,8 @@ void GyroOdometerNode::concat_gyro_and_odometer() gyro.angular_velocity_covariance = transform_covariance(gyro.angular_velocity_covariance); } - using COV_IDX_XYZ = autoware::universe_utils::xyz_covariance_index::XYZ_COV_IDX; - using COV_IDX_XYZRPY = autoware::universe_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; + using COV_IDX_XYZ = autoware_utils::xyz_covariance_index::XYZ_COV_IDX; + using COV_IDX_XYZRPY = autoware_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; // calc mean, covariance double vx_mean = 0; diff --git a/localization/autoware_gyro_odometer/src/gyro_odometer_core.hpp b/localization/autoware_gyro_odometer/src/gyro_odometer_core.hpp index b59e6af341ec2..33b5462efcb12 100644 --- a/localization/autoware_gyro_odometer/src/gyro_odometer_core.hpp +++ b/localization/autoware_gyro_odometer/src/gyro_odometer_core.hpp @@ -15,10 +15,10 @@ #ifndef GYRO_ODOMETER_CORE_HPP_ #define GYRO_ODOMETER_CORE_HPP_ -#include "autoware/universe_utils/ros/diagnostics_interface.hpp" -#include "autoware/universe_utils/ros/logger_level_configure.hpp" -#include "autoware/universe_utils/ros/msg_covariance.hpp" -#include "autoware/universe_utils/ros/transform_listener.hpp" +#include "autoware_utils/ros/diagnostics_interface.hpp" +#include "autoware_utils/ros/logger_level_configure.hpp" +#include "autoware_utils/ros/msg_covariance.hpp" +#include "autoware_utils/ros/transform_listener.hpp" #include @@ -43,7 +43,7 @@ namespace autoware::gyro_odometer class GyroOdometerNode : public rclcpp::Node { private: - using COV_IDX = autoware::universe_utils::xyz_covariance_index::XYZ_COV_IDX; + using COV_IDX = autoware_utils::xyz_covariance_index::XYZ_COV_IDX; public: explicit GyroOdometerNode(const rclcpp::NodeOptions & node_options); @@ -67,8 +67,8 @@ class GyroOdometerNode : public rclcpp::Node rclcpp::Publisher::SharedPtr twist_with_covariance_pub_; - std::shared_ptr transform_listener_; - std::unique_ptr logger_configure_; + std::shared_ptr transform_listener_; + std::unique_ptr logger_configure_; std::string output_frame_; double message_timeout_sec_; @@ -80,7 +80,7 @@ class GyroOdometerNode : public rclcpp::Node std::deque vehicle_twist_queue_; std::deque gyro_queue_; - std::unique_ptr diagnostics_; + std::unique_ptr diagnostics_; }; } // namespace autoware::gyro_odometer diff --git a/localization/autoware_landmark_based_localizer/autoware_ar_tag_based_localizer/src/ar_tag_based_localizer.cpp b/localization/autoware_landmark_based_localizer/autoware_ar_tag_based_localizer/src/ar_tag_based_localizer.cpp index 9d2a8bf814694..b0c638197bc1e 100644 --- a/localization/autoware_landmark_based_localizer/autoware_ar_tag_based_localizer/src/ar_tag_based_localizer.cpp +++ b/localization/autoware_landmark_based_localizer/autoware_ar_tag_based_localizer/src/ar_tag_based_localizer.cpp @@ -70,7 +70,7 @@ #include #endif -#include +#include ArTagBasedLocalizer::ArTagBasedLocalizer(const rclcpp::NodeOptions & options) : rclcpp::Node("ar_tag_based_localizer", options), cam_info_received_(false) @@ -192,8 +192,7 @@ void ArTagBasedLocalizer::image_callback(const Image::ConstSharedPtr & msg) pose_array_msg.header.stamp = sensor_stamp; pose_array_msg.header.frame_id = "map"; for (const Landmark & landmark : landmarks) { - const Pose detected_marker_on_map = - autoware::universe_utils::transformPose(landmark.pose, self_pose); + const Pose detected_marker_on_map = autoware_utils::transform_pose(landmark.pose, self_pose); pose_array_msg.poses.push_back(detected_marker_on_map); } detected_tag_pose_pub_->publish(pose_array_msg); @@ -202,7 +201,7 @@ void ArTagBasedLocalizer::image_callback(const Image::ConstSharedPtr & msg) // calc new_self_pose const Pose new_self_pose = landmark_manager_.calculate_new_self_pose(landmarks, self_pose, consider_orientation_); - const Pose diff_pose = autoware::universe_utils::inverseTransformPose(new_self_pose, self_pose); + const Pose diff_pose = autoware_utils::inverse_transform_pose(new_self_pose, self_pose); const double distance = std::hypot(diff_pose.position.x, diff_pose.position.y, diff_pose.position.z); diff --git a/localization/autoware_landmark_based_localizer/autoware_landmark_manager/package.xml b/localization/autoware_landmark_based_localizer/autoware_landmark_manager/package.xml index 3339070979535..ec3c7c1fde187 100644 --- a/localization/autoware_landmark_based_localizer/autoware_landmark_manager/package.xml +++ b/localization/autoware_landmark_based_localizer/autoware_landmark_manager/package.xml @@ -21,7 +21,7 @@ autoware_lanelet2_extension autoware_localization_util autoware_map_msgs - autoware_universe_utils + autoware_utils geometry_msgs rclcpp tf2_eigen diff --git a/localization/autoware_landmark_based_localizer/autoware_landmark_manager/src/landmark_manager.cpp b/localization/autoware_landmark_based_localizer/autoware_landmark_manager/src/landmark_manager.cpp index a4ab83cde6dfa..c46f7633e1c23 100644 --- a/localization/autoware_landmark_based_localizer/autoware_landmark_manager/src/landmark_manager.cpp +++ b/localization/autoware_landmark_based_localizer/autoware_landmark_manager/src/landmark_manager.cpp @@ -15,8 +15,8 @@ #include "autoware/landmark_manager/landmark_manager.hpp" #include "autoware/localization_util/util_func.hpp" -#include "autoware/universe_utils/geometry/geometry.hpp" #include "autoware_lanelet2_extension/utility/message_conversion.hpp" +#include "autoware_utils/geometry/geometry.hpp" #include #include @@ -165,7 +165,7 @@ geometry_msgs::msg::Pose LandmarkManager::calculate_new_self_pose( // convert base_link to map const Pose detected_landmark_on_map = - autoware::universe_utils::transformPose(detected_landmark_on_base_link, self_pose); + autoware_utils::transform_pose(detected_landmark_on_base_link, self_pose); // match to map if (landmarks_map_.count(landmark.id) == 0) { @@ -175,7 +175,7 @@ geometry_msgs::msg::Pose LandmarkManager::calculate_new_self_pose( // check all poses for (const Pose mapped_landmark_on_map : landmarks_map_.at(landmark.id)) { // check distance - const double curr_distance = autoware::universe_utils::calcDistance3d( + const double curr_distance = autoware_utils::calc_distance3d( mapped_landmark_on_map.position, detected_landmark_on_map.position); if (curr_distance > min_distance) { continue; diff --git a/localization/autoware_landmark_based_localizer/autoware_lidar_marker_localizer/package.xml b/localization/autoware_landmark_based_localizer/autoware_lidar_marker_localizer/package.xml index dc0b8cf663c06..d000e76dd4524 100755 --- a/localization/autoware_landmark_based_localizer/autoware_lidar_marker_localizer/package.xml +++ b/localization/autoware_landmark_based_localizer/autoware_lidar_marker_localizer/package.xml @@ -18,7 +18,7 @@ autoware_localization_util autoware_map_msgs autoware_point_types - autoware_universe_utils + autoware_utils pcl_conversions pcl_ros rclcpp diff --git a/localization/autoware_landmark_based_localizer/autoware_lidar_marker_localizer/src/lidar_marker_localizer.cpp b/localization/autoware_landmark_based_localizer/autoware_lidar_marker_localizer/src/lidar_marker_localizer.cpp index 5a43253964154..42769b5ec1cb7 100644 --- a/localization/autoware_landmark_based_localizer/autoware_lidar_marker_localizer/src/lidar_marker_localizer.cpp +++ b/localization/autoware_landmark_based_localizer/autoware_lidar_marker_localizer/src/lidar_marker_localizer.cpp @@ -15,8 +15,8 @@ #include "lidar_marker_localizer.hpp" #include -#include -#include +#include +#include #include #include @@ -125,7 +125,7 @@ LidarMarkerLocalizer::LidarMarkerLocalizer(const rclcpp::NodeOptions & node_opti tf_listener_ = std::make_shared(*tf_buffer_, this, false); diagnostics_interface_.reset( - new autoware::universe_utils::DiagnosticsInterface(this, "marker_detection_status")); + new autoware_utils::DiagnosticsInterface(this, "marker_detection_status")); } void LidarMarkerLocalizer::initialize_diagnostics() @@ -226,7 +226,7 @@ void LidarMarkerLocalizer::main_process(const PointCloud2::ConstSharedPtr & poin // (4) check distance to the nearest marker const landmark_manager::Landmark nearest_marker = get_nearest_landmark(self_pose, map_landmarks); const Pose nearest_marker_pose_on_base_link = - autoware::universe_utils::inverseTransformPose(nearest_marker.pose, self_pose); + autoware_utils::inverse_transform_pose(nearest_marker.pose, self_pose); const double distance_from_self_pose_to_nearest_marker = std::abs(nearest_marker_pose_on_base_link.position.x); @@ -261,8 +261,7 @@ void LidarMarkerLocalizer::main_process(const PointCloud2::ConstSharedPtr & poin pose_array_msg.header.stamp = sensor_ros_time; pose_array_msg.header.frame_id = "map"; for (const landmark_manager::Landmark & landmark : detected_landmarks) { - const Pose detected_marker_on_map = - autoware::universe_utils::transformPose(landmark.pose, self_pose); + const Pose detected_marker_on_map = autoware_utils::transform_pose(landmark.pose, self_pose); pose_array_msg.poses.push_back(detected_marker_on_map); } pub_marker_detected_->publish(pose_array_msg); @@ -459,7 +458,7 @@ std::vector LidarMarkerLocalizer::detect_landmarks( marker_pose_on_base_link.position.y = min_y[i]; marker_pose_on_base_link.position.z = param_.marker_height_from_ground; marker_pose_on_base_link.orientation = - autoware::universe_utils::createQuaternionFromRPY(M_PI_2, 0.0, 0.0); // TODO(YamatoAndo) + autoware_utils::create_quaternion_from_rpy(M_PI_2, 0.0, 0.0); // TODO(YamatoAndo) detected_landmarks.push_back(landmark_manager::Landmark{"0", marker_pose_on_base_link}); } } @@ -476,7 +475,7 @@ landmark_manager::Landmark get_nearest_landmark( for (const auto & landmark : landmarks) { const double curr_distance = - autoware::universe_utils::calcDistance3d(landmark.pose.position, self_pose.position); + autoware_utils::calc_distance3d(landmark.pose.position, self_pose.position); if (curr_distance > min_distance) { continue; @@ -614,7 +613,7 @@ void LidarMarkerLocalizer::transform_sensor_measurement( } const geometry_msgs::msg::PoseStamped target_to_source_pose_stamped = - autoware::universe_utils::transform2pose(transform); + autoware_utils::transform2pose(transform); const Eigen::Matrix4f base_to_sensor_matrix = autoware::localization_util::pose_to_matrix4f(target_to_source_pose_stamped.pose); pcl_ros::transformPointCloud( diff --git a/localization/autoware_landmark_based_localizer/autoware_lidar_marker_localizer/src/lidar_marker_localizer.hpp b/localization/autoware_landmark_based_localizer/autoware_lidar_marker_localizer/src/lidar_marker_localizer.hpp index 22a0c3f642563..292a97e2a7b73 100644 --- a/localization/autoware_landmark_based_localizer/autoware_lidar_marker_localizer/src/lidar_marker_localizer.hpp +++ b/localization/autoware_landmark_based_localizer/autoware_lidar_marker_localizer/src/lidar_marker_localizer.hpp @@ -16,7 +16,7 @@ #define LIDAR_MARKER_LOCALIZER_HPP_ #include "autoware/localization_util/smart_pose_buffer.hpp" -#include "autoware/universe_utils/ros/diagnostics_interface.hpp" +#include "autoware_utils/ros/diagnostics_interface.hpp" #include @@ -134,7 +134,7 @@ class LidarMarkerLocalizer : public rclcpp::Node rclcpp::Publisher::SharedPtr pub_debug_pose_with_covariance_; rclcpp::Publisher::SharedPtr pub_marker_pointcloud_; - std::shared_ptr diagnostics_interface_; + std::shared_ptr diagnostics_interface_; Param param_; bool is_activated_; diff --git a/localization/autoware_localization_error_monitor/package.xml b/localization/autoware_localization_error_monitor/package.xml index d3ba2e2852e51..469f073c564b2 100644 --- a/localization/autoware_localization_error_monitor/package.xml +++ b/localization/autoware_localization_error_monitor/package.xml @@ -20,7 +20,7 @@ eigen autoware_localization_util - autoware_universe_utils + autoware_utils diagnostic_msgs nav_msgs rclcpp_components diff --git a/localization/autoware_localization_error_monitor/src/localization_error_monitor.cpp b/localization/autoware_localization_error_monitor/src/localization_error_monitor.cpp index 1f26f6b80aa17..f88c19dfd7a34 100644 --- a/localization/autoware_localization_error_monitor/src/localization_error_monitor.cpp +++ b/localization/autoware_localization_error_monitor/src/localization_error_monitor.cpp @@ -56,10 +56,10 @@ LocalizationErrorMonitor::LocalizationErrorMonitor(const rclcpp::NodeOptions & o ellipse_marker_pub_ = this->create_publisher("debug/ellipse_marker", durable_qos); - logger_configure_ = std::make_unique(this); + logger_configure_ = std::make_unique(this); diagnostics_error_monitor_ = - std::make_unique(this, "ellipse_error_status"); + std::make_unique(this, "ellipse_error_status"); } void LocalizationErrorMonitor::on_odom(nav_msgs::msg::Odometry::ConstSharedPtr input_msg) diff --git a/localization/autoware_localization_error_monitor/src/localization_error_monitor.hpp b/localization/autoware_localization_error_monitor/src/localization_error_monitor.hpp index b7d2454eb9f75..69a7be673d07a 100644 --- a/localization/autoware_localization_error_monitor/src/localization_error_monitor.hpp +++ b/localization/autoware_localization_error_monitor/src/localization_error_monitor.hpp @@ -16,10 +16,10 @@ #define LOCALIZATION_ERROR_MONITOR_HPP_ #include "autoware/localization_util/covariance_ellipse.hpp" -#include "autoware/universe_utils/ros/diagnostics_interface.hpp" +#include "autoware_utils/ros/diagnostics_interface.hpp" #include -#include +#include #include #include @@ -37,9 +37,9 @@ class LocalizationErrorMonitor : public rclcpp::Node rclcpp::TimerBase::SharedPtr timer_; - std::unique_ptr logger_configure_; + std::unique_ptr logger_configure_; - std::unique_ptr diagnostics_error_monitor_; + std::unique_ptr diagnostics_error_monitor_; double scale_; double error_ellipse_size_; diff --git a/localization/autoware_ndt_scan_matcher/include/autoware/ndt_scan_matcher/map_update_module.hpp b/localization/autoware_ndt_scan_matcher/include/autoware/ndt_scan_matcher/map_update_module.hpp index 12990259f88cd..20d7ce33a7f41 100644 --- a/localization/autoware_ndt_scan_matcher/include/autoware/ndt_scan_matcher/map_update_module.hpp +++ b/localization/autoware_ndt_scan_matcher/include/autoware/ndt_scan_matcher/map_update_module.hpp @@ -16,13 +16,13 @@ #define AUTOWARE__NDT_SCAN_MATCHER__MAP_UPDATE_MODULE_HPP_ #include "autoware/localization_util/util_func.hpp" -#include "autoware/universe_utils/ros/diagnostics_interface.hpp" +#include "autoware_utils/ros/diagnostics_interface.hpp" #include "hyper_parameters.hpp" #include "ndt_omp/multigrid_ndt_omp.h" #include "particle.hpp" -#include -#include +#include +#include #include #include @@ -42,7 +42,7 @@ namespace autoware::ndt_scan_matcher { -using DiagnosticsInterface = autoware::universe_utils::DiagnosticsInterface; +using DiagnosticsInterface = autoware_utils::DiagnosticsInterface; class MapUpdateModule { diff --git a/localization/autoware_ndt_scan_matcher/include/autoware/ndt_scan_matcher/ndt_scan_matcher_core.hpp b/localization/autoware_ndt_scan_matcher/include/autoware/ndt_scan_matcher/ndt_scan_matcher_core.hpp index d6e629ee2f1c3..edc63ede69bcb 100644 --- a/localization/autoware_ndt_scan_matcher/include/autoware/ndt_scan_matcher/ndt_scan_matcher_core.hpp +++ b/localization/autoware_ndt_scan_matcher/include/autoware/ndt_scan_matcher/ndt_scan_matcher_core.hpp @@ -18,12 +18,12 @@ #define FMT_HEADER_ONLY #include "autoware/localization_util/smart_pose_buffer.hpp" -#include "autoware/universe_utils/ros/diagnostics_interface.hpp" +#include "autoware_utils/ros/diagnostics_interface.hpp" #include "hyper_parameters.hpp" #include "map_update_module.hpp" #include "ndt_omp/multigrid_ndt_omp.h" -#include +#include #include #include @@ -219,7 +219,7 @@ class NDTScanMatcher : public rclcpp::Node std::unique_ptr diagnostics_ndt_align_; std::unique_ptr diagnostics_trigger_node_; std::unique_ptr map_update_module_; - std::unique_ptr logger_configure_; + std::unique_ptr logger_configure_; HyperParameters param_; }; diff --git a/localization/autoware_ndt_scan_matcher/package.xml b/localization/autoware_ndt_scan_matcher/package.xml index bdcee3a98f043..71d2691cc4bee 100644 --- a/localization/autoware_ndt_scan_matcher/package.xml +++ b/localization/autoware_ndt_scan_matcher/package.xml @@ -20,7 +20,7 @@ autoware_internal_debug_msgs autoware_localization_util autoware_map_msgs - autoware_universe_utils + autoware_utils diagnostic_msgs fmt geometry_msgs diff --git a/localization/autoware_ndt_scan_matcher/src/ndt_scan_matcher_core.cpp b/localization/autoware_ndt_scan_matcher/src/ndt_scan_matcher_core.cpp index 3d06dfffa16ed..5bde859d8247c 100644 --- a/localization/autoware_ndt_scan_matcher/src/ndt_scan_matcher_core.cpp +++ b/localization/autoware_ndt_scan_matcher/src/ndt_scan_matcher_core.cpp @@ -20,8 +20,8 @@ #include "autoware/ndt_scan_matcher/ndt_omp/estimate_covariance.hpp" #include "autoware/ndt_scan_matcher/particle.hpp" -#include -#include +#include +#include #include @@ -51,7 +51,7 @@ using autoware::localization_util::pose_to_matrix4f; using autoware::localization_util::SmartPoseBuffer; using autoware::localization_util::TreeStructuredParzenEstimator; -using autoware::universe_utils::DiagnosticsInterface; +using autoware_utils::DiagnosticsInterface; autoware_internal_debug_msgs::msg::Float32Stamped make_float32_stamped( const builtin_interfaces::msg::Time & stamp, const float data) @@ -220,7 +220,7 @@ NDTScanMatcher::NDTScanMatcher(const rclcpp::NodeOptions & options) diagnostics_trigger_node_ = std::make_unique(this, "trigger_node_service_status"); - logger_configure_ = std::make_unique(this); + logger_configure_ = std::make_unique(this); } void NDTScanMatcher::callback_timer() @@ -650,7 +650,7 @@ bool NDTScanMatcher::callback_sensor_points_main( pcl::shared_ptr> sensor_points_in_map_ptr( new pcl::PointCloud); - autoware::universe_utils::transformPointCloud( + autoware_utils::transform_pointcloud( *sensor_points_in_baselink_frame, *sensor_points_in_map_ptr, ndt_result.pose); publish_point_cloud(sensor_ros_time, param_.frame.map_frame, sensor_points_in_map_ptr); @@ -721,10 +721,10 @@ void NDTScanMatcher::transform_sensor_measurement( } const geometry_msgs::msg::PoseStamped target_to_source_pose_stamped = - autoware::universe_utils::transform2pose(transform); + autoware_utils::transform2pose(transform); const Eigen::Matrix4f base_to_sensor_matrix = pose_to_matrix4f(target_to_source_pose_stamped.pose); - autoware::universe_utils::transformPointCloud( + autoware_utils::transform_pointcloud( *sensor_points_input_ptr, *sensor_points_output_ptr, base_to_sensor_matrix); } @@ -736,7 +736,7 @@ void NDTScanMatcher::publish_tf( result_pose_stamped_msg.header.frame_id = param_.frame.map_frame; result_pose_stamped_msg.pose = result_pose_msg; tf2_broadcaster_.sendTransform( - autoware::universe_utils::pose2transform(result_pose_stamped_msg, param_.frame.ndt_base_frame)); + autoware_utils::pose2transform(result_pose_stamped_msg, param_.frame.ndt_base_frame)); } void NDTScanMatcher::publish_pose( @@ -780,7 +780,7 @@ void NDTScanMatcher::publish_marker( marker.header.frame_id = param_.frame.map_frame; marker.type = visualization_msgs::msg::Marker::ARROW; marker.action = visualization_msgs::msg::Marker::ADD; - marker.scale = autoware::universe_utils::createMarkerScale(0.3, 0.1, 0.1); + marker.scale = autoware_utils::create_marker_scale(0.3, 0.1, 0.1); int i = 0; marker.ns = "result_pose_matrix_array"; marker.action = visualization_msgs::msg::Marker::ADD; @@ -809,7 +809,7 @@ void NDTScanMatcher::publish_initial_to_result( { geometry_msgs::msg::PoseStamped initial_to_result_relative_pose_stamped; initial_to_result_relative_pose_stamped.pose = - autoware::universe_utils::inverseTransformPose(result_pose_msg, initial_pose_cov_msg.pose.pose); + autoware_utils::inverse_transform_pose(result_pose_msg, initial_pose_cov_msg.pose.pose); initial_to_result_relative_pose_stamped.header.stamp = sensor_ros_time; initial_to_result_relative_pose_stamped.header.frame_id = param_.frame.map_frame; initial_to_result_relative_pose_pub_->publish(initial_to_result_relative_pose_stamped); @@ -1157,7 +1157,7 @@ std::tuple NDTScanMatcher tpe.add_trial(TreeStructuredParzenEstimator::Trial{result, ndt_result.transform_probability}); auto sensor_points_in_map_ptr = std::make_shared>(); - autoware::universe_utils::transformPointCloud( + autoware_utils::transform_pointcloud( *ndt_ptr_->getInputSource(), *sensor_points_in_map_ptr, ndt_result.pose); publish_point_cloud( initial_pose_with_cov.header.stamp, param_.frame.map_frame, sensor_points_in_map_ptr); diff --git a/localization/autoware_pose_estimator_arbiter/example_rule/src/switch_rule/pcd_map_based_rule.cpp b/localization/autoware_pose_estimator_arbiter/example_rule/src/switch_rule/pcd_map_based_rule.cpp index 023b49c854621..721d1db7fedb2 100644 --- a/localization/autoware_pose_estimator_arbiter/example_rule/src/switch_rule/pcd_map_based_rule.cpp +++ b/localization/autoware_pose_estimator_arbiter/example_rule/src/switch_rule/pcd_map_based_rule.cpp @@ -14,7 +14,7 @@ #include "switch_rule/pcd_map_based_rule.hpp" -#include +#include #include #include @@ -31,9 +31,9 @@ PcdMapBasedRule::PcdMapBasedRule( shared_data_(std::move(shared_data)) { const int pcd_density_upper_threshold = - autoware::universe_utils::getOrDeclareParameter(node, "pcd_density_upper_threshold"); + autoware_utils::get_or_declare_parameter(node, "pcd_density_upper_threshold"); const int pcd_density_lower_threshold = - autoware::universe_utils::getOrDeclareParameter(node, "pcd_density_lower_threshold"); + autoware_utils::get_or_declare_parameter(node, "pcd_density_lower_threshold"); pcd_occupancy_ = std::make_unique( pcd_density_upper_threshold, pcd_density_lower_threshold); diff --git a/localization/autoware_pose_estimator_arbiter/package.xml b/localization/autoware_pose_estimator_arbiter/package.xml index b9ffae2137037..d8318de302f12 100644 --- a/localization/autoware_pose_estimator_arbiter/package.xml +++ b/localization/autoware_pose_estimator_arbiter/package.xml @@ -20,7 +20,7 @@ autoware_adapi_v1_msgs autoware_lanelet2_extension autoware_map_msgs - autoware_universe_utils + autoware_utils diagnostic_msgs geometry_msgs magic_enum diff --git a/localization/autoware_pose_estimator_arbiter/src/pose_estimator_arbiter.hpp b/localization/autoware_pose_estimator_arbiter/src/pose_estimator_arbiter.hpp index b70ab378eac67..a3f3170737fd1 100644 --- a/localization/autoware_pose_estimator_arbiter/src/pose_estimator_arbiter.hpp +++ b/localization/autoware_pose_estimator_arbiter/src/pose_estimator_arbiter.hpp @@ -19,7 +19,7 @@ #include "stopper/base_stopper.hpp" #include "switch_rule/base_switch_rule.hpp" -#include +#include #include #include @@ -53,7 +53,7 @@ class PoseEstimatorArbiter : public rclcpp::Node // Set of running pose estimators specified by ros param `pose_sources` const std::unordered_set running_estimator_list_; // Configuration to allow changing the log level by service - const std::unique_ptr logger_configure_; + const std::unique_ptr logger_configure_; // This is passed to several modules (stoppers & rule) so that all modules can access common data // without passing them as arguments. Also, modules can register subscriber callbacks through diff --git a/localization/autoware_pose_estimator_arbiter/src/pose_estimator_arbiter_core.cpp b/localization/autoware_pose_estimator_arbiter/src/pose_estimator_arbiter_core.cpp index c62c533f23093..ff1289ef2ce24 100644 --- a/localization/autoware_pose_estimator_arbiter/src/pose_estimator_arbiter_core.cpp +++ b/localization/autoware_pose_estimator_arbiter/src/pose_estimator_arbiter_core.cpp @@ -54,7 +54,7 @@ PoseEstimatorArbiter::PoseEstimatorArbiter(const rclcpp::NodeOptions & options) : rclcpp::Node("pose_estimator_arbiter", options), running_estimator_list_(parse_estimator_name_args( declare_parameter>("pose_sources"), get_logger())), - logger_configure_(std::make_unique(this)) + logger_configure_(std::make_unique(this)) { // Shared data shared_data_ = std::make_shared(); diff --git a/localization/autoware_pose_initializer/package.xml b/localization/autoware_pose_initializer/package.xml index cff9fc1fec3e5..e9d56db799b54 100644 --- a/localization/autoware_pose_initializer/package.xml +++ b/localization/autoware_pose_initializer/package.xml @@ -23,7 +23,7 @@ autoware_component_interface_utils autoware_map_height_fitter autoware_motion_utils - autoware_universe_utils + autoware_utils geometry_msgs rclcpp rclcpp_components diff --git a/localization/autoware_pose_initializer/src/pose_initializer_core.cpp b/localization/autoware_pose_initializer/src/pose_initializer_core.cpp index 5bde25a564f1d..603f11ebd6c9d 100644 --- a/localization/autoware_pose_initializer/src/pose_initializer_core.cpp +++ b/localization/autoware_pose_initializer/src/pose_initializer_core.cpp @@ -40,8 +40,8 @@ PoseInitializer::PoseInitializer(const rclcpp::NodeOptions & options) output_pose_covariance_ = get_covariance_parameter(this, "output_pose_covariance"); gnss_particle_covariance_ = get_covariance_parameter(this, "gnss_particle_covariance"); - diagnostics_pose_reliable_ = std::make_unique( - this, "pose_initializer_status"); + diagnostics_pose_reliable_ = + std::make_unique(this, "pose_initializer_status"); if (declare_parameter("ekf_enabled")) { ekf_localization_trigger_ = std::make_unique(this); @@ -64,7 +64,7 @@ PoseInitializer::PoseInitializer(const rclcpp::NodeOptions & options) if (declare_parameter("pose_error_check_enabled")) { pose_error_check_ = std::make_unique(this); } - logger_configure_ = std::make_unique(this); + logger_configure_ = std::make_unique(this); change_state(State::Message::UNINITIALIZED); diff --git a/localization/autoware_pose_initializer/src/pose_initializer_core.hpp b/localization/autoware_pose_initializer/src/pose_initializer_core.hpp index 2e152663dd170..231798022606a 100644 --- a/localization/autoware_pose_initializer/src/pose_initializer_core.hpp +++ b/localization/autoware_pose_initializer/src/pose_initializer_core.hpp @@ -17,8 +17,8 @@ #include #include -#include -#include +#include +#include #include #include @@ -59,8 +59,8 @@ class PoseInitializer : public rclcpp::Node std::unique_ptr pose_error_check_; std::unique_ptr ekf_localization_trigger_; std::unique_ptr ndt_localization_trigger_; - std::unique_ptr logger_configure_; - std::unique_ptr diagnostics_pose_reliable_; + std::unique_ptr logger_configure_; + std::unique_ptr diagnostics_pose_reliable_; double stop_check_duration_; void change_node_trigger(bool flag, bool need_spin = false); diff --git a/localization/autoware_pose_instability_detector/package.xml b/localization/autoware_pose_instability_detector/package.xml index 249ea6d625673..68af1164d0123 100644 --- a/localization/autoware_pose_instability_detector/package.xml +++ b/localization/autoware_pose_instability_detector/package.xml @@ -20,7 +20,7 @@ ament_cmake_cppcheck ament_index_cpp ament_lint_auto - autoware_universe_utils + autoware_utils diagnostic_msgs geometry_msgs nav_msgs diff --git a/localization/autoware_pose_instability_detector/src/pose_instability_detector.cpp b/localization/autoware_pose_instability_detector/src/pose_instability_detector.cpp index d191ec2a3bb22..0944c3a7a9b7e 100644 --- a/localization/autoware_pose_instability_detector/src/pose_instability_detector.cpp +++ b/localization/autoware_pose_instability_detector/src/pose_instability_detector.cpp @@ -14,7 +14,7 @@ #include "autoware/pose_instability_detector/pose_instability_detector.hpp" -#include "autoware/universe_utils/geometry/geometry.hpp" +#include "autoware_utils/geometry/geometry.hpp" #include @@ -127,7 +127,7 @@ void PoseInstabilityDetector::callback_timer() // compare dead reckoning pose and latest_odometry_ const Pose latest_ekf_pose = latest_odometry_->pose.pose; - const Pose ekf_to_dr = autoware::universe_utils::inverseTransformPose(*dr_pose, latest_ekf_pose); + const Pose ekf_to_dr = autoware_utils::inverse_transform_pose(*dr_pose, latest_ekf_pose); const geometry_msgs::msg::Point pos = ekf_to_dr.position; const auto [ang_x, ang_y, ang_z] = quat_to_rpy(ekf_to_dr.orientation); const std::vector values = {pos.x, pos.y, pos.z, ang_x, ang_y, ang_z}; diff --git a/localization/yabloc/yabloc_common/package.xml b/localization/yabloc/yabloc_common/package.xml index 58a77fe138f28..ad46348f9ac6d 100644 --- a/localization/yabloc/yabloc_common/package.xml +++ b/localization/yabloc/yabloc_common/package.xml @@ -19,7 +19,7 @@ autoware_lanelet2_extension autoware_map_msgs autoware_signal_processing - autoware_universe_utils + autoware_utils cv_bridge geometry_msgs lanelet2_core diff --git a/localization/yabloc/yabloc_common/src/ll2_decomposer/ll2_decomposer_core.cpp b/localization/yabloc/yabloc_common/src/ll2_decomposer/ll2_decomposer_core.cpp index cbb6f48450b91..04a3d7dfb9f95 100644 --- a/localization/yabloc/yabloc_common/src/ll2_decomposer/ll2_decomposer_core.cpp +++ b/localization/yabloc/yabloc_common/src/ll2_decomposer/ll2_decomposer_core.cpp @@ -14,8 +14,8 @@ #include "yabloc_common/ll2_decomposer/ll2_decomposer.hpp" -#include #include +#include #include #include @@ -202,7 +202,7 @@ Ll2Decomposer::MarkerArray Ll2Decomposer::make_sign_marker_msg( marker.header.frame_id = "map"; marker.header.stamp = get_clock()->now(); marker.type = Marker::LINE_STRIP; - marker.color = autoware::universe_utils::createMarkerColor(0.6f, 0.6f, 0.6f, 0.999f); + marker.color = autoware_utils::create_marker_color(0.6f, 0.6f, 0.6f, 0.999f); marker.scale.x = 0.1; marker.ns = ns; marker.id = id++; @@ -232,7 +232,7 @@ Ll2Decomposer::MarkerArray Ll2Decomposer::make_polygon_marker_msg( marker.header.frame_id = "map"; marker.header.stamp = get_clock()->now(); marker.type = Marker::LINE_STRIP; - marker.color = autoware::universe_utils::createMarkerColor(0.4f, 0.4f, 0.8f, 0.999f); + marker.color = autoware_utils::create_marker_color(0.4f, 0.4f, 0.8f, 0.999f); marker.scale.x = 0.2; marker.ns = ns; marker.id = id++; diff --git a/localization/yabloc/yabloc_image_processing/package.xml b/localization/yabloc/yabloc_image_processing/package.xml index 2cc9f817b1ec8..372cc6d41042e 100644 --- a/localization/yabloc/yabloc_image_processing/package.xml +++ b/localization/yabloc/yabloc_image_processing/package.xml @@ -16,7 +16,7 @@ ament_cmake autoware_cmake - autoware_universe_utils + autoware_utils cv_bridge pcl_conversions rclcpp diff --git a/localization/yabloc/yabloc_image_processing/src/graph_segment/graph_segment_core.cpp b/localization/yabloc/yabloc_image_processing/src/graph_segment/graph_segment_core.cpp index cedfb570fe74c..e3007c3a9a2b3 100644 --- a/localization/yabloc/yabloc_image_processing/src/graph_segment/graph_segment_core.cpp +++ b/localization/yabloc/yabloc_image_processing/src/graph_segment/graph_segment_core.cpp @@ -15,7 +15,7 @@ #include "yabloc_image_processing/graph_segment/graph_segment.hpp" #include "yabloc_image_processing/graph_segment/histogram.hpp" -#include +#include #include #include #include @@ -103,7 +103,7 @@ void GraphSegment::on_image(const Image & msg) cv::resize(image, resized, cv::Size(), 0.5, 0.5); // Execute graph-based segmentation - autoware::universe_utils::StopWatch stop_watch; + autoware_utils::StopWatch stop_watch; cv::Mat segmented; segmentation_->processImage(resized, segmented); RCLCPP_INFO_STREAM(get_logger(), "segmentation time: " << stop_watch.toc() * 1000 << "[ms]"); diff --git a/localization/yabloc/yabloc_image_processing/src/line_segment_detector/line_segment_detector_core.cpp b/localization/yabloc/yabloc_image_processing/src/line_segment_detector/line_segment_detector_core.cpp index 8b4dd71561276..9e33d262e2666 100644 --- a/localization/yabloc/yabloc_image_processing/src/line_segment_detector/line_segment_detector_core.cpp +++ b/localization/yabloc/yabloc_image_processing/src/line_segment_detector/line_segment_detector_core.cpp @@ -14,7 +14,7 @@ #include "yabloc_image_processing/line_segment_detector/line_segment_detector.hpp" -#include +#include #include #include #include @@ -55,7 +55,7 @@ void LineSegmentDetector::execute(const cv::Mat & image, const rclcpp::Time & st cv::Mat lines; { - autoware::universe_utils::StopWatch stop_watch; + autoware_utils::StopWatch stop_watch; line_segment_detector_->detect(gray_image, lines); if (lines.size().width != 0) { line_segment_detector_->drawSegments(gray_image, lines); diff --git a/localization/yabloc/yabloc_image_processing/src/undistort/undistort_node.cpp b/localization/yabloc/yabloc_image_processing/src/undistort/undistort_node.cpp index 98484d5875e14..334c6548a68a1 100644 --- a/localization/yabloc/yabloc_image_processing/src/undistort/undistort_node.cpp +++ b/localization/yabloc/yabloc_image_processing/src/undistort/undistort_node.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include +#include #include #include #include @@ -152,7 +152,7 @@ class UndistortNode : public rclcpp::Node sub_compressed_image_.reset(); } - autoware::universe_utils::StopWatch stop_watch; + autoware_utils::StopWatch stop_watch; remap_and_publish(common::decompress_to_cv_mat(msg), msg.header); RCLCPP_INFO_STREAM(get_logger(), "image undistort: " << stop_watch.toc() << "[ms]"); } @@ -166,7 +166,7 @@ class UndistortNode : public rclcpp::Node make_remap_lut(); } - autoware::universe_utils::StopWatch stop_watch; + autoware_utils::StopWatch stop_watch; remap_and_publish(common::decompress_to_cv_mat(msg), msg.header); RCLCPP_INFO_STREAM(get_logger(), "image undistort: " << stop_watch.toc() << "[ms]"); } diff --git a/localization/yabloc/yabloc_particle_filter/package.xml b/localization/yabloc/yabloc_particle_filter/package.xml index 244cf88da7b0a..49cfb8c2e0e88 100644 --- a/localization/yabloc/yabloc_particle_filter/package.xml +++ b/localization/yabloc/yabloc_particle_filter/package.xml @@ -17,7 +17,7 @@ autoware_cmake rosidl_default_generators - autoware_universe_utils + autoware_utils geometry_msgs rclcpp rclcpp_components diff --git a/localization/yabloc/yabloc_particle_filter/src/camera_corrector/camera_particle_corrector_core.cpp b/localization/yabloc/yabloc_particle_filter/src/camera_corrector/camera_particle_corrector_core.cpp index 4e8decf1308bf..e7d858d63f153 100644 --- a/localization/yabloc/yabloc_particle_filter/src/camera_corrector/camera_particle_corrector_core.cpp +++ b/localization/yabloc/yabloc_particle_filter/src/camera_corrector/camera_particle_corrector_core.cpp @@ -15,8 +15,8 @@ #include "yabloc_particle_filter/camera_corrector/camera_particle_corrector.hpp" #include "yabloc_particle_filter/camera_corrector/logit.hpp" -#include -#include +#include +#include #include #include #include @@ -136,7 +136,7 @@ CameraParticleCorrector::split_line_segments(const PointCloud2 & msg) void CameraParticleCorrector::on_line_segments(const PointCloud2 & line_segments_msg) { - autoware::universe_utils::StopWatch stop_watch; + autoware_utils::StopWatch stop_watch; const rclcpp::Time stamp = line_segments_msg.header.stamp; std::optional opt_array = this->get_synchronized_particle_array(stamp); if (!opt_array.has_value()) { @@ -262,7 +262,7 @@ float abs_cos(const Eigen::Vector3f & t, float deg) { const auto radian = static_cast(deg * M_PI / 180.0); Eigen::Vector2f x(t.x(), t.y()); - Eigen::Vector2f y(autoware::universe_utils::cos(radian), autoware::universe_utils::sin(radian)); + Eigen::Vector2f y(autoware_utils::cos(radian), autoware_utils::sin(radian)); x.normalize(); return std::abs(x.dot(y)); } diff --git a/localization/yabloc/yabloc_particle_filter/src/ll2_cost_map/hierarchical_cost_map.cpp b/localization/yabloc/yabloc_particle_filter/src/ll2_cost_map/hierarchical_cost_map.cpp index 7450635b70303..cd399e868576a 100644 --- a/localization/yabloc/yabloc_particle_filter/src/ll2_cost_map/hierarchical_cost_map.cpp +++ b/localization/yabloc/yabloc_particle_filter/src/ll2_cost_map/hierarchical_cost_map.cpp @@ -16,7 +16,7 @@ #include "yabloc_particle_filter/ll2_cost_map/direct_cost_map.hpp" -#include +#include #include #include @@ -174,7 +174,7 @@ HierarchicalCostMap::MarkerArray HierarchicalCostMap::show_map_range() const marker.header.frame_id = "map"; marker.id = id++; marker.type = Marker::LINE_STRIP; - marker.color = autoware::universe_utils::createMarkerColor(0, 0, 1.0f, 1.0f); + marker.color = autoware_utils::create_marker_color(0, 0, 1.0f, 1.0f); marker.scale.x = 0.1; Eigen::Vector2f xy = area.real_scale(); marker.points.push_back(point_msg(xy.x(), xy.y())); diff --git a/perception/autoware_cluster_merger/package.xml b/perception/autoware_cluster_merger/package.xml index 1ccfc55f0fb29..26d6e14fd560d 100644 --- a/perception/autoware_cluster_merger/package.xml +++ b/perception/autoware_cluster_merger/package.xml @@ -14,7 +14,7 @@ autoware_object_recognition_utils autoware_test_utils - autoware_universe_utils + autoware_utils geometry_msgs message_filters rclcpp diff --git a/perception/autoware_cluster_merger/src/cluster_merger_node.hpp b/perception/autoware_cluster_merger/src/cluster_merger_node.hpp index 907d59491a5d9..4a38ba6a0812a 100644 --- a/perception/autoware_cluster_merger/src/cluster_merger_node.hpp +++ b/perception/autoware_cluster_merger/src/cluster_merger_node.hpp @@ -15,7 +15,7 @@ #ifndef CLUSTER_MERGER_NODE_HPP_ #define CLUSTER_MERGER_NODE_HPP_ -#include "autoware/universe_utils/ros/transform_listener.hpp" +#include "autoware_utils/ros/transform_listener.hpp" #include "message_filters/subscriber.h" #include "message_filters/sync_policies/approximate_time.h" #include "message_filters/synchronizer.h" diff --git a/perception/autoware_compare_map_segmentation/CMakeLists.txt b/perception/autoware_compare_map_segmentation/CMakeLists.txt index f4b363ea42130..41ec6f766e4f3 100644 --- a/perception/autoware_compare_map_segmentation/CMakeLists.txt +++ b/perception/autoware_compare_map_segmentation/CMakeLists.txt @@ -44,7 +44,7 @@ ament_target_dependencies(${PROJECT_NAME} rclcpp rclcpp_components sensor_msgs - autoware_universe_utils + autoware_utils autoware_map_msgs nav_msgs ) diff --git a/perception/autoware_compare_map_segmentation/package.xml b/perception/autoware_compare_map_segmentation/package.xml index c633ebc808af9..427046ba41c79 100644 --- a/perception/autoware_compare_map_segmentation/package.xml +++ b/perception/autoware_compare_map_segmentation/package.xml @@ -22,7 +22,7 @@ autoware_point_types autoware_pointcloud_preprocessor autoware_test_utils - autoware_universe_utils + autoware_utils grid_map_pcl grid_map_ros nav_msgs diff --git a/perception/autoware_compare_map_segmentation/src/distance_based_compare_map_filter/node.cpp b/perception/autoware_compare_map_segmentation/src/distance_based_compare_map_filter/node.cpp index ac759504de557..91f7add844c74 100644 --- a/perception/autoware_compare_map_segmentation/src/distance_based_compare_map_filter/node.cpp +++ b/perception/autoware_compare_map_segmentation/src/distance_based_compare_map_filter/node.cpp @@ -14,8 +14,8 @@ #include "node.hpp" -#include "autoware/universe_utils/ros/debug_publisher.hpp" -#include "autoware/universe_utils/system/stop_watch.hpp" +#include "autoware_utils/ros/debug_publisher.hpp" +#include "autoware_utils/system/stop_watch.hpp" #include #include @@ -113,8 +113,8 @@ DistanceBasedCompareMapFilterComponent::DistanceBasedCompareMapFilterComponent( { // initialize debug tool { - using autoware::universe_utils::DebugPublisher; - using autoware::universe_utils::StopWatch; + using autoware_utils::DebugPublisher; + using autoware_utils::StopWatch; stop_watch_ptr_ = std::make_unique>(); debug_publisher_ = std::make_unique(this, "distance_based_compare_map_filter"); stop_watch_ptr_->tic("cyclic_time"); diff --git a/perception/autoware_compare_map_segmentation/src/voxel_based_approximate_compare_map_filter/node.cpp b/perception/autoware_compare_map_segmentation/src/voxel_based_approximate_compare_map_filter/node.cpp index 5794cfe37a45b..bfb0027b4e0a0 100644 --- a/perception/autoware_compare_map_segmentation/src/voxel_based_approximate_compare_map_filter/node.cpp +++ b/perception/autoware_compare_map_segmentation/src/voxel_based_approximate_compare_map_filter/node.cpp @@ -14,8 +14,8 @@ #include "node.hpp" -#include "autoware/universe_utils/ros/debug_publisher.hpp" -#include "autoware/universe_utils/system/stop_watch.hpp" +#include "autoware_utils/ros/debug_publisher.hpp" +#include "autoware_utils/system/stop_watch.hpp" #include #include @@ -76,8 +76,8 @@ VoxelBasedApproximateCompareMapFilterComponent::VoxelBasedApproximateCompareMapF { // initialize debug tool { - using autoware::universe_utils::DebugPublisher; - using autoware::universe_utils::StopWatch; + using autoware_utils::DebugPublisher; + using autoware_utils::StopWatch; stop_watch_ptr_ = std::make_unique>(); debug_publisher_ = std::make_unique(this, "voxel_based_approximate_compare_map_filter"); diff --git a/perception/autoware_compare_map_segmentation/src/voxel_based_compare_map_filter/node.cpp b/perception/autoware_compare_map_segmentation/src/voxel_based_compare_map_filter/node.cpp index db5e46a0eff5a..677977ecdfa49 100644 --- a/perception/autoware_compare_map_segmentation/src/voxel_based_compare_map_filter/node.cpp +++ b/perception/autoware_compare_map_segmentation/src/voxel_based_compare_map_filter/node.cpp @@ -14,8 +14,8 @@ #include "node.hpp" -#include "autoware/universe_utils/ros/debug_publisher.hpp" -#include "autoware/universe_utils/system/stop_watch.hpp" +#include "autoware_utils/ros/debug_publisher.hpp" +#include "autoware_utils/system/stop_watch.hpp" #include #include @@ -33,8 +33,8 @@ VoxelBasedCompareMapFilterComponent::VoxelBasedCompareMapFilterComponent( { // initialize debug tool { - using autoware::universe_utils::DebugPublisher; - using autoware::universe_utils::StopWatch; + using autoware_utils::DebugPublisher; + using autoware_utils::StopWatch; stop_watch_ptr_ = std::make_unique>(); debug_publisher_ = std::make_unique(this, "voxel_based_compare_map_filter"); stop_watch_ptr_->tic("cyclic_time"); diff --git a/perception/autoware_compare_map_segmentation/src/voxel_distance_based_compare_map_filter/node.cpp b/perception/autoware_compare_map_segmentation/src/voxel_distance_based_compare_map_filter/node.cpp index 775a931ac4c4c..a94ae92671caf 100644 --- a/perception/autoware_compare_map_segmentation/src/voxel_distance_based_compare_map_filter/node.cpp +++ b/perception/autoware_compare_map_segmentation/src/voxel_distance_based_compare_map_filter/node.cpp @@ -14,8 +14,8 @@ #include "node.hpp" -#include "autoware/universe_utils/ros/debug_publisher.hpp" -#include "autoware/universe_utils/system/stop_watch.hpp" +#include "autoware_utils/ros/debug_publisher.hpp" +#include "autoware_utils/system/stop_watch.hpp" #include #include @@ -107,8 +107,8 @@ VoxelDistanceBasedCompareMapFilterComponent::VoxelDistanceBasedCompareMapFilterC { // initialize debug tool { - using autoware::universe_utils::DebugPublisher; - using autoware::universe_utils::StopWatch; + using autoware_utils::DebugPublisher; + using autoware_utils::StopWatch; stop_watch_ptr_ = std::make_unique>(); debug_publisher_ = std::make_unique(this, "voxel_distance_based_compare_map_filter"); diff --git a/perception/autoware_crosswalk_traffic_light_estimator/include/autoware_crosswalk_traffic_light_estimator/node.hpp b/perception/autoware_crosswalk_traffic_light_estimator/include/autoware_crosswalk_traffic_light_estimator/node.hpp index e41ede2f574ed..67d9c69b62253 100644 --- a/perception/autoware_crosswalk_traffic_light_estimator/include/autoware_crosswalk_traffic_light_estimator/node.hpp +++ b/perception/autoware_crosswalk_traffic_light_estimator/include/autoware_crosswalk_traffic_light_estimator/node.hpp @@ -15,8 +15,8 @@ #ifndef AUTOWARE_CROSSWALK_TRAFFIC_LIGHT_ESTIMATOR__NODE_HPP_ #define AUTOWARE_CROSSWALK_TRAFFIC_LIGHT_ESTIMATOR__NODE_HPP_ -#include -#include +#include +#include #include #include @@ -39,11 +39,11 @@ namespace autoware::crosswalk_traffic_light_estimator { -using autoware::universe_utils::DebugPublisher; -using autoware::universe_utils::StopWatch; using autoware_internal_debug_msgs::msg::Float64Stamped; using autoware_map_msgs::msg::LaneletMapBin; using autoware_planning_msgs::msg::LaneletRoute; +using autoware_utils::DebugPublisher; +using autoware_utils::StopWatch; using TrafficSignal = autoware_perception_msgs::msg::TrafficLightGroup; using TrafficSignalArray = autoware_perception_msgs::msg::TrafficLightGroupArray; using TrafficSignalElement = autoware_perception_msgs::msg::TrafficLightElement; diff --git a/perception/autoware_crosswalk_traffic_light_estimator/package.xml b/perception/autoware_crosswalk_traffic_light_estimator/package.xml index 8976d8c75b101..bea27b1f0e4ee 100644 --- a/perception/autoware_crosswalk_traffic_light_estimator/package.xml +++ b/perception/autoware_crosswalk_traffic_light_estimator/package.xml @@ -18,7 +18,7 @@ autoware_map_msgs autoware_perception_msgs autoware_planning_msgs - autoware_universe_utils + autoware_utils rclcpp rclcpp_components tier4_perception_msgs diff --git a/perception/autoware_detected_object_feature_remover/package.xml b/perception/autoware_detected_object_feature_remover/package.xml index a2b7e89adc1f2..addc4f3587437 100644 --- a/perception/autoware_detected_object_feature_remover/package.xml +++ b/perception/autoware_detected_object_feature_remover/package.xml @@ -13,7 +13,7 @@ autoware_cmake autoware_perception_msgs - autoware_universe_utils + autoware_utils rclcpp rclcpp_components tier4_perception_msgs diff --git a/perception/autoware_detected_object_feature_remover/src/detected_object_feature_remover_node.cpp b/perception/autoware_detected_object_feature_remover/src/detected_object_feature_remover_node.cpp index 21e069144203c..62e056b98a348 100644 --- a/perception/autoware_detected_object_feature_remover/src/detected_object_feature_remover_node.cpp +++ b/perception/autoware_detected_object_feature_remover/src/detected_object_feature_remover_node.cpp @@ -25,8 +25,7 @@ DetectedObjectFeatureRemover::DetectedObjectFeatureRemover(const rclcpp::NodeOpt pub_ = this->create_publisher("~/output", rclcpp::QoS(1)); sub_ = this->create_subscription( "~/input", 1, std::bind(&DetectedObjectFeatureRemover::objectCallback, this, _1)); - published_time_publisher_ = - std::make_unique(this); + published_time_publisher_ = std::make_unique(this); } void DetectedObjectFeatureRemover::objectCallback( diff --git a/perception/autoware_detected_object_feature_remover/src/detected_object_feature_remover_node.hpp b/perception/autoware_detected_object_feature_remover/src/detected_object_feature_remover_node.hpp index 2877d38259f83..2242400b4802a 100644 --- a/perception/autoware_detected_object_feature_remover/src/detected_object_feature_remover_node.hpp +++ b/perception/autoware_detected_object_feature_remover/src/detected_object_feature_remover_node.hpp @@ -15,7 +15,7 @@ #ifndef DETECTED_OBJECT_FEATURE_REMOVER_NODE_HPP_ #define DETECTED_OBJECT_FEATURE_REMOVER_NODE_HPP_ -#include "autoware/universe_utils/ros/published_time_publisher.hpp" +#include "autoware_utils/ros/published_time_publisher.hpp" #include @@ -37,7 +37,7 @@ class DetectedObjectFeatureRemover : public rclcpp::Node private: rclcpp::Subscription::SharedPtr sub_; rclcpp::Publisher::SharedPtr pub_; - std::unique_ptr published_time_publisher_; + std::unique_ptr published_time_publisher_; void objectCallback(const DetectedObjectsWithFeature::ConstSharedPtr input); void convert(const DetectedObjectsWithFeature & objs_with_feature, DetectedObjects & objs); }; diff --git a/perception/autoware_detected_object_validation/package.xml b/perception/autoware_detected_object_validation/package.xml index ffee012c165ae..f1d9c040c3fe9 100644 --- a/perception/autoware_detected_object_validation/package.xml +++ b/perception/autoware_detected_object_validation/package.xml @@ -21,7 +21,7 @@ autoware_object_recognition_utils autoware_perception_msgs autoware_test_utils - autoware_universe_utils + autoware_utils message_filters nav_msgs pcl_conversions diff --git a/perception/autoware_detected_object_validation/src/lanelet_filter/lanelet_filter.cpp b/perception/autoware_detected_object_validation/src/lanelet_filter/lanelet_filter.cpp index 85add97e20078..e4e7272bc2073 100644 --- a/perception/autoware_detected_object_validation/src/lanelet_filter/lanelet_filter.cpp +++ b/perception/autoware_detected_object_validation/src/lanelet_filter/lanelet_filter.cpp @@ -15,10 +15,10 @@ #include "lanelet_filter.hpp" #include "autoware/object_recognition_utils/object_recognition_utils.hpp" -#include "autoware/universe_utils/geometry/geometry.hpp" -#include "autoware/universe_utils/system/time_keeper.hpp" #include "autoware_lanelet2_extension/utility/message_conversion.hpp" #include "autoware_lanelet2_extension/utility/query.hpp" +#include "autoware_utils/geometry/geometry.hpp" +#include "autoware_utils/system/time_keeper.hpp" #include #include @@ -83,11 +83,9 @@ ObjectLaneletFilterNode::ObjectLaneletFilterNode(const rclcpp::NodeOptions & nod "output/object", rclcpp::QoS{1}); debug_publisher_ = - std::make_unique(this, "object_lanelet_filter"); - published_time_publisher_ = - std::make_unique(this); - stop_watch_ptr_ = - std::make_unique>(); + std::make_unique(this, "object_lanelet_filter"); + published_time_publisher_ = std::make_unique(this); + stop_watch_ptr_ = std::make_unique>(); if (filter_settings_.debug) { viz_pub_ = this->create_publisher( "~/debug/marker", rclcpp::QoS{1}); @@ -105,7 +103,7 @@ bool isInPolygon( LinearRing2d expandPolygon(const LinearRing2d & polygon, double distance) { - universe_utils::MultiPolygon2d multi_polygon; + autoware_utils::MultiPolygon2d multi_polygon; bg::strategy::buffer::distance_symmetric distance_strategy(distance); bg::strategy::buffer::join_miter join_strategy; bg::strategy::buffer::end_flat end_strategy; @@ -388,8 +386,7 @@ bool ObjectLaneletFilterNode::isObjectOverlapLanelets( const auto footprint = setFootprint(object); for (const auto & point : footprint.points) { const geometry_msgs::msg::Point32 point_transformed = - autoware::universe_utils::transformPoint( - point, object.kinematics.pose_with_covariance.pose); + autoware_utils::transform_point(point, object.kinematics.pose_with_covariance.pose); polygon.outer().emplace_back(point_transformed.x, point_transformed.y); } polygon.outer().push_back(polygon.outer().front()); @@ -407,8 +404,7 @@ bool ObjectLaneletFilterNode::isObjectOverlapLanelets( // if object do not have bounding box, check each footprint is inside polygon for (const auto & point : object.shape.footprint.points) { const geometry_msgs::msg::Point32 point_transformed = - autoware::universe_utils::transformPoint( - point, object.kinematics.pose_with_covariance.pose); + autoware_utils::transform_point(point, object.kinematics.pose_with_covariance.pose); geometry_msgs::msg::Pose point2d; point2d.position.x = point_transformed.x; point2d.position.y = point_transformed.y; @@ -483,7 +479,7 @@ bool ObjectLaneletFilterNode::isSameDirectionWithLanelets( const double lane_yaw = lanelet::utils::getLaneletAngle( box_and_lanelet.second.lanelet, object.kinematics.pose_with_covariance.pose.position); const double delta_yaw = object_velocity_yaw - lane_yaw; - const double normalized_delta_yaw = autoware::universe_utils::normalizeRadian(delta_yaw); + const double normalized_delta_yaw = autoware_utils::normalize_radian(delta_yaw); const double abs_norm_delta_yaw = std::fabs(normalized_delta_yaw); if (abs_norm_delta_yaw < filter_settings_.lanelet_direction_filter_velocity_yaw_threshold) { diff --git a/perception/autoware_detected_object_validation/src/lanelet_filter/lanelet_filter.hpp b/perception/autoware_detected_object_validation/src/lanelet_filter/lanelet_filter.hpp index bbb0f7c0d43ce..fc8185a10107b 100644 --- a/perception/autoware_detected_object_validation/src/lanelet_filter/lanelet_filter.hpp +++ b/perception/autoware_detected_object_validation/src/lanelet_filter/lanelet_filter.hpp @@ -16,11 +16,11 @@ #define LANELET_FILTER__LANELET_FILTER_HPP_ #include "autoware/detected_object_validation/utils/utils.hpp" -#include "autoware/universe_utils/geometry/geometry.hpp" -#include "autoware/universe_utils/ros/debug_publisher.hpp" -#include "autoware/universe_utils/ros/published_time_publisher.hpp" -#include "autoware/universe_utils/system/stop_watch.hpp" #include "autoware_lanelet2_extension/utility/utilities.hpp" +#include "autoware_utils/geometry/geometry.hpp" +#include "autoware_utils/ros/debug_publisher.hpp" +#include "autoware_utils/ros/published_time_publisher.hpp" +#include "autoware_utils/system/stop_watch.hpp" #include @@ -45,9 +45,9 @@ namespace autoware::detected_object_validation { namespace lanelet_filter { -using autoware::universe_utils::LinearRing2d; -using autoware::universe_utils::MultiPoint2d; -using autoware::universe_utils::Polygon2d; +using autoware_utils::LinearRing2d; +using autoware_utils::MultiPoint2d; +using autoware_utils::Polygon2d; namespace bg = boost::geometry; namespace bgi = boost::geometry::index; @@ -79,8 +79,8 @@ class ObjectLaneletFilterNode : public rclcpp::Node rclcpp::Subscription::SharedPtr map_sub_; rclcpp::Subscription::SharedPtr object_sub_; - std::unique_ptr debug_publisher_{nullptr}; - std::unique_ptr> stop_watch_ptr_; + std::unique_ptr debug_publisher_{nullptr}; + std::unique_ptr> stop_watch_ptr_; lanelet::LaneletMapPtr lanelet_map_ptr_; std::string lanelet_frame_id_; @@ -123,7 +123,7 @@ class ObjectLaneletFilterNode : public rclcpp::Node geometry_msgs::msg::Polygon setFootprint(const autoware_perception_msgs::msg::DetectedObject &); lanelet::BasicPolygon2d getPolygon(const lanelet::ConstLanelet & lanelet); - std::unique_ptr published_time_publisher_; + std::unique_ptr published_time_publisher_; }; } // namespace lanelet_filter diff --git a/perception/autoware_detected_object_validation/src/obstacle_pointcloud/obstacle_pointcloud_validator.cpp b/perception/autoware_detected_object_validation/src/obstacle_pointcloud/obstacle_pointcloud_validator.cpp index 0c69b45291281..5cf362cc0d87c 100644 --- a/perception/autoware_detected_object_validation/src/obstacle_pointcloud/obstacle_pointcloud_validator.cpp +++ b/perception/autoware_detected_object_validation/src/obstacle_pointcloud/obstacle_pointcloud_validator.cpp @@ -17,7 +17,7 @@ #include "obstacle_pointcloud_validator.hpp" #include -#include +#include #include @@ -41,7 +41,7 @@ namespace obstacle_pointcloud { namespace bg = boost::geometry; using Shape = autoware_perception_msgs::msg::Shape; -using Polygon2d = autoware::universe_utils::Polygon2d; +using Polygon2d = autoware_utils::Polygon2d; Validator::Validator(const PointsNumThresholdParam & points_num_threshold_param) { @@ -103,8 +103,8 @@ std::optional Validator2D::getPointCloudWithinObject( { std::vector vertices_array; pcl::Vertices vertices; - Polygon2d poly2d = autoware::universe_utils::toPolygon2d( - object.kinematics.pose_with_covariance.pose, object.shape); + Polygon2d poly2d = + autoware_utils::to_polygon2d(object.kinematics.pose_with_covariance.pose, object.shape); if (bg::is_empty(poly2d)) return std::nullopt; pcl::PointCloud::Ptr poly3d(new pcl::PointCloud); @@ -218,8 +218,8 @@ std::optional Validator3D::getPointCloudWithinObject( auto const object_height = object.shape.dimensions.x; auto z_min = object_position.z - object_height / 2.0f; auto z_max = object_position.z + object_height / 2.0f; - Polygon2d poly2d = autoware::universe_utils::toPolygon2d( - object.kinematics.pose_with_covariance.pose, object.shape); + Polygon2d poly2d = + autoware_utils::to_polygon2d(object.kinematics.pose_with_covariance.pose, object.shape); if (bg::is_empty(poly2d)) return std::nullopt; pcl::PointCloud::Ptr poly3d(new pcl::PointCloud); @@ -313,13 +313,12 @@ ObstaclePointCloudBasedValidator::ObstaclePointCloudBasedValidator( objects_pub_ = create_publisher( "~/output/objects", rclcpp::QoS{1}); - debug_publisher_ = std::make_unique( - this, "obstacle_pointcloud_based_validator"); + debug_publisher_ = + std::make_unique(this, "obstacle_pointcloud_based_validator"); const bool enable_debugger = declare_parameter("enable_debugger"); if (enable_debugger) debugger_ = std::make_shared(this); - published_time_publisher_ = - std::make_unique(this); + published_time_publisher_ = std::make_unique(this); } void ObstaclePointCloudBasedValidator::onObjectsAndObstaclePointCloud( const autoware_perception_msgs::msg::DetectedObjects::ConstSharedPtr & input_objects, diff --git a/perception/autoware_detected_object_validation/src/obstacle_pointcloud/obstacle_pointcloud_validator.hpp b/perception/autoware_detected_object_validation/src/obstacle_pointcloud/obstacle_pointcloud_validator.hpp index f10dbb7531d36..6ddffd211e6f9 100644 --- a/perception/autoware_detected_object_validation/src/obstacle_pointcloud/obstacle_pointcloud_validator.hpp +++ b/perception/autoware_detected_object_validation/src/obstacle_pointcloud/obstacle_pointcloud_validator.hpp @@ -17,8 +17,8 @@ // NOLINTNEXTLINE(whitespace/line_length) #define OBSTACLE_POINTCLOUD__OBSTACLE_POINTCLOUD_VALIDATOR_HPP_ -#include "autoware/universe_utils/ros/debug_publisher.hpp" -#include "autoware/universe_utils/ros/published_time_publisher.hpp" +#include "autoware_utils/ros/debug_publisher.hpp" +#include "autoware_utils/ros/published_time_publisher.hpp" #include "debugger.hpp" #include @@ -142,7 +142,7 @@ class ObstaclePointCloudBasedValidator : public rclcpp::Node rclcpp::Publisher::SharedPtr objects_pub_; message_filters::Subscriber objects_sub_; message_filters::Subscriber obstacle_pointcloud_sub_; - std::unique_ptr debug_publisher_{nullptr}; + std::unique_ptr debug_publisher_{nullptr}; tf2_ros::Buffer tf_buffer_; tf2_ros::TransformListener tf_listener_; @@ -157,7 +157,7 @@ class ObstaclePointCloudBasedValidator : public rclcpp::Node std::shared_ptr debugger_; bool using_2d_validator_; std::unique_ptr validator_; - std::unique_ptr published_time_publisher_; + std::unique_ptr published_time_publisher_; private: void onObjectsAndObstaclePointCloud( diff --git a/perception/autoware_detected_object_validation/src/occupancy_grid_map/occupancy_grid_map_validator.cpp b/perception/autoware_detected_object_validation/src/occupancy_grid_map/occupancy_grid_map_validator.cpp index a78e2bf90d8c2..a362d8770a7b7 100644 --- a/perception/autoware_detected_object_validation/src/occupancy_grid_map/occupancy_grid_map_validator.cpp +++ b/perception/autoware_detected_object_validation/src/occupancy_grid_map/occupancy_grid_map_validator.cpp @@ -18,7 +18,7 @@ #include "autoware/object_recognition_utils/object_classification.hpp" #include "autoware/object_recognition_utils/object_recognition_utils.hpp" -#include "autoware/universe_utils/geometry/boost_polygon_utils.hpp" +#include "autoware_utils/geometry/boost_polygon_utils.hpp" #include @@ -36,7 +36,7 @@ namespace autoware::detected_object_validation namespace occupancy_grid_map { using Shape = autoware_perception_msgs::msg::Shape; -using Polygon2d = autoware::universe_utils::Polygon2d; +using Polygon2d = autoware_utils::Polygon2d; OccupancyGridBasedValidator::OccupancyGridBasedValidator(const rclcpp::NodeOptions & node_options) : rclcpp::Node("occupancy_grid_based_validator", node_options), @@ -55,8 +55,7 @@ OccupancyGridBasedValidator::OccupancyGridBasedValidator(const rclcpp::NodeOptio mean_threshold_ = declare_parameter("mean_threshold"); enable_debug_ = declare_parameter("enable_debug"); - published_time_publisher_ = - std::make_unique(this); + published_time_publisher_ = std::make_unique(this); } void OccupancyGridBasedValidator::onObjectsAndOccGrid( @@ -110,8 +109,8 @@ std::optional OccupancyGridBasedValidator::getMask( const auto & resolution = occupancy_grid.info.resolution; const auto & origin = occupancy_grid.info.origin; std::vector pixel_vertices; - Polygon2d poly2d = autoware::universe_utils::toPolygon2d( - object.kinematics.pose_with_covariance.pose, object.shape); + Polygon2d poly2d = + autoware_utils::to_polygon2d(object.kinematics.pose_with_covariance.pose, object.shape); bool is_polygon_within_image = true; for (const auto & p : poly2d.outer()) { diff --git a/perception/autoware_detected_object_validation/src/occupancy_grid_map/occupancy_grid_map_validator.hpp b/perception/autoware_detected_object_validation/src/occupancy_grid_map/occupancy_grid_map_validator.hpp index 52b6f79e5e20a..7d9566f244308 100644 --- a/perception/autoware_detected_object_validation/src/occupancy_grid_map/occupancy_grid_map_validator.hpp +++ b/perception/autoware_detected_object_validation/src/occupancy_grid_map/occupancy_grid_map_validator.hpp @@ -15,7 +15,7 @@ #ifndef OCCUPANCY_GRID_MAP__OCCUPANCY_GRID_MAP_VALIDATOR_HPP_ #define OCCUPANCY_GRID_MAP__OCCUPANCY_GRID_MAP_VALIDATOR_HPP_ -#include "autoware/universe_utils/ros/published_time_publisher.hpp" +#include "autoware_utils/ros/published_time_publisher.hpp" #include #include @@ -46,7 +46,7 @@ class OccupancyGridBasedValidator : public rclcpp::Node message_filters::Subscriber occ_grid_sub_; tf2_ros::Buffer tf_buffer_; tf2_ros::TransformListener tf_listener_; - std::unique_ptr published_time_publisher_; + std::unique_ptr published_time_publisher_; typedef message_filters::sync_policies::ApproximateTime< autoware_perception_msgs::msg::DetectedObjects, nav_msgs::msg::OccupancyGrid> diff --git a/perception/autoware_detected_object_validation/src/position_filter/position_filter.cpp b/perception/autoware_detected_object_validation/src/position_filter/position_filter.cpp index 8c1db64c61d51..663ca16250836 100644 --- a/perception/autoware_detected_object_validation/src/position_filter/position_filter.cpp +++ b/perception/autoware_detected_object_validation/src/position_filter/position_filter.cpp @@ -46,8 +46,7 @@ ObjectPositionFilterNode::ObjectPositionFilterNode(const rclcpp::NodeOptions & n "input/object", rclcpp::QoS{1}, std::bind(&ObjectPositionFilterNode::objectCallback, this, _1)); object_pub_ = this->create_publisher( "output/object", rclcpp::QoS{1}); - published_time_publisher_ = - std::make_unique(this); + published_time_publisher_ = std::make_unique(this); } void ObjectPositionFilterNode::objectCallback( diff --git a/perception/autoware_detected_object_validation/src/position_filter/position_filter.hpp b/perception/autoware_detected_object_validation/src/position_filter/position_filter.hpp index 30a5d7fc9dfb1..3f5c006ae1249 100644 --- a/perception/autoware_detected_object_validation/src/position_filter/position_filter.hpp +++ b/perception/autoware_detected_object_validation/src/position_filter/position_filter.hpp @@ -16,8 +16,8 @@ #define POSITION_FILTER__POSITION_FILTER_HPP_ #include "autoware/detected_object_validation/utils/utils.hpp" -#include "autoware/universe_utils/geometry/geometry.hpp" -#include "autoware/universe_utils/ros/published_time_publisher.hpp" +#include "autoware_utils/geometry/geometry.hpp" +#include "autoware_utils/ros/published_time_publisher.hpp" #include @@ -56,7 +56,7 @@ class ObjectPositionFilterNode : public rclcpp::Node utils::FilterTargetLabel filter_target_; bool isObjectInBounds(const autoware_perception_msgs::msg::DetectedObject & object) const; - std::unique_ptr published_time_publisher_; + std::unique_ptr published_time_publisher_; }; } // namespace position_filter diff --git a/perception/autoware_detection_by_tracker/package.xml b/perception/autoware_detection_by_tracker/package.xml index fccd8f32bbebc..c20eb0cea8ebd 100644 --- a/perception/autoware_detection_by_tracker/package.xml +++ b/perception/autoware_detection_by_tracker/package.xml @@ -16,7 +16,7 @@ autoware_euclidean_cluster autoware_object_recognition_utils autoware_shape_estimation - autoware_universe_utils + autoware_utils eigen rclcpp rclcpp_components diff --git a/perception/autoware_detection_by_tracker/src/debugger/debugger.hpp b/perception/autoware_detection_by_tracker/src/debugger/debugger.hpp index 8bd1deba35ccb..5d63c0bea6cc9 100644 --- a/perception/autoware_detection_by_tracker/src/debugger/debugger.hpp +++ b/perception/autoware_detection_by_tracker/src/debugger/debugger.hpp @@ -15,8 +15,8 @@ #ifndef DEBUGGER__DEBUGGER_HPP_ #define DEBUGGER__DEBUGGER_HPP_ -#include "autoware/universe_utils/ros/debug_publisher.hpp" -#include "autoware/universe_utils/system/stop_watch.hpp" +#include "autoware_utils/ros/debug_publisher.hpp" +#include "autoware_utils/system/stop_watch.hpp" #include @@ -58,9 +58,8 @@ class Debugger divided_objects_pub_ = node->create_publisher( "debug/divided_objects", 1); processing_time_publisher_ = - std::make_unique(node, "detection_by_tracker"); - stop_watch_ptr_ = - std::make_unique>(); + std::make_unique(node, "detection_by_tracker"); + stop_watch_ptr_ = std::make_unique>(); this->startStopWatch(); } @@ -101,8 +100,8 @@ class Debugger rclcpp::Publisher::SharedPtr merged_objects_pub_; rclcpp::Publisher::SharedPtr divided_objects_pub_; // debug publisher - std::unique_ptr> stop_watch_ptr_; - std::unique_ptr processing_time_publisher_; + std::unique_ptr> stop_watch_ptr_; + std::unique_ptr processing_time_publisher_; static autoware_perception_msgs::msg::DetectedObjects removeFeature( const tier4_perception_msgs::msg::DetectedObjectsWithFeature & input) diff --git a/perception/autoware_detection_by_tracker/src/detection_by_tracker_node.cpp b/perception/autoware_detection_by_tracker/src/detection_by_tracker_node.cpp index 8db8f6cb631d8..1075b0cf3f417 100644 --- a/perception/autoware_detection_by_tracker/src/detection_by_tracker_node.cpp +++ b/perception/autoware_detection_by_tracker/src/detection_by_tracker_node.cpp @@ -17,8 +17,8 @@ #include "detection_by_tracker_node.hpp" #include "autoware/object_recognition_utils/object_recognition_utils.hpp" -#include "autoware/universe_utils/geometry/geometry.hpp" -#include "autoware/universe_utils/math/unit_conversion.hpp" +#include "autoware_utils/geometry/geometry.hpp" +#include "autoware_utils/math/unit_conversion.hpp" #include #include @@ -62,7 +62,7 @@ boost::optional getReferenceYawInf const bool is_vehicle = Label::CAR == label || Label::TRUCK == label || Label::BUS == label || Label::TRAILER == label; if (is_vehicle) { - return autoware::shape_estimation::ReferenceYawInfo{yaw, autoware::universe_utils::deg2rad(30)}; + return autoware::shape_estimation::ReferenceYawInfo{yaw, autoware_utils::deg2rad(30)}; } else { return boost::none; } @@ -118,8 +118,7 @@ DetectionByTracker::DetectionByTracker(const rclcpp::NodeOptions & node_options) cluster_ = std::make_shared( false, 10, 10000, 0.7, 0.3, 0); debugger_ = std::make_shared(this); - published_time_publisher_ = - std::make_unique(this); + published_time_publisher_ = std::make_unique(this); } void DetectionByTracker::setMaxSearchRange() @@ -225,7 +224,7 @@ void DetectionByTracker::divideUnderSegmentedObjects( for (const auto & initial_object : in_cluster_objects.feature_objects) { // search near object - const float distance = autoware::universe_utils::calcDistance2d( + const float distance = autoware_utils::calc_distance2d( tracked_object.kinematics.pose_with_covariance.pose, initial_object.object.kinematics.pose_with_covariance.pose); if (max_search_range < distance) { @@ -362,7 +361,7 @@ void DetectionByTracker::mergeOverSegmentedObjects( pcl::PointCloud pcl_merged_cluster; for (const auto & initial_object : in_cluster_objects.feature_objects) { - const float distance = autoware::universe_utils::calcDistance2d( + const float distance = autoware_utils::calc_distance2d( tracked_object.kinematics.pose_with_covariance.pose, initial_object.object.kinematics.pose_with_covariance.pose); diff --git a/perception/autoware_detection_by_tracker/src/detection_by_tracker_node.hpp b/perception/autoware_detection_by_tracker/src/detection_by_tracker_node.hpp index f0ed51dd1938e..e19f259d18237 100644 --- a/perception/autoware_detection_by_tracker/src/detection_by_tracker_node.hpp +++ b/perception/autoware_detection_by_tracker/src/detection_by_tracker_node.hpp @@ -19,7 +19,7 @@ #include "autoware/euclidean_cluster/utils.hpp" #include "autoware/euclidean_cluster/voxel_grid_based_euclidean_cluster.hpp" #include "autoware/shape_estimation/shape_estimator.hpp" -#include "autoware/universe_utils/ros/published_time_publisher.hpp" +#include "autoware_utils/ros/published_time_publisher.hpp" #include "debugger/debugger.hpp" #include "tracker/tracker_handler.hpp" #include "utils/utils.hpp" @@ -76,7 +76,7 @@ class DetectionByTracker : public rclcpp::Node detection_by_tracker::utils::TrackerIgnoreLabel tracker_ignore_; - std::unique_ptr published_time_publisher_; + std::unique_ptr published_time_publisher_; void setMaxSearchRange(); diff --git a/perception/autoware_detection_by_tracker/src/tracker/tracker_handler.cpp b/perception/autoware_detection_by_tracker/src/tracker/tracker_handler.cpp index 9c7d801332bb8..42f6873ebdfa1 100644 --- a/perception/autoware_detection_by_tracker/src/tracker/tracker_handler.cpp +++ b/perception/autoware_detection_by_tracker/src/tracker/tracker_handler.cpp @@ -14,8 +14,8 @@ #include "tracker_handler.hpp" -#include "autoware/universe_utils/geometry/geometry.hpp" -#include "autoware/universe_utils/math/normalization.hpp" +#include "autoware_utils/geometry/geometry.hpp" +#include "autoware_utils/math/normalization.hpp" #include @@ -78,9 +78,9 @@ bool TrackerHandler::estimateTrackedObjects( estimated_object.kinematics.pose_with_covariance.pose.position.y = y + vx * std::sin(yaw) * dt.seconds(); estimated_object.kinematics.pose_with_covariance.pose.position.z = z; - const float yaw_hat = autoware::universe_utils::normalizeRadian(yaw + wz * dt.seconds()); + const float yaw_hat = autoware_utils::normalize_radian(yaw + wz * dt.seconds()); estimated_object.kinematics.pose_with_covariance.pose.orientation = - autoware::universe_utils::createQuaternionFromYaw(yaw_hat); + autoware_utils::create_quaternion_from_yaw(yaw_hat); output.objects.push_back(estimated_object); } return true; diff --git a/perception/autoware_elevation_map_loader/package.xml b/perception/autoware_elevation_map_loader/package.xml index a24bf2e0b8062..65040a95cde94 100644 --- a/perception/autoware_elevation_map_loader/package.xml +++ b/perception/autoware_elevation_map_loader/package.xml @@ -15,7 +15,7 @@ autoware_grid_map_utils autoware_lanelet2_extension autoware_map_msgs - autoware_universe_utils + autoware_utils grid_map_cv grid_map_pcl grid_map_ros diff --git a/perception/autoware_elevation_map_loader/src/elevation_map_loader_node.cpp b/perception/autoware_elevation_map_loader/src/elevation_map_loader_node.cpp index 20a19cee4de59..e9bc9c71278d7 100644 --- a/perception/autoware_elevation_map_loader/src/elevation_map_loader_node.cpp +++ b/perception/autoware_elevation_map_loader/src/elevation_map_loader_node.cpp @@ -360,7 +360,7 @@ void ElevationMapLoaderNode::inpaintElevationMap(const float radius) // Convert elevation layer to OpenCV image to fill in holes. // Get the inpaint mask (nonzero pixels indicate where values need to be filled in). namespace bg = boost::geometry; - using autoware::universe_utils::Point2d; + using autoware_utils::Point2d; elevation_map_.add("inpaint_mask", 0.0); diff --git a/perception/autoware_elevation_map_loader/src/elevation_map_loader_node.hpp b/perception/autoware_elevation_map_loader/src/elevation_map_loader_node.hpp index ac90b616fd8a5..4631e2a8a64fd 100644 --- a/perception/autoware_elevation_map_loader/src/elevation_map_loader_node.hpp +++ b/perception/autoware_elevation_map_loader/src/elevation_map_loader_node.hpp @@ -15,7 +15,7 @@ #ifndef ELEVATION_MAP_LOADER_NODE_HPP_ #define ELEVATION_MAP_LOADER_NODE_HPP_ -#include "autoware/universe_utils/geometry/boost_geometry.hpp" +#include "autoware_utils/geometry/boost_geometry.hpp" #include #include diff --git a/perception/autoware_euclidean_cluster/package.xml b/perception/autoware_euclidean_cluster/package.xml index 8bac386a5bbb6..1df65fd3efdcd 100644 --- a/perception/autoware_euclidean_cluster/package.xml +++ b/perception/autoware_euclidean_cluster/package.xml @@ -13,7 +13,7 @@ autoware_perception_msgs autoware_point_types - autoware_universe_utils + autoware_utils geometry_msgs libpcl-all-dev pcl_conversions diff --git a/perception/autoware_euclidean_cluster/src/euclidean_cluster_node.cpp b/perception/autoware_euclidean_cluster/src/euclidean_cluster_node.cpp index 1a72a813d581b..2c556495684bc 100644 --- a/perception/autoware_euclidean_cluster/src/euclidean_cluster_node.cpp +++ b/perception/autoware_euclidean_cluster/src/euclidean_cluster_node.cpp @@ -39,10 +39,8 @@ EuclideanClusterNode::EuclideanClusterNode(const rclcpp::NodeOptions & options) cluster_pub_ = this->create_publisher( "output", rclcpp::QoS{1}); debug_pub_ = this->create_publisher("debug/clusters", 1); - stop_watch_ptr_ = - std::make_unique>(); - debug_publisher_ = - std::make_unique(this, "euclidean_cluster"); + stop_watch_ptr_ = std::make_unique>(); + debug_publisher_ = std::make_unique(this, "euclidean_cluster"); stop_watch_ptr_->tic("cyclic_time"); stop_watch_ptr_->tic("processing_time"); } diff --git a/perception/autoware_euclidean_cluster/src/euclidean_cluster_node.hpp b/perception/autoware_euclidean_cluster/src/euclidean_cluster_node.hpp index 5ab48abb4f042..c74350a7984ab 100644 --- a/perception/autoware_euclidean_cluster/src/euclidean_cluster_node.hpp +++ b/perception/autoware_euclidean_cluster/src/euclidean_cluster_node.hpp @@ -16,8 +16,8 @@ #include "autoware/euclidean_cluster/euclidean_cluster.hpp" -#include -#include +#include +#include #include #include @@ -42,8 +42,8 @@ class EuclideanClusterNode : public rclcpp::Node rclcpp::Publisher::SharedPtr debug_pub_; std::shared_ptr cluster_; - std::unique_ptr> stop_watch_ptr_; - std::unique_ptr debug_publisher_; + std::unique_ptr> stop_watch_ptr_; + std::unique_ptr debug_publisher_; }; } // namespace autoware::euclidean_cluster diff --git a/perception/autoware_euclidean_cluster/src/voxel_grid_based_euclidean_cluster_node.cpp b/perception/autoware_euclidean_cluster/src/voxel_grid_based_euclidean_cluster_node.cpp index 18898fae3bfa0..1f60e18fe3c81 100644 --- a/perception/autoware_euclidean_cluster/src/voxel_grid_based_euclidean_cluster_node.cpp +++ b/perception/autoware_euclidean_cluster/src/voxel_grid_based_euclidean_cluster_node.cpp @@ -43,10 +43,9 @@ VoxelGridBasedEuclideanClusterNode::VoxelGridBasedEuclideanClusterNode( cluster_pub_ = this->create_publisher( "output", rclcpp::QoS{1}); debug_pub_ = this->create_publisher("debug/clusters", 1); - stop_watch_ptr_ = - std::make_unique>(); - debug_publisher_ = std::make_unique( - this, "voxel_grid_based_euclidean_cluster"); + stop_watch_ptr_ = std::make_unique>(); + debug_publisher_ = + std::make_unique(this, "voxel_grid_based_euclidean_cluster"); stop_watch_ptr_->tic("cyclic_time"); stop_watch_ptr_->tic("processing_time"); } diff --git a/perception/autoware_euclidean_cluster/src/voxel_grid_based_euclidean_cluster_node.hpp b/perception/autoware_euclidean_cluster/src/voxel_grid_based_euclidean_cluster_node.hpp index b48e30b37de04..8ac8e7aef84dc 100644 --- a/perception/autoware_euclidean_cluster/src/voxel_grid_based_euclidean_cluster_node.hpp +++ b/perception/autoware_euclidean_cluster/src/voxel_grid_based_euclidean_cluster_node.hpp @@ -16,8 +16,8 @@ #include "autoware/euclidean_cluster/voxel_grid_based_euclidean_cluster.hpp" -#include -#include +#include +#include #include #include @@ -41,8 +41,8 @@ class VoxelGridBasedEuclideanClusterNode : public rclcpp::Node rclcpp::Publisher::SharedPtr debug_pub_; std::shared_ptr cluster_; - std::unique_ptr> stop_watch_ptr_; - std::unique_ptr debug_publisher_; + std::unique_ptr> stop_watch_ptr_; + std::unique_ptr debug_publisher_; }; } // namespace autoware::euclidean_cluster diff --git a/perception/autoware_ground_segmentation/package.xml b/perception/autoware_ground_segmentation/package.xml index 3e361df8ef558..bfd2415c5669c 100644 --- a/perception/autoware_ground_segmentation/package.xml +++ b/perception/autoware_ground_segmentation/package.xml @@ -20,7 +20,7 @@ ament_index_cpp autoware_pointcloud_preprocessor - autoware_universe_utils + autoware_utils autoware_vehicle_info_utils libopencv-dev pcl_conversions diff --git a/perception/autoware_ground_segmentation/src/ransac_ground_filter/node.cpp b/perception/autoware_ground_segmentation/src/ransac_ground_filter/node.cpp index 5ef05485d36a5..46e89a60ce5b6 100644 --- a/perception/autoware_ground_segmentation/src/ransac_ground_filter/node.cpp +++ b/perception/autoware_ground_segmentation/src/ransac_ground_filter/node.cpp @@ -82,7 +82,7 @@ PlaneBasis getPlaneBasis(const Eigen::Vector3d & plane_normal) } using autoware::pointcloud_preprocessor::get_param; -using autoware::universe_utils::ScopedTimeTrack; +using autoware_utils::ScopedTimeTrack; RANSACGroundFilterComponent::RANSACGroundFilterComponent(const rclcpp::NodeOptions & options) : Filter("RANSACGroundFilter", options) @@ -119,15 +119,15 @@ RANSACGroundFilterComponent::RANSACGroundFilterComponent(const rclcpp::NodeOptio pcl::console::setVerbosityLevel(pcl::console::L_ALWAYS); managed_tf_buffer_ = - std::make_unique(this, has_static_tf_only_); + std::make_unique(this, has_static_tf_only_); bool use_time_keeper = declare_parameter("publish_processing_time_detail"); if (use_time_keeper) { detailed_processing_time_publisher_ = - this->create_publisher( + this->create_publisher( "~/debug/processing_time_detail_ms", 1); - auto time_keeper = autoware::universe_utils::TimeKeeper(detailed_processing_time_publisher_); - time_keeper_ = std::make_shared(time_keeper); + auto time_keeper = autoware_utils::TimeKeeper(detailed_processing_time_publisher_); + time_keeper_ = std::make_shared(time_keeper); } } @@ -175,7 +175,7 @@ bool RANSACGroundFilterComponent::transformPointCloud( return true; } - return managed_tf_buffer_->transformPointcloud(in_target_frame, *in_cloud_ptr, *out_cloud_ptr); + return managed_tf_buffer_->transform_pointcloud(in_target_frame, *in_cloud_ptr, *out_cloud_ptr); } void RANSACGroundFilterComponent::extractPointsIndices( diff --git a/perception/autoware_ground_segmentation/src/ransac_ground_filter/node.hpp b/perception/autoware_ground_segmentation/src/ransac_ground_filter/node.hpp index f9638086bf811..5424000a98138 100644 --- a/perception/autoware_ground_segmentation/src/ransac_ground_filter/node.hpp +++ b/perception/autoware_ground_segmentation/src/ransac_ground_filter/node.hpp @@ -16,9 +16,9 @@ #define RANSAC_GROUND_FILTER__NODE_HPP_ #include "autoware/pointcloud_preprocessor/filter.hpp" -#include "autoware/universe_utils/system/time_keeper.hpp" +#include "autoware_utils/system/time_keeper.hpp" -#include +#include #include #include @@ -84,12 +84,12 @@ class RANSACGroundFilterComponent : public autoware::pointcloud_preprocessor::Fi bool debug_ = false; bool is_initialized_debug_message_ = false; Eigen::Vector3d unit_vec_ = Eigen::Vector3d::UnitZ(); - std::unique_ptr managed_tf_buffer_{nullptr}; + std::unique_ptr managed_tf_buffer_{nullptr}; // time keeper related - rclcpp::Publisher::SharedPtr + rclcpp::Publisher::SharedPtr detailed_processing_time_publisher_; - std::shared_ptr time_keeper_; + std::shared_ptr time_keeper_; /*! * Output transformed PointCloud from in_cloud_ptr->header.frame_id to in_target_frame diff --git a/perception/autoware_ground_segmentation/src/ray_ground_filter/node.cpp b/perception/autoware_ground_segmentation/src/ray_ground_filter/node.cpp index 70ccffe2412a9..3cf05421781a4 100644 --- a/perception/autoware_ground_segmentation/src/ray_ground_filter/node.cpp +++ b/perception/autoware_ground_segmentation/src/ray_ground_filter/node.cpp @@ -38,7 +38,7 @@ namespace autoware::ground_segmentation { using autoware::pointcloud_preprocessor::get_param; -using autoware::universe_utils::ScopedTimeTrack; +using autoware_utils::ScopedTimeTrack; RayGroundFilterComponent::RayGroundFilterComponent(const rclcpp::NodeOptions & options) : Filter("RayGroundFilter", options) @@ -75,10 +75,10 @@ RayGroundFilterComponent::RayGroundFilterComponent(const rclcpp::NodeOptions & o bool use_time_keeper = declare_parameter("publish_processing_time_detail"); if (use_time_keeper) { detailed_processing_time_publisher_ = - this->create_publisher( + this->create_publisher( "~/debug/processing_time_detail_ms", 1); - auto time_keeper = autoware::universe_utils::TimeKeeper(detailed_processing_time_publisher_); - time_keeper_ = std::make_shared(time_keeper); + auto time_keeper = autoware_utils::TimeKeeper(detailed_processing_time_publisher_); + time_keeper_ = std::make_shared(time_keeper); } } diff --git a/perception/autoware_ground_segmentation/src/ray_ground_filter/node.hpp b/perception/autoware_ground_segmentation/src/ray_ground_filter/node.hpp index f27d4c774e3fe..9acae005fff5e 100644 --- a/perception/autoware_ground_segmentation/src/ray_ground_filter/node.hpp +++ b/perception/autoware_ground_segmentation/src/ray_ground_filter/node.hpp @@ -45,7 +45,7 @@ #ifndef RAY_GROUND_FILTER__NODE_HPP_ #define RAY_GROUND_FILTER__NODE_HPP_ -#include "autoware/universe_utils/system/time_keeper.hpp" +#include "autoware_utils/system/time_keeper.hpp" #include @@ -144,9 +144,9 @@ class RayGroundFilterComponent : public autoware::pointcloud_preprocessor::Filte // ground classification // time keeper related - rclcpp::Publisher::SharedPtr + rclcpp::Publisher::SharedPtr detailed_processing_time_publisher_; - std::shared_ptr time_keeper_; + std::shared_ptr time_keeper_; /*! * Output transformed PointCloud from in_cloud_ptr->header.frame_id to in_target_frame diff --git a/perception/autoware_ground_segmentation/src/scan_ground_filter/grid.hpp b/perception/autoware_ground_segmentation/src/scan_ground_filter/grid.hpp index 0dbbe1eabb8da..7095ac9fbb6c7 100644 --- a/perception/autoware_ground_segmentation/src/scan_ground_filter/grid.hpp +++ b/perception/autoware_ground_segmentation/src/scan_ground_filter/grid.hpp @@ -15,9 +15,9 @@ #ifndef SCAN_GROUND_FILTER__GRID_HPP_ #define SCAN_GROUND_FILTER__GRID_HPP_ -#include -#include -#include +#include +#include +#include #include #include @@ -96,7 +96,7 @@ float pseudoTan(const float theta) namespace autoware::ground_segmentation { -using autoware::universe_utils::ScopedTimeTrack; +using autoware_utils::ScopedTimeTrack; struct Point { @@ -153,7 +153,7 @@ class Grid } ~Grid() = default; - void setTimeKeeper(std::shared_ptr time_keeper_ptr) + void setTimeKeeper(std::shared_ptr time_keeper_ptr) { time_keeper_ = std::move(time_keeper_ptr); } @@ -285,7 +285,7 @@ class Grid std::vector cells_; // debug information - std::shared_ptr time_keeper_; + std::shared_ptr time_keeper_; // Generate grid geometry // the grid is cylindrical mesh grid diff --git a/perception/autoware_ground_segmentation/src/scan_ground_filter/grid_ground_filter.hpp b/perception/autoware_ground_segmentation/src/scan_ground_filter/grid_ground_filter.hpp index 0024582f22b0a..f3c5f29e4d873 100644 --- a/perception/autoware_ground_segmentation/src/scan_ground_filter/grid_ground_filter.hpp +++ b/perception/autoware_ground_segmentation/src/scan_ground_filter/grid_ground_filter.hpp @@ -18,7 +18,7 @@ #include "data.hpp" #include "grid.hpp" -#include +#include #include #include @@ -168,7 +168,7 @@ class GridGroundFilter } ~GridGroundFilter() = default; - void setTimeKeeper(std::shared_ptr time_keeper_ptr) + void setTimeKeeper(std::shared_ptr time_keeper_ptr) { time_keeper_ = std::move(time_keeper_ptr); @@ -196,7 +196,7 @@ class GridGroundFilter std::unique_ptr grid_ptr_; // debug information - std::shared_ptr time_keeper_; + std::shared_ptr time_keeper_; bool recursiveSearch(const int check_idx, const int search_cnt, std::vector & idx) const; bool recursiveSearch( diff --git a/perception/autoware_ground_segmentation/src/scan_ground_filter/node.cpp b/perception/autoware_ground_segmentation/src/scan_ground_filter/node.cpp index 8fda0c1395caf..b5ff2497ed9bc 100644 --- a/perception/autoware_ground_segmentation/src/scan_ground_filter/node.cpp +++ b/perception/autoware_ground_segmentation/src/scan_ground_filter/node.cpp @@ -16,9 +16,9 @@ #include "grid_ground_filter.hpp" -#include -#include -#include +#include +#include +#include #include #include @@ -28,12 +28,12 @@ namespace autoware::ground_segmentation { using autoware::pointcloud_preprocessor::get_param; -using autoware::universe_utils::calcDistance3d; -using autoware::universe_utils::deg2rad; -using autoware::universe_utils::normalizeDegree; -using autoware::universe_utils::normalizeRadian; -using autoware::universe_utils::ScopedTimeTrack; using autoware::vehicle_info_utils::VehicleInfoUtils; +using autoware_utils::calc_distance3d; +using autoware_utils::deg2rad; +using autoware_utils::normalize_degree; +using autoware_utils::normalize_radian; +using autoware_utils::ScopedTimeTrack; ScanGroundFilterComponent::ScanGroundFilterComponent(const rclcpp::NodeOptions & options) : autoware::pointcloud_preprocessor::Filter("ScanGroundFilter", options) @@ -110,8 +110,8 @@ ScanGroundFilterComponent::ScanGroundFilterComponent(const rclcpp::NodeOptions & // initialize debug tool { - using autoware::universe_utils::DebugPublisher; - using autoware::universe_utils::StopWatch; + using autoware_utils::DebugPublisher; + using autoware_utils::StopWatch; stop_watch_ptr_ = std::make_unique>(); debug_publisher_ptr_ = std::make_unique(this, "scan_ground_filter"); stop_watch_ptr_->tic("cyclic_time"); @@ -120,10 +120,10 @@ ScanGroundFilterComponent::ScanGroundFilterComponent(const rclcpp::NodeOptions & bool use_time_keeper = declare_parameter("publish_processing_time_detail"); if (use_time_keeper) { detailed_processing_time_publisher_ = - this->create_publisher( + this->create_publisher( "~/debug/processing_time_detail_ms", 1); - auto time_keeper = autoware::universe_utils::TimeKeeper(detailed_processing_time_publisher_); - time_keeper_ = std::make_shared(time_keeper); + auto time_keeper = autoware_utils::TimeKeeper(detailed_processing_time_publisher_); + time_keeper_ = std::make_shared(time_keeper); // set time keeper to grid grid_ground_filter_ptr_->setTimeKeeper(time_keeper_); @@ -159,7 +159,7 @@ void ScanGroundFilterComponent::convertPointcloud( // determine the azimuth angle group auto radius{static_cast(std::hypot(input_point.x, input_point.y))}; - auto theta{normalizeRadian(std::atan2(input_point.x, input_point.y), 0.0)}; + auto theta{normalize_radian(std::atan2(input_point.x, input_point.y), 0.0)}; auto radial_div{static_cast(std::floor(theta * inv_radial_divider_angle_rad))}; current_point.radius = radius; @@ -238,9 +238,9 @@ void ScanGroundFilterComponent::classifyPointCloud( prev_gnd_slope = 0.0f; ground_cluster.initialize(); non_ground_cluster.initialize(); - points_distance = calcDistance3d(point_curr, prev_gnd_point); + points_distance = calc_distance3d(point_curr, prev_gnd_point); } else { - points_distance = calcDistance3d(point_curr, point_prev); + points_distance = calc_distance3d(point_curr, point_prev); } float radius_distance_from_gnd = pd.radius - prev_gnd_radius; diff --git a/perception/autoware_ground_segmentation/src/scan_ground_filter/node.hpp b/perception/autoware_ground_segmentation/src/scan_ground_filter/node.hpp index 41c41f1899163..83ca9ccc06438 100644 --- a/perception/autoware_ground_segmentation/src/scan_ground_filter/node.hpp +++ b/perception/autoware_ground_segmentation/src/scan_ground_filter/node.hpp @@ -20,7 +20,7 @@ #include #include -#include +#include #include #include @@ -195,9 +195,9 @@ class ScanGroundFilterComponent : public autoware::pointcloud_preprocessor::Filt std::unique_ptr grid_ground_filter_ptr_; // time keeper related - rclcpp::Publisher::SharedPtr + rclcpp::Publisher::SharedPtr detailed_processing_time_publisher_; - std::shared_ptr time_keeper_; + std::shared_ptr time_keeper_; /*! * Output transformed PointCloud from in_cloud_ptr->header.frame_id to in_target_frame @@ -254,9 +254,8 @@ class ScanGroundFilterComponent : public autoware::pointcloud_preprocessor::Filt const std::vector & param); // debugger - std::unique_ptr> stop_watch_ptr_{ - nullptr}; - std::unique_ptr debug_publisher_ptr_{nullptr}; + std::unique_ptr> stop_watch_ptr_{nullptr}; + std::unique_ptr debug_publisher_ptr_{nullptr}; public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW diff --git a/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/fusion_node.hpp b/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/fusion_node.hpp index 964974b96e73d..5b889ac9a3e8b 100644 --- a/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/fusion_node.hpp +++ b/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/fusion_node.hpp @@ -17,9 +17,9 @@ #include #include -#include -#include -#include +#include +#include +#include #include #include @@ -166,16 +166,16 @@ class FusionNode : public rclcpp::Node // debugger std::shared_ptr debugger_; - std::unique_ptr debug_internal_pub_; + std::unique_ptr debug_internal_pub_; /** \brief processing time publisher. **/ - std::unique_ptr> stop_watch_ptr_; - std::unique_ptr debug_publisher_; + std::unique_ptr> stop_watch_ptr_; + std::unique_ptr debug_publisher_; // timekeeper - rclcpp::Publisher::SharedPtr + rclcpp::Publisher::SharedPtr detailed_processing_time_publisher_; - std::shared_ptr time_keeper_; + std::shared_ptr time_keeper_; }; } // namespace autoware::image_projection_based_fusion diff --git a/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/pointpainting_fusion/node.hpp b/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/pointpainting_fusion/node.hpp index 26b4512e345f7..32293d9b85c75 100644 --- a/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/pointpainting_fusion/node.hpp +++ b/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/pointpainting_fusion/node.hpp @@ -23,7 +23,7 @@ #include #include #include -#include +#include #include #include @@ -58,7 +58,7 @@ class PointPaintingFusionNode : public FusionNode::SharedPtr painted_point_pub_ptr_; - std::unique_ptr diagnostics_interface_ptr_; + std::unique_ptr diagnostics_interface_ptr_; int omp_num_threads_{1}; std::vector class_names_; diff --git a/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/roi_detected_object_fusion/node.hpp b/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/roi_detected_object_fusion/node.hpp index ba197126277a0..ffe7e176dfa27 100644 --- a/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/roi_detected_object_fusion/node.hpp +++ b/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/roi_detected_object_fusion/node.hpp @@ -16,7 +16,7 @@ #define AUTOWARE__IMAGE_PROJECTION_BASED_FUSION__ROI_DETECTED_OBJECT_FUSION__NODE_HPP_ #include "autoware/image_projection_based_fusion/fusion_node.hpp" -#include "autoware/universe_utils/ros/debug_publisher.hpp" +#include "autoware_utils/ros/debug_publisher.hpp" #include diff --git a/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/utils/utils.hpp b/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/utils/utils.hpp index 044a71ab57533..1555e0123a368 100644 --- a/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/utils/utils.hpp +++ b/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/utils/utils.hpp @@ -34,7 +34,7 @@ #include "autoware/image_projection_based_fusion/fusion_node.hpp" -#include +#include #include #include #include diff --git a/perception/autoware_image_projection_based_fusion/package.xml b/perception/autoware_image_projection_based_fusion/package.xml index 45924710bd412..75307981fd62e 100644 --- a/perception/autoware_image_projection_based_fusion/package.xml +++ b/perception/autoware_image_projection_based_fusion/package.xml @@ -21,7 +21,7 @@ autoware_object_recognition_utils autoware_perception_msgs autoware_point_types - autoware_universe_utils + autoware_utils cv_bridge image_geometry image_transport diff --git a/perception/autoware_image_projection_based_fusion/src/fusion_node.cpp b/perception/autoware_image_projection_based_fusion/src/fusion_node.cpp index 53b18e20883bc..cb79c04ae2d47 100644 --- a/perception/autoware_image_projection_based_fusion/src/fusion_node.cpp +++ b/perception/autoware_image_projection_based_fusion/src/fusion_node.cpp @@ -45,7 +45,7 @@ static double processing_time_ms = 0; namespace autoware::image_projection_based_fusion { -using autoware::universe_utils::ScopedTimeTrack; +using autoware_utils::ScopedTimeTrack; template FusionNode::FusionNode( @@ -145,25 +145,23 @@ FusionNode::FusionNode( std::make_shared(this, rois_number, image_buffer_size, input_camera_topics); // input topic timing publisher - debug_internal_pub_ = - std::make_unique(this, get_name()); + debug_internal_pub_ = std::make_unique(this, get_name()); } // time keeper bool use_time_keeper = declare_parameter("publish_processing_time_detail"); if (use_time_keeper) { detailed_processing_time_publisher_ = - this->create_publisher( + this->create_publisher( "~/debug/processing_time_detail_ms", 1); - auto time_keeper = autoware::universe_utils::TimeKeeper(detailed_processing_time_publisher_); - time_keeper_ = std::make_shared(time_keeper); + auto time_keeper = autoware_utils::TimeKeeper(detailed_processing_time_publisher_); + time_keeper_ = std::make_shared(time_keeper); } // initialize debug tool { - stop_watch_ptr_ = - std::make_unique>(); - debug_publisher_ = std::make_unique(this, get_name()); + stop_watch_ptr_ = std::make_unique>(); + debug_publisher_ = std::make_unique(this, get_name()); stop_watch_ptr_->tic("cyclic_time"); stop_watch_ptr_->tic("processing_time"); } diff --git a/perception/autoware_image_projection_based_fusion/src/pointpainting_fusion/node.cpp b/perception/autoware_image_projection_based_fusion/src/pointpainting_fusion/node.cpp index 39ada01ba38da..4becd1f587a8e 100644 --- a/perception/autoware_image_projection_based_fusion/src/pointpainting_fusion/node.cpp +++ b/perception/autoware_image_projection_based_fusion/src/pointpainting_fusion/node.cpp @@ -22,9 +22,9 @@ #include #include #include -#include -#include -#include +#include +#include +#include #include #include @@ -36,7 +36,7 @@ namespace { -using autoware::universe_utils::ScopedTimeTrack; +using autoware_utils::ScopedTimeTrack; Eigen::Affine3f _transformToEigen(const geometry_msgs::msg::Transform & t) { @@ -192,7 +192,7 @@ PointPaintingFusionNode::PointPaintingFusionNode(const rclcpp::NodeOptions & opt detector_ptr_ = std::make_unique( encoder_param, head_param, densification_param, config); diagnostics_interface_ptr_ = - std::make_unique(this, "pointpainting_trt"); + std::make_unique(this, "pointpainting_trt"); if (this->declare_parameter("build_only", false)) { RCLCPP_INFO(this->get_logger(), "TensorRT engine is built and shutdown node."); diff --git a/perception/autoware_image_projection_based_fusion/src/pointpainting_fusion/pointpainting_trt.cpp b/perception/autoware_image_projection_based_fusion/src/pointpainting_fusion/pointpainting_trt.cpp index 1fe860e68a967..7ff80339930d8 100644 --- a/perception/autoware_image_projection_based_fusion/src/pointpainting_fusion/pointpainting_trt.cpp +++ b/perception/autoware_image_projection_based_fusion/src/pointpainting_fusion/pointpainting_trt.cpp @@ -17,7 +17,7 @@ #include #include #include -#include +#include #include #include diff --git a/perception/autoware_image_projection_based_fusion/src/roi_cluster_fusion/node.cpp b/perception/autoware_image_projection_based_fusion/src/roi_cluster_fusion/node.cpp index 0a9d9e3391882..374357b0bc181 100644 --- a/perception/autoware_image_projection_based_fusion/src/roi_cluster_fusion/node.cpp +++ b/perception/autoware_image_projection_based_fusion/src/roi_cluster_fusion/node.cpp @@ -16,7 +16,7 @@ #include #include -#include +#include #include #include @@ -38,7 +38,7 @@ namespace autoware::image_projection_based_fusion { -using autoware::universe_utils::ScopedTimeTrack; +using autoware_utils::ScopedTimeTrack; RoiClusterFusionNode::RoiClusterFusionNode(const rclcpp::NodeOptions & options) : FusionNode("roi_cluster_fusion", options) diff --git a/perception/autoware_image_projection_based_fusion/src/roi_detected_object_fusion/node.cpp b/perception/autoware_image_projection_based_fusion/src/roi_detected_object_fusion/node.cpp index 8b6a4fd2b7d10..f01353d0265cd 100644 --- a/perception/autoware_image_projection_based_fusion/src/roi_detected_object_fusion/node.cpp +++ b/perception/autoware_image_projection_based_fusion/src/roi_detected_object_fusion/node.cpp @@ -18,7 +18,7 @@ #include #include -#include +#include #include #include @@ -28,7 +28,7 @@ namespace autoware::image_projection_based_fusion { -using autoware::universe_utils::ScopedTimeTrack; +using autoware_utils::ScopedTimeTrack; RoiDetectedObjectFusionNode::RoiDetectedObjectFusionNode(const rclcpp::NodeOptions & options) : FusionNode("roi_detected_object_fusion", options) diff --git a/perception/autoware_image_projection_based_fusion/src/roi_pointcloud_fusion/node.cpp b/perception/autoware_image_projection_based_fusion/src/roi_pointcloud_fusion/node.cpp index ed2a0cbc5d68f..e6596a9e28602 100644 --- a/perception/autoware_image_projection_based_fusion/src/roi_pointcloud_fusion/node.cpp +++ b/perception/autoware_image_projection_based_fusion/src/roi_pointcloud_fusion/node.cpp @@ -17,7 +17,7 @@ #include "autoware/image_projection_based_fusion/utils/geometry.hpp" #include "autoware/image_projection_based_fusion/utils/utils.hpp" -#include +#include #include #include @@ -34,7 +34,7 @@ namespace autoware::image_projection_based_fusion { -using autoware::universe_utils::ScopedTimeTrack; +using autoware_utils::ScopedTimeTrack; RoiPointCloudFusionNode::RoiPointCloudFusionNode(const rclcpp::NodeOptions & options) : FusionNode("roi_pointcloud_fusion", options) diff --git a/perception/autoware_image_projection_based_fusion/src/segmentation_pointcloud_fusion/node.cpp b/perception/autoware_image_projection_based_fusion/src/segmentation_pointcloud_fusion/node.cpp index 7d63cac7273c6..0cb3b7cb3c65f 100644 --- a/perception/autoware_image_projection_based_fusion/src/segmentation_pointcloud_fusion/node.cpp +++ b/perception/autoware_image_projection_based_fusion/src/segmentation_pointcloud_fusion/node.cpp @@ -17,7 +17,7 @@ #include "autoware/image_projection_based_fusion/utils/geometry.hpp" #include "autoware/image_projection_based_fusion/utils/utils.hpp" -#include +#include #include #include @@ -33,7 +33,7 @@ namespace autoware::image_projection_based_fusion { -using autoware::universe_utils::ScopedTimeTrack; +using autoware_utils::ScopedTimeTrack; SegmentPointCloudFusionNode::SegmentPointCloudFusionNode(const rclcpp::NodeOptions & options) : FusionNode("segmentation_pointcloud_fusion", options) diff --git a/perception/autoware_image_projection_based_fusion/src/utils/utils.cpp b/perception/autoware_image_projection_based_fusion/src/utils/utils.cpp index 81424d4c23a34..3bc4290fe991c 100644 --- a/perception/autoware_image_projection_based_fusion/src/utils/utils.cpp +++ b/perception/autoware_image_projection_based_fusion/src/utils/utils.cpp @@ -86,7 +86,7 @@ void closest_cluster( std::memcpy(&point.y, &cluster.data[i * point_step + y_offset], sizeof(float)); std::memcpy(&point.z, &cluster.data[i * point_step + z_offset], sizeof(float)); - point_data.distance = autoware::universe_utils::calcDistance2d(center, point); + point_data.distance = autoware_utils::calc_distance2d(center, point); point_data.orig_index = i; points_data.push_back(point_data); } diff --git a/perception/autoware_lidar_apollo_instance_segmentation/CMakeLists.txt b/perception/autoware_lidar_apollo_instance_segmentation/CMakeLists.txt index 0555cedc235d9..3f1a3791a8c72 100644 --- a/perception/autoware_lidar_apollo_instance_segmentation/CMakeLists.txt +++ b/perception/autoware_lidar_apollo_instance_segmentation/CMakeLists.txt @@ -8,7 +8,7 @@ find_package(pcl_conversions REQUIRED) find_package(rclcpp REQUIRED) find_package(rclcpp_components REQUIRED) find_package(tf2_eigen REQUIRED) -find_package(autoware_universe_utils REQUIRED) +find_package(autoware_utils REQUIRED) find_package(autoware_internal_debug_msgs REQUIRED) find_package(tier4_perception_msgs REQUIRED) @@ -40,7 +40,7 @@ target_include_directories(${PROJECT_NAME} PRIVATE "$" ${autoware_cuda_utils_INCLUDE_DIRS} ${pcl_conversions_INCLUDE_DIRS} - ${autoware_universe_utils_INCLUDE_DIRS} + ${autoware_utils_INCLUDE_DIRS} ${autoware_perception_msgs_INCLUDE_DIRS} ) diff --git a/perception/autoware_lidar_apollo_instance_segmentation/include/autoware/lidar_apollo_instance_segmentation/detector.hpp b/perception/autoware_lidar_apollo_instance_segmentation/include/autoware/lidar_apollo_instance_segmentation/detector.hpp index 07e823bdafada..77cdf801bdda5 100644 --- a/perception/autoware_lidar_apollo_instance_segmentation/include/autoware/lidar_apollo_instance_segmentation/detector.hpp +++ b/perception/autoware_lidar_apollo_instance_segmentation/include/autoware/lidar_apollo_instance_segmentation/detector.hpp @@ -22,7 +22,7 @@ #include #include #include -#include +#include #include #include diff --git a/perception/autoware_lidar_apollo_instance_segmentation/include/autoware/lidar_apollo_instance_segmentation/node.hpp b/perception/autoware_lidar_apollo_instance_segmentation/include/autoware/lidar_apollo_instance_segmentation/node.hpp index c83b18d426325..05472b18f34bc 100644 --- a/perception/autoware_lidar_apollo_instance_segmentation/include/autoware/lidar_apollo_instance_segmentation/node.hpp +++ b/perception/autoware_lidar_apollo_instance_segmentation/include/autoware/lidar_apollo_instance_segmentation/node.hpp @@ -17,8 +17,8 @@ #include "autoware/lidar_apollo_instance_segmentation/debugger.hpp" -#include -#include +#include +#include #include #include @@ -49,8 +49,8 @@ class LidarInstanceSegmentationNode : public rclcpp::Node std::shared_ptr detector_ptr_; std::shared_ptr debugger_ptr_; void pointCloudCallback(const sensor_msgs::msg::PointCloud2::ConstSharedPtr msg); - std::unique_ptr> stop_watch_ptr_; - std::unique_ptr debug_publisher_; + std::unique_ptr> stop_watch_ptr_; + std::unique_ptr debug_publisher_; public: explicit LidarInstanceSegmentationNode(const rclcpp::NodeOptions & node_options); diff --git a/perception/autoware_lidar_apollo_instance_segmentation/package.xml b/perception/autoware_lidar_apollo_instance_segmentation/package.xml index 7e09587aef8a1..ec094bb61fd3a 100755 --- a/perception/autoware_lidar_apollo_instance_segmentation/package.xml +++ b/perception/autoware_lidar_apollo_instance_segmentation/package.xml @@ -17,7 +17,7 @@ autoware_internal_debug_msgs autoware_perception_msgs autoware_tensorrt_common - autoware_universe_utils + autoware_utils libpcl-all-dev pcl_conversions rclcpp diff --git a/perception/autoware_lidar_apollo_instance_segmentation/src/detector.cpp b/perception/autoware_lidar_apollo_instance_segmentation/src/detector.cpp index 2e3a7a2243399..345efdc0e8bd5 100644 --- a/perception/autoware_lidar_apollo_instance_segmentation/src/detector.cpp +++ b/perception/autoware_lidar_apollo_instance_segmentation/src/detector.cpp @@ -95,8 +95,7 @@ bool LidarApolloInstanceSegmentation::transformCloud( target_frame_, input.header.frame_id, input.header.stamp, std::chrono::milliseconds(500)); Eigen::Matrix4f affine_matrix = tf2::transformToEigen(transform_stamped.transform).matrix().cast(); - autoware::universe_utils::transformPointCloud( - pcl_input, pcl_transformed_cloud, affine_matrix); + autoware_utils::transform_pointcloud(pcl_input, pcl_transformed_cloud, affine_matrix); transformed_cloud.header.frame_id = target_frame_; pcl_transformed_cloud.header.frame_id = target_frame_; } catch (tf2::TransformException & ex) { @@ -111,7 +110,7 @@ bool LidarApolloInstanceSegmentation::transformCloud( pcl::PointCloud pointcloud_with_z_offset; Eigen::Affine3f z_up_translation(Eigen::Translation3f(0, 0, z_offset)); Eigen::Matrix4f z_up_transform = z_up_translation.matrix(); - autoware::universe_utils::transformPointCloud( + autoware_utils::transform_pointcloud( pcl_transformed_cloud, pcl_transformed_cloud, z_up_transform); pcl::toROSMsg(pcl_transformed_cloud, transformed_cloud); diff --git a/perception/autoware_lidar_apollo_instance_segmentation/src/node.cpp b/perception/autoware_lidar_apollo_instance_segmentation/src/node.cpp index c535fc6762e1f..1f7d77724cce7 100644 --- a/perception/autoware_lidar_apollo_instance_segmentation/src/node.cpp +++ b/perception/autoware_lidar_apollo_instance_segmentation/src/node.cpp @@ -16,8 +16,8 @@ #include "autoware/lidar_apollo_instance_segmentation/detector.hpp" -#include -#include +#include +#include #include @@ -32,8 +32,8 @@ LidarInstanceSegmentationNode::LidarInstanceSegmentationNode( using std::placeholders::_1; // initialize debug tool { - using autoware::universe_utils::DebugPublisher; - using autoware::universe_utils::StopWatch; + using autoware_utils::DebugPublisher; + using autoware_utils::StopWatch; stop_watch_ptr_ = std::make_unique>(); debug_publisher_ = std::make_unique(this, "lidar_apollo_instance_segmentation"); stop_watch_ptr_->tic("cyclic_time"); diff --git a/perception/autoware_lidar_centerpoint/README.md b/perception/autoware_lidar_centerpoint/README.md index 06ded16d5fd40..b05f3aa0cc587 100644 --- a/perception/autoware_lidar_centerpoint/README.md +++ b/perception/autoware_lidar_centerpoint/README.md @@ -20,11 +20,11 @@ We trained the models using . ### Output -| Name | Type | Description | -| -------------------------- | ------------------------------------------------ | -------------------- | -| `~/output/objects` | `autoware_perception_msgs::msg::DetectedObjects` | detected objects | -| `debug/cyclic_time_ms` | `tier4_debug_msgs::msg::Float64Stamped` | cyclic time (msg) | -| `debug/processing_time_ms` | `tier4_debug_msgs::msg::Float64Stamped` | processing time (ms) | +| Name | Type | Description | +| -------------------------- | --------------------------------------------------- | -------------------- | +| `~/output/objects` | `autoware_perception_msgs::msg::DetectedObjects` | detected objects | +| `debug/cyclic_time_ms` | `autoware_internal_debug_msgs::msg::Float64Stamped` | cyclic time (msg) | +| `debug/processing_time_ms` | `autoware_internal_debug_msgs::msg::Float64Stamped` | processing time (ms) | ## Parameters diff --git a/perception/autoware_lidar_centerpoint/include/autoware/lidar_centerpoint/node.hpp b/perception/autoware_lidar_centerpoint/include/autoware/lidar_centerpoint/node.hpp index 1748db0e48392..b2f8fde46bf63 100644 --- a/perception/autoware_lidar_centerpoint/include/autoware/lidar_centerpoint/node.hpp +++ b/perception/autoware_lidar_centerpoint/include/autoware/lidar_centerpoint/node.hpp @@ -19,10 +19,10 @@ #include "autoware/lidar_centerpoint/detection_class_remapper.hpp" #include "autoware/lidar_centerpoint/postprocess/non_maximum_suppression.hpp" -#include -#include -#include -#include +#include +#include +#include +#include #include #include @@ -60,14 +60,13 @@ class LidarCenterPointNode : public rclcpp::Node DetectionClassRemapper detection_class_remapper_; std::unique_ptr detector_ptr_{nullptr}; - std::unique_ptr diagnostics_interface_ptr_; + std::unique_ptr diagnostics_interface_ptr_; // debugger - std::unique_ptr> stop_watch_ptr_{ - nullptr}; - std::unique_ptr debug_publisher_ptr_{nullptr}; + std::unique_ptr> stop_watch_ptr_{nullptr}; + std::unique_ptr debug_publisher_ptr_{nullptr}; - std::unique_ptr published_time_publisher_; + std::unique_ptr published_time_publisher_; }; } // namespace autoware::lidar_centerpoint diff --git a/perception/autoware_lidar_centerpoint/lib/centerpoint_trt.cpp b/perception/autoware_lidar_centerpoint/lib/centerpoint_trt.cpp index c586eeb961716..d7fd2b0f30697 100644 --- a/perception/autoware_lidar_centerpoint/lib/centerpoint_trt.cpp +++ b/perception/autoware_lidar_centerpoint/lib/centerpoint_trt.cpp @@ -18,8 +18,8 @@ #include "autoware/lidar_centerpoint/network/scatter_kernel.hpp" #include "autoware/lidar_centerpoint/preprocess/preprocess_kernel.hpp" -#include -#include +#include +#include #include #include diff --git a/perception/autoware_lidar_centerpoint/lib/postprocess/non_maximum_suppression.cpp b/perception/autoware_lidar_centerpoint/lib/postprocess/non_maximum_suppression.cpp index dbc6924f6fd25..8158de1eff82a 100644 --- a/perception/autoware_lidar_centerpoint/lib/postprocess/non_maximum_suppression.cpp +++ b/perception/autoware_lidar_centerpoint/lib/postprocess/non_maximum_suppression.cpp @@ -16,7 +16,7 @@ #include #include -#include +#include #include @@ -46,7 +46,7 @@ bool NonMaximumSuppression::isTargetPairObject( return false; } - const auto sqr_dist_2d = autoware::universe_utils::calcSquaredDistance2d( + const auto sqr_dist_2d = autoware_utils::calc_squared_distance2d( autoware::object_recognition_utils::getPose(object1), autoware::object_recognition_utils::getPose(object2)); return sqr_dist_2d <= search_distance_2d_sq_; diff --git a/perception/autoware_lidar_centerpoint/lib/ros_utils.cpp b/perception/autoware_lidar_centerpoint/lib/ros_utils.cpp index 0ea681538d8c1..4bb5e15934c75 100644 --- a/perception/autoware_lidar_centerpoint/lib/ros_utils.cpp +++ b/perception/autoware_lidar_centerpoint/lib/ros_utils.cpp @@ -15,8 +15,8 @@ #include "autoware/lidar_centerpoint/ros_utils.hpp" #include "autoware/object_recognition_utils/object_recognition_utils.hpp" -#include "autoware/universe_utils/geometry/geometry.hpp" -#include "autoware/universe_utils/math/constants.hpp" +#include "autoware_utils/geometry/geometry.hpp" +#include "autoware_utils/math/constants.hpp" #include #include @@ -53,14 +53,14 @@ void box3DToDetectedObject( // pose and shape // mmdet3d yaw format to ros yaw format - const float yaw = -box3d.yaw - autoware::universe_utils::pi / 2; + const float yaw = -box3d.yaw - autoware_utils::pi / 2; obj.kinematics.pose_with_covariance.pose.position = - autoware::universe_utils::createPoint(box3d.x, box3d.y, box3d.z); + autoware_utils::create_point(box3d.x, box3d.y, box3d.z); obj.kinematics.pose_with_covariance.pose.orientation = - autoware::universe_utils::createQuaternionFromYaw(yaw); + autoware_utils::create_quaternion_from_yaw(yaw); obj.shape.type = autoware_perception_msgs::msg::Shape::BOUNDING_BOX; obj.shape.dimensions = - autoware::universe_utils::createTranslation(box3d.length, box3d.width, box3d.height); + autoware_utils::create_translation(box3d.length, box3d.width, box3d.height); if (has_variance) { obj.kinematics.has_position_covariance = has_variance; obj.kinematics.pose_with_covariance.covariance = convertPoseCovarianceMatrix(box3d); @@ -109,7 +109,7 @@ uint8_t getSemanticType(const std::string & class_name) std::array convertPoseCovarianceMatrix(const Box3D & box3d) { - using POSE_IDX = autoware::universe_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; + using POSE_IDX = autoware_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; std::array pose_covariance{}; pose_covariance[POSE_IDX::X_X] = box3d.x_variance; pose_covariance[POSE_IDX::Y_Y] = box3d.y_variance; @@ -120,7 +120,7 @@ std::array convertPoseCovarianceMatrix(const Box3D & box3d) std::array convertTwistCovarianceMatrix(const Box3D & box3d, const float yaw) { - using POSE_IDX = autoware::universe_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; + using POSE_IDX = autoware_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; // twist covariance matrix is based on the object coordinate system std::array twist_covariance{}; diff --git a/perception/autoware_lidar_centerpoint/package.xml b/perception/autoware_lidar_centerpoint/package.xml index cfb3e278c4ca3..2a979507da71c 100644 --- a/perception/autoware_lidar_centerpoint/package.xml +++ b/perception/autoware_lidar_centerpoint/package.xml @@ -16,7 +16,7 @@ autoware_object_recognition_utils autoware_perception_msgs autoware_tensorrt_common - autoware_universe_utils + autoware_utils pcl_ros rclcpp rclcpp_components diff --git a/perception/autoware_lidar_centerpoint/src/node.cpp b/perception/autoware_lidar_centerpoint/src/node.cpp index 19d1475db9051..ae6bb320b6e70 100644 --- a/perception/autoware_lidar_centerpoint/src/node.cpp +++ b/perception/autoware_lidar_centerpoint/src/node.cpp @@ -108,7 +108,7 @@ LidarCenterPointNode::LidarCenterPointNode(const rclcpp::NodeOptions & node_opti detector_ptr_ = std::make_unique(encoder_param, head_param, densification_param, config); diagnostics_interface_ptr_ = - std::make_unique(this, "centerpoint_trt"); + std::make_unique(this, "centerpoint_trt"); pointcloud_sub_ = this->create_subscription( "~/input/pointcloud", rclcpp::SensorDataQoS{}.keep_last(1), @@ -118,8 +118,8 @@ LidarCenterPointNode::LidarCenterPointNode(const rclcpp::NodeOptions & node_opti // initialize debug tool { - using autoware::universe_utils::DebugPublisher; - using autoware::universe_utils::StopWatch; + using autoware_utils::DebugPublisher; + using autoware_utils::StopWatch; stop_watch_ptr_ = std::make_unique>(); debug_publisher_ptr_ = std::make_unique(this, "lidar_centerpoint"); stop_watch_ptr_->tic("cyclic_time"); @@ -130,8 +130,7 @@ LidarCenterPointNode::LidarCenterPointNode(const rclcpp::NodeOptions & node_opti RCLCPP_INFO(this->get_logger(), "TensorRT engine is built and shutdown node."); rclcpp::shutdown(); } - published_time_publisher_ = - std::make_unique(this); + published_time_publisher_ = std::make_unique(this); } void LidarCenterPointNode::pointCloudCallback( @@ -194,11 +193,11 @@ void LidarCenterPointNode::pointCloudCallback( std::chrono::nanoseconds( (this->get_clock()->now() - output_msg.header.stamp).nanoseconds())) .count(); - debug_publisher_ptr_->publish( + debug_publisher_ptr_->publish( "debug/cyclic_time_ms", cyclic_time_ms); - debug_publisher_ptr_->publish( + debug_publisher_ptr_->publish( "debug/processing_time_ms", processing_time_ms); - debug_publisher_ptr_->publish( + debug_publisher_ptr_->publish( "debug/pipeline_latency_ms", pipeline_latency_ms); } } diff --git a/perception/autoware_lidar_transfusion/README.md b/perception/autoware_lidar_transfusion/README.md index 095170efa66c9..620adbcb40e00 100644 --- a/perception/autoware_lidar_transfusion/README.md +++ b/perception/autoware_lidar_transfusion/README.md @@ -20,15 +20,15 @@ We trained the models using . ### Output -| Name | Type | Description | -| -------------------------------------- | ------------------------------------------------ | --------------------------- | -| `~/output/objects` | `autoware_perception_msgs::msg::DetectedObjects` | Detected objects. | -| `debug/cyclic_time_ms` | `tier4_debug_msgs::msg::Float64Stamped` | Cyclic time (ms). | -| `debug/pipeline_latency_ms` | `tier4_debug_msgs::msg::Float64Stamped` | Pipeline latency time (ms). | -| `debug/processing_time/preprocess_ms` | `tier4_debug_msgs::msg::Float64Stamped` | Preprocess (ms). | -| `debug/processing_time/inference_ms` | `tier4_debug_msgs::msg::Float64Stamped` | Inference time (ms). | -| `debug/processing_time/postprocess_ms` | `tier4_debug_msgs::msg::Float64Stamped` | Postprocess time (ms). | -| `debug/processing_time/total_ms` | `tier4_debug_msgs::msg::Float64Stamped` | Total processing time (ms). | +| Name | Type | Description | +| -------------------------------------- | --------------------------------------------------- | --------------------------- | +| `~/output/objects` | `autoware_perception_msgs::msg::DetectedObjects` | Detected objects. | +| `debug/cyclic_time_ms` | `autoware_internal_debug_msgs::msg::Float64Stamped` | Cyclic time (ms). | +| `debug/pipeline_latency_ms` | `autoware_internal_debug_msgs::msg::Float64Stamped` | Pipeline latency time (ms). | +| `debug/processing_time/preprocess_ms` | `autoware_internal_debug_msgs::msg::Float64Stamped` | Preprocess (ms). | +| `debug/processing_time/inference_ms` | `autoware_internal_debug_msgs::msg::Float64Stamped` | Inference time (ms). | +| `debug/processing_time/postprocess_ms` | `autoware_internal_debug_msgs::msg::Float64Stamped` | Postprocess time (ms). | +| `debug/processing_time/total_ms` | `autoware_internal_debug_msgs::msg::Float64Stamped` | Total processing time (ms). | ## Parameters diff --git a/perception/autoware_lidar_transfusion/include/autoware/lidar_transfusion/lidar_transfusion_node.hpp b/perception/autoware_lidar_transfusion/include/autoware/lidar_transfusion/lidar_transfusion_node.hpp index 466c2202de3e3..3723e0c232b4d 100644 --- a/perception/autoware_lidar_transfusion/include/autoware/lidar_transfusion/lidar_transfusion_node.hpp +++ b/perception/autoware_lidar_transfusion/include/autoware/lidar_transfusion/lidar_transfusion_node.hpp @@ -21,9 +21,9 @@ #include "autoware/lidar_transfusion/transfusion_trt.hpp" #include "autoware/lidar_transfusion/visibility_control.hpp" -#include -#include -#include +#include +#include +#include #include #include @@ -62,10 +62,9 @@ class LIDAR_TRANSFUSION_PUBLIC LidarTransfusionNode : public rclcpp::Node std::unique_ptr detector_ptr_{nullptr}; // debugger - std::unique_ptr> stop_watch_ptr_{ - nullptr}; - std::unique_ptr debug_publisher_ptr_{nullptr}; - std::unique_ptr published_time_pub_{nullptr}; + std::unique_ptr> stop_watch_ptr_{nullptr}; + std::unique_ptr debug_publisher_ptr_{nullptr}; + std::unique_ptr published_time_pub_{nullptr}; }; } // namespace autoware::lidar_transfusion diff --git a/perception/autoware_lidar_transfusion/include/autoware/lidar_transfusion/transfusion_trt.hpp b/perception/autoware_lidar_transfusion/include/autoware/lidar_transfusion/transfusion_trt.hpp index 00bb5e726706c..a67141c7a1c9b 100644 --- a/perception/autoware_lidar_transfusion/include/autoware/lidar_transfusion/transfusion_trt.hpp +++ b/perception/autoware_lidar_transfusion/include/autoware/lidar_transfusion/transfusion_trt.hpp @@ -24,7 +24,7 @@ #include "autoware/lidar_transfusion/visibility_control.hpp" #include -#include +#include #include @@ -64,8 +64,7 @@ class LIDAR_TRANSFUSION_PUBLIC TransfusionTRT std::unique_ptr network_trt_ptr_{nullptr}; std::unique_ptr vg_ptr_{nullptr}; - std::unique_ptr> stop_watch_ptr_{ - nullptr}; + std::unique_ptr> stop_watch_ptr_{nullptr}; std::unique_ptr pre_ptr_{nullptr}; std::unique_ptr post_ptr_{nullptr}; cudaStream_t stream_{nullptr}; diff --git a/perception/autoware_lidar_transfusion/lib/postprocess/non_maximum_suppression.cpp b/perception/autoware_lidar_transfusion/lib/postprocess/non_maximum_suppression.cpp index 003358dc6b431..a25bc7711c898 100644 --- a/perception/autoware_lidar_transfusion/lib/postprocess/non_maximum_suppression.cpp +++ b/perception/autoware_lidar_transfusion/lib/postprocess/non_maximum_suppression.cpp @@ -16,7 +16,7 @@ #include #include -#include +#include #include @@ -46,7 +46,7 @@ bool NonMaximumSuppression::isTargetPairObject( return false; } - const auto sqr_dist_2d = autoware::universe_utils::calcSquaredDistance2d( + const auto sqr_dist_2d = autoware_utils::calc_squared_distance2d( autoware::object_recognition_utils::getPose(object1), autoware::object_recognition_utils::getPose(object2)); return sqr_dist_2d <= search_distance_2d_sq_; diff --git a/perception/autoware_lidar_transfusion/lib/ros_utils.cpp b/perception/autoware_lidar_transfusion/lib/ros_utils.cpp index 82f95697100cd..2f9720a826c86 100644 --- a/perception/autoware_lidar_transfusion/lib/ros_utils.cpp +++ b/perception/autoware_lidar_transfusion/lib/ros_utils.cpp @@ -15,8 +15,8 @@ #include "autoware/lidar_transfusion/ros_utils.hpp" #include -#include -#include +#include +#include #include #include @@ -52,12 +52,12 @@ void box3DToDetectedObject( // pose and shape obj.kinematics.pose_with_covariance.pose.position = - autoware::universe_utils::createPoint(box3d.x, box3d.y, box3d.z); + autoware_utils::create_point(box3d.x, box3d.y, box3d.z); obj.kinematics.pose_with_covariance.pose.orientation = - autoware::universe_utils::createQuaternionFromYaw(box3d.yaw); + autoware_utils::create_quaternion_from_yaw(box3d.yaw); obj.shape.type = autoware_perception_msgs::msg::Shape::BOUNDING_BOX; obj.shape.dimensions = - autoware::universe_utils::createTranslation(box3d.length, box3d.width, box3d.height); + autoware_utils::create_translation(box3d.length, box3d.width, box3d.height); } uint8_t getSemanticType(const std::string & class_name) diff --git a/perception/autoware_lidar_transfusion/lib/transfusion_trt.cpp b/perception/autoware_lidar_transfusion/lib/transfusion_trt.cpp index dd075a45cb29c..8abc75cf04d90 100644 --- a/perception/autoware_lidar_transfusion/lib/transfusion_trt.cpp +++ b/perception/autoware_lidar_transfusion/lib/transfusion_trt.cpp @@ -18,7 +18,7 @@ #include "autoware/lidar_transfusion/transfusion_config.hpp" #include -#include +#include #include #include @@ -40,8 +40,7 @@ TransfusionTRT::TransfusionTRT( : config_(std::move(config)) { vg_ptr_ = std::make_unique(densification_param, config_, stream_); - stop_watch_ptr_ = - std::make_unique>(); + stop_watch_ptr_ = std::make_unique>(); stop_watch_ptr_->tic("processing/inner"); initPtr(); initTrt(trt_config); diff --git a/perception/autoware_lidar_transfusion/package.xml b/perception/autoware_lidar_transfusion/package.xml index f9fd376e8be9a..004caf523fa82 100644 --- a/perception/autoware_lidar_transfusion/package.xml +++ b/perception/autoware_lidar_transfusion/package.xml @@ -16,7 +16,7 @@ autoware_perception_msgs autoware_point_types autoware_tensorrt_common - autoware_universe_utils + autoware_utils launch_ros pcl_ros rclcpp diff --git a/perception/autoware_lidar_transfusion/src/lidar_transfusion_node.cpp b/perception/autoware_lidar_transfusion/src/lidar_transfusion_node.cpp index 563abd97698e0..4b9f68c02fb0b 100644 --- a/perception/autoware_lidar_transfusion/src/lidar_transfusion_node.cpp +++ b/perception/autoware_lidar_transfusion/src/lidar_transfusion_node.cpp @@ -101,12 +101,12 @@ LidarTransfusionNode::LidarTransfusionNode(const rclcpp::NodeOptions & options) objects_pub_ = this->create_publisher( "~/output/objects", rclcpp::QoS(1)); - published_time_pub_ = std::make_unique(this); + published_time_pub_ = std::make_unique(this); // initialize debug tool { - using autoware::universe_utils::DebugPublisher; - using autoware::universe_utils::StopWatch; + using autoware_utils::DebugPublisher; + using autoware_utils::StopWatch; stop_watch_ptr_ = std::make_unique>(); debug_publisher_ptr_ = std::make_unique(this, this->get_name()); stop_watch_ptr_->tic("cyclic"); @@ -165,14 +165,15 @@ void LidarTransfusionNode::cloudCallback(const sensor_msgs::msg::PointCloud2::Co std::chrono::nanoseconds( (this->get_clock()->now() - output_msg.header.stamp).nanoseconds())) .count(); - debug_publisher_ptr_->publish( + debug_publisher_ptr_->publish( "debug/cyclic_time_ms", cyclic_time_ms); - debug_publisher_ptr_->publish( + debug_publisher_ptr_->publish( "debug/pipeline_latency_ms", pipeline_latency_ms); - debug_publisher_ptr_->publish( + debug_publisher_ptr_->publish( "debug/processing_time/total_ms", processing_time_ms); for (const auto & [topic, time_ms] : proc_timing) { - debug_publisher_ptr_->publish(topic, time_ms); + debug_publisher_ptr_->publish( + topic, time_ms); } } } diff --git a/perception/autoware_map_based_prediction/include/map_based_prediction/data_structure.hpp b/perception/autoware_map_based_prediction/include/map_based_prediction/data_structure.hpp index a547971da1820..6ede54838e35b 100644 --- a/perception/autoware_map_based_prediction/include/map_based_prediction/data_structure.hpp +++ b/perception/autoware_map_based_prediction/include/map_based_prediction/data_structure.hpp @@ -15,7 +15,7 @@ #ifndef MAP_BASED_PREDICTION__DATA_STRUCTURE_HPP_ #define MAP_BASED_PREDICTION__DATA_STRUCTURE_HPP_ -#include +#include #include #include @@ -99,7 +99,6 @@ struct CrosswalkUserData using LaneletsData = std::vector; using ManeuverProbability = std::unordered_map; -using autoware::universe_utils::StopWatch; using autoware_map_msgs::msg::LaneletMapBin; using autoware_perception_msgs::msg::ObjectClassification; using autoware_perception_msgs::msg::PredictedObject; @@ -112,6 +111,7 @@ using autoware_perception_msgs::msg::TrackedObjects; using autoware_perception_msgs::msg::TrafficLightElement; using autoware_perception_msgs::msg::TrafficLightGroup; using autoware_perception_msgs::msg::TrafficLightGroupArray; +using autoware_utils::StopWatch; using LaneletPathWithPathInfo = std::pair; } // namespace autoware::map_based_prediction diff --git a/perception/autoware_map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp b/perception/autoware_map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp index 4ed77af694b80..502ed354e13a2 100644 --- a/perception/autoware_map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp +++ b/perception/autoware_map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp @@ -19,14 +19,14 @@ #include "map_based_prediction/path_generator.hpp" #include "map_based_prediction/predictor_vru.hpp" -#include -#include -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include +#include +#include #include #include @@ -86,12 +86,12 @@ class MapBasedPredictionNode : public rclcpp::Node rclcpp::Publisher::SharedPtr pub_debug_markers_; rclcpp::Subscription::SharedPtr sub_objects_; rclcpp::Subscription::SharedPtr sub_map_; - autoware::universe_utils::InterProcessPollingSubscriber - sub_traffic_signals_{this, "/traffic_signals"}; + autoware_utils::InterProcessPollingSubscriber sub_traffic_signals_{ + this, "/traffic_signals"}; // debug publisher - std::unique_ptr> stop_watch_ptr_; - std::unique_ptr processing_time_publisher_; + std::unique_ptr> stop_watch_ptr_; + std::unique_ptr processing_time_publisher_; // Object History std::unordered_map> road_users_history_; @@ -107,7 +107,7 @@ class MapBasedPredictionNode : public rclcpp::Node const std::vector & parameters); // Pose Transform Listener - autoware::universe_utils::TransformListener transform_listener_{this}; + autoware_utils::TransformListener transform_listener_{this}; // Path Generator std::shared_ptr path_generator_; @@ -219,16 +219,16 @@ class MapBasedPredictionNode : public rclcpp::Node std::vector convertPredictedReferencePath( const TrackedObject & object, const std::vector & lanelet_ref_paths) const; - mutable universe_utils::LRUCache> + mutable autoware_utils::LRUCache> lru_cache_of_convert_path_type_{1000}; std::pair convertLaneletPathToPosePath( const lanelet::routing::LaneletPath & path) const; ////// Debugger - std::unique_ptr published_time_publisher_; - rclcpp::Publisher::SharedPtr + std::unique_ptr published_time_publisher_; + rclcpp::Publisher::SharedPtr detailed_processing_time_publisher_; - std::shared_ptr time_keeper_; + std::shared_ptr time_keeper_; visualization_msgs::msg::Marker getDebugMarker( const TrackedObject & object, const Maneuver & maneuver, const size_t obj_num); @@ -242,8 +242,8 @@ class MapBasedPredictionNode : public rclcpp::Node inline std::vector calcTrajectoryCurvatureFrom3Points( const TrajectoryPoints & trajectory, size_t idx_dist) { - using autoware::universe_utils::calcCurvature; - using autoware::universe_utils::getPoint; + using autoware_utils::calc_curvature; + using autoware_utils::get_point; if (trajectory.size() < 3) { const std::vector k_arr(trajectory.size(), 0.0); @@ -263,11 +263,11 @@ class MapBasedPredictionNode : public rclcpp::Node for (size_t i = 1; i + 1 < trajectory.size(); i++) { double curvature = 0.0; - const auto p0 = getPoint(trajectory.at(i - std::min(idx_dist, i))); - const auto p1 = getPoint(trajectory.at(i)); - const auto p2 = getPoint(trajectory.at(i + std::min(idx_dist, trajectory.size() - 1 - i))); + const auto p0 = get_point(trajectory.at(i - std::min(idx_dist, i))); + const auto p1 = get_point(trajectory.at(i)); + const auto p2 = get_point(trajectory.at(i + std::min(idx_dist, trajectory.size() - 1 - i))); try { - curvature = calcCurvature(p0, p1, p2); + curvature = calc_curvature(p0, p1, p2); } catch (std::exception const & e) { // ...code that handles the error... RCLCPP_WARN(rclcpp::get_logger("map_based_prediction"), "%s", e.what()); diff --git a/perception/autoware_map_based_prediction/include/map_based_prediction/path_generator.hpp b/perception/autoware_map_based_prediction/include/map_based_prediction/path_generator.hpp index 8c2244304097f..30fa1962949fb 100644 --- a/perception/autoware_map_based_prediction/include/map_based_prediction/path_generator.hpp +++ b/perception/autoware_map_based_prediction/include/map_based_prediction/path_generator.hpp @@ -18,7 +18,7 @@ #include "map_based_prediction/data_structure.hpp" #include -#include +#include #include #include @@ -89,7 +89,7 @@ class PathGenerator explicit PathGenerator(const double sampling_time_interval); PathGenerator(const double sampling_time_interval, const double min_crosswalk_user_velocity); - void setTimeKeeper(std::shared_ptr time_keeper_ptr); + void setTimeKeeper(std::shared_ptr time_keeper_ptr); PredictedPath generatePathForNonVehicleObject( const TrackedObject & object, const double duration) const; @@ -129,7 +129,7 @@ class PathGenerator bool use_vehicle_acceleration_; double acceleration_exponential_half_life_; - std::shared_ptr time_keeper_; + std::shared_ptr time_keeper_; // Member functions PredictedPath generateStraightPath(const TrackedObject & object, const double duration) const; diff --git a/perception/autoware_map_based_prediction/include/map_based_prediction/predictor_vru.hpp b/perception/autoware_map_based_prediction/include/map_based_prediction/predictor_vru.hpp index 4691178718f60..0fd38d6f35d59 100644 --- a/perception/autoware_map_based_prediction/include/map_based_prediction/predictor_vru.hpp +++ b/perception/autoware_map_based_prediction/include/map_based_prediction/predictor_vru.hpp @@ -18,8 +18,8 @@ #include "map_based_prediction/data_structure.hpp" #include "map_based_prediction/path_generator.hpp" -#include #include +#include #include #include @@ -77,7 +77,7 @@ class PredictorVru void setLaneletMap(std::shared_ptr lanelet_map_ptr); - void setTimeKeeper(std::shared_ptr time_keeper_ptr) + void setTimeKeeper(std::shared_ptr time_keeper_ptr) { time_keeper_ = std::move(time_keeper_ptr); } @@ -94,7 +94,7 @@ class PredictorVru private: rclcpp::Node & node_; - std::shared_ptr time_keeper_; + std::shared_ptr time_keeper_; // Map data std::shared_ptr lanelet_map_ptr_; diff --git a/perception/autoware_map_based_prediction/include/map_based_prediction/utils.hpp b/perception/autoware_map_based_prediction/include/map_based_prediction/utils.hpp index 26a3b46564e30..3811ab272bdac 100644 --- a/perception/autoware_map_based_prediction/include/map_based_prediction/utils.hpp +++ b/perception/autoware_map_based_prediction/include/map_based_prediction/utils.hpp @@ -17,9 +17,9 @@ #include "map_based_prediction/data_structure.hpp" -#include -#include #include +#include +#include #include #include diff --git a/perception/autoware_map_based_prediction/package.xml b/perception/autoware_map_based_prediction/package.xml index fa6a1195b1ed2..e08672b37c0e1 100644 --- a/perception/autoware_map_based_prediction/package.xml +++ b/perception/autoware_map_based_prediction/package.xml @@ -21,7 +21,7 @@ autoware_lanelet2_extension autoware_motion_utils autoware_perception_msgs - autoware_universe_utils + autoware_utils glog rclcpp rclcpp_components diff --git a/perception/autoware_map_based_prediction/src/debug.cpp b/perception/autoware_map_based_prediction/src/debug.cpp index 752b53b34322d..f5336f8e33eaf 100644 --- a/perception/autoware_map_based_prediction/src/debug.cpp +++ b/perception/autoware_map_based_prediction/src/debug.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware/universe_utils/ros/marker_helper.hpp" +#include "autoware_utils/ros/marker_helper.hpp" #include "map_based_prediction/data_structure.hpp" #include "map_based_prediction/map_based_prediction_node.hpp" @@ -30,7 +30,7 @@ visualization_msgs::msg::Marker MapBasedPredictionNode::getDebugMarker( marker.type = visualization_msgs::msg::Marker::CUBE; marker.action = visualization_msgs::msg::Marker::ADD; marker.pose = object.kinematics.pose_with_covariance.pose; - marker.scale = autoware::universe_utils::createMarkerScale(3.0, 1.0, 1.0); + marker.scale = autoware_utils::create_marker_scale(3.0, 1.0, 1.0); // Color by maneuver double r = 0.0; @@ -43,7 +43,7 @@ visualization_msgs::msg::Marker MapBasedPredictionNode::getDebugMarker( } else { b = 1.0; } - marker.color = autoware::universe_utils::createMarkerColor(r, g, b, 0.8); + marker.color = autoware_utils::create_marker_color(r, g, b, 0.8); return marker; } diff --git a/perception/autoware_map_based_prediction/src/map_based_prediction_node.cpp b/perception/autoware_map_based_prediction/src/map_based_prediction_node.cpp index f32a693034431..a7933128c9a6a 100644 --- a/perception/autoware_map_based_prediction/src/map_based_prediction_node.cpp +++ b/perception/autoware_map_based_prediction/src/map_based_prediction_node.cpp @@ -19,15 +19,15 @@ #include #include #include -#include -#include -#include -#include -#include #include #include #include #include +#include +#include +#include +#include +#include #include #include @@ -56,7 +56,7 @@ namespace autoware::map_based_prediction { -using autoware::universe_utils::ScopedTimeTrack; +using autoware_utils::ScopedTimeTrack; namespace { @@ -97,7 +97,7 @@ double calcAbsLateralOffset( for (size_t i = 0; i < boundary_path.size(); ++i) { const double x = boundary_line[i].x(); const double y = boundary_line[i].y(); - boundary_path[i] = autoware::universe_utils::createPoint(x, y, 0.0); + boundary_path[i] = autoware_utils::create_point(x, y, 0.0); } return std::fabs(autoware::motion_utils::calcLateralOffset(boundary_path, search_pose.position)); @@ -482,21 +482,19 @@ MapBasedPredictionNode::MapBasedPredictionNode(const rclcpp::NodeOptions & node_ // debug publishers if (use_time_publisher) { processing_time_publisher_ = - std::make_unique(this, "map_based_prediction"); - published_time_publisher_ = - std::make_unique(this); - stop_watch_ptr_ = - std::make_unique>(); + std::make_unique(this, "map_based_prediction"); + published_time_publisher_ = std::make_unique(this); + stop_watch_ptr_ = std::make_unique>(); stop_watch_ptr_->tic("cyclic_time"); stop_watch_ptr_->tic("processing_time"); } if (use_time_keeper) { detailed_processing_time_publisher_ = - this->create_publisher( + this->create_publisher( "~/debug/processing_time_detail_ms", 1); - auto time_keeper = autoware::universe_utils::TimeKeeper(detailed_processing_time_publisher_); - time_keeper_ = std::make_shared(time_keeper); + auto time_keeper = autoware_utils::TimeKeeper(detailed_processing_time_publisher_); + time_keeper_ = std::make_shared(time_keeper); path_generator_->setTimeKeeper(time_keeper_); predictor_vru_->setTimeKeeper(time_keeper_); } @@ -513,15 +511,15 @@ MapBasedPredictionNode::MapBasedPredictionNode(const rclcpp::NodeOptions & node_ rcl_interfaces::msg::SetParametersResult MapBasedPredictionNode::onParam( const std::vector & parameters) { - using autoware::universe_utils::updateParam; + using autoware_utils::update_param; - updateParam(parameters, "max_lateral_accel", max_lateral_accel_); - updateParam(parameters, "min_acceleration_before_curve", min_acceleration_before_curve_); - updateParam( + update_param(parameters, "max_lateral_accel", max_lateral_accel_); + update_param(parameters, "min_acceleration_before_curve", min_acceleration_before_curve_); + update_param( parameters, "check_lateral_acceleration_constraints", check_lateral_acceleration_constraints_); - updateParam(parameters, "use_vehicle_acceleration", use_vehicle_acceleration_); - updateParam(parameters, "speed_limit_multiplier", speed_limit_multiplier_); - updateParam( + update_param(parameters, "use_vehicle_acceleration", use_vehicle_acceleration_); + update_param(parameters, "speed_limit_multiplier", speed_limit_multiplier_); + update_param( parameters, "acceleration_exponential_half_life", acceleration_exponential_half_life_); path_generator_->setUseVehicleAcceleration(use_vehicle_acceleration_); @@ -561,7 +559,7 @@ void MapBasedPredictionNode::objectsCallback(const TrackedObjects::ConstSharedPt // take traffic_signal { - const auto msg = sub_traffic_signals_.takeData(); + const auto msg = sub_traffic_signals_.take_data(); if (msg) { trafficSignalsCallback(msg); } @@ -576,7 +574,7 @@ void MapBasedPredictionNode::objectsCallback(const TrackedObjects::ConstSharedPt geometry_msgs::msg::TransformStamped::ConstSharedPtr world2map_transform; bool is_object_not_in_map_frame = in_objects->header.frame_id != "map"; if (is_object_not_in_map_frame) { - world2map_transform = transform_listener_.getTransform( + world2map_transform = transform_listener_.get_transform( "map", // target in_objects->header.frame_id, // src in_objects->header.stamp, rclcpp::Duration::from_seconds(0.1)); @@ -702,7 +700,7 @@ void MapBasedPredictionNode::updateObjectData(TrackedObject & object) // Compute yaw angle from the velocity and position of the object const auto & object_pose = object.kinematics.pose_with_covariance.pose; const auto & object_twist = object.kinematics.twist_with_covariance.twist; - const auto future_object_pose = autoware::universe_utils::calcOffsetPose( + const auto future_object_pose = autoware_utils::calc_offset_pose( object_pose, object_twist.linear.x * 0.1, object_twist.linear.y * 0.1, 0.0); // assumption: the object vx is much larger than vy @@ -721,16 +719,15 @@ void MapBasedPredictionNode::updateObjectData(TrackedObject & object) tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation); // flip the angle object.kinematics.pose_with_covariance.pose.orientation = - autoware::universe_utils::createQuaternionFromYaw( - autoware::universe_utils::pi + original_yaw); + autoware_utils::create_quaternion_from_yaw(autoware_utils::pi + original_yaw); break; } default: { - const auto updated_object_yaw = autoware::universe_utils::calcAzimuthAngle( - object_pose.position, future_object_pose.position); + const auto updated_object_yaw = + autoware_utils::calc_azimuth_angle(object_pose.position, future_object_pose.position); object.kinematics.pose_with_covariance.pose.orientation = - autoware::universe_utils::createQuaternionFromYaw(updated_object_yaw); + autoware_utils::create_quaternion_from_yaw(updated_object_yaw); break; } } @@ -757,7 +754,7 @@ void MapBasedPredictionNode::updateRoadUsersHistory( std::unique_ptr st_ptr; if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); - std::string object_id = autoware::universe_utils::toHexString(object.object_id); + std::string object_id = autoware_utils::to_hex_string(object.object_id); const auto current_lanelets = getLanelets(current_lanelets_data); ObjectData single_object_data; @@ -766,8 +763,7 @@ void MapBasedPredictionNode::updateRoadUsersHistory( single_object_data.future_possible_lanelets = current_lanelets; single_object_data.pose = object.kinematics.pose_with_covariance.pose; const double object_yaw = tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation); - single_object_data.pose.orientation = - autoware::universe_utils::createQuaternionFromYaw(object_yaw); + single_object_data.pose.orientation = autoware_utils::create_quaternion_from_yaw(object_yaw); single_object_data.time_delay = std::fabs((this->get_clock()->now() - header.stamp).seconds()); single_object_data.twist = object.kinematics.twist_with_covariance.twist; @@ -845,7 +841,7 @@ std::vector MapBasedPredictionNode::getPredictedReferen return search_dist; }; - std::string object_id = autoware::universe_utils::toHexString(object.object_id); + std::string object_id = autoware_utils::to_hex_string(object.object_id); geometry_msgs::msg::Pose object_pose = object.kinematics.pose_with_covariance.pose; // Step 2. Get possible paths for each lanelet @@ -1180,7 +1176,7 @@ Maneuver MapBasedPredictionNode::predictObjectManeuverByLatDiffDistance( for (const auto & lanelet : prev_lanelets) { const double lane_yaw = lanelet::utils::getLaneletAngle(lanelet, prev_pose.position); const double delta_yaw = tf2::getYaw(prev_pose.orientation) - lane_yaw; - const double normalized_delta_yaw = autoware::universe_utils::normalizeRadian(delta_yaw); + const double normalized_delta_yaw = autoware_utils::normalize_radian(delta_yaw); if (normalized_delta_yaw < closest_prev_yaw) { closest_prev_yaw = normalized_delta_yaw; prev_lanelet = lanelet; @@ -1190,7 +1186,7 @@ Maneuver MapBasedPredictionNode::predictObjectManeuverByLatDiffDistance( // Step4. Check if the vehicle has changed lane const auto current_lanelet = current_lanelet_data.lanelet; const auto current_pose = object_pose; - const double dist = autoware::universe_utils::calcDistance2d(prev_pose, current_pose); + const double dist = autoware_utils::calc_distance2d(prev_pose, current_pose); lanelet::routing::LaneletPaths possible_paths = routing_graph_ptr_->possiblePaths(prev_lanelet, dist + 2.0, 0, false); bool has_lane_changed = true; @@ -1252,7 +1248,7 @@ double MapBasedPredictionNode::calcRightLateralOffset( for (size_t i = 0; i < boundary_path.size(); ++i) { const double x = boundary_line[i].x(); const double y = boundary_line[i].y(); - boundary_path[i] = autoware::universe_utils::createPoint(x, y, 0.0); + boundary_path[i] = autoware_utils::create_point(x, y, 0.0); } return std::fabs(autoware::motion_utils::calcLateralOffset(boundary_path, search_pose.position)); @@ -1438,7 +1434,7 @@ std::pair MapBasedPredictionNode::convertLaneletPathToPosePath // Prevent from inserting same points if (!converted_path.empty()) { const auto last_p = converted_path.back(); - const double tmp_dist = autoware::universe_utils::calcDistance2d(last_p, current_p); + const double tmp_dist = autoware_utils::calc_distance2d(last_p, current_p); if (tmp_dist < 1e-6) { prev_p = current_p; continue; diff --git a/perception/autoware_map_based_prediction/src/path_generator.cpp b/perception/autoware_map_based_prediction/src/path_generator.cpp index 3bd6d62aba999..b9ae4ba79645f 100644 --- a/perception/autoware_map_based_prediction/src/path_generator.cpp +++ b/perception/autoware_map_based_prediction/src/path_generator.cpp @@ -17,7 +17,7 @@ #include #include #include -#include +#include #include #include @@ -26,7 +26,7 @@ namespace autoware::map_based_prediction { -using autoware::universe_utils::ScopedTimeTrack; +using autoware_utils::ScopedTimeTrack; PathGenerator::PathGenerator(const double sampling_time_interval) : sampling_time_interval_(sampling_time_interval) @@ -41,8 +41,7 @@ PathGenerator::PathGenerator( { } -void PathGenerator::setTimeKeeper( - std::shared_ptr time_keeper_ptr) +void PathGenerator::setTimeKeeper(std::shared_ptr time_keeper_ptr) { time_keeper_ = std::move(time_keeper_ptr); } @@ -73,9 +72,8 @@ PredictedPath PathGenerator::generatePathToTargetPoint( const auto arrival_time = pedestrian_to_entry_point.norm() / velocity; const auto pedestrian_to_entry_point_normalized = pedestrian_to_entry_point.normalized(); - const auto pedestrian_to_entry_point_orientation = - autoware::universe_utils::createQuaternionFromYaw(std::atan2( - pedestrian_to_entry_point_normalized.y(), pedestrian_to_entry_point_normalized.x())); + const auto pedestrian_to_entry_point_orientation = autoware_utils::create_quaternion_from_yaw( + std::atan2(pedestrian_to_entry_point_normalized.y(), pedestrian_to_entry_point_normalized.x())); for (double dt = 0.0; dt < arrival_time + ep; dt += sampling_time_interval_) { geometry_msgs::msg::Pose world_frame_pose; @@ -120,11 +118,10 @@ PredictedPath PathGenerator::generatePathForCrosswalkUser( const auto arrival_time = pedestrian_to_entry_point.norm() / velocity; const auto pedestrian_to_entry_point_normalized = pedestrian_to_entry_point.normalized(); - const auto pedestrian_to_entry_point_orientation = - autoware::universe_utils::createQuaternionFromYaw(std::atan2( - pedestrian_to_entry_point_normalized.y(), pedestrian_to_entry_point_normalized.x())); + const auto pedestrian_to_entry_point_orientation = autoware_utils::create_quaternion_from_yaw( + std::atan2(pedestrian_to_entry_point_normalized.y(), pedestrian_to_entry_point_normalized.x())); const auto entry_to_exit_point_normalized = entry_to_exit_point.normalized(); - const auto entry_to_exit_point_orientation = autoware::universe_utils::createQuaternionFromYaw( + const auto entry_to_exit_point_orientation = autoware_utils::create_quaternion_from_yaw( std::atan2(entry_to_exit_point_normalized.y(), entry_to_exit_point_normalized.x())); for (double dt = 0.0; dt < duration + ep; dt += sampling_time_interval_) { @@ -230,7 +227,7 @@ PredictedPath PathGenerator::generateStraightPath( path.time_step = rclcpp::Duration::from_seconds(sampling_time_interval_); path.path.reserve(static_cast((duration) / sampling_time_interval_)); for (double dt = 0.0; dt < duration; dt += sampling_time_interval_) { - const auto future_obj_pose = autoware::universe_utils::calcOffsetPose( + const auto future_obj_pose = autoware_utils::calc_offset_pose( object_pose, object_twist.linear.x * dt, object_twist.linear.y * dt, 0.0); path.path.push_back(future_obj_pose); } @@ -536,8 +533,8 @@ PosePath PathGenerator::interpolateReferencePath( tf2::fromMsg(base_path.at(i).orientation, src_tf); base_path_orientation.at(i) = src_tf; if (i > 0) { - base_path_s.at(i) = base_path_s.at(i - 1) + autoware::universe_utils::calcDistance2d( - base_path.at(i - 1), base_path.at(i)); + base_path_s.at(i) = base_path_s.at(i - 1) + + autoware_utils::calc_distance2d(base_path.at(i - 1), base_path.at(i)); } } @@ -558,7 +555,7 @@ PosePath PathGenerator::interpolateReferencePath( interpolated_path.resize(interpolate_num); for (size_t i = 0; i < interpolate_num; ++i) { geometry_msgs::msg::Pose interpolated_pose; - interpolated_pose.position = autoware::universe_utils::createPoint( + interpolated_pose.position = autoware_utils::create_point( lerp_ref_path_x.at(i), lerp_ref_path_y.at(i), lerp_ref_path_z.at(i)); interpolated_pose.orientation = tf2::toMsg(lerp_ref_path_orientation.at(i)); interpolated_path.at(i) = interpolated_pose; @@ -596,11 +593,11 @@ PredictedPath PathGenerator::convertToPredictedPath( double d_offset = frenet_point.d; // Converted Pose - auto predicted_pose = autoware::universe_utils::calcOffsetPose(ref_pose, 0.0, d_offset, 0.0); + auto predicted_pose = autoware_utils::calc_offset_pose(ref_pose, 0.0, d_offset, 0.0); predicted_pose.position.z += object_height; - const double yaw = autoware::universe_utils::calcAzimuthAngle( + const double yaw = autoware_utils::calc_azimuth_angle( predicted_path.path.at(i - 1).position, predicted_pose.position); - predicted_pose.orientation = autoware::universe_utils::createQuaternionFromYaw(yaw); + predicted_pose.orientation = autoware_utils::create_quaternion_from_yaw(yaw); predicted_path.path.at(i) = predicted_pose; } diff --git a/perception/autoware_map_based_prediction/src/predictor_vru.cpp b/perception/autoware_map_based_prediction/src/predictor_vru.cpp index ffbdbd97b50a2..8776d987bf173 100644 --- a/perception/autoware_map_based_prediction/src/predictor_vru.cpp +++ b/perception/autoware_map_based_prediction/src/predictor_vru.cpp @@ -16,8 +16,8 @@ #include "map_based_prediction/utils.hpp" -#include -#include +#include +#include #include #include @@ -30,7 +30,7 @@ namespace autoware::map_based_prediction { -using autoware::universe_utils::ScopedTimeTrack; +using autoware_utils::ScopedTimeTrack; namespace { @@ -43,7 +43,7 @@ boost::optional isReachableCrosswalkEdgePoints( const auto & obj_pos = object.kinematics.pose_with_covariance.pose.position; const auto & obj_vel = object.kinematics.twist_with_covariance.twist.linear; - const auto yaw = autoware::universe_utils::getRPY(object.kinematics.pose_with_covariance.pose).z; + const auto yaw = autoware_utils::get_rpy(object.kinematics.pose_with_covariance.pose).z; lanelet::BasicPoint2d obj_pos_as_lanelet(obj_pos.x, obj_pos.y); @@ -145,7 +145,7 @@ bool hasPotentialToReach( { const auto & obj_pos = object.kinematics.pose_with_covariance.pose.position; const auto & obj_vel = object.kinematics.twist_with_covariance.twist.linear; - const auto yaw = autoware::universe_utils::getRPY(object.kinematics.pose_with_covariance.pose).z; + const auto yaw = autoware_utils::get_rpy(object.kinematics.pose_with_covariance.pose).z; constexpr double stop_velocity_th = 0.14; // [m/s] const auto estimated_velocity = std::hypot(obj_vel.x, obj_vel.y); @@ -163,16 +163,16 @@ bool hasPotentialToReach( const double pedestrian_to_crosswalk_left_direction = std::atan2(left_point.y() - obj_pos.y, left_point.x() - obj_pos.x); return std::make_pair( - autoware::universe_utils::normalizeRadian( + autoware_utils::normalize_radian( pedestrian_to_crosswalk_right_direction - pedestrian_to_crosswalk_center_direction), - autoware::universe_utils::normalizeRadian( + autoware_utils::normalize_radian( pedestrian_to_crosswalk_left_direction - pedestrian_to_crosswalk_center_direction)); }(); const double pedestrian_heading_rel_direction = [&]() { const double pedestrian_heading_direction = std::atan2(obj_vel.x * std::sin(yaw), obj_vel.x * std::cos(yaw)); - return autoware::universe_utils::normalizeRadian( + return autoware_utils::normalize_radian( pedestrian_heading_direction - pedestrian_to_crosswalk_center_direction); }(); @@ -219,11 +219,11 @@ bool isIntersecting( const geometry_msgs::msg::Point & point1, const geometry_msgs::msg::Point & point2, const lanelet::ConstPoint3d & point3, const lanelet::ConstPoint3d & point4) { - const auto p1 = autoware::universe_utils::createPoint(point1.x, point1.y, 0.0); - const auto p2 = autoware::universe_utils::createPoint(point2.x, point2.y, 0.0); - const auto p3 = autoware::universe_utils::createPoint(point3.x(), point3.y(), 0.0); - const auto p4 = autoware::universe_utils::createPoint(point4.x(), point4.y(), 0.0); - const auto intersection = autoware::universe_utils::intersect(p1, p2, p3, p4); + const auto p1 = autoware_utils::create_point(point1.x, point1.y, 0.0); + const auto p2 = autoware_utils::create_point(point2.x, point2.y, 0.0); + const auto p3 = autoware_utils::create_point(point3.x(), point3.y(), 0.0); + const auto p4 = autoware_utils::create_point(point4.x(), point4.y(), 0.0); + const auto intersection = autoware_utils::intersect(p1, p2, p3, p4); return intersection.has_value(); } @@ -310,7 +310,7 @@ void PredictorVru::loadCurrentCrosswalkUsers(const TrackedObjects & objects) const bool isDisappeared = std::none_of( objects.objects.begin(), objects.objects.end(), [&it](autoware_perception_msgs::msg::TrackedObject obj) { - return autoware::universe_utils::toHexString(obj.object_id) == it->first.first; + return autoware_utils::to_hex_string(obj.object_id) == it->first.first; }); if (isDisappeared) { it = stopped_times_against_green_.erase(it); @@ -329,7 +329,7 @@ void PredictorVru::loadCurrentCrosswalkUsers(const TrackedObjects & objects) if ( label_for_prediction == ObjectClassification::PEDESTRIAN || label_for_prediction == ObjectClassification::BICYCLE) { - const std::string object_id = autoware::universe_utils::toHexString(object.object_id); + const std::string object_id = autoware_utils::to_hex_string(object.object_id); current_crosswalk_users_.emplace(object_id, object); } } @@ -355,7 +355,7 @@ PredictedObject PredictorVru::predict( std::unique_ptr st_ptr; if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); - std::string object_id = autoware::universe_utils::toHexString(object.object_id); + std::string object_id = autoware_utils::to_hex_string(object.object_id); if (match_lost_and_appeared_crosswalk_users_) { object_id = tryMatchNewObjectToDisappeared(object_id, current_crosswalk_users_); } @@ -641,11 +641,10 @@ bool PredictorVru::calcIntentionToCrossWithTrafficSignal( return elem_opt ? elem_opt.value().color : TrafficLightElement::UNKNOWN; }(); - const auto key = - std::make_pair(autoware::universe_utils::toHexString(object.object_id), signal_id); + const auto key = std::make_pair(autoware_utils::to_hex_string(object.object_id), signal_id); if ( signal_color == TrafficLightElement::GREEN && - autoware::universe_utils::calcNorm(object.kinematics.twist_with_covariance.twist.linear) < + autoware_utils::calc_norm(object.kinematics.twist_with_covariance.twist.linear) < threshold_velocity_assumed_as_stopping_) { stopped_times_against_green_.try_emplace(key, node_.get_clock()->now()); diff --git a/perception/autoware_map_based_prediction/src/utils.cpp b/perception/autoware_map_based_prediction/src/utils.cpp index cfc5036106daf..27d9673ffccfb 100644 --- a/perception/autoware_map_based_prediction/src/utils.cpp +++ b/perception/autoware_map_based_prediction/src/utils.cpp @@ -15,9 +15,9 @@ #include "map_based_prediction/utils.hpp" #include -#include -#include #include +#include +#include #include #include @@ -50,7 +50,7 @@ double calcAbsYawDiffBetweenLaneletAndObject( const double lane_yaw = lanelet::utils::getLaneletAngle(lanelet, object.kinematics.pose_with_covariance.pose.position); const double delta_yaw = object_yaw - lane_yaw; - const double normalized_delta_yaw = autoware::universe_utils::normalizeRadian(delta_yaw); + const double normalized_delta_yaw = autoware_utils::normalize_radian(delta_yaw); const double abs_norm_delta = std::fabs(normalized_delta_yaw); return abs_norm_delta; } @@ -125,7 +125,7 @@ ObjectClassification::_label_type changeLabelForPrediction( if (within_road_lanelet) return ObjectClassification::MOTORCYCLE; constexpr float high_speed_threshold = - autoware::universe_utils::kmph2mps(25.0); // High speed bicycle 25 km/h + autoware_utils::kmph2mps(25.0); // High speed bicycle 25 km/h // calc abs speed from x and y velocity const double abs_speed = std::hypot( object.kinematics.twist_with_covariance.twist.linear.x, @@ -139,7 +139,7 @@ ObjectClassification::_label_type changeLabelForPrediction( case ObjectClassification::PEDESTRIAN: { const float max_velocity_for_human_mps = - autoware::universe_utils::kmph2mps(25.0); // Max human being motion speed is 25km/h + autoware_utils::kmph2mps(25.0); // Max human being motion speed is 25km/h const double abs_speed = std::hypot( object.kinematics.twist_with_covariance.twist.linear.x, object.kinematics.twist_with_covariance.twist.linear.y); @@ -245,7 +245,7 @@ double calculateLocalLikelihood( const double obj_yaw = tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation); const double lane_yaw = lanelet::utils::getLaneletAngle(current_lanelet, obj_point); const double delta_yaw = obj_yaw - lane_yaw; - const double abs_norm_delta_yaw = std::fabs(autoware::universe_utils::normalizeRadian(delta_yaw)); + const double abs_norm_delta_yaw = std::fabs(autoware_utils::normalize_radian(delta_yaw)); // compute lateral distance const auto centerline = current_lanelet.centerline(); @@ -295,7 +295,7 @@ bool isDuplicated( for (const auto & prev_predicted_path : predicted_paths) { const auto prev_path_end = prev_predicted_path.path.back().position; const auto current_path_end = predicted_path.path.back().position; - const double dist = autoware::universe_utils::calcDistance2d(prev_path_end, current_path_end); + const double dist = autoware_utils::calc_distance2d(prev_path_end, current_path_end); if (dist < CLOSE_PATH_THRESHOLD) { return true; } @@ -317,7 +317,7 @@ bool checkCloseLaneletCondition( // If the object is in the objects history, we check if the target lanelet is // inside the current lanelets id or following lanelets - const std::string object_id = autoware::universe_utils::toHexString(object.object_id); + const std::string object_id = autoware_utils::to_hex_string(object.object_id); if (road_users_history.count(object_id) != 0) { const std::vector & possible_lanelet = road_users_history.at(object_id).back().future_possible_lanelets; @@ -335,7 +335,7 @@ bool checkCloseLaneletCondition( const double lane_yaw = lanelet::utils::getLaneletAngle( lanelet.second, object.kinematics.pose_with_covariance.pose.position); const double delta_yaw = object_yaw - lane_yaw; - const double normalized_delta_yaw = autoware::universe_utils::normalizeRadian(delta_yaw); + const double normalized_delta_yaw = autoware_utils::normalize_radian(delta_yaw); const double abs_norm_delta = std::fabs(normalized_delta_yaw); // Step3. Check if the closest lanelet is valid, and add all diff --git a/perception/autoware_multi_object_tracker/lib/association/association.cpp b/perception/autoware_multi_object_tracker/lib/association/association.cpp index d74d87489f8db..c66d11d839496 100644 --- a/perception/autoware_multi_object_tracker/lib/association/association.cpp +++ b/perception/autoware_multi_object_tracker/lib/association/association.cpp @@ -53,9 +53,8 @@ double getFormedYawAngle( const geometry_msgs::msg::Quaternion & measurement_quat, const geometry_msgs::msg::Quaternion & tracker_quat, const bool distinguish_front_or_back = true) { - const double measurement_yaw = - autoware::universe_utils::normalizeRadian(tf2::getYaw(measurement_quat)); - const double tracker_yaw = autoware::universe_utils::normalizeRadian(tf2::getYaw(tracker_quat)); + const double measurement_yaw = autoware_utils::normalize_radian(tf2::getYaw(measurement_quat)); + const double tracker_yaw = autoware_utils::normalize_radian(tf2::getYaw(tracker_quat)); const double angle_range = distinguish_front_or_back ? M_PI : M_PI_2; const double angle_step = distinguish_front_or_back ? 2.0 * M_PI : M_PI; // Fixed measurement_yaw to be in the range of +-90 or 180 degrees of X_t(IDX::YAW) @@ -174,7 +173,7 @@ Eigen::MatrixXd DataAssociation::calcScoreMatrix( (*tracker_itr)->getTrackedObject(measurements.header.stamp, tracked_object); const double max_dist = max_dist_matrix_(tracker_label, measurement_label); - const double dist = autoware::universe_utils::calcDistance2d( + const double dist = autoware_utils::calc_distance2d( measurement_object.kinematics.pose_with_covariance.pose.position, tracked_object.kinematics.pose_with_covariance.pose.position); @@ -187,7 +186,7 @@ Eigen::MatrixXd DataAssociation::calcScoreMatrix( if (passed_gate) { const double max_area = max_area_matrix_(tracker_label, measurement_label); const double min_area = min_area_matrix_(tracker_label, measurement_label); - const double area = autoware::universe_utils::getArea(measurement_object.shape); + const double area = autoware_utils::get_area(measurement_object.shape); if (area < min_area || max_area < area) passed_gate = false; } // angle gate diff --git a/perception/autoware_multi_object_tracker/lib/object_model/shapes.cpp b/perception/autoware_multi_object_tracker/lib/object_model/shapes.cpp index 2818e6dc53bb9..1e1e6cf5bbeef 100644 --- a/perception/autoware_multi_object_tracker/lib/object_model/shapes.cpp +++ b/perception/autoware_multi_object_tracker/lib/object_model/shapes.cpp @@ -18,8 +18,8 @@ #include "autoware/multi_object_tracker/object_model/shapes.hpp" #include -#include -#include +#include +#include #include #include @@ -36,28 +36,27 @@ namespace autoware::multi_object_tracker { namespace shapes { -inline double getSumArea(const std::vector & polygons) +inline double getSumArea(const std::vector & polygons) { return std::accumulate( - polygons.begin(), polygons.end(), 0.0, [](double acc, autoware::universe_utils::Polygon2d p) { - return acc + boost::geometry::area(p); - }); + polygons.begin(), polygons.end(), 0.0, + [](double acc, autoware_utils::Polygon2d p) { return acc + boost::geometry::area(p); }); } inline double getIntersectionArea( - const autoware::universe_utils::Polygon2d & source_polygon, - const autoware::universe_utils::Polygon2d & target_polygon) + const autoware_utils::Polygon2d & source_polygon, + const autoware_utils::Polygon2d & target_polygon) { - std::vector intersection_polygons; + std::vector intersection_polygons; boost::geometry::intersection(source_polygon, target_polygon, intersection_polygons); return getSumArea(intersection_polygons); } inline double getUnionArea( - const autoware::universe_utils::Polygon2d & source_polygon, - const autoware::universe_utils::Polygon2d & target_polygon) + const autoware_utils::Polygon2d & source_polygon, + const autoware_utils::Polygon2d & target_polygon) { - std::vector union_polygons; + std::vector union_polygons; boost::geometry::union_(source_polygon, target_polygon, union_polygons); return getSumArea(union_polygons); } @@ -68,10 +67,10 @@ double get2dIoU( { static const double MIN_AREA = 1e-6; - const auto source_polygon = autoware::universe_utils::toPolygon2d( + const auto source_polygon = autoware_utils::to_polygon2d( source_object.kinematics.pose_with_covariance.pose, source_object.shape); if (boost::geometry::area(source_polygon) < MIN_AREA) return 0.0; - const auto target_polygon = autoware::universe_utils::toPolygon2d( + const auto target_polygon = autoware_utils::to_polygon2d( target_object.kinematics.pose_with_covariance.pose, target_object.shape); if (boost::geometry::area(target_polygon) < MIN_AREA) return 0.0; diff --git a/perception/autoware_multi_object_tracker/lib/tracker/model/bicycle_tracker.cpp b/perception/autoware_multi_object_tracker/lib/tracker/model/bicycle_tracker.cpp index 32caf16b17a2d..1fcfd56908db0 100644 --- a/perception/autoware_multi_object_tracker/lib/tracker/model/bicycle_tracker.cpp +++ b/perception/autoware_multi_object_tracker/lib/tracker/model/bicycle_tracker.cpp @@ -24,10 +24,10 @@ #include #include #include -#include -#include -#include -#include +#include +#include +#include +#include #include #include @@ -99,7 +99,7 @@ BicycleTracker::BicycleTracker( // Set initial state { - using autoware::universe_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; + using autoware_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; const double x = object.kinematics.pose_with_covariance.pose.position.x; const double y = object.kinematics.pose_with_covariance.pose.position.y; const double yaw = tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation); @@ -291,7 +291,7 @@ bool BicycleTracker::getTrackedObject( const auto origin_yaw = tf2::getYaw(object_.kinematics.pose_with_covariance.pose.orientation); const auto ekf_pose_yaw = tf2::getYaw(pose_with_cov.pose.orientation); object.shape.footprint = - autoware::universe_utils::rotatePolygon(object.shape.footprint, origin_yaw - ekf_pose_yaw); + autoware_utils::rotate_polygon(object.shape.footprint, origin_yaw - ekf_pose_yaw); return true; } diff --git a/perception/autoware_multi_object_tracker/lib/tracker/model/pass_through_tracker.cpp b/perception/autoware_multi_object_tracker/lib/tracker/model/pass_through_tracker.cpp index e9852baaa94af..0ec33d3fafcbe 100644 --- a/perception/autoware_multi_object_tracker/lib/tracker/model/pass_through_tracker.cpp +++ b/perception/autoware_multi_object_tracker/lib/tracker/model/pass_through_tracker.cpp @@ -21,7 +21,7 @@ #include #include #include -#include +#include #include #include @@ -84,7 +84,7 @@ bool PassThroughTracker::measure( bool PassThroughTracker::getTrackedObject( const rclcpp::Time & time, types::DynamicObject & object) const { - using autoware::universe_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; + using autoware_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; object = object_; object.object_id = getUUID(); object.classification = getClassification(); diff --git a/perception/autoware_multi_object_tracker/lib/tracker/model/pedestrian_tracker.cpp b/perception/autoware_multi_object_tracker/lib/tracker/model/pedestrian_tracker.cpp index 779fba7376857..fecd02d4ea898 100644 --- a/perception/autoware_multi_object_tracker/lib/tracker/model/pedestrian_tracker.cpp +++ b/perception/autoware_multi_object_tracker/lib/tracker/model/pedestrian_tracker.cpp @@ -22,10 +22,10 @@ #include #include #include -#include -#include -#include -#include +#include +#include +#include +#include #include #include @@ -96,7 +96,7 @@ PedestrianTracker::PedestrianTracker( // Set initial state { - using autoware::universe_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; + using autoware_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; const double x = object.kinematics.pose_with_covariance.pose.position.x; const double y = object.kinematics.pose_with_covariance.pose.position.y; const double yaw = tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation); @@ -293,7 +293,7 @@ bool PedestrianTracker::getTrackedObject( const auto origin_yaw = tf2::getYaw(object_.kinematics.pose_with_covariance.pose.orientation); const auto ekf_pose_yaw = tf2::getYaw(pose_with_cov.pose.orientation); object.shape.footprint = - autoware::universe_utils::rotatePolygon(object.shape.footprint, origin_yaw - ekf_pose_yaw); + autoware_utils::rotate_polygon(object.shape.footprint, origin_yaw - ekf_pose_yaw); } return true; diff --git a/perception/autoware_multi_object_tracker/lib/tracker/model/unknown_tracker.cpp b/perception/autoware_multi_object_tracker/lib/tracker/model/unknown_tracker.cpp index 933eba820b1d1..1da27db525b45 100644 --- a/perception/autoware_multi_object_tracker/lib/tracker/model/unknown_tracker.cpp +++ b/perception/autoware_multi_object_tracker/lib/tracker/model/unknown_tracker.cpp @@ -18,10 +18,10 @@ #include #include #include -#include -#include -#include -#include +#include +#include +#include +#include #include #include @@ -64,13 +64,13 @@ UnknownTracker::UnknownTracker( // Set motion limits motion_model_.setMotionLimits( - autoware::universe_utils::kmph2mps(60), /* [m/s] maximum velocity, x */ - autoware::universe_utils::kmph2mps(60) /* [m/s] maximum velocity, y */ + autoware_utils::kmph2mps(60), /* [m/s] maximum velocity, x */ + autoware_utils::kmph2mps(60) /* [m/s] maximum velocity, y */ ); // Set initial state { - using autoware::universe_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; + using autoware_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; const double x = object.kinematics.pose_with_covariance.pose.position.x; const double y = object.kinematics.pose_with_covariance.pose.position.y; auto pose_cov = object.kinematics.pose_with_covariance.covariance; @@ -103,8 +103,8 @@ UnknownTracker::UnknownTracker( } if (!object.kinematics.has_twist_covariance) { - constexpr double p0_stddev_vx = autoware::universe_utils::kmph2mps(10); // [m/s] - constexpr double p0_stddev_vy = autoware::universe_utils::kmph2mps(10); // [m/s] + constexpr double p0_stddev_vx = autoware_utils::kmph2mps(10); // [m/s] + constexpr double p0_stddev_vy = autoware_utils::kmph2mps(10); // [m/s] const double p0_cov_vx = std::pow(p0_stddev_vx, 2.0); const double p0_cov_vy = std::pow(p0_stddev_vy, 2.0); twist_cov[XYZRPY_COV_IDX::X_X] = p0_cov_vx; @@ -144,7 +144,7 @@ types::DynamicObject UnknownTracker::getUpdatingObject( // UNCERTAINTY MODEL if (!object.kinematics.has_position_covariance) { // fill covariance matrix - using autoware::universe_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; + using autoware_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; const double & r_cov_x = ekf_params_.r_cov_x; const double & r_cov_y = ekf_params_.r_cov_y; auto & pose_cov = updating_object.kinematics.pose_with_covariance.covariance; diff --git a/perception/autoware_multi_object_tracker/lib/tracker/model/vehicle_tracker.cpp b/perception/autoware_multi_object_tracker/lib/tracker/model/vehicle_tracker.cpp index f497df1f52c6c..201db246fb4b3 100644 --- a/perception/autoware_multi_object_tracker/lib/tracker/model/vehicle_tracker.cpp +++ b/perception/autoware_multi_object_tracker/lib/tracker/model/vehicle_tracker.cpp @@ -24,10 +24,10 @@ #include #include #include -#include -#include -#include -#include +#include +#include +#include +#include #include #include @@ -60,7 +60,7 @@ VehicleTracker::VehicleTracker( // velocity deviation threshold // if the predicted velocity is close to the observed velocity, // the observed velocity is used as the measurement. - velocity_deviation_threshold_ = autoware::universe_utils::kmph2mps(10); // [m/s] + velocity_deviation_threshold_ = autoware_utils::kmph2mps(10); // [m/s] // OBJECT SHAPE MODEL if (object.shape.type == autoware_perception_msgs::msg::Shape::BOUNDING_BOX) { @@ -118,7 +118,7 @@ VehicleTracker::VehicleTracker( // Set initial state { - using autoware::universe_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; + using autoware_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; const double x = object.kinematics.pose_with_covariance.pose.position.x; const double y = object.kinematics.pose_with_covariance.pose.position.y; const double yaw = tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation); @@ -350,7 +350,7 @@ bool VehicleTracker::getTrackedObject( const auto origin_yaw = tf2::getYaw(object_.kinematics.pose_with_covariance.pose.orientation); const auto ekf_pose_yaw = tf2::getYaw(pose_with_cov.pose.orientation); object.shape.footprint = - autoware::universe_utils::rotatePolygon(object.shape.footprint, origin_yaw - ekf_pose_yaw); + autoware_utils::rotate_polygon(object.shape.footprint, origin_yaw - ekf_pose_yaw); return true; } diff --git a/perception/autoware_multi_object_tracker/lib/tracker/motion_model/bicycle_motion_model.cpp b/perception/autoware_multi_object_tracker/lib/tracker/motion_model/bicycle_motion_model.cpp index b9518afeaf95a..52dd3bc3cc66f 100644 --- a/perception/autoware_multi_object_tracker/lib/tracker/motion_model/bicycle_motion_model.cpp +++ b/perception/autoware_multi_object_tracker/lib/tracker/motion_model/bicycle_motion_model.cpp @@ -23,9 +23,9 @@ #include #include -#include -#include -#include +#include +#include +#include #include @@ -37,7 +37,7 @@ namespace autoware::multi_object_tracker // cspell: ignore CTRV // Bicycle CTRV motion model // CTRV : Constant Turn Rate and constant Velocity -using autoware::universe_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; +using autoware_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; BicycleMotionModel::BicycleMotionModel() : logger_(rclcpp::get_logger("BicycleMotionModel")) { @@ -245,7 +245,7 @@ bool BicycleMotionModel::limitStates() X_t(IDX::SLIP) = X_t(IDX::SLIP) < 0 ? -motion_params_.max_slip : motion_params_.max_slip; } // normalize yaw - X_t(IDX::YAW) = autoware::universe_utils::normalizeRadian(X_t(IDX::YAW)); + X_t(IDX::YAW) = autoware_utils::normalize_radian(X_t(IDX::YAW)); // overwrite state ekf_.init(X_t, P_t); diff --git a/perception/autoware_multi_object_tracker/lib/tracker/motion_model/ctrv_motion_model.cpp b/perception/autoware_multi_object_tracker/lib/tracker/motion_model/ctrv_motion_model.cpp index 2a2d7ea80bae1..14d7ce8eb67aa 100644 --- a/perception/autoware_multi_object_tracker/lib/tracker/motion_model/ctrv_motion_model.cpp +++ b/perception/autoware_multi_object_tracker/lib/tracker/motion_model/ctrv_motion_model.cpp @@ -23,9 +23,9 @@ #include #include -#include -#include -#include +#include +#include +#include #include @@ -33,7 +33,7 @@ namespace autoware::multi_object_tracker { // cspell: ignore CTRV // Constant Turn Rate and constant Velocity (CTRV) motion model -using autoware::universe_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; +using autoware_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; CTRVMotionModel::CTRVMotionModel() : logger_(rclcpp::get_logger("CTRVMotionModel")) { @@ -220,7 +220,7 @@ bool CTRVMotionModel::limitStates() X_t(IDX::WZ) = X_t(IDX::WZ) < 0 ? -motion_params_.max_wz : motion_params_.max_wz; } // normalize yaw - X_t(IDX::YAW) = autoware::universe_utils::normalizeRadian(X_t(IDX::YAW)); + X_t(IDX::YAW) = autoware_utils::normalize_radian(X_t(IDX::YAW)); // overwrite state ekf_.init(X_t, P_t); diff --git a/perception/autoware_multi_object_tracker/lib/tracker/motion_model/cv_motion_model.cpp b/perception/autoware_multi_object_tracker/lib/tracker/motion_model/cv_motion_model.cpp index fd3b03da398e1..e208f8b6ad1f9 100644 --- a/perception/autoware_multi_object_tracker/lib/tracker/motion_model/cv_motion_model.cpp +++ b/perception/autoware_multi_object_tracker/lib/tracker/motion_model/cv_motion_model.cpp @@ -23,9 +23,9 @@ #include #include -#include -#include -#include +#include +#include +#include #include @@ -33,7 +33,7 @@ namespace autoware::multi_object_tracker { // cspell: ignore CV // Constant Velocity (CV) motion model -using autoware::universe_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; +using autoware_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; CVMotionModel::CVMotionModel() : logger_(rclcpp::get_logger("CVMotionModel")) { diff --git a/perception/autoware_multi_object_tracker/lib/uncertainty/uncertainty_processor.cpp b/perception/autoware_multi_object_tracker/lib/uncertainty/uncertainty_processor.cpp index bdd3a78a198a8..5923000c6bfc7 100644 --- a/perception/autoware_multi_object_tracker/lib/uncertainty/uncertainty_processor.cpp +++ b/perception/autoware_multi_object_tracker/lib/uncertainty/uncertainty_processor.cpp @@ -28,7 +28,7 @@ namespace autoware::multi_object_tracker { namespace uncertainty { -using autoware::universe_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; +using autoware_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; object_model::StateCovariance covarianceFromObjectClass(const ObjectClassification & object_class) { diff --git a/perception/autoware_multi_object_tracker/package.xml b/perception/autoware_multi_object_tracker/package.xml index 08f9ac89e1d91..72b14c2a3008b 100644 --- a/perception/autoware_multi_object_tracker/package.xml +++ b/perception/autoware_multi_object_tracker/package.xml @@ -16,7 +16,7 @@ autoware_kalman_filter autoware_object_recognition_utils autoware_perception_msgs - autoware_universe_utils + autoware_utils diagnostic_updater eigen glog diff --git a/perception/autoware_multi_object_tracker/src/debugger/debug_object.hpp b/perception/autoware_multi_object_tracker/src/debugger/debug_object.hpp index d507a0350bbb8..fb4e9d2ded898 100644 --- a/perception/autoware_multi_object_tracker/src/debugger/debug_object.hpp +++ b/perception/autoware_multi_object_tracker/src/debugger/debug_object.hpp @@ -16,7 +16,7 @@ #define DEBUGGER__DEBUG_OBJECT_HPP_ #include "autoware/multi_object_tracker/tracker/model/tracker_base.hpp" -#include "autoware/universe_utils/ros/uuid_helper.hpp" +#include "autoware_utils/ros/uuid_helper.hpp" #include diff --git a/perception/autoware_multi_object_tracker/src/debugger/debugger.cpp b/perception/autoware_multi_object_tracker/src/debugger/debugger.cpp index 3761fc1e6f43a..4c1b330c0fa5c 100644 --- a/perception/autoware_multi_object_tracker/src/debugger/debugger.cpp +++ b/perception/autoware_multi_object_tracker/src/debugger/debugger.cpp @@ -29,7 +29,7 @@ TrackerDebugger::TrackerDebugger(rclcpp::Node & node, const std::string & frame_ // initialize debug publishers if (debug_settings_.publish_processing_time) { processing_time_publisher_ = - std::make_unique(&node_, "multi_object_tracker"); + std::make_unique(&node_, "multi_object_tracker"); } if (debug_settings_.publish_tentative_objects) { diff --git a/perception/autoware_multi_object_tracker/src/debugger/debugger.hpp b/perception/autoware_multi_object_tracker/src/debugger/debugger.hpp index 3df901a63505c..4b85b6bcb9c87 100644 --- a/perception/autoware_multi_object_tracker/src/debugger/debugger.hpp +++ b/perception/autoware_multi_object_tracker/src/debugger/debugger.hpp @@ -15,8 +15,8 @@ #ifndef DEBUGGER__DEBUGGER_HPP_ #define DEBUGGER__DEBUGGER_HPP_ -#include "autoware/universe_utils/ros/debug_publisher.hpp" -#include "autoware/universe_utils/ros/published_time_publisher.hpp" +#include "autoware_utils/ros/debug_publisher.hpp" +#include "autoware_utils/ros/published_time_publisher.hpp" #include "debug_object.hpp" #include @@ -59,7 +59,7 @@ class TrackerDebugger rclcpp::Node & node_; rclcpp::Publisher::SharedPtr debug_tentative_objects_pub_; - std::unique_ptr processing_time_publisher_; + std::unique_ptr processing_time_publisher_; rclcpp::Publisher::SharedPtr debug_objects_markers_pub_; diagnostic_updater::Updater diagnostic_updater_; diff --git a/perception/autoware_multi_object_tracker/src/multi_object_tracker_node.cpp b/perception/autoware_multi_object_tracker/src/multi_object_tracker_node.cpp index b90c570e7f4a6..2dab26e67b5a2 100644 --- a/perception/autoware_multi_object_tracker/src/multi_object_tracker_node.cpp +++ b/perception/autoware_multi_object_tracker/src/multi_object_tracker_node.cpp @@ -193,8 +193,7 @@ MultiObjectTracker::MultiObjectTracker(const rclcpp::NodeOptions & node_options) // Debugger debugger_ = std::make_unique(*this, world_frame_id_); debugger_->setObjectChannels(input_names_short); - published_time_publisher_ = - std::make_unique(this); + published_time_publisher_ = std::make_unique(this); } void MultiObjectTracker::onTrigger() diff --git a/perception/autoware_multi_object_tracker/src/multi_object_tracker_node.hpp b/perception/autoware_multi_object_tracker/src/multi_object_tracker_node.hpp index 6a904dc7e2ef9..c967f8403a918 100644 --- a/perception/autoware_multi_object_tracker/src/multi_object_tracker_node.hpp +++ b/perception/autoware_multi_object_tracker/src/multi_object_tracker_node.hpp @@ -70,7 +70,7 @@ class MultiObjectTracker : public rclcpp::Node // debugger std::unique_ptr debugger_; - std::unique_ptr published_time_publisher_; + std::unique_ptr published_time_publisher_; // publish timer rclcpp::TimerBase::SharedPtr publish_timer_; diff --git a/perception/autoware_object_merger/include/autoware/object_merger/object_association_merger_node.hpp b/perception/autoware_object_merger/include/autoware/object_merger/object_association_merger_node.hpp index 81fa34803d6cc..0bf6f43dc6928 100644 --- a/perception/autoware_object_merger/include/autoware/object_merger/object_association_merger_node.hpp +++ b/perception/autoware_object_merger/include/autoware/object_merger/object_association_merger_node.hpp @@ -16,9 +16,9 @@ #define AUTOWARE__OBJECT_MERGER__OBJECT_ASSOCIATION_MERGER_NODE_HPP_ #include "autoware/object_merger/association/data_association.hpp" -#include "autoware/universe_utils/ros/debug_publisher.hpp" -#include "autoware/universe_utils/ros/published_time_publisher.hpp" -#include "autoware/universe_utils/system/stop_watch.hpp" +#include "autoware_utils/ros/debug_publisher.hpp" +#include "autoware_utils/ros/published_time_publisher.hpp" +#include "autoware_utils/system/stop_watch.hpp" #include @@ -84,10 +84,10 @@ class ObjectAssociationMergerNode : public rclcpp::Node } overlapped_judge_param_; // debug publisher - std::unique_ptr processing_time_publisher_; - std::unique_ptr> stop_watch_ptr_; + std::unique_ptr processing_time_publisher_; + std::unique_ptr> stop_watch_ptr_; - std::unique_ptr published_time_publisher_; + std::unique_ptr published_time_publisher_; }; } // namespace autoware::object_merger diff --git a/perception/autoware_object_merger/package.xml b/perception/autoware_object_merger/package.xml index be9dec7a83c80..f3334120e8734 100644 --- a/perception/autoware_object_merger/package.xml +++ b/perception/autoware_object_merger/package.xml @@ -15,7 +15,7 @@ autoware_object_recognition_utils autoware_perception_msgs - autoware_universe_utils + autoware_utils eigen mussp rclcpp diff --git a/perception/autoware_object_merger/src/association/data_association.cpp b/perception/autoware_object_merger/src/association/data_association.cpp index bec2d48469562..26f4ffb3a0fee 100644 --- a/perception/autoware_object_merger/src/association/data_association.cpp +++ b/perception/autoware_object_merger/src/association/data_association.cpp @@ -16,7 +16,7 @@ #include "autoware/object_merger/association/solver/gnn_solver.hpp" #include "autoware/object_recognition_utils/object_recognition_utils.hpp" -#include "autoware/universe_utils/geometry/geometry.hpp" +#include "autoware_utils/geometry/geometry.hpp" #include #include @@ -30,8 +30,8 @@ double getFormedYawAngle( const geometry_msgs::msg::Quaternion & quat0, const geometry_msgs::msg::Quaternion & quat1, const bool distinguish_front_or_back = true) { - const double yaw0 = autoware::universe_utils::normalizeRadian(tf2::getYaw(quat0)); - const double yaw1 = autoware::universe_utils::normalizeRadian(tf2::getYaw(quat1)); + const double yaw0 = autoware_utils::normalize_radian(tf2::getYaw(quat0)); + const double yaw1 = autoware_utils::normalize_radian(tf2::getYaw(quat1)); const double angle_range = distinguish_front_or_back ? M_PI : M_PI_2; const double angle_step = distinguish_front_or_back ? 2.0 * M_PI : M_PI; // Fixed yaw0 to be in the range of +-90 or 180 degrees of yaw1 @@ -135,7 +135,7 @@ Eigen::MatrixXd DataAssociation::calcScoreMatrix( double score = 0.0; if (can_assign_matrix_(object1_label, object0_label)) { const double max_dist = max_dist_matrix_(object1_label, object0_label); - const double dist = autoware::universe_utils::calcDistance2d( + const double dist = autoware_utils::calc_distance2d( object0.kinematics.pose_with_covariance.pose.position, object1.kinematics.pose_with_covariance.pose.position); diff --git a/perception/autoware_object_merger/src/object_association_merger_node.cpp b/perception/autoware_object_merger/src/object_association_merger_node.cpp index 40bdd0a7d6938..bf2d93013a549 100644 --- a/perception/autoware_object_merger/src/object_association_merger_node.cpp +++ b/perception/autoware_object_merger/src/object_association_merger_node.cpp @@ -17,7 +17,7 @@ #include "autoware/object_merger/object_association_merger_node.hpp" #include "autoware/object_recognition_utils/object_recognition_utils.hpp" -#include "autoware/universe_utils/geometry/geometry.hpp" +#include "autoware_utils/geometry/geometry.hpp" #include #include @@ -48,7 +48,7 @@ bool isUnknownObjectOverlapped( const double distance_threshold = distance_threshold_map.at( autoware::object_recognition_utils::getHighestProbLabel(known_object.classification)); const double sq_distance_threshold = std::pow(distance_threshold, 2.0); - const double sq_distance = autoware::universe_utils::calcSquaredDistance2d( + const double sq_distance = autoware_utils::calc_squared_distance2d( unknown_object.kinematics.pose_with_covariance.pose, known_object.kinematics.pose_with_covariance.pose); if (sq_distance_threshold < sq_distance) return false; @@ -125,13 +125,11 @@ ObjectAssociationMergerNode::ObjectAssociationMergerNode(const rclcpp::NodeOptio // Debug publisher processing_time_publisher_ = - std::make_unique(this, "object_association_merger"); - stop_watch_ptr_ = - std::make_unique>(); + std::make_unique(this, "object_association_merger"); + stop_watch_ptr_ = std::make_unique>(); stop_watch_ptr_->tic("cyclic_time"); stop_watch_ptr_->tic("processing_time"); - published_time_publisher_ = - std::make_unique(this); + published_time_publisher_ = std::make_unique(this); } void ObjectAssociationMergerNode::objectsCallback( diff --git a/perception/autoware_object_velocity_splitter/package.xml b/perception/autoware_object_velocity_splitter/package.xml index 548933c42bba2..e208e6b7e78b8 100644 --- a/perception/autoware_object_velocity_splitter/package.xml +++ b/perception/autoware_object_velocity_splitter/package.xml @@ -12,7 +12,7 @@ ament_cmake_auto autoware_perception_msgs - autoware_universe_utils + autoware_utils geometry_msgs rclcpp rclcpp_components diff --git a/perception/autoware_object_velocity_splitter/src/object_velocity_splitter_node.cpp b/perception/autoware_object_velocity_splitter/src/object_velocity_splitter_node.cpp index ddc7171b4d57d..223da2fa80979 100644 --- a/perception/autoware_object_velocity_splitter/src/object_velocity_splitter_node.cpp +++ b/perception/autoware_object_velocity_splitter/src/object_velocity_splitter_node.cpp @@ -14,7 +14,7 @@ #include "object_velocity_splitter_node.hpp" -#include "autoware/universe_utils/geometry/geometry.hpp" +#include "autoware_utils/geometry/geometry.hpp" #include #include @@ -74,8 +74,8 @@ void ObjectVelocitySplitterNode::onObjects(const DetectedObjects::ConstSharedPtr for (const auto & object : msg->objects) { if ( - std::abs(autoware::universe_utils::calcNorm( - object.kinematics.twist_with_covariance.twist.linear)) < node_param_.velocity_threshold) { + std::abs(autoware_utils::calc_norm(object.kinematics.twist_with_covariance.twist.linear)) < + node_param_.velocity_threshold) { low_speed_objects.objects.emplace_back(object); } else { high_speed_objects.objects.emplace_back(object); diff --git a/perception/autoware_occupancy_grid_map_outlier_filter/package.xml b/perception/autoware_occupancy_grid_map_outlier_filter/package.xml index 113213b65e6d0..9e242be4567c1 100644 --- a/perception/autoware_occupancy_grid_map_outlier_filter/package.xml +++ b/perception/autoware_occupancy_grid_map_outlier_filter/package.xml @@ -19,7 +19,7 @@ autoware_lanelet2_extension autoware_pointcloud_preprocessor - autoware_universe_utils + autoware_utils libpcl-all-dev message_filters nav_msgs diff --git a/perception/autoware_occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_node.cpp b/perception/autoware_occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_node.cpp index 094825056e62c..eba97ee9b92dd 100644 --- a/perception/autoware_occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_node.cpp +++ b/perception/autoware_occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_node.cpp @@ -14,9 +14,9 @@ #include "occupancy_grid_map_outlier_filter_node.hpp" -#include "autoware/universe_utils/geometry/geometry.hpp" -#include "autoware/universe_utils/ros/debug_publisher.hpp" -#include "autoware/universe_utils/system/stop_watch.hpp" +#include "autoware_utils/geometry/geometry.hpp" +#include "autoware_utils/ros/debug_publisher.hpp" +#include "autoware_utils/system/stop_watch.hpp" #include @@ -40,7 +40,7 @@ namespace { -using autoware::universe_utils::ScopedTimeTrack; +using autoware_utils::ScopedTimeTrack; bool transformPointcloud( const sensor_msgs::msg::PointCloud2 & input, const tf2_ros::Buffer & tf2, @@ -77,7 +77,7 @@ geometry_msgs::msg::PoseStamped getPoseStamped( RCLCPP_WARN_THROTTLE( rclcpp::get_logger("occupancy_grid_map_outlier_filter"), clock, 5000, "%s", ex.what()); } - return autoware::universe_utils::transform2pose(tf_stamped); + return autoware_utils::transform2pose(tf_stamped); } boost::optional getCost( @@ -231,8 +231,8 @@ OccupancyGridMapOutlierFilterComponent::OccupancyGridMapOutlierFilterComponent( { // initialize debug tool { - using autoware::universe_utils::DebugPublisher; - using autoware::universe_utils::StopWatch; + using autoware_utils::DebugPublisher; + using autoware_utils::StopWatch; stop_watch_ptr_ = std::make_unique>(); debug_publisher_ = std::make_unique(this, "occupancy_grid_map_outlier_filter"); stop_watch_ptr_->tic("cyclic_time"); @@ -273,10 +273,10 @@ OccupancyGridMapOutlierFilterComponent::OccupancyGridMapOutlierFilterComponent( bool use_time_keeper = declare_parameter("publish_processing_time_detail"); if (use_time_keeper) { detailed_processing_time_publisher_ = - this->create_publisher( + this->create_publisher( "~/debug/processing_time_detail_ms", 1); - auto time_keeper = autoware::universe_utils::TimeKeeper(detailed_processing_time_publisher_); - time_keeper_ = std::make_shared(time_keeper); + auto time_keeper = autoware_utils::TimeKeeper(detailed_processing_time_publisher_); + time_keeper_ = std::make_shared(time_keeper); } } diff --git a/perception/autoware_occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_node.hpp b/perception/autoware_occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_node.hpp index bf2604a6fccae..bb959858c3583 100644 --- a/perception/autoware_occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_node.hpp +++ b/perception/autoware_occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_node.hpp @@ -16,8 +16,8 @@ #define OCCUPANCY_GRID_MAP_OUTLIER_FILTER_NODE_HPP_ #include "autoware/pointcloud_preprocessor/filter.hpp" -#include "autoware/universe_utils/ros/published_time_publisher.hpp" -#include "autoware/universe_utils/system/time_keeper.hpp" +#include "autoware_utils/ros/published_time_publisher.hpp" +#include "autoware_utils/system/time_keeper.hpp" #include #include @@ -121,9 +121,9 @@ class OccupancyGridMapOutlierFilterComponent : public rclcpp::Node // Debugger std::shared_ptr debugger_ptr_; - std::unique_ptr> stop_watch_ptr_; - std::unique_ptr debug_publisher_; - std::unique_ptr published_time_publisher_; + std::unique_ptr> stop_watch_ptr_; + std::unique_ptr debug_publisher_; + std::unique_ptr published_time_publisher_; // ROS Parameters std::string map_frame_; @@ -131,9 +131,9 @@ class OccupancyGridMapOutlierFilterComponent : public rclcpp::Node int cost_threshold_; // time keeper - rclcpp::Publisher::SharedPtr + rclcpp::Publisher::SharedPtr detailed_processing_time_publisher_; - std::shared_ptr time_keeper_; + std::shared_ptr time_keeper_; }; } // namespace autoware::occupancy_grid_map_outlier_filter diff --git a/perception/autoware_probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/costmap_2d/occupancy_grid_map_base.hpp b/perception/autoware_probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/costmap_2d/occupancy_grid_map_base.hpp index 41beafd7dfbef..ef548d9a5de93 100644 --- a/perception/autoware_probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/costmap_2d/occupancy_grid_map_base.hpp +++ b/perception/autoware_probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/costmap_2d/occupancy_grid_map_base.hpp @@ -58,7 +58,7 @@ #include #endif -#include +#include #include #include #include @@ -99,9 +99,9 @@ class OccupancyGridMapInterface : public nav2_costmap_2d::Costmap2D double min_height_; double max_height_; - const double min_angle_ = autoware::universe_utils::deg2rad(-180.0); - const double max_angle_ = autoware::universe_utils::deg2rad(180.0); - const double angle_increment_inv_ = 1.0 / autoware::universe_utils::deg2rad(0.1); + const double min_angle_ = autoware_utils::deg2rad(-180.0); + const double max_angle_ = autoware_utils::deg2rad(180.0); + const double angle_increment_inv_ = 1.0 / autoware_utils::deg2rad(0.1); Eigen::Matrix4f mat_map_, mat_scan_; diff --git a/perception/autoware_probabilistic_occupancy_grid_map/lib/costmap_2d/occupancy_grid_map_fixed.cpp b/perception/autoware_probabilistic_occupancy_grid_map/lib/costmap_2d/occupancy_grid_map_fixed.cpp index b74d9e0c25d52..5f96ade24ecbd 100644 --- a/perception/autoware_probabilistic_occupancy_grid_map/lib/costmap_2d/occupancy_grid_map_fixed.cpp +++ b/perception/autoware_probabilistic_occupancy_grid_map/lib/costmap_2d/occupancy_grid_map_fixed.cpp @@ -20,7 +20,7 @@ #include "autoware/probabilistic_occupancy_grid_map/utils/utils_kernel.hpp" #include -#include +#include #include #include diff --git a/perception/autoware_probabilistic_occupancy_grid_map/lib/costmap_2d/occupancy_grid_map_projective.cpp b/perception/autoware_probabilistic_occupancy_grid_map/lib/costmap_2d/occupancy_grid_map_projective.cpp index 5bc30872fcf70..51bc9705a2d61 100644 --- a/perception/autoware_probabilistic_occupancy_grid_map/lib/costmap_2d/occupancy_grid_map_projective.cpp +++ b/perception/autoware_probabilistic_occupancy_grid_map/lib/costmap_2d/occupancy_grid_map_projective.cpp @@ -18,7 +18,7 @@ #include "autoware/probabilistic_occupancy_grid_map/costmap_2d/occupancy_grid_map_projective_kernel.hpp" #include "autoware/probabilistic_occupancy_grid_map/utils/utils.hpp" -#include +#include #include #include #include diff --git a/perception/autoware_probabilistic_occupancy_grid_map/lib/utils/utils.cpp b/perception/autoware_probabilistic_occupancy_grid_map/lib/utils/utils.cpp index 43a76eaf478bd..d010ff81563a4 100644 --- a/perception/autoware_probabilistic_occupancy_grid_map/lib/utils/utils.cpp +++ b/perception/autoware_probabilistic_occupancy_grid_map/lib/utils/utils.cpp @@ -14,7 +14,7 @@ #include "autoware/probabilistic_occupancy_grid_map/utils/utils.hpp" -#include +#include #include @@ -76,7 +76,7 @@ bool transformPointcloudAsync( Eigen::Matrix4f getTransformMatrix(const geometry_msgs::msg::Pose & pose) { - auto transform = autoware::universe_utils::pose2transform(pose); + auto transform = autoware_utils::pose2transform(pose); Eigen::Matrix4f tf_matrix = tf2::transformToEigen(transform).matrix().cast(); return tf_matrix; } @@ -119,7 +119,7 @@ geometry_msgs::msg::Pose getPose( geometry_msgs::msg::TransformStamped tf_stamped; tf_stamped = tf2.lookupTransform( target_frame, source_header.frame_id, source_header.stamp, rclcpp::Duration::from_seconds(0.5)); - pose = autoware::universe_utils::transform2pose(tf_stamped.transform); + pose = autoware_utils::transform2pose(tf_stamped.transform); return pose; } @@ -131,7 +131,7 @@ geometry_msgs::msg::Pose getPose( geometry_msgs::msg::TransformStamped tf_stamped; tf_stamped = tf2.lookupTransform(target_frame, source_frame, stamp, rclcpp::Duration::from_seconds(0.5)); - pose = autoware::universe_utils::transform2pose(tf_stamped.transform); + pose = autoware_utils::transform2pose(tf_stamped.transform); return pose; } diff --git a/perception/autoware_probabilistic_occupancy_grid_map/package.xml b/perception/autoware_probabilistic_occupancy_grid_map/package.xml index b0e6463519fd9..55d12e1ce23bd 100644 --- a/perception/autoware_probabilistic_occupancy_grid_map/package.xml +++ b/perception/autoware_probabilistic_occupancy_grid_map/package.xml @@ -17,7 +17,7 @@ eigen3_cmake_module autoware_cuda_utils - autoware_universe_utils + autoware_utils grid_map_costmap_2d grid_map_msgs grid_map_ros diff --git a/perception/autoware_probabilistic_occupancy_grid_map/src/fusion/synchronized_grid_map_fusion_node.cpp b/perception/autoware_probabilistic_occupancy_grid_map/src/fusion/synchronized_grid_map_fusion_node.cpp index cf1d828493035..51fa7ceffbaa8 100644 --- a/perception/autoware_probabilistic_occupancy_grid_map/src/fusion/synchronized_grid_map_fusion_node.cpp +++ b/perception/autoware_probabilistic_occupancy_grid_map/src/fusion/synchronized_grid_map_fusion_node.cpp @@ -21,7 +21,7 @@ namespace autoware::occupancy_grid_map { -using autoware::universe_utils::ScopedTimeTrack; +using autoware_utils::ScopedTimeTrack; using costmap_2d::OccupancyGridMapFixedBlindSpot; using costmap_2d::OccupancyGridMapLOBFUpdater; using geometry_msgs::msg::Pose; @@ -137,8 +137,8 @@ GridMapFusionNode::GridMapFusionNode(const rclcpp::NodeOptions & node_options) // debug tools { - using autoware::universe_utils::DebugPublisher; - using autoware::universe_utils::StopWatch; + using autoware_utils::DebugPublisher; + using autoware_utils::StopWatch; stop_watch_ptr_ = std::make_unique>(); debug_publisher_ptr_ = std::make_unique(this, "synchronized_grid_map_fusion"); stop_watch_ptr_->tic("cyclic_time"); @@ -148,10 +148,10 @@ GridMapFusionNode::GridMapFusionNode(const rclcpp::NodeOptions & node_options) bool use_time_keeper = declare_parameter("publish_processing_time_detail"); if (use_time_keeper) { detailed_processing_time_publisher_ = - this->create_publisher( + this->create_publisher( "~/debug/processing_time_detail_ms", 1); - auto time_keeper = autoware::universe_utils::TimeKeeper(detailed_processing_time_publisher_); - time_keeper_ = std::make_shared(time_keeper); + auto time_keeper = autoware_utils::TimeKeeper(detailed_processing_time_publisher_); + time_keeper_ = std::make_shared(time_keeper); } } } diff --git a/perception/autoware_probabilistic_occupancy_grid_map/src/fusion/synchronized_grid_map_fusion_node.hpp b/perception/autoware_probabilistic_occupancy_grid_map/src/fusion/synchronized_grid_map_fusion_node.hpp index 0d894d99a68a1..6bc023b658289 100644 --- a/perception/autoware_probabilistic_occupancy_grid_map/src/fusion/synchronized_grid_map_fusion_node.hpp +++ b/perception/autoware_probabilistic_occupancy_grid_map/src/fusion/synchronized_grid_map_fusion_node.hpp @@ -20,9 +20,9 @@ #include "autoware/probabilistic_occupancy_grid_map/updater/log_odds_bayes_filter_updater.hpp" #include "autoware/probabilistic_occupancy_grid_map/updater/ogm_updater_interface.hpp" -#include -#include -#include +#include +#include +#include #include #include @@ -82,8 +82,8 @@ class GridMapFusionNode : public rclcpp::Node rclcpp::Publisher::SharedPtr fused_map_pub_; rclcpp::Publisher::SharedPtr single_frame_pub_; std::vector::SharedPtr> grid_map_subs_; - std::unique_ptr> stop_watch_ptr_{}; - std::unique_ptr debug_publisher_ptr_{}; + std::unique_ptr> stop_watch_ptr_{}; + std::unique_ptr debug_publisher_ptr_{}; // Topics manager std::size_t num_input_topics_{1}; @@ -122,9 +122,9 @@ class GridMapFusionNode : public rclcpp::Node fusion_policy::FusionMethod fusion_method_; // time keeper - rclcpp::Publisher::SharedPtr + rclcpp::Publisher::SharedPtr detailed_processing_time_publisher_; - std::shared_ptr time_keeper_; + std::shared_ptr time_keeper_; }; } // namespace autoware::occupancy_grid_map diff --git a/perception/autoware_probabilistic_occupancy_grid_map/src/laserscan_based_occupancy_grid_map/laserscan_based_occupancy_grid_map_node.cpp b/perception/autoware_probabilistic_occupancy_grid_map/src/laserscan_based_occupancy_grid_map/laserscan_based_occupancy_grid_map_node.cpp index 4a8c9626de013..8581ae3fa9e5a 100644 --- a/perception/autoware_probabilistic_occupancy_grid_map/src/laserscan_based_occupancy_grid_map/laserscan_based_occupancy_grid_map_node.cpp +++ b/perception/autoware_probabilistic_occupancy_grid_map/src/laserscan_based_occupancy_grid_map/laserscan_based_occupancy_grid_map_node.cpp @@ -37,7 +37,7 @@ namespace autoware::occupancy_grid_map { -using autoware::universe_utils::ScopedTimeTrack; +using autoware_utils::ScopedTimeTrack; using costmap_2d::OccupancyGridMap; using costmap_2d::OccupancyGridMapBBFUpdater; using geometry_msgs::msg::Pose; @@ -110,10 +110,10 @@ LaserscanBasedOccupancyGridMapNode::LaserscanBasedOccupancyGridMapNode( bool use_time_keeper = declare_parameter("publish_processing_time_detail"); if (use_time_keeper) { detailed_processing_time_publisher_ = - this->create_publisher( + this->create_publisher( "~/debug/processing_time_detail_ms", 1); - auto time_keeper = autoware::universe_utils::TimeKeeper(detailed_processing_time_publisher_); - time_keeper_ = std::make_shared(time_keeper); + auto time_keeper = autoware_utils::TimeKeeper(detailed_processing_time_publisher_); + time_keeper_ = std::make_shared(time_keeper); } } diff --git a/perception/autoware_probabilistic_occupancy_grid_map/src/laserscan_based_occupancy_grid_map/laserscan_based_occupancy_grid_map_node.hpp b/perception/autoware_probabilistic_occupancy_grid_map/src/laserscan_based_occupancy_grid_map/laserscan_based_occupancy_grid_map_node.hpp index ebb5b66625939..c7bb22b3549c2 100644 --- a/perception/autoware_probabilistic_occupancy_grid_map/src/laserscan_based_occupancy_grid_map/laserscan_based_occupancy_grid_map_node.hpp +++ b/perception/autoware_probabilistic_occupancy_grid_map/src/laserscan_based_occupancy_grid_map/laserscan_based_occupancy_grid_map_node.hpp @@ -18,7 +18,7 @@ #include "autoware/probabilistic_occupancy_grid_map/costmap_2d/occupancy_grid_map.hpp" #include "autoware/probabilistic_occupancy_grid_map/updater/binary_bayes_filter_updater.hpp" #include "autoware/probabilistic_occupancy_grid_map/updater/ogm_updater_interface.hpp" -#include "autoware/universe_utils/system/time_keeper.hpp" +#include "autoware_utils/system/time_keeper.hpp" #include #include @@ -101,9 +101,9 @@ class LaserscanBasedOccupancyGridMapNode : public rclcpp::Node bool enable_single_frame_mode_; // time keeper - rclcpp::Publisher::SharedPtr + rclcpp::Publisher::SharedPtr detailed_processing_time_publisher_; - std::shared_ptr time_keeper_; + std::shared_ptr time_keeper_; }; } // namespace autoware::occupancy_grid_map diff --git a/perception/autoware_probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.cpp b/perception/autoware_probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.cpp index 1d45bfb642b37..133810517149a 100644 --- a/perception/autoware_probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.cpp +++ b/perception/autoware_probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.cpp @@ -19,8 +19,8 @@ #include "autoware/probabilistic_occupancy_grid_map/costmap_2d/occupancy_grid_map_projective.hpp" #include "autoware/probabilistic_occupancy_grid_map/utils/utils.hpp" -#include -#include +#include +#include #include #include @@ -43,7 +43,7 @@ namespace autoware::occupancy_grid_map { -using autoware::universe_utils::ScopedTimeTrack; +using autoware_utils::ScopedTimeTrack; using costmap_2d::OccupancyGridMapBBFUpdater; using costmap_2d::OccupancyGridMapFixedBlindSpot; using costmap_2d::OccupancyGridMapProjectiveBlindSpot; @@ -127,8 +127,8 @@ PointcloudBasedOccupancyGridMapNode::PointcloudBasedOccupancyGridMapNode( // initialize debug tool { - using autoware::universe_utils::DebugPublisher; - using autoware::universe_utils::StopWatch; + using autoware_utils::DebugPublisher; + using autoware_utils::StopWatch; stop_watch_ptr_ = std::make_unique>(); debug_publisher_ptr_ = std::make_unique(this, "pointcloud_based_occupancy_grid_map"); @@ -139,10 +139,10 @@ PointcloudBasedOccupancyGridMapNode::PointcloudBasedOccupancyGridMapNode( bool use_time_keeper = declare_parameter("publish_processing_time_detail"); if (use_time_keeper) { detailed_processing_time_publisher_ = - this->create_publisher( + this->create_publisher( "~/debug/processing_time_detail_ms", 1); - auto time_keeper = autoware::universe_utils::TimeKeeper(detailed_processing_time_publisher_); - time_keeper_ = std::make_shared(time_keeper); + auto time_keeper = autoware_utils::TimeKeeper(detailed_processing_time_publisher_); + time_keeper_ = std::make_shared(time_keeper); } } } diff --git a/perception/autoware_probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.hpp b/perception/autoware_probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.hpp index eb85e8a468ee3..6be9c033feb75 100644 --- a/perception/autoware_probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.hpp +++ b/perception/autoware_probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.hpp @@ -20,9 +20,9 @@ #include "autoware/probabilistic_occupancy_grid_map/updater/ogm_updater_interface.hpp" #include "autoware/probabilistic_occupancy_grid_map/utils/cuda_pointcloud.hpp" -#include -#include -#include +#include +#include +#include #include #include #include @@ -68,8 +68,8 @@ class PointcloudBasedOccupancyGridMapNode : public rclcpp::Node rclcpp::Publisher::SharedPtr occupancy_grid_map_pub_; rclcpp::Subscription::SharedPtr obstacle_pointcloud_sub_ptr_; rclcpp::Subscription::SharedPtr raw_pointcloud_sub_ptr_; - std::unique_ptr> stop_watch_ptr_{}; - std::unique_ptr debug_publisher_ptr_{}; + std::unique_ptr> stop_watch_ptr_{}; + std::unique_ptr debug_publisher_ptr_{}; std::shared_ptr tf2_{std::make_shared(get_clock())}; std::shared_ptr tf2_listener_{std::make_shared(*tf2_)}; @@ -93,9 +93,9 @@ class PointcloudBasedOccupancyGridMapNode : public rclcpp::Node bool filter_obstacle_pointcloud_by_raw_pointcloud_; // time keeper - rclcpp::Publisher::SharedPtr + rclcpp::Publisher::SharedPtr detailed_processing_time_publisher_; - std::shared_ptr time_keeper_; + std::shared_ptr time_keeper_; }; } // namespace autoware::occupancy_grid_map diff --git a/perception/autoware_radar_crossing_objects_noise_filter/package.xml b/perception/autoware_radar_crossing_objects_noise_filter/package.xml index fc3aafc1c5227..cc9f0508a7bd4 100644 --- a/perception/autoware_radar_crossing_objects_noise_filter/package.xml +++ b/perception/autoware_radar_crossing_objects_noise_filter/package.xml @@ -14,7 +14,7 @@ ament_cmake_auto autoware_perception_msgs - autoware_universe_utils + autoware_utils rclcpp rclcpp_components tf2 diff --git a/perception/autoware_radar_crossing_objects_noise_filter/src/radar_crossing_objects_noise_filter_node.cpp b/perception/autoware_radar_crossing_objects_noise_filter/src/radar_crossing_objects_noise_filter_node.cpp index 13231b83bf27b..ca96deff46bd5 100644 --- a/perception/autoware_radar_crossing_objects_noise_filter/src/radar_crossing_objects_noise_filter_node.cpp +++ b/perception/autoware_radar_crossing_objects_noise_filter/src/radar_crossing_objects_noise_filter_node.cpp @@ -14,8 +14,8 @@ #include "radar_crossing_objects_noise_filter_node.hpp" -#include "autoware/universe_utils/geometry/geometry.hpp" -#include "autoware/universe_utils/math/normalization.hpp" +#include "autoware_utils/geometry/geometry.hpp" +#include "autoware_utils/math/normalization.hpp" #include @@ -124,14 +124,14 @@ rcl_interfaces::msg::SetParametersResult RadarCrossingObjectsNoiseFilterNode::on bool RadarCrossingObjectsNoiseFilterNode::isNoise(const DetectedObject & object) { - const double velocity = std::abs( - autoware::universe_utils::calcNorm(object.kinematics.twist_with_covariance.twist.linear)); + const double velocity = + std::abs(autoware_utils::calc_norm(object.kinematics.twist_with_covariance.twist.linear)); const double object_angle = tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation); const double object_position_angle = std::atan2( object.kinematics.pose_with_covariance.pose.position.y, object.kinematics.pose_with_covariance.pose.position.x); const double crossing_yaw = - autoware::universe_utils::normalizeRadian(object_angle - object_position_angle); + autoware_utils::normalize_radian(object_angle - object_position_angle); if ( velocity > node_param_.velocity_threshold && diff --git a/perception/autoware_radar_crossing_objects_noise_filter/test/test_radar_crossing_objects_noise_filter/test_radar_crossing_objects_filter_is_noise.cpp b/perception/autoware_radar_crossing_objects_noise_filter/test/test_radar_crossing_objects_noise_filter/test_radar_crossing_objects_filter_is_noise.cpp index 9a0477befd5be..0f24307786c45 100644 --- a/perception/autoware_radar_crossing_objects_noise_filter/test/test_radar_crossing_objects_noise_filter/test_radar_crossing_objects_filter_is_noise.cpp +++ b/perception/autoware_radar_crossing_objects_noise_filter/test/test_radar_crossing_objects_noise_filter/test_radar_crossing_objects_filter_is_noise.cpp @@ -13,8 +13,8 @@ // limitations under the License. #include "../../src/radar_crossing_objects_noise_filter_node.hpp" -#include "autoware/universe_utils/geometry/geometry.hpp" -#include "autoware/universe_utils/math/unit_conversion.hpp" +#include "autoware_utils/geometry/geometry.hpp" +#include "autoware_utils/math/unit_conversion.hpp" #include @@ -50,9 +50,9 @@ TEST(RadarCrossingObjectsFilter, IsNoise) { rclcpp::init(0, nullptr); { - auto velocity = autoware::universe_utils::createVector3(40.0, 30.0, 0.0); - auto position = autoware::universe_utils::createPoint(1.0, 0.0, 0.0); - auto orientation = autoware::universe_utils::createQuaternion(1.0, 1.0, 1.0, 0.0); + auto velocity = autoware_utils::create_vector3(40.0, 30.0, 0.0); + auto position = autoware_utils::create_point(1.0, 0.0, 0.0); + auto orientation = autoware_utils::create_quaternion(1.0, 1.0, 1.0, 0.0); auto object = get_object(velocity, position, orientation); { double velocity_threshold = 40.0; @@ -83,9 +83,9 @@ TEST(RadarCrossingObjectsFilter, IsNoise) } { - auto velocity = autoware::universe_utils::createVector3(40.0, 30.0, 0.0); - auto position = autoware::universe_utils::createPoint(1.0, 2.0, 0.0); - auto orientation = autoware::universe_utils::createQuaternion(1.0, 1.0, 1.0, 0.0); + auto velocity = autoware_utils::create_vector3(40.0, 30.0, 0.0); + auto position = autoware_utils::create_point(1.0, 2.0, 0.0); + auto orientation = autoware_utils::create_quaternion(1.0, 1.0, 1.0, 0.0); auto object = get_object(velocity, position, orientation); { double velocity_threshold = 40.0; @@ -114,9 +114,9 @@ TEST(RadarCrossingObjectsFilter, IsNoise) } { - auto velocity = autoware::universe_utils::createVector3(24.0, 18.0, 0.0); - auto position = autoware::universe_utils::createPoint(1.0, 0.0, 0.0); - auto orientation = autoware::universe_utils::createQuaternion(1.0, 1.0, 1.0, 0.0); + auto velocity = autoware_utils::create_vector3(24.0, 18.0, 0.0); + auto position = autoware_utils::create_point(1.0, 0.0, 0.0); + auto orientation = autoware_utils::create_quaternion(1.0, 1.0, 1.0, 0.0); auto object = get_object(velocity, position, orientation); { double velocity_threshold = 40.0; @@ -145,9 +145,9 @@ TEST(RadarCrossingObjectsFilter, IsNoise) } { - auto velocity = autoware::universe_utils::createVector3(24.0, 18.0, 0.0); - auto position = autoware::universe_utils::createPoint(1.0, 2.0, 0.0); - auto orientation = autoware::universe_utils::createQuaternion(1.0, 1.0, 1.0, 0.0); + auto velocity = autoware_utils::create_vector3(24.0, 18.0, 0.0); + auto position = autoware_utils::create_point(1.0, 2.0, 0.0); + auto orientation = autoware_utils::create_quaternion(1.0, 1.0, 1.0, 0.0); auto object = get_object(velocity, position, orientation); { double velocity_threshold = 40.0; diff --git a/perception/autoware_radar_fusion_to_detected_object/package.xml b/perception/autoware_radar_fusion_to_detected_object/package.xml index d7b022911088c..892a899366d3f 100644 --- a/perception/autoware_radar_fusion_to_detected_object/package.xml +++ b/perception/autoware_radar_fusion_to_detected_object/package.xml @@ -15,7 +15,7 @@ autoware_cmake autoware_perception_msgs - autoware_universe_utils + autoware_utils eigen geometry_msgs message_filters diff --git a/perception/autoware_radar_fusion_to_detected_object/src/include/radar_fusion_to_detected_object.hpp b/perception/autoware_radar_fusion_to_detected_object/src/include/radar_fusion_to_detected_object.hpp index 3c95d8f068af1..c4910799a018e 100644 --- a/perception/autoware_radar_fusion_to_detected_object/src/include/radar_fusion_to_detected_object.hpp +++ b/perception/autoware_radar_fusion_to_detected_object/src/include/radar_fusion_to_detected_object.hpp @@ -17,7 +17,7 @@ #define EIGEN_MPL2_ONLY -#include "autoware/universe_utils/geometry/boost_geometry.hpp" +#include "autoware_utils/geometry/boost_geometry.hpp" #include "rclcpp/logger.hpp" #include @@ -33,10 +33,10 @@ namespace autoware::radar_fusion_to_detected_object { -using autoware::universe_utils::LinearRing2d; -using autoware::universe_utils::Point2d; using autoware_perception_msgs::msg::DetectedObject; using autoware_perception_msgs::msg::DetectedObjects; +using autoware_utils::LinearRing2d; +using autoware_utils::Point2d; using geometry_msgs::msg::Point; using geometry_msgs::msg::PoseWithCovariance; using geometry_msgs::msg::Twist; diff --git a/perception/autoware_radar_fusion_to_detected_object/src/radar_fusion_to_detected_object.cpp b/perception/autoware_radar_fusion_to_detected_object/src/radar_fusion_to_detected_object.cpp index 6d19f299973bd..eb7fd6587ab38 100644 --- a/perception/autoware_radar_fusion_to_detected_object/src/radar_fusion_to_detected_object.cpp +++ b/perception/autoware_radar_fusion_to_detected_object/src/radar_fusion_to_detected_object.cpp @@ -15,8 +15,8 @@ #include "include/radar_fusion_to_detected_object.hpp" -#include "autoware/universe_utils/geometry/geometry.hpp" -#include "autoware/universe_utils/math/normalization.hpp" +#include "autoware_utils/geometry/geometry.hpp" +#include "autoware_utils/math/normalization.hpp" #include @@ -29,10 +29,10 @@ namespace autoware::radar_fusion_to_detected_object { -using autoware::universe_utils::LinearRing2d; -using autoware::universe_utils::Point2d; using autoware_perception_msgs::msg::DetectedObject; using autoware_perception_msgs::msg::DetectedObjects; +using autoware_utils::LinearRing2d; +using autoware_utils::Point2d; using geometry_msgs::msg::Point; using geometry_msgs::msg::PoseWithCovariance; using geometry_msgs::msg::Twist; @@ -138,7 +138,7 @@ RadarFusionToDetectedObject::Output RadarFusionToDetectedObject::update( bool RadarFusionToDetectedObject::hasTwistCovariance( const TwistWithCovariance & twist_with_covariance) { - using IDX = autoware::universe_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; + using IDX = autoware_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; auto covariance = twist_with_covariance.covariance; if (covariance[IDX::X_X] == 0.0 && covariance[IDX::Y_Y] == 0.0 && covariance[IDX::Z_Z] == 0.0) { return false; @@ -153,11 +153,11 @@ bool RadarFusionToDetectedObject::isYawCorrect( const DetectedObject & object, const TwistWithCovariance & twist_with_covariance, const double & yaw_threshold) { - const double twist_yaw = autoware::universe_utils::normalizeRadian( + const double twist_yaw = autoware_utils::normalize_radian( std::atan2(twist_with_covariance.twist.linear.y, twist_with_covariance.twist.linear.x)); - const double object_yaw = autoware::universe_utils::normalizeRadian( + const double object_yaw = autoware_utils::normalize_radian( tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation)); - const double diff_yaw = autoware::universe_utils::normalizeRadian(twist_yaw - object_yaw); + const double diff_yaw = autoware_utils::normalize_radian(twist_yaw - object_yaw); if (std::abs(diff_yaw) < yaw_threshold || M_PI - yaw_threshold < std::abs(diff_yaw)) { return true; } else { @@ -174,12 +174,10 @@ RadarFusionToDetectedObject::filterRadarWithinObject( { std::vector outputs{}; - autoware::universe_utils::Point2d object_size{ - object.shape.dimensions.x, object.shape.dimensions.y}; + autoware_utils::Point2d object_size{object.shape.dimensions.x, object.shape.dimensions.y}; LinearRing2d object_box = createObject2dWithMargin(object_size, param_.bounding_box_margin); - object_box = autoware::universe_utils::transformVector( - object_box, - autoware::universe_utils::pose2transform(object.kinematics.pose_with_covariance.pose)); + object_box = autoware_utils::transform_vector( + object_box, autoware_utils::pose2transform(object.kinematics.pose_with_covariance.pose)); for (const auto & radar : (*radars)) { Point2d radar_point{ @@ -214,10 +212,10 @@ TwistWithCovariance RadarFusionToDetectedObject::estimateTwist( Eigen::Vector2d vec_min_distance(0.0, 0.0); if (param_.velocity_weight_min_distance > 0.0) { auto comp_func = [&](const RadarInput & a, const RadarInput & b) { - return autoware::universe_utils::calcSquaredDistance2d( + return autoware_utils::calc_squared_distance2d( a.pose_with_covariance.pose.position, object.kinematics.pose_with_covariance.pose.position) < - autoware::universe_utils::calcSquaredDistance2d( + autoware_utils::calc_squared_distance2d( b.pose_with_covariance.pose.position, object.kinematics.pose_with_covariance.pose.position); }; diff --git a/perception/autoware_radar_object_clustering/package.xml b/perception/autoware_radar_object_clustering/package.xml index 0e26ec3b32132..4a0b8c3cae02e 100644 --- a/perception/autoware_radar_object_clustering/package.xml +++ b/perception/autoware_radar_object_clustering/package.xml @@ -15,7 +15,7 @@ autoware_object_recognition_utils autoware_perception_msgs - autoware_universe_utils + autoware_utils geometry_msgs rclcpp rclcpp_components diff --git a/perception/autoware_radar_object_clustering/src/radar_object_clustering_node.cpp b/perception/autoware_radar_object_clustering/src/radar_object_clustering_node.cpp index cf686e3ae1055..e180909068ac3 100644 --- a/perception/autoware_radar_object_clustering/src/radar_object_clustering_node.cpp +++ b/perception/autoware_radar_object_clustering/src/radar_object_clustering_node.cpp @@ -15,8 +15,8 @@ #include "radar_object_clustering_node.hpp" #include "autoware/object_recognition_utils/object_recognition_utils.hpp" -#include "autoware/universe_utils/geometry/geometry.hpp" -#include "autoware/universe_utils/math/unit_conversion.hpp" +#include "autoware_utils/geometry/geometry.hpp" +#include "autoware_utils/math/unit_conversion.hpp" #include @@ -180,13 +180,13 @@ void RadarObjectClusteringNode::onObjects(const DetectedObjects::ConstSharedPtr bool RadarObjectClusteringNode::isSameObject( const DetectedObject & object_1, const DetectedObject & object_2) { - const double angle_diff = std::abs(autoware::universe_utils::normalizeRadian( + const double angle_diff = std::abs(autoware_utils::normalize_radian( tf2::getYaw(object_1.kinematics.pose_with_covariance.pose.orientation) - tf2::getYaw(object_2.kinematics.pose_with_covariance.pose.orientation))); const double velocity_diff = std::abs( object_1.kinematics.twist_with_covariance.twist.linear.x - object_2.kinematics.twist_with_covariance.twist.linear.x); - const double distance = autoware::universe_utils::calcDistance2d( + const double distance = autoware_utils::calc_distance2d( object_1.kinematics.pose_with_covariance.pose.position, object_2.kinematics.pose_with_covariance.pose.position); diff --git a/perception/autoware_radar_object_tracker/include/autoware_radar_object_tracker/utils/radar_object_tracker_utils.hpp b/perception/autoware_radar_object_tracker/include/autoware_radar_object_tracker/utils/radar_object_tracker_utils.hpp index f4fb977d65de2..3edae2497df5c 100644 --- a/perception/autoware_radar_object_tracker/include/autoware_radar_object_tracker/utils/radar_object_tracker_utils.hpp +++ b/perception/autoware_radar_object_tracker/include/autoware_radar_object_tracker/utils/radar_object_tracker_utils.hpp @@ -15,8 +15,8 @@ #ifndef AUTOWARE_RADAR_OBJECT_TRACKER__UTILS__RADAR_OBJECT_TRACKER_UTILS_HPP_ #define AUTOWARE_RADAR_OBJECT_TRACKER__UTILS__RADAR_OBJECT_TRACKER_UTILS_HPP_ -#include "autoware/universe_utils/geometry/geometry.hpp" -#include "autoware/universe_utils/math/unit_conversion.hpp" +#include "autoware_utils/geometry/geometry.hpp" +#include "autoware_utils/math/unit_conversion.hpp" #include #include diff --git a/perception/autoware_radar_object_tracker/package.xml b/perception/autoware_radar_object_tracker/package.xml index 6fee7b009cb91..ff80c23b366b2 100644 --- a/perception/autoware_radar_object_tracker/package.xml +++ b/perception/autoware_radar_object_tracker/package.xml @@ -17,7 +17,7 @@ autoware_lanelet2_extension autoware_object_recognition_utils autoware_perception_msgs - autoware_universe_utils + autoware_utils eigen glog mussp diff --git a/perception/autoware_radar_object_tracker/src/association/data_association.cpp b/perception/autoware_radar_object_tracker/src/association/data_association.cpp index 79c91a96c4cc8..b880100ee71a8 100644 --- a/perception/autoware_radar_object_tracker/src/association/data_association.cpp +++ b/perception/autoware_radar_object_tracker/src/association/data_association.cpp @@ -53,9 +53,8 @@ double getFormedYawAngle( const geometry_msgs::msg::Quaternion & measurement_quat, const geometry_msgs::msg::Quaternion & tracker_quat, const bool distinguish_front_or_back = true) { - const double measurement_yaw = - autoware::universe_utils::normalizeRadian(tf2::getYaw(measurement_quat)); - const double tracker_yaw = autoware::universe_utils::normalizeRadian(tf2::getYaw(tracker_quat)); + const double measurement_yaw = autoware_utils::normalize_radian(tf2::getYaw(measurement_quat)); + const double tracker_yaw = autoware_utils::normalize_radian(tf2::getYaw(tracker_quat)); const double angle_range = distinguish_front_or_back ? M_PI : M_PI_2; const double angle_step = distinguish_front_or_back ? 2.0 * M_PI : M_PI; // Fixed measurement_yaw to be in the range of +-90 or 180 degrees of X_t(IDX::YAW) @@ -204,7 +203,7 @@ Eigen::MatrixXd DataAssociation::calcScoreMatrix( double score = 0.0; if (can_assign_matrix_(tracker_label, measurement_label)) { const double max_dist = max_dist_matrix_(tracker_label, measurement_label); - const double dist = autoware::universe_utils::calcDistance2d( + const double dist = autoware_utils::calc_distance2d( measurement_object.kinematics.pose_with_covariance.pose.position, tracked_object.kinematics.pose_with_covariance.pose.position); @@ -222,7 +221,7 @@ Eigen::MatrixXd DataAssociation::calcScoreMatrix( if (passed_gate) { const double max_area = max_area_matrix_(tracker_label, measurement_label); const double min_area = min_area_matrix_(tracker_label, measurement_label); - const double area = autoware::universe_utils::getArea(measurement_object.shape); + const double area = autoware_utils::get_area(measurement_object.shape); if (area < min_area || max_area < area) { passed_gate = false; } diff --git a/perception/autoware_radar_object_tracker/src/tracker/model/constant_turn_rate_motion_tracker.cpp b/perception/autoware_radar_object_tracker/src/tracker/model/constant_turn_rate_motion_tracker.cpp index 04b2bd24d4b8d..39e1653990954 100644 --- a/perception/autoware_radar_object_tracker/src/tracker/model/constant_turn_rate_motion_tracker.cpp +++ b/perception/autoware_radar_object_tracker/src/tracker/model/constant_turn_rate_motion_tracker.cpp @@ -20,10 +20,10 @@ #include "autoware_radar_object_tracker/tracker/model/constant_turn_rate_motion_tracker.hpp" -#include "autoware/universe_utils/geometry/boost_polygon_utils.hpp" -#include "autoware/universe_utils/math/unit_conversion.hpp" -#include "autoware/universe_utils/ros/msg_covariance.hpp" #include "autoware_radar_object_tracker/utils/utils.hpp" +#include "autoware_utils/geometry/boost_polygon_utils.hpp" +#include "autoware_utils/math/unit_conversion.hpp" +#include "autoware_utils/ros/msg_covariance.hpp" #include #include @@ -49,7 +49,7 @@ namespace autoware::radar_object_tracker { using Label = autoware_perception_msgs::msg::ObjectClassification; -using autoware::universe_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; +using autoware_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; // init static member variables bool ConstantTurnRateMotionTracker::is_initialized_ = false; @@ -216,7 +216,7 @@ void ConstantTurnRateMotionTracker::loadDefaultModelParameters(const std::string // limitation // (TODO): this may be written in another yaml file based on classify result const float max_speed_kmph = config["default"]["limit"]["max_speed"].as(); // [km/h] - max_vx_ = autoware::universe_utils::kmph2mps(max_speed_kmph); // [m/s] + max_vx_ = autoware_utils::kmph2mps(max_speed_kmph); // [m/s] } bool ConstantTurnRateMotionTracker::predict(const rclcpp::Time & time) @@ -397,7 +397,7 @@ bool ConstantTurnRateMotionTracker::measureWithPose( Eigen::MatrixXd Y_yaw = Eigen::MatrixXd::Zero(1, 1); const auto yaw = [&] { - auto obj_yaw = autoware::universe_utils::normalizeRadian( + auto obj_yaw = autoware_utils::normalize_radian( tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation)); while (M_PI_2 <= yaw_state - obj_yaw) { obj_yaw = obj_yaw + M_PI; @@ -623,7 +623,7 @@ bool ConstantTurnRateMotionTracker::getTrackedObject( const auto origin_yaw = tf2::getYaw(object_.kinematics.pose_with_covariance.pose.orientation); const auto ekf_pose_yaw = tf2::getYaw(pose_with_cov.pose.orientation); object.shape.footprint = - autoware::universe_utils::rotatePolygon(object.shape.footprint, origin_yaw - ekf_pose_yaw); + autoware_utils::rotate_polygon(object.shape.footprint, origin_yaw - ekf_pose_yaw); } return true; diff --git a/perception/autoware_radar_object_tracker/src/tracker/model/linear_motion_tracker.cpp b/perception/autoware_radar_object_tracker/src/tracker/model/linear_motion_tracker.cpp index 80b965ddb3840..16ea415808952 100644 --- a/perception/autoware_radar_object_tracker/src/tracker/model/linear_motion_tracker.cpp +++ b/perception/autoware_radar_object_tracker/src/tracker/model/linear_motion_tracker.cpp @@ -20,10 +20,10 @@ #include "autoware_radar_object_tracker/tracker/model/linear_motion_tracker.hpp" -#include "autoware/universe_utils/geometry/boost_polygon_utils.hpp" -#include "autoware/universe_utils/math/unit_conversion.hpp" -#include "autoware/universe_utils/ros/msg_covariance.hpp" #include "autoware_radar_object_tracker/utils/utils.hpp" +#include "autoware_utils/geometry/boost_polygon_utils.hpp" +#include "autoware_utils/math/unit_conversion.hpp" +#include "autoware_utils/ros/msg_covariance.hpp" #include #include @@ -49,7 +49,7 @@ namespace autoware::radar_object_tracker { using Label = autoware_perception_msgs::msg::ObjectClassification; -using autoware::universe_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; +using autoware_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; // initialize static parameter bool LinearMotionTracker::is_initialized_ = false; @@ -230,8 +230,8 @@ void LinearMotionTracker::loadDefaultModelParameters(const std::string & path) // limitation // (TODO): this may be written in another yaml file based on classify result const float max_speed_kmph = config["default"]["limit"]["max_speed"].as(); // [km/h] - max_vx_ = autoware::universe_utils::kmph2mps(max_speed_kmph); // [m/s] - max_vy_ = autoware::universe_utils::kmph2mps(max_speed_kmph); // [rad/s] + max_vx_ = autoware_utils::kmph2mps(max_speed_kmph); // [m/s] + max_vy_ = autoware_utils::kmph2mps(max_speed_kmph); // [rad/s] } bool LinearMotionTracker::predict(const rclcpp::Time & time) @@ -669,7 +669,7 @@ bool LinearMotionTracker::getTrackedObject( const auto origin_yaw = tf2::getYaw(object_.kinematics.pose_with_covariance.pose.orientation); const auto ekf_pose_yaw = tf2::getYaw(pose_with_cov.pose.orientation); object.shape.footprint = - autoware::universe_utils::rotatePolygon(object.shape.footprint, origin_yaw - ekf_pose_yaw); + autoware_utils::rotate_polygon(object.shape.footprint, origin_yaw - ekf_pose_yaw); } return true; diff --git a/perception/autoware_radar_object_tracker/src/utils/radar_object_tracker_utils.cpp b/perception/autoware_radar_object_tracker/src/utils/radar_object_tracker_utils.cpp index cfbffdc089c11..e486bb944705a 100644 --- a/perception/autoware_radar_object_tracker/src/utils/radar_object_tracker_utils.cpp +++ b/perception/autoware_radar_object_tracker/src/utils/radar_object_tracker_utils.cpp @@ -85,10 +85,10 @@ bool checkCloseLaneletCondition( double object_motion_yaw = object_yaw; bool velocity_is_reverted = object.kinematics.twist_with_covariance.twist.linear.x < 0.0; if (velocity_is_reverted) { - object_motion_yaw = autoware::universe_utils::normalizeRadian(object_yaw + M_PI); + object_motion_yaw = autoware_utils::normalize_radian(object_yaw + M_PI); } const double delta_yaw = object_motion_yaw - lane_yaw; - const double normalized_delta_yaw = autoware::universe_utils::normalizeRadian(delta_yaw); + const double normalized_delta_yaw = autoware_utils::normalize_radian(delta_yaw); const double abs_norm_delta_yaw = std::fabs(normalized_delta_yaw); if (abs_norm_delta_yaw > max_angle_diff_from_lane) { @@ -138,14 +138,14 @@ bool hasValidVelocityDirectionToLanelet( const double object_vel_y = object.kinematics.twist_with_covariance.twist.linear.y; const double object_vel_yaw = std::atan2(object_vel_y, object_vel_x); const double object_vel_yaw_global = - autoware::universe_utils::normalizeRadian(object_yaw + object_vel_yaw); + autoware_utils::normalize_radian(object_yaw + object_vel_yaw); const double object_vel = std::hypot(object_vel_x, object_vel_y); for (const auto & lanelet : lanelets) { const double lane_yaw = lanelet::utils::getLaneletAngle( lanelet, object.kinematics.pose_with_covariance.pose.position); const double delta_yaw = object_vel_yaw_global - lane_yaw; - const double normalized_delta_yaw = autoware::universe_utils::normalizeRadian(delta_yaw); + const double normalized_delta_yaw = autoware_utils::normalize_radian(delta_yaw); const double lane_vel = object_vel * std::sin(normalized_delta_yaw); if (std::fabs(lane_vel) < max_lateral_velocity) { diff --git a/perception/autoware_radar_tracks_msgs_converter/package.xml b/perception/autoware_radar_tracks_msgs_converter/package.xml index 4401d9cea9e58..b414a8d4b25c5 100644 --- a/perception/autoware_radar_tracks_msgs_converter/package.xml +++ b/perception/autoware_radar_tracks_msgs_converter/package.xml @@ -15,7 +15,7 @@ autoware_cmake autoware_perception_msgs - autoware_universe_utils + autoware_utils nav_msgs radar_msgs rclcpp diff --git a/perception/autoware_radar_tracks_msgs_converter/src/radar_tracks_msgs_converter_node.cpp b/perception/autoware_radar_tracks_msgs_converter/src/radar_tracks_msgs_converter_node.cpp index 0e06541f90af1..cb022dfdea671 100644 --- a/perception/autoware_radar_tracks_msgs_converter/src/radar_tracks_msgs_converter_node.cpp +++ b/perception/autoware_radar_tracks_msgs_converter/src/radar_tracks_msgs_converter_node.cpp @@ -14,9 +14,9 @@ #include "radar_tracks_msgs_converter_node.hpp" -#include "autoware/universe_utils/geometry/geometry.hpp" -#include "autoware/universe_utils/math/unit_conversion.hpp" -#include "autoware/universe_utils/ros/msg_covariance.hpp" +#include "autoware_utils/geometry/geometry.hpp" +#include "autoware_utils/math/unit_conversion.hpp" +#include "autoware_utils/ros/msg_covariance.hpp" #include @@ -92,7 +92,7 @@ RadarTracksMsgsConverterNode::RadarTracksMsgsConverterNode(const rclcpp::NodeOpt sub_odometry_ = create_subscription( "~/input/odometry", rclcpp::QoS{1}, std::bind(&RadarTracksMsgsConverterNode::onTwist, this, _1)); - transform_listener_ = std::make_shared(this); + transform_listener_ = std::make_shared(this); // Publisher pub_tracked_objects_ = create_publisher("~/output/radar_tracked_objects", 1); @@ -158,7 +158,7 @@ void RadarTracksMsgsConverterNode::onTimer() return; } const auto & header = radar_data_->header; - transform_ = transform_listener_->getTransform( + transform_ = transform_listener_->get_transform( node_param_.new_frame_id, header.frame_id, header.stamp, rclcpp::Duration::from_seconds(0.01)); TrackedObjects tracked_objects = convertRadarTrackToTrackedObjects(); @@ -261,8 +261,8 @@ TrackedObjects RadarTracksMsgsConverterNode::convertRadarTrackToTrackedObjects() } // yaw angle (vehicle heading) is obtained from ground velocity - double yaw = autoware::universe_utils::normalizeRadian( - std::atan2(compensated_velocity.y, compensated_velocity.x)); + double yaw = + autoware_utils::normalize_radian(std::atan2(compensated_velocity.y, compensated_velocity.x)); // kinematics setting TrackedObjectKinematics kinematics; @@ -273,7 +273,7 @@ TrackedObjects RadarTracksMsgsConverterNode::convertRadarTrackToTrackedObjects() // velocity of object is defined in the object coordinate // heading is obtained from ground velocity kinematics.pose_with_covariance.pose.orientation = - autoware::universe_utils::createQuaternionFromYaw(yaw); + autoware_utils::create_quaternion_from_yaw(yaw); // longitudinal velocity is the length of the velocity vector // lateral velocity is zero, use default value kinematics.twist_with_covariance.twist.linear.x = std::sqrt( @@ -334,8 +334,8 @@ geometry_msgs::msg::Vector3 RadarTracksMsgsConverterNode::compensateVelocityEgoM std::array RadarTracksMsgsConverterNode::convertPoseCovarianceMatrix( const radar_msgs::msg::RadarTrack & radar_track) { - using POSE_IDX = autoware::universe_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; - using RADAR_IDX = autoware::universe_utils::xyz_upper_covariance_index::XYZ_UPPER_COV_IDX; + using POSE_IDX = autoware_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; + using RADAR_IDX = autoware_utils::xyz_upper_covariance_index::XYZ_UPPER_COV_IDX; std::array pose_covariance{}; pose_covariance[POSE_IDX::X_X] = radar_track.position_covariance[RADAR_IDX::X_X]; pose_covariance[POSE_IDX::X_Y] = radar_track.position_covariance[RADAR_IDX::X_Y]; @@ -351,8 +351,8 @@ std::array RadarTracksMsgsConverterNode::convertPoseCovarianceMatrix std::array RadarTracksMsgsConverterNode::convertTwistCovarianceMatrix( const radar_msgs::msg::RadarTrack & radar_track) { - using POSE_IDX = autoware::universe_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; - using RADAR_IDX = autoware::universe_utils::xyz_upper_covariance_index::XYZ_UPPER_COV_IDX; + using POSE_IDX = autoware_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; + using RADAR_IDX = autoware_utils::xyz_upper_covariance_index::XYZ_UPPER_COV_IDX; std::array twist_covariance{}; twist_covariance[POSE_IDX::X_X] = radar_track.velocity_covariance[RADAR_IDX::X_X]; twist_covariance[POSE_IDX::X_Y] = radar_track.velocity_covariance[RADAR_IDX::X_Y]; @@ -368,8 +368,8 @@ std::array RadarTracksMsgsConverterNode::convertTwistCovarianceMatri std::array RadarTracksMsgsConverterNode::convertAccelerationCovarianceMatrix( const radar_msgs::msg::RadarTrack & radar_track) { - using POSE_IDX = autoware::universe_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; - using RADAR_IDX = autoware::universe_utils::xyz_upper_covariance_index::XYZ_UPPER_COV_IDX; + using POSE_IDX = autoware_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; + using RADAR_IDX = autoware_utils::xyz_upper_covariance_index::XYZ_UPPER_COV_IDX; std::array acceleration_covariance{}; acceleration_covariance[POSE_IDX::X_X] = radar_track.acceleration_covariance[RADAR_IDX::X_X]; acceleration_covariance[POSE_IDX::X_Y] = radar_track.acceleration_covariance[RADAR_IDX::X_Y]; diff --git a/perception/autoware_radar_tracks_msgs_converter/src/radar_tracks_msgs_converter_node.hpp b/perception/autoware_radar_tracks_msgs_converter/src/radar_tracks_msgs_converter_node.hpp index 36528711cc403..0b08007dd56b0 100644 --- a/perception/autoware_radar_tracks_msgs_converter/src/radar_tracks_msgs_converter_node.hpp +++ b/perception/autoware_radar_tracks_msgs_converter/src/radar_tracks_msgs_converter_node.hpp @@ -15,7 +15,7 @@ #ifndef RADAR_TRACKS_MSGS_CONVERTER_NODE_HPP_ #define RADAR_TRACKS_MSGS_CONVERTER_NODE_HPP_ -#include "autoware/universe_utils/ros/transform_listener.hpp" +#include "autoware_utils/ros/transform_listener.hpp" #include "rclcpp/rclcpp.hpp" #include "autoware_perception_msgs/msg/detected_objects.hpp" @@ -64,7 +64,7 @@ class RadarTracksMsgsConverterNode : public rclcpp::Node // Subscriber rclcpp::Subscription::SharedPtr sub_radar_{}; rclcpp::Subscription::SharedPtr sub_odometry_{}; - std::shared_ptr transform_listener_; + std::shared_ptr transform_listener_; // Callback void onRadarTracks(const RadarTracks::ConstSharedPtr msg); diff --git a/perception/autoware_raindrop_cluster_filter/package.xml b/perception/autoware_raindrop_cluster_filter/package.xml index d4d237ff73586..234e771335523 100644 --- a/perception/autoware_raindrop_cluster_filter/package.xml +++ b/perception/autoware_raindrop_cluster_filter/package.xml @@ -16,7 +16,7 @@ autoware_lanelet2_extension autoware_perception_msgs autoware_point_types - autoware_universe_utils + autoware_utils message_filters nav_msgs pcl_conversions diff --git a/perception/autoware_raindrop_cluster_filter/src/low_intensity_cluster_filter_node.cpp b/perception/autoware_raindrop_cluster_filter/src/low_intensity_cluster_filter_node.cpp index 70ddeadf00b8e..3082ea1620028 100644 --- a/perception/autoware_raindrop_cluster_filter/src/low_intensity_cluster_filter_node.cpp +++ b/perception/autoware_raindrop_cluster_filter/src/low_intensity_cluster_filter_node.cpp @@ -14,7 +14,7 @@ #include "low_intensity_cluster_filter_node.hpp" -#include "autoware/universe_utils/geometry/geometry.hpp" +#include "autoware_utils/geometry/geometry.hpp" #include @@ -56,8 +56,8 @@ LowIntensityClusterFilter::LowIntensityClusterFilter(const rclcpp::NodeOptions & "output/objects", rclcpp::QoS{1}); // initialize debug tool { - using autoware::universe_utils::DebugPublisher; - using autoware::universe_utils::StopWatch; + using autoware_utils::DebugPublisher; + using autoware_utils::StopWatch; stop_watch_ptr_ = std::make_unique>(); debug_publisher_ptr_ = std::make_unique(this, "low_intensity_cluster_filter_node"); @@ -89,8 +89,8 @@ void LowIntensityClusterFilter::objectCallback( geometry_msgs::msg::Pose max_pose; max_pose.position.x = max_x_; max_pose.position.y = max_y_; - auto min_ranged_transformed = autoware::universe_utils::transformPose(min_range, transform_stamp); - auto max_range_transformed = autoware::universe_utils::transformPose(max_pose, transform_stamp); + auto min_ranged_transformed = autoware_utils::transform_pose(min_range, transform_stamp); + auto max_range_transformed = autoware_utils::transform_pose(max_pose, transform_stamp); for (const auto & feature_object : input_msg->feature_objects) { const auto & object = feature_object.object; const auto & label = object.classification.front().label; diff --git a/perception/autoware_raindrop_cluster_filter/src/low_intensity_cluster_filter_node.hpp b/perception/autoware_raindrop_cluster_filter/src/low_intensity_cluster_filter_node.hpp index 5f2dc05ba7eff..99945a04c5398 100644 --- a/perception/autoware_raindrop_cluster_filter/src/low_intensity_cluster_filter_node.hpp +++ b/perception/autoware_raindrop_cluster_filter/src/low_intensity_cluster_filter_node.hpp @@ -16,8 +16,8 @@ #define LOW_INTENSITY_CLUSTER_FILTER_NODE_HPP_ #include "autoware/detected_object_validation/utils/utils.hpp" -#include "autoware/universe_utils/ros/debug_publisher.hpp" -#include "autoware/universe_utils/system/stop_watch.hpp" +#include "autoware_utils/ros/debug_publisher.hpp" +#include "autoware_utils/system/stop_watch.hpp" #include #include @@ -62,9 +62,8 @@ class LowIntensityClusterFilter : public rclcpp::Node autoware::detected_object_validation::utils::FilterTargetLabel filter_target_; // debugger - std::unique_ptr> stop_watch_ptr_{ - nullptr}; - std::unique_ptr debug_publisher_ptr_{nullptr}; + std::unique_ptr> stop_watch_ptr_{nullptr}; + std::unique_ptr debug_publisher_ptr_{nullptr}; }; } // namespace autoware::low_intensity_cluster_filter diff --git a/perception/autoware_shape_estimation/lib/corrector/utils.cpp b/perception/autoware_shape_estimation/lib/corrector/utils.cpp index 678eb41c56831..f8156169a3167 100644 --- a/perception/autoware_shape_estimation/lib/corrector/utils.cpp +++ b/perception/autoware_shape_estimation/lib/corrector/utils.cpp @@ -16,7 +16,7 @@ #include "autoware/shape_estimation/corrector/utils.hpp" -#include "autoware/universe_utils/geometry/geometry.hpp" +#include "autoware_utils/geometry/geometry.hpp" #include #include @@ -250,9 +250,9 @@ bool correctWithDefaultValue( // correct to set long length is x, short length is y if (shape.dimensions.x < shape.dimensions.y) { - geometry_msgs::msg::Vector3 rpy = autoware::universe_utils::getRPY(pose.orientation); + geometry_msgs::msg::Vector3 rpy = autoware_utils::get_rpy(pose.orientation); rpy.z = rpy.z + M_PI_2; - pose.orientation = autoware::universe_utils::createQuaternionFromRPY(rpy.x, rpy.y, rpy.z); + pose.orientation = autoware_utils::create_quaternion_from_rpy(rpy.x, rpy.y, rpy.z); double temp = shape.dimensions.x; shape.dimensions.x = shape.dimensions.y; shape.dimensions.y = temp; diff --git a/perception/autoware_shape_estimation/package.xml b/perception/autoware_shape_estimation/package.xml index 54fb99ecb4ab6..7c603ab75c5aa 100644 --- a/perception/autoware_shape_estimation/package.xml +++ b/perception/autoware_shape_estimation/package.xml @@ -20,7 +20,7 @@ autoware_cuda_utils autoware_perception_msgs autoware_tensorrt_common - autoware_universe_utils + autoware_utils eigen libopencv-dev libpcl-all-dev diff --git a/perception/autoware_shape_estimation/src/shape_estimation_node.cpp b/perception/autoware_shape_estimation/src/shape_estimation_node.cpp index 28b2a6e398e91..a2ce98e3edc25 100644 --- a/perception/autoware_shape_estimation/src/shape_estimation_node.cpp +++ b/perception/autoware_shape_estimation/src/shape_estimation_node.cpp @@ -15,7 +15,7 @@ #include "shape_estimation_node.hpp" #include "autoware/shape_estimation/shape_estimator.hpp" -#include "autoware/universe_utils/math/unit_conversion.hpp" +#include "autoware_utils/math/unit_conversion.hpp" #include "autoware_perception_msgs/msg/object_classification.hpp" @@ -73,13 +73,11 @@ ShapeEstimationNode::ShapeEstimationNode(const rclcpp::NodeOptions & node_option #endif processing_time_publisher_ = - std::make_unique(this, "shape_estimation"); - stop_watch_ptr_ = - std::make_unique>(); + std::make_unique(this, "shape_estimation"); + stop_watch_ptr_ = std::make_unique>(); stop_watch_ptr_->tic("cyclic_time"); stop_watch_ptr_->tic("processing_time"); - published_time_publisher_ = - std::make_unique(this); + published_time_publisher_ = std::make_unique(this); } static autoware_perception_msgs::msg::ObjectClassification::_label_type get_label( @@ -145,7 +143,7 @@ void ShapeEstimationNode::callback(const DetectedObjectsWithFeature::ConstShared if (use_vehicle_reference_yaw_ && is_vehicle) { ref_yaw_info = ReferenceYawInfo{ static_cast(tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation)), - autoware::universe_utils::deg2rad(10)}; + autoware_utils::deg2rad(10)}; } if (use_vehicle_reference_shape_size_ && is_vehicle) { ref_shape_size_info = ReferenceShapeSizeInfo{object.shape, ReferenceShapeSizeInfo::Mode::Min}; diff --git a/perception/autoware_shape_estimation/src/shape_estimation_node.hpp b/perception/autoware_shape_estimation/src/shape_estimation_node.hpp index afb020de5838d..3ce63b028f0f2 100644 --- a/perception/autoware_shape_estimation/src/shape_estimation_node.hpp +++ b/perception/autoware_shape_estimation/src/shape_estimation_node.hpp @@ -23,9 +23,9 @@ #include #endif -#include -#include -#include +#include +#include +#include #include #include @@ -44,11 +44,11 @@ class ShapeEstimationNode : public rclcpp::Node // ros rclcpp::Publisher::SharedPtr pub_; rclcpp::Subscription::SharedPtr sub_; - std::unique_ptr published_time_publisher_; + std::unique_ptr published_time_publisher_; // debug publisher - std::unique_ptr> stop_watch_ptr_; - std::unique_ptr processing_time_publisher_; + std::unique_ptr> stop_watch_ptr_; + std::unique_ptr processing_time_publisher_; void callback(const DetectedObjectsWithFeature::ConstSharedPtr input_msg); diff --git a/perception/autoware_simple_object_merger/package.xml b/perception/autoware_simple_object_merger/package.xml index ac6d97cef7044..fbc2e1acf2dd8 100644 --- a/perception/autoware_simple_object_merger/package.xml +++ b/perception/autoware_simple_object_merger/package.xml @@ -12,7 +12,7 @@ ament_cmake_auto autoware_perception_msgs - autoware_universe_utils + autoware_utils geometry_msgs rclcpp rclcpp_components diff --git a/perception/autoware_simple_object_merger/src/simple_object_merger_node.cpp b/perception/autoware_simple_object_merger/src/simple_object_merger_node.cpp index 98c7fd03c1e99..c42170e8304a4 100644 --- a/perception/autoware_simple_object_merger/src/simple_object_merger_node.cpp +++ b/perception/autoware_simple_object_merger/src/simple_object_merger_node.cpp @@ -98,7 +98,7 @@ SimpleObjectMergerNode::SimpleObjectMergerNode(const rclcpp::NodeOptions & node_ } // Subscriber - transform_listener_ = std::make_shared(this); + transform_listener_ = std::make_shared(this); sub_objects_array.resize(input_topic_size); objects_data_.resize(input_topic_size); @@ -173,7 +173,7 @@ void SimpleObjectMergerNode::onTimer() double time_diff = rclcpp::Time(objects_data_.at(i)->header.stamp).seconds() - rclcpp::Time(objects_data_.at(0)->header.stamp).seconds(); if (std::abs(time_diff) < node_param_.timeout_threshold) { - transform_ = transform_listener_->getTransform( + transform_ = transform_listener_->get_transform( node_param_.new_frame_id, objects_data_.at(i)->header.frame_id, objects_data_.at(i)->header.stamp, rclcpp::Duration::from_seconds(0.01)); diff --git a/perception/autoware_simple_object_merger/src/simple_object_merger_node.hpp b/perception/autoware_simple_object_merger/src/simple_object_merger_node.hpp index d5f907ff09ef2..9addd84c90131 100644 --- a/perception/autoware_simple_object_merger/src/simple_object_merger_node.hpp +++ b/perception/autoware_simple_object_merger/src/simple_object_merger_node.hpp @@ -15,7 +15,7 @@ #ifndef SIMPLE_OBJECT_MERGER_NODE_HPP_ #define SIMPLE_OBJECT_MERGER_NODE_HPP_ -#include "autoware/universe_utils/ros/transform_listener.hpp" +#include "autoware_utils/ros/transform_listener.hpp" #include "rclcpp/rclcpp.hpp" #include "autoware_perception_msgs/msg/detected_objects.hpp" @@ -47,7 +47,7 @@ class SimpleObjectMergerNode : public rclcpp::Node // Subscriber rclcpp::Subscription::SharedPtr sub_objects_{}; std::vector::SharedPtr> sub_objects_array{}; - std::shared_ptr transform_listener_; + std::shared_ptr transform_listener_; // Callback void onData(const DetectedObjects::ConstSharedPtr msg, size_t array_number); diff --git a/perception/autoware_tensorrt_yolox/include/autoware/tensorrt_yolox/tensorrt_yolox_node.hpp b/perception/autoware_tensorrt_yolox/include/autoware/tensorrt_yolox/tensorrt_yolox_node.hpp index 905957178ebd6..d30e5f3be70aa 100644 --- a/perception/autoware_tensorrt_yolox/include/autoware/tensorrt_yolox/tensorrt_yolox_node.hpp +++ b/perception/autoware_tensorrt_yolox/include/autoware/tensorrt_yolox/tensorrt_yolox_node.hpp @@ -18,8 +18,8 @@ #include "autoware/object_recognition_utils/object_recognition_utils.hpp" #include -#include -#include +#include +#include #include #include #include @@ -108,8 +108,8 @@ class TrtYoloXNode : public rclcpp::Node {"MOTORBIKE", 8}, // motorcycle }; RoiOverlaySemsegLabel roi_overlay_segment_labels_; - std::unique_ptr> stop_watch_ptr_; - std::unique_ptr debug_publisher_; + std::unique_ptr> stop_watch_ptr_; + std::unique_ptr debug_publisher_; }; } // namespace autoware::tensorrt_yolox diff --git a/perception/autoware_tensorrt_yolox/src/tensorrt_yolox_node.cpp b/perception/autoware_tensorrt_yolox/src/tensorrt_yolox_node.cpp index c8a6b676e6c32..948b1d9a7ff18 100644 --- a/perception/autoware_tensorrt_yolox/src/tensorrt_yolox_node.cpp +++ b/perception/autoware_tensorrt_yolox/src/tensorrt_yolox_node.cpp @@ -31,10 +31,8 @@ TrtYoloXNode::TrtYoloXNode(const rclcpp::NodeOptions & node_options) : Node("tensorrt_yolox", node_options) { { - stop_watch_ptr_ = - std::make_unique>(); - debug_publisher_ = - std::make_unique(this, this->get_name()); + stop_watch_ptr_ = std::make_unique>(); + debug_publisher_ = std::make_unique(this, this->get_name()); stop_watch_ptr_->tic("cyclic_time"); stop_watch_ptr_->tic("processing_time"); } diff --git a/perception/autoware_tracking_object_merger/include/autoware/tracking_object_merger/decorative_tracker_merger_node.hpp b/perception/autoware_tracking_object_merger/include/autoware/tracking_object_merger/decorative_tracker_merger_node.hpp index 536489b0de8fe..79aed145ceaa2 100644 --- a/perception/autoware_tracking_object_merger/include/autoware/tracking_object_merger/decorative_tracker_merger_node.hpp +++ b/perception/autoware_tracking_object_merger/include/autoware/tracking_object_merger/decorative_tracker_merger_node.hpp @@ -18,9 +18,9 @@ #include "autoware/tracking_object_merger/association/data_association.hpp" #include "autoware/tracking_object_merger/utils/tracker_state.hpp" #include "autoware/tracking_object_merger/utils/utils.hpp" -#include "autoware/universe_utils/ros/debug_publisher.hpp" -#include "autoware/universe_utils/ros/published_time_publisher.hpp" -#include "autoware/universe_utils/system/stop_watch.hpp" +#include "autoware_utils/ros/debug_publisher.hpp" +#include "autoware_utils/ros/published_time_publisher.hpp" +#include "autoware_utils/system/stop_watch.hpp" #include @@ -86,8 +86,8 @@ class DecorativeTrackerMergerNode : public rclcpp::Node // debug object publisher rclcpp::Publisher::SharedPtr debug_object_pub_; bool publish_interpolated_sub_objects_; - std::unique_ptr> stop_watch_ptr_; - std::unique_ptr processing_time_publisher_; + std::unique_ptr> stop_watch_ptr_; + std::unique_ptr processing_time_publisher_; /* handle objects */ std::unordered_map> @@ -106,7 +106,7 @@ class DecorativeTrackerMergerNode : public rclcpp::Node // tracker default settings TrackerStateParameter tracker_state_parameter_; - std::unique_ptr published_time_publisher_; + std::unique_ptr published_time_publisher_; // merge policy (currently not used) struct diff --git a/perception/autoware_tracking_object_merger/include/autoware/tracking_object_merger/utils/utils.hpp b/perception/autoware_tracking_object_merger/include/autoware/tracking_object_merger/utils/utils.hpp index f40767274ebfd..01feea5f1fd54 100644 --- a/perception/autoware_tracking_object_merger/include/autoware/tracking_object_merger/utils/utils.hpp +++ b/perception/autoware_tracking_object_merger/include/autoware/tracking_object_merger/utils/utils.hpp @@ -17,7 +17,7 @@ #ifndef AUTOWARE__TRACKING_OBJECT_MERGER__UTILS__UTILS_HPP_ #define AUTOWARE__TRACKING_OBJECT_MERGER__UTILS__UTILS_HPP_ -#include "autoware/universe_utils/geometry/geometry.hpp" +#include "autoware_utils/geometry/geometry.hpp" #include diff --git a/perception/autoware_tracking_object_merger/package.xml b/perception/autoware_tracking_object_merger/package.xml index d61479f44f31d..6c236943d8337 100644 --- a/perception/autoware_tracking_object_merger/package.xml +++ b/perception/autoware_tracking_object_merger/package.xml @@ -15,7 +15,7 @@ autoware_object_recognition_utils autoware_perception_msgs - autoware_universe_utils + autoware_utils eigen glog mussp diff --git a/perception/autoware_tracking_object_merger/src/association/data_association.cpp b/perception/autoware_tracking_object_merger/src/association/data_association.cpp index 461dfb8ef14ce..7a6417f0a67a0 100644 --- a/perception/autoware_tracking_object_merger/src/association/data_association.cpp +++ b/perception/autoware_tracking_object_merger/src/association/data_association.cpp @@ -17,7 +17,7 @@ #include "autoware/object_recognition_utils/object_recognition_utils.hpp" #include "autoware/tracking_object_merger/association/solver/gnn_solver.hpp" #include "autoware/tracking_object_merger/utils/utils.hpp" -#include "autoware/universe_utils/geometry/geometry.hpp" +#include "autoware_utils/geometry/geometry.hpp" #include #include @@ -31,8 +31,8 @@ double getFormedYawAngle( const geometry_msgs::msg::Quaternion & quat0, const geometry_msgs::msg::Quaternion & quat1, const bool distinguish_front_or_back = true) { - const double yaw0 = autoware::universe_utils::normalizeRadian(tf2::getYaw(quat0)); - const double yaw1 = autoware::universe_utils::normalizeRadian(tf2::getYaw(quat1)); + const double yaw0 = autoware_utils::normalize_radian(tf2::getYaw(quat0)); + const double yaw1 = autoware_utils::normalize_radian(tf2::getYaw(quat1)); const double angle_range = distinguish_front_or_back ? M_PI : M_PI_2; const double angle_step = distinguish_front_or_back ? 2.0 * M_PI : M_PI; // Fixed yaw0 to be in the range of +-90 or 180 degrees of yaw1 @@ -189,7 +189,7 @@ double DataAssociation::calcScoreBetweenObjects( double score = 0.0; if (can_assign_matrix_(object1_label, object0_label)) { const double max_dist = max_dist_matrix_(object1_label, object0_label); - const double dist = autoware::universe_utils::calcDistance2d( + const double dist = autoware_utils::calc_distance2d( object0.kinematics.pose_with_covariance.pose.position, object1.kinematics.pose_with_covariance.pose.position); diff --git a/perception/autoware_tracking_object_merger/src/decorative_tracker_merger_node.cpp b/perception/autoware_tracking_object_merger/src/decorative_tracker_merger_node.cpp index 91345c2d10685..706f3c3e531cf 100644 --- a/perception/autoware_tracking_object_merger/src/decorative_tracker_merger_node.cpp +++ b/perception/autoware_tracking_object_merger/src/decorative_tracker_merger_node.cpp @@ -162,14 +162,12 @@ DecorativeTrackerMergerNode::DecorativeTrackerMergerNode(const rclcpp::NodeOptio set3dDataAssociation("radar-radar", data_association_map_); // debug publisher - processing_time_publisher_ = std::make_unique( - this, "decorative_object_merger_node"); - stop_watch_ptr_ = - std::make_unique>(); + processing_time_publisher_ = + std::make_unique(this, "decorative_object_merger_node"); + stop_watch_ptr_ = std::make_unique>(); stop_watch_ptr_->tic("cyclic_time"); stop_watch_ptr_->tic("processing_time"); - published_time_publisher_ = - std::make_unique(this); + published_time_publisher_ = std::make_unique(this); } void DecorativeTrackerMergerNode::set3dDataAssociation( diff --git a/perception/autoware_tracking_object_merger/src/utils/utils.cpp b/perception/autoware_tracking_object_merger/src/utils/utils.cpp index 2d7b930da9e66..5fa69c13155b1 100644 --- a/perception/autoware_tracking_object_merger/src/utils/utils.cpp +++ b/perception/autoware_tracking_object_merger/src/utils/utils.cpp @@ -281,7 +281,7 @@ bool objectsHaveSameMotionDirections(const TrackedObject & main_obj, const Track // diff of motion yaw angle const auto motion_yaw_diff = std::fabs(main_motion_yaw - sub_motion_yaw); const auto normalized_motion_yaw_diff = - autoware::universe_utils::normalizeRadian(motion_yaw_diff); // -pi ~ pi + autoware_utils::normalize_radian(motion_yaw_diff); // -pi ~ pi // evaluate if motion yaw angle is same constexpr double yaw_threshold = M_PI / 4.0; // 45 deg if (std::abs(normalized_motion_yaw_diff) < yaw_threshold) { diff --git a/perception/autoware_traffic_light_category_merger/package.xml b/perception/autoware_traffic_light_category_merger/package.xml index 7a20ad463fbfa..a9e931c74da64 100644 --- a/perception/autoware_traffic_light_category_merger/package.xml +++ b/perception/autoware_traffic_light_category_merger/package.xml @@ -13,7 +13,7 @@ ament_cmake_auto autoware_test_utils - autoware_universe_utils + autoware_utils message_filters rclcpp rclcpp_components diff --git a/perception/autoware_traffic_light_category_merger/src/traffic_light_category_merger_node.hpp b/perception/autoware_traffic_light_category_merger/src/traffic_light_category_merger_node.hpp index 23aa62fdcd2a0..2bd8615a51eef 100644 --- a/perception/autoware_traffic_light_category_merger/src/traffic_light_category_merger_node.hpp +++ b/perception/autoware_traffic_light_category_merger/src/traffic_light_category_merger_node.hpp @@ -15,7 +15,7 @@ #ifndef TRAFFIC_LIGHT_CATEGORY_MERGER_NODE_HPP_ #define TRAFFIC_LIGHT_CATEGORY_MERGER_NODE_HPP_ -#include +#include #include #include diff --git a/perception/autoware_traffic_light_map_based_detector/package.xml b/perception/autoware_traffic_light_map_based_detector/package.xml index aa1e801169156..ec5fccf688399 100644 --- a/perception/autoware_traffic_light_map_based_detector/package.xml +++ b/perception/autoware_traffic_light_map_based_detector/package.xml @@ -16,7 +16,7 @@ autoware_lanelet2_extension autoware_map_msgs - autoware_universe_utils + autoware_utils geometry_msgs image_geometry rclcpp diff --git a/perception/autoware_traffic_light_map_based_detector/src/traffic_light_map_based_detector_node.cpp b/perception/autoware_traffic_light_map_based_detector/src/traffic_light_map_based_detector_node.cpp index 6171e85223c27..d56949fabd53d 100644 --- a/perception/autoware_traffic_light_map_based_detector/src/traffic_light_map_based_detector_node.cpp +++ b/perception/autoware_traffic_light_map_based_detector/src/traffic_light_map_based_detector_node.cpp @@ -18,11 +18,11 @@ #include #include -#include -#include #include #include #include +#include +#include #include #include @@ -515,11 +515,9 @@ void MapBasedDetector::getVisibleTrafficLights( // set different max angle range for ped and car traffic light double max_angle_range; if (pedestrian_tl_id_.find(traffic_light.id()) != pedestrian_tl_id_.end()) { - max_angle_range = - autoware::universe_utils::deg2rad(config_.pedestrian_traffic_light_max_angle_range); + max_angle_range = autoware_utils::deg2rad(config_.pedestrian_traffic_light_max_angle_range); } else { - max_angle_range = - autoware::universe_utils::deg2rad(config_.car_traffic_light_max_angle_range); + max_angle_range = autoware_utils::deg2rad(config_.car_traffic_light_max_angle_range); } // traffic light bottom left const auto & tl_bl = traffic_light.front(); @@ -535,7 +533,7 @@ void MapBasedDetector::getVisibleTrafficLights( } // check angle range - const double tl_yaw = autoware::universe_utils::normalizeRadian( + const double tl_yaw = autoware_utils::normalize_radian( std::atan2(tl_br.y() - tl_bl.y(), tl_br.x() - tl_bl.x()) + M_PI_2); // get direction of z axis @@ -543,7 +541,7 @@ void MapBasedDetector::getVisibleTrafficLights( tf2::Matrix3x3 camera_rotation_matrix(tf_map2camera.getRotation()); camera_z_dir = camera_rotation_matrix * camera_z_dir; double camera_yaw = std::atan2(camera_z_dir.y(), camera_z_dir.x()); - camera_yaw = autoware::universe_utils::normalizeRadian(camera_yaw); + camera_yaw = autoware_utils::normalize_radian(camera_yaw); if (!isInAngleRange(tl_yaw, camera_yaw, max_angle_range)) { continue; } diff --git a/perception/autoware_traffic_light_occlusion_predictor/package.xml b/perception/autoware_traffic_light_occlusion_predictor/package.xml index 04a38fd701a0f..a3844aff9a931 100644 --- a/perception/autoware_traffic_light_occlusion_predictor/package.xml +++ b/perception/autoware_traffic_light_occlusion_predictor/package.xml @@ -16,7 +16,7 @@ autoware_lanelet2_extension autoware_map_msgs autoware_traffic_light_utils - autoware_universe_utils + autoware_utils geometry_msgs image_geometry pcl_msgs diff --git a/perception/autoware_traffic_light_occlusion_predictor/src/node.cpp b/perception/autoware_traffic_light_occlusion_predictor/src/node.cpp index 0285b26813eb8..b44027aff9de1 100644 --- a/perception/autoware_traffic_light_occlusion_predictor/src/node.cpp +++ b/perception/autoware_traffic_light_occlusion_predictor/src/node.cpp @@ -18,10 +18,10 @@ #include #include -#include #include #include #include +#include #include #include diff --git a/perception/autoware_traffic_light_occlusion_predictor/src/occlusion_predictor.cpp b/perception/autoware_traffic_light_occlusion_predictor/src/occlusion_predictor.cpp index 751e80e101e84..687afd01483ce 100644 --- a/perception/autoware_traffic_light_occlusion_predictor/src/occlusion_predictor.cpp +++ b/perception/autoware_traffic_light_occlusion_predictor/src/occlusion_predictor.cpp @@ -105,7 +105,7 @@ void CloudOcclusionPredictor::predict( pcl::PointCloud cloud_camera; // points within roi pcl::PointCloud cloud_roi; - autoware::universe_utils::transformPointCloudFromROSMsg(*cloud_msg, cloud_camera, camera2cloud); + autoware_utils::transform_point_cloud_from_ros_msg(*cloud_msg, cloud_camera, camera2cloud); filterCloud(cloud_camera, roi_tls, roi_brs, cloud_roi); diff --git a/perception/autoware_traffic_light_occlusion_predictor/src/occlusion_predictor.hpp b/perception/autoware_traffic_light_occlusion_predictor/src/occlusion_predictor.hpp index 46365d0219b9f..d3378bd7fc164 100644 --- a/perception/autoware_traffic_light_occlusion_predictor/src/occlusion_predictor.hpp +++ b/perception/autoware_traffic_light_occlusion_predictor/src/occlusion_predictor.hpp @@ -16,8 +16,8 @@ #ifndef OCCLUSION_PREDICTOR_HPP_ #define OCCLUSION_PREDICTOR_HPP_ -#include -#include +#include +#include #include #include diff --git a/perception/autoware_traffic_light_selector/package.xml b/perception/autoware_traffic_light_selector/package.xml index ca4add0e85060..a9e8ffce2856c 100644 --- a/perception/autoware_traffic_light_selector/package.xml +++ b/perception/autoware_traffic_light_selector/package.xml @@ -14,7 +14,7 @@ ament_cmake_auto autoware_test_utils - autoware_universe_utils + autoware_utils cgal cv_bridge geometry_msgs diff --git a/perception/autoware_traffic_light_selector/src/traffic_light_selector_node.cpp b/perception/autoware_traffic_light_selector/src/traffic_light_selector_node.cpp index 240d2439cb29e..e00951cc160eb 100644 --- a/perception/autoware_traffic_light_selector/src/traffic_light_selector_node.cpp +++ b/perception/autoware_traffic_light_selector/src/traffic_light_selector_node.cpp @@ -37,10 +37,8 @@ TrafficLightSelectorNode::TrafficLightSelectorNode(const rclcpp::NodeOptions & n sync_(SyncPolicy(10), detected_rois_sub_, rough_rois_sub_, expected_rois_sub_, camera_info_sub_) { { - stop_watch_ptr_ = - std::make_unique>(); - debug_publisher_ = - std::make_unique(this, this->get_name()); + stop_watch_ptr_ = std::make_unique>(); + debug_publisher_ = std::make_unique(this, this->get_name()); stop_watch_ptr_->tic("cyclic_time"); stop_watch_ptr_->tic("processing_time"); } @@ -155,11 +153,11 @@ void TrafficLightSelectorNode::objectsCallback( std::chrono::duration( std::chrono::nanoseconds((this->get_clock()->now() - output.header.stamp).nanoseconds())) .count(); - debug_publisher_->publish( + debug_publisher_->publish( "debug/cyclic_time_ms", cyclic_time_ms); - debug_publisher_->publish( + debug_publisher_->publish( "debug/processing_time_ms", processing_time_ms); - debug_publisher_->publish( + debug_publisher_->publish( "debug/pipeline_latency_ms", pipeline_latency_ms); } return; diff --git a/perception/autoware_traffic_light_selector/src/traffic_light_selector_node.hpp b/perception/autoware_traffic_light_selector/src/traffic_light_selector_node.hpp index 29357b32a0b45..a2a77172d5891 100644 --- a/perception/autoware_traffic_light_selector/src/traffic_light_selector_node.hpp +++ b/perception/autoware_traffic_light_selector/src/traffic_light_selector_node.hpp @@ -17,9 +17,9 @@ #include "utils.hpp" -#include -#include -#include +#include +#include +#include #include #include @@ -75,8 +75,8 @@ class TrafficLightSelectorNode : public rclcpp::Node double max_iou_threshold_; - std::unique_ptr> stop_watch_ptr_; - std::unique_ptr debug_publisher_; + std::unique_ptr> stop_watch_ptr_; + std::unique_ptr debug_publisher_; }; } // namespace autoware::traffic_light diff --git a/planning/autoware_costmap_generator/include/autoware/costmap_generator/costmap_generator.hpp b/planning/autoware_costmap_generator/include/autoware/costmap_generator/costmap_generator.hpp index 350ad3fbdd3e4..adcd9d7604357 100644 --- a/planning/autoware_costmap_generator/include/autoware/costmap_generator/costmap_generator.hpp +++ b/planning/autoware_costmap_generator/include/autoware/costmap_generator/costmap_generator.hpp @@ -49,11 +49,11 @@ #include "autoware/costmap_generator/utils/points_to_costmap.hpp" #include "costmap_generator_node_parameters.hpp" -#include -#include -#include -#include #include +#include +#include +#include +#include #include #include #include @@ -94,21 +94,20 @@ class CostmapGenerator : public rclcpp::Node sensor_msgs::msg::PointCloud2::ConstSharedPtr points_; grid_map::GridMap costmap_; - std::shared_ptr time_keeper_; + std::shared_ptr time_keeper_; rclcpp::Publisher::SharedPtr pub_costmap_; rclcpp::Publisher::SharedPtr pub_occupancy_grid_; - rclcpp::Publisher::SharedPtr pub_processing_time_; + rclcpp::Publisher::SharedPtr pub_processing_time_; rclcpp::Publisher::SharedPtr pub_processing_time_ms_; rclcpp::Subscription::SharedPtr sub_lanelet_bin_map_; - autoware::universe_utils::InterProcessPollingSubscriber - sub_points_{this, "~/input/points_no_ground", autoware::universe_utils::SingleDepthSensorQoS()}; - autoware::universe_utils::InterProcessPollingSubscriber sub_objects_{ + autoware_utils::InterProcessPollingSubscriber sub_points_{ + this, "~/input/points_no_ground", autoware_utils::single_depth_sensor_qos()}; + autoware_utils::InterProcessPollingSubscriber sub_objects_{ this, "~/input/objects"}; - autoware::universe_utils::InterProcessPollingSubscriber< - autoware_internal_planning_msgs::msg::Scenario> + autoware_utils::InterProcessPollingSubscriber sub_scenario_{this, "~/input/scenario"}; rclcpp::TimerBase::SharedPtr timer_; @@ -192,7 +191,7 @@ class CostmapGenerator : public rclcpp::Node grid_map::Matrix generateCombinedCostmap(); /// \brief measure processing time - autoware::universe_utils::StopWatch stop_watch; + autoware_utils::StopWatch stop_watch; friend class ::TestCostmapGenerator; }; diff --git a/planning/autoware_costmap_generator/package.xml b/planning/autoware_costmap_generator/package.xml index d2d0eba64cdd0..6814c5793fb68 100644 --- a/planning/autoware_costmap_generator/package.xml +++ b/planning/autoware_costmap_generator/package.xml @@ -20,7 +20,7 @@ autoware_map_msgs autoware_perception_msgs autoware_test_utils - autoware_universe_utils + autoware_utils generate_parameter_library grid_map_ros libpcl-all-dev diff --git a/planning/autoware_costmap_generator/src/costmap_generator.cpp b/planning/autoware_costmap_generator/src/costmap_generator.cpp index 200a68eb5ec42..571536a6a48a7 100644 --- a/planning/autoware_costmap_generator/src/costmap_generator.cpp +++ b/planning/autoware_costmap_generator/src/costmap_generator.cpp @@ -173,8 +173,8 @@ CostmapGenerator::CostmapGenerator(const rclcpp::NodeOptions & node_options) pub_occupancy_grid_ = this->create_publisher("~/output/occupancy_grid", 1); pub_processing_time_ = - create_publisher("processing_time", 1); - time_keeper_ = std::make_shared(pub_processing_time_); + create_publisher("processing_time", 1); + time_keeper_ = std::make_shared(pub_processing_time_); pub_processing_time_ms_ = this->create_publisher( "~/debug/processing_time_ms", 1); @@ -249,9 +249,9 @@ void CostmapGenerator::onLaneletMapBin( void CostmapGenerator::update_data() { - objects_ = sub_objects_.takeData(); - points_ = sub_points_.takeData(); - scenario_ = sub_scenario_.takeData(); + objects_ = sub_objects_.take_data(); + points_ = sub_points_.take_data(); + scenario_ = sub_scenario_.take_data(); } void CostmapGenerator::set_current_pose() @@ -263,7 +263,7 @@ void CostmapGenerator::onTimer() { update_data(); - autoware::universe_utils::ScopedTimeTrack scoped_time_track(__func__, *time_keeper_); + autoware_utils::ScopedTimeTrack scoped_time_track(__func__, *time_keeper_); stop_watch.tic(); if (!param_->activate_by_scenario) set_current_pose(); @@ -292,22 +292,22 @@ void CostmapGenerator::onTimer() set_grid_center(tf); if ((param_->use_wayarea || param_->use_parkinglot) && lanelet_map_) { - autoware::universe_utils::ScopedTimeTrack st("generatePrimitivesCostmap()", *time_keeper_); + autoware_utils::ScopedTimeTrack st("generatePrimitivesCostmap()", *time_keeper_); costmap_[LayerName::primitives] = generatePrimitivesCostmap(); } if (param_->use_objects && objects_) { - autoware::universe_utils::ScopedTimeTrack st("generateObjectsCostmap()", *time_keeper_); + autoware_utils::ScopedTimeTrack st("generateObjectsCostmap()", *time_keeper_); costmap_[LayerName::objects] = generateObjectsCostmap(objects_); } if (param_->use_points && points_) { - autoware::universe_utils::ScopedTimeTrack st("generatePointsCostmap()", *time_keeper_); + autoware_utils::ScopedTimeTrack st("generatePointsCostmap()", *time_keeper_); costmap_[LayerName::points] = generatePointsCostmap(points_, tf.transform.translation.z); } { - autoware::universe_utils::ScopedTimeTrack st("generateCombinedCostmap()", *time_keeper_); + autoware_utils::ScopedTimeTrack st("generateCombinedCostmap()", *time_keeper_); costmap_[LayerName::combined] = generateCombinedCostmap(); } diff --git a/planning/autoware_costmap_generator/test/test_object_map_utils.cpp b/planning/autoware_costmap_generator/test/test_object_map_utils.cpp index 5225b089f845f..ef5484b2bec3e 100644 --- a/planning/autoware_costmap_generator/test/test_object_map_utils.cpp +++ b/planning/autoware_costmap_generator/test/test_object_map_utils.cpp @@ -13,7 +13,7 @@ // limitations under the License. #include -#include +#include #include diff --git a/planning/autoware_costmap_generator/test/test_objects_to_costmap.cpp b/planning/autoware_costmap_generator/test/test_objects_to_costmap.cpp index 83f7055531e7d..8fbf5183b1cfa 100644 --- a/planning/autoware_costmap_generator/test/test_objects_to_costmap.cpp +++ b/planning/autoware_costmap_generator/test/test_objects_to_costmap.cpp @@ -13,7 +13,7 @@ // limitations under the License. #include -#include +#include #include #include @@ -100,17 +100,17 @@ PredictedObject ObjectsToCostMapTest::get_object( object.shape.type = autoware_perception_msgs::msg::Shape::POLYGON; - object.shape.footprint.points.emplace_back(toPoint32( - autoware::universe_utils::calcOffsetPose(pose, -0.5 * dimension.x, -0.5 * dimension.y, 0.0))); + object.shape.footprint.points.emplace_back( + toPoint32(autoware_utils::calc_offset_pose(pose, -0.5 * dimension.x, -0.5 * dimension.y, 0.0))); - object.shape.footprint.points.emplace_back(toPoint32( - autoware::universe_utils::calcOffsetPose(pose, -0.5 * dimension.x, 0.5 * dimension.y, 0.0))); + object.shape.footprint.points.emplace_back( + toPoint32(autoware_utils::calc_offset_pose(pose, -0.5 * dimension.x, 0.5 * dimension.y, 0.0))); - object.shape.footprint.points.emplace_back(toPoint32( - autoware::universe_utils::calcOffsetPose(pose, 0.5 * dimension.x, 0.5 * dimension.y, 0.0))); + object.shape.footprint.points.emplace_back( + toPoint32(autoware_utils::calc_offset_pose(pose, 0.5 * dimension.x, 0.5 * dimension.y, 0.0))); - object.shape.footprint.points.emplace_back(toPoint32( - autoware::universe_utils::calcOffsetPose(pose, 0.5 * dimension.x, -0.5 * dimension.y, 0.0))); + object.shape.footprint.points.emplace_back( + toPoint32(autoware_utils::calc_offset_pose(pose, 0.5 * dimension.x, -0.5 * dimension.y, 0.0))); return object; } diff --git a/planning/autoware_freespace_planner/include/autoware/freespace_planner/freespace_planner_node.hpp b/planning/autoware_freespace_planner/include/autoware/freespace_planner/freespace_planner_node.hpp index 2d8cfe241ae6b..447a9a1d85ace 100644 --- a/planning/autoware_freespace_planner/include/autoware/freespace_planner/freespace_planner_node.hpp +++ b/planning/autoware_freespace_planner/include/autoware/freespace_planner/freespace_planner_node.hpp @@ -31,11 +31,11 @@ #ifndef AUTOWARE__FREESPACE_PLANNER__FREESPACE_PLANNER_NODE_HPP_ #define AUTOWARE__FREESPACE_PLANNER__FREESPACE_PLANNER_NODE_HPP_ -#include "autoware/universe_utils/ros/logger_level_configure.hpp" +#include "autoware_utils/ros/logger_level_configure.hpp" #include #include -#include +#include #include #include @@ -117,12 +117,10 @@ class FreespacePlannerNode : public rclcpp::Node rclcpp::Subscription::SharedPtr route_sub_; - autoware::universe_utils::InterProcessPollingSubscriber occupancy_grid_sub_{ + autoware_utils::InterProcessPollingSubscriber occupancy_grid_sub_{ this, "~/input/occupancy_grid"}; - autoware::universe_utils::InterProcessPollingSubscriber scenario_sub_{ - this, "~/input/scenario"}; - autoware::universe_utils::InterProcessPollingSubscriber< - Odometry, autoware::universe_utils::polling_policy::All> + autoware_utils::InterProcessPollingSubscriber scenario_sub_{this, "~/input/scenario"}; + autoware_utils::InterProcessPollingSubscriber odom_sub_{this, "~/input/odometry", rclcpp::QoS{100}}; rclcpp::TimerBase::SharedPtr timer_; @@ -205,7 +203,7 @@ class FreespacePlannerNode : public rclcpp::Node TransformStamped getTransform(const std::string & from, const std::string & to); - std::unique_ptr logger_configure_; + std::unique_ptr logger_configure_; friend class ::TestFreespacePlanner; }; diff --git a/planning/autoware_freespace_planner/package.xml b/planning/autoware_freespace_planner/package.xml index 7586d110d4f51..cd048705326d8 100644 --- a/planning/autoware_freespace_planner/package.xml +++ b/planning/autoware_freespace_planner/package.xml @@ -20,7 +20,7 @@ autoware_planning_msgs autoware_planning_test_manager autoware_route_handler - autoware_universe_utils + autoware_utils autoware_vehicle_info_utils geometry_msgs nav_msgs diff --git a/planning/autoware_freespace_planner/src/autoware_freespace_planner/freespace_planner_node.cpp b/planning/autoware_freespace_planner/src/autoware_freespace_planner/freespace_planner_node.cpp index 2749408bedb7b..4c13bd485244b 100644 --- a/planning/autoware_freespace_planner/src/autoware_freespace_planner/freespace_planner_node.cpp +++ b/planning/autoware_freespace_planner/src/autoware_freespace_planner/freespace_planner_node.cpp @@ -34,9 +34,9 @@ #include "autoware/freespace_planning_algorithms/abstract_algorithm.hpp" #include -#include -#include -#include +#include +#include +#include #include #include @@ -112,7 +112,7 @@ FreespacePlannerNode::FreespacePlannerNode(const rclcpp::NodeOptions & node_opti this, get_clock(), period_ns, std::bind(&FreespacePlannerNode::onTimer, this)); } - logger_configure_ = std::make_unique(this); + logger_configure_ = std::make_unique(this); } PlannerCommonParam FreespacePlannerNode::getPlannerCommonParam() @@ -246,10 +246,10 @@ void FreespacePlannerNode::onOdometry(const Odometry::ConstSharedPtr msg) void FreespacePlannerNode::updateData() { - occupancy_grid_ = occupancy_grid_sub_.takeData(); + occupancy_grid_ = occupancy_grid_sub_.take_data(); { - auto msgs = odom_sub_.takeData(); + auto msgs = odom_sub_.take_data(); for (const auto & msg : msgs) { onOdometry(msg); } @@ -280,9 +280,9 @@ bool FreespacePlannerNode::isDataReady() void FreespacePlannerNode::onTimer() { - autoware::universe_utils::StopWatch stop_watch; + autoware_utils::StopWatch stop_watch; - scenario_ = scenario_sub_.takeData(); + scenario_ = scenario_sub_.take_data(); if (!utils::is_active(scenario_)) { reset(); return; diff --git a/planning/autoware_freespace_planner/src/autoware_freespace_planner/utils.cpp b/planning/autoware_freespace_planner/src/autoware_freespace_planner/utils.cpp index ea58ce626bcc9..322e9fa337337 100644 --- a/planning/autoware_freespace_planner/src/autoware_freespace_planner/utils.cpp +++ b/planning/autoware_freespace_planner/src/autoware_freespace_planner/utils.cpp @@ -15,8 +15,8 @@ #include "autoware/freespace_planner/utils.hpp" #include -#include -#include +#include +#include #include #include @@ -39,7 +39,7 @@ PoseArray trajectory_to_pose_array(const Trajectory & trajectory) double calc_distance_2d(const Trajectory & trajectory, const Pose & pose) { const auto idx = autoware::motion_utils::findNearestIndex(trajectory.points, pose.position); - return autoware::universe_utils::calcDistance2d(trajectory.points.at(idx), pose); + return autoware_utils::calc_distance2d(trajectory.points.at(idx), pose); } Pose transform_pose(const Pose & pose, const TransformStamped & transform) @@ -178,7 +178,7 @@ bool is_stopped( bool is_near_target(const Pose & target_pose, const Pose & current_pose, const double th_distance_m) { - const auto pose_dev = autoware::universe_utils::calcPoseDeviation(target_pose, current_pose); + const auto pose_dev = autoware_utils::calc_pose_deviation(target_pose, current_pose); return abs(pose_dev.yaw) < M_PI_2 && abs(pose_dev.longitudinal) < th_distance_m && abs(pose_dev.lateral) < th_distance_m; } diff --git a/planning/autoware_freespace_planner/test/test_freespace_planner_utils.cpp b/planning/autoware_freespace_planner/test/test_freespace_planner_utils.cpp index e374c4056df1e..e407b3f8c6ea6 100644 --- a/planning/autoware_freespace_planner/test/test_freespace_planner_utils.cpp +++ b/planning/autoware_freespace_planner/test/test_freespace_planner_utils.cpp @@ -158,7 +158,7 @@ TEST(FreespacePlannerUtilsTest, testIsNearTarget) auto current_pose = target_pose; current_pose.position.x -= 1.0; current_pose.position.y += 1.0; - current_pose.orientation = autoware::universe_utils::createQuaternionFromYaw(M_PI_2 + eps); + current_pose.orientation = autoware_utils::create_quaternion_from_yaw(M_PI_2 + eps); const double th_distance_m = 0.5; @@ -173,7 +173,7 @@ TEST(FreespacePlannerUtilsTest, testIsNearTarget) EXPECT_FALSE( autoware::freespace_planner::utils::is_near_target(target_pose, current_pose, th_distance_m)); - current_pose.orientation = autoware::universe_utils::createQuaternionFromYaw(M_PI_4); + current_pose.orientation = autoware_utils::create_quaternion_from_yaw(M_PI_4); EXPECT_TRUE( autoware::freespace_planner::utils::is_near_target(target_pose, current_pose, th_distance_m)); } diff --git a/planning/autoware_freespace_planning_algorithms/include/autoware/freespace_planning_algorithms/abstract_algorithm.hpp b/planning/autoware_freespace_planning_algorithms/include/autoware/freespace_planning_algorithms/abstract_algorithm.hpp index 52b6b404ba6eb..416762e094a54 100644 --- a/planning/autoware_freespace_planning_algorithms/include/autoware/freespace_planning_algorithms/abstract_algorithm.hpp +++ b/planning/autoware_freespace_planning_algorithms/include/autoware/freespace_planning_algorithms/abstract_algorithm.hpp @@ -15,8 +15,8 @@ #ifndef AUTOWARE__FREESPACE_PLANNING_ALGORITHMS__ABSTRACT_ALGORITHM_HPP_ #define AUTOWARE__FREESPACE_PLANNING_ALGORITHMS__ABSTRACT_ALGORITHM_HPP_ -#include -#include +#include +#include #include #include @@ -30,7 +30,7 @@ namespace autoware::freespace_planning_algorithms { -using autoware::universe_utils::normalizeRadian; +using autoware_utils::normalize_radian; geometry_msgs::msg::Pose transformPose( const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::TransformStamped & transform); @@ -262,7 +262,7 @@ class AbstractPlanningAlgorithm inline double getVehicleBaseToFrameDistance(const double angle) const { - const double normalized_angle = std::abs(normalizeRadian(angle)); + const double normalized_angle = std::abs(normalize_radian(angle)); const double w = 0.5 * collision_vehicle_shape_.width; const double l_b = collision_vehicle_shape_.base2back; const double l_f = collision_vehicle_shape_.length - l_b; diff --git a/planning/autoware_freespace_planning_algorithms/include/autoware/freespace_planning_algorithms/kinematic_bicycle_model.hpp b/planning/autoware_freespace_planning_algorithms/include/autoware/freespace_planning_algorithms/kinematic_bicycle_model.hpp index b9b6fe8c79af0..27ccdb83ae04a 100644 --- a/planning/autoware_freespace_planning_algorithms/include/autoware/freespace_planning_algorithms/kinematic_bicycle_model.hpp +++ b/planning/autoware_freespace_planning_algorithms/include/autoware/freespace_planning_algorithms/kinematic_bicycle_model.hpp @@ -15,7 +15,7 @@ #ifndef AUTOWARE__FREESPACE_PLANNING_ALGORITHMS__KINEMATIC_BICYCLE_MODEL_HPP_ #define AUTOWARE__FREESPACE_PLANNING_ALGORITHMS__KINEMATIC_BICYCLE_MODEL_HPP_ -#include +#include #include #include @@ -50,7 +50,7 @@ inline geometry_msgs::msg::Pose getPose( const double beta = distance / R; pose.position.x += (R * std::sin(yaw + beta) - R * std::sin(yaw)); pose.position.y += (R * std::cos(yaw) - R * std::cos(yaw + beta)); - pose.orientation = autoware::universe_utils::createQuaternionFromYaw(yaw + beta); + pose.orientation = autoware_utils::create_quaternion_from_yaw(yaw + beta); return pose; } diff --git a/planning/autoware_freespace_planning_algorithms/package.xml b/planning/autoware_freespace_planning_algorithms/package.xml index 873e95ec42b14..b522fb616c67f 100644 --- a/planning/autoware_freespace_planning_algorithms/package.xml +++ b/planning/autoware_freespace_planning_algorithms/package.xml @@ -16,7 +16,7 @@ autoware_cmake python_cmake_module - autoware_universe_utils + autoware_utils autoware_vehicle_info_utils geometry_msgs nav_msgs diff --git a/planning/autoware_freespace_planning_algorithms/src/abstract_algorithm.cpp b/planning/autoware_freespace_planning_algorithms/src/abstract_algorithm.cpp index 255288603016b..c80ab4452640b 100644 --- a/planning/autoware_freespace_planning_algorithms/src/abstract_algorithm.cpp +++ b/planning/autoware_freespace_planning_algorithms/src/abstract_algorithm.cpp @@ -16,8 +16,8 @@ #include "autoware/freespace_planning_algorithms/kinematic_bicycle_model.hpp" -#include -#include +#include +#include #include #include @@ -26,7 +26,7 @@ namespace autoware::freespace_planning_algorithms { -using autoware::universe_utils::createQuaternionFromYaw; +using autoware_utils::create_quaternion_from_yaw; geometry_msgs::msg::Pose transformPose( const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::TransformStamped & transform) @@ -40,7 +40,7 @@ geometry_msgs::msg::Pose transformPose( int discretizeAngle(const double theta, const int theta_size) { const double angle_resolution = 2.0 * M_PI / theta_size; - return static_cast(std::round(normalizeRadian(theta, 0.0) / angle_resolution)) % theta_size; + return static_cast(std::round(normalize_radian(theta, 0.0) / angle_resolution)) % theta_size; } IndexXYT pose2index( @@ -63,7 +63,7 @@ geometry_msgs::msg::Pose index2pose( const double angle_resolution = 2.0 * M_PI / theta_size; const double yaw = index.theta * angle_resolution; - pose_local.orientation = createQuaternionFromYaw(yaw); + pose_local.orientation = create_quaternion_from_yaw(yaw); return pose_local; } @@ -101,7 +101,7 @@ double PlannerWaypoints::compute_length() const for (size_t i = 0; i < waypoints.size() - 1; ++i) { const auto pose_a = waypoints.at(i); const auto pose_b = waypoints.at(i + 1); - total_cost += autoware::universe_utils::calcDistance2d(pose_a.pose, pose_b.pose); + total_cost += autoware_utils::calc_distance2d(pose_a.pose, pose_b.pose); } return total_cost; } diff --git a/planning/autoware_freespace_planning_algorithms/src/astar_search.cpp b/planning/autoware_freespace_planning_algorithms/src/astar_search.cpp index c9f4b46dab737..302fce4944b68 100644 --- a/planning/autoware_freespace_planning_algorithms/src/astar_search.cpp +++ b/planning/autoware_freespace_planning_algorithms/src/astar_search.cpp @@ -17,8 +17,8 @@ #include "autoware/freespace_planning_algorithms/abstract_algorithm.hpp" #include "autoware/freespace_planning_algorithms/kinematic_bicycle_model.hpp" -#include -#include +#include +#include #include #include @@ -39,7 +39,7 @@ namespace autoware::freespace_planning_algorithms { -using autoware::universe_utils::calcDistance2d; +using autoware_utils::calc_distance2d; double calcReedsSheppDistance(const Pose & p1, const Pose & p2, double radius) { @@ -267,7 +267,7 @@ void AstarSearch::setStartNode(const double cost_offset) const double initial_cost = estimateCost(start_pose_, index) + cost_offset; start_node->set(start_pose_, 0.0, initial_cost, 0, false); start_node->dir_distance = 0.0; - start_node->dist_to_goal = calcDistance2d(start_pose_, goal_pose_); + start_node->dist_to_goal = calc_distance2d(start_pose_, goal_pose_); start_node->dist_to_obs = getObstacleEDT(index).distance; start_node->status = NodeStatus::Open; start_node->parent = nullptr; @@ -365,7 +365,7 @@ void AstarSearch::expandNodes(AstarNode & current_node, const bool is_back) next_node->set(next_pose, move_cost, total_cost, steering_index, is_back); next_node->dir_distance = std::abs(distance) + (is_direction_switch ? 0.0 : current_node.dir_distance); - next_node->dist_to_goal = calcDistance2d(next_pose, goal_pose_); + next_node->dist_to_goal = calc_distance2d(next_pose, goal_pose_); next_node->dist_to_obs = obs_edt.distance; next_node->parent = ¤t_node; openlist_.push(next_node); @@ -420,7 +420,7 @@ double AstarSearch::getLatDistanceCost(const Pose & pose) const { if (is_multiple_goals_) return 0.0; const auto ref_pose = is_backward_search_ ? start_pose_ : goal_pose_; - const double distance_to_goal = calcDistance2d(pose, ref_pose); + const double distance_to_goal = calc_distance2d(pose, ref_pose); if (distance_to_goal > near_goal_dist_) return 0.0; const double lat_distance = std::abs(calcRelativePose(ref_pose, pose).position.y); return astar_param_.goal_lat_distance_weight * lat_distance; @@ -448,7 +448,7 @@ void AstarSearch::setPath(const AstarNode & goal_node) const auto interpolate = [this, &waypoints, &pose](const AstarNode & node) { if (node.parent == nullptr || !astar_param_.adapt_expansion_distance) return; const auto parent_pose = node2pose(*node.parent); - const double distance_2d = calcDistance2d(node2pose(node), parent_pose); + const double distance_2d = calc_distance2d(node2pose(node), parent_pose); const int n = static_cast(distance_2d / min_expansion_dist_); for (int i = 1; i < n; ++i) { const double dist = @@ -499,8 +499,7 @@ bool AstarSearch::isGoal(const AstarNode & node) const { const double lateral_goal_range = planner_common_param_.lateral_goal_range / 2.0; const double longitudinal_goal_range = planner_common_param_.longitudinal_goal_range / 2.0; - const double goal_angle = - autoware::universe_utils::deg2rad(planner_common_param_.angle_goal_range / 2.0); + const double goal_angle = autoware_utils::deg2rad(planner_common_param_.angle_goal_range / 2.0); const auto node_pose = node2pose(node); @@ -526,7 +525,7 @@ bool AstarSearch::isGoal(const AstarNode & node) const } const auto angle_diff = - autoware::universe_utils::normalizeRadian(tf2::getYaw(relative_pose.orientation)); + autoware_utils::normalize_radian(tf2::getYaw(relative_pose.orientation)); if (std::abs(angle_diff) > goal_angle) { return false; } @@ -570,7 +569,7 @@ Pose AstarSearch::node2pose(const AstarNode & node) const pose_local.position.x = node.x; pose_local.position.y = node.y; pose_local.position.z = goal_pose_.position.z; - pose_local.orientation = autoware::universe_utils::createQuaternionFromYaw(node.theta); + pose_local.orientation = autoware_utils::create_quaternion_from_yaw(node.theta); return pose_local; } diff --git a/planning/autoware_mission_planner_universe/package.xml b/planning/autoware_mission_planner_universe/package.xml index 804623d60a63f..cd6349dade6ef 100644 --- a/planning/autoware_mission_planner_universe/package.xml +++ b/planning/autoware_mission_planner_universe/package.xml @@ -23,7 +23,7 @@ autoware_motion_utils autoware_planning_msgs autoware_route_handler - autoware_universe_utils + autoware_utils autoware_vehicle_info_utils geometry_msgs pluginlib diff --git a/planning/autoware_mission_planner_universe/src/lanelet2_plugins/default_planner.cpp b/planning/autoware_mission_planner_universe/src/lanelet2_plugins/default_planner.cpp index 41e70537e4736..09068be173c9c 100644 --- a/planning/autoware_mission_planner_universe/src/lanelet2_plugins/default_planner.cpp +++ b/planning/autoware_mission_planner_universe/src/lanelet2_plugins/default_planner.cpp @@ -18,14 +18,14 @@ #include #include -#include -#include -#include -#include #include #include #include #include +#include +#include +#include +#include #include #include @@ -105,13 +105,11 @@ PlannerPlugin::MarkerArray DefaultPlanner::visualize(const LaneletRoute & route) } const std_msgs::msg::ColorRGBA cl_route = - autoware::universe_utils::createMarkerColor(0.8, 0.99, 0.8, 0.15); + autoware_utils::create_marker_color(0.8, 0.99, 0.8, 0.15); const std_msgs::msg::ColorRGBA cl_ll_borders = - autoware::universe_utils::createMarkerColor(1.0, 1.0, 1.0, 0.999); - const std_msgs::msg::ColorRGBA cl_end = - autoware::universe_utils::createMarkerColor(0.2, 0.2, 0.4, 0.05); - const std_msgs::msg::ColorRGBA cl_goal = - autoware::universe_utils::createMarkerColor(0.2, 0.4, 0.4, 0.05); + autoware_utils::create_marker_color(1.0, 1.0, 1.0, 0.999); + const std_msgs::msg::ColorRGBA cl_end = autoware_utils::create_marker_color(0.2, 0.2, 0.4, 0.05); + const std_msgs::msg::ColorRGBA cl_goal = autoware_utils::create_marker_color(0.2, 0.4, 0.4, 0.05); visualization_msgs::msg::MarkerArray route_marker_array; insert_marker_array( @@ -131,27 +129,27 @@ PlannerPlugin::MarkerArray DefaultPlanner::visualize(const LaneletRoute & route) } visualization_msgs::msg::MarkerArray DefaultPlanner::visualize_debug_footprint( - autoware::universe_utils::LinearRing2d goal_footprint) + autoware_utils::LinearRing2d goal_footprint) { visualization_msgs::msg::MarkerArray msg; - auto marker = autoware::universe_utils::createDefaultMarker( + auto marker = autoware_utils::create_default_marker( "map", rclcpp::Clock().now(), "goal_footprint", 0, visualization_msgs::msg::Marker::LINE_STRIP, - autoware::universe_utils::createMarkerScale(0.05, 0.0, 0.0), - autoware::universe_utils::createMarkerColor(0.99, 0.99, 0.2, 1.0)); + autoware_utils::create_marker_scale(0.05, 0.0, 0.0), + autoware_utils::create_marker_color(0.99, 0.99, 0.2, 1.0)); marker.lifetime = rclcpp::Duration::from_seconds(2.5); marker.points.push_back( - autoware::universe_utils::createPoint(goal_footprint[0][0], goal_footprint[0][1], 0.0)); + autoware_utils::create_point(goal_footprint[0][0], goal_footprint[0][1], 0.0)); marker.points.push_back( - autoware::universe_utils::createPoint(goal_footprint[1][0], goal_footprint[1][1], 0.0)); + autoware_utils::create_point(goal_footprint[1][0], goal_footprint[1][1], 0.0)); marker.points.push_back( - autoware::universe_utils::createPoint(goal_footprint[2][0], goal_footprint[2][1], 0.0)); + autoware_utils::create_point(goal_footprint[2][0], goal_footprint[2][1], 0.0)); marker.points.push_back( - autoware::universe_utils::createPoint(goal_footprint[3][0], goal_footprint[3][1], 0.0)); + autoware_utils::create_point(goal_footprint[3][0], goal_footprint[3][1], 0.0)); marker.points.push_back( - autoware::universe_utils::createPoint(goal_footprint[4][0], goal_footprint[4][1], 0.0)); + autoware_utils::create_point(goal_footprint[4][0], goal_footprint[4][1], 0.0)); marker.points.push_back( - autoware::universe_utils::createPoint(goal_footprint[5][0], goal_footprint[5][1], 0.0)); + autoware_utils::create_point(goal_footprint[5][0], goal_footprint[5][1], 0.0)); marker.points.push_back(marker.points.front()); msg.markers.push_back(marker); @@ -179,10 +177,10 @@ lanelet::ConstLanelets next_lanelets_up_to( bool DefaultPlanner::check_goal_footprint_inside_lanes( const lanelet::ConstLanelet & closest_lanelet_to_goal, const lanelet::ConstLanelets & path_lanelets, - const universe_utils::Polygon2d & goal_footprint) const + const autoware_utils::Polygon2d & goal_footprint) const { - universe_utils::MultiPolygon2d ego_lanes; - universe_utils::Polygon2d poly; + autoware_utils::MultiPolygon2d ego_lanes; + autoware_utils::Polygon2d poly; for (const auto & ll : path_lanelets) { const auto left_shoulder = route_handler_.getLeftShoulderLanelet(ll); if (left_shoulder) { @@ -216,7 +214,7 @@ bool DefaultPlanner::check_goal_footprint_inside_lanes( } // check if goal footprint is in the ego lane - universe_utils::MultiPolygon2d difference; + autoware_utils::MultiPolygon2d difference; boost::geometry::difference(goal_footprint, ego_lanes, difference); return boost::geometry::is_empty(difference); } @@ -235,8 +233,8 @@ bool DefaultPlanner::is_goal_valid( shoulder_lanelets, goal, &closest_shoulder_lanelet)) { const auto lane_yaw = lanelet::utils::getLaneletAngle(closest_shoulder_lanelet, goal.position); const auto goal_yaw = tf2::getYaw(goal.orientation); - const auto angle_diff = autoware::universe_utils::normalizeRadian(lane_yaw - goal_yaw); - const double th_angle = autoware::universe_utils::deg2rad(param_.goal_angle_threshold_deg); + const auto angle_diff = autoware_utils::normalize_radian(lane_yaw - goal_yaw); + const double th_angle = autoware_utils::deg2rad(param_.goal_angle_threshold_deg); if (std::abs(angle_diff) < th_angle) { return true; } @@ -266,8 +264,8 @@ bool DefaultPlanner::is_goal_valid( } const auto local_vehicle_footprint = vehicle_info_.createFootprint(); - autoware::universe_utils::LinearRing2d goal_footprint = autoware::universe_utils::transformVector( - local_vehicle_footprint, autoware::universe_utils::pose2transform(goal)); + autoware_utils::LinearRing2d goal_footprint = + autoware_utils::transform_vector(local_vehicle_footprint, autoware_utils::pose2transform(goal)); pub_goal_footprint_marker_->publish(visualize_debug_footprint(goal_footprint)); const auto polygon_footprint = convert_linear_ring_to_polygon(goal_footprint); @@ -285,9 +283,9 @@ bool DefaultPlanner::is_goal_valid( if (is_in_lane(closest_lanelet_to_goal, goal_lanelet_pt)) { const auto lane_yaw = lanelet::utils::getLaneletAngle(closest_lanelet_to_goal, goal.position); const auto goal_yaw = tf2::getYaw(goal.orientation); - const auto angle_diff = autoware::universe_utils::normalizeRadian(lane_yaw - goal_yaw); + const auto angle_diff = autoware_utils::normalize_radian(lane_yaw - goal_yaw); - const double th_angle = autoware::universe_utils::deg2rad(param_.goal_angle_threshold_deg); + const double th_angle = autoware_utils::deg2rad(param_.goal_angle_threshold_deg); if (std::abs(angle_diff) < th_angle) { return true; } diff --git a/planning/autoware_mission_planner_universe/src/lanelet2_plugins/default_planner.hpp b/planning/autoware_mission_planner_universe/src/lanelet2_plugins/default_planner.hpp index faa540e0b29c0..785ba4a4efc78 100644 --- a/planning/autoware_mission_planner_universe/src/lanelet2_plugins/default_planner.hpp +++ b/planning/autoware_mission_planner_universe/src/lanelet2_plugins/default_planner.hpp @@ -17,7 +17,7 @@ #include #include -#include +#include #include #include @@ -54,7 +54,7 @@ class DefaultPlanner : public mission_planner_universe::PlannerPlugin void clearRoute() override; [[nodiscard]] MarkerArray visualize(const LaneletRoute & route) const override; [[nodiscard]] static MarkerArray visualize_debug_footprint( - autoware::universe_utils::LinearRing2d goal_footprint); + autoware_utils::LinearRing2d goal_footprint); autoware::vehicle_info_utils::VehicleInfo vehicle_info_; protected: @@ -86,7 +86,7 @@ class DefaultPlanner : public mission_planner_universe::PlannerPlugin [[nodiscard]] bool check_goal_footprint_inside_lanes( const lanelet::ConstLanelet & closest_lanelet_to_goal, const lanelet::ConstLanelets & path_lanelets, - const universe_utils::Polygon2d & goal_footprint) const; + const autoware_utils::Polygon2d & goal_footprint) const; /** * @brief return true if (1)the goal is in parking area or (2)the goal is on the lanes and the diff --git a/planning/autoware_mission_planner_universe/src/lanelet2_plugins/utility_functions.cpp b/planning/autoware_mission_planner_universe/src/lanelet2_plugins/utility_functions.cpp index fd6e40ab0566e..c482ceb771727 100644 --- a/planning/autoware_mission_planner_universe/src/lanelet2_plugins/utility_functions.cpp +++ b/planning/autoware_mission_planner_universe/src/lanelet2_plugins/utility_functions.cpp @@ -27,10 +27,9 @@ namespace autoware::mission_planner_universe::lanelet2 { -autoware::universe_utils::Polygon2d convert_linear_ring_to_polygon( - autoware::universe_utils::LinearRing2d footprint) +autoware_utils::Polygon2d convert_linear_ring_to_polygon(autoware_utils::LinearRing2d footprint) { - autoware::universe_utils::Polygon2d footprint_polygon; + autoware_utils::Polygon2d footprint_polygon; boost::geometry::append(footprint_polygon.outer(), footprint[0]); boost::geometry::append(footprint_polygon.outer(), footprint[1]); boost::geometry::append(footprint_polygon.outer(), footprint[2]); @@ -69,7 +68,7 @@ geometry_msgs::msg::Pose convertBasicPoint3dToPose( pose.position.y = point.y(); pose.position.z = point.z(); - pose.orientation = autoware::universe_utils::createQuaternionFromYaw(lane_yaw); + pose.orientation = autoware_utils::create_quaternion_from_yaw(lane_yaw); return pose; } diff --git a/planning/autoware_mission_planner_universe/src/lanelet2_plugins/utility_functions.hpp b/planning/autoware_mission_planner_universe/src/lanelet2_plugins/utility_functions.hpp index 233c8ef08ec23..9e3203d2a2c9d 100644 --- a/planning/autoware_mission_planner_universe/src/lanelet2_plugins/utility_functions.hpp +++ b/planning/autoware_mission_planner_universe/src/lanelet2_plugins/utility_functions.hpp @@ -16,7 +16,7 @@ #define LANELET2_PLUGINS__UTILITY_FUNCTIONS_HPP_ #include -#include +#include #include #include @@ -44,8 +44,7 @@ bool exists(const std::vector & vectors, const T & item) return false; } -autoware::universe_utils::Polygon2d convert_linear_ring_to_polygon( - autoware::universe_utils::LinearRing2d footprint); +autoware_utils::Polygon2d convert_linear_ring_to_polygon(autoware_utils::LinearRing2d footprint); void insert_marker_array( visualization_msgs::msg::MarkerArray * a1, const visualization_msgs::msg::MarkerArray & a2); diff --git a/planning/autoware_mission_planner_universe/src/mission_planner/arrival_checker.cpp b/planning/autoware_mission_planner_universe/src/mission_planner/arrival_checker.cpp index 37e31a77ab5d5..a546a7f5696a5 100644 --- a/planning/autoware_mission_planner_universe/src/mission_planner/arrival_checker.cpp +++ b/planning/autoware_mission_planner_universe/src/mission_planner/arrival_checker.cpp @@ -14,9 +14,9 @@ #include "arrival_checker.hpp" -#include -#include -#include +#include +#include +#include #include @@ -26,7 +26,7 @@ namespace autoware::mission_planner_universe ArrivalChecker::ArrivalChecker(rclcpp::Node * node) : vehicle_stop_checker_(node) { const double angle_deg = node->declare_parameter("arrival_check_angle_deg"); - angle_ = autoware::universe_utils::deg2rad(angle_deg); + angle_ = autoware_utils::deg2rad(angle_deg); distance_ = node->declare_parameter("arrival_check_distance"); duration_ = node->declare_parameter("arrival_check_duration"); } @@ -56,14 +56,14 @@ bool ArrivalChecker::is_arrived(const PoseStamped & pose) const } // Check distance. - if (distance_ < autoware::universe_utils::calcDistance2d(pose.pose, goal.pose)) { + if (distance_ < autoware_utils::calc_distance2d(pose.pose, goal.pose)) { return false; } // Check angle. const double yaw_pose = tf2::getYaw(pose.pose.orientation); const double yaw_goal = tf2::getYaw(goal.pose.orientation); - const double yaw_diff = autoware::universe_utils::normalizeRadian(yaw_pose - yaw_goal); + const double yaw_diff = autoware_utils::normalize_radian(yaw_pose - yaw_goal); if (angle_ < std::fabs(yaw_diff)) { return false; } diff --git a/planning/autoware_mission_planner_universe/src/mission_planner/mission_planner.cpp b/planning/autoware_mission_planner_universe/src/mission_planner/mission_planner.cpp index 58a081a6d6a2b..dda03f3892926 100644 --- a/planning/autoware_mission_planner_universe/src/mission_planner/mission_planner.cpp +++ b/planning/autoware_mission_planner_universe/src/mission_planner/mission_planner.cpp @@ -87,13 +87,13 @@ MissionPlanner::MissionPlanner(const rclcpp::NodeOptions & options) data_check_timer_ = create_wall_timer(period, [this] { check_initialization(); }); is_mission_planner_ready_ = false; - logger_configure_ = std::make_unique(this); + logger_configure_ = std::make_unique(this); pub_processing_time_ = this->create_publisher( "~/debug/processing_time_ms", 1); } void MissionPlanner::publish_processing_time( - autoware::universe_utils::StopWatch stop_watch) + autoware_utils::StopWatch stop_watch) { autoware_internal_debug_msgs::msg::Float64Stamped processing_time_msg; processing_time_msg.stamp = get_clock()->now(); @@ -263,7 +263,7 @@ void MissionPlanner::on_set_lanelet_route( } if (is_reroute && is_autonomous_driving) { - const auto reroute_availability = sub_reroute_availability_.takeData(); + const auto reroute_availability = sub_reroute_availability_.take_data(); if (!reroute_availability || !reroute_availability->availability) { throw service_utils::ServiceException( ResponseCode::ERROR_INVALID_STATE, @@ -321,7 +321,7 @@ void MissionPlanner::on_set_waypoint_route( : false; if (is_reroute && is_autonomous_driving) { - const auto reroute_availability = sub_reroute_availability_.takeData(); + const auto reroute_availability = sub_reroute_availability_.take_data(); if (!reroute_availability || !reroute_availability->availability) { throw service_utils::ServiceException( ResponseCode::ERROR_INVALID_STATE, diff --git a/planning/autoware_mission_planner_universe/src/mission_planner/mission_planner.hpp b/planning/autoware_mission_planner_universe/src/mission_planner/mission_planner.hpp index 10288e18c8b2a..30dbd656e0e64 100644 --- a/planning/autoware_mission_planner_universe/src/mission_planner/mission_planner.hpp +++ b/planning/autoware_mission_planner_universe/src/mission_planner/mission_planner.hpp @@ -16,12 +16,12 @@ #define MISSION_PLANNER__MISSION_PLANNER_HPP_ #include "arrival_checker.hpp" -#include "autoware/universe_utils/ros/polling_subscriber.hpp" +#include "autoware_utils/ros/polling_subscriber.hpp" #include #include -#include -#include +#include +#include #include #include @@ -70,8 +70,7 @@ class MissionPlanner : public rclcpp::Node { public: explicit MissionPlanner(const rclcpp::NodeOptions & options); - void publish_processing_time( - autoware::universe_utils::StopWatch stop_watch); + void publish_processing_time(autoware_utils::StopWatch stop_watch); private: ArrivalChecker arrival_checker_; @@ -92,8 +91,8 @@ class MissionPlanner : public rclcpp::Node rclcpp::Subscription::SharedPtr sub_modified_goal_; rclcpp::Subscription::SharedPtr sub_odometry_; rclcpp::Subscription::SharedPtr sub_operation_mode_state_; - autoware::universe_utils::InterProcessPollingSubscriber - sub_reroute_availability_{this, "~/input/reroute_availability"}; + autoware_utils::InterProcessPollingSubscriber sub_reroute_availability_{ + this, "~/input/reroute_availability"}; rclcpp::Subscription::SharedPtr sub_vector_map_; rclcpp::Publisher::SharedPtr pub_marker_; @@ -145,7 +144,7 @@ class MissionPlanner : public rclcpp::Node bool allow_reroute_in_autonomous_mode_; bool check_reroute_safety(const LaneletRoute & original_route, const LaneletRoute & target_route); - std::unique_ptr logger_configure_; + std::unique_ptr logger_configure_; rclcpp::Publisher::SharedPtr pub_processing_time_; }; diff --git a/planning/autoware_mission_planner_universe/src/mission_planner/route_selector.cpp b/planning/autoware_mission_planner_universe/src/mission_planner/route_selector.cpp index b628f90794a44..04eb709e0eb7a 100644 --- a/planning/autoware_mission_planner_universe/src/mission_planner/route_selector.cpp +++ b/planning/autoware_mission_planner_universe/src/mission_planner/route_selector.cpp @@ -143,7 +143,7 @@ RouteSelector::RouteSelector(const rclcpp::NodeOptions & options) } void RouteSelector::publish_processing_time( - autoware::universe_utils::StopWatch stop_watch) + autoware_utils::StopWatch stop_watch) { autoware_internal_debug_msgs::msg::Float64Stamped processing_time_msg; processing_time_msg.stamp = get_clock()->now(); diff --git a/planning/autoware_mission_planner_universe/src/mission_planner/route_selector.hpp b/planning/autoware_mission_planner_universe/src/mission_planner/route_selector.hpp index 4b631c05c1f88..6bf3e996cbe00 100644 --- a/planning/autoware_mission_planner_universe/src/mission_planner/route_selector.hpp +++ b/planning/autoware_mission_planner_universe/src/mission_planner/route_selector.hpp @@ -15,7 +15,7 @@ #ifndef MISSION_PLANNER__ROUTE_SELECTOR_HPP_ #define MISSION_PLANNER__ROUTE_SELECTOR_HPP_ -#include +#include #include #include @@ -66,8 +66,7 @@ class RouteSelector : public rclcpp::Node { public: explicit RouteSelector(const rclcpp::NodeOptions & options); - void publish_processing_time( - autoware::universe_utils::StopWatch stop_watch); + void publish_processing_time(autoware_utils::StopWatch stop_watch); private: using WaypointRequest = SetWaypointRoute::Request::SharedPtr; diff --git a/planning/autoware_mission_planner_universe/src/mission_planner/service_utils.hpp b/planning/autoware_mission_planner_universe/src/mission_planner/service_utils.hpp index 8b6e5c9916705..b5a5e5809c7f8 100644 --- a/planning/autoware_mission_planner_universe/src/mission_planner/service_utils.hpp +++ b/planning/autoware_mission_planner_universe/src/mission_planner/service_utils.hpp @@ -15,7 +15,7 @@ #ifndef MISSION_PLANNER__SERVICE_UTILS_HPP_ #define MISSION_PLANNER__SERVICE_UTILS_HPP_ -#include +#include #include @@ -60,7 +60,7 @@ template std::function handle_exception(void (T::*callback)(Req, Res), T * instance) { return [instance, callback](Req req, Res res) { - autoware::universe_utils::StopWatch stop_watch; + autoware_utils::StopWatch stop_watch; try { (instance->*callback)(req, res); } catch (const ServiceException & error) { diff --git a/planning/autoware_mission_planner_universe/test/test_lanelet2_plugins_default_planner.cpp b/planning/autoware_mission_planner_universe/test/test_lanelet2_plugins_default_planner.cpp index bbe3b6c40fe47..b7a93d9f4dbcf 100644 --- a/planning/autoware_mission_planner_universe/test/test_lanelet2_plugins_default_planner.cpp +++ b/planning/autoware_mission_planner_universe/test/test_lanelet2_plugins_default_planner.cpp @@ -14,9 +14,9 @@ #include <../src/lanelet2_plugins/default_planner.hpp> #include -#include -#include #include +#include +#include #include @@ -33,9 +33,9 @@ #include #include -using autoware::universe_utils::calcOffsetPose; -using autoware::universe_utils::createQuaternionFromRPY; using autoware_planning_msgs::msg::LaneletRoute; +using autoware_utils::calc_offset_pose; +using autoware_utils::create_quaternion_from_rpy; using geometry_msgs::msg::Pose; using RoutePoints = std::vector; @@ -47,7 +47,7 @@ struct DefaultPlanner : public autoware::mission_planner_universe::lanelet2::Def [[nodiscard]] bool check_goal_inside_lanes( const lanelet::ConstLanelet & closest_lanelet_to_goal, const lanelet::ConstLanelets & path_lanelets, - const autoware::universe_utils::Polygon2d & goal_footprint) const + const autoware_utils::Polygon2d & goal_footprint) const { return check_goal_footprint_inside_lanes( closest_lanelet_to_goal, path_lanelets, goal_footprint); @@ -118,7 +118,7 @@ TEST_F(DefaultPlannerTest, checkGoalInsideLane) lanelet::ConstLanelet goal_lanelet{lanelet::InvalId, left_bound, right_bound}; // simple case where the footprint is completely inside the lane - autoware::universe_utils::Polygon2d goal_footprint; + autoware_utils::Polygon2d goal_footprint; goal_footprint.outer().emplace_back(0, 0); goal_footprint.outer().emplace_back(0, 0.5); goal_footprint.outer().emplace_back(0.5, 0.5); @@ -232,21 +232,21 @@ TEST_F(DefaultPlannerTest, isValidGoal) EXPECT_TRUE(planner_.is_goal_valid_wrapper(goal_pose, path_lanelets)); // move 1m to the right to make the goal outside of the lane - Pose right_offset_goal_pose = calcOffsetPose(goal_pose, 0.0, 1.0, 0.0); + Pose right_offset_goal_pose = calc_offset_pose(goal_pose, 0.0, 1.0, 0.0); EXPECT_FALSE(planner_.is_goal_valid_wrapper(right_offset_goal_pose, path_lanelets)); // move 1m to the left to make the goal outside of the lane - Pose left_offset_goal_pose = calcOffsetPose(goal_pose, 0.0, -1.0, 0.0); + Pose left_offset_goal_pose = calc_offset_pose(goal_pose, 0.0, -1.0, 0.0); EXPECT_FALSE(planner_.is_goal_valid_wrapper(left_offset_goal_pose, path_lanelets)); // rotate to the right Pose right_rotated_goal_pose = goal_pose; - right_rotated_goal_pose.orientation = createQuaternionFromRPY(0.0, 0.0, yaw + yaw_threshold); + right_rotated_goal_pose.orientation = create_quaternion_from_rpy(0.0, 0.0, yaw + yaw_threshold); EXPECT_FALSE(planner_.is_goal_valid_wrapper(right_rotated_goal_pose, path_lanelets)); // rotate to the left Pose left_rotated_goal_pose = goal_pose; - left_rotated_goal_pose.orientation = createQuaternionFromRPY(0.0, 0.0, yaw - yaw_threshold); + left_rotated_goal_pose.orientation = create_quaternion_from_rpy(0.0, 0.0, yaw - yaw_threshold); EXPECT_FALSE(planner_.is_goal_valid_wrapper(left_rotated_goal_pose, path_lanelets)); /** @@ -282,14 +282,14 @@ TEST_F(DefaultPlannerTest, isValidGoal) // move goal pose outside of the road shoulder Pose goal_pose_outside_road_shoulder = - calcOffsetPose(goal_pose_on_road_shoulder_left_bound, 0.0, 0.1, 0.0); + calc_offset_pose(goal_pose_on_road_shoulder_left_bound, 0.0, 0.1, 0.0); EXPECT_FALSE(planner_.is_goal_valid_wrapper( goal_pose_outside_road_shoulder, path_lanelets_to_road_shoulder)); // rotate goal to the right Pose right_rotated_goal_pose_on_road_shoulder = goal_pose_on_road_shoulder; right_rotated_goal_pose_on_road_shoulder.orientation = - createQuaternionFromRPY(0.0, 0.0, yaw + yaw_threshold); + create_quaternion_from_rpy(0.0, 0.0, yaw + yaw_threshold); EXPECT_FALSE(planner_.is_goal_valid_wrapper( right_rotated_goal_pose_on_road_shoulder, path_lanelets_to_road_shoulder)); @@ -403,7 +403,7 @@ TEST_F(DefaultPlannerTest, visualizeDebugFootprint) DefaultPlanner planner; planner_.set_default_test_map(); - autoware::universe_utils::LinearRing2d footprint; + autoware_utils::LinearRing2d footprint; footprint.push_back({1.0, 1.0}); footprint.push_back({1.0, -1.0}); footprint.push_back({0.0, -1.0}); diff --git a/planning/autoware_mission_planner_universe/test/test_utility_functions.cpp b/planning/autoware_mission_planner_universe/test/test_utility_functions.cpp index a531900d69dc1..3661aacac1a2b 100644 --- a/planning/autoware_mission_planner_universe/test/test_utility_functions.cpp +++ b/planning/autoware_mission_planner_universe/test/test_utility_functions.cpp @@ -13,8 +13,8 @@ // limitations under the License. #include <../src/lanelet2_plugins/utility_functions.hpp> -#include #include +#include #include #include @@ -43,7 +43,7 @@ TEST(TestUtilityFunctions, convertLinearRingToPolygon) { // clockwise { - autoware::universe_utils::LinearRing2d footprint; + autoware_utils::LinearRing2d footprint; footprint.push_back({1.0, 1.0}); footprint.push_back({1.0, -1.0}); footprint.push_back({0.0, -1.0}); @@ -51,7 +51,7 @@ TEST(TestUtilityFunctions, convertLinearRingToPolygon) footprint.push_back({-1.0, 1.0}); footprint.push_back({0.0, 1.0}); footprint.push_back({1.0, 1.0}); - autoware::universe_utils::Polygon2d polygon = convert_linear_ring_to_polygon(footprint); + autoware_utils::Polygon2d polygon = convert_linear_ring_to_polygon(footprint); ASSERT_EQ(polygon.outer().size(), footprint.size()); for (std::size_t i = 0; i < footprint.size(); ++i) { @@ -69,7 +69,7 @@ TEST(TestUtilityFunctions, convertLinearRingToPolygon) // counterclockwise { - autoware::universe_utils::LinearRing2d footprint; + autoware_utils::LinearRing2d footprint; footprint.push_back({1.0, 1.0}); footprint.push_back({0.0, 1.0}); footprint.push_back({-1.0, 1.0}); @@ -77,7 +77,7 @@ TEST(TestUtilityFunctions, convertLinearRingToPolygon) footprint.push_back({0.0, -1.0}); footprint.push_back({1.0, -1.0}); footprint.push_back({1.0, 1.0}); - autoware::universe_utils::Polygon2d polygon = convert_linear_ring_to_polygon(footprint); + autoware_utils::Polygon2d polygon = convert_linear_ring_to_polygon(footprint); ASSERT_EQ(polygon.outer().size(), footprint.size()); diff --git a/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/common_structs.hpp b/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/common_structs.hpp index 8abff415e47df..96e71b1b3e771 100644 --- a/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/common_structs.hpp +++ b/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/common_structs.hpp @@ -19,9 +19,9 @@ #include "autoware/motion_utils/trajectory/interpolation.hpp" #include "autoware/motion_utils/trajectory/trajectory.hpp" #include "autoware/obstacle_cruise_planner/type_alias.hpp" -#include "autoware/universe_utils/geometry/boost_polygon_utils.hpp" -#include "autoware/universe_utils/ros/update_param.hpp" -#include "autoware/universe_utils/ros/uuid_helper.hpp" +#include "autoware_utils/geometry/boost_polygon_utils.hpp" +#include "autoware_utils/ros/update_param.hpp" +#include "autoware_utils/ros/uuid_helper.hpp" #include @@ -74,7 +74,7 @@ struct Obstacle twist(object.kinematics.initial_twist_with_covariance.twist), twist_reliable(true), classification(object.classification.at(0)), - uuid(autoware::universe_utils::toHexString(object.object_id)), + uuid(autoware_utils::to_hex_string(object.object_id)), shape(object.shape) { predicted_paths.clear(); @@ -232,33 +232,33 @@ struct LongitudinalInfo void onParam(const std::vector & parameters) { - autoware::universe_utils::updateParam(parameters, "normal.max_accel", max_accel); - autoware::universe_utils::updateParam(parameters, "normal.min_accel", min_accel); - autoware::universe_utils::updateParam(parameters, "normal.max_jerk", max_jerk); - autoware::universe_utils::updateParam(parameters, "normal.min_jerk", min_jerk); - autoware::universe_utils::updateParam(parameters, "limit.max_accel", limit_max_accel); - autoware::universe_utils::updateParam(parameters, "limit.min_accel", limit_min_accel); - autoware::universe_utils::updateParam(parameters, "limit.max_jerk", limit_max_jerk); - autoware::universe_utils::updateParam(parameters, "limit.min_jerk", limit_min_jerk); - autoware::universe_utils::updateParam( + autoware_utils::update_param(parameters, "normal.max_accel", max_accel); + autoware_utils::update_param(parameters, "normal.min_accel", min_accel); + autoware_utils::update_param(parameters, "normal.max_jerk", max_jerk); + autoware_utils::update_param(parameters, "normal.min_jerk", min_jerk); + autoware_utils::update_param(parameters, "limit.max_accel", limit_max_accel); + autoware_utils::update_param(parameters, "limit.min_accel", limit_min_accel); + autoware_utils::update_param(parameters, "limit.max_jerk", limit_max_jerk); + autoware_utils::update_param(parameters, "limit.min_jerk", limit_min_jerk); + autoware_utils::update_param( parameters, "common.slow_down_min_accel", slow_down_min_accel); - autoware::universe_utils::updateParam( + autoware_utils::update_param( parameters, "common.slow_down_min_jerk", slow_down_min_jerk); - autoware::universe_utils::updateParam(parameters, "common.idling_time", idling_time); - autoware::universe_utils::updateParam( + autoware_utils::update_param(parameters, "common.idling_time", idling_time); + autoware_utils::update_param( parameters, "common.min_ego_accel_for_rss", min_ego_accel_for_rss); - autoware::universe_utils::updateParam( + autoware_utils::update_param( parameters, "common.min_object_accel_for_rss", min_object_accel_for_rss); - autoware::universe_utils::updateParam( + autoware_utils::update_param( parameters, "common.safe_distance_margin", safe_distance_margin); - autoware::universe_utils::updateParam( + autoware_utils::update_param( parameters, "common.terminal_safe_distance_margin", terminal_safe_distance_margin); - autoware::universe_utils::updateParam( + autoware_utils::update_param( parameters, "common.hold_stop_velocity_threshold", hold_stop_velocity_threshold); - autoware::universe_utils::updateParam( + autoware_utils::update_param( parameters, "common.hold_stop_distance_threshold", hold_stop_distance_threshold); } @@ -299,7 +299,7 @@ struct DebugData MarkerArray stop_wall_marker; MarkerArray cruise_wall_marker; MarkerArray slow_down_wall_marker; - std::vector detection_polygons; + std::vector detection_polygons; std::optional> stop_metrics{std::nullopt}; std::optional> slow_down_metrics{std::nullopt}; std::optional> cruise_metrics{std::nullopt}; diff --git a/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/node.hpp b/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/node.hpp index 6a106776e48b5..f668521e225b1 100644 --- a/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/node.hpp +++ b/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/node.hpp @@ -20,12 +20,12 @@ #include "autoware/obstacle_cruise_planner/pid_based_planner/pid_based_planner.hpp" #include "autoware/obstacle_cruise_planner/type_alias.hpp" #include "autoware/signal_processing/lowpass_filter_1d.hpp" -#include "autoware/universe_utils/ros/logger_level_configure.hpp" -#include "autoware/universe_utils/ros/polling_subscriber.hpp" -#include "autoware/universe_utils/system/stop_watch.hpp" +#include "autoware_utils/ros/logger_level_configure.hpp" +#include "autoware_utils/ros/polling_subscriber.hpp" +#include "autoware_utils/system/stop_watch.hpp" #include -#include +#include #include #include @@ -173,13 +173,12 @@ class ObstacleCruisePlannerNode : public rclcpp::Node // subscriber rclcpp::Subscription::SharedPtr traj_sub_; - autoware::universe_utils::InterProcessPollingSubscriber ego_odom_sub_{ - this, "~/input/odometry"}; - autoware::universe_utils::InterProcessPollingSubscriber objects_sub_{ + autoware_utils::InterProcessPollingSubscriber ego_odom_sub_{this, "~/input/odometry"}; + autoware_utils::InterProcessPollingSubscriber objects_sub_{ this, "~/input/objects"}; - autoware::universe_utils::InterProcessPollingSubscriber pointcloud_sub_{ + autoware_utils::InterProcessPollingSubscriber pointcloud_sub_{ this, "~/input/pointcloud"}; - autoware::universe_utils::InterProcessPollingSubscriber acc_sub_{ + autoware_utils::InterProcessPollingSubscriber acc_sub_{ this, "~/input/acceleration"}; autoware::objects_of_interest_marker_interface::ObjectsOfInterestMarkerInterface objects_of_interest_marker_interface_{this, "obstacle_cruise_planner"}; @@ -196,7 +195,7 @@ class ObstacleCruisePlannerNode : public rclcpp::Node PlanningAlgorithm planning_algorithm_; // stop watch - mutable autoware::universe_utils::StopWatch< + mutable autoware_utils::StopWatch< std::chrono::milliseconds, std::chrono::microseconds, std::chrono::steady_clock> stop_watch_; mutable std::shared_ptr debug_data_ptr_{nullptr}; @@ -329,9 +328,9 @@ class ObstacleCruisePlannerNode : public rclcpp::Node std::vector prev_closest_stop_object_obstacles_{}; - std::unique_ptr logger_configure_; + std::unique_ptr logger_configure_; - std::unique_ptr published_time_publisher_; + std::unique_ptr published_time_publisher_; }; } // namespace autoware::motion_planning diff --git a/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/planner_interface.hpp b/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/planner_interface.hpp index f5e183afd12bd..ae472e139196d 100644 --- a/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/planner_interface.hpp +++ b/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/planner_interface.hpp @@ -21,8 +21,8 @@ #include "autoware/obstacle_cruise_planner/type_alias.hpp" #include "autoware/obstacle_cruise_planner/utils.hpp" #include "autoware/planning_factor_interface/planning_factor_interface.hpp" -#include "autoware/universe_utils/ros/update_param.hpp" -#include "autoware/universe_utils/system/stop_watch.hpp" +#include "autoware_utils/ros/update_param.hpp" +#include "autoware_utils/system/stop_watch.hpp" #include #include @@ -146,7 +146,7 @@ class PlannerInterface bool suppress_sudden_obstacle_stop_; // stop watch - autoware::universe_utils::StopWatch< + autoware_utils::StopWatch< std::chrono::milliseconds, std::chrono::microseconds, std::chrono::steady_clock> stop_watch_; @@ -323,29 +323,29 @@ class PlannerInterface if (obstacle_to_param_struct_map.count(label + "." + movement_postfix) < 1) continue; auto & param_by_obstacle_label = obstacle_to_param_struct_map.at(label + "." + movement_postfix); - autoware::universe_utils::updateParam( + autoware_utils::update_param( parameters, "slow_down." + label + "." + movement_postfix + ".max_lat_margin", param_by_obstacle_label.max_lat_margin); - autoware::universe_utils::updateParam( + autoware_utils::update_param( parameters, "slow_down." + label + "." + movement_postfix + ".min_lat_margin", param_by_obstacle_label.min_lat_margin); - autoware::universe_utils::updateParam( + autoware_utils::update_param( parameters, "slow_down." + label + "." + movement_postfix + ".max_ego_velocity", param_by_obstacle_label.max_ego_velocity); - autoware::universe_utils::updateParam( + autoware_utils::update_param( parameters, "slow_down." + label + "." + movement_postfix + ".min_ego_velocity", param_by_obstacle_label.min_ego_velocity); } } // common parameters - autoware::universe_utils::updateParam( + autoware_utils::update_param( parameters, "slow_down.time_margin_on_target_velocity", time_margin_on_target_velocity); - autoware::universe_utils::updateParam( + autoware_utils::update_param( parameters, "slow_down.lpf_gain_slow_down_vel", lpf_gain_slow_down_vel); - autoware::universe_utils::updateParam( + autoware_utils::update_param( parameters, "slow_down.lpf_gain_lat_dist", lpf_gain_lat_dist); - autoware::universe_utils::updateParam( + autoware_utils::update_param( parameters, "slow_down.lpf_gain_dist_to_slow_down", lpf_gain_dist_to_slow_down); } @@ -405,15 +405,15 @@ class PlannerInterface if (type_str == "default") { continue; } - autoware::universe_utils::updateParam( + autoware_utils::update_param( parameters, param_prefix + type_str + ".limit_min_acc", param.limit_min_acc); - autoware::universe_utils::updateParam( + autoware_utils::update_param( parameters, param_prefix + type_str + ".sudden_object_acc_threshold", param.sudden_object_acc_threshold); - autoware::universe_utils::updateParam( + autoware_utils::update_param( parameters, param_prefix + type_str + ".sudden_object_dist_threshold", param.sudden_object_dist_threshold); - autoware::universe_utils::updateParam( + autoware_utils::update_param( parameters, param_prefix + type_str + ".abandon_to_stop", param.abandon_to_stop); param.sudden_object_acc_threshold = diff --git a/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/polygon_utils.hpp b/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/polygon_utils.hpp index f84aa26480765..1c896fc13c217 100644 --- a/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/polygon_utils.hpp +++ b/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/polygon_utils.hpp @@ -17,7 +17,7 @@ #include "autoware/obstacle_cruise_planner/common_structs.hpp" #include "autoware/obstacle_cruise_planner/type_alias.hpp" -#include "autoware/universe_utils/geometry/boost_geometry.hpp" +#include "autoware_utils/geometry/boost_geometry.hpp" #include "autoware_vehicle_info_utils/vehicle_info_utils.hpp" #include @@ -30,8 +30,8 @@ namespace polygon_utils { namespace bg = boost::geometry; -using autoware::universe_utils::Point2d; -using autoware::universe_utils::Polygon2d; +using autoware_utils::Point2d; +using autoware_utils::Polygon2d; Polygon2d createOneStepPolygon( const std::vector & last_poses, diff --git a/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/type_alias.hpp b/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/type_alias.hpp index e1fb9a250a59e..7d40465f59054 100644 --- a/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/type_alias.hpp +++ b/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/type_alias.hpp @@ -17,7 +17,7 @@ #include "autoware_vehicle_info_utils/vehicle_info_utils.hpp" -#include +#include #include "autoware_internal_debug_msgs/msg/float32_stamped.hpp" #include "autoware_internal_debug_msgs/msg/float64_stamped.hpp" @@ -59,8 +59,8 @@ using tier4_planning_msgs::msg::VelocityLimitClearCommand; using visualization_msgs::msg::Marker; using visualization_msgs::msg::MarkerArray; namespace bg = boost::geometry; -using autoware::universe_utils::Point2d; -using autoware::universe_utils::Polygon2d; +using autoware_utils::Point2d; +using autoware_utils::Polygon2d; using PointCloud = pcl::PointCloud; diff --git a/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/utils.hpp b/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/utils.hpp index ebabd96a54608..f54ce95f3d338 100644 --- a/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/utils.hpp +++ b/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/utils.hpp @@ -16,7 +16,7 @@ #define AUTOWARE__OBSTACLE_CRUISE_PLANNER__UTILS_HPP_ #include "autoware/obstacle_cruise_planner/type_alias.hpp" -#include "autoware/universe_utils/geometry/geometry.hpp" +#include "autoware_utils/geometry/geometry.hpp" #include "common_structs.hpp" #include @@ -61,8 +61,7 @@ size_t getIndexWithLongitudinalOffset( double sum_length = 0.0; if (longitudinal_offset > 0) { for (size_t i = *start_idx; i < points.size() - 1; ++i) { - const double segment_length = - autoware::universe_utils::calcDistance2d(points.at(i), points.at(i + 1)); + const double segment_length = autoware_utils::calc_distance2d(points.at(i), points.at(i + 1)); sum_length += segment_length; if (sum_length >= longitudinal_offset) { const double back_length = sum_length - longitudinal_offset; @@ -78,8 +77,7 @@ size_t getIndexWithLongitudinalOffset( } for (size_t i = *start_idx; 0 < i; --i) { - const double segment_length = - autoware::universe_utils::calcDistance2d(points.at(i - 1), points.at(i)); + const double segment_length = autoware_utils::calc_distance2d(points.at(i - 1), points.at(i)); sum_length += segment_length; if (sum_length >= -longitudinal_offset) { const double back_length = sum_length + longitudinal_offset; diff --git a/planning/autoware_obstacle_cruise_planner/package.xml b/planning/autoware_obstacle_cruise_planner/package.xml index c70c8ce6f710f..8fb68dac48448 100644 --- a/planning/autoware_obstacle_cruise_planner/package.xml +++ b/planning/autoware_obstacle_cruise_planner/package.xml @@ -30,7 +30,7 @@ autoware_planning_msgs autoware_planning_test_manager autoware_signal_processing - autoware_universe_utils + autoware_utils autoware_vehicle_info_utils geometry_msgs nav_msgs diff --git a/planning/autoware_obstacle_cruise_planner/src/node.cpp b/planning/autoware_obstacle_cruise_planner/src/node.cpp index 4242f0d559946..7c1d830d06514 100644 --- a/planning/autoware_obstacle_cruise_planner/src/node.cpp +++ b/planning/autoware_obstacle_cruise_planner/src/node.cpp @@ -19,9 +19,9 @@ #include "autoware/object_recognition_utils/predicted_path_utils.hpp" #include "autoware/obstacle_cruise_planner/polygon_utils.hpp" #include "autoware/obstacle_cruise_planner/utils.hpp" -#include "autoware/universe_utils/geometry/boost_polygon_utils.hpp" -#include "autoware/universe_utils/ros/marker_helper.hpp" -#include "autoware/universe_utils/ros/update_param.hpp" +#include "autoware_utils/geometry/boost_polygon_utils.hpp" +#include "autoware_utils/ros/marker_helper.hpp" +#include "autoware_utils/ros/update_param.hpp" #include #include @@ -121,7 +121,7 @@ double calcDiffAngleAgainstTrajectory( const double target_yaw = tf2::getYaw(target_pose.orientation); - const double diff_yaw = autoware::universe_utils::normalizeRadian(target_yaw - traj_yaw); + const double diff_yaw = autoware_utils::normalize_radian(target_yaw - traj_yaw); return diff_yaw; } @@ -150,7 +150,7 @@ std::pair calculateObstacleVelocitiesRelativeToTrajectory( const double traj_yaw = tf2::getYaw(nearest_point.pose.orientation); const double obstacle_yaw = tf2::getYaw(obstacle_pose.orientation); const Eigen::Rotation2Dd R_ego_to_obstacle( - autoware::universe_utils::normalizeRadian(obstacle_yaw - traj_yaw)); + autoware_utils::normalize_radian(obstacle_yaw - traj_yaw)); // Calculate the trajectory direction and the vector from the trajectory to the obstacle const Eigen::Vector2d traj_direction(std::cos(traj_yaw), std::sin(traj_yaw)); @@ -193,7 +193,7 @@ TrajectoryPoint getExtendTrajectoryPoint( const double extend_distance, const TrajectoryPoint & goal_point, const bool is_driving_forward) { TrajectoryPoint extend_trajectory_point; - extend_trajectory_point.pose = autoware::universe_utils::calcOffsetPose( + extend_trajectory_point.pose = autoware_utils::calc_offset_pose( goal_point.pose, extend_distance * (is_driving_forward ? 1.0 : -1.0), 0.0, 0.0); extend_trajectory_point.longitudinal_velocity_mps = goal_point.longitudinal_velocity_mps; extend_trajectory_point.lateral_velocity_mps = goal_point.lateral_velocity_mps; @@ -264,7 +264,7 @@ geometry_msgs::msg::Point toGeomPoint(const pcl::PointXYZ & point) return geom_point; } -geometry_msgs::msg::Point toGeomPoint(const autoware::universe_utils::Point2d & point) +geometry_msgs::msg::Point toGeomPoint(const autoware_utils::Point2d & point) { geometry_msgs::msg::Point geom_point; geom_point.x = point.x(); @@ -385,111 +385,111 @@ void ObstacleCruisePlannerNode::BehaviorDeterminationParam::onParam( const std::vector & parameters) { // behavior determination - autoware::universe_utils::updateParam( + autoware_utils::update_param( parameters, "behavior_determination.decimate_trajectory_step_length", decimate_trajectory_step_length); - autoware::universe_utils::updateParam( + autoware_utils::update_param( parameters, "behavior_determination.pointcloud_search_radius", pointcloud_search_radius); - autoware::universe_utils::updateParam( + autoware_utils::update_param( parameters, "behavior_determination.pointcloud_voxel_grid_x", pointcloud_voxel_grid_x); - autoware::universe_utils::updateParam( + autoware_utils::update_param( parameters, "behavior_determination.pointcloud_voxel_grid_y", pointcloud_voxel_grid_y); - autoware::universe_utils::updateParam( + autoware_utils::update_param( parameters, "behavior_determination.pointcloud_voxel_grid_z", pointcloud_voxel_grid_z); - autoware::universe_utils::updateParam( + autoware_utils::update_param( parameters, "behavior_determination.pointcloud_cluster_tolerance", pointcloud_cluster_tolerance); - autoware::universe_utils::updateParam( + autoware_utils::update_param( parameters, "behavior_determination.pointcloud_min_cluster_size", pointcloud_min_cluster_size); - autoware::universe_utils::updateParam( + autoware_utils::update_param( parameters, "behavior_determination.pointcloud_max_cluster_size", pointcloud_max_cluster_size); - autoware::universe_utils::updateParam( + autoware_utils::update_param( parameters, "behavior_determination.crossing_obstacle.obstacle_velocity_threshold", crossing_obstacle_velocity_threshold); - autoware::universe_utils::updateParam( + autoware_utils::update_param( parameters, "behavior_determination.crossing_obstacle.obstacle_traj_angle_threshold", crossing_obstacle_traj_angle_threshold); - autoware::universe_utils::updateParam( + autoware_utils::update_param( parameters, "behavior_determination.stop.crossing_obstacle.collision_time_margin", collision_time_margin); - autoware::universe_utils::updateParam( + autoware_utils::update_param( parameters, "behavior_determination.cruise.outside_obstacle.obstacle_velocity_threshold", outside_obstacle_min_velocity_threshold); - autoware::universe_utils::updateParam( + autoware_utils::update_param( parameters, "behavior_determination.cruise.outside_obstacle.ego_obstacle_overlap_time_threshold", ego_obstacle_overlap_time_threshold); - autoware::universe_utils::updateParam( + autoware_utils::update_param( parameters, "behavior_determination.cruise.outside_obstacle.max_prediction_time_for_collision_check", max_prediction_time_for_collision_check); - autoware::universe_utils::updateParam( + autoware_utils::update_param( parameters, "behavior_determination.stop_obstacle_hold_time_threshold", stop_obstacle_hold_time_threshold); - autoware::universe_utils::updateParam( + autoware_utils::update_param( parameters, "behavior_determination.prediction_resampling_time_interval", prediction_resampling_time_interval); - autoware::universe_utils::updateParam( + autoware_utils::update_param( parameters, "behavior_determination.prediction_resampling_time_horizon", prediction_resampling_time_horizon); - autoware::universe_utils::updateParam( + autoware_utils::update_param( parameters, "behavior_determination.stop.max_lat_margin", max_lat_margin_for_stop); - autoware::universe_utils::updateParam( + autoware_utils::update_param( parameters, "behavior_determination.stop.max_lat_margin_against_unknown", max_lat_margin_for_stop_against_unknown); - autoware::universe_utils::updateParam( + autoware_utils::update_param( parameters, "behavior_determination.cruise.max_lat_margin", max_lat_margin_for_cruise); - autoware::universe_utils::updateParam( + autoware_utils::update_param( parameters, "behavior_determination.cruise.yield.enable_yield", enable_yield); - autoware::universe_utils::updateParam( + autoware_utils::update_param( parameters, "behavior_determination.cruise.yield.lat_distance_threshold", yield_lat_distance_threshold); - autoware::universe_utils::updateParam( + autoware_utils::update_param( parameters, "behavior_determination.cruise.yield.max_lat_dist_between_obstacles", max_lat_dist_between_obstacles); - autoware::universe_utils::updateParam( + autoware_utils::update_param( parameters, "behavior_determination.cruise.yield.stopped_obstacle_velocity_threshold", stopped_obstacle_velocity_threshold); - autoware::universe_utils::updateParam( + autoware_utils::update_param( parameters, "behavior_determination.cruise.yield.max_obstacles_collision_time", max_obstacles_collision_time); - autoware::universe_utils::updateParam( + autoware_utils::update_param( parameters, "behavior_determination.slow_down.max_lat_margin", max_lat_margin_for_slow_down); - autoware::universe_utils::updateParam( + autoware_utils::update_param( parameters, "behavior_determination.slow_down.lat_hysteresis_margin", lat_hysteresis_margin_for_slow_down); - autoware::universe_utils::updateParam( + autoware_utils::update_param( parameters, "behavior_determination.slow_down.successive_num_to_entry_slow_down_condition", successive_num_to_entry_slow_down_condition); - autoware::universe_utils::updateParam( + autoware_utils::update_param( parameters, "behavior_determination.slow_down.successive_num_to_exit_slow_down_condition", successive_num_to_exit_slow_down_condition); - autoware::universe_utils::updateParam( + autoware_utils::update_param( parameters, "behavior_determination.consider_current_pose.enable_to_consider_current_pose", enable_to_consider_current_pose); - autoware::universe_utils::updateParam( + autoware_utils::update_param( parameters, "behavior_determination.consider_current_pose.time_to_convergence", time_to_convergence); - autoware::universe_utils::updateParam( + autoware_utils::update_param( parameters, "behavior_determination.stop.min_velocity_to_reach_collision_point", min_velocity_to_reach_collision_point); - autoware::universe_utils::updateParam( + autoware_utils::update_param( parameters, "behavior_determination.stop.outside_obstacle.max_lateral_time_margin", max_lat_time_margin_for_stop); - autoware::universe_utils::updateParam( + autoware_utils::update_param( parameters, "behavior_determination.cruise.outside_obstacle.max_lateral_time_margin", max_lat_time_margin_for_cruise); - autoware::universe_utils::updateParam( + autoware_utils::update_param( parameters, "behavior_determination.cruise.outside_obstacle.num_of_predicted_paths", num_of_predicted_paths_for_outside_cruise_obstacle); - autoware::universe_utils::updateParam( + autoware_utils::update_param( parameters, "behavior_determination.stop.outside_obstacle.num_of_predicted_paths", num_of_predicted_paths_for_outside_stop_obstacle); - autoware::universe_utils::updateParam( + autoware_utils::update_param( parameters, "behavior_determination.stop.outside_obstacle.pedestrian_deceleration_rate", pedestrian_deceleration_rate); - autoware::universe_utils::updateParam( + autoware_utils::update_param( parameters, "behavior_determination.stop.outside_obstacle.bicycle_deceleration_rate", bicycle_deceleration_rate); } @@ -589,9 +589,8 @@ ObstacleCruisePlannerNode::ObstacleCruisePlannerNode(const rclcpp::NodeOptions & set_param_res_ = this->add_on_set_parameters_callback( std::bind(&ObstacleCruisePlannerNode::onParam, this, std::placeholders::_1)); - logger_configure_ = std::make_unique(this); - published_time_publisher_ = - std::make_unique(this); + logger_configure_ = std::make_unique(this); + published_time_publisher_ = std::make_unique(this); } ObstacleCruisePlannerNode::PlanningAlgorithm ObstacleCruisePlannerNode::getPlanningAlgorithmType( @@ -610,17 +609,16 @@ rcl_interfaces::msg::SetParametersResult ObstacleCruisePlannerNode::onParam( { planner_ptr_->onParam(parameters); - autoware::universe_utils::updateParam( - parameters, "common.enable_debug_info", enable_debug_info_); - autoware::universe_utils::updateParam( + autoware_utils::update_param(parameters, "common.enable_debug_info", enable_debug_info_); + autoware_utils::update_param( parameters, "common.enable_calculation_time_info", enable_calculation_time_info_); - autoware::universe_utils::updateParam( + autoware_utils::update_param( parameters, "common.stop_on_curve.enable_approaching", enable_approaching_on_curve_); - autoware::universe_utils::updateParam( + autoware_utils::update_param( parameters, "common.stop_on_curve.additional_safe_distance_margin", additional_safe_distance_margin_on_curve_); - autoware::universe_utils::updateParam( + autoware_utils::update_param( parameters, "common.stop_on_curve.min_safe_distance_margin", min_safe_distance_margin_on_curve_); @@ -629,7 +627,7 @@ rcl_interfaces::msg::SetParametersResult ObstacleCruisePlannerNode::onParam( enable_approaching_on_curve_, additional_safe_distance_margin_on_curve_, min_safe_distance_margin_on_curve_, suppress_sudden_obstacle_stop_); - autoware::universe_utils::updateParam( + autoware_utils::update_param( parameters, "common.enable_slow_down_planning", enable_slow_down_planning_); behavior_determination_param_.onParam(parameters); @@ -642,10 +640,10 @@ rcl_interfaces::msg::SetParametersResult ObstacleCruisePlannerNode::onParam( void ObstacleCruisePlannerNode::onTrajectory(const Trajectory::ConstSharedPtr msg) { - const auto ego_odom_ptr = ego_odom_sub_.takeData(); - const auto objects_ptr = objects_sub_.takeData(); - const auto pointcloud_ptr = use_pointcloud_ ? pointcloud_sub_.takeData() : nullptr; - const auto acc_ptr = acc_sub_.takeData(); + const auto ego_odom_ptr = ego_odom_sub_.take_data(); + const auto objects_ptr = objects_sub_.take_data(); + const auto pointcloud_ptr = use_pointcloud_ ? pointcloud_sub_.take_data() : nullptr; + const auto acc_ptr = acc_sub_.take_data(); const bool can_detect_obstacles = objects_ptr || pointcloud_ptr; if (!ego_odom_ptr || !can_detect_obstacles || !acc_ptr) { return; @@ -753,7 +751,7 @@ std::vector ObstacleCruisePlannerNode::createOneStepPolygons( autoware::motion_utils::findNearestSegmentIndex(traj_points, current_ego_pose.position); const auto nearest_pose = traj_points.at(nearest_idx).pose; const auto current_ego_pose_error = - autoware::universe_utils::inverseTransformPose(current_ego_pose, nearest_pose); + autoware_utils::inverse_transform_pose(current_ego_pose, nearest_pose); const double current_ego_lat_error = current_ego_pose_error.position.y; const double current_ego_yaw_error = tf2::getYaw(current_ego_pose_error.orientation); double time_elapsed{0.0}; @@ -769,11 +767,11 @@ std::vector ObstacleCruisePlannerNode::createOneStepPolygons( const double rem_ratio = (p.time_to_convergence - time_elapsed) / p.time_to_convergence; geometry_msgs::msg::Pose indexed_pose_err; indexed_pose_err.set__orientation( - autoware::universe_utils::createQuaternionFromYaw(current_ego_yaw_error * rem_ratio)); + autoware_utils::create_quaternion_from_yaw(current_ego_yaw_error * rem_ratio)); indexed_pose_err.set__position( - autoware::universe_utils::createPoint(0.0, current_ego_lat_error * rem_ratio, 0.0)); + autoware_utils::create_point(0.0, current_ego_lat_error * rem_ratio, 0.0)); current_poses.push_back( - autoware::universe_utils::transformPose(indexed_pose_err, traj_points.at(i).pose)); + autoware_utils::transform_pose(indexed_pose_err, traj_points.at(i).pose)); if (traj_points.at(i).longitudinal_velocity_mps != 0.0) { time_elapsed += p.decimate_trajectory_step_length / std::abs(traj_points.at(i).longitudinal_velocity_mps); @@ -787,23 +785,22 @@ std::vector ObstacleCruisePlannerNode::createOneStepPolygons( if (i == 0 && traj_points.at(i).longitudinal_velocity_mps > 1e-3) { boost::geometry::append( idx_poly, - autoware::universe_utils::toFootprint(pose, front_length, rear_length, vehicle_width) - .outer()); + autoware_utils::to_footprint(pose, front_length, rear_length, vehicle_width).outer()); boost::geometry::append( - idx_poly, autoware::universe_utils::fromMsg( - autoware::universe_utils::calcOffsetPose( - pose, front_length, vehicle_width * 0.5 + lat_margin, 0.0) - .position) - .to_2d()); + idx_poly, + autoware_utils::from_msg(autoware_utils::calc_offset_pose( + pose, front_length, vehicle_width * 0.5 + lat_margin, 0.0) + .position) + .to_2d()); boost::geometry::append( - idx_poly, autoware::universe_utils::fromMsg( - autoware::universe_utils::calcOffsetPose( - pose, front_length, -vehicle_width * 0.5 - lat_margin, 0.0) - .position) - .to_2d()); + idx_poly, + autoware_utils::from_msg(autoware_utils::calc_offset_pose( + pose, front_length, -vehicle_width * 0.5 - lat_margin, 0.0) + .position) + .to_2d()); } else { boost::geometry::append( - idx_poly, autoware::universe_utils::toFootprint( + idx_poly, autoware_utils::to_footprint( pose, front_length, rear_length, vehicle_width + lat_margin * 2.0) .outer()); } @@ -839,8 +836,7 @@ std::vector ObstacleCruisePlannerNode::convertToObstacles( std::vector target_obstacles; for (const auto & predicted_object : objects.objects) { - const auto & object_id = - autoware::universe_utils::toHexString(predicted_object.object_id).substr(0, 4); + const auto & object_id = autoware_utils::to_hex_string(predicted_object.object_id).substr(0, 4); // brkay54: When use_prediction is true, we observed wrong orientation for the object in // scenario simulator. @@ -1107,7 +1103,7 @@ ObstacleCruisePlannerNode::determineEgoBehaviorAgainstPredictedObjectObstacles( std::vector slow_down_obstacles; slow_down_condition_counter_.resetCurrentUuids(); for (const auto & obstacle : obstacles) { - const auto obstacle_poly = autoware::universe_utils::toPolygon2d(obstacle.pose, obstacle.shape); + const auto obstacle_poly = autoware_utils::to_polygon2d(obstacle.pose, obstacle.shape); // Calculate distance between trajectory and obstacle first double precise_lat_dist = std::numeric_limits::max(); @@ -1891,7 +1887,7 @@ std::optional ObstacleCruisePlannerNode::createSlowDownObstacl return std::nullopt; } - const auto obstacle_poly = autoware::universe_utils::toPolygon2d(obstacle.pose, obstacle.shape); + const auto obstacle_poly = autoware_utils::to_polygon2d(obstacle.pose, obstacle.shape); // calculate collision points with trajectory with lateral stop margin // NOTE: For additional margin, hysteresis is not divided by two. @@ -1998,8 +1994,7 @@ void ObstacleCruisePlannerNode::checkConsistency( const auto predicted_object_itr = std::find_if( predicted_objects.objects.begin(), predicted_objects.objects.end(), [&prev_closest_stop_obstacle](const PredictedObject & po) { - return autoware::universe_utils::toHexString(po.object_id) == - prev_closest_stop_obstacle.uuid; + return autoware_utils::to_hex_string(po.object_id) == prev_closest_stop_obstacle.uuid; }); // If previous closest obstacle disappear from the perception result, do nothing anymore. if (predicted_object_itr == predicted_objects.objects.end()) { @@ -2133,10 +2128,10 @@ void ObstacleCruisePlannerNode::publishDebugMarker() const } } for (size_t i = 0; i < stop_collision_points.size(); ++i) { - auto collision_point_marker = autoware::universe_utils::createDefaultMarker( + auto collision_point_marker = autoware_utils::create_default_marker( "map", now(), "cruise_collision_points", i, Marker::SPHERE, - autoware::universe_utils::createMarkerScale(0.25, 0.25, 0.25), - autoware::universe_utils::createMarkerColor(1.0, 0.0, 0.0, 0.999)); + autoware_utils::create_marker_scale(0.25, 0.25, 0.25), + autoware_utils::create_marker_color(1.0, 0.0, 0.0, 0.999)); collision_point_marker.pose.position = stop_collision_points.at(i); debug_marker.markers.push_back(collision_point_marker); } @@ -2149,10 +2144,10 @@ void ObstacleCruisePlannerNode::publishDebugMarker() const debug_marker.markers.push_back(obstacle_marker); // collision point - auto collision_point_marker = autoware::universe_utils::createDefaultMarker( + auto collision_point_marker = autoware_utils::create_default_marker( "map", now(), "stop_collision_points", 0, Marker::SPHERE, - autoware::universe_utils::createMarkerScale(0.25, 0.25, 0.25), - autoware::universe_utils::createMarkerColor(1.0, 0.0, 0.0, 0.999)); + autoware_utils::create_marker_scale(0.25, 0.25, 0.25), + autoware_utils::create_marker_color(1.0, 0.0, 0.0, 0.999)); collision_point_marker.pose.position = debug_data_ptr_->obstacles_to_stop.at(i).collision_point; debug_marker.markers.push_back(collision_point_marker); } @@ -2166,16 +2161,16 @@ void ObstacleCruisePlannerNode::publishDebugMarker() const debug_marker.markers.push_back(obstacle_marker); // collision points - auto front_collision_point_marker = autoware::universe_utils::createDefaultMarker( + auto front_collision_point_marker = autoware_utils::create_default_marker( "map", now(), "slow_down_collision_points", i * 2 + 0, Marker::SPHERE, - autoware::universe_utils::createMarkerScale(0.25, 0.25, 0.25), - autoware::universe_utils::createMarkerColor(1.0, 0.0, 0.0, 0.999)); + autoware_utils::create_marker_scale(0.25, 0.25, 0.25), + autoware_utils::create_marker_color(1.0, 0.0, 0.0, 0.999)); front_collision_point_marker.pose.position = debug_data_ptr_->obstacles_to_slow_down.at(i).front_collision_point; - auto back_collision_point_marker = autoware::universe_utils::createDefaultMarker( + auto back_collision_point_marker = autoware_utils::create_default_marker( "map", now(), "slow_down_collision_points", i * 2 + 1, Marker::SPHERE, - autoware::universe_utils::createMarkerScale(0.25, 0.25, 0.25), - autoware::universe_utils::createMarkerColor(1.0, 0.0, 0.0, 0.999)); + autoware_utils::create_marker_scale(0.25, 0.25, 0.25), + autoware_utils::create_marker_color(1.0, 0.0, 0.0, 0.999)); back_collision_point_marker.pose.position = debug_data_ptr_->obstacles_to_slow_down.at(i).back_collision_point; @@ -2192,10 +2187,10 @@ void ObstacleCruisePlannerNode::publishDebugMarker() const } { // footprint polygons - auto marker = autoware::universe_utils::createDefaultMarker( + auto marker = autoware_utils::create_default_marker( "map", now(), "detection_polygons", 0, Marker::LINE_LIST, - autoware::universe_utils::createMarkerScale(0.01, 0.0, 0.0), - autoware::universe_utils::createMarkerColor(0.0, 1.0, 0.0, 0.999)); + autoware_utils::create_marker_scale(0.01, 0.0, 0.0), + autoware_utils::create_marker_color(0.0, 1.0, 0.0, 0.999)); for (const auto & detection_polygon : debug_data_ptr_->detection_polygons) { for (size_t dp_idx = 0; dp_idx < detection_polygon.outer().size(); ++dp_idx) { @@ -2204,17 +2199,15 @@ void ObstacleCruisePlannerNode::publishDebugMarker() const detection_polygon.outer().at((dp_idx + 1) % detection_polygon.outer().size()); marker.points.push_back( - autoware::universe_utils::createPoint(current_point.x(), current_point.y(), 0.0)); - marker.points.push_back( - autoware::universe_utils::createPoint(next_point.x(), next_point.y(), 0.0)); + autoware_utils::create_point(current_point.x(), current_point.y(), 0.0)); + marker.points.push_back(autoware_utils::create_point(next_point.x(), next_point.y(), 0.0)); } } debug_marker.markers.push_back(marker); } // slow down debug wall marker - autoware::universe_utils::appendMarkerArray( - debug_data_ptr_->slow_down_debug_wall_marker, &debug_marker); + autoware_utils::append_marker_array(debug_data_ptr_->slow_down_debug_wall_marker, &debug_marker); debug_marker_pub_->publish(debug_marker); diff --git a/planning/autoware_obstacle_cruise_planner/src/optimization_based_planner/optimization_based_planner.cpp b/planning/autoware_obstacle_cruise_planner/src/optimization_based_planner/optimization_based_planner.cpp index 62edf82beec5a..426851526177d 100644 --- a/planning/autoware_obstacle_cruise_planner/src/optimization_based_planner/optimization_based_planner.cpp +++ b/planning/autoware_obstacle_cruise_planner/src/optimization_based_planner/optimization_based_planner.cpp @@ -22,8 +22,8 @@ #include "autoware/motion_utils/trajectory/interpolation.hpp" #include "autoware/motion_utils/trajectory/trajectory.hpp" #include "autoware/obstacle_cruise_planner/utils.hpp" -#include "autoware/universe_utils/geometry/geometry.hpp" -#include "autoware/universe_utils/ros/marker_helper.hpp" +#include "autoware_utils/geometry/geometry.hpp" +#include "autoware_utils/ros/marker_helper.hpp" #include #include @@ -464,7 +464,7 @@ std::optional OptimizationBasedPlanner::getSBoundaries( const auto markers = autoware::motion_utils::createSlowDownVirtualWallMarker( marker_pose.value(), "obstacle to follow", current_time, 0); - autoware::universe_utils::appendMarkerArray(markers, &wall_msg); + autoware_utils::append_marker_array(markers, &wall_msg); // publish rviz marker debug_wall_marker_pub_->publish(wall_msg); @@ -641,7 +641,7 @@ geometry_msgs::msg::Pose OptimizationBasedPlanner::transformBaseLink2Center( geometry_msgs::msg::Pose center_pose; center_pose.position = - autoware::universe_utils::createPoint(map2center.x(), map2center.y(), map2center.z()); + autoware_utils::create_point(map2center.x(), map2center.y(), map2center.z()); center_pose.orientation = pose_base_link.orientation; return center_pose; diff --git a/planning/autoware_obstacle_cruise_planner/src/pid_based_planner/pid_based_planner.cpp b/planning/autoware_obstacle_cruise_planner/src/pid_based_planner/pid_based_planner.cpp index 20b564addf149..195f47b5e4ee4 100644 --- a/planning/autoware_obstacle_cruise_planner/src/pid_based_planner/pid_based_planner.cpp +++ b/planning/autoware_obstacle_cruise_planner/src/pid_based_planner/pid_based_planner.cpp @@ -17,9 +17,9 @@ #include "autoware/interpolation/spline_interpolation.hpp" #include "autoware/motion_utils/marker/marker_helper.hpp" #include "autoware/obstacle_cruise_planner/utils.hpp" -#include "autoware/universe_utils/geometry/geometry.hpp" -#include "autoware/universe_utils/ros/marker_helper.hpp" -#include "autoware/universe_utils/ros/update_param.hpp" +#include "autoware_utils/geometry/geometry.hpp" +#include "autoware_utils/ros/marker_helper.hpp" +#include "autoware_utils/ros/update_param.hpp" #include "tier4_planning_msgs/msg/velocity_limit.hpp" @@ -327,9 +327,8 @@ std::vector PIDBasedPlanner::planCruise( stop_traj_points.at(wall_idx).pose, wall_reason_string, planner_data.current_time, 0); // NOTE: use a different color from slow down one to visualize cruise and slow down // separately. - markers.markers.front().color = - autoware::universe_utils::createMarkerColor(1.0, 0.6, 0.1, 0.5); - autoware::universe_utils::appendMarkerArray(markers, &debug_data_ptr_->cruise_wall_marker); + markers.markers.front().color = autoware_utils::create_marker_color(1.0, 0.6, 0.1, 0.5); + autoware_utils::append_marker_array(markers, &debug_data_ptr_->cruise_wall_marker); // cruise obstacle debug_data_ptr_->obstacles_to_cruise.push_back(cruise_obstacle_info->obstacle); @@ -362,7 +361,7 @@ std::vector PIDBasedPlanner::planCruise( // delete marker const auto markers = autoware::motion_utils::createDeletedSlowDownVirtualWallMarker(planner_data.current_time, 0); - autoware::universe_utils::appendMarkerArray(markers, &debug_data_ptr_->cruise_wall_marker); + autoware_utils::append_marker_array(markers, &debug_data_ptr_->cruise_wall_marker); return stop_traj_points; } @@ -614,7 +613,7 @@ std::vector PIDBasedPlanner::getAccelerationLimitedTrajectory( double sum_dist = 0.0; for (size_t i = ego_seg_idx; i < acc_limited_traj_points.size(); ++i) { if (i != ego_seg_idx) { - sum_dist += autoware::universe_utils::calcDistance2d( + sum_dist += autoware_utils::calc_distance2d( acc_limited_traj_points.at(i - 1), acc_limited_traj_points.at(i)); } @@ -637,7 +636,7 @@ std::vector PIDBasedPlanner::getAccelerationLimitedTrajectory( void PIDBasedPlanner::updateCruiseParam(const std::vector & parameters) { - autoware::universe_utils::updateParam( + autoware_utils::update_param( parameters, "cruise.pid_based_planner.min_accel_during_cruise", min_accel_during_cruise_); { // velocity limit based planner @@ -647,26 +646,26 @@ void PIDBasedPlanner::updateCruiseParam(const std::vector & p double ki = p.pid_vel_controller->getKi(); double kd = p.pid_vel_controller->getKd(); - autoware::universe_utils::updateParam( + autoware_utils::update_param( parameters, "cruise.pid_based_planner.velocity_limit_based_planner.kp", kp); - autoware::universe_utils::updateParam( + autoware_utils::update_param( parameters, "cruise.pid_based_planner.velocity_limit_based_planner.ki", ki); - autoware::universe_utils::updateParam( + autoware_utils::update_param( parameters, "cruise.pid_based_planner.velocity_limit_based_planner.kd", kd); p.pid_vel_controller->updateParam(kp, ki, kd); - autoware::universe_utils::updateParam( + autoware_utils::update_param( parameters, "cruise.pid_based_planner.velocity_limit_based_planner.output_ratio_during_accel", p.output_ratio_during_accel); - autoware::universe_utils::updateParam( + autoware_utils::update_param( parameters, "cruise.pid_based_planner.vel_to_acc_weight", p.vel_to_acc_weight); - autoware::universe_utils::updateParam( + autoware_utils::update_param( parameters, "cruise.pid_based_planner.velocity_limit_based_planner.enable_jerk_limit_to_output_acc", p.enable_jerk_limit_to_output_acc); - autoware::universe_utils::updateParam( + autoware_utils::update_param( parameters, "cruise.pid_based_planner.velocity_limit_based_planner.disable_target_acceleration", p.disable_target_acceleration); @@ -680,11 +679,11 @@ void PIDBasedPlanner::updateCruiseParam(const std::vector & p double ki_acc = p.pid_acc_controller->getKi(); double kd_acc = p.pid_acc_controller->getKd(); - autoware::universe_utils::updateParam( + autoware_utils::update_param( parameters, "cruise.pid_based_planner.velocity_insertion_based_planner.kp_acc", kp_acc); - autoware::universe_utils::updateParam( + autoware_utils::update_param( parameters, "cruise.pid_based_planner.velocity_insertion_based_planner.ki_acc", ki_acc); - autoware::universe_utils::updateParam( + autoware_utils::update_param( parameters, "cruise.pid_based_planner.velocity_insertion_based_planner.kd_acc", kd_acc); p.pid_acc_controller->updateParam(kp_acc, ki_acc, kd_acc); @@ -693,32 +692,32 @@ void PIDBasedPlanner::updateCruiseParam(const std::vector & p double ki_jerk = p.pid_jerk_controller->getKi(); double kd_jerk = p.pid_jerk_controller->getKd(); - autoware::universe_utils::updateParam( + autoware_utils::update_param( parameters, "cruise.pid_based_planner.velocity_insertion_based_planner.kp_jerk", kp_jerk); - autoware::universe_utils::updateParam( + autoware_utils::update_param( parameters, "cruise.pid_based_planner.velocity_insertion_based_planner.ki_jerk", ki_jerk); - autoware::universe_utils::updateParam( + autoware_utils::update_param( parameters, "cruise.pid_based_planner.velocity_insertion_based_planner.kd_jerk", kd_jerk); p.pid_jerk_controller->updateParam(kp_jerk, ki_jerk, kd_jerk); - autoware::universe_utils::updateParam( + autoware_utils::update_param( parameters, "cruise.pid_based_planner.velocity_insertion_based_planner.output_acc_ratio_during_accel", p.output_acc_ratio_during_accel); - autoware::universe_utils::updateParam( + autoware_utils::update_param( parameters, "cruise.pid_based_planner.velocity_insertion_based_planner.output_jerk_ratio_during_accel", p.output_jerk_ratio_during_accel); - autoware::universe_utils::updateParam( + autoware_utils::update_param( parameters, "cruise.pid_based_planner.velocity_insertion_based_planner.enable_jerk_limit_to_output_acc", p.enable_jerk_limit_to_output_acc); } // min_cruise_target_vel - autoware::universe_utils::updateParam( + autoware_utils::update_param( parameters, "cruise.pid_based_planner.min_cruise_target_vel", min_cruise_target_vel_); - autoware::universe_utils::updateParam( + autoware_utils::update_param( parameters, "cruise.pid_based_planner.time_to_evaluate_rss", time_to_evaluate_rss_); } diff --git a/planning/autoware_obstacle_cruise_planner/src/planner_interface.cpp b/planning/autoware_obstacle_cruise_planner/src/planner_interface.cpp index b32d7f3a1bcbb..a969ec1a1c890 100644 --- a/planning/autoware_obstacle_cruise_planner/src/planner_interface.cpp +++ b/planning/autoware_obstacle_cruise_planner/src/planner_interface.cpp @@ -20,7 +20,7 @@ #include "autoware/motion_utils/trajectory/conversion.hpp" #include "autoware/motion_utils/trajectory/trajectory.hpp" #include "autoware/signal_processing/lowpass_filter_1d.hpp" -#include "autoware/universe_utils/ros/marker_helper.hpp" +#include "autoware_utils/ros/marker_helper.hpp" #include #include @@ -162,9 +162,9 @@ std::vector resampleTrajectoryPoints( return autoware::motion_utils::convertToTrajectoryPointArray(resampled_traj); } -autoware::universe_utils::Point2d convertPoint(const geometry_msgs::msg::Point & p) +autoware_utils::Point2d convertPoint(const geometry_msgs::msg::Point & p) { - return autoware::universe_utils::Point2d{p.x, p.y}; + return autoware_utils::Point2d{p.x, p.y}; } } // namespace @@ -185,7 +185,7 @@ std::vector PlannerInterface::generateStopTrajectory( // delete marker const auto markers = autoware::motion_utils::createDeletedStopVirtualWallMarker(planner_data.current_time, 0); - autoware::universe_utils::appendMarkerArray(markers, &debug_data_ptr_->stop_wall_marker); + autoware_utils::append_marker_array(markers, &debug_data_ptr_->stop_wall_marker); prev_stop_distance_info_ = std::nullopt; return planner_data.traj_points; @@ -293,7 +293,7 @@ std::vector PlannerInterface::generateStopTrajectory( // delete marker const auto markers = autoware::motion_utils::createDeletedStopVirtualWallMarker(planner_data.current_time, 0); - autoware::universe_utils::appendMarkerArray(markers, &debug_data_ptr_->stop_wall_marker); + autoware_utils::append_marker_array(markers, &debug_data_ptr_->stop_wall_marker); prev_stop_distance_info_ = std::nullopt; return planner_data.traj_points; @@ -329,7 +329,7 @@ std::vector PlannerInterface::generateStopTrajectory( const auto markers = autoware::motion_utils::createStopVirtualWallMarker( output_traj_points.at(*zero_vel_idx).pose, "obstacle stop", planner_data.current_time, 0, abs_ego_offset, "", planner_data.is_driving_forward); - autoware::universe_utils::appendMarkerArray(markers, &debug_data_ptr_->stop_wall_marker); + autoware_utils::append_marker_array(markers, &debug_data_ptr_->stop_wall_marker); debug_data_ptr_->obstacles_to_stop.push_back(*determined_stop_obstacle); // Publish Stop Reason @@ -398,7 +398,7 @@ double PlannerInterface::calculateMarginFromObstacleOnCurve( longitudinal_info_.safe_distance_margin + abs_ego_offset < sum_short_traj_length) { break; } - sum_short_traj_length += autoware::universe_utils::calcDistance2d( + sum_short_traj_length += autoware_utils::calc_distance2d( planner_data.traj_points.at(i), planner_data.traj_points.at(i + 1)); } std::reverse(short_traj_points.begin(), short_traj_points.end()); @@ -409,15 +409,14 @@ double PlannerInterface::calculateMarginFromObstacleOnCurve( // calculate collision index between straight line from ego pose and object const auto calculate_distance_from_straight_ego_path = [&](const auto & ego_pose, const auto & object_polygon) { - const auto forward_ego_pose = autoware::universe_utils::calcOffsetPose( + const auto forward_ego_pose = autoware_utils::calc_offset_pose( ego_pose, longitudinal_info_.safe_distance_margin + 3.0, 0.0, 0.0); - const auto ego_straight_segment = autoware::universe_utils::Segment2d{ + const auto ego_straight_segment = autoware_utils::Segment2d{ convertPoint(ego_pose.position), convertPoint(forward_ego_pose.position)}; return boost::geometry::distance(ego_straight_segment, object_polygon); }; const auto resampled_short_traj_points = resampleTrajectoryPoints(short_traj_points, 0.5); - const auto object_polygon = - autoware::universe_utils::toPolygon2d(stop_obstacle.pose, stop_obstacle.shape); + const auto object_polygon = autoware_utils::to_polygon2d(stop_obstacle.pose, stop_obstacle.shape); const auto collision_idx = [&]() -> std::optional { for (size_t i = 0; i < resampled_short_traj_points.size(); ++i) { const double dist_to_obj = calculate_distance_from_straight_ego_path( @@ -437,7 +436,7 @@ double PlannerInterface::calculateMarginFromObstacleOnCurve( // calculate margin from obstacle const double partial_segment_length = [&]() { - const double collision_segment_length = autoware::universe_utils::calcDistance2d( + const double collision_segment_length = autoware_utils::calc_distance2d( resampled_short_traj_points.at(*collision_idx - 1), resampled_short_traj_points.at(*collision_idx)); const double prev_dist = calculate_distance_from_straight_ego_path( @@ -590,7 +589,7 @@ std::vector PlannerInterface::generateSlowDownTrajectory( const auto markers = autoware::motion_utils::createSlowDownVirtualWallMarker( slow_down_traj_points.at(slow_down_wall_idx).pose, "obstacle slow down", planner_data.current_time, i, abs_ego_offset, "", planner_data.is_driving_forward); - autoware::universe_utils::appendMarkerArray(markers, &debug_data_ptr_->slow_down_wall_marker); + autoware_utils::append_marker_array(markers, &debug_data_ptr_->slow_down_wall_marker); planning_factor_interface_->add( slow_down_traj_points, planner_data.ego_pose, slow_down_traj_points.at(*slow_down_start_idx).pose, @@ -605,15 +604,13 @@ std::vector PlannerInterface::generateSlowDownTrajectory( const auto markers = autoware::motion_utils::createSlowDownVirtualWallMarker( slow_down_traj_points.at(*slow_down_start_idx).pose, "obstacle slow down start", planner_data.current_time, i * 2, abs_ego_offset, "", planner_data.is_driving_forward); - autoware::universe_utils::appendMarkerArray( - markers, &debug_data_ptr_->slow_down_debug_wall_marker); + autoware_utils::append_marker_array(markers, &debug_data_ptr_->slow_down_debug_wall_marker); } if (slow_down_end_idx) { const auto markers = autoware::motion_utils::createSlowDownVirtualWallMarker( slow_down_traj_points.at(*slow_down_end_idx).pose, "obstacle slow down end", planner_data.current_time, i * 2 + 1, abs_ego_offset, "", planner_data.is_driving_forward); - autoware::universe_utils::appendMarkerArray( - markers, &debug_data_ptr_->slow_down_debug_wall_marker); + autoware_utils::append_marker_array(markers, &debug_data_ptr_->slow_down_debug_wall_marker); } // Add debug data diff --git a/planning/autoware_obstacle_cruise_planner/src/polygon_utils.cpp b/planning/autoware_obstacle_cruise_planner/src/polygon_utils.cpp index 4d6e6568067f4..abb4e51845da6 100644 --- a/planning/autoware_obstacle_cruise_planner/src/polygon_utils.cpp +++ b/planning/autoware_obstacle_cruise_planner/src/polygon_utils.cpp @@ -15,8 +15,8 @@ #include "autoware/obstacle_cruise_planner/polygon_utils.hpp" #include "autoware/motion_utils/trajectory/trajectory.hpp" -#include "autoware/universe_utils/geometry/boost_polygon_utils.hpp" -#include "autoware/universe_utils/geometry/geometry.hpp" +#include "autoware_utils/geometry/boost_polygon_utils.hpp" +#include "autoware_utils/geometry/geometry.hpp" #include #include @@ -56,10 +56,10 @@ std::optional>> getCollisionIndex( const geometry_msgs::msg::Pose & object_pose, const rclcpp::Time & object_time, const Shape & object_shape, const double max_dist = std::numeric_limits::max()) { - const auto obj_polygon = autoware::universe_utils::toPolygon2d(object_pose, object_shape); + const auto obj_polygon = autoware_utils::to_polygon2d(object_pose, object_shape); for (size_t i = 0; i < traj_polygons.size(); ++i) { const double approximated_dist = - autoware::universe_utils::calcDistance2d(traj_points.at(i).pose, object_pose); + autoware_utils::calc_distance2d(traj_points.at(i).pose, object_pose); if (approximated_dist > max_dist) { continue; } @@ -110,14 +110,14 @@ std::optional> getCollisionPoint( const double x_diff_to_bumper = is_driving_forward ? vehicle_info.max_longitudinal_offset_m : vehicle_info.min_longitudinal_offset_m; - const auto bumper_pose = autoware::universe_utils::calcOffsetPose( + const auto bumper_pose = autoware_utils::calc_offset_pose( traj_points.at(collision_info->first).pose, x_diff_to_bumper, 0.0, 0.0); std::optional max_collision_length = std::nullopt; std::optional max_collision_point = std::nullopt; for (const auto & poly_vertex : collision_info->second) { const double dist_from_bumper = - std::abs(autoware::universe_utils::inverseTransformPoint(poly_vertex.point, bumper_pose).x); + std::abs(autoware_utils::inverse_transform_point(poly_vertex.point, bumper_pose).x); if (!max_collision_length.has_value() || dist_from_bumper > *max_collision_length) { max_collision_length = dist_from_bumper; @@ -141,14 +141,14 @@ std::optional> getCollisionPoint( const double x_diff_to_bumper = is_driving_forward ? vehicle_info.max_longitudinal_offset_m : vehicle_info.min_longitudinal_offset_m; - const auto bumper_pose = autoware::universe_utils::calcOffsetPose( + const auto bumper_pose = autoware_utils::calc_offset_pose( traj_points.at(collision_info.first).pose, x_diff_to_bumper, 0.0, 0.0); std::optional max_collision_length = std::nullopt; std::optional max_collision_point = std::nullopt; for (const auto & poly_vertex : collision_info.second) { const double dist_from_bumper = - std::abs(autoware::universe_utils::inverseTransformPoint(poly_vertex.point, bumper_pose).x); + std::abs(autoware_utils::inverse_transform_point(poly_vertex.point, bumper_pose).x); if (!max_collision_length.has_value() || dist_from_bumper > *max_collision_length) { max_collision_length = dist_from_bumper; diff --git a/planning/autoware_obstacle_cruise_planner/src/utils.cpp b/planning/autoware_obstacle_cruise_planner/src/utils.cpp index b27def0bfcbe5..e66ef9b3312ba 100644 --- a/planning/autoware_obstacle_cruise_planner/src/utils.cpp +++ b/planning/autoware_obstacle_cruise_planner/src/utils.cpp @@ -15,7 +15,7 @@ #include "autoware/obstacle_cruise_planner/utils.hpp" #include "autoware/object_recognition_utils/predicted_path_utils.hpp" -#include "autoware/universe_utils/ros/marker_helper.hpp" +#include "autoware_utils/ros/marker_helper.hpp" #include #include @@ -64,10 +64,10 @@ visualization_msgs::msg::Marker getObjectMarker( { const auto current_time = rclcpp::Clock().now(); - auto marker = autoware::universe_utils::createDefaultMarker( + auto marker = autoware_utils::create_default_marker( "map", current_time, ns, idx, visualization_msgs::msg::Marker::SPHERE, - autoware::universe_utils::createMarkerScale(2.0, 2.0, 2.0), - autoware::universe_utils::createMarkerColor(r, g, b, 0.8)); + autoware_utils::create_marker_scale(2.0, 2.0, 2.0), + autoware_utils::create_marker_color(r, g, b, 0.8)); marker.pose = obj_pose; diff --git a/planning/autoware_obstacle_stop_planner/package.xml b/planning/autoware_obstacle_stop_planner/package.xml index d0e3a0f7e2f82..a25f92ab0696b 100644 --- a/planning/autoware_obstacle_stop_planner/package.xml +++ b/planning/autoware_obstacle_stop_planner/package.xml @@ -28,7 +28,7 @@ autoware_planning_msgs autoware_planning_test_manager autoware_signal_processing - autoware_universe_utils + autoware_utils autoware_vehicle_info_utils diagnostic_msgs nav_msgs diff --git a/planning/autoware_obstacle_stop_planner/src/adaptive_cruise_control.cpp b/planning/autoware_obstacle_stop_planner/src/adaptive_cruise_control.cpp index 3154ce45972a3..b9d18cde62a5b 100644 --- a/planning/autoware_obstacle_stop_planner/src/adaptive_cruise_control.cpp +++ b/planning/autoware_obstacle_stop_planner/src/adaptive_cruise_control.cpp @@ -30,7 +30,7 @@ #include #endif -#include +#include #include #include @@ -99,7 +99,7 @@ double getDistanceFromTwoPoint( return dist; } -[[maybe_unused]] double normalizeRadian( +[[maybe_unused]] double normalize_radian( const double rad, const double min_rad = -boost::math::constants::pi(), const double max_rad = boost::math::constants::pi()) { @@ -449,8 +449,7 @@ void AdaptiveCruiseController::calculateProjectedVelocityFromObject( object.kinematics.initial_twist_with_covariance.twist.linear.y, object.kinematics.initial_twist_with_covariance.twist.linear.x); - *velocity = - obj_vel_norm * std::cos(autoware::universe_utils::normalizeRadian(obj_vel_yaw - traj_yaw)); + *velocity = obj_vel_norm * std::cos(autoware_utils::normalize_radian(obj_vel_yaw - traj_yaw)); debug_values_.data.at(DBGVAL::ESTIMATED_VEL_OBJ) = *velocity; } diff --git a/planning/autoware_obstacle_stop_planner/src/debug_marker.cpp b/planning/autoware_obstacle_stop_planner/src/debug_marker.cpp index a3337c5dd2524..4fb3cb8828abe 100644 --- a/planning/autoware_obstacle_stop_planner/src/debug_marker.cpp +++ b/planning/autoware_obstacle_stop_planner/src/debug_marker.cpp @@ -15,8 +15,8 @@ #include "debug_marker.hpp" #include -#include -#include +#include +#include #include @@ -34,12 +34,12 @@ using autoware::motion_utils::createDeletedSlowDownVirtualWallMarker; using autoware::motion_utils::createDeletedStopVirtualWallMarker; using autoware::motion_utils::createSlowDownVirtualWallMarker; using autoware::motion_utils::createStopVirtualWallMarker; -using autoware::universe_utils::appendMarkerArray; -using autoware::universe_utils::calcOffsetPose; -using autoware::universe_utils::createDefaultMarker; -using autoware::universe_utils::createMarkerColor; -using autoware::universe_utils::createMarkerScale; -using autoware::universe_utils::createPoint; +using autoware_utils::append_marker_array; +using autoware_utils::calc_offset_pose; +using autoware_utils::create_default_marker; +using autoware_utils::create_marker_color; +using autoware_utils::create_marker_scale; +using autoware_utils::create_point; namespace autoware::motion_planning { @@ -58,7 +58,7 @@ ObstacleStopPlannerDebugNode::ObstacleStopPlannerDebugNode( } bool ObstacleStopPlannerDebugNode::pushPolygon( - const autoware::universe_utils::Polygon2d & polygon, const double z, const PolygonType & type) + const autoware_utils::Polygon2d & polygon, const double z, const PolygonType & type) { std::vector eigen_polygon; for (const auto & point : polygon.outer()) { @@ -104,7 +104,7 @@ bool ObstacleStopPlannerDebugNode::pushPolygon( } bool ObstacleStopPlannerDebugNode::pushPolyhedron( - const autoware::universe_utils::Polygon2d & polyhedron, const double z_min, const double z_max, + const autoware_utils::Polygon2d & polyhedron, const double z_min, const double z_max, const PolygonType & type) { std::vector eigen_polyhedron; @@ -229,40 +229,40 @@ MarkerArray ObstacleStopPlannerDebugNode::makeVirtualWallMarker() rclcpp::Time current_time = node_->now(); if (stop_pose_ptr_ != nullptr) { - const auto p = calcOffsetPose(*stop_pose_ptr_, base_link2front_, 0.0, 0.0); + const auto p = calc_offset_pose(*stop_pose_ptr_, base_link2front_, 0.0, 0.0); const auto markers = createStopVirtualWallMarker(p, "obstacle on the path", current_time, 0); - appendMarkerArray(markers, &msg); + append_marker_array(markers, &msg); } else { const auto markers = createDeletedStopVirtualWallMarker(current_time, 0); - appendMarkerArray(markers, &msg); + append_marker_array(markers, &msg); } if (slow_down_start_pose_ptr_ != nullptr && stop_pose_ptr_ == nullptr) { - const auto p = calcOffsetPose(*slow_down_start_pose_ptr_, base_link2front_, 0.0, 0.0); + const auto p = calc_offset_pose(*slow_down_start_pose_ptr_, base_link2front_, 0.0, 0.0); { const auto markers = createSlowDownVirtualWallMarker(p, "obstacle beside the path", current_time, 0); - appendMarkerArray(markers, &msg); + append_marker_array(markers, &msg); } { auto markers = createSlowDownVirtualWallMarker(p, "slow down\nstart", current_time, 1); markers.markers.front().ns = "slow_down_start_virtual_wall"; markers.markers.back().ns = "slow_down_start_factor_text"; - appendMarkerArray(markers, &msg); + append_marker_array(markers, &msg); } } else { const auto markers = createDeletedSlowDownVirtualWallMarker(current_time, 0); - appendMarkerArray(markers, &msg); + append_marker_array(markers, &msg); } if (slow_down_end_pose_ptr_ != nullptr && stop_pose_ptr_ == nullptr) { - const auto p = calcOffsetPose(*slow_down_end_pose_ptr_, base_link2front_, 0.0, 0.0); + const auto p = calc_offset_pose(*slow_down_end_pose_ptr_, base_link2front_, 0.0, 0.0); auto markers = createSlowDownVirtualWallMarker(p, "slow down\nend", current_time, 2); markers.markers.front().ns = "slow_down_end_virtual_wall"; markers.markers.back().ns = "slow_down_end_factor_text"; - appendMarkerArray(markers, &msg); + append_marker_array(markers, &msg); } return msg; @@ -275,64 +275,64 @@ MarkerArray ObstacleStopPlannerDebugNode::makeVisualizationMarker() // cube if (!vehicle_polyhedrons_.empty()) { - auto marker = createDefaultMarker( + auto marker = create_default_marker( "map", current_time, "detection_cubes", 0, Marker::LINE_LIST, - createMarkerScale(0.01, 0.0, 0.0), createMarkerColor(0.0, 1.0, 0.0, 0.999)); + create_marker_scale(0.01, 0.0, 0.0), create_marker_color(0.0, 1.0, 0.0, 0.999)); for (size_t i = 0; i < vehicle_polyhedrons_.size(); ++i) { for (size_t j = 0; j < vehicle_polyhedrons_.at(i).size(); ++j) { const auto & p = vehicle_polyhedrons_.at(i).at(j); - marker.points.push_back(createPoint(p.x(), p.y(), p.z())); + marker.points.push_back(create_point(p.x(), p.y(), p.z())); } } for (size_t i = 0; i < vehicle_polyhedrons_.size(); ++i) { for (size_t j = 0; j + 2 < vehicle_polyhedrons_.at(i).size(); ++j) { const auto & p = vehicle_polyhedrons_.at(i).at(j); - marker.points.push_back(createPoint(p.x(), p.y(), p.z())); + marker.points.push_back(create_point(p.x(), p.y(), p.z())); const auto & p1 = vehicle_polyhedrons_.at(i).at(j + 2); - marker.points.push_back(createPoint(p1.x(), p1.y(), p1.z())); + marker.points.push_back(create_point(p1.x(), p1.y(), p1.z())); } const auto & p = vehicle_polyhedrons_.at(i).at(1); - marker.points.push_back(createPoint(p.x(), p.y(), p.z())); + marker.points.push_back(create_point(p.x(), p.y(), p.z())); const auto & p1 = vehicle_polyhedrons_.at(i).at(vehicle_polyhedrons_.at(i).size() - 1); - marker.points.push_back(createPoint(p1.x(), p1.y(), p1.z())); + marker.points.push_back(create_point(p1.x(), p1.y(), p1.z())); const auto & p2 = vehicle_polyhedrons_.at(i).at(0); - marker.points.push_back(createPoint(p2.x(), p2.y(), p2.z())); + marker.points.push_back(create_point(p2.x(), p2.y(), p2.z())); const auto & p3 = vehicle_polyhedrons_.at(i).at(vehicle_polyhedrons_.at(i).size() - 2); - marker.points.push_back(createPoint(p3.x(), p3.y(), p3.z())); + marker.points.push_back(create_point(p3.x(), p3.y(), p3.z())); } msg.markers.push_back(marker); } if (!collision_polyhedrons_.empty()) { - auto marker = createDefaultMarker( + auto marker = create_default_marker( "map", current_time, "collision_cubes", 0, Marker::LINE_LIST, - createMarkerScale(0.05, 0.0, 0.0), createMarkerColor(1.0, 0.0, 0.0, 0.999)); + create_marker_scale(0.05, 0.0, 0.0), create_marker_color(1.0, 0.0, 0.0, 0.999)); for (size_t i = 0; i < collision_polyhedrons_.size(); ++i) { for (size_t j = 0; j < collision_polyhedrons_.at(i).size(); ++j) { const auto & p = collision_polyhedrons_.at(i).at(j); - marker.points.push_back(createPoint(p.x(), p.y(), p.z())); + marker.points.push_back(create_point(p.x(), p.y(), p.z())); } } for (size_t i = 0; i < collision_polyhedrons_.size(); ++i) { for (size_t j = 0; j + 2 < collision_polyhedrons_.at(i).size(); ++j) { const auto & p = collision_polyhedrons_.at(i).at(j); - marker.points.push_back(createPoint(p.x(), p.y(), p.z())); + marker.points.push_back(create_point(p.x(), p.y(), p.z())); const auto & p1 = collision_polyhedrons_.at(i).at(j + 2); - marker.points.push_back(createPoint(p1.x(), p1.y(), p1.z())); + marker.points.push_back(create_point(p1.x(), p1.y(), p1.z())); } const auto & p = collision_polyhedrons_.at(i).at(1); - marker.points.push_back(createPoint(p.x(), p.y(), p.z())); + marker.points.push_back(create_point(p.x(), p.y(), p.z())); const auto & p1 = collision_polyhedrons_.at(i).at(collision_polyhedrons_.at(i).size() - 1); - marker.points.push_back(createPoint(p1.x(), p1.y(), p1.z())); + marker.points.push_back(create_point(p1.x(), p1.y(), p1.z())); const auto & p2 = collision_polyhedrons_.at(i).at(0); - marker.points.push_back(createPoint(p2.x(), p2.y(), p2.z())); + marker.points.push_back(create_point(p2.x(), p2.y(), p2.z())); const auto & p3 = collision_polyhedrons_.at(i).at(collision_polyhedrons_.at(i).size() - 2); - marker.points.push_back(createPoint(p3.x(), p3.y(), p3.z())); + marker.points.push_back(create_point(p3.x(), p3.y(), p3.z())); } msg.markers.push_back(marker); @@ -340,22 +340,22 @@ MarkerArray ObstacleStopPlannerDebugNode::makeVisualizationMarker() // polygon if (!vehicle_polygons_.empty()) { - auto marker = createDefaultMarker( + auto marker = create_default_marker( "map", current_time, "detection_polygons", 0, Marker::LINE_LIST, - createMarkerScale(0.01, 0.0, 0.0), createMarkerColor(0.0, 1.0, 0.0, 0.999)); + create_marker_scale(0.01, 0.0, 0.0), create_marker_color(0.0, 1.0, 0.0, 0.999)); for (size_t i = 0; i < vehicle_polygons_.size(); ++i) { for (size_t j = 0; j < vehicle_polygons_.at(i).size(); ++j) { { const auto & p = vehicle_polygons_.at(i).at(j); - marker.points.push_back(createPoint(p.x(), p.y(), p.z())); + marker.points.push_back(create_point(p.x(), p.y(), p.z())); } if (j + 1 == vehicle_polygons_.at(i).size()) { const auto & p = vehicle_polygons_.at(i).at(0); - marker.points.push_back(createPoint(p.x(), p.y(), p.z())); + marker.points.push_back(create_point(p.x(), p.y(), p.z())); } else { const auto & p = vehicle_polygons_.at(i).at(j + 1); - marker.points.push_back(createPoint(p.x(), p.y(), p.z())); + marker.points.push_back(create_point(p.x(), p.y(), p.z())); } } } @@ -363,22 +363,22 @@ MarkerArray ObstacleStopPlannerDebugNode::makeVisualizationMarker() } if (!collision_polygons_.empty()) { - auto marker = createDefaultMarker( + auto marker = create_default_marker( "map", current_time, "collision_polygons", 0, Marker::LINE_LIST, - createMarkerScale(0.05, 0.0, 0.0), createMarkerColor(1.0, 0.0, 0.0, 0.999)); + create_marker_scale(0.05, 0.0, 0.0), create_marker_color(1.0, 0.0, 0.0, 0.999)); for (size_t i = 0; i < collision_polygons_.size(); ++i) { for (size_t j = 0; j < collision_polygons_.at(i).size(); ++j) { { const auto & p = collision_polygons_.at(i).at(j); - marker.points.push_back(createPoint(p.x(), p.y(), p.z())); + marker.points.push_back(create_point(p.x(), p.y(), p.z())); } if (j + 1 == collision_polygons_.at(i).size()) { const auto & p = collision_polygons_.at(i).at(0); - marker.points.push_back(createPoint(p.x(), p.y(), p.z())); + marker.points.push_back(create_point(p.x(), p.y(), p.z())); } else { const auto & p = collision_polygons_.at(i).at(j + 1); - marker.points.push_back(createPoint(p.x(), p.y(), p.z())); + marker.points.push_back(create_point(p.x(), p.y(), p.z())); } } } @@ -386,22 +386,22 @@ MarkerArray ObstacleStopPlannerDebugNode::makeVisualizationMarker() } if (!slow_down_range_polygons_.empty()) { - auto marker = createDefaultMarker( + auto marker = create_default_marker( "map", current_time, "slow_down_detection_polygons", 0, Marker::LINE_LIST, - createMarkerScale(0.01, 0.0, 0.0), createMarkerColor(0.0, 1.0, 0.0, 0.999)); + create_marker_scale(0.01, 0.0, 0.0), create_marker_color(0.0, 1.0, 0.0, 0.999)); for (size_t i = 0; i < slow_down_range_polygons_.size(); ++i) { for (size_t j = 0; j < slow_down_range_polygons_.at(i).size(); ++j) { { const auto & p = slow_down_range_polygons_.at(i).at(j); - marker.points.push_back(createPoint(p.x(), p.y(), p.z())); + marker.points.push_back(create_point(p.x(), p.y(), p.z())); } if (j + 1 == slow_down_range_polygons_.at(i).size()) { const auto & p = slow_down_range_polygons_.at(i).at(0); - marker.points.push_back(createPoint(p.x(), p.y(), p.z())); + marker.points.push_back(create_point(p.x(), p.y(), p.z())); } else { const auto & p = slow_down_range_polygons_.at(i).at(j + 1); - marker.points.push_back(createPoint(p.x(), p.y(), p.z())); + marker.points.push_back(create_point(p.x(), p.y(), p.z())); } } } @@ -409,22 +409,22 @@ MarkerArray ObstacleStopPlannerDebugNode::makeVisualizationMarker() } if (!slow_down_polygons_.empty()) { - auto marker = createDefaultMarker( + auto marker = create_default_marker( "map", current_time, "slow_down_polygons", 0, Marker::LINE_LIST, - createMarkerScale(0.05, 0.0, 0.0), createMarkerColor(1.0, 1.0, 0.0, 0.999)); + create_marker_scale(0.05, 0.0, 0.0), create_marker_color(1.0, 1.0, 0.0, 0.999)); for (size_t i = 0; i < slow_down_polygons_.size(); ++i) { for (size_t j = 0; j < slow_down_polygons_.at(i).size(); ++j) { { const auto & p = slow_down_polygons_.at(i).at(j); - marker.points.push_back(createPoint(p.x(), p.y(), p.z())); + marker.points.push_back(create_point(p.x(), p.y(), p.z())); } if (j + 1 == slow_down_polygons_.at(i).size()) { const auto & p = slow_down_polygons_.at(i).at(0); - marker.points.push_back(createPoint(p.x(), p.y(), p.z())); + marker.points.push_back(create_point(p.x(), p.y(), p.z())); } else { const auto & p = slow_down_polygons_.at(i).at(j + 1); - marker.points.push_back(createPoint(p.x(), p.y(), p.z())); + marker.points.push_back(create_point(p.x(), p.y(), p.z())); } } } @@ -432,22 +432,22 @@ MarkerArray ObstacleStopPlannerDebugNode::makeVisualizationMarker() } if (!obstacle_polygons_.empty()) { - auto marker = createDefaultMarker( + auto marker = create_default_marker( "map", current_time, "obstacle_polygons", 0, Marker::LINE_LIST, - createMarkerScale(0.05, 0.0, 0.0), createMarkerColor(1.0, 1.0, 0.0, 0.999)); + create_marker_scale(0.05, 0.0, 0.0), create_marker_color(1.0, 1.0, 0.0, 0.999)); for (size_t i = 0; i < obstacle_polygons_.size(); ++i) { for (size_t j = 0; j < obstacle_polygons_.at(i).size(); ++j) { { const auto & p = obstacle_polygons_.at(i).at(j); - marker.points.push_back(createPoint(p.x(), p.y(), p.z())); + marker.points.push_back(create_point(p.x(), p.y(), p.z())); } if (j + 1 == obstacle_polygons_.at(i).size()) { const auto & p = obstacle_polygons_.at(i).at(0); - marker.points.push_back(createPoint(p.x(), p.y(), p.z())); + marker.points.push_back(create_point(p.x(), p.y(), p.z())); } else { const auto & p = obstacle_polygons_.at(i).at(j + 1); - marker.points.push_back(createPoint(p.x(), p.y(), p.z())); + marker.points.push_back(create_point(p.x(), p.y(), p.z())); } } } @@ -455,25 +455,25 @@ MarkerArray ObstacleStopPlannerDebugNode::makeVisualizationMarker() } if (target_stop_pose_ptr_ != nullptr) { - const auto p = calcOffsetPose(*target_stop_pose_ptr_, base_link2front_, 0.0, 0.0); + const auto p = calc_offset_pose(*target_stop_pose_ptr_, base_link2front_, 0.0, 0.0); const auto markers = createStopVirtualWallMarker(p, "obstacle_stop_target_stop_line", current_time, 0); - appendMarkerArray(markers, &msg); + append_marker_array(markers, &msg); } else { const auto markers = createDeletedStopVirtualWallMarker(current_time, 0); - appendMarkerArray(markers, &msg); + append_marker_array(markers, &msg); } if (stop_obstacle_point_ptr_ != nullptr) { - auto marker1 = createDefaultMarker( + auto marker1 = create_default_marker( "map", current_time, "stop_obstacle_point", 0, Marker::SPHERE, - createMarkerScale(0.25, 0.25, 0.25), createMarkerColor(1.0, 0.0, 0.0, 0.999)); + create_marker_scale(0.25, 0.25, 0.25), create_marker_color(1.0, 0.0, 0.0, 0.999)); marker1.pose.position = *stop_obstacle_point_ptr_; msg.markers.push_back(marker1); - auto marker2 = createDefaultMarker( + auto marker2 = create_default_marker( "map", current_time, "stop_obstacle_text", 0, Marker::TEXT_VIEW_FACING, - createMarkerScale(0.0, 0.0, 1.0), createMarkerColor(1.0, 1.0, 1.0, 0.999)); + create_marker_scale(0.0, 0.0, 1.0), create_marker_color(1.0, 1.0, 1.0, 0.999)); marker2.pose.position = *stop_obstacle_point_ptr_; marker2.pose.position.z += 2.0; marker2.text = "!"; @@ -481,15 +481,15 @@ MarkerArray ObstacleStopPlannerDebugNode::makeVisualizationMarker() } if (slow_down_obstacle_point_ptr_ != nullptr) { - auto marker1 = createDefaultMarker( + auto marker1 = create_default_marker( "map", current_time, "slow_down_obstacle_point", 0, Marker::SPHERE, - createMarkerScale(0.25, 0.25, 0.25), createMarkerColor(1.0, 0.0, 0.0, 0.999)); + create_marker_scale(0.25, 0.25, 0.25), create_marker_color(1.0, 0.0, 0.0, 0.999)); marker1.pose.position = *slow_down_obstacle_point_ptr_; msg.markers.push_back(marker1); - auto marker2 = createDefaultMarker( + auto marker2 = create_default_marker( "map", current_time, "slow_down_obstacle_text", 0, Marker::TEXT_VIEW_FACING, - createMarkerScale(0.0, 0.0, 1.0), createMarkerColor(1.0, 1.0, 1.0, 0.999)); + create_marker_scale(0.0, 0.0, 1.0), create_marker_color(1.0, 1.0, 1.0, 0.999)); marker2.pose.position = *slow_down_obstacle_point_ptr_; marker2.pose.position.z += 2.0; marker2.text = "!"; diff --git a/planning/autoware_obstacle_stop_planner/src/debug_marker.hpp b/planning/autoware_obstacle_stop_planner/src/debug_marker.hpp index 3b8b57e7de4c6..4ef6b4ecc8798 100644 --- a/planning/autoware_obstacle_stop_planner/src/debug_marker.hpp +++ b/planning/autoware_obstacle_stop_planner/src/debug_marker.hpp @@ -31,7 +31,7 @@ #define EIGEN_MPL2_ONLY #include #include -#include +#include namespace autoware::motion_planning { @@ -94,10 +94,10 @@ class ObstacleStopPlannerDebugNode explicit ObstacleStopPlannerDebugNode(rclcpp::Node * node, const double base_link2front); ~ObstacleStopPlannerDebugNode() {} bool pushPolygon( - const autoware::universe_utils::Polygon2d & polygon, const double z, const PolygonType & type); + const autoware_utils::Polygon2d & polygon, const double z, const PolygonType & type); bool pushPolygon(const std::vector & polygon, const PolygonType & type); bool pushPolyhedron( - const autoware::universe_utils::Polygon2d & polyhedron, const double z_min, const double z_max, + const autoware_utils::Polygon2d & polyhedron, const double z_min, const double z_max, const PolygonType & type); bool pushPolyhedron(const std::vector & polyhedron, const PolygonType & type); bool pushPose(const Pose & pose, const PoseType & type); diff --git a/planning/autoware_obstacle_stop_planner/src/node.cpp b/planning/autoware_obstacle_stop_planner/src/node.cpp index dde218c94e77c..2285723465e2b 100644 --- a/planning/autoware_obstacle_stop_planner/src/node.cpp +++ b/planning/autoware_obstacle_stop_planner/src/node.cpp @@ -45,12 +45,12 @@ using autoware::motion_utils::calcLongitudinalOffsetToSegment; using autoware::motion_utils::calcSignedArcLength; using autoware::motion_utils::findFirstNearestIndexWithSoftConstraints; using autoware::motion_utils::findFirstNearestSegmentIndexWithSoftConstraints; -using autoware::universe_utils::calcDistance2d; -using autoware::universe_utils::createPoint; -using autoware::universe_utils::getPoint; -using autoware::universe_utils::getPose; -using autoware::universe_utils::getRPY; using autoware_perception_msgs::msg::PredictedObject; +using autoware_utils::calc_distance2d; +using autoware_utils::create_point; +using autoware_utils::get_point; +using autoware_utils::get_pose; +using autoware_utils::get_rpy; ObstacleStopPlannerNode::ObstacleStopPlannerNode(const rclcpp::NodeOptions & node_options) : Node("obstacle_stop_planner", node_options) @@ -245,9 +245,8 @@ ObstacleStopPlannerNode::ObstacleStopPlannerNode(const rclcpp::NodeOptions & nod std::bind(&ObstacleStopPlannerNode::onExpandStopRange, this, std::placeholders::_1), createSubscriptionOptions(this)); - logger_configure_ = std::make_unique(this); - published_time_publisher_ = - std::make_unique(this); + logger_configure_ = std::make_unique(this); + published_time_publisher_ = std::make_unique(this); } void ObstacleStopPlannerNode::onPointCloud(const PointCloud2::ConstSharedPtr input_msg) @@ -1004,7 +1003,7 @@ void ObstacleStopPlannerNode::insertVelocity( index_with_dist_remain = findNearestFrontIndex( std::min(idx, traj_end_idx), output, - createPoint( + create_point( planner_data.nearest_collision_point.x, planner_data.nearest_collision_point.y, 0)); if (index_with_dist_remain) { @@ -1037,7 +1036,7 @@ void ObstacleStopPlannerNode::insertVelocity( size_t stop_seg_idx = 0; if (stop_point.index < output.size() - 1) { const double lon_offset = - calcLongitudinalOffsetToSegment(output, stop_point.index, getPoint(stop_point.point)); + calcLongitudinalOffsetToSegment(output, stop_point.index, get_point(stop_point.point)); if (lon_offset < 0) { stop_seg_idx = std::max(static_cast(0), stop_point.index - 1); } else { @@ -1048,7 +1047,7 @@ void ObstacleStopPlannerNode::insertVelocity( } return calcSignedArcLength( - output, ego_pose.position, ego_seg_idx, getPoint(stop_point.point), stop_seg_idx); + output, ego_pose.position, ego_seg_idx, get_point(stop_point.point), stop_seg_idx); }(); const auto is_stopped = current_vel < 0.01; @@ -1065,15 +1064,15 @@ void ObstacleStopPlannerNode::insertVelocity( insertStopPoint(current_stop_pos, output, planner_data.stop_reason_diag); - debug_ptr_->pushPose(getPose(stop_point.point), PoseType::TargetStop); - debug_ptr_->pushPose(getPose(current_stop_pos.point), PoseType::Stop); + debug_ptr_->pushPose(get_pose(stop_point.point), PoseType::TargetStop); + debug_ptr_->pushPose(get_pose(current_stop_pos.point), PoseType::Stop); } } else { insertStopPoint(stop_point, output, planner_data.stop_reason_diag); - debug_ptr_->pushPose(getPose(stop_point.point), PoseType::TargetStop); - debug_ptr_->pushPose(getPose(stop_point.point), PoseType::Stop); + debug_ptr_->pushPose(get_pose(stop_point.point), PoseType::TargetStop); + debug_ptr_->pushPose(get_pose(stop_point.point), PoseType::Stop); } } } @@ -1085,7 +1084,7 @@ void ObstacleStopPlannerNode::insertVelocity( planner_data.decimate_trajectory_slow_down_index); const auto index_with_dist_remain = findNearestFrontIndex( std::min(idx, traj_end_idx), output, - createPoint( + create_point( planner_data.nearest_slow_down_point.x, planner_data.nearest_slow_down_point.y, 0)); if (index_with_dist_remain) { @@ -1361,9 +1360,9 @@ void ObstacleStopPlannerNode::insertSlowDownSection( const auto is_valid_index_start = checkValidIndex(p_base_start.pose, p_next_start.pose, p_insert_start.pose); const auto is_start_p_base_and_p_insert_overlap = - calcDistance2d(p_base_start, p_insert_start) < min_dist; + calc_distance2d(p_base_start, p_insert_start) < min_dist; const auto is_start_p_next_and_p_insert_overlap = - calcDistance2d(p_next_start, p_insert_start) < min_dist; + calc_distance2d(p_next_start, p_insert_start) < min_dist; auto update_start_idx = start_idx; auto update_end_idx = end_idx; @@ -1381,9 +1380,9 @@ void ObstacleStopPlannerNode::insertSlowDownSection( } const auto is_end_p_base_and_p_insert_overlap = - calcDistance2d(p_base_end, p_insert_end) < min_dist; + calc_distance2d(p_base_end, p_insert_end) < min_dist; const auto is_end_p_next_and_p_insert_overlap = - calcDistance2d(p_next_end, p_insert_end) < min_dist; + calc_distance2d(p_next_end, p_insert_end) < min_dist; const auto is_valid_index_end = checkValidIndex(p_base_end.pose, p_next_end.pose, p_insert_end.pose); @@ -1559,8 +1558,8 @@ void ObstacleStopPlannerNode::filterObstacles( const double max_length = calcObstacleMaxLength(object.shape); const size_t seg_idx = autoware::motion_utils::findNearestSegmentIndex( traj, object.kinematics.initial_pose_with_covariance.pose.position); - const auto p_front = autoware::universe_utils::getPoint(traj.at(seg_idx)); - const auto p_back = autoware::universe_utils::getPoint(traj.at(seg_idx + 1)); + const auto p_front = autoware_utils::get_point(traj.at(seg_idx)); + const auto p_back = autoware_utils::get_point(traj.at(seg_idx + 1)); const auto & p_target = object.kinematics.initial_pose_with_covariance.pose.position; const Eigen::Vector3d segment_vec{p_back.x - p_front.x, p_back.y - p_front.y, 0.0}; const Eigen::Vector3d target_vec{p_target.x - p_front.x, p_target.y - p_front.y, 0.0}; diff --git a/planning/autoware_obstacle_stop_planner/src/node.hpp b/planning/autoware_obstacle_stop_planner/src/node.hpp index a48c8c9ba53ac..8976591655a33 100644 --- a/planning/autoware_obstacle_stop_planner/src/node.hpp +++ b/planning/autoware_obstacle_stop_planner/src/node.hpp @@ -16,16 +16,16 @@ #define NODE_HPP_ #include "adaptive_cruise_control.hpp" -#include "autoware/universe_utils/ros/logger_level_configure.hpp" -#include "autoware/universe_utils/system/stop_watch.hpp" +#include "autoware_utils/ros/logger_level_configure.hpp" +#include "autoware_utils/system/stop_watch.hpp" #include "debug_marker.hpp" #include "planner_data.hpp" #include #include -#include -#include -#include +#include +#include +#include #include #include #include @@ -78,9 +78,6 @@ using nav_msgs::msg::Odometry; using sensor_msgs::msg::PointCloud2; using std_msgs::msg::Header; -using autoware::universe_utils::Point2d; -using autoware::universe_utils::Polygon2d; -using autoware::universe_utils::StopWatch; using autoware::vehicle_info_utils::VehicleInfo; using autoware_internal_debug_msgs::msg::BoolStamped; using autoware_internal_debug_msgs::msg::Float32MultiArrayStamped; @@ -89,6 +86,9 @@ using autoware_internal_debug_msgs::msg::Float64Stamped; using autoware_perception_msgs::msg::PredictedObjects; using autoware_planning_msgs::msg::Trajectory; using autoware_planning_msgs::msg::TrajectoryPoint; +using autoware_utils::Point2d; +using autoware_utils::Polygon2d; +using autoware_utils::StopWatch; using tier4_planning_msgs::msg::ExpandStopRange; using tier4_planning_msgs::msg::VelocityLimit; using tier4_planning_msgs::msg::VelocityLimitClearCommand; @@ -316,9 +316,9 @@ class ObstacleStopPlannerNode : public rclcpp::Node return ret; } - std::unique_ptr logger_configure_; + std::unique_ptr logger_configure_; - std::unique_ptr published_time_publisher_; + std::unique_ptr published_time_publisher_; }; } // namespace autoware::motion_planning diff --git a/planning/autoware_obstacle_stop_planner/src/planner_utils.cpp b/planning/autoware_obstacle_stop_planner/src/planner_utils.cpp index 91f84520e0cd9..050592721fcf7 100644 --- a/planning/autoware_obstacle_stop_planner/src/planner_utils.cpp +++ b/planning/autoware_obstacle_stop_planner/src/planner_utils.cpp @@ -17,8 +17,8 @@ #include #include #include -#include -#include +#include +#include #include @@ -42,10 +42,10 @@ namespace autoware::motion_planning using autoware::motion_utils::calcDecelDistWithJerkAndAccConstraints; using autoware::motion_utils::findFirstNearestIndexWithSoftConstraints; using autoware::motion_utils::findFirstNearestSegmentIndexWithSoftConstraints; -using autoware::universe_utils::calcDistance2d; -using autoware::universe_utils::getRPY; using autoware_perception_msgs::msg::PredictedObject; using autoware_perception_msgs::msg::PredictedObjects; +using autoware_utils::calc_distance2d; +using autoware_utils::get_rpy; std::optional> calcFeasibleMarginAndVelocity( const SlowDownParam & slow_down_param, const double dist_baselink_to_obstacle, @@ -127,7 +127,7 @@ std::optional> getForwardInsertPointFromBaseP const auto & p_front = trajectory.at(i); const auto & p_back = trajectory.at(i + 1); - length_sum += calcDistance2d(p_front, p_back); + length_sum += calc_distance2d(p_front, p_back); length_residual = length_sum - margin; if (length_residual >= std::numeric_limits::epsilon()) { @@ -163,7 +163,7 @@ std::optional> getBackwardInsertPointFromBase const auto & p_front = trajectory.at(i - 1); const auto & p_back = trajectory.at(i); - length_sum += calcDistance2d(p_front, p_back); + length_sum += calc_distance2d(p_front, p_back); length_residual = length_sum - margin; if (length_residual >= std::numeric_limits::epsilon()) { @@ -187,7 +187,7 @@ std::optional> findNearestFrontIndex( { for (size_t i = start_idx; i < trajectory.size(); ++i) { const auto & p_traj = trajectory.at(i).pose; - const auto yaw = getRPY(p_traj).z; + const auto yaw = get_rpy(p_traj).z; const Point2d p_traj_direction(std::cos(yaw), std::sin(yaw)); const Point2d p_traj_to_target(point.x - p_traj.position.x, point.y - p_traj.position.y); @@ -205,7 +205,7 @@ std::optional> findNearestFrontIndex( bool isInFrontOfTargetPoint(const Pose & pose, const Point & point) { - const auto yaw = getRPY(pose).z; + const auto yaw = get_rpy(pose).z; const Point2d pose_direction(std::cos(yaw), std::sin(yaw)); const Point2d to_target(point.x - pose.position.x, point.y - pose.position.y); @@ -334,40 +334,35 @@ void createOneStepPolygon( { // base step appendPointToPolygon( polygon, - autoware::universe_utils::calcOffsetPose(base_step_pose, longitudinal_offset, width, 0.0) - .position); + autoware_utils::calc_offset_pose(base_step_pose, longitudinal_offset, width, 0.0).position); appendPointToPolygon( polygon, - autoware::universe_utils::calcOffsetPose(base_step_pose, longitudinal_offset, -width, 0.0) - .position); + autoware_utils::calc_offset_pose(base_step_pose, longitudinal_offset, -width, 0.0).position); appendPointToPolygon( - polygon, autoware::universe_utils::calcOffsetPose(base_step_pose, -rear_overhang, -width, 0.0) - .position); + polygon, + autoware_utils::calc_offset_pose(base_step_pose, -rear_overhang, -width, 0.0).position); appendPointToPolygon( - polygon, autoware::universe_utils::calcOffsetPose(base_step_pose, -rear_overhang, width, 0.0) - .position); + polygon, + autoware_utils::calc_offset_pose(base_step_pose, -rear_overhang, width, 0.0).position); } { // next step appendPointToPolygon( polygon, - autoware::universe_utils::calcOffsetPose(next_step_pose, longitudinal_offset, width, 0.0) - .position); + autoware_utils::calc_offset_pose(next_step_pose, longitudinal_offset, width, 0.0).position); appendPointToPolygon( polygon, - autoware::universe_utils::calcOffsetPose(next_step_pose, longitudinal_offset, -width, 0.0) - .position); + autoware_utils::calc_offset_pose(next_step_pose, longitudinal_offset, -width, 0.0).position); appendPointToPolygon( - polygon, autoware::universe_utils::calcOffsetPose(next_step_pose, -rear_overhang, -width, 0.0) - .position); + polygon, + autoware_utils::calc_offset_pose(next_step_pose, -rear_overhang, -width, 0.0).position); appendPointToPolygon( - polygon, autoware::universe_utils::calcOffsetPose(next_step_pose, -rear_overhang, width, 0.0) - .position); + polygon, + autoware_utils::calc_offset_pose(next_step_pose, -rear_overhang, width, 0.0).position); } - polygon = autoware::universe_utils::isClockwise(polygon) - ? polygon - : autoware::universe_utils::inverseClockwise(polygon); + polygon = + autoware_utils::is_clockwise(polygon) ? polygon : autoware_utils::inverse_clockwise(polygon); boost::geometry::convex_hull(polygon, hull_polygon); } @@ -384,8 +379,8 @@ void insertStopPoint( constexpr double min_dist = 1e-3; - const auto is_p_base_and_p_insert_overlap = calcDistance2d(p_base, p_insert) < min_dist; - const auto is_p_next_and_p_insert_overlap = calcDistance2d(p_next, p_insert) < min_dist; + const auto is_p_base_and_p_insert_overlap = calc_distance2d(p_base, p_insert) < min_dist; + const auto is_p_next_and_p_insert_overlap = calc_distance2d(p_next, p_insert) < min_dist; const auto is_valid_index = checkValidIndex(p_base.pose, p_next.pose, p_insert.pose); auto update_stop_idx = stop_idx; @@ -451,7 +446,7 @@ TrajectoryPoints decimateTrajectory( continue; } - trajectory_length_sum += calcDistance2d(p_front, p_back); + trajectory_length_sum += calc_distance2d(p_front, p_back); } if (!input.empty()) { output.push_back(input.back()); @@ -516,7 +511,7 @@ void getNearestPoint( { double min_norm = 0.0; bool is_init = false; - const auto yaw = getRPY(base_pose).z; + const auto yaw = get_rpy(base_pose).z; const Eigen::Vector2d base_pose_vec(std::cos(yaw), std::sin(yaw)); for (const auto & p : pointcloud) { @@ -536,7 +531,7 @@ void getLateralNearestPoint( double * deviation) { double min_norm = std::numeric_limits::max(); - const auto yaw = getRPY(base_pose).z; + const auto yaw = get_rpy(base_pose).z; const Eigen::Vector2d base_pose_vec(std::cos(yaw), std::sin(yaw)); for (size_t i = 0; i < pointcloud.size(); ++i) { const Eigen::Vector2d pointcloud_vec( @@ -559,7 +554,7 @@ double getNearestPointAndDistanceForPredictedObject( bool is_init = false; for (const auto & p : points.poses) { - double norm = autoware::universe_utils::calcDistance2d(p, base_pose); + double norm = autoware_utils::calc_distance2d(p, base_pose); if (norm < min_norm || !is_init) { min_norm = norm; *nearest_collision_point = p.position; @@ -575,7 +570,7 @@ void getLateralNearestPointForPredictedObject( { double min_norm = std::numeric_limits::max(); for (const auto & pose : object.poses) { - double norm = calcDistance2d(pose, base_pose); + double norm = calc_distance2d(pose, base_pose); if (norm < min_norm) { min_norm = norm; *lateral_nearest_point = pointToPcl(pose.position.x, pose.position.y, base_pose.position.z); @@ -587,7 +582,7 @@ void getLateralNearestPointForPredictedObject( Pose getVehicleCenterFromBase(const Pose & base_pose, const VehicleInfo & vehicle_info) { const auto & i = vehicle_info; - const auto yaw = getRPY(base_pose).z; + const auto yaw = get_rpy(base_pose).z; Pose center_pose; center_pose.position.x = @@ -634,9 +629,9 @@ Polygon2d convertBoundingBoxObjectToGeometryPolygon( object_polygon.outer().emplace_back(p4_obj.x(), p4_obj.y()); object_polygon.outer().push_back(object_polygon.outer().front()); - object_polygon = autoware::universe_utils::isClockwise(object_polygon) + object_polygon = autoware_utils::is_clockwise(object_polygon) ? object_polygon - : autoware::universe_utils::inverseClockwise(object_polygon); + : autoware_utils::inverse_clockwise(object_polygon); return object_polygon; } @@ -657,9 +652,9 @@ Polygon2d convertCylindricalObjectToGeometryPolygon( } object_polygon.outer().push_back(object_polygon.outer().front()); - object_polygon = autoware::universe_utils::isClockwise(object_polygon) + object_polygon = autoware_utils::is_clockwise(object_polygon) ? object_polygon - : autoware::universe_utils::inverseClockwise(object_polygon); + : autoware_utils::inverse_clockwise(object_polygon); return object_polygon; } @@ -677,9 +672,9 @@ Polygon2d convertPolygonObjectToGeometryPolygon( object_polygon.outer().emplace_back(tf_obj.x(), tf_obj.y()); } object_polygon.outer().push_back(object_polygon.outer().front()); - object_polygon = autoware::universe_utils::isClockwise(object_polygon) + object_polygon = autoware_utils::is_clockwise(object_polygon) ? object_polygon - : autoware::universe_utils::inverseClockwise(object_polygon); + : autoware_utils::inverse_clockwise(object_polygon); return object_polygon; } @@ -699,7 +694,7 @@ std::optional getObstacleFromUuid( bool isFrontObstacle(const Pose & ego_pose, const geometry_msgs::msg::Point & obstacle_pos) { - const auto yaw = autoware::universe_utils::getRPY(ego_pose).z; + const auto yaw = autoware_utils::get_rpy(ego_pose).z; const Eigen::Vector2d base_pose_vec(std::cos(yaw), std::sin(yaw)); const Eigen::Vector2d obstacle_vec( obstacle_pos.x - ego_pose.position.x, obstacle_pos.y - ego_pose.position.y); diff --git a/planning/autoware_obstacle_stop_planner/src/planner_utils.hpp b/planning/autoware_obstacle_stop_planner/src/planner_utils.hpp index 228c71d08a9b9..a1a62f6a0c8a0 100644 --- a/planning/autoware_obstacle_stop_planner/src/planner_utils.hpp +++ b/planning/autoware_obstacle_stop_planner/src/planner_utils.hpp @@ -18,7 +18,7 @@ #include "planner_data.hpp" -#include +#include #include #include @@ -45,10 +45,10 @@ using geometry_msgs::msg::Pose; using geometry_msgs::msg::PoseArray; using std_msgs::msg::Header; -using autoware::universe_utils::Point2d; -using autoware::universe_utils::Polygon2d; using autoware::vehicle_info_utils::VehicleInfo; using autoware_planning_msgs::msg::TrajectoryPoint; +using autoware_utils::Point2d; +using autoware_utils::Polygon2d; using TrajectoryPoints = std::vector; using PointCloud = pcl::PointCloud; diff --git a/planning/autoware_path_optimizer/include/autoware/path_optimizer/common_structs.hpp b/planning/autoware_path_optimizer/include/autoware/path_optimizer/common_structs.hpp index 399262f93e19d..af677d654b077 100644 --- a/planning/autoware_path_optimizer/include/autoware/path_optimizer/common_structs.hpp +++ b/planning/autoware_path_optimizer/include/autoware/path_optimizer/common_structs.hpp @@ -16,7 +16,7 @@ #define AUTOWARE__PATH_OPTIMIZER__COMMON_STRUCTS_HPP_ #include "autoware/path_optimizer/type_alias.hpp" -#include "autoware/universe_utils/ros/update_param.hpp" +#include "autoware_utils/ros/update_param.hpp" #include "rclcpp/rclcpp.hpp" #include @@ -70,12 +70,12 @@ struct TrajectoryParam void onParam(const std::vector & parameters) { - using autoware::universe_utils::updateParam; + using autoware_utils::update_param; // common - updateParam( + update_param( parameters, "common.output_backward_traj_length", output_backward_traj_length); - updateParam(parameters, "common.output_delta_arc_length", output_delta_arc_length); + update_param(parameters, "common.output_delta_arc_length", output_delta_arc_length); } double output_delta_arc_length; @@ -93,9 +93,9 @@ struct EgoNearestParam void onParam(const std::vector & parameters) { - using autoware::universe_utils::updateParam; - updateParam(parameters, "ego_nearest_dist_threshold", dist_threshold); - updateParam(parameters, "ego_nearest_yaw_threshold", yaw_threshold); + using autoware_utils::update_param; + update_param(parameters, "ego_nearest_dist_threshold", dist_threshold); + update_param(parameters, "ego_nearest_yaw_threshold", yaw_threshold); } double dist_threshold{0.0}; diff --git a/planning/autoware_path_optimizer/include/autoware/path_optimizer/debug_marker.hpp b/planning/autoware_path_optimizer/include/autoware/path_optimizer/debug_marker.hpp index 487320330f953..d6b197a10050e 100644 --- a/planning/autoware_path_optimizer/include/autoware/path_optimizer/debug_marker.hpp +++ b/planning/autoware_path_optimizer/include/autoware/path_optimizer/debug_marker.hpp @@ -16,8 +16,8 @@ #include "autoware/path_optimizer/common_structs.hpp" #include "autoware/path_optimizer/type_alias.hpp" -#include "autoware/universe_utils/geometry/geometry.hpp" -#include "autoware/universe_utils/ros/marker_helper.hpp" +#include "autoware_utils/geometry/geometry.hpp" +#include "autoware_utils/ros/marker_helper.hpp" #include "autoware_vehicle_info_utils/vehicle_info_utils.hpp" #include "rclcpp/clock.hpp" diff --git a/planning/autoware_path_optimizer/include/autoware/path_optimizer/mpt_optimizer.hpp b/planning/autoware_path_optimizer/include/autoware/path_optimizer/mpt_optimizer.hpp index 9a397e255da13..f1c3695347762 100644 --- a/planning/autoware_path_optimizer/include/autoware/path_optimizer/mpt_optimizer.hpp +++ b/planning/autoware_path_optimizer/include/autoware/path_optimizer/mpt_optimizer.hpp @@ -21,8 +21,8 @@ #include "autoware/path_optimizer/common_structs.hpp" #include "autoware/path_optimizer/state_equation_generator.hpp" #include "autoware/path_optimizer/type_alias.hpp" -#include "autoware/universe_utils/geometry/geometry.hpp" -#include "autoware/universe_utils/system/time_keeper.hpp" +#include "autoware_utils/geometry/geometry.hpp" +#include "autoware_utils/system/time_keeper.hpp" #include "autoware_vehicle_info_utils/vehicle_info_utils.hpp" #include "gtest/gtest.h" @@ -94,9 +94,9 @@ struct ReferencePoint geometry_msgs::msg::Pose offsetDeviation(const double lat_dev, const double yaw_dev) const { - auto pose_with_deviation = autoware::universe_utils::calcOffsetPose(pose, 0.0, lat_dev, 0.0); + auto pose_with_deviation = autoware_utils::calc_offset_pose(pose, 0.0, lat_dev, 0.0); pose_with_deviation.orientation = - autoware::universe_utils::createQuaternionFromYaw(getYaw() + yaw_dev); + autoware_utils::create_quaternion_from_yaw(getYaw() + yaw_dev); return pose_with_deviation; } }; @@ -108,7 +108,7 @@ class MPTOptimizer rclcpp::Node * node, const bool enable_debug_info, const EgoNearestParam ego_nearest_param, const autoware::vehicle_info_utils::VehicleInfo & vehicle_info, const TrajectoryParam & traj_param, const std::shared_ptr debug_data_ptr, - const std::shared_ptr time_keeper_); + const std::shared_ptr time_keeper_); std::vector optimizeTrajectory(const PlannerData & planner_data); std::optional> getPrevOptimizedTrajectoryPoints() const; @@ -223,7 +223,7 @@ class MPTOptimizer autoware::vehicle_info_utils::VehicleInfo vehicle_info_; TrajectoryParam traj_param_; mutable std::shared_ptr debug_data_ptr_; - mutable std::shared_ptr time_keeper_; + mutable std::shared_ptr time_keeper_; rclcpp::Logger logger_; MPTParam mpt_param_; diff --git a/planning/autoware_path_optimizer/include/autoware/path_optimizer/node.hpp b/planning/autoware_path_optimizer/include/autoware/path_optimizer/node.hpp index cad3a9fc89340..90f8722f1e643 100644 --- a/planning/autoware_path_optimizer/include/autoware/path_optimizer/node.hpp +++ b/planning/autoware_path_optimizer/include/autoware/path_optimizer/node.hpp @@ -20,13 +20,13 @@ #include "autoware/path_optimizer/mpt_optimizer.hpp" #include "autoware/path_optimizer/replan_checker.hpp" #include "autoware/path_optimizer/type_alias.hpp" -#include "autoware/universe_utils/ros/logger_level_configure.hpp" -#include "autoware/universe_utils/ros/polling_subscriber.hpp" -#include "autoware/universe_utils/system/stop_watch.hpp" -#include "autoware/universe_utils/system/time_keeper.hpp" +#include "autoware_utils/ros/logger_level_configure.hpp" +#include "autoware_utils/ros/polling_subscriber.hpp" +#include "autoware_utils/system/stop_watch.hpp" +#include "autoware_utils/system/time_keeper.hpp" #include "autoware_vehicle_info_utils/vehicle_info_utils.hpp" -#include +#include #include #include @@ -67,7 +67,7 @@ class PathOptimizer : public rclcpp::Node // argument variables autoware::vehicle_info_utils::VehicleInfo vehicle_info_{}; mutable std::shared_ptr debug_data_ptr_{nullptr}; - mutable std::shared_ptr time_keeper_{nullptr}; + mutable std::shared_ptr time_keeper_{nullptr}; // flags for some functions bool enable_pub_debug_marker_; @@ -92,15 +92,14 @@ class PathOptimizer : public rclcpp::Node // interface subscriber rclcpp::Subscription::SharedPtr path_sub_; - autoware::universe_utils::InterProcessPollingSubscriber ego_odom_sub_{ - this, "~/input/odometry"}; + autoware_utils::InterProcessPollingSubscriber ego_odom_sub_{this, "~/input/odometry"}; // debug publisher rclcpp::Publisher::SharedPtr debug_extended_traj_pub_; rclcpp::Publisher::SharedPtr debug_markers_pub_; rclcpp::Publisher::SharedPtr debug_calculation_time_str_pub_; rclcpp::Publisher::SharedPtr debug_calculation_time_float_pub_; - rclcpp::Publisher::SharedPtr + rclcpp::Publisher::SharedPtr debug_processing_time_detail_pub_; // parameter callback @@ -141,11 +140,11 @@ class PathOptimizer : public rclcpp::Node private: double vehicle_stop_margin_outside_drivable_area_; - std::unique_ptr logger_configure_; + std::unique_ptr logger_configure_; - std::unique_ptr published_time_publisher_; + std::unique_ptr published_time_publisher_; - autoware::universe_utils::StopWatch stop_watch_; + autoware_utils::StopWatch stop_watch_; }; } // namespace autoware::path_optimizer diff --git a/planning/autoware_path_optimizer/include/autoware/path_optimizer/state_equation_generator.hpp b/planning/autoware_path_optimizer/include/autoware/path_optimizer/state_equation_generator.hpp index 1d23d7788dca1..3c47734fab200 100644 --- a/planning/autoware_path_optimizer/include/autoware/path_optimizer/state_equation_generator.hpp +++ b/planning/autoware_path_optimizer/include/autoware/path_optimizer/state_equation_generator.hpp @@ -18,7 +18,7 @@ #include "autoware/path_optimizer/common_structs.hpp" #include "autoware/path_optimizer/vehicle_model/vehicle_model_bicycle_kinematics.hpp" #include "autoware/path_optimizer/vehicle_model/vehicle_model_interface.hpp" -#include "autoware/universe_utils/system/time_keeper.hpp" +#include "autoware_utils/system/time_keeper.hpp" #include #include @@ -40,7 +40,7 @@ class StateEquationGenerator StateEquationGenerator() = default; StateEquationGenerator( const double wheel_base, const double max_steer_rad, - const std::shared_ptr time_keeper) + const std::shared_ptr time_keeper) : vehicle_model_ptr_(std::make_unique(wheel_base, max_steer_rad)), time_keeper_(time_keeper) { @@ -57,7 +57,7 @@ class StateEquationGenerator private: std::unique_ptr vehicle_model_ptr_; - mutable std::shared_ptr time_keeper_; + mutable std::shared_ptr time_keeper_; }; } // namespace autoware::path_optimizer #endif // AUTOWARE__PATH_OPTIMIZER__STATE_EQUATION_GENERATOR_HPP_ diff --git a/planning/autoware_path_optimizer/package.xml b/planning/autoware_path_optimizer/package.xml index 2cda65a923da8..626f38dafde7b 100644 --- a/planning/autoware_path_optimizer/package.xml +++ b/planning/autoware_path_optimizer/package.xml @@ -20,7 +20,7 @@ autoware_osqp_interface autoware_planning_msgs autoware_planning_test_manager - autoware_universe_utils + autoware_utils autoware_vehicle_info_utils geometry_msgs nav_msgs diff --git a/planning/autoware_path_optimizer/src/debug_marker.cpp b/planning/autoware_path_optimizer/src/debug_marker.cpp index cf994dadf38cc..717cd13e38d9d 100644 --- a/planning/autoware_path_optimizer/src/debug_marker.cpp +++ b/planning/autoware_path_optimizer/src/debug_marker.cpp @@ -23,10 +23,10 @@ namespace autoware::path_optimizer { -using autoware::universe_utils::appendMarkerArray; -using autoware::universe_utils::createDefaultMarker; -using autoware::universe_utils::createMarkerColor; -using autoware::universe_utils::createMarkerScale; +using autoware_utils::append_marker_array; +using autoware_utils::create_default_marker; +using autoware_utils::create_marker_color; +using autoware_utils::create_marker_scale; namespace { @@ -34,9 +34,9 @@ MarkerArray getFootprintsMarkerArray( const std::vector & mpt_traj, const autoware::vehicle_info_utils::VehicleInfo & vehicle_info, const size_t sampling_num) { - auto marker = createDefaultMarker( + auto marker = create_default_marker( "map", rclcpp::Clock().now(), "mpt_footprints", 0, Marker::LINE_STRIP, - createMarkerScale(0.05, 0.0, 0.0), createMarkerColor(0.99, 0.99, 0.2, 0.99)); + create_marker_scale(0.05, 0.0, 0.0), create_marker_color(0.99, 0.99, 0.2, 0.99)); marker.lifetime = rclcpp::Duration::from_seconds(1.5); MarkerArray marker_array; @@ -56,17 +56,15 @@ MarkerArray getFootprintsMarkerArray( const double base_to_rear = vehicle_info.rear_overhang_m; marker.points.push_back( - autoware::universe_utils::calcOffsetPose(traj_point.pose, base_to_front, base_to_left, 0.0) - .position); + autoware_utils::calc_offset_pose(traj_point.pose, base_to_front, base_to_left, 0.0).position); marker.points.push_back( - autoware::universe_utils::calcOffsetPose(traj_point.pose, base_to_front, -base_to_right, 0.0) + autoware_utils::calc_offset_pose(traj_point.pose, base_to_front, -base_to_right, 0.0) .position); marker.points.push_back( - autoware::universe_utils::calcOffsetPose(traj_point.pose, -base_to_rear, -base_to_right, 0.0) + autoware_utils::calc_offset_pose(traj_point.pose, -base_to_rear, -base_to_right, 0.0) .position); marker.points.push_back( - autoware::universe_utils::calcOffsetPose(traj_point.pose, -base_to_rear, base_to_left, 0.0) - .position); + autoware_utils::calc_offset_pose(traj_point.pose, -base_to_rear, base_to_left, 0.0).position); marker.points.push_back(marker.points.front()); marker_array.markers.push_back(marker); @@ -84,15 +82,15 @@ MarkerArray getBoundsWidthMarkerArray( if (ref_points.empty()) return marker_array; // create lower bound marker - auto lb_marker = createDefaultMarker( - "map", rclcpp::Clock().now(), "", 0, Marker::LINE_LIST, createMarkerScale(0.05, 0.0, 0.0), - createMarkerColor(0.5, 0.99, 0.2, 0.8)); + auto lb_marker = create_default_marker( + "map", rclcpp::Clock().now(), "", 0, Marker::LINE_LIST, create_marker_scale(0.05, 0.0, 0.0), + create_marker_color(0.5, 0.99, 0.2, 0.8)); lb_marker.lifetime = rclcpp::Duration::from_seconds(1.5); // create upper bound marker - auto ub_marker = createDefaultMarker( - "map", rclcpp::Clock().now(), "", 1, Marker::LINE_LIST, createMarkerScale(0.05, 0.0, 0.0), - createMarkerColor(0.99, 0.5, 0.2, 0.8)); + auto ub_marker = create_default_marker( + "map", rclcpp::Clock().now(), "", 1, Marker::LINE_LIST, create_marker_scale(0.05, 0.0, 0.0), + create_marker_color(0.99, 0.5, 0.2, 0.8)); ub_marker.lifetime = rclcpp::Duration::from_seconds(1.5); for (size_t bound_idx = 0; bound_idx < ref_points.at(0).bounds_on_constraints.size(); @@ -113,7 +111,7 @@ MarkerArray getBoundsWidthMarkerArray( (vehicle_info.wheel_tread_m / 2.0) + vehicle_info.right_overhang_m; const double lb_y = ref_points.at(i).bounds_on_constraints.at(bound_idx).lower_bound - base_to_right; - const auto lb = autoware::universe_utils::calcOffsetPose(pose, 0.0, lb_y, 0.0).position; + const auto lb = autoware_utils::calc_offset_pose(pose, 0.0, lb_y, 0.0).position; lb_marker.points.push_back(pose.position); lb_marker.points.push_back(lb); @@ -135,7 +133,7 @@ MarkerArray getBoundsWidthMarkerArray( (vehicle_info.wheel_tread_m / 2.0) + vehicle_info.left_overhang_m; const double ub_y = ref_points.at(i).bounds_on_constraints.at(bound_idx).upper_bound + base_to_left; - const auto ub = autoware::universe_utils::calcOffsetPose(pose, 0.0, ub_y, 0.0).position; + const auto ub = autoware_utils::calc_offset_pose(pose, 0.0, ub_y, 0.0).position; ub_marker.points.push_back(pose.position); ub_marker.points.push_back(ub); @@ -155,14 +153,14 @@ MarkerArray getBoundsLineMarkerArray( if (ref_points.empty()) return marker_array; - auto ub_marker = createDefaultMarker( + auto ub_marker = create_default_marker( "map", rclcpp::Clock().now(), "left_bounds", 0, Marker::LINE_STRIP, - createMarkerScale(0.05, 0.0, 0.0), createMarkerColor(0.0, 1.0, 1.0, 0.8)); + create_marker_scale(0.05, 0.0, 0.0), create_marker_color(0.0, 1.0, 1.0, 0.8)); ub_marker.lifetime = rclcpp::Duration::from_seconds(1.5); - auto lb_marker = createDefaultMarker( + auto lb_marker = create_default_marker( "map", rclcpp::Clock().now(), "right_bounds", 0, Marker::LINE_STRIP, - createMarkerScale(0.05, 0.0, 0.0), createMarkerColor(0.0, 1.0, 1.0, 0.8)); + create_marker_scale(0.05, 0.0, 0.0), create_marker_color(0.0, 1.0, 1.0, 0.8)); lb_marker.lifetime = rclcpp::Duration::from_seconds(1.5); for (size_t i = 0; i < ref_points.size(); i++) { @@ -170,11 +168,11 @@ MarkerArray getBoundsLineMarkerArray( const double base_to_right = (vehicle_info.wheel_tread_m / 2.0) + vehicle_info.right_overhang_m; const double base_to_left = (vehicle_info.wheel_tread_m / 2.0) + vehicle_info.left_overhang_m; const double ub_y = ref_points.at(i).bounds.upper_bound + base_to_left; - const auto ub = autoware::universe_utils::calcOffsetPose(pose, 0.0, ub_y, 0.0).position; + const auto ub = autoware_utils::calc_offset_pose(pose, 0.0, ub_y, 0.0).position; ub_marker.points.push_back(ub); const double lb_y = ref_points.at(i).bounds.lower_bound - base_to_right; - const auto lb = autoware::universe_utils::calcOffsetPose(pose, 0.0, lb_y, 0.0).position; + const auto lb = autoware_utils::calc_offset_pose(pose, 0.0, lb_y, 0.0).position; lb_marker.points.push_back(lb); } marker_array.markers.push_back(ub_marker); @@ -198,32 +196,30 @@ MarkerArray getVehicleCircleLinesMarkerArray( } const auto & ref_point = ref_points.at(i); - auto marker = createDefaultMarker( - "map", rclcpp::Clock().now(), ns, i, Marker::LINE_LIST, createMarkerScale(0.1, 0, 0), - createMarkerColor(0.99, 0.99, 0.2, 0.25)); + auto marker = create_default_marker( + "map", rclcpp::Clock().now(), ns, i, Marker::LINE_LIST, create_marker_scale(0.1, 0, 0), + create_marker_color(0.99, 0.99, 0.2, 0.25)); marker.lifetime = rclcpp::Duration::from_seconds(1.5); const double lat_dev = ref_point.optimized_kinematic_state.lat; const double yaw_dev = ref_point.optimized_kinematic_state.yaw; // apply lateral and yaw deviation - auto pose_with_deviation = - autoware::universe_utils::calcOffsetPose(ref_point.pose, 0.0, lat_dev, 0.0); + auto pose_with_deviation = autoware_utils::calc_offset_pose(ref_point.pose, 0.0, lat_dev, 0.0); pose_with_deviation.orientation = - autoware::universe_utils::createQuaternionFromYaw(ref_point.getYaw() + yaw_dev); + autoware_utils::create_quaternion_from_yaw(ref_point.getYaw() + yaw_dev); for (const double d : vehicle_circle_longitudinal_offsets) { // apply longitudinal offset - auto base_pose = autoware::universe_utils::calcOffsetPose(pose_with_deviation, d, 0.0, 0.0); + auto base_pose = autoware_utils::calc_offset_pose(pose_with_deviation, d, 0.0, 0.0); base_pose.orientation = - autoware::universe_utils::createQuaternionFromYaw(ref_point.getYaw() + ref_point.alpha); + autoware_utils::create_quaternion_from_yaw(ref_point.getYaw() + ref_point.alpha); const double base_to_right = (vehicle_info.wheel_tread_m / 2.0) + vehicle_info.right_overhang_m; const double base_to_left = (vehicle_info.wheel_tread_m / 2.0) + vehicle_info.left_overhang_m; - const auto ub = - autoware::universe_utils::calcOffsetPose(base_pose, 0.0, base_to_left, 0.0).position; + const auto ub = autoware_utils::calc_offset_pose(base_pose, 0.0, base_to_left, 0.0).position; const auto lb = - autoware::universe_utils::calcOffsetPose(base_pose, 0.0, -base_to_right, 0.0).position; + autoware_utils::calc_offset_pose(base_pose, 0.0, -base_to_right, 0.0).position; marker.points.push_back(ub); marker.points.push_back(lb); @@ -246,11 +242,11 @@ MarkerArray getCurrentVehicleCirclesMarkerArray( for (size_t v_idx = 0; v_idx < vehicle_circle_longitudinal_offsets.size(); ++v_idx) { const double offset = vehicle_circle_longitudinal_offsets.at(v_idx); - auto marker = createDefaultMarker( - "map", rclcpp::Clock().now(), ns, id, Marker::LINE_STRIP, createMarkerScale(0.05, 0.0, 0.0), - createMarkerColor(r, g, b, 0.8)); + auto marker = create_default_marker( + "map", rclcpp::Clock().now(), ns, id, Marker::LINE_STRIP, create_marker_scale(0.05, 0.0, 0.0), + create_marker_color(r, g, b, 0.8)); marker.lifetime = rclcpp::Duration::from_seconds(1.5); - marker.pose = autoware::universe_utils::calcOffsetPose(ego_pose, offset, 0.0, 0.0); + marker.pose = autoware_utils::calc_offset_pose(ego_pose, offset, 0.0, 0.0); constexpr size_t circle_dividing_num = 16; for (size_t e_idx = 0; e_idx < circle_dividing_num + 1; ++e_idx) { @@ -288,11 +284,11 @@ MarkerArray getVehicleCirclesMarkerArray( for (size_t v_idx = 0; v_idx < vehicle_circle_longitudinal_offsets.size(); ++v_idx) { const double offset = vehicle_circle_longitudinal_offsets.at(v_idx); - auto marker = createDefaultMarker( - "map", rclcpp::Clock().now(), ns, id, Marker::LINE_STRIP, createMarkerScale(0.05, 0.0, 0.0), - createMarkerColor(r, g, b, 0.8)); + auto marker = create_default_marker( + "map", rclcpp::Clock().now(), ns, id, Marker::LINE_STRIP, + create_marker_scale(0.05, 0.0, 0.0), create_marker_color(r, g, b, 0.8)); marker.lifetime = rclcpp::Duration::from_seconds(1.5); - marker.pose = autoware::universe_utils::calcOffsetPose(mpt_traj_point.pose, offset, 0.0, 0.0); + marker.pose = autoware_utils::calc_offset_pose(mpt_traj_point.pose, offset, 0.0, 0.0); constexpr size_t circle_dividing_num = 16; for (size_t e_idx = 0; e_idx < circle_dividing_num + 1; ++e_idx) { @@ -320,9 +316,9 @@ visualization_msgs::msg::MarkerArray getPointsTextMarkerArray( return visualization_msgs::msg::MarkerArray{}; } - auto marker = createDefaultMarker( + auto marker = create_default_marker( "map", rclcpp::Clock().now(), "text", 0, visualization_msgs::msg::Marker::TEXT_VIEW_FACING, - createMarkerScale(0.0, 0.0, 0.15), createMarkerColor(1.0, 1.0, 0.0, 0.99)); + create_marker_scale(0.0, 0.0, 0.15), create_marker_color(1.0, 1.0, 0.0, 0.99)); marker.lifetime = rclcpp::Duration::from_seconds(1.5); visualization_msgs::msg::MarkerArray msg; @@ -348,9 +344,9 @@ visualization_msgs::msg::MarkerArray getFootprintByDrivableAreaMarkerArray( { visualization_msgs::msg::MarkerArray msg; - auto marker = createDefaultMarker( + auto marker = create_default_marker( "map", rclcpp::Clock().now(), ns, 1, visualization_msgs::msg::Marker::LINE_STRIP, - createMarkerScale(0.05, 0.0, 0.0), createMarkerColor(r, g, b, 1.0)); + create_marker_scale(0.05, 0.0, 0.0), create_marker_color(r, g, b, 1.0)); marker.lifetime = rclcpp::Duration::from_seconds(1.5); const double base_to_right = (vehicle_info.wheel_tread_m / 2.0) + vehicle_info.right_overhang_m; @@ -359,15 +355,13 @@ visualization_msgs::msg::MarkerArray getFootprintByDrivableAreaMarkerArray( const double base_to_rear = vehicle_info.rear_overhang_m; marker.points.push_back( - autoware::universe_utils::calcOffsetPose(stop_pose, base_to_front, base_to_left, 0.0).position); + autoware_utils::calc_offset_pose(stop_pose, base_to_front, base_to_left, 0.0).position); marker.points.push_back( - autoware::universe_utils::calcOffsetPose(stop_pose, base_to_front, -base_to_right, 0.0) - .position); + autoware_utils::calc_offset_pose(stop_pose, base_to_front, -base_to_right, 0.0).position); marker.points.push_back( - autoware::universe_utils::calcOffsetPose(stop_pose, -base_to_rear, -base_to_right, 0.0) - .position); + autoware_utils::calc_offset_pose(stop_pose, -base_to_rear, -base_to_right, 0.0).position); marker.points.push_back( - autoware::universe_utils::calcOffsetPose(stop_pose, -base_to_rear, base_to_left, 0.0).position); + autoware_utils::calc_offset_pose(stop_pose, -base_to_rear, base_to_left, 0.0).position); marker.points.push_back(marker.points.front()); msg.markers.push_back(marker); @@ -384,16 +378,16 @@ MarkerArray getDebugMarker( MarkerArray marker_array; // bounds line - appendMarkerArray(getBoundsLineMarkerArray(debug_data.ref_points, vehicle_info), &marker_array); + append_marker_array(getBoundsLineMarkerArray(debug_data.ref_points, vehicle_info), &marker_array); // bounds width - appendMarkerArray( + append_marker_array( getBoundsWidthMarkerArray( debug_data.ref_points, vehicle_info, debug_data.mpt_visualize_sampling_num), &marker_array); // current vehicle circles - appendMarkerArray( + append_marker_array( getCurrentVehicleCirclesMarkerArray( debug_data.ego_pose, debug_data.vehicle_circle_longitudinal_offsets, debug_data.vehicle_circle_radiuses, "current_vehicle_circles", 1.0, 0.3, 0.3), @@ -403,7 +397,7 @@ MarkerArray getDebugMarker( // Circles visualization is comparatively heavy. if (publish_extra_marker) { // vehicle circles - appendMarkerArray( + append_marker_array( getVehicleCirclesMarkerArray( optimized_points, debug_data.vehicle_circle_longitudinal_offsets, debug_data.vehicle_circle_radiuses, debug_data.mpt_visualize_sampling_num, @@ -411,13 +405,13 @@ MarkerArray getDebugMarker( &marker_array); // mpt footprints - appendMarkerArray( + append_marker_array( getFootprintsMarkerArray( optimized_points, vehicle_info, debug_data.mpt_visualize_sampling_num), &marker_array); // vehicle circle line - appendMarkerArray( + append_marker_array( getVehicleCircleLinesMarkerArray( debug_data.ref_points, debug_data.vehicle_circle_longitudinal_offsets, vehicle_info, debug_data.mpt_visualize_sampling_num, "vehicle_circle_lines"), @@ -425,7 +419,7 @@ MarkerArray getDebugMarker( // footprint by drivable area if (debug_data.stop_pose_by_drivable_area) { - appendMarkerArray( + append_marker_array( getFootprintByDrivableAreaMarkerArray( *debug_data.stop_pose_by_drivable_area, vehicle_info, "footprint_by_drivable_area", 1.0, 0.0, 0.0), @@ -433,7 +427,7 @@ MarkerArray getDebugMarker( } // debug text - appendMarkerArray(getPointsTextMarkerArray(debug_data.ref_points), &marker_array); + append_marker_array(getPointsTextMarkerArray(debug_data.ref_points), &marker_array); } return marker_array; diff --git a/planning/autoware_path_optimizer/src/mpt_optimizer.cpp b/planning/autoware_path_optimizer/src/mpt_optimizer.cpp index a73e8cc4ca69c..88145e8a28511 100644 --- a/planning/autoware_path_optimizer/src/mpt_optimizer.cpp +++ b/planning/autoware_path_optimizer/src/mpt_optimizer.cpp @@ -19,8 +19,8 @@ #include "autoware/motion_utils/trajectory/trajectory.hpp" #include "autoware/path_optimizer/utils/geometry_utils.hpp" #include "autoware/path_optimizer/utils/trajectory_utils.hpp" -#include "autoware/universe_utils/geometry/geometry.hpp" -#include "autoware/universe_utils/math/normalization.hpp" +#include "autoware_utils/geometry/geometry.hpp" +#include "autoware_utils/math/normalization.hpp" #include "tf2/utils.h" #include @@ -129,8 +129,8 @@ std::vector toStdVector(const Eigen::VectorXd & eigen_vec) bool isLeft(const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::Point & target_pos) { const double base_theta = tf2::getYaw(pose.orientation); - const double target_theta = autoware::universe_utils::calcAzimuthAngle(pose.position, target_pos); - const double diff_theta = autoware::universe_utils::normalizeRadian(target_theta - base_theta); + const double target_theta = autoware_utils::calc_azimuth_angle(pose.position, target_pos); + const double diff_theta = autoware_utils::normalize_radian(target_theta - base_theta); return diff_theta > 0; } @@ -145,13 +145,13 @@ double calcLateralDistToBounds( const double max_lat_offset = is_left_bound ? max_lat_offset_for_left : -max_lat_offset_for_left; const double min_lat_offset = is_left_bound ? min_lat_offset_for_left : -min_lat_offset_for_left; const auto max_lat_offset_point = - autoware::universe_utils::calcOffsetPose(pose, 0.0, max_lat_offset, 0.0).position; + autoware_utils::calc_offset_pose(pose, 0.0, max_lat_offset, 0.0).position; const auto min_lat_offset_point = - autoware::universe_utils::calcOffsetPose(pose, 0.0, min_lat_offset, 0.0).position; + autoware_utils::calc_offset_pose(pose, 0.0, min_lat_offset, 0.0).position; double closest_dist_to_bound = max_lat_offset; for (size_t i = 0; i < bound.size() - 1; ++i) { - const auto intersect_point = autoware::universe_utils::intersect( + const auto intersect_point = autoware_utils::intersect( min_lat_offset_point, max_lat_offset_point, bound.at(i), bound.at(i + 1)); if (intersect_point) { const bool is_point_left = isLeft(pose, *intersect_point); @@ -287,111 +287,111 @@ MPTOptimizer::MPTParam::MPTParam( void MPTOptimizer::MPTParam::onParam(const std::vector & parameters) { - using autoware::universe_utils::updateParam; + using autoware_utils::update_param; { // option - updateParam(parameters, "mpt.option.steer_limit_constraint", steer_limit_constraint); - updateParam(parameters, "mpt.option.enable_warm_start", enable_warm_start); - updateParam(parameters, "mpt.option.enable_manual_warm_start", enable_manual_warm_start); - updateParam( + update_param(parameters, "mpt.option.steer_limit_constraint", steer_limit_constraint); + update_param(parameters, "mpt.option.enable_warm_start", enable_warm_start); + update_param(parameters, "mpt.option.enable_manual_warm_start", enable_manual_warm_start); + update_param( parameters, "mpt.option.enable_optimization_validation", enable_optimization_validation); - updateParam(parameters, "mpt.option.visualize_sampling_num", mpt_visualize_sampling_num); + update_param(parameters, "mpt.option.visualize_sampling_num", mpt_visualize_sampling_num); } // common - updateParam(parameters, "mpt.common.num_points", num_points); - updateParam(parameters, "mpt.common.delta_arc_length", delta_arc_length); + update_param(parameters, "mpt.common.num_points", num_points); + update_param(parameters, "mpt.common.delta_arc_length", delta_arc_length); // kinematics - updateParam( + update_param( parameters, "mpt.kinematics.optimization_center_offset", optimization_center_offset); // collision_free_constraints - updateParam(parameters, "mpt.collision_free_constraints.option.l_inf_norm", l_inf_norm); - updateParam( + update_param(parameters, "mpt.collision_free_constraints.option.l_inf_norm", l_inf_norm); + update_param( parameters, "mpt.collision_free_constraints.option.soft_constraint", soft_constraint); - updateParam( + update_param( parameters, "mpt.collision_free_constraints.option.hard_constraint", hard_constraint); { // vehicle_circles - updateParam( + update_param( parameters, "mpt.collision_free_constraints.vehicle_circles.method", vehicle_circles_method); // uniform circles - updateParam( + update_param( parameters, "mpt.collision_free_constraints.vehicle_circles.uniform_circle.num", vehicle_circles_uniform_circle_num); - updateParam( + update_param( parameters, "mpt.collision_free_constraints.vehicle_circles.uniform_circle.radius_ratio", vehicle_circles_uniform_circle_radius_ratio); // bicycle model - updateParam( + update_param( parameters, "mpt.collision_free_constraints.vehicle_circles.bicycle_model.num_for_calculation", vehicle_circles_bicycle_model_num); - updateParam( + update_param( parameters, "mpt.collision_free_constraints.vehicle_circles.bicycle_model.rear_radius_ratio", vehicle_circles_bicycle_model_rear_radius_ratio); - updateParam( + update_param( parameters, "mpt.collision_free_constraints.vehicle_circles.bicycle_model.front_radius_ratio", vehicle_circles_bicycle_model_front_radius_ratio); // fitting uniform circles - updateParam( + update_param( parameters, "mpt.collision_free_constraints.vehicle_circles.fitting_uniform_circle.num", vehicle_circles_fitting_uniform_circle_num); } { // clearance - updateParam( + update_param( parameters, "mpt.clearance.hard_clearance_from_road", hard_clearance_from_road); - updateParam( + update_param( parameters, "mpt.clearance.soft_clearance_from_road", soft_clearance_from_road); } { // weight - updateParam( + update_param( parameters, "mpt.weight.soft_collision_free_weight", soft_collision_free_weight); - updateParam(parameters, "mpt.weight.lat_error_weight", lat_error_weight); - updateParam(parameters, "mpt.weight.yaw_error_weight", yaw_error_weight); - updateParam(parameters, "mpt.weight.yaw_error_rate_weight", yaw_error_rate_weight); - updateParam(parameters, "mpt.weight.steer_input_weight", steer_input_weight); - updateParam(parameters, "mpt.weight.steer_rate_weight", steer_rate_weight); + update_param(parameters, "mpt.weight.lat_error_weight", lat_error_weight); + update_param(parameters, "mpt.weight.yaw_error_weight", yaw_error_weight); + update_param(parameters, "mpt.weight.yaw_error_rate_weight", yaw_error_rate_weight); + update_param(parameters, "mpt.weight.steer_input_weight", steer_input_weight); + update_param(parameters, "mpt.weight.steer_rate_weight", steer_rate_weight); - updateParam( + update_param( parameters, "mpt.weight.terminal_lat_error_weight", terminal_lat_error_weight); - updateParam( + update_param( parameters, "mpt.weight.terminal_yaw_error_weight", terminal_yaw_error_weight); - updateParam(parameters, "mpt.weight.goal_lat_error_weight", goal_lat_error_weight); - updateParam(parameters, "mpt.weight.goal_yaw_error_weight", goal_yaw_error_weight); + update_param(parameters, "mpt.weight.goal_lat_error_weight", goal_lat_error_weight); + update_param(parameters, "mpt.weight.goal_yaw_error_weight", goal_yaw_error_weight); } { // avoidance - updateParam( + update_param( parameters, "mpt.avoidance.max_longitudinal_margin_for_bound_violation", max_longitudinal_margin_for_bound_violation); - updateParam(parameters, "mpt.avoidance.max_bound_fixing_time", max_bound_fixing_time); - updateParam(parameters, "mpt.avoidance.min_drivable_width", min_drivable_width); - updateParam(parameters, "mpt.avoidance.max_avoidance_cost", max_avoidance_cost); - updateParam(parameters, "mpt.avoidance.avoidance_cost_margin", avoidance_cost_margin); - updateParam( + update_param(parameters, "mpt.avoidance.max_bound_fixing_time", max_bound_fixing_time); + update_param(parameters, "mpt.avoidance.min_drivable_width", min_drivable_width); + update_param(parameters, "mpt.avoidance.max_avoidance_cost", max_avoidance_cost); + update_param(parameters, "mpt.avoidance.avoidance_cost_margin", avoidance_cost_margin); + update_param( parameters, "mpt.avoidance.avoidance_cost_band_length", avoidance_cost_band_length); - updateParam( + update_param( parameters, "mpt.avoidance.avoidance_cost_decrease_rate", avoidance_cost_decrease_rate); - updateParam( + update_param( parameters, "mpt.avoidance.weight.lat_error_weight", avoidance_lat_error_weight); - updateParam( + update_param( parameters, "mpt.avoidance.weight.yaw_error_weight", avoidance_yaw_error_weight); - updateParam( + update_param( parameters, "mpt.avoidance.weight.steer_input_weight", avoidance_steer_input_weight); } { // validation - updateParam(parameters, "mpt.validation.max_lat_error", max_validation_lat_error); - updateParam(parameters, "mpt.validation.max_yaw_error", max_validation_yaw_error); + update_param(parameters, "mpt.validation.max_lat_error", max_validation_lat_error); + update_param(parameters, "mpt.validation.max_yaw_error", max_validation_yaw_error); } } @@ -399,7 +399,7 @@ MPTOptimizer::MPTOptimizer( rclcpp::Node * node, const bool enable_debug_info, const EgoNearestParam ego_nearest_param, const autoware::vehicle_info_utils::VehicleInfo & vehicle_info, const TrajectoryParam & traj_param, const std::shared_ptr debug_data_ptr, - const std::shared_ptr time_keeper) + const std::shared_ptr time_keeper) : enable_debug_info_(enable_debug_info), ego_nearest_param_(ego_nearest_param), vehicle_info_(vehicle_info), @@ -474,7 +474,7 @@ void MPTOptimizer::onParam(const std::vector & parameters) std::vector MPTOptimizer::optimizeTrajectory(const PlannerData & planner_data) { - autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + autoware_utils::ScopedTimeTrack st(__func__, *time_keeper_); const auto & p = planner_data; const auto & traj_points = p.traj_points; @@ -547,7 +547,7 @@ std::optional> MPTOptimizer::getPrevOptimizedTrajec std::vector MPTOptimizer::calcReferencePoints( const PlannerData & planner_data, const std::vector & smoothed_points) const { - autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + autoware_utils::ScopedTimeTrack st(__func__, *time_keeper_); const auto & p = planner_data; @@ -626,8 +626,7 @@ void MPTOptimizer::updateOrientation( { const auto yaw_vec = ref_points_spline.getSplineInterpolatedYaws(); for (size_t i = 0; i < ref_points.size(); ++i) { - ref_points.at(i).pose.orientation = - autoware::universe_utils::createQuaternionFromYaw(yaw_vec.at(i)); + ref_points.at(i).pose.orientation = autoware_utils::create_quaternion_from_yaw(yaw_vec.at(i)); } } @@ -643,7 +642,7 @@ void MPTOptimizer::updateCurvature( void MPTOptimizer::updateFixedPoint(std::vector & ref_points) const { - autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + autoware_utils::ScopedTimeTrack st(__func__, *time_keeper_); if (!prev_ref_points_ptr_) { // no fixed point @@ -699,7 +698,7 @@ void MPTOptimizer::updateDeltaArcLength(std::vector & ref_points void MPTOptimizer::updateExtraPoints(std::vector & ref_points) const { - autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + autoware_utils::ScopedTimeTrack st(__func__, *time_keeper_); // alpha for (size_t i = 0; i < ref_points.size(); ++i) { @@ -708,12 +707,12 @@ void MPTOptimizer::updateExtraPoints(std::vector & ref_points) c const bool are_too_close_points = autoware_utils::calc_distance2d(front_wheel_pos, ref_points.at(i).pose.position) < 1e-03; - const auto front_wheel_yaw = are_too_close_points - ? ref_points.at(i).getYaw() - : autoware::universe_utils::calcAzimuthAngle( - ref_points.at(i).pose.position, front_wheel_pos); + const auto front_wheel_yaw = + are_too_close_points + ? ref_points.at(i).getYaw() + : autoware_utils::calc_azimuth_angle(ref_points.at(i).pose.position, front_wheel_pos); ref_points.at(i).alpha = - autoware::universe_utils::normalizeRadian(front_wheel_yaw - ref_points.at(i).getYaw()); + autoware_utils::normalize_radian(front_wheel_yaw - ref_points.at(i).getYaw()); } { // avoidance @@ -795,7 +794,7 @@ void MPTOptimizer::updateBounds( const std::vector & right_bound, const geometry_msgs::msg::Pose & ego_pose, const double ego_vel) const { - autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + autoware_utils::ScopedTimeTrack st(__func__, *time_keeper_); const double soft_road_clearance = mpt_param_.soft_clearance_from_road + vehicle_info_.vehicle_width_m / 2.0; @@ -1100,7 +1099,7 @@ void MPTOptimizer::updateVehicleBounds( std::vector & ref_points, const autoware::interpolation::SplineInterpolationPoints2d & ref_points_spline) const { - autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + autoware_utils::ScopedTimeTrack st(__func__, *time_keeper_); for (size_t p_idx = 0; p_idx < ref_points.size(); ++p_idx) { const auto & ref_point = ref_points.at(p_idx); @@ -1126,7 +1125,7 @@ void MPTOptimizer::updateVehicleBounds( std::sin(tmp_yaw - collision_check_yaw); const auto vehicle_bounds_pose = - autoware::universe_utils::calcOffsetPose(collision_check_pose, 0.0, offset_y, 0.0); + autoware_utils::calc_offset_pose(collision_check_pose, 0.0, offset_y, 0.0); // interpolate bounds const auto bounds = [&]() { @@ -1162,7 +1161,7 @@ MPTOptimizer::ValueMatrix MPTOptimizer::calcValueMatrix( const std::vector & ref_points, const std::vector & traj_points) const { - autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + autoware_utils::ScopedTimeTrack st(__func__, *time_keeper_); const size_t D_x = state_equation_generator_.getDimX(); const size_t D_u = state_equation_generator_.getDimU(); @@ -1228,7 +1227,7 @@ MPTOptimizer::ObjectiveMatrix MPTOptimizer::calcObjectiveMatrix( [[maybe_unused]] const StateEquationGenerator::Matrix & mpt_mat, const ValueMatrix & val_mat, const std::vector & ref_points) const { - autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + autoware_utils::ScopedTimeTrack st(__func__, *time_keeper_); const size_t D_x = state_equation_generator_.getDimX(); const size_t D_u = state_equation_generator_.getDimU(); @@ -1288,7 +1287,7 @@ MPTOptimizer::ConstraintMatrix MPTOptimizer::calcConstraintMatrix( const StateEquationGenerator::Matrix & mpt_mat, const std::vector & ref_points) const { - autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + autoware_utils::ScopedTimeTrack st(__func__, *time_keeper_); const size_t D_x = state_equation_generator_.getDimX(); const size_t D_u = state_equation_generator_.getDimU(); @@ -1468,7 +1467,7 @@ std::optional MPTOptimizer::calcOptimizedSteerAngles( const std::vector & ref_points, const ObjectiveMatrix & obj_mat, const ConstraintMatrix & const_mat) { - autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + autoware_utils::ScopedTimeTrack st(__func__, *time_keeper_); const size_t D_x = state_equation_generator_.getDimX(); const size_t D_u = state_equation_generator_.getDimU(); @@ -1635,7 +1634,7 @@ std::optional> MPTOptimizer::calcMPTPoints( std::vector & ref_points, const Eigen::VectorXd & optimized_variables, [[maybe_unused]] const StateEquationGenerator::Matrix & mpt_mat) const { - autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + autoware_utils::ScopedTimeTrack st(__func__, *time_keeper_); const size_t D_x = state_equation_generator_.getDimX(); const size_t D_u = state_equation_generator_.getDimU(); @@ -1695,7 +1694,7 @@ void MPTOptimizer::publishDebugTrajectories( const std_msgs::msg::Header & header, const std::vector & ref_points, const std::vector & mpt_traj_points) const { - autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + autoware_utils::ScopedTimeTrack st(__func__, *time_keeper_); // reference points const auto ref_traj = autoware::motion_utils::convertToTrajectory( diff --git a/planning/autoware_path_optimizer/src/node.cpp b/planning/autoware_path_optimizer/src/node.cpp index 0a58dd7041e5f..1d69afd057f4f 100644 --- a/planning/autoware_path_optimizer/src/node.cpp +++ b/planning/autoware_path_optimizer/src/node.cpp @@ -106,8 +106,7 @@ PathOptimizer::PathOptimizer(const rclcpp::NodeOptions & node_options) debug_calculation_time_float_pub_ = create_publisher("~/debug/processing_time_ms", 1); debug_processing_time_detail_pub_ = - create_publisher( - "~/debug/processing_time_detail_ms", 1); + create_publisher("~/debug/processing_time_detail_ms", 1); { // parameters // parameter for option @@ -137,8 +136,7 @@ PathOptimizer::PathOptimizer(const rclcpp::NodeOptions & node_options) traj_param_ = TrajectoryParam(this); } - time_keeper_ = - std::make_shared(debug_processing_time_detail_pub_); + time_keeper_ = std::make_shared(debug_processing_time_detail_pub_); // create core algorithm pointers with parameter declaration replan_checker_ptr_ = std::make_shared(this, ego_nearest_param_); @@ -157,35 +155,34 @@ PathOptimizer::PathOptimizer(const rclcpp::NodeOptions & node_options) set_param_res_ = this->add_on_set_parameters_callback( std::bind(&PathOptimizer::onParam, this, std::placeholders::_1)); - logger_configure_ = std::make_unique(this); - published_time_publisher_ = - std::make_unique(this); + logger_configure_ = std::make_unique(this); + published_time_publisher_ = std::make_unique(this); } rcl_interfaces::msg::SetParametersResult PathOptimizer::onParam( const std::vector & parameters) { - using autoware::universe_utils::updateParam; + using autoware_utils::update_param; // parameters for option - updateParam( + update_param( parameters, "option.enable_outside_drivable_area_stop", enable_outside_drivable_area_stop_); - updateParam(parameters, "option.enable_skip_optimization", enable_skip_optimization_); - updateParam( + update_param(parameters, "option.enable_skip_optimization", enable_skip_optimization_); + update_param( parameters, "option.enable_reset_prev_optimization", enable_reset_prev_optimization_); - updateParam( + update_param( parameters, "option.use_footprint_polygon_for_outside_drivable_area_check", use_footprint_polygon_for_outside_drivable_area_check_); // parameters for debug marker - updateParam(parameters, "option.debug.enable_pub_debug_marker", enable_pub_debug_marker_); - updateParam( + update_param(parameters, "option.debug.enable_pub_debug_marker", enable_pub_debug_marker_); + update_param( parameters, "option.debug.enable_pub_extra_debug_marker", enable_pub_extra_debug_marker_); // parameters for debug info - updateParam(parameters, "option.debug.enable_debug_info", enable_debug_info_); + update_param(parameters, "option.debug.enable_debug_info", enable_debug_info_); - updateParam( + update_param( parameters, "common.vehicle_stop_margin_outside_drivable_area", vehicle_stop_margin_outside_drivable_area_); @@ -224,7 +221,7 @@ void PathOptimizer::resetPreviousData() void PathOptimizer::onPath(const Path::ConstSharedPtr path_ptr) { - universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + autoware_utils::ScopedTimeTrack st(__func__, *time_keeper_); stop_watch_.tic(); // check if input path is valid @@ -233,7 +230,7 @@ void PathOptimizer::onPath(const Path::ConstSharedPtr path_ptr) } // check if ego's odometry is valid - const auto ego_odom_ptr = ego_odom_sub_.takeData(); + const auto ego_odom_ptr = ego_odom_sub_.take_data(); if (!ego_odom_ptr) { RCLCPP_INFO_SKIPFIRST_THROTTLE( get_logger(), *get_clock(), 5000, "Waiting for ego pose and twist."); @@ -316,7 +313,7 @@ PlannerData PathOptimizer::createPlannerData( std::vector PathOptimizer::generateOptimizedTrajectory( const PlannerData & planner_data) { - autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + autoware_utils::ScopedTimeTrack st(__func__, *time_keeper_); const auto & input_traj_points = planner_data.traj_points; @@ -341,7 +338,7 @@ std::vector PathOptimizer::generateOptimizedTrajectory( std::vector PathOptimizer::optimizeTrajectory(const PlannerData & planner_data) { - autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + autoware_utils::ScopedTimeTrack st(__func__, *time_keeper_); const auto & p = planner_data; // 1. check if replan (= optimization) is required @@ -386,7 +383,7 @@ void PathOptimizer::applyInputVelocity( const std::vector & input_traj_points, const geometry_msgs::msg::Pose & ego_pose) const { - autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + autoware_utils::ScopedTimeTrack st(__func__, *time_keeper_); // crop forward for faster calculation const auto forward_cropped_input_traj_points = [&]() { @@ -486,7 +483,7 @@ void PathOptimizer::applyInputVelocity( void PathOptimizer::insertZeroVelocityOutsideDrivableArea( const PlannerData & planner_data, std::vector & optimized_traj_points) const { - autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + autoware_utils::ScopedTimeTrack st(__func__, *time_keeper_); if (optimized_traj_points.empty()) { return; @@ -550,13 +547,13 @@ void PathOptimizer::insertZeroVelocityOutsideDrivableArea( void PathOptimizer::publishVirtualWall(const geometry_msgs::msg::Pose & stop_pose) const { - autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + autoware_utils::ScopedTimeTrack st(__func__, *time_keeper_); auto virtual_wall_marker = autoware::motion_utils::createStopVirtualWallMarker( stop_pose, "outside drivable area", now(), 0, vehicle_info_.max_longitudinal_offset_m); if (!enable_outside_drivable_area_stop_) { virtual_wall_marker.markers.front().color = - autoware::universe_utils::createMarkerColor(0.0, 1.0, 0.0, 0.5); + autoware_utils::create_marker_color(0.0, 1.0, 0.0, 0.5); } virtual_wall_pub_->publish(virtual_wall_marker); @@ -565,7 +562,7 @@ void PathOptimizer::publishVirtualWall(const geometry_msgs::msg::Pose & stop_pos void PathOptimizer::publishDebugMarkerOfOptimization( const std::vector & traj_points) const { - autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + autoware_utils::ScopedTimeTrack st(__func__, *time_keeper_); if (!enable_pub_debug_marker_) { return; @@ -586,7 +583,7 @@ std::vector PathOptimizer::extendTrajectory( const std::vector & traj_points, const std::vector & optimized_traj_points) const { - autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + autoware_utils::ScopedTimeTrack st(__func__, *time_keeper_); const auto & joint_start_pose = optimized_traj_points.back().pose; @@ -647,7 +644,7 @@ std::vector PathOptimizer::extendTrajectory( void PathOptimizer::publishDebugData(const Header & header) const { - autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + autoware_utils::ScopedTimeTrack st(__func__, *time_keeper_); // publish trajectories const auto debug_extended_traj = diff --git a/planning/autoware_path_optimizer/src/replan_checker.cpp b/planning/autoware_path_optimizer/src/replan_checker.cpp index d3aac49747910..3cbf8946b75b8 100644 --- a/planning/autoware_path_optimizer/src/replan_checker.cpp +++ b/planning/autoware_path_optimizer/src/replan_checker.cpp @@ -16,8 +16,8 @@ #include "autoware/motion_utils/trajectory/trajectory.hpp" #include "autoware/path_optimizer/utils/trajectory_utils.hpp" -#include "autoware/universe_utils/geometry/geometry.hpp" -#include "autoware/universe_utils/ros/update_param.hpp" +#include "autoware_utils/geometry/geometry.hpp" +#include "autoware_utils/ros/update_param.hpp" #include #include @@ -40,17 +40,17 @@ ReplanChecker::ReplanChecker(rclcpp::Node * node, const EgoNearestParam & ego_ne void ReplanChecker::onParam(const std::vector & parameters) { - using autoware::universe_utils::updateParam; + using autoware_utils::update_param; - updateParam( + update_param( parameters, "replan.max_path_shape_around_ego_lat_dist", max_path_shape_around_ego_lat_dist_); - updateParam( + update_param( parameters, "replan.max_path_shape_forward_lat_dist", max_path_shape_forward_lat_dist_); - updateParam( + update_param( parameters, "replan.max_path_shape_forward_lon_dist", max_path_shape_forward_lon_dist_); - updateParam(parameters, "replan.max_ego_moving_dist", max_ego_moving_dist_); - updateParam(parameters, "replan.max_goal_moving_dist", max_goal_moving_dist_); - updateParam(parameters, "replan.max_delta_time_sec", max_delta_time_sec_); + update_param(parameters, "replan.max_ego_moving_dist", max_ego_moving_dist_); + update_param(parameters, "replan.max_goal_moving_dist", max_goal_moving_dist_); + update_param(parameters, "replan.max_delta_time_sec", max_delta_time_sec_); } bool ReplanChecker::isResetRequired(const PlannerData & planner_data) const diff --git a/planning/autoware_path_optimizer/src/state_equation_generator.cpp b/planning/autoware_path_optimizer/src/state_equation_generator.cpp index 6bb95d665c8d5..ba757f385ff94 100644 --- a/planning/autoware_path_optimizer/src/state_equation_generator.cpp +++ b/planning/autoware_path_optimizer/src/state_equation_generator.cpp @@ -25,7 +25,7 @@ namespace autoware::path_optimizer StateEquationGenerator::Matrix StateEquationGenerator::calcMatrix( const std::vector & ref_points) const { - autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + autoware_utils::ScopedTimeTrack st(__func__, *time_keeper_); const size_t D_x = vehicle_model_ptr_->getDimX(); const size_t D_u = vehicle_model_ptr_->getDimU(); diff --git a/planning/autoware_path_optimizer/src/utils/geometry_utils.cpp b/planning/autoware_path_optimizer/src/utils/geometry_utils.cpp index 96b8f1a5f383e..7e3a187c95bab 100644 --- a/planning/autoware_path_optimizer/src/utils/geometry_utils.cpp +++ b/planning/autoware_path_optimizer/src/utils/geometry_utils.cpp @@ -39,10 +39,10 @@ namespace autoware::path_optimizer { namespace bg = boost::geometry; -using autoware::universe_utils::LinearRing2d; -using autoware::universe_utils::LineString2d; -using autoware::universe_utils::Point2d; -using autoware::universe_utils::Polygon2d; +using autoware_utils::LinearRing2d; +using autoware_utils::LineString2d; +using autoware_utils::Point2d; +using autoware_utils::Polygon2d; namespace { @@ -136,13 +136,13 @@ bool isOutsideDrivableAreaFromRectangleFootprint( // calculate footprint corner points const auto top_left_pos = - autoware::universe_utils::calcOffsetPose(pose, base_to_front, base_to_left, 0.0).position; + autoware_utils::calc_offset_pose(pose, base_to_front, base_to_left, 0.0).position; const auto top_right_pos = - autoware::universe_utils::calcOffsetPose(pose, base_to_front, -base_to_right, 0.0).position; + autoware_utils::calc_offset_pose(pose, base_to_front, -base_to_right, 0.0).position; const auto bottom_right_pos = - autoware::universe_utils::calcOffsetPose(pose, -base_to_rear, -base_to_right, 0.0).position; + autoware_utils::calc_offset_pose(pose, -base_to_rear, -base_to_right, 0.0).position; const auto bottom_left_pos = - autoware::universe_utils::calcOffsetPose(pose, -base_to_rear, base_to_left, 0.0).position; + autoware_utils::calc_offset_pose(pose, -base_to_rear, base_to_left, 0.0).position; if (use_footprint_polygon_for_outside_drivable_area_check) { // calculate footprint polygon diff --git a/planning/autoware_path_smoother/include/autoware/path_smoother/common_structs.hpp b/planning/autoware_path_smoother/include/autoware/path_smoother/common_structs.hpp index 0b7e28ee1cbb5..7b353241053e9 100644 --- a/planning/autoware_path_smoother/include/autoware/path_smoother/common_structs.hpp +++ b/planning/autoware_path_smoother/include/autoware/path_smoother/common_structs.hpp @@ -16,8 +16,8 @@ #define AUTOWARE__PATH_SMOOTHER__COMMON_STRUCTS_HPP_ #include "autoware/path_smoother/type_alias.hpp" -#include "autoware/universe_utils/ros/update_param.hpp" -#include "autoware/universe_utils/system/stop_watch.hpp" +#include "autoware_utils/ros/update_param.hpp" +#include "autoware_utils/system/stop_watch.hpp" #include "rclcpp/rclcpp.hpp" #include @@ -83,7 +83,7 @@ struct TimeKeeper double accumulated_time{0.0}; - autoware::universe_utils::StopWatch< + autoware_utils::StopWatch< std::chrono::milliseconds, std::chrono::microseconds, std::chrono::steady_clock> stop_watch_; }; @@ -100,12 +100,12 @@ struct CommonParam void onParam(const std::vector & parameters) { - using autoware::universe_utils::updateParam; + using autoware_utils::update_param; // common - updateParam( + update_param( parameters, "common.output_backward_traj_length", output_backward_traj_length); - updateParam(parameters, "common.output_delta_arc_length", output_delta_arc_length); + update_param(parameters, "common.output_delta_arc_length", output_delta_arc_length); } double output_delta_arc_length; @@ -123,9 +123,9 @@ struct EgoNearestParam void onParam(const std::vector & parameters) { - using autoware::universe_utils::updateParam; - updateParam(parameters, "ego_nearest_dist_threshold", dist_threshold); - updateParam(parameters, "ego_nearest_yaw_threshold", yaw_threshold); + using autoware_utils::update_param; + update_param(parameters, "ego_nearest_dist_threshold", dist_threshold); + update_param(parameters, "ego_nearest_yaw_threshold", yaw_threshold); } double dist_threshold{0.0}; diff --git a/planning/autoware_path_smoother/include/autoware/path_smoother/elastic_band_smoother.hpp b/planning/autoware_path_smoother/include/autoware/path_smoother/elastic_band_smoother.hpp index 2de74a4f14f4f..f3e1076076916 100644 --- a/planning/autoware_path_smoother/include/autoware/path_smoother/elastic_band_smoother.hpp +++ b/planning/autoware_path_smoother/include/autoware/path_smoother/elastic_band_smoother.hpp @@ -20,11 +20,11 @@ #include "autoware/path_smoother/elastic_band.hpp" #include "autoware/path_smoother/replan_checker.hpp" #include "autoware/path_smoother/type_alias.hpp" -#include "autoware/universe_utils/ros/logger_level_configure.hpp" -#include "autoware/universe_utils/ros/polling_subscriber.hpp" +#include "autoware_utils/ros/logger_level_configure.hpp" +#include "autoware_utils/ros/polling_subscriber.hpp" #include "rclcpp/rclcpp.hpp" -#include +#include #include #include @@ -82,8 +82,7 @@ class ElasticBandSmoother : public rclcpp::Node // interface subscriber rclcpp::Subscription::SharedPtr path_sub_; - autoware::universe_utils::InterProcessPollingSubscriber odom_sub_{ - this, "~/input/odometry"}; + autoware_utils::InterProcessPollingSubscriber odom_sub_{this, "~/input/odometry"}; // debug publisher rclcpp::Publisher::SharedPtr debug_extended_traj_pub_; @@ -113,9 +112,9 @@ class ElasticBandSmoother : public rclcpp::Node const std::vector & traj_points, const std::vector & optimized_points) const; - std::unique_ptr logger_configure_; + std::unique_ptr logger_configure_; - std::unique_ptr published_time_publisher_; + std::unique_ptr published_time_publisher_; }; } // namespace autoware::path_smoother diff --git a/planning/autoware_path_smoother/include/autoware/path_smoother/utils/geometry_utils.hpp b/planning/autoware_path_smoother/include/autoware/path_smoother/utils/geometry_utils.hpp index 603ae8a91f944..9a397d731bd78 100644 --- a/planning/autoware_path_smoother/include/autoware/path_smoother/utils/geometry_utils.hpp +++ b/planning/autoware_path_smoother/include/autoware/path_smoother/utils/geometry_utils.hpp @@ -15,7 +15,7 @@ #ifndef AUTOWARE__PATH_SMOOTHER__UTILS__GEOMETRY_UTILS_HPP_ #define AUTOWARE__PATH_SMOOTHER__UTILS__GEOMETRY_UTILS_HPP_ -#include +#include namespace autoware::path_smoother { @@ -24,8 +24,8 @@ namespace geometry_utils template bool isSamePoint(const T1 & t1, const T2 & t2) { - const auto p1 = autoware::universe_utils::getPoint(t1); - const auto p2 = autoware::universe_utils::getPoint(t2); + const auto p1 = autoware_utils::get_point(t1); + const auto p2 = autoware_utils::get_point(t2); constexpr double epsilon = 1e-6; return (std::abs(p1.x - p2.x) <= epsilon && std::abs(p1.y - p2.y) <= epsilon); diff --git a/planning/autoware_path_smoother/include/autoware/path_smoother/utils/trajectory_utils.hpp b/planning/autoware_path_smoother/include/autoware/path_smoother/utils/trajectory_utils.hpp index f523eb14986e7..45b0e80ab98bb 100644 --- a/planning/autoware_path_smoother/include/autoware/path_smoother/utils/trajectory_utils.hpp +++ b/planning/autoware_path_smoother/include/autoware/path_smoother/utils/trajectory_utils.hpp @@ -21,7 +21,7 @@ #include "autoware/motion_utils/trajectory/trajectory.hpp" #include "autoware/path_smoother/common_structs.hpp" #include "autoware/path_smoother/type_alias.hpp" -#include "autoware/universe_utils/geometry/geometry.hpp" +#include "autoware_utils/geometry/geometry.hpp" #include @@ -57,7 +57,7 @@ std::optional getPointIndexAfter( // search forward if (sum_length < min_offset) { for (size_t i = target_seg_idx + 1; i < points.size(); ++i) { - sum_length += autoware::universe_utils::calcDistance2d(points.at(i), points.at(i - 1)); + sum_length += autoware_utils::calc_distance2d(points.at(i), points.at(i - 1)); if (min_offset < sum_length) { output_idx = i - 1; } @@ -71,7 +71,7 @@ std::optional getPointIndexAfter( // search backward for (size_t i = target_seg_idx; 0 < i; --i) { // NOTE: use size_t since i is always positive value - sum_length -= autoware::universe_utils::calcDistance2d(points.at(i), points.at(i - 1)); + sum_length -= autoware_utils::calc_distance2d(points.at(i), points.at(i - 1)); if (sum_length < min_offset) { output_idx = i - 1; } @@ -87,7 +87,7 @@ template TrajectoryPoint convertToTrajectoryPoint(const T & point) { TrajectoryPoint traj_point; - traj_point.pose = autoware::universe_utils::getPose(point); + traj_point.pose = autoware_utils::get_pose(point); traj_point.longitudinal_velocity_mps = point.longitudinal_velocity_mps; traj_point.lateral_velocity_mps = point.lateral_velocity_mps; traj_point.heading_rate_rps = point.heading_rate_rps; @@ -129,7 +129,7 @@ std::optional updateFrontPointForFix( { // calculate front point to insert in points as a fixed point const size_t front_seg_idx_for_fix = trajectory_utils::findEgoSegmentIndex( - points_for_fix, autoware::universe_utils::getPose(points.front()), ego_nearest_param); + points_for_fix, autoware_utils::get_pose(points.front()), ego_nearest_param); const size_t front_point_idx_for_fix = front_seg_idx_for_fix; const auto & front_fix_point = points_for_fix.at(front_point_idx_for_fix); @@ -143,7 +143,7 @@ std::optional updateFrontPointForFix( return std::nullopt; } - const double dist = autoware::universe_utils::calcDistance2d(points.front(), front_fix_point); + const double dist = autoware_utils::calc_distance2d(points.front(), front_fix_point); // check if deviation is not too large constexpr double max_lat_error = 3.0; diff --git a/planning/autoware_path_smoother/package.xml b/planning/autoware_path_smoother/package.xml index 6d2ea76a15c12..3d99b757357b8 100644 --- a/planning/autoware_path_smoother/package.xml +++ b/planning/autoware_path_smoother/package.xml @@ -20,7 +20,7 @@ autoware_osqp_interface autoware_planning_msgs autoware_planning_test_manager - autoware_universe_utils + autoware_utils geometry_msgs nav_msgs rclcpp diff --git a/planning/autoware_path_smoother/src/elastic_band.cpp b/planning/autoware_path_smoother/src/elastic_band.cpp index 8c751e698c8fb..59cc4024aeedf 100644 --- a/planning/autoware_path_smoother/src/elastic_band.cpp +++ b/planning/autoware_path_smoother/src/elastic_band.cpp @@ -126,38 +126,38 @@ EBPathSmoother::EBParam::EBParam(rclcpp::Node * node) void EBPathSmoother::EBParam::onParam(const std::vector & parameters) { - using autoware::universe_utils::updateParam; + using autoware_utils::update_param; { // option - updateParam(parameters, "elastic_band.option.enable_warm_start", enable_warm_start); - updateParam( + update_param(parameters, "elastic_band.option.enable_warm_start", enable_warm_start); + update_param( parameters, "elastic_band.option.enable_optimization_validation", enable_optimization_validation); } { // common - updateParam(parameters, "elastic_band.common.delta_arc_length", delta_arc_length); - updateParam(parameters, "elastic_band.common.num_points", num_points); + update_param(parameters, "elastic_band.common.delta_arc_length", delta_arc_length); + update_param(parameters, "elastic_band.common.num_points", num_points); } { // clearance - updateParam(parameters, "elastic_band.clearance.num_joint_points", num_joint_points); - updateParam(parameters, "elastic_band.clearance.clearance_for_fix", clearance_for_fix); - updateParam( + update_param(parameters, "elastic_band.clearance.num_joint_points", num_joint_points); + update_param(parameters, "elastic_band.clearance.clearance_for_fix", clearance_for_fix); + update_param( parameters, "elastic_band.clearance.clearance_for_joint", clearance_for_joint); - updateParam( + update_param( parameters, "elastic_band.clearance.clearance_for_smooth", clearance_for_smooth); } { // weight - updateParam(parameters, "elastic_band.weight.smooth_weight", smooth_weight); - updateParam(parameters, "elastic_band.weight.lat_error_weight", lat_error_weight); + update_param(parameters, "elastic_band.weight.smooth_weight", smooth_weight); + update_param(parameters, "elastic_band.weight.lat_error_weight", lat_error_weight); } { // qp - updateParam(parameters, "elastic_band.qp.max_iteration", qp_param.max_iteration); - updateParam(parameters, "elastic_band.qp.eps_abs", qp_param.eps_abs); - updateParam(parameters, "elastic_band.qp.eps_rel", qp_param.eps_rel); + update_param(parameters, "elastic_band.qp.max_iteration", qp_param.max_iteration); + update_param(parameters, "elastic_band.qp.eps_abs", qp_param.eps_abs); + update_param(parameters, "elastic_band.qp.eps_rel", qp_param.eps_rel); } } @@ -443,8 +443,7 @@ std::optional> EBPathSmoother::convertOptimizedPoin } auto eb_traj_point = traj_points.at(i); - eb_traj_point.pose = - autoware::universe_utils::calcOffsetPose(eb_traj_point.pose, 0.0, lat_offset, 0.0); + eb_traj_point.pose = autoware_utils::calc_offset_pose(eb_traj_point.pose, 0.0, lat_offset, 0.0); eb_traj_points.push_back(eb_traj_point); } diff --git a/planning/autoware_path_smoother/src/elastic_band_smoother.cpp b/planning/autoware_path_smoother/src/elastic_band_smoother.cpp index 2569faa4b8232..2a15a9c2b2229 100644 --- a/planning/autoware_path_smoother/src/elastic_band_smoother.cpp +++ b/planning/autoware_path_smoother/src/elastic_band_smoother.cpp @@ -108,15 +108,14 @@ ElasticBandSmoother::ElasticBandSmoother(const rclcpp::NodeOptions & node_option set_param_res_ = this->add_on_set_parameters_callback( std::bind(&ElasticBandSmoother::onParam, this, std::placeholders::_1)); - logger_configure_ = std::make_unique(this); - published_time_publisher_ = - std::make_unique(this); + logger_configure_ = std::make_unique(this); + published_time_publisher_ = std::make_unique(this); } rcl_interfaces::msg::SetParametersResult ElasticBandSmoother::onParam( const std::vector & parameters) { - using autoware::universe_utils::updateParam; + using autoware_utils::update_param; // parameters for ego nearest search ego_nearest_param_.onParam(parameters); @@ -158,7 +157,7 @@ void ElasticBandSmoother::onPath(const Path::ConstSharedPtr path_ptr) time_keeper_ptr_->tic(__func__); // check if data is ready and valid - const auto ego_state_ptr = odom_sub_.takeData(); + const auto ego_state_ptr = odom_sub_.take_data(); if (!isDataReady(*path_ptr, ego_state_ptr, *get_clock())) { return; } diff --git a/planning/autoware_path_smoother/src/replan_checker.cpp b/planning/autoware_path_smoother/src/replan_checker.cpp index 5739aa7481831..9388351a62179 100644 --- a/planning/autoware_path_smoother/src/replan_checker.cpp +++ b/planning/autoware_path_smoother/src/replan_checker.cpp @@ -16,8 +16,8 @@ #include "autoware/motion_utils/trajectory/trajectory.hpp" #include "autoware/path_smoother/utils/trajectory_utils.hpp" -#include "autoware/universe_utils/geometry/geometry.hpp" -#include "autoware/universe_utils/ros/update_param.hpp" +#include "autoware_utils/geometry/geometry.hpp" +#include "autoware_utils/ros/update_param.hpp" #include #include @@ -41,18 +41,18 @@ ReplanChecker::ReplanChecker(rclcpp::Node * node, const EgoNearestParam & ego_ne void ReplanChecker::onParam(const std::vector & parameters) { - using autoware::universe_utils::updateParam; + using autoware_utils::update_param; - updateParam(parameters, "replan.enable", enable_); - updateParam( + update_param(parameters, "replan.enable", enable_); + update_param( parameters, "replan.max_path_shape_around_ego_lat_dist", max_path_shape_around_ego_lat_dist_); - updateParam( + update_param( parameters, "replan.max_path_shape_forward_lat_dist", max_path_shape_forward_lat_dist_); - updateParam( + update_param( parameters, "replan.max_path_shape_forward_lon_dist", max_path_shape_forward_lon_dist_); - updateParam(parameters, "replan.max_ego_moving_dist", max_ego_moving_dist_); - updateParam(parameters, "replan.max_goal_moving_dist", max_goal_moving_dist_); - updateParam(parameters, "replan.max_delta_time_sec", max_delta_time_sec_); + update_param(parameters, "replan.max_ego_moving_dist", max_ego_moving_dist_); + update_param(parameters, "replan.max_goal_moving_dist", max_goal_moving_dist_); + update_param(parameters, "replan.max_delta_time_sec", max_delta_time_sec_); } bool ReplanChecker::isResetRequired(const PlannerData & planner_data) const @@ -81,7 +81,7 @@ bool ReplanChecker::isResetRequired(const PlannerData & planner_data) const // ego pose is lost or new ego pose is designated in simulation const double delta_dist = - autoware::universe_utils::calcDistance2d(p.ego_pose, prev_ego_pose_ptr_->position); + autoware_utils::calc_distance2d(p.ego_pose, prev_ego_pose_ptr_->position); if (max_ego_moving_dist_ < delta_dist) { RCLCPP_DEBUG( logger_, @@ -202,7 +202,7 @@ bool ReplanChecker::isPathGoalChanged( } const double goal_moving_dist = - autoware::universe_utils::calcDistance2d(p.traj_points.back(), prev_traj_points.back()); + autoware_utils::calc_distance2d(p.traj_points.back(), prev_traj_points.back()); if (goal_moving_dist < max_goal_moving_dist_) { return false; } diff --git a/planning/autoware_path_smoother/src/utils/trajectory_utils.cpp b/planning/autoware_path_smoother/src/utils/trajectory_utils.cpp index 15784bf814dc7..36b0faecb1073 100644 --- a/planning/autoware_path_smoother/src/utils/trajectory_utils.cpp +++ b/planning/autoware_path_smoother/src/utils/trajectory_utils.cpp @@ -17,7 +17,7 @@ #include "autoware/motion_utils/resample/resample.hpp" #include "autoware/motion_utils/trajectory/conversion.hpp" #include "autoware/path_smoother/utils/geometry_utils.hpp" -#include "autoware/universe_utils/geometry/geometry.hpp" +#include "autoware_utils/geometry/geometry.hpp" #include "autoware_planning_msgs/msg/path_point.hpp" #include "autoware_planning_msgs/msg/trajectory_point.hpp" diff --git a/planning/autoware_planning_factor_interface/package.xml b/planning/autoware_planning_factor_interface/package.xml index 1a5a4d3f9dbd7..c2be2168f8ce7 100644 --- a/planning/autoware_planning_factor_interface/package.xml +++ b/planning/autoware_planning_factor_interface/package.xml @@ -15,7 +15,7 @@ autoware_motion_utils autoware_planning_msgs - autoware_universe_utils + autoware_utils rclcpp tier4_planning_msgs diff --git a/planning/autoware_planning_topic_converter/package.xml b/planning/autoware_planning_topic_converter/package.xml index cc81643ca5a37..dee592d332248 100644 --- a/planning/autoware_planning_topic_converter/package.xml +++ b/planning/autoware_planning_topic_converter/package.xml @@ -16,7 +16,7 @@ autoware_motion_utils autoware_planning_msgs - autoware_universe_utils + autoware_utils rclcpp rclcpp_components diff --git a/planning/autoware_planning_topic_converter/src/path_to_trajectory.cpp b/planning/autoware_planning_topic_converter/src/path_to_trajectory.cpp index 76bb660259323..01964a7f1336d 100644 --- a/planning/autoware_planning_topic_converter/src/path_to_trajectory.cpp +++ b/planning/autoware_planning_topic_converter/src/path_to_trajectory.cpp @@ -15,7 +15,7 @@ #include "autoware/planning_topic_converter/path_to_trajectory.hpp" #include -#include +#include #include @@ -26,7 +26,7 @@ namespace TrajectoryPoint convertToTrajectoryPoint(const PathPoint & point) { TrajectoryPoint traj_point; - traj_point.pose = autoware::universe_utils::getPose(point); + traj_point.pose = autoware_utils::get_pose(point); traj_point.longitudinal_velocity_mps = point.longitudinal_velocity_mps; traj_point.lateral_velocity_mps = point.lateral_velocity_mps; traj_point.heading_rate_rps = point.heading_rate_rps; diff --git a/planning/autoware_planning_validator/include/autoware/planning_validator/planning_validator.hpp b/planning/autoware_planning_validator/include/autoware/planning_validator/planning_validator.hpp index 298e7f74c289b..427f3cc93aa9e 100644 --- a/planning/autoware_planning_validator/include/autoware/planning_validator/planning_validator.hpp +++ b/planning/autoware_planning_validator/include/autoware/planning_validator/planning_validator.hpp @@ -16,13 +16,13 @@ #define AUTOWARE__PLANNING_VALIDATOR__PLANNING_VALIDATOR_HPP_ #include "autoware/planning_validator/debug_marker.hpp" -#include "autoware/universe_utils/ros/logger_level_configure.hpp" -#include "autoware/universe_utils/ros/polling_subscriber.hpp" -#include "autoware/universe_utils/system/stop_watch.hpp" #include "autoware_planning_validator/msg/planning_validator_status.hpp" +#include "autoware_utils/ros/logger_level_configure.hpp" +#include "autoware_utils/ros/polling_subscriber.hpp" +#include "autoware_utils/system/stop_watch.hpp" #include "autoware_vehicle_info_utils/vehicle_info_utils.hpp" -#include +#include #include #include @@ -36,11 +36,11 @@ namespace autoware::planning_validator { -using autoware::universe_utils::StopWatch; using autoware_internal_debug_msgs::msg::Float64Stamped; using autoware_planning_msgs::msg::Trajectory; using autoware_planning_msgs::msg::TrajectoryPoint; using autoware_planning_validator::msg::PlanningValidatorStatus; +using autoware_utils::StopWatch; using diagnostic_updater::DiagnosticStatusWrapper; using diagnostic_updater::Updater; using nav_msgs::msg::Odometry; @@ -103,7 +103,7 @@ class PlanningValidator : public rclcpp::Node void setStatus(DiagnosticStatusWrapper & stat, const bool & is_ok, const std::string & msg); - autoware::universe_utils::InterProcessPollingSubscriber sub_kinematics_{ + autoware_utils::InterProcessPollingSubscriber sub_kinematics_{ this, "~/input/kinematics"}; rclcpp::Subscription::SharedPtr sub_traj_; rclcpp::Publisher::SharedPtr pub_traj_; @@ -137,9 +137,9 @@ class PlanningValidator : public rclcpp::Node std::shared_ptr debug_pose_publisher_; - std::unique_ptr logger_configure_; + std::unique_ptr logger_configure_; - std::unique_ptr published_time_publisher_; + std::unique_ptr published_time_publisher_; StopWatch stop_watch_; }; diff --git a/planning/autoware_planning_validator/package.xml b/planning/autoware_planning_validator/package.xml index fd369a259bd66..85c053bf2d2f8 100644 --- a/planning/autoware_planning_validator/package.xml +++ b/planning/autoware_planning_validator/package.xml @@ -18,7 +18,7 @@ autoware_motion_utils autoware_planning_msgs autoware_planning_test_manager - autoware_universe_utils + autoware_utils autoware_vehicle_info_utils diagnostic_updater geometry_msgs diff --git a/planning/autoware_planning_validator/src/debug_marker.cpp b/planning/autoware_planning_validator/src/debug_marker.cpp index 7de23de8eda01..fb7d9d9960a1f 100644 --- a/planning/autoware_planning_validator/src/debug_marker.cpp +++ b/planning/autoware_planning_validator/src/debug_marker.cpp @@ -15,7 +15,7 @@ #include "autoware/planning_validator/debug_marker.hpp" #include -#include +#include #include #include @@ -48,25 +48,25 @@ void PlanningValidatorDebugMarkerPublisher::pushPoseMarker( void PlanningValidatorDebugMarkerPublisher::pushPoseMarker( const geometry_msgs::msg::Pose & pose, const std::string & ns, int id) { - using autoware::universe_utils::createMarkerColor; + using autoware_utils::create_marker_color; // append arrow marker std_msgs::msg::ColorRGBA color; if (id == 0) // Red { - color = createMarkerColor(1.0, 0.0, 0.0, 0.999); + color = create_marker_color(1.0, 0.0, 0.0, 0.999); } if (id == 1) // Green { - color = createMarkerColor(0.0, 1.0, 0.0, 0.999); + color = create_marker_color(0.0, 1.0, 0.0, 0.999); } if (id == 2) // Blue { - color = createMarkerColor(0.0, 0.0, 1.0, 0.999); + color = create_marker_color(0.0, 0.0, 1.0, 0.999); } - Marker marker = autoware::universe_utils::createDefaultMarker( + Marker marker = autoware_utils::create_default_marker( "map", node_->get_clock()->now(), ns, getMarkerId(ns), Marker::ARROW, - autoware::universe_utils::createMarkerScale(0.2, 0.1, 0.3), color); + autoware_utils::create_marker_scale(0.2, 0.1, 0.3), color); marker.lifetime = rclcpp::Duration::from_seconds(0.2); marker.pose = pose; @@ -76,10 +76,10 @@ void PlanningValidatorDebugMarkerPublisher::pushPoseMarker( void PlanningValidatorDebugMarkerPublisher::pushWarningMsg( const geometry_msgs::msg::Pose & pose, const std::string & msg) { - visualization_msgs::msg::Marker marker = autoware::universe_utils::createDefaultMarker( + visualization_msgs::msg::Marker marker = autoware_utils::create_default_marker( "map", node_->get_clock()->now(), "warning_msg", 0, Marker::TEXT_VIEW_FACING, - autoware::universe_utils::createMarkerScale(0.0, 0.0, 1.0), - autoware::universe_utils::createMarkerColor(1.0, 0.1, 0.1, 0.999)); + autoware_utils::create_marker_scale(0.0, 0.0, 1.0), + autoware_utils::create_marker_color(1.0, 0.1, 0.1, 0.999)); marker.lifetime = rclcpp::Duration::from_seconds(0.2); marker.pose = pose; marker.text = msg; @@ -91,7 +91,7 @@ void PlanningValidatorDebugMarkerPublisher::pushVirtualWall(const geometry_msgs: const auto now = node_->get_clock()->now(); const auto stop_wall_marker = autoware::motion_utils::createStopVirtualWallMarker( pose, "autoware_planning_validator", now, 0); - autoware::universe_utils::appendMarkerArray(stop_wall_marker, &marker_array_virtual_wall_, now); + autoware_utils::append_marker_array(stop_wall_marker, &marker_array_virtual_wall_, now); } void PlanningValidatorDebugMarkerPublisher::publish() diff --git a/planning/autoware_planning_validator/src/planning_validator.cpp b/planning/autoware_planning_validator/src/planning_validator.cpp index 29e8932c87842..e608192873c98 100644 --- a/planning/autoware_planning_validator/src/planning_validator.cpp +++ b/planning/autoware_planning_validator/src/planning_validator.cpp @@ -17,7 +17,7 @@ #include "autoware/planning_validator/utils.hpp" #include -#include +#include #include #include @@ -44,9 +44,8 @@ PlanningValidator::PlanningValidator(const rclcpp::NodeOptions & options) setupParameters(); - logger_configure_ = std::make_unique(this); - published_time_publisher_ = - std::make_unique(this); + logger_configure_ = std::make_unique(this); + published_time_publisher_ = std::make_unique(this); } void PlanningValidator::setupParameters() @@ -194,7 +193,7 @@ void PlanningValidator::onTrajectory(const Trajectory::ConstSharedPtr msg) current_trajectory_ = msg; // receive data - current_kinematics_ = sub_kinematics_.takeData(); + current_kinematics_ = sub_kinematics_.take_data(); if (!isDataReady()) return; @@ -469,8 +468,8 @@ bool PlanningValidator::checkValidDistanceDeviation(const Trajectory & trajector const auto idx = autoware::motion_utils::findFirstNearestIndexWithSoftConstraints( trajectory.points, current_kinematics_->pose.pose); - validation_status_.distance_deviation = autoware::universe_utils::calcDistance2d( - trajectory.points.at(idx), current_kinematics_->pose.pose); + validation_status_.distance_deviation = + autoware_utils::calc_distance2d(trajectory.points.at(idx), current_kinematics_->pose.pose); if (validation_status_.distance_deviation > validation_params_.distance_deviation_threshold) { return false; @@ -501,7 +500,7 @@ bool PlanningValidator::checkValidLongitudinalDistanceDeviation(const Trajectory // for last, need to remove distance for the last segment. if (is_last) { const auto size = trajectory.points.size(); - long_offset -= autoware::universe_utils::calcDistance2d( + long_offset -= autoware_utils::calc_distance2d( trajectory.points.at(size - 1), trajectory.points.at(size - 2)); } diff --git a/planning/autoware_planning_validator/src/utils.cpp b/planning/autoware_planning_validator/src/utils.cpp index 4304cd6146647..73629d5d813fd 100644 --- a/planning/autoware_planning_validator/src/utils.cpp +++ b/planning/autoware_planning_validator/src/utils.cpp @@ -15,7 +15,7 @@ #include "autoware/planning_validator/utils.hpp" #include -#include +#include #include #include @@ -25,9 +25,9 @@ namespace autoware::planning_validator { -using autoware::universe_utils::calcCurvature; -using autoware::universe_utils::calcDistance2d; -using autoware::universe_utils::getPoint; +using autoware_utils::calc_curvature; +using autoware_utils::calc_distance2d; +using autoware_utils::get_point; namespace { @@ -69,7 +69,7 @@ Trajectory resampleTrajectory(const Trajectory & trajectory, const double min_in for (size_t i = 1; i < trajectory.points.size(); ++i) { const auto prev = resampled.points.back(); const auto curr = trajectory.points.at(i); - if (calcDistance2d(prev, curr) > min_interval) { + if (calc_distance2d(prev, curr) > min_interval) { resampled.points.push_back(curr); } } @@ -90,7 +90,7 @@ void calcCurvature( std::vector arc_length(trajectory.points.size(), 0.0); for (size_t i = 1; i < trajectory.points.size(); ++i) { arc_length.at(i) = - arc_length.at(i - 1) + calcDistance2d(trajectory.points.at(i - 1), trajectory.points.at(i)); + arc_length.at(i - 1) + calc_distance2d(trajectory.points.at(i - 1), trajectory.points.at(i)); } // initialize with 0 curvature @@ -121,11 +121,11 @@ void calcCurvature( } } - const auto p1 = getPoint(trajectory.points.at(prev_idx)); - const auto p2 = getPoint(trajectory.points.at(i)); - const auto p3 = getPoint(trajectory.points.at(next_idx)); + const auto p1 = get_point(trajectory.points.at(prev_idx)); + const auto p2 = get_point(trajectory.points.at(i)); + const auto p3 = get_point(trajectory.points.at(next_idx)); try { - curvature_arr.at(i) = autoware::universe_utils::calcCurvature(p1, p2, p3); + curvature_arr.at(i) = autoware_utils::calc_curvature(p1, p2, p3); } catch (...) { curvature_arr.at(i) = 0.0; // maybe distance is too close } @@ -164,7 +164,7 @@ std::pair calcMaxIntervalDistance(const Trajectory & trajectory) double max_interval_distances = 0.0; size_t max_index = 0; for (size_t i = 1; i < trajectory.points.size(); ++i) { - const auto d = calcDistance2d(trajectory.points.at(i), trajectory.points.at(i - 1)); + const auto d = calc_distance2d(trajectory.points.at(i), trajectory.points.at(i - 1)); if (max_interval_distances < std::abs(d)) { takeBigger(max_interval_distances, max_index, std::abs(d), i); } @@ -223,12 +223,11 @@ std::pair calcMaxRelativeAngles(const Trajectory & trajectory) const auto & p2 = trajectory.points.at(i + 1).pose.position; const auto & p3 = trajectory.points.at(i + 2).pose.position; - const auto angle_a = autoware::universe_utils::calcAzimuthAngle(p1, p2); - const auto angle_b = autoware::universe_utils::calcAzimuthAngle(p2, p3); + const auto angle_a = autoware_utils::calc_azimuth_angle(p1, p2); + const auto angle_b = autoware_utils::calc_azimuth_angle(p2, p3); // convert relative angle to [-pi ~ pi] - const auto relative_angle = - std::abs(autoware::universe_utils::normalizeRadian(angle_b - angle_a)); + const auto relative_angle = std::abs(autoware_utils::normalize_radian(angle_b - angle_a)); takeBigger(max_relative_angles, max_index, std::abs(relative_angle), i); } @@ -276,7 +275,7 @@ std::pair calcMaxSteeringRates( for (size_t i = 0; i < trajectory.points.size() - 1; ++i) { const auto & p_prev = trajectory.points.at(i); const auto & p_next = trajectory.points.at(i + 1); - const auto delta_s = calcDistance2d(p_prev, p_next); + const auto delta_s = calc_distance2d(p_prev, p_next); const auto v = 0.5 * (p_next.longitudinal_velocity_mps + p_prev.longitudinal_velocity_mps); const auto dt = delta_s / std::max(v, 1.0e-5); diff --git a/planning/autoware_planning_validator/test/src/test_planning_validator_helper.cpp b/planning/autoware_planning_validator/test/src/test_planning_validator_helper.cpp index bdf4ca96d9fdd..af3ae6518a057 100644 --- a/planning/autoware_planning_validator/test/src/test_planning_validator_helper.cpp +++ b/planning/autoware_planning_validator/test/src/test_planning_validator_helper.cpp @@ -14,14 +14,14 @@ #include "test_planning_validator_helper.hpp" -#include "autoware/universe_utils/geometry/geometry.hpp" +#include "autoware_utils/geometry/geometry.hpp" #include "test_parameter.hpp" #include -using autoware::universe_utils::createQuaternionFromYaw; using autoware_planning_msgs::msg::Trajectory; using autoware_planning_msgs::msg::TrajectoryPoint; +using autoware_utils::create_quaternion_from_yaw; Trajectory generateTrajectoryWithConstantAcceleration( const double interval_distance, const double speed, const double yaw, const size_t size, @@ -34,7 +34,7 @@ Trajectory generateTrajectoryWithConstantAcceleration( TrajectoryPoint p; p.pose.position.x = s * std::cos(yaw); p.pose.position.y = s * std::sin(yaw); - p.pose.orientation = createQuaternionFromYaw(yaw); + p.pose.orientation = create_quaternion_from_yaw(yaw); p.longitudinal_velocity_mps = v; p.acceleration_mps2 = a; p.front_wheel_angle_rad = 0.0; @@ -77,7 +77,7 @@ Trajectory generateTrajectoryWithConstantCurvature( TrajectoryPoint p; p.pose.position.x = x; p.pose.position.y = y; - p.pose.orientation = createQuaternionFromYaw(yaw); + p.pose.orientation = create_quaternion_from_yaw(yaw); p.longitudinal_velocity_mps = speed; p.front_wheel_angle_rad = steering; trajectory.points.push_back(p); @@ -118,7 +118,7 @@ Trajectory generateTrajectoryWithConstantSteeringRate( TrajectoryPoint p; p.pose.position.x = x; p.pose.position.y = y; - p.pose.orientation = createQuaternionFromYaw(yaw); + p.pose.orientation = create_quaternion_from_yaw(yaw); p.longitudinal_velocity_mps = speed; p.front_wheel_angle_rad = steering_angle_rad; p.acceleration_mps2 = 0.0; diff --git a/planning/autoware_remaining_distance_time_calculator/package.xml b/planning/autoware_remaining_distance_time_calculator/package.xml index 25a223fbd7a3c..488811bb19b14 100644 --- a/planning/autoware_remaining_distance_time_calculator/package.xml +++ b/planning/autoware_remaining_distance_time_calculator/package.xml @@ -17,7 +17,7 @@ autoware_planning_msgs autoware_route_handler autoware_test_utils - autoware_universe_utils + autoware_utils generate_parameter_library geometry_msgs nav_msgs diff --git a/planning/autoware_remaining_distance_time_calculator/src/remaining_distance_time_calculator_node.cpp b/planning/autoware_remaining_distance_time_calculator/src/remaining_distance_time_calculator_node.cpp index 778e7ab30538f..85778c8acd484 100644 --- a/planning/autoware_remaining_distance_time_calculator/src/remaining_distance_time_calculator_node.cpp +++ b/planning/autoware_remaining_distance_time_calculator/src/remaining_distance_time_calculator_node.cpp @@ -14,10 +14,10 @@ #include "remaining_distance_time_calculator_node.hpp" -#include #include #include #include +#include #include #include diff --git a/planning/autoware_rtc_interface/README.md b/planning/autoware_rtc_interface/README.md index 0446ca2d3ca4d..00b481719e021 100644 --- a/planning/autoware_rtc_interface/README.md +++ b/planning/autoware_rtc_interface/README.md @@ -13,7 +13,7 @@ RTC Interface is an interface to publish the decision status of behavior plannin autoware::rtc_interface::RTCInterface rtc_interface(node, "intersection"); // Generate UUID -const unique_identifier_msgs::msg::UUID uuid = generateUUID(getModuleId()); +const unique_identifier_msgs::msg::UUID uuid = generate_uuid(getModuleId()); // Repeat while module is running while (...) { diff --git a/planning/autoware_rtc_interface/package.xml b/planning/autoware_rtc_interface/package.xml index c6d35ea0e5e93..3fbbdb80e02fe 100644 --- a/planning/autoware_rtc_interface/package.xml +++ b/planning/autoware_rtc_interface/package.xml @@ -16,7 +16,7 @@ ament_cmake_auto autoware_cmake - autoware_universe_utils + autoware_utils rclcpp tier4_rtc_msgs unique_identifier_msgs diff --git a/planning/autoware_rtc_interface/test/test_rtc_interface.cpp b/planning/autoware_rtc_interface/test/test_rtc_interface.cpp index d4b0195186282..016801519c948 100644 --- a/planning/autoware_rtc_interface/test/test_rtc_interface.cpp +++ b/planning/autoware_rtc_interface/test/test_rtc_interface.cpp @@ -14,7 +14,7 @@ #include "autoware/rtc_interface/rtc_interface.hpp" -#include +#include #include #include @@ -56,7 +56,7 @@ class RTCInterfaceTest : public ::testing::Test TEST_F(RTCInterfaceTest, uuid_to_string) { - auto uuid = autoware::universe_utils::generateUUID(); + auto uuid = autoware_utils::generate_uuid(); rclcpp::Time stamp(1.0, 0); // Condition: no registered uuid diff --git a/planning/autoware_scenario_selector/include/autoware/scenario_selector/node.hpp b/planning/autoware_scenario_selector/include/autoware/scenario_selector/node.hpp index c74f39d50c93d..ae6a29b691845 100644 --- a/planning/autoware_scenario_selector/include/autoware/scenario_selector/node.hpp +++ b/planning/autoware_scenario_selector/include/autoware/scenario_selector/node.hpp @@ -15,7 +15,7 @@ #ifndef AUTOWARE__SCENARIO_SELECTOR__NODE_HPP_ #define AUTOWARE__SCENARIO_SELECTOR__NODE_HPP_ -#include +#include #include #include @@ -37,8 +37,8 @@ #include #endif #include -#include -#include +#include +#include #include #include @@ -101,10 +101,10 @@ class ScenarioSelectorNode : public rclcpp::Node pub_processing_time_; // polling subscribers - universe_utils::InterProcessPollingSubscriber< - nav_msgs::msg::Odometry, autoware::universe_utils::polling_policy::All>::SharedPtr sub_odom_; - universe_utils::InterProcessPollingSubscriber::SharedPtr sub_parking_state_; - universe_utils::InterProcessPollingSubscriber< + autoware_utils::InterProcessPollingSubscriber< + nav_msgs::msg::Odometry, autoware_utils::polling_policy::All>::SharedPtr sub_odom_; + autoware_utils::InterProcessPollingSubscriber::SharedPtr sub_parking_state_; + autoware_utils::InterProcessPollingSubscriber< autoware_adapi_v1_msgs::msg::OperationModeState>::SharedPtr sub_operation_mode_state_; autoware_adapi_v1_msgs::msg::OperationModeState::ConstSharedPtr operation_mode_state_; @@ -118,7 +118,7 @@ class ScenarioSelectorNode : public rclcpp::Node std::deque twist_buffer_; std::shared_ptr route_handler_; - std::unique_ptr published_time_publisher_; + std::unique_ptr published_time_publisher_; // Parameters double update_rate_; @@ -136,7 +136,7 @@ class ScenarioSelectorNode : public rclcpp::Node static constexpr double empty_parking_trajectory_timeout_s = 3.0; // processing time - autoware::universe_utils::StopWatch stop_watch; + autoware_utils::StopWatch stop_watch; }; } // namespace autoware::scenario_selector #endif // AUTOWARE__SCENARIO_SELECTOR__NODE_HPP_ diff --git a/planning/autoware_scenario_selector/package.xml b/planning/autoware_scenario_selector/package.xml index eabd381891771..2ee4f01c3b6d4 100644 --- a/planning/autoware_scenario_selector/package.xml +++ b/planning/autoware_scenario_selector/package.xml @@ -21,7 +21,7 @@ autoware_planning_msgs autoware_planning_test_manager autoware_route_handler - autoware_universe_utils + autoware_utils nav_msgs rclcpp rclcpp_components diff --git a/planning/autoware_scenario_selector/src/node.cpp b/planning/autoware_scenario_selector/src/node.cpp index dcb87b7487fe0..743a5eb625f94 100644 --- a/planning/autoware_scenario_selector/src/node.cpp +++ b/planning/autoware_scenario_selector/src/node.cpp @@ -14,9 +14,9 @@ #include "autoware/scenario_selector/node.hpp" -#include #include #include +#include #include #include @@ -350,19 +350,19 @@ void ScenarioSelectorNode::updateData() stop_watch.tic(); } { - auto msg = sub_parking_state_->takeData(); + auto msg = sub_parking_state_->take_data(); is_parking_completed_ = msg ? msg->data : is_parking_completed_; } { - auto msgs = sub_odom_->takeData(); + auto msgs = sub_odom_->take_data(); for (const auto & msg : msgs) { onOdom(msg); } } { - auto msg = sub_operation_mode_state_->takeData(); + auto msg = sub_operation_mode_state_->take_data(); if (msg) operation_mode_state_ = msg; } } @@ -489,8 +489,7 @@ ScenarioSelectorNode::ScenarioSelectorNode(const rclcpp::NodeOptions & node_opti timer_ = rclcpp::create_timer( this, get_clock(), period_ns, std::bind(&ScenarioSelectorNode::onTimer, this)); - published_time_publisher_ = - std::make_unique(this); + published_time_publisher_ = std::make_unique(this); pub_processing_time_ = this->create_publisher( "~/debug/processing_time_ms", 1); } diff --git a/planning/autoware_surround_obstacle_checker/package.xml b/planning/autoware_surround_obstacle_checker/package.xml index e5ee999aa24d8..075a03b7002a2 100644 --- a/planning/autoware_surround_obstacle_checker/package.xml +++ b/planning/autoware_surround_obstacle_checker/package.xml @@ -19,7 +19,7 @@ autoware_planning_factor_interface autoware_planning_msgs autoware_planning_test_manager - autoware_universe_utils + autoware_utils autoware_vehicle_info_utils diagnostic_msgs eigen diff --git a/planning/autoware_surround_obstacle_checker/src/debug_marker.cpp b/planning/autoware_surround_obstacle_checker/src/debug_marker.cpp index e34315ad986a5..67e2f34177090 100644 --- a/planning/autoware_surround_obstacle_checker/src/debug_marker.cpp +++ b/planning/autoware_surround_obstacle_checker/src/debug_marker.cpp @@ -15,8 +15,8 @@ #include "debug_marker.hpp" #include -#include -#include +#include +#include #include #ifdef ROS_DISTRO_GALACTIC @@ -54,12 +54,12 @@ Polygon2d createSelfPolygon( } } // namespace -using autoware::universe_utils::appendMarkerArray; -using autoware::universe_utils::calcOffsetPose; -using autoware::universe_utils::createDefaultMarker; -using autoware::universe_utils::createMarkerColor; -using autoware::universe_utils::createMarkerScale; -using autoware::universe_utils::createPoint; +using autoware_utils::append_marker_array; +using autoware_utils::calc_offset_pose; +using autoware_utils::create_default_marker; +using autoware_utils::create_marker_color; +using autoware_utils::create_marker_scale; +using autoware_utils::create_point; SurroundObstacleCheckerDebugNode::SurroundObstacleCheckerDebugNode( const autoware::vehicle_info_utils::VehicleInfo & vehicle_info, const std::string & object_label, @@ -163,9 +163,9 @@ MarkerArray SurroundObstacleCheckerDebugNode::makeVisualizationMarker() // visualize surround object if (stop_obstacle_point_ptr_ != nullptr) { - auto marker = createDefaultMarker( + auto marker = create_default_marker( "map", current_time, "no_start_obstacle_text", 0, Marker::TEXT_VIEW_FACING, - createMarkerScale(0.0, 0.0, 1.0), createMarkerColor(1.0, 1.0, 1.0, 0.999)); + create_marker_scale(0.0, 0.0, 1.0), create_marker_color(1.0, 1.0, 1.0, 0.999)); marker.pose.position = *stop_obstacle_point_ptr_; marker.pose.position.z += 2.0; // add half of the heights of obj roughly marker.text = "!"; diff --git a/planning/autoware_surround_obstacle_checker/src/node.cpp b/planning/autoware_surround_obstacle_checker/src/node.cpp index b3881a8f4fbd9..69cf53f900377 100644 --- a/planning/autoware_surround_obstacle_checker/src/node.cpp +++ b/planning/autoware_surround_obstacle_checker/src/node.cpp @@ -14,11 +14,11 @@ #include "node.hpp" -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include #include #include @@ -54,9 +54,9 @@ namespace autoware::surround_obstacle_checker namespace bg = boost::geometry; using Point2d = bg::model::d2::point_xy; using Polygon2d = bg::model::polygon; -using autoware::universe_utils::createPoint; -using autoware::universe_utils::pose2transform; using autoware_perception_msgs::msg::ObjectClassification; +using autoware_utils::create_point; +using autoware_utils::pose2transform; SurroundObstacleCheckerNode::SurroundObstacleCheckerNode(const rclcpp::NodeOptions & node_options) : Node("surround_obstacle_checker_node", node_options) @@ -71,7 +71,7 @@ SurroundObstacleCheckerNode::SurroundObstacleCheckerNode(const rclcpp::NodeOptio param_listener_ = std::make_shared( this->get_node_parameters_interface()); - logger_configure_ = std::make_unique(this); + logger_configure_ = std::make_unique(this); } vehicle_info_ = autoware::vehicle_info_utils::VehicleInfoUtils(*this).getVehicleInfo(); @@ -125,12 +125,12 @@ bool SurroundObstacleCheckerNode::getUseDynamicObject() const void SurroundObstacleCheckerNode::onTimer() { - autoware::universe_utils::StopWatch stop_watch; + autoware_utils::StopWatch stop_watch; stop_watch.tic(); - odometry_ptr_ = sub_odometry_.takeData(); - pointcloud_ptr_ = sub_pointcloud_.takeData(); - object_ptr_ = sub_dynamic_objects_.takeData(); + odometry_ptr_ = sub_odometry_.take_data(); + pointcloud_ptr_ = sub_pointcloud_.take_data(); + object_ptr_ = sub_dynamic_objects_.take_data(); if (!odometry_ptr_) { RCLCPP_INFO_THROTTLE( @@ -282,8 +282,7 @@ std::optional SurroundObstacleCheckerNode::getNearestObstacleByPointCl tf2::transformToEigen(transform_stamped.value().transform).cast(); pcl::PointCloud transformed_pointcloud; pcl::fromROSMsg(*pointcloud_ptr_, transformed_pointcloud); - autoware::universe_utils::transformPointCloud( - transformed_pointcloud, transformed_pointcloud, isometry); + autoware_utils::transform_pointcloud(transformed_pointcloud, transformed_pointcloud, isometry); const auto & pointcloud_param = param.obstacle_types_map.at("pointcloud"); const double front_margin = pointcloud_param.surround_check_front_distance; @@ -292,8 +291,8 @@ std::optional SurroundObstacleCheckerNode::getNearestObstacleByPointCl const double base_to_front = vehicle_info_.max_longitudinal_offset_m + front_margin; const double base_to_rear = vehicle_info_.rear_overhang_m + back_margin; const double width = vehicle_info_.vehicle_width_m + side_margin * 2; - const auto ego_polygon = autoware::universe_utils::toFootprint( - odometry_ptr_->pose.pose, base_to_front, base_to_rear, width); + const auto ego_polygon = + autoware_utils::to_footprint(odometry_ptr_->pose.pose, base_to_front, base_to_rear, width); geometry_msgs::msg::Point nearest_point; double minimum_distance = std::numeric_limits::max(); @@ -304,7 +303,7 @@ std::optional SurroundObstacleCheckerNode::getNearestObstacleByPointCl const auto distance_to_object = bg::distance(ego_polygon, boost_point); if (distance_to_object < minimum_distance) { - nearest_point = createPoint(p.x, p.y, p.z); + nearest_point = create_point(p.x, p.y, p.z); minimum_distance = distance_to_object; was_minimum_distance_updated = true; } @@ -341,10 +340,10 @@ std::optional SurroundObstacleCheckerNode::getNearestObstacleByDynamic const double base_to_front = vehicle_info_.max_longitudinal_offset_m + front_margin; const double base_to_rear = vehicle_info_.rear_overhang_m + back_margin; const double width = vehicle_info_.vehicle_width_m + side_margin * 2; - const auto ego_polygon = autoware::universe_utils::toFootprint( - odometry_ptr_->pose.pose, base_to_front, base_to_rear, width); + const auto ego_polygon = + autoware_utils::to_footprint(odometry_ptr_->pose.pose, base_to_front, base_to_rear, width); - const auto object_polygon = autoware::universe_utils::toPolygon2d(object); + const auto object_polygon = autoware_utils::to_polygon2d(object); const auto distance_to_object = bg::distance(ego_polygon, object_polygon); diff --git a/planning/autoware_surround_obstacle_checker/src/node.hpp b/planning/autoware_surround_obstacle_checker/src/node.hpp index b6d6bffefed1a..3ed1f5081ae1d 100644 --- a/planning/autoware_surround_obstacle_checker/src/node.hpp +++ b/planning/autoware_surround_obstacle_checker/src/node.hpp @@ -15,8 +15,8 @@ #ifndef NODE_HPP_ #define NODE_HPP_ -#include "autoware/universe_utils/ros/logger_level_configure.hpp" -#include "autoware/universe_utils/ros/polling_subscriber.hpp" +#include "autoware_utils/ros/logger_level_configure.hpp" +#include "autoware_utils/ros/polling_subscriber.hpp" #include "debug_marker.hpp" #include "surround_obstacle_checker_node_parameters.hpp" @@ -98,11 +98,11 @@ class SurroundObstacleCheckerNode : public rclcpp::Node rclcpp::TimerBase::SharedPtr timer_; // publisher and subscriber - autoware::universe_utils::InterProcessPollingSubscriber sub_odometry_{ + autoware_utils::InterProcessPollingSubscriber sub_odometry_{ this, "~/input/odometry"}; - autoware::universe_utils::InterProcessPollingSubscriber - sub_pointcloud_{this, "~/input/pointcloud", autoware::universe_utils::SingleDepthSensorQoS()}; - autoware::universe_utils::InterProcessPollingSubscriber sub_dynamic_objects_{ + autoware_utils::InterProcessPollingSubscriber sub_pointcloud_{ + this, "~/input/pointcloud", autoware_utils::single_depth_sensor_qos()}; + autoware_utils::InterProcessPollingSubscriber sub_dynamic_objects_{ this, "~/input/objects"}; rclcpp::Publisher::SharedPtr pub_clear_velocity_limit_; rclcpp::Publisher::SharedPtr pub_velocity_limit_; @@ -131,7 +131,7 @@ class SurroundObstacleCheckerNode : public rclcpp::Node State state_ = State::PASS; std::optional last_obstacle_found_time_; - std::unique_ptr logger_configure_; + std::unique_ptr logger_configure_; std::unordered_map label_map_; diff --git a/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/node.hpp b/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/node.hpp index d519356c7aa18..d8dccc7778c34 100644 --- a/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/node.hpp +++ b/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/node.hpp @@ -18,14 +18,6 @@ #include "autoware/motion_utils/trajectory/conversion.hpp" #include "autoware/motion_utils/trajectory/trajectory.hpp" #include "autoware/osqp_interface/osqp_interface.hpp" -#include "autoware/universe_utils/geometry/geometry.hpp" -#include "autoware/universe_utils/math/unit_conversion.hpp" -#include "autoware/universe_utils/ros/diagnostics_interface.hpp" -#include "autoware/universe_utils/ros/logger_level_configure.hpp" -#include "autoware/universe_utils/ros/polling_subscriber.hpp" -#include "autoware/universe_utils/ros/self_pose_listener.hpp" -#include "autoware/universe_utils/system/stop_watch.hpp" -#include "autoware/universe_utils/system/time_keeper.hpp" #include "autoware/velocity_smoother/resample.hpp" #include "autoware/velocity_smoother/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.hpp" #include "autoware/velocity_smoother/smoother/jerk_filtered_smoother.hpp" @@ -33,11 +25,19 @@ #include "autoware/velocity_smoother/smoother/linf_pseudo_jerk_smoother.hpp" #include "autoware/velocity_smoother/smoother/smoother_base.hpp" #include "autoware/velocity_smoother/trajectory_utils.hpp" +#include "autoware_utils/geometry/geometry.hpp" +#include "autoware_utils/math/unit_conversion.hpp" +#include "autoware_utils/ros/diagnostics_interface.hpp" +#include "autoware_utils/ros/logger_level_configure.hpp" +#include "autoware_utils/ros/polling_subscriber.hpp" +#include "autoware_utils/ros/self_pose_listener.hpp" +#include "autoware_utils/system/stop_watch.hpp" +#include "autoware_utils/system/time_keeper.hpp" #include "rclcpp/rclcpp.hpp" #include "tf2/utils.h" #include "tf2_ros/transform_listener.h" -#include +#include #include "autoware_adapi_v1_msgs/msg/operation_mode_state.hpp" #include "autoware_internal_debug_msgs/msg/float32_stamped.hpp" @@ -61,10 +61,10 @@ namespace autoware::velocity_smoother using autoware_planning_msgs::msg::Trajectory; using autoware_planning_msgs::msg::TrajectoryPoint; using TrajectoryPoints = std::vector; -using autoware::universe_utils::DiagnosticsInterface; using autoware_adapi_v1_msgs::msg::OperationModeState; using autoware_internal_debug_msgs::msg::Float32Stamped; using autoware_internal_debug_msgs::msg::Float64Stamped; +using autoware_utils::DiagnosticsInterface; using geometry_msgs::msg::AccelWithCovarianceStamped; using geometry_msgs::msg::Pose; using geometry_msgs::msg::PoseStamped; @@ -90,14 +90,14 @@ class VelocitySmootherNode : public rclcpp::Node rclcpp::Publisher::SharedPtr pub_trajectory_; rclcpp::Publisher::SharedPtr pub_virtual_wall_; rclcpp::Subscription::SharedPtr sub_current_trajectory_; - autoware::universe_utils::InterProcessPollingSubscriber sub_current_odometry_{ + autoware_utils::InterProcessPollingSubscriber sub_current_odometry_{ this, "/localization/kinematic_state"}; - autoware::universe_utils::InterProcessPollingSubscriber + autoware_utils::InterProcessPollingSubscriber sub_current_acceleration_{this, "~/input/acceleration"}; - autoware::universe_utils::InterProcessPollingSubscriber< - VelocityLimit, universe_utils::polling_policy::Newest> + autoware_utils::InterProcessPollingSubscriber< + VelocityLimit, autoware_utils::polling_policy::Newest> sub_external_velocity_limit_{this, "~/input/external_velocity_limit_mps"}; - autoware::universe_utils::InterProcessPollingSubscriber sub_operation_mode_{ + autoware_utils::InterProcessPollingSubscriber sub_operation_mode_{ this, "~/input/operation_mode_state"}; Odometry::ConstSharedPtr current_odometry_ptr_; // current odometry @@ -255,7 +255,7 @@ class VelocitySmootherNode : public rclcpp::Node void initCommonParam(); // debug - autoware::universe_utils::StopWatch stop_watch_; + autoware_utils::StopWatch stop_watch_; std::shared_ptr prev_time_; double prev_acc_; rclcpp::Publisher::SharedPtr pub_dist_to_stopline_; @@ -270,8 +270,7 @@ class VelocitySmootherNode : public rclcpp::Node rclcpp::Publisher::SharedPtr debug_closest_jerk_; rclcpp::Publisher::SharedPtr debug_calculation_time_; rclcpp::Publisher::SharedPtr debug_closest_max_velocity_; - rclcpp::Publisher::SharedPtr - debug_processing_time_detail_; + rclcpp::Publisher::SharedPtr debug_processing_time_detail_; // For Jerk Filtered Algorithm Debug rclcpp::Publisher::SharedPtr pub_forward_filtered_trajectory_; @@ -285,10 +284,10 @@ class VelocitySmootherNode : public rclcpp::Node void flipVelocity(TrajectoryPoints & points) const; void publishStopWatchTime(); - std::unique_ptr logger_configure_; - std::unique_ptr published_time_publisher_; + std::unique_ptr logger_configure_; + std::unique_ptr published_time_publisher_; - mutable std::shared_ptr time_keeper_{nullptr}; + mutable std::shared_ptr time_keeper_{nullptr}; std::unique_ptr diagnostics_interface_{nullptr}; }; diff --git a/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.hpp b/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.hpp index bfcd7db6b9c43..7a74351b47be4 100644 --- a/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.hpp +++ b/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.hpp @@ -16,9 +16,9 @@ #define AUTOWARE__VELOCITY_SMOOTHER__SMOOTHER__ANALYTICAL_JERK_CONSTRAINED_SMOOTHER__ANALYTICAL_JERK_CONSTRAINED_SMOOTHER_HPP_ // NOLINT #include "autoware/motion_utils/trajectory/trajectory.hpp" -#include "autoware/universe_utils/system/time_keeper.hpp" #include "autoware/velocity_smoother/smoother/analytical_jerk_constrained_smoother/velocity_planning_utils.hpp" #include "autoware/velocity_smoother/smoother/smoother_base.hpp" +#include "autoware_utils/system/time_keeper.hpp" #include "rclcpp/rclcpp.hpp" #include "tf2/utils.h" @@ -68,8 +68,8 @@ class AnalyticalJerkConstrainedSmoother : public SmootherBase }; explicit AnalyticalJerkConstrainedSmoother( - rclcpp::Node & node, const std::shared_ptr time_keeper = - std::make_shared()); + rclcpp::Node & node, const std::shared_ptr time_keeper = + std::make_shared()); bool apply( const double initial_vel, const double initial_acc, const TrajectoryPoints & input, diff --git a/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/jerk_filtered_smoother.hpp b/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/jerk_filtered_smoother.hpp index 6ddba29bf05a7..96f8645f9d6e4 100644 --- a/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/jerk_filtered_smoother.hpp +++ b/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/jerk_filtered_smoother.hpp @@ -17,9 +17,9 @@ #include "autoware/motion_utils/trajectory/trajectory.hpp" #include "autoware/qp_interface/qp_interface.hpp" -#include "autoware/universe_utils/geometry/geometry.hpp" -#include "autoware/universe_utils/system/time_keeper.hpp" #include "autoware/velocity_smoother/smoother/smoother_base.hpp" +#include "autoware_utils/geometry/geometry.hpp" +#include "autoware_utils/system/time_keeper.hpp" #include "autoware_planning_msgs/msg/trajectory_point.hpp" @@ -43,7 +43,7 @@ class JerkFilteredSmoother : public SmootherBase }; explicit JerkFilteredSmoother( - rclcpp::Node & node, const std::shared_ptr time_keeper); + rclcpp::Node & node, const std::shared_ptr time_keeper); bool apply( const double initial_vel, const double initial_acc, const TrajectoryPoints & input, diff --git a/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/l2_pseudo_jerk_smoother.hpp b/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/l2_pseudo_jerk_smoother.hpp index fda8e8f31df95..cb41a72225fc8 100644 --- a/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/l2_pseudo_jerk_smoother.hpp +++ b/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/l2_pseudo_jerk_smoother.hpp @@ -17,9 +17,9 @@ #include "autoware/motion_utils/trajectory/trajectory.hpp" #include "autoware/osqp_interface/osqp_interface.hpp" -#include "autoware/universe_utils/geometry/geometry.hpp" -#include "autoware/universe_utils/system/time_keeper.hpp" #include "autoware/velocity_smoother/smoother/smoother_base.hpp" +#include "autoware_utils/geometry/geometry.hpp" +#include "autoware_utils/system/time_keeper.hpp" #include "autoware_planning_msgs/msg/trajectory_point.hpp" @@ -41,7 +41,7 @@ class L2PseudoJerkSmoother : public SmootherBase }; explicit L2PseudoJerkSmoother( - rclcpp::Node & node, const std::shared_ptr time_keeper); + rclcpp::Node & node, const std::shared_ptr time_keeper); bool apply( const double initial_vel, const double initial_acc, const TrajectoryPoints & input, diff --git a/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/linf_pseudo_jerk_smoother.hpp b/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/linf_pseudo_jerk_smoother.hpp index 119cb4d358de2..3a80304202176 100644 --- a/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/linf_pseudo_jerk_smoother.hpp +++ b/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/linf_pseudo_jerk_smoother.hpp @@ -17,9 +17,9 @@ #include "autoware/motion_utils/trajectory/trajectory.hpp" #include "autoware/osqp_interface/osqp_interface.hpp" -#include "autoware/universe_utils/geometry/geometry.hpp" -#include "autoware/universe_utils/system/time_keeper.hpp" #include "autoware/velocity_smoother/smoother/smoother_base.hpp" +#include "autoware_utils/geometry/geometry.hpp" +#include "autoware_utils/system/time_keeper.hpp" #include "autoware_planning_msgs/msg/trajectory_point.hpp" @@ -41,7 +41,7 @@ class LinfPseudoJerkSmoother : public SmootherBase }; explicit LinfPseudoJerkSmoother( - rclcpp::Node & node, const std::shared_ptr time_keeper); + rclcpp::Node & node, const std::shared_ptr time_keeper); bool apply( const double initial_vel, const double initial_acc, const TrajectoryPoints & input, diff --git a/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/smoother_base.hpp b/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/smoother_base.hpp index d51431383f88a..d7b6422711aef 100644 --- a/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/smoother_base.hpp +++ b/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/smoother_base.hpp @@ -15,8 +15,8 @@ #ifndef AUTOWARE__VELOCITY_SMOOTHER__SMOOTHER__SMOOTHER_BASE_HPP_ #define AUTOWARE__VELOCITY_SMOOTHER__SMOOTHER__SMOOTHER_BASE_HPP_ -#include "autoware/universe_utils/system/time_keeper.hpp" #include "autoware/velocity_smoother/resample.hpp" +#include "autoware_utils/system/time_keeper.hpp" #include "rclcpp/rclcpp.hpp" #include "autoware_planning_msgs/msg/trajectory_point.hpp" @@ -57,7 +57,7 @@ class SmootherBase }; explicit SmootherBase( - rclcpp::Node & node, const std::shared_ptr time_keeper); + rclcpp::Node & node, const std::shared_ptr time_keeper); virtual ~SmootherBase() = default; virtual bool apply( const double initial_vel, const double initial_acc, const TrajectoryPoints & input, @@ -91,7 +91,7 @@ class SmootherBase protected: BaseParam base_param_; - mutable std::shared_ptr time_keeper_{nullptr}; + mutable std::shared_ptr time_keeper_{nullptr}; }; } // namespace autoware::velocity_smoother diff --git a/planning/autoware_velocity_smoother/package.xml b/planning/autoware_velocity_smoother/package.xml index dc49d947c058d..d25889fa551f7 100644 --- a/planning/autoware_velocity_smoother/package.xml +++ b/planning/autoware_velocity_smoother/package.xml @@ -28,7 +28,7 @@ autoware_planning_msgs autoware_planning_test_manager autoware_qp_interface - autoware_universe_utils + autoware_utils autoware_vehicle_info_utils geometry_msgs nav_msgs diff --git a/planning/autoware_velocity_smoother/src/node.cpp b/planning/autoware_velocity_smoother/src/node.cpp index 1f2b1cad6e42a..c6f08ea910522 100644 --- a/planning/autoware_velocity_smoother/src/node.cpp +++ b/planning/autoware_velocity_smoother/src/node.cpp @@ -15,10 +15,10 @@ #include "autoware/velocity_smoother/node.hpp" #include "autoware/motion_utils/marker/marker_helper.hpp" -#include "autoware/universe_utils/ros/update_param.hpp" #include "autoware/velocity_smoother/smoother/jerk_filtered_smoother.hpp" #include "autoware/velocity_smoother/smoother/l2_pseudo_jerk_smoother.hpp" #include "autoware/velocity_smoother/smoother/linf_pseudo_jerk_smoother.hpp" +#include "autoware_utils/ros/update_param.hpp" #include @@ -50,10 +50,9 @@ VelocitySmootherNode::VelocitySmootherNode(const rclcpp::NodeOptions & node_opti // create time_keeper and its publisher // NOTE: This has to be called before setupSmoother to pass the time_keeper to the smoother. - debug_processing_time_detail_ = create_publisher( - "~/debug/processing_time_detail_ms", 1); - time_keeper_ = - std::make_shared(debug_processing_time_detail_); + debug_processing_time_detail_ = + create_publisher("~/debug/processing_time_detail_ms", 1); + time_keeper_ = std::make_shared(debug_processing_time_detail_); // create smoother setupSmoother(wheelbase_); @@ -98,9 +97,8 @@ VelocitySmootherNode::VelocitySmootherNode(const rclcpp::NodeOptions & node_opti clock_ = get_clock(); - logger_configure_ = std::make_unique(this); - published_time_publisher_ = - std::make_unique(this); + logger_configure_ = std::make_unique(this); + published_time_publisher_ = std::make_unique(this); } void VelocitySmootherNode::setupSmoother(const double wheelbase) @@ -317,7 +315,7 @@ void VelocitySmootherNode::publishTrajectory(const TrajectoryPoints & trajectory void VelocitySmootherNode::calcExternalVelocityLimit() { - autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + autoware_utils::ScopedTimeTrack st(__func__, *time_keeper_); if (!external_velocity_limit_ptr_) { external_velocity_limit_.acceleration_request.request = false; @@ -439,7 +437,7 @@ bool VelocitySmootherNode::checkData() const void VelocitySmootherNode::onCurrentTrajectory(const Trajectory::ConstSharedPtr msg) { - autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + autoware_utils::ScopedTimeTrack st(__func__, *time_keeper_); RCLCPP_DEBUG(get_logger(), "========================= run start ========================="); stop_watch_.tic(); @@ -448,10 +446,10 @@ void VelocitySmootherNode::onCurrentTrajectory(const Trajectory::ConstSharedPtr base_traj_raw_ptr_ = msg; // receive data - current_odometry_ptr_ = sub_current_odometry_.takeData(); - current_acceleration_ptr_ = sub_current_acceleration_.takeData(); - external_velocity_limit_ptr_ = sub_external_velocity_limit_.takeData(); - const auto operation_mode_ptr = sub_operation_mode_.takeData(); + current_odometry_ptr_ = sub_current_odometry_.take_data(); + current_acceleration_ptr_ = sub_current_acceleration_.take_data(); + external_velocity_limit_ptr_ = sub_external_velocity_limit_.take_data(); + const auto operation_mode_ptr = sub_operation_mode_.take_data(); if (operation_mode_ptr) { operation_mode_ = *operation_mode_ptr; } @@ -534,7 +532,7 @@ void VelocitySmootherNode::onCurrentTrajectory(const Trajectory::ConstSharedPtr void VelocitySmootherNode::updateDataForExternalVelocityLimit() { - autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + autoware_utils::ScopedTimeTrack st(__func__, *time_keeper_); if (prev_output_.empty()) { return; @@ -553,7 +551,7 @@ void VelocitySmootherNode::updateDataForExternalVelocityLimit() TrajectoryPoints VelocitySmootherNode::calcTrajectoryVelocity( const TrajectoryPoints & traj_input) const { - autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + autoware_utils::ScopedTimeTrack st(__func__, *time_keeper_); TrajectoryPoints output{}; // velocity is optimized by qp solver @@ -602,7 +600,7 @@ bool VelocitySmootherNode::smoothVelocity( const TrajectoryPoints & input, const size_t input_closest, TrajectoryPoints & traj_smoothed) const { - autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + autoware_utils::ScopedTimeTrack st(__func__, *time_keeper_); if (input.empty()) { return false; // cannot apply smoothing @@ -721,7 +719,7 @@ bool VelocitySmootherNode::smoothVelocity( void VelocitySmootherNode::insertBehindVelocity( const size_t output_closest, const InitializeType type, TrajectoryPoints & output) const { - autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + autoware_utils::ScopedTimeTrack st(__func__, *time_keeper_); const bool keep_closest_vel_for_behind = (type == InitializeType::EGO_VELOCITY || type == InitializeType::LARGE_DEVIATION_REPLAN || @@ -785,7 +783,7 @@ void VelocitySmootherNode::publishStopDistance(const TrajectoryPoints & trajecto std::pair VelocitySmootherNode::calcInitialMotion( const TrajectoryPoints & input_traj, const size_t input_closest) const { - autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + autoware_utils::ScopedTimeTrack st(__func__, *time_keeper_); const double vehicle_speed = std::fabs(current_odometry_ptr_->twist.twist.linear.x); const double vehicle_acceleration = current_acceleration_ptr_->accel.accel.linear.x; @@ -873,7 +871,7 @@ std::pair VelocitySmootherNode::ca void VelocitySmootherNode::overwriteStopPoint( const TrajectoryPoints & input, TrajectoryPoints & output) const { - autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + autoware_utils::ScopedTimeTrack st(__func__, *time_keeper_); const auto stop_idx = autoware::motion_utils::searchZeroVelocityIndex(input); if (!stop_idx) { @@ -917,7 +915,7 @@ void VelocitySmootherNode::overwriteStopPoint( void VelocitySmootherNode::applyExternalVelocityLimit(TrajectoryPoints & traj) const { - autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + autoware_utils::ScopedTimeTrack st(__func__, *time_keeper_); if (traj.size() < 1) { return; @@ -958,7 +956,7 @@ void VelocitySmootherNode::applyExternalVelocityLimit(TrajectoryPoints & traj) c void VelocitySmootherNode::applyStopApproachingVelocity(TrajectoryPoints & traj) const { - autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + autoware_utils::ScopedTimeTrack st(__func__, *time_keeper_); const auto stop_idx = autoware::motion_utils::searchZeroVelocityIndex(traj); if (!stop_idx) { @@ -966,7 +964,7 @@ void VelocitySmootherNode::applyStopApproachingVelocity(TrajectoryPoints & traj) } double distance_sum = 0.0; for (size_t i = *stop_idx - 1; i < traj.size(); --i) { // search backward - distance_sum += autoware::universe_utils::calcDistance2d(traj.at(i), traj.at(i + 1)); + distance_sum += autoware_utils::calc_distance2d(traj.at(i), traj.at(i + 1)); if (distance_sum > node_param_.stopping_distance) { break; } @@ -1075,8 +1073,7 @@ double VelocitySmootherNode::calcTravelDistance() const const auto closest_point = calcProjectedTrajectoryPointFromEgo(prev_output_); if (prev_closest_point_) { - const double travel_dist = - autoware::universe_utils::calcDistance2d(*prev_closest_point_, closest_point); + const double travel_dist = autoware_utils::calc_distance2d(*prev_closest_point_, closest_point); return travel_dist; } diff --git a/planning/autoware_velocity_smoother/src/resample.cpp b/planning/autoware_velocity_smoother/src/resample.cpp index 7be67cea85f6d..cb8710c66a8e5 100644 --- a/planning/autoware_velocity_smoother/src/resample.cpp +++ b/planning/autoware_velocity_smoother/src/resample.cpp @@ -17,8 +17,8 @@ #include "autoware/motion_utils/resample/resample.hpp" #include "autoware/motion_utils/trajectory/conversion.hpp" #include "autoware/motion_utils/trajectory/trajectory.hpp" -#include "autoware/universe_utils/geometry/geometry.hpp" #include "autoware/velocity_smoother/trajectory_utils.hpp" +#include "autoware_utils/geometry/geometry.hpp" #include #include @@ -136,7 +136,7 @@ TrajectoryPoints resampleTrajectory( // add end point directly to consider the endpoint velocity. if (is_endpoint_included) { constexpr double ep_dist = 1.0E-3; - if (autoware::universe_utils::calcDistance2d(output.back(), input.back()) < ep_dist) { + if (autoware_utils::calc_distance2d(output.back(), input.back()) < ep_dist) { output.back() = input.back(); } else { output.push_back(input.back()); @@ -256,7 +256,7 @@ TrajectoryPoints resampleTrajectory( // add end point directly to consider the endpoint velocity. if (is_endpoint_included) { constexpr double ep_dist = 1.0E-3; - if (autoware::universe_utils::calcDistance2d(output.back(), input.back()) < ep_dist) { + if (autoware_utils::calc_distance2d(output.back(), input.back()) < ep_dist) { output.back() = input.back(); } else { output.push_back(input.back()); diff --git a/planning/autoware_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.cpp b/planning/autoware_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.cpp index 85034dfee2bd8..e385c1f9c17d0 100644 --- a/planning/autoware_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.cpp +++ b/planning/autoware_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.cpp @@ -16,8 +16,8 @@ #include "autoware/motion_utils/resample/resample.hpp" #include "autoware/motion_utils/trajectory/conversion.hpp" -#include "autoware/universe_utils/geometry/geometry.hpp" #include "autoware/velocity_smoother/trajectory_utils.hpp" +#include "autoware_utils/geometry/geometry.hpp" #include #include @@ -69,7 +69,7 @@ bool applyMaxVelocity( namespace autoware::velocity_smoother { AnalyticalJerkConstrainedSmoother::AnalyticalJerkConstrainedSmoother( - rclcpp::Node & node, const std::shared_ptr time_keeper) + rclcpp::Node & node, const std::shared_ptr time_keeper) : SmootherBase(node, time_keeper) { auto & p = smoother_param_; @@ -256,7 +256,7 @@ TrajectoryPoints AnalyticalJerkConstrainedSmoother::resampleTrajectory( const auto tp1 = input.at(i + 1); const double dist_thr = 0.001; // 1mm - const double dist_tp0_tp1 = autoware::universe_utils::calcDistance2d(tp0, tp1); + const double dist_tp0_tp1 = autoware_utils::calc_distance2d(tp0, tp1); if (std::fabs(dist_tp0_tp1) < dist_thr) { output.push_back(input.at(i)); continue; @@ -358,9 +358,7 @@ TrajectoryPoints AnalyticalJerkConstrainedSmoother::applyLateralAccelerationFilt continue; } - if ( - autoware::universe_utils::calcDistance2d(output.at(end_index), output.at(index)) < - dist_threshold) { + if (autoware_utils::calc_distance2d(output.at(end_index), output.at(index)) < dist_threshold) { end_index = index; min_latacc_velocity = std::min( static_cast(output.at(index).longitudinal_velocity_mps), min_latacc_velocity); @@ -445,7 +443,7 @@ bool AnalyticalJerkConstrainedSmoother::applyForwardJerkFilter( for (size_t i = start_index + 1; i < base_trajectory.size(); ++i) { const double prev_vel = output_trajectory.at(i - 1).longitudinal_velocity_mps; const double ds = - autoware::universe_utils::calcDistance2d(base_trajectory.at(i - 1), base_trajectory.at(i)); + autoware_utils::calc_distance2d(base_trajectory.at(i - 1), base_trajectory.at(i)); const double dt = ds / std::max(prev_vel, 1.0); const double prev_acc = output_trajectory.at(i - 1).acceleration_mps2; @@ -491,8 +489,7 @@ bool AnalyticalJerkConstrainedSmoother::applyBackwardDecelFilter( } } for (size_t i = decel_target_index; i > start_index; --i) { - dist += autoware::universe_utils::calcDistance2d( - output_trajectory.at(i - 1), output_trajectory.at(i)); + dist += autoware_utils::calc_distance2d(output_trajectory.at(i - 1), output_trajectory.at(i)); dist_to_target.at(i - 1) = dist; } diff --git a/planning/autoware_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/velocity_planning_utils.cpp b/planning/autoware_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/velocity_planning_utils.cpp index 0f4f7121c7597..f9db6277c3c6f 100644 --- a/planning/autoware_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/velocity_planning_utils.cpp +++ b/planning/autoware_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/velocity_planning_utils.cpp @@ -15,7 +15,7 @@ #include "autoware/velocity_smoother/smoother/analytical_jerk_constrained_smoother/velocity_planning_utils.hpp" #include "autoware/interpolation/linear_interpolation.hpp" -#include "autoware/universe_utils/geometry/geometry.hpp" +#include "autoware_utils/geometry/geometry.hpp" #include #include @@ -226,8 +226,8 @@ bool calcStopVelocityWithConstantJerkAccLimit( std::vector distances; distances.push_back(distance); for (size_t i = start_index; i < output_trajectory.size() - 1; ++i) { - distance += autoware::universe_utils::calcDistance2d( - output_trajectory.at(i), output_trajectory.at(i + 1)); + distance += + autoware_utils::calc_distance2d(output_trajectory.at(i), output_trajectory.at(i + 1)); if (distance > xs.back()) { break; } diff --git a/planning/autoware_velocity_smoother/src/smoother/jerk_filtered_smoother.cpp b/planning/autoware_velocity_smoother/src/smoother/jerk_filtered_smoother.cpp index 0f61171a3bb70..c375472d52756 100644 --- a/planning/autoware_velocity_smoother/src/smoother/jerk_filtered_smoother.cpp +++ b/planning/autoware_velocity_smoother/src/smoother/jerk_filtered_smoother.cpp @@ -33,7 +33,7 @@ namespace autoware::velocity_smoother { JerkFilteredSmoother::JerkFilteredSmoother( - rclcpp::Node & node, const std::shared_ptr time_keeper) + rclcpp::Node & node, const std::shared_ptr time_keeper) : SmootherBase(node, time_keeper) { auto & p = smoother_param_; @@ -61,7 +61,7 @@ bool JerkFilteredSmoother::apply( const double v0, const double a0, const TrajectoryPoints & input, TrajectoryPoints & output, std::vector & debug_trajectories, const bool publish_debug_trajs) { - autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + autoware_utils::ScopedTimeTrack st(__func__, *time_keeper_); output = input; @@ -106,7 +106,7 @@ bool JerkFilteredSmoother::apply( const auto initial_traj_pose = filtered.front().pose; const auto resample = [&](const auto & trajectory) { - autoware::universe_utils::ScopedTimeTrack st("resample", *time_keeper_); + autoware_utils::ScopedTimeTrack st("resample", *time_keeper_); return resampling::resampleTrajectory( trajectory, v0, initial_traj_pose, std::numeric_limits::max(), @@ -360,7 +360,7 @@ TrajectoryPoints JerkFilteredSmoother::forwardJerkFilter( const double v0, const double a0, const double a_max, const double a_start, const double j_max, const TrajectoryPoints & input) const { - autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + autoware_utils::ScopedTimeTrack st(__func__, *time_keeper_); auto applyLimits = [&input, &a_start](double & v, double & a, size_t i) { double v_lim = input.at(i).longitudinal_velocity_mps; @@ -391,7 +391,7 @@ TrajectoryPoints JerkFilteredSmoother::forwardJerkFilter( output.front().longitudinal_velocity_mps = current_vel; output.front().acceleration_mps2 = current_acc; for (size_t i = 1; i < input.size(); ++i) { - const double ds = autoware::universe_utils::calcDistance2d(input.at(i), input.at(i - 1)); + const double ds = autoware_utils::calc_distance2d(input.at(i), input.at(i - 1)); const double max_dt = std::pow(6.0 * ds / j_max, 1.0 / 3.0); // assuming v0 = a0 = 0. const double dt = std::min(ds / std::max(current_vel, 1.0e-6), max_dt); @@ -414,7 +414,7 @@ TrajectoryPoints JerkFilteredSmoother::backwardJerkFilter( const double v0, const double a0, const double a_min, const double a_stop, const double j_min, const TrajectoryPoints & input) const { - autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + autoware_utils::ScopedTimeTrack st(__func__, *time_keeper_); auto input_rev = input; std::reverse(input_rev.begin(), input_rev.end()); @@ -431,7 +431,7 @@ TrajectoryPoints JerkFilteredSmoother::mergeFilteredTrajectory( const double v0, const double a0, const double a_min, const double j_min, const TrajectoryPoints & forward_filtered, const TrajectoryPoints & backward_filtered) const { - autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + autoware_utils::ScopedTimeTrack st(__func__, *time_keeper_); TrajectoryPoints merged; merged = forward_filtered; @@ -449,8 +449,8 @@ TrajectoryPoints JerkFilteredSmoother::mergeFilteredTrajectory( merged.at(i).longitudinal_velocity_mps = current_vel; merged.at(i).acceleration_mps2 = current_acc; - const double ds = autoware::universe_utils::calcDistance2d( - forward_filtered.at(i + 1), forward_filtered.at(i)); + const double ds = + autoware_utils::calc_distance2d(forward_filtered.at(i + 1), forward_filtered.at(i)); const double max_dt = std::pow(6.0 * ds / std::fabs(j_min), 1.0 / 3.0); // assuming v0 = a0 = 0. const double dt = std::min(ds / std::max(current_vel, 1.0e-6), max_dt); @@ -485,7 +485,7 @@ TrajectoryPoints JerkFilteredSmoother::resampleTrajectory( const geometry_msgs::msg::Pose & current_pose, const double nearest_dist_threshold, const double nearest_yaw_threshold) const { - autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + autoware_utils::ScopedTimeTrack st(__func__, *time_keeper_); return resampling::resampleTrajectory( input, current_pose, nearest_dist_threshold, nearest_yaw_threshold, base_param_.resample_param, diff --git a/planning/autoware_velocity_smoother/src/smoother/l2_pseudo_jerk_smoother.cpp b/planning/autoware_velocity_smoother/src/smoother/l2_pseudo_jerk_smoother.cpp index 3280512376a58..1c031e97417a6 100644 --- a/planning/autoware_velocity_smoother/src/smoother/l2_pseudo_jerk_smoother.cpp +++ b/planning/autoware_velocity_smoother/src/smoother/l2_pseudo_jerk_smoother.cpp @@ -27,7 +27,7 @@ namespace autoware::velocity_smoother { L2PseudoJerkSmoother::L2PseudoJerkSmoother( - rclcpp::Node & node, const std::shared_ptr time_keeper) + rclcpp::Node & node, const std::shared_ptr time_keeper) : SmootherBase(node, time_keeper) { auto & p = smoother_param_; diff --git a/planning/autoware_velocity_smoother/src/smoother/linf_pseudo_jerk_smoother.cpp b/planning/autoware_velocity_smoother/src/smoother/linf_pseudo_jerk_smoother.cpp index eaad896562ccd..a148f92d72b89 100644 --- a/planning/autoware_velocity_smoother/src/smoother/linf_pseudo_jerk_smoother.cpp +++ b/planning/autoware_velocity_smoother/src/smoother/linf_pseudo_jerk_smoother.cpp @@ -27,7 +27,7 @@ namespace autoware::velocity_smoother { LinfPseudoJerkSmoother::LinfPseudoJerkSmoother( - rclcpp::Node & node, const std::shared_ptr time_keeper) + rclcpp::Node & node, const std::shared_ptr time_keeper) : SmootherBase(node, time_keeper) { auto & p = smoother_param_; diff --git a/planning/autoware_velocity_smoother/src/smoother/smoother_base.cpp b/planning/autoware_velocity_smoother/src/smoother/smoother_base.cpp index e4271a9c387de..5dcd530c791a0 100644 --- a/planning/autoware_velocity_smoother/src/smoother/smoother_base.cpp +++ b/planning/autoware_velocity_smoother/src/smoother/smoother_base.cpp @@ -17,10 +17,10 @@ #include "autoware/motion_utils/resample/resample.hpp" #include "autoware/motion_utils/trajectory/conversion.hpp" #include "autoware/motion_utils/trajectory/trajectory.hpp" -#include "autoware/universe_utils/geometry/geometry.hpp" -#include "autoware/universe_utils/math/unit_conversion.hpp" #include "autoware/velocity_smoother/resample.hpp" #include "autoware/velocity_smoother/trajectory_utils.hpp" +#include "autoware_utils/geometry/geometry.hpp" +#include "autoware_utils/math/unit_conversion.hpp" #include #include @@ -63,7 +63,7 @@ TrajectoryPoints applyPreProcess( } // namespace SmootherBase::SmootherBase( - rclcpp::Node & node, const std::shared_ptr time_keeper) + rclcpp::Node & node, const std::shared_ptr time_keeper) : time_keeper_(time_keeper) { auto & p = base_param_; @@ -144,7 +144,7 @@ TrajectoryPoints SmootherBase::applyLateralAccelerationFilter( [[maybe_unused]] const double a0, [[maybe_unused]] const bool enable_smooth_limit, const bool use_resampling, const double input_points_interval) const { - autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + autoware_utils::ScopedTimeTrack st(__func__, *time_keeper_); if (input.size() < 3) { return input; // cannot calculate lateral acc. do nothing. @@ -214,7 +214,7 @@ TrajectoryPoints SmootherBase::applySteeringRateLimit( const TrajectoryPoints & input, const bool use_resampling, const double input_points_interval) const { - autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + autoware_utils::ScopedTimeTrack st(__func__, *time_keeper_); if (input.size() < 3) { return input; // cannot calculate the desired velocity. do nothing. @@ -267,15 +267,14 @@ TrajectoryPoints SmootherBase::applySteeringRateLimit( } const auto steer_rate = steer_rate_arr.at(i); - if (steer_rate < autoware::universe_utils::deg2rad(base_param_.max_steering_angle_rate)) { + if (steer_rate < autoware_utils::deg2rad(base_param_.max_steering_angle_rate)) { continue; } const auto mean_vel = (output.at(i).longitudinal_velocity_mps + output.at(i + 1).longitudinal_velocity_mps) / 2.0; const auto target_mean_vel = - mean_vel * - (autoware::universe_utils::deg2rad(base_param_.max_steering_angle_rate) / steer_rate); + mean_vel * (autoware_utils::deg2rad(base_param_.max_steering_angle_rate) / steer_rate); for (size_t k = 0; k < 2; k++) { auto & velocity = output.at(i + k).longitudinal_velocity_mps; diff --git a/planning/autoware_velocity_smoother/src/trajectory_utils.cpp b/planning/autoware_velocity_smoother/src/trajectory_utils.cpp index cf031601cfd57..e3748ffcead3d 100644 --- a/planning/autoware_velocity_smoother/src/trajectory_utils.cpp +++ b/planning/autoware_velocity_smoother/src/trajectory_utils.cpp @@ -16,7 +16,7 @@ #include "autoware/interpolation/linear_interpolation.hpp" #include "autoware/motion_utils/trajectory/trajectory.hpp" -#include "autoware/universe_utils/geometry/geometry.hpp" +#include "autoware_utils/geometry/geometry.hpp" #include #include @@ -34,7 +34,7 @@ inline void convertEulerAngleToMonotonic(std::vector & a) { for (unsigned int i = 1; i < a.size(); ++i) { const double da = a[i] - a[i - 1]; - a[i] = a[i - 1] + autoware::universe_utils::normalizeRadian(da); + a[i] = a[i - 1] + autoware_utils::normalize_radian(da); } } @@ -87,7 +87,7 @@ TrajectoryPoint calcInterpolatedTrajectoryPoint( { const auto & seg_pt = trajectory.at(seg_idx); const auto & next_pt = trajectory.at(seg_idx + 1); - traj_p.pose = autoware::universe_utils::calcInterpolatedPose(seg_pt.pose, next_pt.pose, prop); + traj_p.pose = autoware_utils::calc_interpolated_pose(seg_pt.pose, next_pt.pose, prop); traj_p.longitudinal_velocity_mps = autoware::interpolation::lerp( seg_pt.longitudinal_velocity_mps, next_pt.longitudinal_velocity_mps, prop); traj_p.acceleration_mps2 = @@ -110,7 +110,7 @@ TrajectoryPoints extractPathAroundIndex( { double dist_sum = 0.0; for (size_t i = index; i < trajectory.size() - 1; ++i) { - dist_sum += autoware::universe_utils::calcDistance2d(trajectory.at(i), trajectory.at(i + 1)); + dist_sum += autoware_utils::calc_distance2d(trajectory.at(i), trajectory.at(i + 1)); if (dist_sum > ahead_length) { ahead_index = i + 1; break; @@ -123,7 +123,7 @@ TrajectoryPoints extractPathAroundIndex( { double dist_sum{0.0}; for (size_t i = index; i != 0; --i) { - dist_sum += autoware::universe_utils::calcDistance2d(trajectory.at(i), trajectory[i - 1]); + dist_sum += autoware_utils::calc_distance2d(trajectory.at(i), trajectory[i - 1]); if (dist_sum > behind_length) { behind_index = i - 1; break; @@ -152,7 +152,7 @@ std::vector calcArclengthArray(const TrajectoryPoints & trajectory) for (unsigned int i = 1; i < trajectory.size(); ++i) { const TrajectoryPoint tp = trajectory.at(i); const TrajectoryPoint tp_prev = trajectory.at(i - 1); - dist += autoware::universe_utils::calcDistance2d(tp.pose, tp_prev.pose); + dist += autoware_utils::calc_distance2d(tp.pose, tp_prev.pose); arclength.at(i) = dist; } return arclength; @@ -164,7 +164,7 @@ std::vector calcTrajectoryIntervalDistance(const TrajectoryPoints & traj for (unsigned int i = 1; i < trajectory.size(); ++i) { const TrajectoryPoint tp = trajectory.at(i); const TrajectoryPoint tp_prev = trajectory.at(i - 1); - const double dist = autoware::universe_utils::calcDistance2d(tp.pose, tp_prev.pose); + const double dist = autoware_utils::calc_distance2d(tp.pose, tp_prev.pose); intervals.push_back(dist); } return intervals; @@ -173,8 +173,8 @@ std::vector calcTrajectoryIntervalDistance(const TrajectoryPoints & traj std::vector calcTrajectoryCurvatureFrom3Points( const TrajectoryPoints & trajectory, size_t idx_dist) { - using autoware::universe_utils::calcCurvature; - using autoware::universe_utils::getPoint; + using autoware_utils::calc_curvature; + using autoware_utils::get_point; if (trajectory.size() < 3) { const std::vector k_arr(trajectory.size(), 0.0); @@ -194,11 +194,11 @@ std::vector calcTrajectoryCurvatureFrom3Points( for (size_t i = 1; i + 1 < trajectory.size(); i++) { double curvature = 0.0; - const auto p0 = getPoint(trajectory.at(i - std::min(idx_dist, i))); - const auto p1 = getPoint(trajectory.at(i)); - const auto p2 = getPoint(trajectory.at(i + std::min(idx_dist, trajectory.size() - 1 - i))); + const auto p0 = get_point(trajectory.at(i - std::min(idx_dist, i))); + const auto p1 = get_point(trajectory.at(i)); + const auto p2 = get_point(trajectory.at(i + std::min(idx_dist, trajectory.size() - 1 - i))); try { - curvature = calcCurvature(p0, p1, p2); + curvature = calc_curvature(p0, p1, p2); } catch (std::exception const & e) { // ...code that handles the error... RCLCPP_WARN( @@ -466,7 +466,7 @@ double calcStopDistance(const TrajectoryPoints & trajectory, const size_t closes // TODO(Horibe): use arc length distance const double stop_dist = - autoware::universe_utils::calcDistance2d(trajectory.at(*idx), trajectory.at(closest)); + autoware_utils::calc_distance2d(trajectory.at(*idx), trajectory.at(closest)); return stop_dist; } diff --git a/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/package.xml b/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/package.xml index 5350adf23fa26..7780176ccbf5c 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/package.xml +++ b/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/package.xml @@ -27,7 +27,7 @@ autoware_lanelet2_extension autoware_motion_utils autoware_rtc_interface - autoware_universe_utils + autoware_utils pluginlib rclcpp tier4_planning_msgs diff --git a/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/manager.cpp b/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/manager.cpp index 2bf8a60676d72..d99a0ea7ab0d9 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/manager.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/manager.cpp @@ -15,8 +15,8 @@ #include "manager.hpp" #include "autoware/behavior_path_static_obstacle_avoidance_module/parameter_helper.hpp" -#include "autoware/universe_utils/ros/parameter.hpp" -#include "autoware/universe_utils/ros/update_param.hpp" +#include "autoware_utils/ros/parameter.hpp" +#include "autoware_utils/ros/update_param.hpp" #include "data_structs.hpp" #include @@ -32,8 +32,8 @@ using autoware::behavior_path_planner::ObjectParameter; void AvoidanceByLaneChangeModuleManager::init(rclcpp::Node * node) { - using autoware::universe_utils::getOrDeclareParameter; using autoware_perception_msgs::msg::ObjectClassification; + using autoware_utils::get_or_declare_parameter; // init manager interface initInterface(node, {"left", "right"}); @@ -48,35 +48,36 @@ void AvoidanceByLaneChangeModuleManager::init(rclcpp::Node * node) { const std::string ns = "avoidance_by_lane_change."; p.execute_object_longitudinal_margin = - getOrDeclareParameter(*node, ns + "execute_object_longitudinal_margin"); - p.execute_only_when_lane_change_finish_before_object = - getOrDeclareParameter(*node, ns + "execute_only_when_lane_change_finish_before_object"); + get_or_declare_parameter(*node, ns + "execute_object_longitudinal_margin"); + p.execute_only_when_lane_change_finish_before_object = get_or_declare_parameter( + *node, ns + "execute_only_when_lane_change_finish_before_object"); } // general params { const std::string ns = "avoidance."; p.resample_interval_for_planning = - getOrDeclareParameter(*node, ns + "resample_interval_for_planning"); + get_or_declare_parameter(*node, ns + "resample_interval_for_planning"); p.resample_interval_for_output = - getOrDeclareParameter(*node, ns + "resample_interval_for_output"); + get_or_declare_parameter(*node, ns + "resample_interval_for_output"); } // target object { const auto get_object_param = [&](std::string && ns) { ObjectParameter param{}; - param.moving_speed_threshold = getOrDeclareParameter(*node, ns + "th_moving_speed"); - param.moving_time_threshold = getOrDeclareParameter(*node, ns + "th_moving_time"); - param.max_expand_ratio = getOrDeclareParameter(*node, ns + "max_expand_ratio"); + param.moving_speed_threshold = + get_or_declare_parameter(*node, ns + "th_moving_speed"); + param.moving_time_threshold = get_or_declare_parameter(*node, ns + "th_moving_time"); + param.max_expand_ratio = get_or_declare_parameter(*node, ns + "max_expand_ratio"); param.envelope_buffer_margin = - getOrDeclareParameter(*node, ns + "envelope_buffer_margin"); + get_or_declare_parameter(*node, ns + "envelope_buffer_margin"); param.lateral_soft_margin = - getOrDeclareParameter(*node, ns + "lateral_margin.soft_margin"); + get_or_declare_parameter(*node, ns + "lateral_margin.soft_margin"); param.lateral_hard_margin = - getOrDeclareParameter(*node, ns + "lateral_margin.hard_margin"); - param.lateral_hard_margin_for_parked_vehicle = - getOrDeclareParameter(*node, ns + "lateral_margin.hard_margin_for_parked_vehicle"); + get_or_declare_parameter(*node, ns + "lateral_margin.hard_margin"); + param.lateral_hard_margin_for_parked_vehicle = get_or_declare_parameter( + *node, ns + "lateral_margin.hard_margin_for_parked_vehicle"); return param; }; @@ -93,9 +94,9 @@ void AvoidanceByLaneChangeModuleManager::init(rclcpp::Node * node) p.object_parameters.emplace(ObjectClassification::UNKNOWN, get_object_param(ns + "unknown.")); p.lower_distance_for_polygon_expansion = - getOrDeclareParameter(*node, ns + "lower_distance_for_polygon_expansion"); + get_or_declare_parameter(*node, ns + "lower_distance_for_polygon_expansion"); p.upper_distance_for_polygon_expansion = - getOrDeclareParameter(*node, ns + "upper_distance_for_polygon_expansion"); + get_or_declare_parameter(*node, ns + "upper_distance_for_polygon_expansion"); } // target filtering @@ -105,7 +106,7 @@ void AvoidanceByLaneChangeModuleManager::init(rclcpp::Node * node) return; } p.object_parameters.at(object_type).is_avoidance_target = - getOrDeclareParameter(*node, ns); + get_or_declare_parameter(*node, ns); }; const std::string ns = "avoidance.target_filtering."; @@ -119,68 +120,68 @@ void AvoidanceByLaneChangeModuleManager::init(rclcpp::Node * node) set_target_flag(ObjectClassification::UNKNOWN, ns + "target_type.unknown"); p.object_check_goal_distance = - getOrDeclareParameter(*node, ns + "object_check_goal_distance"); + get_or_declare_parameter(*node, ns + "object_check_goal_distance"); p.object_last_seen_threshold = - getOrDeclareParameter(*node, ns + "max_compensation_time"); + get_or_declare_parameter(*node, ns + "max_compensation_time"); } { const std::string ns = "avoidance.target_filtering.parked_vehicle."; p.threshold_distance_object_is_on_center = - getOrDeclareParameter(*node, ns + "th_offset_from_centerline"); + get_or_declare_parameter(*node, ns + "th_offset_from_centerline"); p.object_check_shiftable_ratio = - getOrDeclareParameter(*node, ns + "th_shiftable_ratio"); + get_or_declare_parameter(*node, ns + "th_shiftable_ratio"); p.object_check_min_road_shoulder_width = - getOrDeclareParameter(*node, ns + "min_road_shoulder_width"); + get_or_declare_parameter(*node, ns + "min_road_shoulder_width"); } { const std::string ns = "avoidance.target_filtering.avoidance_for_ambiguous_vehicle."; - p.policy_ambiguous_vehicle = getOrDeclareParameter(*node, ns + "policy"); - p.wait_and_see_target_behaviors = - getOrDeclareParameter>(*node, ns + "wait_and_see.target_behaviors"); + p.policy_ambiguous_vehicle = get_or_declare_parameter(*node, ns + "policy"); + p.wait_and_see_target_behaviors = get_or_declare_parameter>( + *node, ns + "wait_and_see.target_behaviors"); p.wait_and_see_th_closest_distance = - getOrDeclareParameter(*node, ns + "wait_and_see.th_closest_distance"); + get_or_declare_parameter(*node, ns + "wait_and_see.th_closest_distance"); p.time_threshold_for_ambiguous_vehicle = - getOrDeclareParameter(*node, ns + "condition.th_stopped_time"); + get_or_declare_parameter(*node, ns + "condition.th_stopped_time"); p.distance_threshold_for_ambiguous_vehicle = - getOrDeclareParameter(*node, ns + "condition.th_moving_distance"); + get_or_declare_parameter(*node, ns + "condition.th_moving_distance"); p.object_ignore_section_traffic_light_in_front_distance = - getOrDeclareParameter(*node, ns + "ignore_area.traffic_light.front_distance"); + get_or_declare_parameter(*node, ns + "ignore_area.traffic_light.front_distance"); p.object_ignore_section_crosswalk_in_front_distance = - getOrDeclareParameter(*node, ns + "ignore_area.crosswalk.front_distance"); + get_or_declare_parameter(*node, ns + "ignore_area.crosswalk.front_distance"); p.object_ignore_section_crosswalk_behind_distance = - getOrDeclareParameter(*node, ns + "ignore_area.crosswalk.behind_distance"); + get_or_declare_parameter(*node, ns + "ignore_area.crosswalk.behind_distance"); } // avoidance maneuver (longitudinal) { const std::string ns = "avoidance.avoidance.longitudinal."; - p.min_prepare_time = getOrDeclareParameter(*node, ns + "min_prepare_time"); - p.max_prepare_time = getOrDeclareParameter(*node, ns + "max_prepare_time"); - p.min_prepare_distance = getOrDeclareParameter(*node, ns + "min_prepare_distance"); - p.min_slow_down_speed = getOrDeclareParameter(*node, ns + "min_slow_down_speed"); - p.buf_slow_down_speed = getOrDeclareParameter(*node, ns + "buf_slow_down_speed"); + p.min_prepare_time = get_or_declare_parameter(*node, ns + "min_prepare_time"); + p.max_prepare_time = get_or_declare_parameter(*node, ns + "max_prepare_time"); + p.min_prepare_distance = get_or_declare_parameter(*node, ns + "min_prepare_distance"); + p.min_slow_down_speed = get_or_declare_parameter(*node, ns + "min_slow_down_speed"); + p.buf_slow_down_speed = get_or_declare_parameter(*node, ns + "buf_slow_down_speed"); p.nominal_avoidance_speed = - getOrDeclareParameter(*node, ns + "nominal_avoidance_speed"); + get_or_declare_parameter(*node, ns + "nominal_avoidance_speed"); } { const std::string ns = "avoidance.target_filtering.detection_area."; - p.use_static_detection_area = getOrDeclareParameter(*node, ns + "static"); + p.use_static_detection_area = get_or_declare_parameter(*node, ns + "static"); p.object_check_min_forward_distance = - getOrDeclareParameter(*node, ns + "min_forward_distance"); + get_or_declare_parameter(*node, ns + "min_forward_distance"); p.object_check_max_forward_distance = - getOrDeclareParameter(*node, ns + "max_forward_distance"); + get_or_declare_parameter(*node, ns + "max_forward_distance"); p.object_check_backward_distance = - getOrDeclareParameter(*node, ns + "backward_distance"); + get_or_declare_parameter(*node, ns + "backward_distance"); } // safety check { const std::string ns = "avoidance.safety_check."; p.hysteresis_factor_expand_rate = - getOrDeclareParameter(*node, ns + "hysteresis_factor_expand_rate"); + get_or_declare_parameter(*node, ns + "hysteresis_factor_expand_rate"); } avoidance_parameters_ = std::make_shared(p); diff --git a/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/scene.cpp b/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/scene.cpp index 7e4bc4d5ba6ac..fe1a7a7e9e291 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/scene.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/scene.cpp @@ -59,14 +59,10 @@ geometry_msgs::msg::Polygon create_execution_area( const double backward_lon_offset = -base_to_rear; const double lat_offset = width / 2.0 + additional_lat_offset; - const auto p1 = - autoware::universe_utils::calcOffsetPose(pose, forward_lon_offset, lat_offset, 0.0); - const auto p2 = - autoware::universe_utils::calcOffsetPose(pose, forward_lon_offset, -lat_offset, 0.0); - const auto p3 = - autoware::universe_utils::calcOffsetPose(pose, backward_lon_offset, -lat_offset, 0.0); - const auto p4 = - autoware::universe_utils::calcOffsetPose(pose, backward_lon_offset, lat_offset, 0.0); + const auto p1 = autoware_utils::calc_offset_pose(pose, forward_lon_offset, lat_offset, 0.0); + const auto p2 = autoware_utils::calc_offset_pose(pose, forward_lon_offset, -lat_offset, 0.0); + const auto p3 = autoware_utils::calc_offset_pose(pose, backward_lon_offset, -lat_offset, 0.0); + const auto p4 = autoware_utils::calc_offset_pose(pose, backward_lon_offset, lat_offset, 0.0); geometry_msgs::msg::Polygon polygon; polygon.points.push_back(create_point32(p1)); @@ -237,8 +233,8 @@ std::optional AvoidanceByLaneChange::createObjectData( const AvoidancePlanningData & data, const PredictedObject & object) const { using autoware::motion_utils::findNearestIndex; - using autoware::universe_utils::calcDistance2d; - using autoware::universe_utils::calcLateralDeviation; + using autoware_utils::calc_distance2d; + using autoware_utils::calc_lateral_deviation; using boost::geometry::return_centroid; const auto p = std::dynamic_pointer_cast(avoidance_parameters_); @@ -266,7 +262,7 @@ std::optional AvoidanceByLaneChange::createObjectData( const auto lower = p->lower_distance_for_polygon_expansion; const auto upper = p->upper_distance_for_polygon_expansion; const auto clamp = - std::clamp(calcDistance2d(getEgoPose(), object_pose) - lower, 0.0, upper) / upper; + std::clamp(calc_distance2d(getEgoPose(), object_pose) - lower, 0.0, upper) / upper; object_data.distance_factor = object_parameter.max_expand_ratio * clamp + 1.0; // Calc envelop polygon. @@ -279,7 +275,7 @@ std::optional AvoidanceByLaneChange::createObjectData( // Calc moving time. utils::static_obstacle_avoidance::fillObjectMovingTime(object_data, stopped_objects_, p); - object_data.direction = calcLateralDeviation(object_closest_pose, object_pose.position) > 0.0 + object_data.direction = calc_lateral_deviation(object_closest_pose, object_pose.position) > 0.0 ? Direction::LEFT : Direction::RIGHT; diff --git a/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/include/autoware/behavior_path_dynamic_obstacle_avoidance_module/scene.hpp b/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/include/autoware/behavior_path_dynamic_obstacle_avoidance_module/scene.hpp index 0f164ad05455e..8d20d1f4289a9 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/include/autoware/behavior_path_dynamic_obstacle_avoidance_module/scene.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/include/autoware/behavior_path_dynamic_obstacle_avoidance_module/scene.hpp @@ -17,7 +17,7 @@ #include "autoware/behavior_path_planner_common/interface/scene_module_interface.hpp" -#include +#include #include #include @@ -57,9 +57,9 @@ std::vector getAllKeys(const std::unordered_map & map) namespace autoware::behavior_path_planner { -using autoware::universe_utils::Polygon2d; using autoware_internal_planning_msgs::msg::PathWithLaneId; using autoware_perception_msgs::msg::PredictedPath; +using autoware_utils::Polygon2d; struct MinMaxValue { @@ -183,7 +183,7 @@ class DynamicObstacleAvoidanceModule : public SceneModuleInterface const PredictedObject & predicted_object, const double arg_vel, const double arg_lat_vel, const bool arg_is_object_on_ego_path, const std::optional & arg_latest_time_inside_ego_path) - : uuid(autoware::universe_utils::toHexString(predicted_object.object_id)), + : uuid(autoware_utils::to_hex_string(predicted_object.object_id)), label(predicted_object.classification.front().label), pose(predicted_object.kinematics.initial_pose_with_covariance.pose), shape(predicted_object.shape), @@ -382,8 +382,8 @@ class DynamicObstacleAvoidanceModule : public SceneModuleInterface }; struct EgoPathReservePoly { - const autoware::universe_utils::Polygon2d left_avoid; - const autoware::universe_utils::Polygon2d right_avoid; + const autoware_utils::Polygon2d left_avoid; + const autoware_utils::Polygon2d right_avoid; }; bool canTransitSuccessState() override; @@ -440,11 +440,11 @@ class DynamicObstacleAvoidanceModule : public SceneModuleInterface const DynamicAvoidanceObject & object) const; std::pair getAdjacentLanes( const double forward_distance, const double backward_distance) const; - std::optional calcEgoPathBasedDynamicObstaclePolygon( + std::optional calcEgoPathBasedDynamicObstaclePolygon( const DynamicAvoidanceObject & object) const; - std::optional calcObjectPathBasedDynamicObstaclePolygon( + std::optional calcObjectPathBasedDynamicObstaclePolygon( const DynamicAvoidanceObject & object) const; - std::optional calcPredictedPathBasedDynamicObstaclePolygon( + std::optional calcPredictedPathBasedDynamicObstaclePolygon( const DynamicAvoidanceObject & object, const EgoPathReservePoly & ego_path_poly) const; EgoPathReservePoly calcEgoPathReservePoly(const PathWithLaneId & ego_path) const; lanelet::ConstLanelets getCurrentLanesFromPath( diff --git a/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/package.xml b/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/package.xml index bc52f39a030d4..b789c3492cf06 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/package.xml +++ b/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/package.xml @@ -24,7 +24,7 @@ autoware_perception_msgs autoware_planning_msgs autoware_signal_processing - autoware_universe_utils + autoware_utils autoware_vehicle_msgs geometry_msgs lanelet2_core diff --git a/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/src/manager.cpp b/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/src/manager.cpp index 87038e1b2f474..15751eaee5ee7 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/src/manager.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/src/manager.cpp @@ -14,7 +14,7 @@ #include "autoware/behavior_path_dynamic_obstacle_avoidance_module/manager.hpp" -#include "autoware/universe_utils/ros/update_param.hpp" +#include "autoware_utils/ros/update_param.hpp" #include @@ -153,133 +153,133 @@ void DynamicObstacleAvoidanceModuleManager::init(rclcpp::Node * node) void DynamicObstacleAvoidanceModuleManager::updateModuleParams( [[maybe_unused]] const std::vector & parameters) { - using autoware::universe_utils::updateParam; + using autoware_utils::update_param; auto & p = parameters_; { // common const std::string ns = "dynamic_avoidance.common."; - updateParam(parameters, ns + "enable_debug_info", p->enable_debug_info); - updateParam(parameters, ns + "use_hatched_road_markings", p->use_hatched_road_markings); + update_param(parameters, ns + "enable_debug_info", p->enable_debug_info); + update_param(parameters, ns + "use_hatched_road_markings", p->use_hatched_road_markings); } { // target object const std::string ns = "dynamic_avoidance.target_object."; - updateParam(parameters, ns + "car", p->avoid_car); - updateParam(parameters, ns + "truck", p->avoid_truck); - updateParam(parameters, ns + "bus", p->avoid_bus); - updateParam(parameters, ns + "trailer", p->avoid_trailer); - updateParam(parameters, ns + "unknown", p->avoid_unknown); - updateParam(parameters, ns + "bicycle", p->avoid_bicycle); - updateParam(parameters, ns + "motorcycle", p->avoid_motorcycle); - updateParam(parameters, ns + "pedestrian", p->avoid_pedestrian); + update_param(parameters, ns + "car", p->avoid_car); + update_param(parameters, ns + "truck", p->avoid_truck); + update_param(parameters, ns + "bus", p->avoid_bus); + update_param(parameters, ns + "trailer", p->avoid_trailer); + update_param(parameters, ns + "unknown", p->avoid_unknown); + update_param(parameters, ns + "bicycle", p->avoid_bicycle); + update_param(parameters, ns + "motorcycle", p->avoid_motorcycle); + update_param(parameters, ns + "pedestrian", p->avoid_pedestrian); - updateParam(parameters, ns + "max_obstacle_vel", p->max_obstacle_vel); - updateParam(parameters, ns + "min_obstacle_vel", p->min_obstacle_vel); + update_param(parameters, ns + "max_obstacle_vel", p->max_obstacle_vel); + update_param(parameters, ns + "min_obstacle_vel", p->min_obstacle_vel); - updateParam( + update_param( parameters, ns + "successive_num_to_entry_dynamic_avoidance_condition", p->successive_num_to_entry_dynamic_avoidance_condition); - updateParam( + update_param( parameters, ns + "successive_num_to_exit_dynamic_avoidance_condition", p->successive_num_to_exit_dynamic_avoidance_condition); - updateParam( + update_param( parameters, ns + "min_obj_lat_offset_to_ego_path", p->min_obj_lat_offset_to_ego_path); - updateParam( + update_param( parameters, ns + "max_obj_lat_offset_to_ego_path", p->max_obj_lat_offset_to_ego_path); - updateParam( + update_param( parameters, ns + "cut_in_object.min_time_to_start_cut_in", p->min_time_to_start_cut_in); - updateParam( + update_param( parameters, ns + "cut_in_object.min_lon_offset_ego_to_object", p->min_lon_offset_ego_to_cut_in_object); - updateParam(parameters, ns + "cut_in_object.min_object_vel", p->min_cut_in_object_vel); + update_param(parameters, ns + "cut_in_object.min_object_vel", p->min_cut_in_object_vel); - updateParam( + update_param( parameters, ns + "cut_out_object.max_time_from_outside_ego_path", p->max_time_from_outside_ego_path_for_cut_out); - updateParam( + update_param( parameters, ns + "cut_out_object.min_object_lat_vel", p->min_cut_out_object_lat_vel); - updateParam( + update_param( parameters, ns + "cut_out_object.min_object_vel", p->min_cut_out_object_vel); - updateParam( + update_param( parameters, ns + "front_object.max_object_angle", p->max_front_object_angle); - updateParam( + update_param( parameters, ns + "front_object.max_ego_path_lat_cover_ratio", p->max_front_object_ego_path_lat_cover_ratio); - updateParam(parameters, ns + "front_object.min_object_vel", p->min_front_object_vel); + update_param(parameters, ns + "front_object.min_object_vel", p->min_front_object_vel); - updateParam( + update_param( parameters, ns + "crossing_object.min_overtaking_object_vel", p->min_overtaking_crossing_object_vel); - updateParam( + update_param( parameters, ns + "crossing_object.max_overtaking_object_angle", p->max_overtaking_crossing_object_angle); - updateParam( + update_param( parameters, ns + "crossing_object.min_oncoming_object_vel", p->min_oncoming_crossing_object_vel); - updateParam( + update_param( parameters, ns + "crossing_object.max_oncoming_object_angle", p->max_oncoming_crossing_object_angle); - updateParam( + update_param( parameters, ns + "crossing_object.max_pedestrian_crossing_vel", p->max_pedestrian_crossing_vel); - updateParam( + update_param( parameters, ns + "stopped_object.max_object_vel", p->max_stopped_object_vel); } { // drivable_area_generation const std::string ns = "dynamic_avoidance.drivable_area_generation."; std::string polygon_generation_method_str; - if (updateParam( + if (update_param( parameters, ns + "polygon_generation_method", polygon_generation_method_str)) { p->polygon_generation_method = convertToPolygonGenerationMethod(polygon_generation_method_str); } - updateParam(parameters, ns + "expand_drivable_area", p->expand_drivable_area); - updateParam( + update_param(parameters, ns + "expand_drivable_area", p->expand_drivable_area); + update_param( parameters, ns + "object_path_base.min_longitudinal_polygon_margin", p->min_obj_path_based_lon_polygon_margin); - updateParam(parameters, ns + "lat_offset_from_obstacle", p->lat_offset_from_obstacle); - updateParam( + update_param(parameters, ns + "lat_offset_from_obstacle", p->lat_offset_from_obstacle); + update_param( parameters, ns + "margin_distance_around_pedestrian", p->margin_distance_around_pedestrian); - updateParam( + update_param( parameters, ns + "predicted_path.end_time_to_consider", p->end_time_to_consider); - updateParam( + update_param( parameters, ns + "predicted_path.threshold_confidence", p->threshold_confidence); - updateParam(parameters, ns + "max_lat_offset_to_avoid", p->max_lat_offset_to_avoid); - updateParam( + update_param(parameters, ns + "max_lat_offset_to_avoid", p->max_lat_offset_to_avoid); + update_param( parameters, ns + "max_time_for_object_lat_shift", p->max_time_for_lat_shift); - updateParam( + update_param( parameters, ns + "lpf_gain_for_lat_avoid_to_offset", p->lpf_gain_for_lat_avoid_to_offset); - updateParam(parameters, ns + "max_ego_lat_acc", p->max_ego_lat_acc); - updateParam(parameters, ns + "max_ego_lat_jerk", p->max_ego_lat_jerk); - updateParam(parameters, ns + "delay_time_ego_shift", p->delay_time_ego_shift); + update_param(parameters, ns + "max_ego_lat_acc", p->max_ego_lat_acc); + update_param(parameters, ns + "max_ego_lat_jerk", p->max_ego_lat_jerk); + update_param(parameters, ns + "delay_time_ego_shift", p->delay_time_ego_shift); - updateParam( + update_param( parameters, ns + "overtaking_object.max_time_to_collision", p->max_time_to_collision_overtaking_object); - updateParam( + update_param( parameters, ns + "overtaking_object.start_duration_to_avoid", p->start_duration_to_avoid_overtaking_object); - updateParam( + update_param( parameters, ns + "overtaking_object.end_duration_to_avoid", p->end_duration_to_avoid_overtaking_object); - updateParam( + update_param( parameters, ns + "overtaking_object.duration_to_hold_avoidance", p->duration_to_hold_avoidance_overtaking_object); - updateParam( + update_param( parameters, ns + "oncoming_object.max_time_to_collision", p->max_time_to_collision_oncoming_object); - updateParam( + update_param( parameters, ns + "oncoming_object.start_duration_to_avoid", p->start_duration_to_avoid_oncoming_object); - updateParam( + update_param( parameters, ns + "oncoming_object.end_duration_to_avoid", p->end_duration_to_avoid_oncoming_object); } diff --git a/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/src/scene.cpp b/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/src/scene.cpp index 145e0df2675b4..1973b7d42efd9 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/src/scene.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/src/scene.cpp @@ -18,10 +18,10 @@ #include "autoware/behavior_path_planner_common/utils/utils.hpp" #include "autoware/object_recognition_utils/predicted_path_utils.hpp" #include "autoware/signal_processing/lowpass_filter_1d.hpp" -#include "autoware/universe_utils/geometry/boost_polygon_utils.hpp" +#include "autoware_utils/geometry/boost_polygon_utils.hpp" -#include #include +#include #include #include @@ -44,7 +44,7 @@ namespace autoware::behavior_path_planner { namespace { -geometry_msgs::msg::Point toGeometryPoint(const autoware::universe_utils::Point2d & point) +geometry_msgs::msg::Point toGeometryPoint(const autoware_utils::Point2d & point) { geometry_msgs::msg::Point geom_obj_point; geom_obj_point.x = point.x(); @@ -68,25 +68,23 @@ MinMaxValue combineMinMaxValues(const MinMaxValue & r1, const MinMaxValue & r2) void appendObjectMarker(MarkerArray & marker_array, const geometry_msgs::msg::Pose & obj_pose) { - auto marker = autoware::universe_utils::createDefaultMarker( + auto marker = autoware_utils::create_default_marker( "map", rclcpp::Clock{RCL_ROS_TIME}.now(), "dynamic_objects_to_avoid", marker_array.markers.size(), visualization_msgs::msg::Marker::CUBE, - autoware::universe_utils::createMarkerScale(3.0, 1.0, 1.0), - autoware::universe_utils::createMarkerColor(1.0, 0.5, 0.6, 0.8)); + autoware_utils::create_marker_scale(3.0, 1.0, 1.0), + autoware_utils::create_marker_color(1.0, 0.5, 0.6, 0.8)); marker.pose = obj_pose; marker_array.markers.push_back(marker); } void appendExtractedPolygonMarker( - MarkerArray & marker_array, const autoware::universe_utils::Polygon2d & obj_poly, - const double obj_z) + MarkerArray & marker_array, const autoware_utils::Polygon2d & obj_poly, const double obj_z) { - auto marker = autoware::universe_utils::createDefaultMarker( + auto marker = autoware_utils::create_default_marker( "map", rclcpp::Clock{RCL_ROS_TIME}.now(), "extracted_polygons", marker_array.markers.size(), - visualization_msgs::msg::Marker::LINE_STRIP, - autoware::universe_utils::createMarkerScale(0.1, 0.0, 0.0), - autoware::universe_utils::createMarkerColor(1.0, 0.5, 0.6, 0.8)); + visualization_msgs::msg::Marker::LINE_STRIP, autoware_utils::create_marker_scale(0.1, 0.0, 0.0), + autoware_utils::create_marker_color(1.0, 0.5, 0.6, 0.8)); // NOTE: obj_poly.outer() has already duplicated points to close the polygon. for (size_t i = 0; i < obj_poly.outer().size(); ++i) { @@ -194,7 +192,7 @@ double calcDiffAngleAgainstPath( const double traj_yaw = tf2::getYaw(path_points.at(target_idx).point.pose.orientation); const double target_yaw = tf2::getYaw(target_pose.orientation); - const double diff_yaw = autoware::universe_utils::normalizeRadian(target_yaw - traj_yaw); + const double diff_yaw = autoware_utils::normalize_radian(target_yaw - traj_yaw); return diff_yaw; } @@ -205,14 +203,13 @@ double calcDistanceToPath( if (target_idx == 0 || target_idx == points.size() - 1) { const double target_yaw = tf2::getYaw(points.at(target_idx).orientation); const double angle_to_target_pos = - autoware::universe_utils::calcAzimuthAngle(points.at(target_idx).position, target_pos); - const double diff_yaw = - autoware::universe_utils::normalizeRadian(angle_to_target_pos - target_yaw); + autoware_utils::calc_azimuth_angle(points.at(target_idx).position, target_pos); + const double diff_yaw = autoware_utils::normalize_radian(angle_to_target_pos - target_yaw); if ( (target_idx == 0 && (diff_yaw < -M_PI_2 || M_PI_2 < diff_yaw)) || (target_idx == points.size() - 1 && (-M_PI_2 < diff_yaw && diff_yaw < M_PI_2))) { - return autoware::universe_utils::calcDistance2d(points.at(target_idx), target_pos); + return autoware_utils::calc_distance2d(points.at(target_idx), target_pos); } } @@ -224,10 +221,9 @@ bool isLeft( const geometry_msgs::msg::Point & target_pos, const size_t target_idx) { const double target_yaw = tf2::getYaw(path_points.at(target_idx).point.pose.orientation); - const double angle_to_target_pos = autoware::universe_utils::calcAzimuthAngle( - path_points.at(target_idx).point.pose.position, target_pos); - const double diff_yaw = - autoware::universe_utils::normalizeRadian(angle_to_target_pos - target_yaw); + const double angle_to_target_pos = + autoware_utils::calc_azimuth_angle(path_points.at(target_idx).point.pose.position, target_pos); + const double diff_yaw = autoware_utils::normalize_radian(angle_to_target_pos - target_yaw); if (0 < diff_yaw) { return true; @@ -278,7 +274,7 @@ std::optional> intersectLines( ++source_seg_idx) { for (int target_seg_idx = 0; target_seg_idx < static_cast(target_line.size()) - 1; ++target_seg_idx) { - const auto intersect_point = autoware::universe_utils::intersect( + const auto intersect_point = autoware_utils::intersect( source_line.at(source_seg_idx).position, source_line.at(source_seg_idx + 1).position, target_line.at(target_seg_idx), target_line.at(target_seg_idx + 1)); if (intersect_point) { @@ -315,10 +311,8 @@ size_t getNearestIndexFromSegmentIndex( const std::vector & points, const size_t seg_idx, const geometry_msgs::msg::Point & target_pos) { - const double first_dist = - autoware::universe_utils::calcDistance2d(points.at(seg_idx), target_pos); - const double second_dist = - autoware::universe_utils::calcDistance2d(points.at(seg_idx + 1), target_pos); + const double first_dist = autoware_utils::calc_distance2d(points.at(seg_idx), target_pos); + const double second_dist = autoware_utils::calc_distance2d(points.at(seg_idx + 1), target_pos); if (first_dist < second_dist) { return seg_idx; } @@ -373,7 +367,7 @@ bool DynamicObstacleAvoidanceModule::isExecutionReady() const void DynamicObstacleAvoidanceModule::updateData() { - universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + autoware_utils::ScopedTimeTrack st(__func__, *time_keeper_); info_marker_.markers.clear(); debug_marker_.markers.clear(); @@ -513,14 +507,14 @@ ObjectType DynamicObstacleAvoidanceModule::getObjectType(const uint8_t label) co void DynamicObstacleAvoidanceModule::registerRegulatedObjects( const std::vector & prev_objects) { - universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + autoware_utils::ScopedTimeTrack st(__func__, *time_keeper_); const auto input_path = getPreviousModuleOutput().path; const auto input_points = toGeometryPoints(input_path.points); // for efficient computation const auto & predicted_objects = planner_data_->dynamic_object->objects; for (const auto & predicted_object : predicted_objects) { - const auto obj_uuid = autoware::universe_utils::toHexString(predicted_object.object_id); + const auto obj_uuid = autoware_utils::to_hex_string(predicted_object.object_id); const auto & obj_pose = predicted_object.kinematics.initial_pose_with_covariance.pose; const double obj_vel_norm = std::hypot( predicted_object.kinematics.initial_twist_with_covariance.twist.linear.x, @@ -608,14 +602,14 @@ void DynamicObstacleAvoidanceModule::registerRegulatedObjects( void DynamicObstacleAvoidanceModule::registerUnregulatedObjects( const std::vector & prev_objects) { - universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + autoware_utils::ScopedTimeTrack st(__func__, *time_keeper_); const auto input_path = getPreviousModuleOutput().path; const auto input_points = toGeometryPoints(input_path.points); // for efficient computation const auto & predicted_objects = planner_data_->dynamic_object->objects; for (const auto & predicted_object : predicted_objects) { - const auto obj_uuid = autoware::universe_utils::toHexString(predicted_object.object_id); + const auto obj_uuid = autoware_utils::to_hex_string(predicted_object.object_id); const auto & obj_pose = predicted_object.kinematics.initial_pose_with_covariance.pose; const double obj_vel_norm = std::hypot( predicted_object.kinematics.initial_twist_with_covariance.twist.linear.x, @@ -689,7 +683,7 @@ void DynamicObstacleAvoidanceModule::registerUnregulatedObjects( void DynamicObstacleAvoidanceModule::determineWhetherToAvoidAgainstRegulatedObjects( const std::vector & prev_objects) { - universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + autoware_utils::ScopedTimeTrack st(__func__, *time_keeper_); const auto & input_path = getPreviousModuleOutput().path; const auto input_points = toGeometryPoints(input_path.points); // for efficient computation @@ -831,7 +825,7 @@ void DynamicObstacleAvoidanceModule::determineWhetherToAvoidAgainstRegulatedObje // 2.h. calculate longitudinal and lateral offset to avoid to generate object polygon by // "ego_path_base" - const auto obj_points = autoware::universe_utils::toPolygon2d(object.pose, object.shape); + const auto obj_points = autoware_utils::to_polygon2d(object.pose, object.shape); const auto lon_offset_to_avoid = calcMinMaxLongitudinalOffsetToAvoid( ref_points_for_obj_poly, object.pose, obj_points, object.vel, obj_path, object.shape, time_while_collision); @@ -859,7 +853,7 @@ void DynamicObstacleAvoidanceModule::determineWhetherToAvoidAgainstRegulatedObje void DynamicObstacleAvoidanceModule::determineWhetherToAvoidAgainstUnregulatedObjects( const std::vector & prev_objects) { - universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + autoware_utils::ScopedTimeTrack st(__func__, *time_keeper_); const auto & input_path = getPreviousModuleOutput().path; const auto input_points = toGeometryPoints(input_path.points); // for efficient computation @@ -942,19 +936,19 @@ LatFeasiblePaths DynamicObstacleAvoidanceModule::generateLateralFeasiblePaths( const double y = feasible_lat_offset; const auto feasible_left_bound_point = - autoware::universe_utils::calcOffsetPose(ego_pose, x, -y, 0.0).position; + autoware_utils::calc_offset_pose(ego_pose, x, -y, 0.0).position; ego_lat_feasible_paths.left_path.push_back(feasible_left_bound_point); const auto feasible_right_bound_point = - autoware::universe_utils::calcOffsetPose(ego_pose, x, y, 0.0).position; + autoware_utils::calc_offset_pose(ego_pose, x, y, 0.0).position; ego_lat_feasible_paths.right_path.push_back(feasible_right_bound_point); } - autoware::universe_utils::appendMarkerArray( + autoware_utils::append_marker_array( marker_utils::createPointsMarkerArray( ego_lat_feasible_paths.left_path, "ego_lat_feasible_left_path", 0, 0.6, 0.9, 0.9), &debug_marker_); - autoware::universe_utils::appendMarkerArray( + autoware_utils::append_marker_array( marker_utils::createPointsMarkerArray( ego_lat_feasible_paths.right_path, "ego_lat_feasible_right_path", 0, 0.6, 0.9, 0.9), &debug_marker_); @@ -998,7 +992,7 @@ DynamicObstacleAvoidanceModule::calcCollisionSection( std::optional collision_start_idx{std::nullopt}; double lon_dist = 0.0; for (size_t i = ego_idx; i < ego_path.size() - 1; ++i) { - lon_dist += autoware::universe_utils::calcDistance2d(ego_path.at(i), ego_path.at(i + 1)); + lon_dist += autoware_utils::calc_distance2d(ego_path.at(i), ego_path.at(i + 1)); const double elapsed_time = lon_dist / ego_vel; const auto future_ego_pose = ego_path.at(i); @@ -1007,7 +1001,7 @@ DynamicObstacleAvoidanceModule::calcCollisionSection( if (future_obj_pose) { const double dist_ego_to_obj = - autoware::universe_utils::calcDistance2d(future_ego_pose, *future_obj_pose); + autoware_utils::calc_distance2d(future_ego_pose, *future_obj_pose); if (dist_ego_to_obj < 1.0) { if (!collision_start_idx) { collision_start_idx = i; @@ -1269,7 +1263,7 @@ DynamicObstacleAvoidanceModule::getLateralLongitudinalOffset( obj_lon_offset_vec.push_back(obj_lon_offset - radius); obj_lon_offset_vec.push_back(obj_lon_offset + radius); } else { - const auto obj_points = autoware::universe_utils::toPolygon2d(obj_pose, obj_shape); + const auto obj_points = autoware_utils::to_polygon2d(obj_pose, obj_shape); for (size_t i = 0; i < obj_points.outer().size(); ++i) { const auto geom_obj_point = toGeometryPoint(obj_points.outer().at(i)); const size_t obj_point_seg_idx = @@ -1446,7 +1440,7 @@ double DynamicObstacleAvoidanceModule::calcValidLengthToAvoid( const auto prev_min_dist = calc_min_dist(prev_valid_obj_path_end_idx); const auto next_min_dist = calc_min_dist(next_valid_obj_path_end_idx); if (prev_min_dist && next_min_dist) { - const double segment_length = autoware::universe_utils::calcDistance2d( + const double segment_length = autoware_utils::calc_distance2d( obj_path.path.at(prev_valid_obj_path_end_idx), obj_path.path.at(next_valid_obj_path_end_idx)); const double partial_segment_length = segment_length * @@ -1561,7 +1555,7 @@ DynamicObstacleAvoidanceModule::calcMinMaxLateralOffsetToAvoidUnregulatedObject( const std::optional & prev_object, const DynamicAvoidanceObject & object) const { - universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + autoware_utils::ScopedTimeTrack st(__func__, *time_keeper_); time_keeper_->start_track("findNearestSegmentIndex of object position"); const size_t obj_seg_idx = @@ -1571,7 +1565,7 @@ DynamicObstacleAvoidanceModule::calcMinMaxLateralOffsetToAvoidUnregulatedObject( getNearestIndexFromSegmentIndex(ref_points_for_obj_poly, obj_seg_idx, object.pose.position); const bool enable_lowpass_filter = [&]() { - universe_utils::ScopedTimeTrack st("calc enable_lowpass_filter", *time_keeper_); + autoware_utils::ScopedTimeTrack st("calc enable_lowpass_filter", *time_keeper_); if ( !prev_object || prev_object->ref_points_for_obj_poly.size() < 2 || ref_points_for_obj_poly.size() < 2) { @@ -1590,7 +1584,7 @@ DynamicObstacleAvoidanceModule::calcMinMaxLateralOffsetToAvoidUnregulatedObject( }(); const auto obj_occupancy_region = [&]() { - universe_utils::ScopedTimeTrack st("calc obj_occupancy_region", *time_keeper_); + autoware_utils::ScopedTimeTrack st("calc obj_occupancy_region", *time_keeper_); std::vector lat_pos_vec; if (object.shape.type == autoware_perception_msgs::msg::Shape::CYLINDER) { // NOTE: efficient calculation for the CYLINDER object. @@ -1609,7 +1603,7 @@ DynamicObstacleAvoidanceModule::calcMinMaxLateralOffsetToAvoidUnregulatedObject( }(); lat_pos_vec.push_back(obj_min_lat_offset); } else { - const auto obj_points = autoware::universe_utils::toPolygon2d(object.pose, object.shape); + const auto obj_points = autoware_utils::to_polygon2d(object.pose, object.shape); for (size_t i = 0; i < obj_points.outer().size(); ++i) { const auto geom_obj_point = toGeometryPoint(obj_points.outer().at(i)); const double obj_point_lat_offset = autoware::motion_utils::calcLateralOffset( @@ -1665,7 +1659,7 @@ DynamicObstacleAvoidanceModule::calcMinMaxLateralOffsetToAvoidUnregulatedObject( } // NOTE: object does not have const only to update min_bound_lat_offset. -std::optional +std::optional DynamicObstacleAvoidanceModule::calcEgoPathBasedDynamicObstaclePolygon( const DynamicAvoidanceObject & object) const { @@ -1677,7 +1671,7 @@ DynamicObstacleAvoidanceModule::calcEgoPathBasedDynamicObstaclePolygon( const size_t obj_seg_idx = autoware::motion_utils::findNearestSegmentIndex(ref_points_for_obj_poly, object.pose.position); - // const auto obj_points = autoware::universe_utils::toPolygon2d(object.pose, object.shape); + // const auto obj_points = autoware_utils::to_polygon2d(object.pose, object.shape); const auto lon_bound_start_idx_opt = autoware::motion_utils::insertTargetPoint( obj_seg_idx, object.lon_offset_to_avoid->min_value, ref_points_for_obj_poly); @@ -1701,7 +1695,7 @@ DynamicObstacleAvoidanceModule::calcEgoPathBasedDynamicObstaclePolygon( std::vector obj_inner_bound_poses; for (size_t i = lon_bound_start_idx; i <= lon_bound_end_idx; ++i) { // NOTE: object.lat_offset_to_avoid->min_value is not the minimum value but the inner value. - obj_inner_bound_poses.push_back(autoware::universe_utils::calcOffsetPose( + obj_inner_bound_poses.push_back(autoware_utils::calc_offset_pose( ref_points_for_obj_poly.at(i), 0.0, object.lat_offset_to_avoid->min_value, 0.0)); } @@ -1716,8 +1710,8 @@ DynamicObstacleAvoidanceModule::calcEgoPathBasedDynamicObstaclePolygon( // Check if the object polygon intersects with the ego_lat_feasible_path. if (intersect_result) { const auto & [bound_seg_idx, intersect_point] = *intersect_result; - const double lon_offset = autoware::universe_utils::calcDistance2d( - obj_inner_bound_poses.at(bound_seg_idx), intersect_point); + const double lon_offset = + autoware_utils::calc_distance2d(obj_inner_bound_poses.at(bound_seg_idx), intersect_point); const auto obj_inner_bound_start_idx_opt = autoware::motion_utils::insertTargetPoint(bound_seg_idx, lon_offset, obj_inner_bound_poses); @@ -1748,17 +1742,17 @@ DynamicObstacleAvoidanceModule::calcEgoPathBasedDynamicObstaclePolygon( std::vector feasible_obj_outer_bound_points; for (const auto & feasible_obj_inner_bound_pose : feasible_obj_inner_bound_poses) { feasible_obj_outer_bound_points.push_back( - autoware::universe_utils::calcOffsetPose( + autoware_utils::calc_offset_pose( feasible_obj_inner_bound_pose, 0.0, object.lat_offset_to_avoid->max_value - object.lat_offset_to_avoid->min_value, 0.0) .position); } // create obj_polygon from inner/outer bound points - autoware::universe_utils::Polygon2d obj_poly; + autoware_utils::Polygon2d obj_poly; const auto add_points_to_obj_poly = [&](const auto & bound_points) { for (const auto & bound_point : bound_points) { - obj_poly.outer().push_back(autoware::universe_utils::Point2d(bound_point.x, bound_point.y)); + obj_poly.outer().push_back(autoware_utils::Point2d(bound_point.x, bound_point.y)); } }; add_points_to_obj_poly(feasible_obj_inner_bound_points); @@ -1770,7 +1764,7 @@ DynamicObstacleAvoidanceModule::calcEgoPathBasedDynamicObstaclePolygon( } // should be replace by the function calcPredictedPathBasedDynamicObstaclePolygon() (takagi) -std::optional +std::optional DynamicObstacleAvoidanceModule::calcObjectPathBasedDynamicObstaclePolygon( const DynamicAvoidanceObject & object) const { @@ -1799,26 +1793,26 @@ DynamicObstacleAvoidanceModule::calcObjectPathBasedDynamicObstaclePolygon( const auto & obj_pose = obj_path.path.at(i); obj_left_bound_points.push_back( - autoware::universe_utils::calcOffsetPose( + autoware_utils::calc_offset_pose( obj_pose, lon_offset, object.shape.dimensions.y / 2.0 + parameters_->lat_offset_from_obstacle, 0.0) .position); obj_right_bound_points.push_back( - autoware::universe_utils::calcOffsetPose( + autoware_utils::calc_offset_pose( obj_pose, lon_offset, -object.shape.dimensions.y / 2.0 - parameters_->lat_offset_from_obstacle, 0.0) .position); } // create obj_polygon from inner/outer bound points - autoware::universe_utils::Polygon2d obj_poly; + autoware_utils::Polygon2d obj_poly; for (const auto & bound_point : obj_right_bound_points) { - const auto obj_poly_point = autoware::universe_utils::Point2d(bound_point.x, bound_point.y); + const auto obj_poly_point = autoware_utils::Point2d(bound_point.x, bound_point.y); obj_poly.outer().push_back(obj_poly_point); } std::reverse(obj_left_bound_points.begin(), obj_left_bound_points.end()); for (const auto & bound_point : obj_left_bound_points) { - const auto obj_poly_point = autoware::universe_utils::Point2d(bound_point.x, bound_point.y); + const auto obj_poly_point = autoware_utils::Point2d(bound_point.x, bound_point.y); obj_poly.outer().push_back(obj_poly_point); } @@ -1829,7 +1823,7 @@ DynamicObstacleAvoidanceModule::calcObjectPathBasedDynamicObstaclePolygon( // Calculate polygons according to predicted_path with certain confidence, // except for the area required for ego safety. // input: an object, the minimum area required for ego safety, and some global params -std::optional +std::optional DynamicObstacleAvoidanceModule::calcPredictedPathBasedDynamicObstaclePolygon( const DynamicAvoidanceObject & object, const EgoPathReservePoly & ego_path_poly) const { @@ -1845,10 +1839,10 @@ DynamicObstacleAvoidanceModule::calcPredictedPathBasedDynamicObstaclePolygon( } } - autoware::universe_utils::Polygon2d obj_points_as_poly; + autoware_utils::Polygon2d obj_points_as_poly; for (const auto pose : obj_poses) { boost::geometry::append( - obj_points_as_poly, autoware::universe_utils::toFootprint( + obj_points_as_poly, autoware_utils::to_footprint( pose, object.shape.dimensions.x * 0.5, object.shape.dimensions.x * 0.5, object.shape.dimensions.y * 0.5) .outer()); @@ -1857,7 +1851,7 @@ DynamicObstacleAvoidanceModule::calcPredictedPathBasedDynamicObstaclePolygon( Polygon2d obj_poly; boost::geometry::convex_hull(obj_points_as_poly, obj_poly); - autoware::universe_utils::MultiPolygon2d expanded_poly; + autoware_utils::MultiPolygon2d expanded_poly; namespace strategy = boost::geometry::strategy::buffer; boost::geometry::buffer( obj_poly, expanded_poly, @@ -1866,7 +1860,7 @@ DynamicObstacleAvoidanceModule::calcPredictedPathBasedDynamicObstaclePolygon( strategy::point_circle()); if (expanded_poly.empty()) return {}; - autoware::universe_utils::MultiPolygon2d output_poly; + autoware_utils::MultiPolygon2d output_poly; boost::geometry::difference( expanded_poly[0], object.is_collision_left ? ego_path_poly.right_avoid : ego_path_poly.left_avoid, output_poly); @@ -1899,27 +1893,26 @@ DynamicObstacleAvoidanceModule::calcEgoPathReservePoly(const PathWithLaneId & eg assert(!ego_path.points.empty()); - autoware::universe_utils::LineString2d ego_path_lines; + autoware_utils::LineString2d ego_path_lines; for (const auto & path_point : ego_path.points) { - ego_path_lines.push_back( - autoware::universe_utils::fromMsg(path_point.point.pose.position).to_2d()); + ego_path_lines.push_back(autoware_utils::from_msg(path_point.point.pose.position).to_2d()); } - auto calcReservePoly = [&ego_path_lines]( - const strategy::distance_asymmetric path_expand_strategy, - const strategy::distance_asymmetric steer_expand_strategy, - const std::vector & outer_body_path) - -> autoware::universe_utils::Polygon2d { + auto calcReservePoly = + [&ego_path_lines]( + const strategy::distance_asymmetric path_expand_strategy, + const strategy::distance_asymmetric steer_expand_strategy, + const std::vector & outer_body_path) -> autoware_utils::Polygon2d { // reserve area based on the reference path - autoware::universe_utils::MultiPolygon2d path_poly; + autoware_utils::MultiPolygon2d path_poly; boost::geometry::buffer( ego_path_lines, path_poly, path_expand_strategy, strategy::side_straight(), strategy::join_round(), strategy::end_flat(), strategy::point_circle()); // reserve area steer to the avoidance path - autoware::universe_utils::LineString2d steer_lines; + autoware_utils::LineString2d steer_lines; for (const auto & point : outer_body_path) { - const auto bg_point = autoware::universe_utils::fromMsg(point).to_2d(); + const auto bg_point = autoware_utils::from_msg(point).to_2d(); if (boost::geometry::within(bg_point, path_poly)) { if (steer_lines.size() != 0) { ; @@ -1929,12 +1922,12 @@ DynamicObstacleAvoidanceModule::calcEgoPathReservePoly(const PathWithLaneId & eg } // boost::geometry::append(steer_lines, bg_point); } - autoware::universe_utils::MultiPolygon2d steer_poly; + autoware_utils::MultiPolygon2d steer_poly; boost::geometry::buffer( steer_lines, steer_poly, steer_expand_strategy, strategy::side_straight(), strategy::join_round(), strategy::end_flat(), strategy::point_circle()); - autoware::universe_utils::MultiPolygon2d output_poly; + autoware_utils::MultiPolygon2d output_poly; boost::geometry::union_(path_poly, steer_poly, output_poly); if (output_poly.size() != 1) { assert(false); @@ -1948,11 +1941,11 @@ DynamicObstacleAvoidanceModule::calcEgoPathReservePoly(const PathWithLaneId & eg const double vehicle_half_width = planner_data_->parameters.vehicle_width * 0.5; const double reserve_width_obj_side = vehicle_half_width - parameters_->max_lat_offset_to_avoid; - const autoware::universe_utils::Polygon2d left_avoid_poly = calcReservePoly( + const autoware_utils::Polygon2d left_avoid_poly = calcReservePoly( strategy::distance_asymmetric(vehicle_half_width, reserve_width_obj_side), strategy::distance_asymmetric(vehicle_half_width, 0.0), motion_saturated_outer_paths.right_path); - const autoware::universe_utils::Polygon2d right_avoid_poly = calcReservePoly( + const autoware_utils::Polygon2d right_avoid_poly = calcReservePoly( strategy::distance_asymmetric(reserve_width_obj_side, vehicle_half_width), strategy::distance_asymmetric(0.0, vehicle_half_width), motion_saturated_outer_paths.left_path); diff --git a/planning/behavior_path_planner/autoware_behavior_path_external_request_lane_change_module/package.xml b/planning/behavior_path_planner/autoware_behavior_path_external_request_lane_change_module/package.xml index c192fdb315c6a..8423188eee34a 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_external_request_lane_change_module/package.xml +++ b/planning/behavior_path_planner/autoware_behavior_path_external_request_lane_change_module/package.xml @@ -25,7 +25,7 @@ autoware_behavior_path_planner_common autoware_motion_utils autoware_rtc_interface - autoware_universe_utils + autoware_utils pluginlib rclcpp visualization_msgs diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/examples/plot_map_case1.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/examples/plot_map_case1.cpp index d4f6948de5a14..b980325f398b4 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/examples/plot_map_case1.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/examples/plot_map_case1.cpp @@ -25,11 +25,11 @@ #include #include #include -#include #include #include #include #include +#include #include #include @@ -89,7 +89,7 @@ std::vector g_colors = { "#FFFFFF", "#F5F5F5", "#FFFF00", "#9ACD32"}; void plot_footprint( - matplotlibcpp17::axes::Axes & axes, const autoware::universe_utils::LinearRing2d & footprint, + matplotlibcpp17::axes::Axes & axes, const autoware_utils::LinearRing2d & footprint, const std::string & color) { std::vector xs, ys; @@ -116,17 +116,17 @@ void plot_lanelet_polygon(matplotlibcpp17::axes::Axes & axes, const lanelet::Bas void plot_goal_candidate( matplotlibcpp17::axes::Axes & axes, const GoalCandidate & goal, const size_t prio, - const autoware::universe_utils::LinearRing2d & local_footprint, const std::string & color) + const autoware_utils::LinearRing2d & local_footprint, const std::string & color) { std::vector xs, ys; std::vector yaw_cos, yaw_sin; const auto goal_footprint = - transformVector(local_footprint, autoware::universe_utils::pose2transform(goal.goal_pose)); + transformVector(local_footprint, autoware_utils::pose2transform(goal.goal_pose)); plot_footprint(axes, goal_footprint, color); xs.push_back(goal.goal_pose.position.x); ys.push_back(goal.goal_pose.position.y); axes.text(Args(xs.back(), ys.back(), std::to_string(prio))); - const double yaw = autoware::universe_utils::getRPY(goal.goal_pose).z; + const double yaw = autoware_utils::get_rpy(goal.goal_pose).z; yaw_cos.push_back(std::cos(yaw)); yaw_sin.push_back(std::sin(yaw)); axes.scatter(Args(xs, ys), Kwargs("color"_a = color)); @@ -138,8 +138,7 @@ void plot_goal_candidate( void plot_goal_candidates( matplotlibcpp17::axes::Axes & axes, const GoalCandidates & goals, const std::map & goal_id2prio, - const autoware::universe_utils::LinearRing2d & local_footprint, - const std::string & color = "green") + const autoware_utils::LinearRing2d & local_footprint, const std::string & color = "green") { for (const auto & goal : goals) { const auto it = goal_id2prio.find(goal.id); @@ -158,7 +157,7 @@ void plot_path_with_lane_id( for (const auto & point : path.points) { xs.push_back(point.point.pose.position.x); ys.push_back(point.point.pose.position.y); - const double yaw = autoware::universe_utils::getRPY(point.point.pose).z; + const double yaw = autoware_utils::get_rpy(point.point.pose).z; yaw_cos.push_back(std::cos(yaw)); yaw_sin.push_back(std::sin(yaw)); axes.scatter( @@ -676,8 +675,8 @@ int main(int argc, char ** argv) plot_goal_candidate(ax1, filtered_path.modified_goal(), prio, footprint, color); plot_path_with_lane_id(ax2, filtered_path.full_path(), color, "most prio", 2.0); for (const auto & path_point : filtered_path.full_path().points) { - const auto pose_footprint = transformVector( - footprint, autoware::universe_utils::pose2transform(path_point.point.pose)); + const auto pose_footprint = + transformVector(footprint, autoware_utils::pose2transform(path_point.point.pose)); plot_footprint(ax2, pose_footprint, "blue"); } } else if (i % 50 == 0) { diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/examples/plot_map_case2.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/examples/plot_map_case2.cpp index 2fe4a0fa63dc7..a7f0c98c70055 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/examples/plot_map_case2.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/examples/plot_map_case2.cpp @@ -25,11 +25,11 @@ #include #include #include -#include #include #include #include #include +#include #include #include @@ -88,7 +88,7 @@ std::vector g_colors = { "#FFFFFF", "#F5F5F5", "#FFFF00", "#9ACD32"}; void plot_footprint( - matplotlibcpp17::axes::Axes & axes, const autoware::universe_utils::LinearRing2d & footprint, + matplotlibcpp17::axes::Axes & axes, const autoware_utils::LinearRing2d & footprint, const std::string & color) { std::vector xs, ys; @@ -115,17 +115,17 @@ void plot_lanelet_polygon(matplotlibcpp17::axes::Axes & axes, const lanelet::Bas void plot_goal_candidate( matplotlibcpp17::axes::Axes & axes, const GoalCandidate & goal, const size_t prio, - const autoware::universe_utils::LinearRing2d & local_footprint, const std::string & color) + const autoware_utils::LinearRing2d & local_footprint, const std::string & color) { std::vector xs, ys; std::vector yaw_cos, yaw_sin; const auto goal_footprint = - transformVector(local_footprint, autoware::universe_utils::pose2transform(goal.goal_pose)); + transformVector(local_footprint, autoware_utils::pose2transform(goal.goal_pose)); plot_footprint(axes, goal_footprint, color); xs.push_back(goal.goal_pose.position.x); ys.push_back(goal.goal_pose.position.y); axes.text(Args(xs.back(), ys.back(), std::to_string(prio))); - const double yaw = autoware::universe_utils::getRPY(goal.goal_pose).z; + const double yaw = autoware_utils::get_rpy(goal.goal_pose).z; yaw_cos.push_back(std::cos(yaw)); yaw_sin.push_back(std::sin(yaw)); axes.scatter(Args(xs, ys), Kwargs("color"_a = color)); @@ -137,8 +137,7 @@ void plot_goal_candidate( void plot_goal_candidates( matplotlibcpp17::axes::Axes & axes, const GoalCandidates & goals, const std::map & goal_id2prio, - const autoware::universe_utils::LinearRing2d & local_footprint, - const std::string & color = "green") + const autoware_utils::LinearRing2d & local_footprint, const std::string & color = "green") { for (const auto & goal : goals) { const auto it = goal_id2prio.find(goal.id); @@ -157,7 +156,7 @@ void plot_path_with_lane_id( for (const auto & point : path.points) { xs.push_back(point.point.pose.position.x); ys.push_back(point.point.pose.position.y); - const double yaw = autoware::universe_utils::getRPY(point.point.pose).z; + const double yaw = autoware_utils::get_rpy(point.point.pose).z; yaw_cos.push_back(std::cos(yaw)); yaw_sin.push_back(std::sin(yaw)); axes.scatter( @@ -677,8 +676,8 @@ int main(int argc, char ** argv) plot_goal_candidate(ax1, filtered_path.modified_goal(), prio, footprint, color); plot_path_with_lane_id(ax2, filtered_path.full_path(), color, "most prio", 2.0); for (const auto & path_point : filtered_path.full_path().points) { - const auto pose_footprint = transformVector( - footprint, autoware::universe_utils::pose2transform(path_point.point.pose)); + const auto pose_footprint = + transformVector(footprint, autoware_utils::pose2transform(path_point.point.pose)); plot_footprint(ax2, pose_footprint, "blue"); } } else if (i % 50 == 0) { diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/decision_state.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/decision_state.hpp index f4152d5da67b7..e61f4263cecc1 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/decision_state.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/decision_state.hpp @@ -56,7 +56,7 @@ class PathDecisionStateController const std::shared_ptr occupancy_grid_map, const bool is_current_safe, const GoalPlannerParameters & parameters, const GoalSearcher & goal_searcher, - std::vector & ego_polygons_expanded); + std::vector & ego_polygons_expanded); PathDecisionState get_current_state() const { return current_state_; } @@ -75,7 +75,7 @@ class PathDecisionStateController const std::shared_ptr occupancy_grid_map, const bool is_current_safe, const GoalPlannerParameters & parameters, const GoalSearcher & goal_searcher, - std::vector & ego_polygons_expanded) const; + std::vector & ego_polygons_expanded) const; }; } // namespace autoware::behavior_path_planner diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/fixed_goal_planner_base.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/fixed_goal_planner_base.hpp index a2978d8cd32b0..1bbdd6ac86812 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/fixed_goal_planner_base.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/fixed_goal_planner_base.hpp @@ -22,8 +22,8 @@ #include -using autoware::universe_utils::LinearRing2d; using autoware_internal_planning_msgs::msg::PathWithLaneId; +using autoware_utils::LinearRing2d; using geometry_msgs::msg::Pose; namespace autoware::behavior_path_planner diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_module.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_module.hpp index 0dbec17f890b3..cd8c5d7c9f34b 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_module.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_module.hpp @@ -55,7 +55,7 @@ using autoware::behavior_path_planner::utils::path_safety_checker::ObjectsFilter using autoware::behavior_path_planner::utils::path_safety_checker::PoseWithVelocityStamped; using autoware::behavior_path_planner::utils::path_safety_checker::SafetyCheckParams; using autoware::behavior_path_planner::utils::path_safety_checker::TargetObjectsOnLane; -using autoware::universe_utils::Polygon2d; +using autoware_utils::Polygon2d; struct FreespacePlannerDebugData { @@ -303,7 +303,7 @@ class GoalPlannerModule : public SceneModuleInterface const SafetyCheckParams safety_check_params_ = parameters_.safety_check_params; const autoware::vehicle_info_utils::VehicleInfo vehicle_info_; - const autoware::universe_utils::LinearRing2d vehicle_footprint_; + const autoware_utils::LinearRing2d vehicle_footprint_; const bool left_side_parking_; diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_searcher.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_searcher.hpp index 8744fc1d12f43..f427aafe4b460 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_searcher.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_searcher.hpp @@ -27,9 +27,9 @@ namespace autoware::behavior_path_planner { -using autoware::universe_utils::LinearRing2d; +using autoware_utils::LinearRing2d; using BasicPolygons2d = std::vector; -using autoware::universe_utils::MultiPolygon2d; +using autoware_utils::MultiPolygon2d; class GoalSearcher { diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/pull_over_planner_base.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/pull_over_planner_base.hpp index 541596fdd3c3b..38851216570b1 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/pull_over_planner_base.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/pull_over_planner_base.hpp @@ -25,8 +25,8 @@ #include #include -using autoware::universe_utils::LinearRing2d; using autoware_internal_planning_msgs::msg::PathWithLaneId; +using autoware_utils::LinearRing2d; using geometry_msgs::msg::Pose; namespace autoware::behavior_path_planner diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/thread_data.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/thread_data.hpp index d00379c63258f..51083720e9807 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/thread_data.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/thread_data.hpp @@ -32,9 +32,8 @@ class LaneParkingRequest { public: LaneParkingRequest( - const autoware::universe_utils::LinearRing2d & vehicle_footprint, - const GoalCandidates & goal_candidates, const BehaviorModuleOutput & upstream_module_output, - const bool use_bus_stop_area) + const autoware_utils::LinearRing2d & vehicle_footprint, const GoalCandidates & goal_candidates, + const BehaviorModuleOutput & upstream_module_output, const bool use_bus_stop_area) : vehicle_footprint_(vehicle_footprint), goal_candidates_(goal_candidates), use_bus_stop_area_(use_bus_stop_area), @@ -48,7 +47,7 @@ class LaneParkingRequest const std::optional & pull_over_path, const PathDecisionState & prev_data, const bool trigger_thread_on_approach); - const autoware::universe_utils::LinearRing2d vehicle_footprint_; + const autoware_utils::LinearRing2d vehicle_footprint_; const GoalCandidates goal_candidates_; const bool use_bus_stop_area_; @@ -83,8 +82,8 @@ class FreespaceParkingRequest public: FreespaceParkingRequest( const GoalPlannerParameters & parameters, - const autoware::universe_utils::LinearRing2d & vehicle_footprint, - const GoalCandidates & goal_candidates, const PlannerData & planner_data) + const autoware_utils::LinearRing2d & vehicle_footprint, const GoalCandidates & goal_candidates, + const PlannerData & planner_data) : parameters_(parameters), vehicle_footprint_(vehicle_footprint), goal_candidates_(goal_candidates) @@ -99,7 +98,7 @@ class FreespaceParkingRequest const std::optional & last_path_update_time, const bool is_stopped); const GoalPlannerParameters parameters_; - const autoware::universe_utils::LinearRing2d vehicle_footprint_; + const autoware_utils::LinearRing2d vehicle_footprint_; const GoalCandidates goal_candidates_; const std::shared_ptr & get_planner_data() const { return planner_data_; } diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/util.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/util.hpp index 5f0710382e46d..8ec70f136cf6c 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/util.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/util.hpp @@ -44,7 +44,7 @@ using geometry_msgs::msg::Twist; using visualization_msgs::msg::Marker; using visualization_msgs::msg::MarkerArray; using Shape = autoware_perception_msgs::msg::Shape; -using Polygon2d = autoware::universe_utils::Polygon2d; +using Polygon2d = autoware_utils::Polygon2d; lanelet::ConstLanelets getPullOverLanes( const RouteHandler & route_handler, const bool left_side, const double backward_distance, @@ -139,8 +139,8 @@ bool checkObjectsCollision( // debug MarkerArray createPullOverAreaMarkerArray( - const autoware::universe_utils::MultiPolygon2d area_polygons, - const std_msgs::msg::Header & header, const std_msgs::msg::ColorRGBA & color, const double z); + const autoware_utils::MultiPolygon2d area_polygons, const std_msgs::msg::Header & header, + const std_msgs::msg::ColorRGBA & color, const double z); MarkerArray createPosesMarkerArray( const std::vector & poses, std::string && ns, const std_msgs::msg::ColorRGBA & color); MarkerArray createTextsMarkerArray( diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/package.xml b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/package.xml index ee52ff5261f0a..e4dcc03cbfebf 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/package.xml +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/package.xml @@ -27,7 +27,7 @@ autoware_motion_utils autoware_rtc_interface autoware_test_utils - autoware_universe_utils + autoware_utils pluginlib rclcpp visualization_msgs diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/decision_state.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/decision_state.cpp index fbaeb1ca52f59..9538c51d73c82 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/decision_state.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/decision_state.cpp @@ -32,7 +32,7 @@ void PathDecisionStateController::transit_state( const std::shared_ptr occupancy_grid_map, const bool is_current_safe, const GoalPlannerParameters & parameters, const GoalSearcher & goal_searcher, - std::vector & ego_polygons_expanded) + std::vector & ego_polygons_expanded) { const auto next_state = get_next_state( pull_over_path_opt, now, static_target_objects, dynamic_target_objects, planner_data, @@ -47,7 +47,7 @@ PathDecisionState PathDecisionStateController::get_next_state( const std::shared_ptr occupancy_grid_map, const bool is_current_safe, const GoalPlannerParameters & parameters, const GoalSearcher & goal_searcher, - std::vector & ego_polygons_expanded) const + std::vector & ego_polygons_expanded) const { auto next_state = current_state_; diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/default_fixed_goal_planner.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/default_fixed_goal_planner.cpp index b4b0cad48847a..c8793d20510f9 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/default_fixed_goal_planner.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/default_fixed_goal_planner.cpp @@ -23,7 +23,7 @@ namespace autoware::behavior_path_planner { -using Point2d = autoware::universe_utils::Point2d; +using Point2d = autoware_utils::Point2d; using autoware_internal_planning_msgs::msg::PathWithLaneId; BehaviorModuleOutput DefaultFixedGoalPlanner::plan( const std::shared_ptr & planner_data) const diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp index 82e0a5546c6df..0e8e804f6236b 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp @@ -27,13 +27,13 @@ #include "autoware/behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp" #include "autoware/behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" #include "autoware/behavior_path_planner_common/utils/utils.hpp" -#include "autoware/universe_utils/geometry/boost_polygon_utils.hpp" +#include "autoware_utils/geometry/boost_polygon_utils.hpp" -#include -#include #include #include #include +#include +#include #include #include @@ -57,9 +57,9 @@ using autoware::motion_utils::calcLongitudinalOffsetPose; using autoware::motion_utils::calcSignedArcLength; using autoware::motion_utils::findFirstNearestSegmentIndexWithSoftConstraints; using autoware::motion_utils::insertDecelPoint; -using autoware::universe_utils::calcDistance2d; -using autoware::universe_utils::calcOffsetPose; -using autoware::universe_utils::createMarkerColor; +using autoware_utils::calc_distance2d; +using autoware_utils::calc_offset_pose; +using autoware_utils::create_marker_color; using nav_msgs::msg::OccupancyGrid; namespace autoware::behavior_path_planner @@ -118,7 +118,7 @@ bool isOnModifiedGoal( const Pose & current_pose, const GoalCandidate & modified_goal, const GoalPlannerParameters & parameters) { - return calcDistance2d(current_pose, modified_goal.goal_pose) < parameters.th_arrived_distance; + return calc_distance2d(current_pose, modified_goal.goal_pose) < parameters.th_arrived_distance; } bool isOnModifiedGoal( @@ -465,10 +465,9 @@ void LaneParkingPlanner::normal_pullover_planning_helper( const auto original_pose = planner_data->route_handler->getOriginalGoalPose(); if ( parameters_.bus_stop_area.use_bus_stop_area && - std::fabs(autoware::universe_utils::normalizeRadian( - autoware::universe_utils::getRPY(original_pose).z - - autoware::universe_utils::getRPY(closest_start_pose.value()).z)) > - pull_over_angle_threshold) { + std::fabs(autoware_utils::normalize_radian( + autoware_utils::get_rpy(original_pose).z - + autoware_utils::get_rpy(closest_start_pose.value()).z)) > pull_over_angle_threshold) { // reset and try bezier next time switch_bezier_ = true; path_candidates.clear(); @@ -488,7 +487,7 @@ void LaneParkingPlanner::bezier_planning_helper( std::vector & path_candidates, std::optional> & sorted_indices_opt) const { - autoware::universe_utils::StopWatch timer; + autoware_utils::StopWatch timer; timer.tic("bezier"); std::vector path_candidates_all; for (const auto & goal_candidate : goal_candidates) { @@ -702,7 +701,7 @@ std::pair GoalPlannerModule::sync void GoalPlannerModule::updateData() { - universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + autoware_utils::ScopedTimeTrack st(__func__, *time_keeper_); if (!goal_searcher_) { goal_searcher_.emplace(GoalSearcher::create(parameters_, vehicle_footprint_, planner_data_)); @@ -720,7 +719,7 @@ void GoalPlannerModule::updateData() bus_stop_area_polygons.begin(), bus_stop_area_polygons.end(), [&](const auto & area) { const auto & goal_position = planner_data_->route_handler->getOriginalGoalPose().position; return boost::geometry::within( - universe_utils::Point2d{goal_position.x, goal_position.y}, area); + autoware_utils::Point2d{goal_position.x, goal_position.y}, area); }); goal_candidates_ = generateGoalCandidates(goal_searcher_.value(), use_bus_stop_area_); } @@ -886,7 +885,7 @@ bool GoalPlannerModule::isExecutionRequested() const // we want to activate the pull over at this time as well. const Pose previous_terminal_pose = getPreviousModuleOutput().path.points.back().point.pose; const double vehicle_half_width = planner_data_->parameters.vehicle_width / 2.0; - const Pose previous_terminal_vehicle_edge_pose = calcOffsetPose( + const Pose previous_terminal_vehicle_edge_pose = calc_offset_pose( previous_terminal_pose, 0, left_side_parking_ ? vehicle_half_width : -vehicle_half_width, 0); lanelet::ConstLanelet previous_module_terminal_vehicle_edge_lane{}; route_handler->getClosestLaneletWithinRoute( @@ -989,7 +988,7 @@ bool GoalPlannerModule::canReturnToLaneParking(const PullOverContextData & conte GoalCandidates GoalPlannerModule::generateGoalCandidates( GoalSearcher & goal_searcher, const bool use_bus_stop_area) const { - universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + autoware_utils::ScopedTimeTrack st(__func__, *time_keeper_); // calculate goal candidates const auto & route_handler = planner_data_->route_handler; @@ -1011,7 +1010,7 @@ GoalCandidates GoalPlannerModule::generateGoalCandidates( BehaviorModuleOutput GoalPlannerModule::plan() { - universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + autoware_utils::ScopedTimeTrack st(__func__, *time_keeper_); if (utils::isAllowedGoalModification(planner_data_->route_handler)) { if (!context_data_) { @@ -1186,7 +1185,7 @@ std::optional GoalPlannerModule::selectPullOverPath( const std::vector & pull_over_path_candidates, const std::optional> sorted_bezier_indices_opt) const { - universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + autoware_utils::ScopedTimeTrack st(__func__, *time_keeper_); const auto & goal_pose = planner_data_->route_handler->getOriginalGoalPose(); const double backward_length = @@ -1272,7 +1271,7 @@ std::optional GoalPlannerModule::selectPullOverPath( std::vector GoalPlannerModule::generateDrivableLanes() const { - universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + autoware_utils::ScopedTimeTrack st(__func__, *time_keeper_); const lanelet::ConstLanelets current_lanes = utils::getExtendedCurrentLanes( planner_data_, parameters_.backward_goal_search_length, parameters_.forward_goal_search_length, @@ -1287,7 +1286,7 @@ void GoalPlannerModule::setOutput( const std::optional selected_pull_over_path_with_velocity_opt, const PullOverContextData & context_data, BehaviorModuleOutput & output) { - universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + autoware_utils::ScopedTimeTrack st(__func__, *time_keeper_); output.reference_path = getPreviousModuleOutput().reference_path; @@ -1336,7 +1335,7 @@ void GoalPlannerModule::setOutput( void GoalPlannerModule::setDrivableAreaInfo( const PullOverContextData & context_data, BehaviorModuleOutput & output) const { - universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + autoware_utils::ScopedTimeTrack st(__func__, *time_keeper_); if ( context_data.pull_over_path_opt && @@ -1402,7 +1401,7 @@ void GoalPlannerModule::updateSteeringFactor( void GoalPlannerModule::decideVelocity(PullOverPath & pull_over_path) { - universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + autoware_utils::ScopedTimeTrack st(__func__, *time_keeper_); const double current_vel = planner_data_->self_odometry->twist.twist.linear.x; @@ -1417,7 +1416,7 @@ void GoalPlannerModule::decideVelocity(PullOverPath & pull_over_path) BehaviorModuleOutput GoalPlannerModule::planPullOver(PullOverContextData & context_data) { - universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + autoware_utils::ScopedTimeTrack st(__func__, *time_keeper_); const auto current_state = path_decision_controller_.get_current_state(); if (current_state.state != PathDecisionState::DecisionKind::DECIDED) { @@ -1437,7 +1436,7 @@ BehaviorModuleOutput GoalPlannerModule::planPullOver(PullOverContextData & conte BehaviorModuleOutput GoalPlannerModule::planPullOverAsCandidate( PullOverContextData & context_data, const std::string & detail) { - universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + autoware_utils::ScopedTimeTrack st(__func__, *time_keeper_); // if pull over path candidates generation is not finished, use previous module output if (context_data.lane_parking_response.pull_over_path_candidates.empty()) { @@ -1567,7 +1566,7 @@ BehaviorModuleOutput GoalPlannerModule::planPullOverAsCandidate( BehaviorModuleOutput GoalPlannerModule::planPullOverAsOutput(PullOverContextData & context_data) { - universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + autoware_utils::ScopedTimeTrack st(__func__, *time_keeper_); std::chrono::system_clock::time_point start; start = std::chrono::system_clock::now(); @@ -1674,7 +1673,7 @@ BehaviorModuleOutput GoalPlannerModule::planPullOverAsOutput(PullOverContextData void GoalPlannerModule::postProcess() { - universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + autoware_utils::ScopedTimeTrack st(__func__, *time_keeper_); if (!context_data_) { RCLCPP_WARN_THROTTLE( @@ -1710,7 +1709,7 @@ void GoalPlannerModule::postProcess() BehaviorModuleOutput GoalPlannerModule::planWaitingApproval() { - universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + autoware_utils::ScopedTimeTrack st(__func__, *time_keeper_); if (utils::isAllowedGoalModification(planner_data_->route_handler)) { if (!context_data_) { @@ -1731,7 +1730,7 @@ BehaviorModuleOutput GoalPlannerModule::planWaitingApproval() std::pair GoalPlannerModule::calcDistanceToPathChange( const PullOverContextData & context_data) const { - universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + autoware_utils::ScopedTimeTrack st(__func__, *time_keeper_); if (!context_data.pull_over_path_opt) { return {std::numeric_limits::max(), std::numeric_limits::max()}; @@ -1764,7 +1763,7 @@ std::pair GoalPlannerModule::calcDistanceToPathChange( PathWithLaneId GoalPlannerModule::generateStopPath( const PullOverContextData & context_data, const std::string & detail) const { - universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + autoware_utils::ScopedTimeTrack st(__func__, *time_keeper_); assert(goal_searcher_); const auto & goal_searcher = goal_searcher_.value(); @@ -1881,7 +1880,7 @@ PathWithLaneId GoalPlannerModule::generateStopPath( PathWithLaneId GoalPlannerModule::generateFeasibleStopPath( const PathWithLaneId & path, const std::string & detail) const { - universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + autoware_utils::ScopedTimeTrack st(__func__, *time_keeper_); // calc minimum stop distance under maximum deceleration const auto min_stop_distance = calcFeasibleDecelDistance( @@ -1948,7 +1947,7 @@ bool FreespaceParkingPlanner::isStuck( bool GoalPlannerModule::hasFinishedCurrentPath(const PullOverContextData & ctx_data) const { - universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + autoware_utils::ScopedTimeTrack st(__func__, *time_keeper_); if (!decided_time_) { return false; @@ -1988,13 +1987,13 @@ bool GoalPlannerModule::hasFinishedCurrentPath(const PullOverContextData & ctx_d const auto & current_path_end = ctx_data.pull_over_path_opt.value().getCurrentPath().points.back(); const auto & self_pose = planner_data_->self_odometry->pose.pose; - return autoware::universe_utils::calcDistance2d(current_path_end, self_pose) < + return autoware_utils::calc_distance2d(current_path_end, self_pose) < parameters_.th_arrived_distance; } TurnSignalInfo GoalPlannerModule::calcTurnSignalInfo(const PullOverContextData & context_data) { - universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + autoware_utils::ScopedTimeTrack st(__func__, *time_keeper_); if (!context_data.pull_over_path_opt) { return {}; @@ -2140,7 +2139,7 @@ double GoalPlannerModule::calcSignedArcLengthFromEgo( void GoalPlannerModule::deceleratePath(PullOverPath & pull_over_path) const { - universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + autoware_utils::ScopedTimeTrack st(__func__, *time_keeper_); assert(goal_searcher_); const auto & goal_searcher = goal_searcher_.value(); @@ -2176,7 +2175,7 @@ void GoalPlannerModule::deceleratePath(PullOverPath & pull_over_path) const void GoalPlannerModule::decelerateForTurnSignal(const Pose & stop_pose, PathWithLaneId & path) const { - universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + autoware_utils::ScopedTimeTrack st(__func__, *time_keeper_); const double time = planner_data_->parameters.turn_signal_search_time; const Pose & current_pose = planner_data_->self_odometry->pose.pose; @@ -2217,7 +2216,7 @@ void GoalPlannerModule::decelerateForTurnSignal(const Pose & stop_pose, PathWith void GoalPlannerModule::decelerateBeforeSearchStart( const Pose & search_start_offset_pose, PathWithLaneId & path) const { - universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + autoware_utils::ScopedTimeTrack st(__func__, *time_keeper_); const double pull_over_velocity = parameters_.pull_over_velocity; const Pose & current_pose = planner_data_->self_odometry->pose.pose; @@ -2237,7 +2236,7 @@ void GoalPlannerModule::decelerateBeforeSearchStart( bool GoalPlannerModule::isCrossingPossible( const lanelet::ConstLanelet & start_lane, const lanelet::ConstLanelet & end_lane) const { - universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + autoware_utils::ScopedTimeTrack st(__func__, *time_keeper_); if (start_lane.centerline().empty() || end_lane.centerline().empty()) { return false; @@ -2347,7 +2346,7 @@ static std::vector filterOb for (const auto & target_lane : target_lanes) { const auto lane_poly = target_lane.polygon2d().basicPolygon(); for (const auto & filtered_object : filtered_objects.objects) { - const auto object_bbox = autoware::universe_utils::toPolygon2d(filtered_object); + const auto object_bbox = autoware_utils::to_polygon2d(filtered_object); if (boost::geometry::within(object_bbox, lane_poly)) { within_filtered_objects.push_back(filtered_object); } @@ -2434,7 +2433,7 @@ std::pair GoalPlannerM lanelet::utils::conversion::toGeomMsgPt(fist_road_lane.centerline().front()); const double lane_yaw = lanelet::utils::getLaneletAngle(fist_road_lane, first_road_point); first_road_pose.position = first_road_point; - first_road_pose.orientation = autoware::universe_utils::createQuaternionFromYaw(lane_yaw); + first_road_pose.orientation = autoware_utils::create_quaternion_from_yaw(lane_yaw); // if current ego pose is before pull over lanes segment, use first road lanelet center pose if ( calcSignedArcLength( @@ -2499,14 +2498,14 @@ std::pair GoalPlannerM void GoalPlannerModule::setDebugData(const PullOverContextData & context_data) { - universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + autoware_utils::ScopedTimeTrack st(__func__, *time_keeper_); debug_marker_.markers.clear(); using autoware::motion_utils::createStopVirtualWallMarker; - using autoware::universe_utils::createDefaultMarker; - using autoware::universe_utils::createMarkerColor; - using autoware::universe_utils::createMarkerScale; + using autoware_utils::create_default_marker; + using autoware_utils::create_marker_color; + using autoware_utils::create_marker_scale; using marker_utils::createObjectsMarkerArray; using marker_utils::createPathMarkerArray; using marker_utils::createPoseMarkerArray; @@ -2521,14 +2520,14 @@ void GoalPlannerModule::setDebugData(const PullOverContextData & context_data) for (auto & marker : added.markers) { marker.lifetime = rclcpp::Duration::from_seconds(1.5); } - autoware::universe_utils::appendMarkerArray(added, &debug_marker_); + autoware_utils::append_marker_array(added, &debug_marker_); }; if (utils::isAllowedGoalModification(planner_data_->route_handler)) { // Visualize pull over areas const auto color = path_decision_controller_.get_current_state().state == PathDecisionState::DecisionKind::DECIDED - ? createMarkerColor(1.0, 1.0, 0.0, 0.999) // yellow - : createMarkerColor(0.0, 1.0, 0.0, 0.999); // green + ? create_marker_color(1.0, 1.0, 0.0, 0.999) // yellow + : create_marker_color(0.0, 1.0, 0.0, 0.999); // green const double z = planner_data_->route_handler->getGoalPose().position.z; add(goal_planner_utils::createPullOverAreaMarkerArray( goal_searcher_->getAreaPolygons(), header, color, z)); @@ -2537,19 +2536,18 @@ void GoalPlannerModule::setDebugData(const PullOverContextData & context_data) add(goal_planner_utils::createGoalCandidatesMarkerArray(goal_candidates_, color)); // Visualize objects extraction polygon - auto marker = autoware::universe_utils::createDefaultMarker( + auto marker = autoware_utils::create_default_marker( "map", rclcpp::Clock{RCL_ROS_TIME}.now(), "objects_extraction_polygon", 0, Marker::LINE_LIST, - autoware::universe_utils::createMarkerScale(0.1, 0.0, 0.0), - autoware::universe_utils::createMarkerColor(0.0, 1.0, 1.0, 0.999)); + autoware_utils::create_marker_scale(0.1, 0.0, 0.0), + autoware_utils::create_marker_color(0.0, 1.0, 1.0, 0.999)); const double ego_z = planner_data_->self_odometry->pose.pose.position.z; for (size_t i = 0; i < debug_data_.objects_extraction_polygon.outer().size(); ++i) { const auto & current_point = debug_data_.objects_extraction_polygon.outer().at(i); const auto & next_point = debug_data_.objects_extraction_polygon.outer().at( (i + 1) % debug_data_.objects_extraction_polygon.outer().size()); marker.points.push_back( - autoware::universe_utils::createPoint(current_point.x(), current_point.y(), ego_z)); - marker.points.push_back( - autoware::universe_utils::createPoint(next_point.x(), next_point.y(), ego_z)); + autoware_utils::create_point(current_point.x(), current_point.y(), ego_z)); + marker.points.push_back(autoware_utils::create_point(next_point.x(), next_point.y(), ego_z)); } debug_marker_.markers.push_back(marker); @@ -2576,10 +2574,10 @@ void GoalPlannerModule::setDebugData(const PullOverContextData & context_data) createPathMarkerArray(partial_path, "partial_path_" + std::to_string(i), 0, 0.9, 0.5, 0.9)); } - auto marker = autoware::universe_utils::createDefaultMarker( + auto marker = autoware_utils::create_default_marker( "map", rclcpp::Clock{RCL_ROS_TIME}.now(), "detection_polygons", 0, Marker::LINE_LIST, - autoware::universe_utils::createMarkerScale(0.01, 0.0, 0.0), - autoware::universe_utils::createMarkerColor(0.0, 0.0, 1.0, 0.999)); + autoware_utils::create_marker_scale(0.01, 0.0, 0.0), + autoware_utils::create_marker_color(0.0, 0.0, 1.0, 0.999)); const double ego_z = planner_data_->self_odometry->pose.pose.position.z; for (const auto & ego_polygon : debug_data_.ego_polygons_expanded) { for (size_t ep_idx = 0; ep_idx < ego_polygon.outer().size(); ++ep_idx) { @@ -2587,18 +2585,18 @@ void GoalPlannerModule::setDebugData(const PullOverContextData & context_data) const auto & next_point = ego_polygon.outer().at((ep_idx + 1) % ego_polygon.outer().size()); marker.points.push_back( - autoware::universe_utils::createPoint(current_point.x(), current_point.y(), ego_z)); + autoware_utils::create_point(current_point.x(), current_point.y(), ego_z)); marker.points.push_back( - autoware::universe_utils::createPoint(next_point.x(), next_point.y(), ego_z)); + autoware_utils::create_point(next_point.x(), next_point.y(), ego_z)); } } debug_marker_.markers.push_back(marker); - autoware::universe_utils::appendMarkerArray( + autoware_utils::append_marker_array( goal_planner_utils::createLaneletPolygonMarkerArray( debug_data_.expanded_pull_over_lane_between_ego.polygon3d(), header, "expanded_pull_over_lane_between_ego", - autoware::universe_utils::createMarkerColor(1.0, 0.7, 0.0, 0.999)), + autoware_utils::create_marker_color(1.0, 0.7, 0.0, 0.999)), &debug_marker_); // Visualize debug poses @@ -2629,11 +2627,11 @@ void GoalPlannerModule::setDebugData(const PullOverContextData & context_data) { const auto & prev_data = context_data.prev_state_for_debug; visualization_msgs::msg::MarkerArray marker_array{}; - const auto color = prev_data.is_stable_safe ? createMarkerColor(1.0, 1.0, 1.0, 0.99) - : createMarkerColor(1.0, 0.0, 0.0, 0.99); - auto marker = createDefaultMarker( + const auto color = prev_data.is_stable_safe ? create_marker_color(1.0, 1.0, 1.0, 0.99) + : create_marker_color(1.0, 0.0, 0.0, 0.99); + auto marker = create_default_marker( header.frame_id, header.stamp, "safety_status", 0, - visualization_msgs::msg::Marker::TEXT_VIEW_FACING, createMarkerScale(0.0, 0.0, 1.0), color); + visualization_msgs::msg::Marker::TEXT_VIEW_FACING, create_marker_scale(0.0, 0.0, 1.0), color); marker.pose = planner_data_->self_odometry->pose.pose; marker.text += "is_safe: " + std::to_string(prev_data.is_stable_safe) + "\n"; @@ -2650,11 +2648,11 @@ void GoalPlannerModule::setDebugData(const PullOverContextData & context_data) // Visualize planner type text { visualization_msgs::msg::MarkerArray planner_type_marker_array{}; - const auto color = context_data.pull_over_path_opt ? createMarkerColor(1.0, 1.0, 1.0, 0.99) - : createMarkerColor(1.0, 0.0, 0.0, 0.99); - auto marker = createDefaultMarker( + const auto color = context_data.pull_over_path_opt ? create_marker_color(1.0, 1.0, 1.0, 0.99) + : create_marker_color(1.0, 0.0, 0.0, 0.99); + auto marker = create_default_marker( header.frame_id, header.stamp, "planner_type", 0, - visualization_msgs::msg::Marker::TEXT_VIEW_FACING, createMarkerScale(0.0, 0.0, 1.0), color); + visualization_msgs::msg::Marker::TEXT_VIEW_FACING, create_marker_scale(0.0, 0.0, 1.0), color); marker.pose = context_data.pull_over_path_opt ? context_data.pull_over_path_opt.value().modified_goal_pose() : planner_data_->self_odometry->pose.pose; diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_searcher.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_searcher.cpp index 9c13a7bd5aa11..72db734a9b998 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_searcher.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_searcher.cpp @@ -17,12 +17,12 @@ #include "autoware/behavior_path_goal_planner_module/util.hpp" #include "autoware/behavior_path_planner_common/utils/path_utils.hpp" #include "autoware/behavior_path_planner_common/utils/utils.hpp" -#include "autoware/universe_utils/geometry/boost_polygon_utils.hpp" #include "autoware_lanelet2_extension/regulatory_elements/bus_stop_area.hpp" #include "autoware_lanelet2_extension/regulatory_elements/no_parking_area.hpp" #include "autoware_lanelet2_extension/regulatory_elements/no_stopping_area.hpp" #include "autoware_lanelet2_extension/utility/query.hpp" #include "autoware_lanelet2_extension/utility/utilities.hpp" +#include "autoware_utils/geometry/boost_polygon_utils.hpp" #include @@ -37,7 +37,7 @@ namespace autoware::behavior_path_planner { -using autoware::universe_utils::calcOffsetPose; +using autoware_utils::calc_offset_pose; using lanelet::autoware::NoParkingArea; using lanelet::autoware::NoStoppingArea; @@ -194,7 +194,7 @@ GoalCandidates GoalSearcher::search( // todo(kosuke55): fix orientation for inverseTransformPoint temporarily Pose center_pose = p.point.pose; center_pose.orientation = - autoware::universe_utils::createQuaternionFromYaw(tf2::getYaw(center_pose.orientation)); + autoware_utils::create_quaternion_from_yaw(tf2::getYaw(center_pose.orientation)); // ignore goal_pose near lane start const double distance_from_lane_start = @@ -213,7 +213,7 @@ GoalCandidates GoalSearcher::search( use_bus_stop_area ? -distance_from_bound.value() : -distance_from_bound.value() + sign * margin_from_boundary; // original means non lateral offset poses - const Pose original_search_pose = calcOffsetPose(center_pose, 0, offset_from_center_line, 0); + const Pose original_search_pose = calc_offset_pose(center_pose, 0, offset_from_center_line, 0); const double longitudinal_distance_from_original_goal = std::abs(autoware::motion_utils::calcSignedArcLength( center_line_path.points, reference_goal_pose.position, original_search_pose.position)); @@ -221,10 +221,10 @@ GoalCandidates GoalSearcher::search( Pose search_pose{}; // search goal_pose in lateral direction for (double dy = 0; dy <= max_lateral_offset; dy += lateral_offset_interval) { - search_pose = calcOffsetPose(original_search_pose, 0, sign * dy, 0); + search_pose = calc_offset_pose(original_search_pose, 0, sign * dy, 0); - const auto transformed_vehicle_footprint = autoware::universe_utils::transformVector( - vehicle_footprint_, autoware::universe_utils::pose2transform(search_pose)); + const auto transformed_vehicle_footprint = autoware_utils::transform_vector( + vehicle_footprint_, autoware_utils::pose2transform(search_pose)); if ( parameters_.bus_stop_area.use_bus_stop_area && @@ -264,7 +264,7 @@ GoalCandidates GoalSearcher::search( const auto pull_over_lanelet = lanelet::utils::combineLaneletsShape(pull_over_lanes_); const auto vehicle_center_pose_for_bound_opt = goal_planner_utils::calcClosestPose( left_side_parking_ ? pull_over_lanelet.leftBound() : pull_over_lanelet.rightBound(), - autoware::universe_utils::createPoint( + autoware_utils::create_point( vehicle_center_point.x(), vehicle_center_point.y(), search_pose.position.z)); if (!vehicle_center_pose_for_bound_opt) { continue; @@ -329,9 +329,9 @@ void GoalSearcher::countObjectsToAvoid( // count number of objects to avoid for (const auto & object : objects.objects) { for (const auto & p : current_center_line_path.points) { - const auto transformed_vehicle_footprint = autoware::universe_utils::transformVector( - vehicle_footprint_, autoware::universe_utils::pose2transform(p.point.pose)); - const auto obj_polygon = autoware::universe_utils::toPolygon2d(object); + const auto transformed_vehicle_footprint = autoware_utils::transform_vector( + vehicle_footprint_, autoware_utils::pose2transform(p.point.pose)); + const auto obj_polygon = autoware_utils::to_polygon2d(object); const double distance = boost::geometry::distance(obj_polygon, transformed_vehicle_footprint); if (distance > parameters_.object_recognition_collision_check_hard_margins.back()) { continue; @@ -470,7 +470,7 @@ bool GoalSearcher::checkCollisionWithLongitudinalDistance( parameters_.longitudinal_margin - parameters_.occupancy_grid_collision_check_margin, 0.0); // check forward collision - const Pose ego_pose_moved_forward = calcOffsetPose(ego_pose, offset, 0, 0); + const Pose ego_pose_moved_forward = calc_offset_pose(ego_pose, offset, 0, 0); const Pose forward_pose_grid_coords = global2local(occupancy_grid_map->getMap(), ego_pose_moved_forward); const auto forward_idx = pose2index( @@ -481,7 +481,7 @@ bool GoalSearcher::checkCollisionWithLongitudinalDistance( } // check backward collision - const Pose ego_pose_moved_backward = calcOffsetPose(ego_pose, -offset, 0, 0); + const Pose ego_pose_moved_backward = calc_offset_pose(ego_pose, -offset, 0, 0); const Pose backward_pose_grid_coords = global2local(occupancy_grid_map->getMap(), ego_pose_moved_backward); const auto backward_idx = pose2index( @@ -504,9 +504,9 @@ bool GoalSearcher::checkCollisionWithLongitudinalDistance( void GoalSearcher::createAreaPolygons( std::vector original_search_poses, const std::shared_ptr & planner_data) { - using autoware::universe_utils::MultiPolygon2d; - using autoware::universe_utils::Point2d; - using autoware::universe_utils::Polygon2d; + using autoware_utils::MultiPolygon2d; + using autoware_utils::Point2d; + using autoware_utils::Polygon2d; const double vehicle_width = planner_data->parameters.vehicle_width; const double base_link2front = planner_data->parameters.base_link2front; @@ -527,22 +527,23 @@ void GoalSearcher::createAreaPolygons( const double left_front_offset = left_side_parking_ ? vehicle_width / 2 : vehicle_width / 2 + max_lateral_offset; - const Point p_left_front = calcOffsetPose(p, base_link2front, left_front_offset, 0).position; + const Point p_left_front = calc_offset_pose(p, base_link2front, left_front_offset, 0).position; appendPointToPolygon(footprint, p_left_front); const double right_front_offset = left_side_parking_ ? -vehicle_width / 2 - max_lateral_offset : -vehicle_width / 2; - const Point p_right_front = calcOffsetPose(p, base_link2front, right_front_offset, 0).position; + const Point p_right_front = + calc_offset_pose(p, base_link2front, right_front_offset, 0).position; appendPointToPolygon(footprint, p_right_front); const double right_back_offset = left_side_parking_ ? -vehicle_width / 2 - max_lateral_offset : -vehicle_width / 2; - const Point p_right_back = calcOffsetPose(p, -base_link2rear, right_back_offset, 0).position; + const Point p_right_back = calc_offset_pose(p, -base_link2rear, right_back_offset, 0).position; appendPointToPolygon(footprint, p_right_back); const double left_back_offset = left_side_parking_ ? vehicle_width / 2 : vehicle_width / 2 + max_lateral_offset; - const Point p_left_back = calcOffsetPose(p, -base_link2rear, left_back_offset, 0).position; + const Point p_left_back = calc_offset_pose(p, -base_link2rear, left_back_offset, 0).position; appendPointToPolygon(footprint, p_left_back); appendPointToPolygon(footprint, p_left_front); diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/manager.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/manager.cpp index 4b89ddab1f272..51359214189fe 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/manager.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/manager.cpp @@ -16,7 +16,7 @@ #include "autoware/behavior_path_goal_planner_module/goal_planner_module.hpp" #include "autoware/behavior_path_goal_planner_module/goal_planner_parameters.hpp" -#include "autoware/universe_utils/ros/update_param.hpp" +#include "autoware_utils/ros/update_param.hpp" #include @@ -456,7 +456,7 @@ void GoalPlannerModuleManager::updateModuleParams( // object_recognition_collision_check_hard_margins, maximum_deceleration, shift_sampling_num or // parking_policy, there seems to be a problem when we use a temp value to check these values. - using autoware::universe_utils::updateParam; + using autoware_utils::update_param; auto & p = parameters_; @@ -464,93 +464,93 @@ void GoalPlannerModuleManager::updateModuleParams( // general params { - updateParam(parameters, base_ns + "th_stopped_velocity", p->th_stopped_velocity); - updateParam(parameters, base_ns + "th_arrived_distance", p->th_arrived_distance); - updateParam(parameters, base_ns + "th_stopped_time", p->th_stopped_time); + update_param(parameters, base_ns + "th_stopped_velocity", p->th_stopped_velocity); + update_param(parameters, base_ns + "th_arrived_distance", p->th_arrived_distance); + update_param(parameters, base_ns + "th_stopped_time", p->th_stopped_time); - updateParam( + update_param( parameters, base_ns + "center_line_path_interval", p->center_line_path_interval); } // goal search { const std::string ns = base_ns + "goal_search."; - updateParam(parameters, ns + "goal_priority", p->goal_priority); - updateParam( + update_param(parameters, ns + "goal_priority", p->goal_priority); + update_param( parameters, ns + "minimum_weighted_distance.lateral_weight", p->minimum_weighted_distance_lateral_weight); - updateParam( + update_param( parameters, ns + "prioritize_goals_before_objects", p->prioritize_goals_before_objects); - updateParam( + update_param( parameters, ns + "forward_goal_search_length", p->forward_goal_search_length); - updateParam( + update_param( parameters, ns + "backward_goal_search_length", p->backward_goal_search_length); - updateParam(parameters, ns + "goal_search_interval", p->goal_search_interval); - updateParam(parameters, ns + "longitudinal_margin", p->longitudinal_margin); - updateParam(parameters, ns + "max_lateral_offset", p->max_lateral_offset); - updateParam(parameters, ns + "lateral_offset_interval", p->lateral_offset_interval); - updateParam( + update_param(parameters, ns + "goal_search_interval", p->goal_search_interval); + update_param(parameters, ns + "longitudinal_margin", p->longitudinal_margin); + update_param(parameters, ns + "max_lateral_offset", p->max_lateral_offset); + update_param(parameters, ns + "lateral_offset_interval", p->lateral_offset_interval); + update_param( parameters, ns + "ignore_distance_from_lane_start", p->ignore_distance_from_lane_start); - updateParam(parameters, ns + "margin_from_boundary", p->margin_from_boundary); + update_param(parameters, ns + "margin_from_boundary", p->margin_from_boundary); } // occupancy grid map { const std::string ns = base_ns + "occupancy_grid."; - updateParam( + update_param( parameters, ns + "use_occupancy_grid_for_goal_search", p->use_occupancy_grid_for_goal_search); - updateParam( + update_param( parameters, ns + "use_occupancy_grid_for_path_collision_check", p->use_occupancy_grid_for_path_collision_check); - updateParam( + update_param( parameters, ns + "use_occupancy_grid_for_goal_longitudinal_margin", p->use_occupancy_grid_for_goal_longitudinal_margin); - updateParam( + update_param( parameters, ns + "occupancy_grid_collision_check_margin", p->occupancy_grid_collision_check_margin); - updateParam(parameters, ns + "theta_size", p->theta_size); - updateParam(parameters, ns + "obstacle_threshold", p->obstacle_threshold); + update_param(parameters, ns + "theta_size", p->theta_size); + update_param(parameters, ns + "obstacle_threshold", p->obstacle_threshold); } // object recognition { const std::string ns = base_ns + "object_recognition."; - updateParam( + update_param( parameters, ns + "object_recognition_collision_check_max_extra_stopping_margin", p->object_recognition_collision_check_max_extra_stopping_margin); - updateParam(parameters, ns + "th_moving_object_velocity", p->th_moving_object_velocity); - updateParam(parameters, ns + "detection_bound_offset", p->detection_bound_offset); - updateParam(parameters, ns + "outer_road_detection_offset", p->outer_road_detection_offset); - updateParam(parameters, ns + "inner_road_detection_offset", p->inner_road_detection_offset); + update_param(parameters, ns + "th_moving_object_velocity", p->th_moving_object_velocity); + update_param(parameters, ns + "detection_bound_offset", p->detection_bound_offset); + update_param(parameters, ns + "outer_road_detection_offset", p->outer_road_detection_offset); + update_param(parameters, ns + "inner_road_detection_offset", p->inner_road_detection_offset); } // pull over general params { const std::string ns = base_ns + "pull_over."; - updateParam( + update_param( parameters, ns + "minimum_request_length", p->pull_over_minimum_request_length); - updateParam(parameters, ns + "pull_over_velocity", p->pull_over_velocity); - updateParam( + update_param(parameters, ns + "pull_over_velocity", p->pull_over_velocity); + update_param( parameters, ns + "pull_over_minimum_velocity", p->pull_over_minimum_velocity); - updateParam(parameters, ns + "decide_path_distance", p->decide_path_distance); - updateParam(parameters, ns + "maximum_jerk", p->maximum_jerk); - updateParam(parameters, ns + "path_priority", p->path_priority); - updateParam>( + update_param(parameters, ns + "decide_path_distance", p->decide_path_distance); + update_param(parameters, ns + "maximum_jerk", p->maximum_jerk); + update_param(parameters, ns + "path_priority", p->path_priority); + update_param>( parameters, ns + "efficient_path_order", p->efficient_path_order); } // shift parking { const std::string ns = base_ns + "pull_over.shift_parking."; - updateParam(parameters, ns + "enable_shift_parking", p->enable_shift_parking); - updateParam(parameters, ns + "maximum_lateral_jerk", p->maximum_lateral_jerk); - updateParam(parameters, ns + "minimum_lateral_jerk", p->minimum_lateral_jerk); - updateParam(parameters, ns + "deceleration_interval", p->deceleration_interval); - updateParam( + update_param(parameters, ns + "enable_shift_parking", p->enable_shift_parking); + update_param(parameters, ns + "maximum_lateral_jerk", p->maximum_lateral_jerk); + update_param(parameters, ns + "minimum_lateral_jerk", p->minimum_lateral_jerk); + update_param(parameters, ns + "deceleration_interval", p->deceleration_interval); + update_param( parameters, ns + "after_shift_straight_distance", p->after_shift_straight_distance); - updateParam( + update_param( parameters, ns + "lane_departure_check_expansion_margin", p->lane_departure_check_expansion_margin); } @@ -564,20 +564,21 @@ void GoalPlannerModuleManager::updateModuleParams( // forward parallel parking forward { const std::string ns = base_ns + "pull_over.parallel_parking.forward."; - updateParam(parameters, ns + "enable_arc_forward_parking", p->enable_arc_forward_parking); - updateParam( + update_param( + parameters, ns + "enable_arc_forward_parking", p->enable_arc_forward_parking); + update_param( parameters, ns + "after_forward_parking_straight_distance", p->parallel_parking_parameters.after_forward_parking_straight_distance); - updateParam( + update_param( parameters, ns + "forward_parking_velocity", p->parallel_parking_parameters.forward_parking_velocity); - updateParam( + update_param( parameters, ns + "forward_parking_lane_departure_margin", p->parallel_parking_parameters.forward_parking_lane_departure_margin); - updateParam( + update_param( parameters, ns + "forward_parking_path_interval", p->parallel_parking_parameters.forward_parking_path_interval); - updateParam( + update_param( parameters, ns + "forward_parking_max_steer_angle", p->parallel_parking_parameters.forward_parking_max_steer_angle); } @@ -585,21 +586,21 @@ void GoalPlannerModuleManager::updateModuleParams( // forward parallel parking backward { const std::string ns = base_ns + "pull_over.parallel_parking.backward."; - updateParam( + update_param( parameters, ns + "enable_arc_backward_parking", p->enable_arc_backward_parking); - updateParam( + update_param( parameters, ns + "after_backward_parking_straight_distance", p->parallel_parking_parameters.after_backward_parking_straight_distance); - updateParam( + update_param( parameters, ns + "backward_parking_velocity", p->parallel_parking_parameters.backward_parking_velocity); - updateParam( + update_param( parameters, ns + "backward_parking_lane_departure_margin", p->parallel_parking_parameters.backward_parking_lane_departure_margin); - updateParam( + update_param( parameters, ns + "backward_parking_path_interval", p->parallel_parking_parameters.backward_parking_path_interval); - updateParam( + update_param( parameters, ns + "backward_parking_max_steer_angle", p->parallel_parking_parameters.backward_parking_max_steer_angle); } @@ -607,36 +608,36 @@ void GoalPlannerModuleManager::updateModuleParams( // freespace parking general params { const std::string ns = base_ns + "pull_over.freespace_parking."; - updateParam(parameters, ns + "enable_freespace_parking", p->enable_freespace_parking); - updateParam( + update_param(parameters, ns + "enable_freespace_parking", p->enable_freespace_parking); + update_param( parameters, ns + "freespace_parking_algorithm", p->freespace_parking_algorithm); - updateParam(parameters, ns + "velocity", p->freespace_parking_velocity); + update_param(parameters, ns + "velocity", p->freespace_parking_velocity); - updateParam(parameters, ns + "vehicle_shape_margin", p->vehicle_shape_margin); - updateParam( + update_param(parameters, ns + "vehicle_shape_margin", p->vehicle_shape_margin); + update_param( parameters, ns + "time_limit", p->freespace_parking_common_parameters.time_limit); - updateParam( + update_param( parameters, ns + "max_turning_ratio", p->freespace_parking_common_parameters.max_turning_ratio); - updateParam( + update_param( parameters, ns + "turning_steps", p->freespace_parking_common_parameters.turning_steps); } // freespace parking search config { const std::string ns = base_ns + "pull_over.freespace_parking.search_configs."; - updateParam( + update_param( parameters, ns + "theta_size", p->freespace_parking_common_parameters.theta_size); - updateParam( + update_param( parameters, ns + "angle_goal_range", p->freespace_parking_common_parameters.angle_goal_range); - updateParam( + update_param( parameters, ns + "curve_weight", p->freespace_parking_common_parameters.curve_weight); - updateParam( + update_param( parameters, ns + "reverse_weight", p->freespace_parking_common_parameters.reverse_weight); - updateParam( + update_param( parameters, ns + "lateral_goal_range", p->freespace_parking_common_parameters.lateral_goal_range); - updateParam( + update_param( parameters, ns + "longitudinal_goal_range", p->freespace_parking_common_parameters.longitudinal_goal_range); } @@ -644,7 +645,7 @@ void GoalPlannerModuleManager::updateModuleParams( // freespace parking costmap configs { const std::string ns = base_ns + "pull_over.freespace_parking.costmap_configs."; - updateParam( + update_param( parameters, ns + "obstacle_threshold", p->freespace_parking_common_parameters.obstacle_threshold); } @@ -652,11 +653,11 @@ void GoalPlannerModuleManager::updateModuleParams( // freespace parking astar { const std::string ns = base_ns + "pull_over.freespace_parking.astar."; - updateParam(parameters, ns + "search_method", p->astar_parameters.search_method); - updateParam( + update_param(parameters, ns + "search_method", p->astar_parameters.search_method); + update_param( parameters, ns + "only_behind_solutions", p->astar_parameters.only_behind_solutions); - updateParam(parameters, ns + "use_back", p->astar_parameters.use_back); - updateParam( + update_param(parameters, ns + "use_back", p->astar_parameters.use_back); + update_param( parameters, ns + "distance_heuristic_weight", p->astar_parameters.distance_heuristic_weight); } @@ -664,21 +665,22 @@ void GoalPlannerModuleManager::updateModuleParams( { const std::string ns = base_ns + "pull_over.freespace_parking.rrtstar."; - updateParam(parameters, ns + "enable_update", p->rrt_star_parameters.enable_update); - updateParam( + update_param(parameters, ns + "enable_update", p->rrt_star_parameters.enable_update); + update_param( parameters, ns + "use_informed_sampling", p->rrt_star_parameters.use_informed_sampling); - updateParam( + update_param( parameters, ns + "max_planning_time", p->rrt_star_parameters.max_planning_time); - updateParam(parameters, ns + "neighbor_radius", p->rrt_star_parameters.neighbor_radius); - updateParam(parameters, ns + "margin", p->rrt_star_parameters.margin); + update_param( + parameters, ns + "neighbor_radius", p->rrt_star_parameters.neighbor_radius); + update_param(parameters, ns + "margin", p->rrt_star_parameters.margin); } // stop condition { - updateParam( + update_param( parameters, base_ns + "stop_condition.maximum_deceleration_for_stop", p->maximum_deceleration_for_stop); - updateParam( + update_param( parameters, base_ns + "stop_condition.maximum_jerk_for_stop", p->maximum_jerk_for_stop); } @@ -687,19 +689,19 @@ void GoalPlannerModuleManager::updateModuleParams( // EgoPredictedPath { - updateParam( + update_param( parameters, ego_path_ns + "min_velocity", p->ego_predicted_path_params.min_velocity); - updateParam( + update_param( parameters, ego_path_ns + "min_acceleration", p->ego_predicted_path_params.acceleration); - updateParam( + update_param( parameters, ego_path_ns + "time_horizon_for_front_object", p->ego_predicted_path_params.time_horizon_for_front_object); - updateParam( + update_param( parameters, ego_path_ns + "time_horizon_for_rear_object", p->ego_predicted_path_params.time_horizon_for_rear_object); - updateParam( + update_param( parameters, ego_path_ns + "time_resolution", p->ego_predicted_path_params.time_resolution); - updateParam( + update_param( parameters, ego_path_ns + "delay_until_departure", p->ego_predicted_path_params.delay_until_departure); } @@ -707,34 +709,34 @@ void GoalPlannerModuleManager::updateModuleParams( // ObjectFilteringParams const std::string obj_filter_ns = path_safety_check_ns + "target_filtering."; { - updateParam( + update_param( parameters, obj_filter_ns + "safety_check_time_horizon", p->objects_filtering_params.safety_check_time_horizon); - updateParam( + update_param( parameters, obj_filter_ns + "safety_check_time_resolution", p->objects_filtering_params.safety_check_time_resolution); - updateParam( + update_param( parameters, obj_filter_ns + "object_check_forward_distance", p->objects_filtering_params.object_check_forward_distance); - updateParam( + update_param( parameters, obj_filter_ns + "object_check_backward_distance", p->objects_filtering_params.object_check_backward_distance); - updateParam( + update_param( parameters, obj_filter_ns + "ignore_object_velocity_threshold", p->objects_filtering_params.ignore_object_velocity_threshold); - updateParam( + update_param( parameters, obj_filter_ns + "include_opposite_lane", p->objects_filtering_params.include_opposite_lane); - updateParam( + update_param( parameters, obj_filter_ns + "invert_opposite_lane", p->objects_filtering_params.invert_opposite_lane); - updateParam( + update_param( parameters, obj_filter_ns + "check_all_predicted_path", p->objects_filtering_params.check_all_predicted_path); - updateParam( + update_param( parameters, obj_filter_ns + "use_all_predicted_path", p->objects_filtering_params.use_all_predicted_path); - updateParam( + update_param( parameters, obj_filter_ns + "use_predicted_path_outside_lanelet", p->objects_filtering_params.use_predicted_path_outside_lanelet); } @@ -742,47 +744,47 @@ void GoalPlannerModuleManager::updateModuleParams( // ObjectTypesToCheck const std::string obj_types_ns = obj_filter_ns + "object_types_to_check."; { - updateParam( + update_param( parameters, obj_types_ns + "check_car", p->objects_filtering_params.object_types_to_check.check_car); - updateParam( + update_param( parameters, obj_types_ns + "check_truck", p->objects_filtering_params.object_types_to_check.check_truck); - updateParam( + update_param( parameters, obj_types_ns + "check_bus", p->objects_filtering_params.object_types_to_check.check_bus); - updateParam( + update_param( parameters, obj_types_ns + "check_trailer", p->objects_filtering_params.object_types_to_check.check_trailer); - updateParam( + update_param( parameters, obj_types_ns + "check_unknown", p->objects_filtering_params.object_types_to_check.check_unknown); - updateParam( + update_param( parameters, obj_types_ns + "check_bicycle", p->objects_filtering_params.object_types_to_check.check_bicycle); - updateParam( + update_param( parameters, obj_types_ns + "check_motorcycle", p->objects_filtering_params.object_types_to_check.check_motorcycle); - updateParam( + update_param( parameters, obj_types_ns + "check_pedestrian", p->objects_filtering_params.object_types_to_check.check_pedestrian); } // ObjectLaneConfiguration const std::string obj_lane_ns = obj_filter_ns + "object_lane_configuration."; { - updateParam( + update_param( parameters, obj_lane_ns + "check_current_lane", p->objects_filtering_params.object_lane_configuration.check_current_lane); - updateParam( + update_param( parameters, obj_lane_ns + "check_right_side_lane", p->objects_filtering_params.object_lane_configuration.check_right_lane); - updateParam( + update_param( parameters, obj_lane_ns + "check_left_side_lane", p->objects_filtering_params.object_lane_configuration.check_left_lane); - updateParam( + update_param( parameters, obj_lane_ns + "check_shoulder_lane", p->objects_filtering_params.object_lane_configuration.check_shoulder_lane); - updateParam( + update_param( parameters, obj_lane_ns + "check_other_lane", p->objects_filtering_params.object_lane_configuration.check_other_lane); } @@ -790,22 +792,23 @@ void GoalPlannerModuleManager::updateModuleParams( // SafetyCheckParams const std::string safety_check_ns = path_safety_check_ns + "safety_check_params."; { - updateParam( + update_param( parameters, safety_check_ns + "keep_unsafe_time", p->safety_check_params.keep_unsafe_time); - updateParam(parameters, safety_check_ns + "method", p->safety_check_params.method); - updateParam( + update_param( + parameters, safety_check_ns + "method", p->safety_check_params.method); + update_param( parameters, safety_check_ns + "hysteresis_factor_expand_rate", p->safety_check_params.hysteresis_factor_expand_rate); - updateParam( + update_param( parameters, safety_check_ns + "collision_check_yaw_diff_threshold", p->safety_check_params.collision_check_yaw_diff_threshold); - updateParam( + update_param( parameters, safety_check_ns + "backward_path_length", p->safety_check_params.backward_path_length); - updateParam( + update_param( parameters, safety_check_ns + "forward_path_length", p->safety_check_params.forward_path_length); - updateParam( + update_param( parameters, safety_check_ns + "publish_debug_marker", p->safety_check_params.publish_debug_marker); } @@ -813,19 +816,19 @@ void GoalPlannerModuleManager::updateModuleParams( // RSSparams const std::string rss_ns = safety_check_ns + "rss_params."; { - updateParam( + update_param( parameters, rss_ns + "rear_vehicle_reaction_time", p->safety_check_params.rss_params.rear_vehicle_reaction_time); - updateParam( + update_param( parameters, rss_ns + "rear_vehicle_safety_time_margin", p->safety_check_params.rss_params.rear_vehicle_safety_time_margin); - updateParam( + update_param( parameters, rss_ns + "lateral_distance_max_threshold", p->safety_check_params.rss_params.lateral_distance_max_threshold); - updateParam( + update_param( parameters, rss_ns + "longitudinal_distance_min_threshold", p->safety_check_params.rss_params.longitudinal_distance_min_threshold); - updateParam( + update_param( parameters, rss_ns + "longitudinal_velocity_delta_time", p->safety_check_params.rss_params.longitudinal_velocity_delta_time); } @@ -833,16 +836,16 @@ void GoalPlannerModuleManager::updateModuleParams( // IntegralPredictedPolygonParams const std::string integral_ns = safety_check_ns + "integral_predicted_polygon_params."; { - updateParam( + update_param( parameters, integral_ns + "forward_margin", p->safety_check_params.integral_predicted_polygon_params.forward_margin); - updateParam( + update_param( parameters, integral_ns + "backward_margin", p->safety_check_params.integral_predicted_polygon_params.backward_margin); - updateParam( + update_param( parameters, integral_ns + "lat_margin", p->safety_check_params.integral_predicted_polygon_params.lat_margin); - updateParam( + update_param( parameters, integral_ns + "time_horizon", p->safety_check_params.integral_predicted_polygon_params.time_horizon); } @@ -850,7 +853,7 @@ void GoalPlannerModuleManager::updateModuleParams( // debug { const std::string ns = base_ns + "debug."; - updateParam(parameters, ns + "print_debug_info", p->print_debug_info); + update_param(parameters, ns + "print_debug_info", p->print_debug_info); } std::for_each(observers_.begin(), observers_.end(), [&p](const auto & observer) { diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/bezier_pull_over.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/bezier_pull_over.cpp index 3347f2e4a6bdf..f29e409f60365 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/bezier_pull_over.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/bezier_pull_over.cpp @@ -134,7 +134,7 @@ std::vector BezierPullOver::generateBezierPath( // shift end pose is longitudinal offset from goal pose to improve parking angle accuracy const Pose shift_end_pose = - autoware::universe_utils::calcOffsetPose(goal_pose, -after_shift_straight_distance, 0, 0); + autoware_utils::calc_offset_pose(goal_pose, -after_shift_straight_distance, 0, 0); // calculate lateral shift of previous module path terminal pose from road lane reference path const auto road_lane_reference_path_to_shift_end = utils::resamplePathWithSpline( @@ -170,7 +170,7 @@ std::vector BezierPullOver::generateBezierPath( const Pose & shift_end_pose_prev_module_path = processed_prev_module_path->points.back().point.pose; const double shift_end_road_to_target_distance = - autoware::universe_utils::inverseTransformPoint( + autoware_utils::inverse_transform_point( shift_end_pose.position, shift_end_pose_prev_module_path) .y; @@ -260,7 +260,7 @@ std::vector BezierPullOver::generateBezierPath( p.point.pose.position.x = bezier_points[i].x(); p.point.pose.position.y = bezier_points[i].y(); p.point.pose.orientation = - universe_utils::createQuaternionFromRPY(0.0, 0.0, bezier_points[i].z()); + autoware_utils::create_quaternion_from_rpy(0.0, 0.0, bezier_points[i].z()); } // interpolate between shift end pose to goal pose diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/freespace_pull_over.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/freespace_pull_over.cpp index 6605d258a8d5e..e3f14c3f9643c 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/freespace_pull_over.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/freespace_pull_over.cpp @@ -68,7 +68,7 @@ std::optional FreespacePullOver::plan( const auto & goal_pose = modified_goal_pose.goal_pose; const Pose end_pose = use_back_ ? goal_pose - : autoware::universe_utils::calcOffsetPose(goal_pose, -straight_distance, 0.0, 0.0); + : autoware_utils::calc_offset_pose(goal_pose, -straight_distance, 0.0, 0.0); try { if (!planner_->makePlan(current_pose, end_pose)) { @@ -101,7 +101,7 @@ std::optional FreespacePullOver::plan( size_t index = std::distance(last_path.points.begin(), it); if (index == 0) continue; const double distance = - autoware::universe_utils::calcDistance2d(end_pose.position, it->point.pose.position); + autoware_utils::calc_distance2d(end_pose.position, it->point.pose.position); if (distance < th_goal_distance) { last_path.points.erase(it, last_path.points.end()); break; diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/shift_pull_over.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/shift_pull_over.cpp index b144cdbb12b39..cf695949fcf13 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/shift_pull_over.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/shift_pull_over.cpp @@ -122,7 +122,7 @@ std::optional ShiftPullOver::generatePullOverPath( // shift end pose is longitudinal offset from goal pose to improve parking angle accuracy const Pose shift_end_pose = - autoware::universe_utils::calcOffsetPose(goal_pose, -after_shift_straight_distance, 0, 0); + autoware_utils::calc_offset_pose(goal_pose, -after_shift_straight_distance, 0, 0); // calculate lateral shift of previous module path terminal pose from road lane reference path const auto road_lane_reference_path_to_shift_end = utils::resamplePathWithSpline( @@ -158,7 +158,7 @@ std::optional ShiftPullOver::generatePullOverPath( const Pose & shift_end_pose_prev_module_path = processed_prev_module_path->points.back().point.pose; const double shift_end_road_to_target_distance = - autoware::universe_utils::inverseTransformPoint( + autoware_utils::inverse_transform_point( shift_end_pose.position, shift_end_pose_prev_module_path) .y; diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/util.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/util.cpp index fb39a34c9ac91..52c775be3f3fa 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/util.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/util.cpp @@ -18,10 +18,10 @@ #include "autoware/behavior_path_planner_common/utils/utils.hpp" #include "autoware_lanelet2_extension/regulatory_elements/bus_stop_area.hpp" -#include #include #include #include +#include #include #include @@ -43,12 +43,12 @@ namespace autoware::behavior_path_planner::goal_planner_utils { -using autoware::universe_utils::calcDistance2d; -using autoware::universe_utils::calcOffsetPose; -using autoware::universe_utils::createDefaultMarker; -using autoware::universe_utils::createMarkerColor; -using autoware::universe_utils::createMarkerScale; -using autoware::universe_utils::createPoint; +using autoware_utils::calc_distance2d; +using autoware_utils::calc_offset_pose; +using autoware_utils::create_default_marker; +using autoware_utils::create_marker_color; +using autoware_utils::create_marker_scale; +using autoware_utils::create_point; lanelet::ConstLanelets getPullOverLanes( const RouteHandler & route_handler, const bool left_side, const double backward_distance, @@ -111,12 +111,12 @@ lanelet::ConstLanelets generateBetweenEgoAndExpandedPullOverLanes( const double ego_length_to_front = wheel_base + front_overhang; const double ego_width_to_front = !left_side ? (-wheel_tread / 2.0 - side_overhang) : (wheel_tread / 2.0 + side_overhang); - autoware::universe_utils::Point2d front_edge_local{ego_length_to_front, ego_width_to_front}; - const auto front_edge_glob = autoware::universe_utils::transformPoint( - front_edge_local, autoware::universe_utils::pose2transform(ego_pose)); + autoware_utils::Point2d front_edge_local{ego_length_to_front, ego_width_to_front}; + const auto front_edge_glob = + autoware_utils::transform_point(front_edge_local, autoware_utils::pose2transform(ego_pose)); geometry_msgs::msg::Pose ego_front_pose; ego_front_pose.position = - createPoint(front_edge_glob.x(), front_edge_glob.y(), ego_pose.position.z); + create_point(front_edge_glob.x(), front_edge_glob.y(), ego_pose.position.z); // ========================================================================================== // NOTE: the point which is on the right side of a directed line has negative distance @@ -142,9 +142,9 @@ std::optional generateObjectExtractionPolygon( const auto & bound = left_side ? lane.leftBound() : lane.rightBound(); for (const auto & p : bound) { Pose pose{}; - pose.position = createPoint(p.x(), p.y(), p.z()); + pose.position = create_point(p.x(), p.y(), p.z()); if (std::any_of(base_boundary_poses.begin(), base_boundary_poses.end(), [&](const auto & p) { - return calcDistance2d(p.position, pose.position) < 0.1; + return calc_distance2d(p.position, pose.position) < 0.1; })) { continue; } @@ -159,8 +159,8 @@ std::optional generateObjectExtractionPolygon( for (auto it = base_boundary_poses.begin(); it != std::prev(base_boundary_poses.end()); ++it) { const auto & p = it->position; const auto & next_p = std::next(it)->position; - const double yaw = autoware::universe_utils::calcAzimuthAngle(p, next_p); - it->orientation = autoware::universe_utils::createQuaternionFromYaw(yaw); + const double yaw = autoware_utils::calc_azimuth_angle(p, next_p); + it->orientation = autoware_utils::create_quaternion_from_yaw(yaw); } base_boundary_poses.back().orientation = base_boundary_poses[base_boundary_poses.size() - 2].orientation; @@ -170,8 +170,8 @@ std::optional generateObjectExtractionPolygon( std::vector inner_boundary_points{}; const double outer_direction_sign = left_side ? 1.0 : -1.0; for (const auto & base_pose : base_boundary_poses) { - const Pose outer_pose = calcOffsetPose(base_pose, 0, outer_direction_sign * outer_offset, 0); - const Pose inner_pose = calcOffsetPose(base_pose, 0, -outer_direction_sign * inner_offset, 0); + const Pose outer_pose = calc_offset_pose(base_pose, 0, outer_direction_sign * outer_offset, 0); + const Pose inner_pose = calc_offset_pose(base_pose, 0, -outer_direction_sign * inner_offset, 0); outer_boundary_points.push_back(outer_pose.position); inner_boundary_points.push_back(inner_pose.position); } @@ -192,7 +192,7 @@ std::optional generateObjectExtractionPolygon( p_line.push_back(p2); bool intersection_found = false; for (size_t j = i + 2; j < bound.size() - 1; j++) { - const double distance = autoware::universe_utils::calcDistance2d(bound.at(i), bound.at(j)); + const double distance = autoware_utils::calc_distance2d(bound.at(i), bound.at(j)); if (distance > INTERSECTION_CHECK_DISTANCE) { break; } @@ -234,14 +234,14 @@ std::optional generateObjectExtractionPolygon( std::vector reversed_right_boundary_points = right_boundary_points; std::reverse(reversed_right_boundary_points.begin(), reversed_right_boundary_points.end()); for (const auto & left_point : left_boundary_points) { - autoware::universe_utils::Point2d point{left_point.x, left_point.y}; + autoware_utils::Point2d point{left_point.x, left_point.y}; polygon.outer().push_back(point); } for (const auto & right_point : reversed_right_boundary_points) { - autoware::universe_utils::Point2d point{right_point.x, right_point.y}; + autoware_utils::Point2d point{right_point.x, right_point.y}; polygon.outer().push_back(point); } - autoware::universe_utils::Point2d first_point{ + autoware_utils::Point2d first_point{ left_boundary_points.front().x, left_boundary_points.front().y}; polygon.outer().push_back(first_point); @@ -338,7 +338,7 @@ bool checkObjectsCollision( std::vector obj_polygons; for (const auto & object : target_objects.objects) { - obj_polygons.push_back(autoware::universe_utils::toPolygon2d(object)); + obj_polygons.push_back(autoware_utils::to_polygon2d(object)); } /* Expand ego collision check polygon @@ -360,7 +360,7 @@ bool checkObjectsCollision( extra_stopping_margin, std::abs(curvatures.at(i) * std::pow(p.point.longitudinal_velocity_mps, 2))); - const auto ego_polygon = autoware::universe_utils::toFootprint( + const auto ego_polygon = autoware_utils::to_footprint( p.point.pose, behavior_path_parameters.base_link2front + collision_check_margin + extra_stopping_margin, behavior_path_parameters.base_link2rear + collision_check_margin, @@ -377,17 +377,17 @@ bool checkObjectsCollision( } MarkerArray createPullOverAreaMarkerArray( - const autoware::universe_utils::MultiPolygon2d area_polygons, - const std_msgs::msg::Header & header, const std_msgs::msg::ColorRGBA & color, const double z) + const autoware_utils::MultiPolygon2d area_polygons, const std_msgs::msg::Header & header, + const std_msgs::msg::ColorRGBA & color, const double z) { MarkerArray marker_array{}; for (size_t i = 0; i < area_polygons.size(); ++i) { - Marker marker = createDefaultMarker( + Marker marker = create_default_marker( header.frame_id, header.stamp, "pull_over_area_" + std::to_string(i), i, - visualization_msgs::msg::Marker::LINE_STRIP, createMarkerScale(0.1, 0.0, 0.0), color); + visualization_msgs::msg::Marker::LINE_STRIP, create_marker_scale(0.1, 0.0, 0.0), color); const auto & poly = area_polygons.at(i); for (const auto & p : poly.outer()) { - marker.points.push_back(createPoint(p.x(), p.y(), z)); + marker.points.push_back(create_point(p.x(), p.y(), z)); } marker_array.markers.push_back(marker); @@ -402,9 +402,9 @@ MarkerArray createPosesMarkerArray( MarkerArray msg{}; int32_t i = 0; for (const auto & pose : poses) { - Marker marker = autoware::universe_utils::createDefaultMarker( + Marker marker = autoware_utils::create_default_marker( "map", rclcpp::Clock{RCL_ROS_TIME}.now(), ns, i, Marker::ARROW, - createMarkerScale(0.5, 0.25, 0.25), color); + create_marker_scale(0.5, 0.25, 0.25), color); marker.pose = pose; marker.id = i++; msg.markers.push_back(marker); @@ -419,10 +419,10 @@ MarkerArray createGoalPriorityTextsMarkerArray( MarkerArray msg{}; int32_t i = 0; for (const auto & pose : poses) { - Marker marker = createDefaultMarker( + Marker marker = create_default_marker( "map", rclcpp::Clock{RCL_ROS_TIME}.now(), ns, i, Marker::TEXT_VIEW_FACING, - createMarkerScale(0.3, 0.3, 0.3), color); - marker.pose = calcOffsetPose(pose, 0, 0, 1.0); + create_marker_scale(0.3, 0.3, 0.3), color); + marker.pose = calc_offset_pose(pose, 0, 0, 1.0); marker.id = i; marker.text = std::to_string(i); msg.markers.push_back(marker); @@ -439,10 +439,10 @@ MarkerArray createNumObjectsToAvoidTextsMarkerArray( int32_t i = 0; for (const auto & goal_candidate : goal_candidates) { const Pose & pose = goal_candidate.goal_pose; - Marker marker = createDefaultMarker( + Marker marker = create_default_marker( "map", rclcpp::Clock{RCL_ROS_TIME}.now(), ns, i, Marker::TEXT_VIEW_FACING, - createMarkerScale(0.3, 0.3, 0.3), color); - marker.pose = calcOffsetPose(pose, -0.5, 0, 1.0); + create_marker_scale(0.3, 0.3, 0.3), color); + marker.pose = calc_offset_pose(pose, -0.5, 0, 1.0); marker.id = i; marker.text = std::to_string(goal_candidate.num_objects_to_avoid); msg.markers.push_back(marker); @@ -468,13 +468,13 @@ MarkerArray createGoalCandidatesMarkerArray( auto marker_array = createPosesMarkerArray(pose_vector, "goal_candidates", color); for (const auto & text_marker : createGoalPriorityTextsMarkerArray( - pose_vector, "goal_candidates_priority", createMarkerColor(1.0, 1.0, 1.0, 0.999)) + pose_vector, "goal_candidates_priority", create_marker_color(1.0, 1.0, 1.0, 0.999)) .markers) { marker_array.markers.push_back(text_marker); } for (const auto & text_marker : createNumObjectsToAvoidTextsMarkerArray( safe_goal_candidates, "goal_candidates_num_objects_to_avoid", - createMarkerColor(0.5, 0.5, 0.5, 0.999)) + create_marker_color(0.5, 0.5, 0.5, 0.999)) .markers) { marker_array.markers.push_back(text_marker); } @@ -487,11 +487,11 @@ MarkerArray createLaneletPolygonMarkerArray( const std::string & ns, const std_msgs::msg::ColorRGBA & color) { visualization_msgs::msg::MarkerArray marker_array{}; - auto marker = createDefaultMarker( + auto marker = create_default_marker( header.frame_id, header.stamp, ns, 0, visualization_msgs::msg::Marker::LINE_STRIP, - createMarkerScale(0.1, 0.0, 0.0), color); + create_marker_scale(0.1, 0.0, 0.0), color); for (const auto & p : polygon) { - marker.points.push_back(createPoint(p.x(), p.y(), p.z())); + marker.points.push_back(create_point(p.x(), p.y(), p.z())); } marker_array.markers.push_back(marker); return marker_array; @@ -506,7 +506,7 @@ double calcLateralDeviationBetweenPaths( reference_path.points, target_point.point.pose.position); lateral_deviation = std::max( lateral_deviation, - std::abs(autoware::universe_utils::calcLateralDeviation( + std::abs(autoware_utils::calc_lateral_deviation( reference_path.points[nearest_index].point.pose, target_point.point.pose.position))); } return lateral_deviation; @@ -535,7 +535,7 @@ std::optional cropPath(const PathWithLaneId & path, const Pose & const double offset = autoware::motion_utils::calcSignedArcLength(path.points, end_idx, end_pose.position); projected_point.point.pose = - autoware::universe_utils::calcOffsetPose(clipped_points.back().point.pose, offset, 0, 0); + autoware_utils::calc_offset_pose(clipped_points.back().point.pose, offset, 0, 0); clipped_points.push_back(projected_point); auto clipped_path = path; clipped_path.points = clipped_points; @@ -550,8 +550,7 @@ PathWithLaneId cropForwardPoints( double sum_length = 0; for (size_t i = target_seg_idx + 1; i < points.size(); ++i) { - const double seg_length = - autoware::universe_utils::calcDistance2d(points.at(i), points.at(i - 1)); + const double seg_length = autoware_utils::calc_distance2d(points.at(i), points.at(i - 1)); if (forward_length < sum_length + seg_length) { const auto cropped_points = std::vector{points.begin() + target_seg_idx, points.begin() + i}; @@ -561,7 +560,7 @@ PathWithLaneId cropForwardPoints( // add precise end pose to cropped points const double remaining_length = forward_length - sum_length; const Pose precise_end_pose = - calcOffsetPose(cropped_path.points.back().point.pose, remaining_length, 0, 0); + calc_offset_pose(cropped_path.points.back().point.pose, remaining_length, 0, 0); if (remaining_length < 0.1) { // if precise_end_pose is too close, replace the last point cropped_path.points.back().point.pose = precise_end_pose; @@ -600,8 +599,8 @@ PathWithLaneId extendPath( const double lateral_shift_from_reference_path = autoware::motion_utils::calcLateralOffset(reference_path.points, target_terminal_pose.position); for (auto & p : clipped_path.points) { - p.point.pose = autoware::universe_utils::calcOffsetPose( - p.point.pose, 0, lateral_shift_from_reference_path, 0); + p.point.pose = + autoware_utils::calc_offset_pose(p.point.pose, 0, lateral_shift_from_reference_path, 0); } auto extended_path = target_path; @@ -613,10 +612,10 @@ PathWithLaneId extendPath( const auto start_point = std::find_if(clipped_path.points.begin(), clipped_path.points.end(), [&](const auto & p) { const bool is_forward = - autoware::universe_utils::inverseTransformPoint(p.point.pose.position, target_terminal_pose) - .x > 0.0; - const bool is_close = autoware::universe_utils::calcDistance2d( - p.point.pose.position, target_terminal_pose.position) < 0.1; + autoware_utils::inverse_transform_point(p.point.pose.position, target_terminal_pose).x > + 0.0; + const bool is_close = + autoware_utils::calc_distance2d(p.point.pose.position, target_terminal_pose.position) < 0.1; return is_forward && !is_close; }); std::copy(start_point, clipped_path.points.end(), std::back_inserter(extended_path.points)); @@ -646,8 +645,7 @@ std::vector createPathFootPrints( std::vector footprints; for (const auto & point : path.points) { const auto & pose = point.point.pose; - footprints.push_back( - autoware::universe_utils::toFootprint(pose, base_to_front, base_to_rear, width)); + footprints.push_back(autoware_utils::to_footprint(pose, base_to_front, base_to_rear, width)); } return footprints; } @@ -797,7 +795,7 @@ std::optional calcRefinedGoal( const double offset_from_center_line = sign * (distance_from_bound.value() + parameters.margin_from_boundary); - const auto refined_goal_pose = calcOffsetPose(center_pose, 0, -offset_from_center_line, 0); + const auto refined_goal_pose = calc_offset_pose(center_pose, 0, -offset_from_center_line, 0); return refined_goal_pose; } @@ -846,7 +844,7 @@ autoware_perception_msgs::msg::PredictedObjects extract_dynamic_objects( PredictedObjects dynamic_target_objects{}; for (const auto & object : original_objects.objects) { - const auto object_polygon = universe_utils::toPolygon2d(object); + const auto object_polygon = autoware_utils::to_polygon2d(object); if ( objects_extraction_polygon.has_value() && boost::geometry::intersects(object_polygon, objects_extraction_polygon.value())) { diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/test/test_util.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/test/test_util.cpp index 472adb2d57944..e970e4f25af86 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/test/test_util.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/test/test_util.cpp @@ -110,8 +110,8 @@ TEST_F(DISABLED_TestUtilWithMap, isWithinAreas) .orientation( geometry_msgs::build().x(0.0).y(0.0).z(0.707390).w( 0.706824)); - const auto baselink_footprint = autoware::universe_utils::transformVector( - footprint, autoware::universe_utils::pose2transform(baselink_pose)); + const auto baselink_footprint = + autoware_utils::transform_vector(footprint, autoware_utils::pose2transform(baselink_pose)); EXPECT_EQ( autoware::behavior_path_planner::goal_planner_utils::isWithinAreas( baselink_footprint, bus_stop_area_polygons), diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/base_class.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/base_class.hpp index e9df7f0a821b6..8c51e6a5b31de 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/base_class.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/base_class.hpp @@ -22,9 +22,9 @@ #include "autoware/behavior_path_planner_common/interface/scene_module_interface.hpp" #include "autoware/behavior_path_planner_common/turn_signal_decider.hpp" #include "autoware/behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" -#include "autoware/universe_utils/system/stop_watch.hpp" +#include "autoware_utils/system/stop_watch.hpp" -#include +#include #include #include @@ -41,8 +41,8 @@ namespace autoware::behavior_path_planner { using autoware::behavior_path_planner::PoseWithDetailOpt; using autoware::route_handler::Direction; -using autoware::universe_utils::StopWatch; using autoware_internal_planning_msgs::msg::PathWithLaneId; +using autoware_utils::StopWatch; using geometry_msgs::msg::Point; using geometry_msgs::msg::Pose; using geometry_msgs::msg::Twist; @@ -58,7 +58,7 @@ class LaneChangeBase common_data_ptr_{std::make_shared()}, direction_{direction}, type_{type}, - time_keeper_(std::make_shared()) + time_keeper_(std::make_shared()) { } @@ -191,7 +191,7 @@ class LaneChangeBase common_data_ptr_->direction = direction_; } - void setTimeKeeper(const std::shared_ptr & time_keeper) + void setTimeKeeper(const std::shared_ptr & time_keeper) { time_keeper_ = time_keeper; } @@ -302,7 +302,7 @@ class LaneChangeBase rclcpp::Logger logger_ = utils::lane_change::getLogger(getModuleTypeStr()); mutable rclcpp::Clock clock_{RCL_ROS_TIME}; - mutable std::shared_ptr time_keeper_; + mutable std::shared_ptr time_keeper_; friend class ::TestNormalLaneChange; }; diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/interface.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/interface.hpp index b1b8f29b9c321..fb067b86cf2ab 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/interface.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/interface.hpp @@ -23,7 +23,7 @@ #include "autoware/behavior_path_planner_common/turn_signal_decider.hpp" #include "autoware/behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" -#include +#include #include #include @@ -113,7 +113,7 @@ class LaneChangeInterface : public SceneModuleInterface const double start_distance, const double finish_distance, const bool safe, const uint8_t & state) { - universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + autoware_utils::ScopedTimeTrack st(__func__, *time_keeper_); for (const auto & [module_name, ptr] : rtc_interface_ptr_map_) { if (ptr) { ptr->updateCooperateStatus( diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/structs/data.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/structs/data.hpp index c844b6b218d10..612f6df707da7 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/structs/data.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/structs/data.hpp @@ -21,7 +21,7 @@ #include #include #include -#include +#include #include @@ -35,12 +35,12 @@ namespace autoware::behavior_path_planner::lane_change { +using autoware_utils::Polygon2d; using geometry_msgs::msg::Pose; using geometry_msgs::msg::Twist; using nav_msgs::msg::Odometry; using route_handler::Direction; using route_handler::RouteHandler; -using universe_utils::Polygon2d; using utils::path_safety_checker::ExtendedPredictedObjects; enum class States { diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/structs/parameters.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/structs/parameters.hpp index 358f60f3193d5..a2c95d83ee31a 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/structs/parameters.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/structs/parameters.hpp @@ -18,7 +18,7 @@ #include "autoware/behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" #include -#include +#include #include #include @@ -189,7 +189,7 @@ struct Parameters // finish judge parameter double lane_change_finish_judge_buffer{3.0}; double th_finish_judge_lateral_diff{0.2}; - double th_finish_judge_yaw_diff{autoware::universe_utils::deg2rad(3.0)}; + double th_finish_judge_yaw_diff{autoware_utils::deg2rad(3.0)}; // debug marker bool publish_debug_marker{false}; diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/utils.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/utils.hpp index 394c91172a979..c4f5b9dd2b8bd 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/utils.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/utils.hpp @@ -23,9 +23,9 @@ #include "rclcpp/logger.hpp" #include -#include #include #include +#include #include #include @@ -46,13 +46,13 @@ using autoware::behavior_path_planner::utils::path_safety_checker:: using autoware::behavior_path_planner::utils::path_safety_checker::PoseWithVelocityStamped; using autoware::behavior_path_planner::utils::path_safety_checker::PredictedPathWithPolygon; using autoware::route_handler::Direction; -using autoware::universe_utils::LineString2d; -using autoware::universe_utils::Polygon2d; using autoware::vehicle_info_utils::VehicleInfo; using autoware_internal_planning_msgs::msg::PathWithLaneId; using autoware_perception_msgs::msg::PredictedObject; using autoware_perception_msgs::msg::PredictedObjects; using autoware_perception_msgs::msg::PredictedPath; +using autoware_utils::LineString2d; +using autoware_utils::Polygon2d; using behavior_path_planner::lane_change::CommonDataPtr; using behavior_path_planner::lane_change::LanesPolygon; using behavior_path_planner::lane_change::LCParamPtr; diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/package.xml b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/package.xml index aab5f4c01e8ee..6488a3743546e 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/package.xml +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/package.xml @@ -26,7 +26,7 @@ autoware_frenet_planner autoware_motion_utils autoware_rtc_interface - autoware_universe_utils + autoware_utils fmt pluginlib range-v3 diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/interface.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/interface.cpp index 4519b20d4644d..12853b165f433 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/interface.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/interface.cpp @@ -20,8 +20,8 @@ #include "autoware/behavior_path_planner_common/interface/scene_module_visitor.hpp" #include "autoware/behavior_path_planner_common/marker_utils/utils.hpp" -#include -#include +#include +#include #include #include @@ -32,7 +32,7 @@ namespace autoware::behavior_path_planner { -using autoware::universe_utils::appendMarkerArray; +using autoware_utils::append_marker_array; using utils::lane_change::assignToCandidate; LaneChangeInterface::LaneChangeInterface( @@ -74,7 +74,7 @@ bool LaneChangeInterface::isExecutionReady() const void LaneChangeInterface::updateData() { - universe_utils::ScopedTimeTrack st(__func__, *getTimeKeeper()); + autoware_utils::ScopedTimeTrack st(__func__, *getTimeKeeper()); module_type_->setPreviousModuleOutput(getPreviousModuleOutput()); module_type_->update_lanes(getCurrentStatus() == ModuleStatus::RUNNING); module_type_->update_filtered_objects(); @@ -101,7 +101,7 @@ void LaneChangeInterface::postProcess() BehaviorModuleOutput LaneChangeInterface::plan() { - universe_utils::ScopedTimeTrack st(__func__, *getTimeKeeper()); + autoware_utils::ScopedTimeTrack st(__func__, *getTimeKeeper()); resetPathCandidate(); resetPathReference(); @@ -185,7 +185,7 @@ BehaviorModuleOutput LaneChangeInterface::planWaitingApproval() CandidateOutput LaneChangeInterface::planCandidate() const { - universe_utils::ScopedTimeTrack st(__func__, *getTimeKeeper()); + autoware_utils::ScopedTimeTrack st(__func__, *getTimeKeeper()); const auto selected_path = module_type_->getLaneChangePath(); if (selected_path.path.points.empty()) { @@ -375,14 +375,14 @@ MarkerArray LaneChangeInterface::getModuleVirtualWall() const auto end_marker = createLaneChangingVirtualWallMarker(end_pose, name(), clock_->now(), "lane_change_end"); marker.markers.reserve(start_marker.markers.size() + end_marker.markers.size()); - appendMarkerArray(start_marker, &marker); - appendMarkerArray(end_marker, &marker); + append_marker_array(start_marker, &marker); + append_marker_array(end_marker, &marker); return marker; } void LaneChangeInterface::updateSteeringFactorPtr(const BehaviorModuleOutput & output) { - universe_utils::ScopedTimeTrack st(__func__, *getTimeKeeper()); + autoware_utils::ScopedTimeTrack st(__func__, *getTimeKeeper()); const auto current_position = module_type_->getEgoPosition(); const auto status = module_type_->getLaneChangeStatus(); diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/manager.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/manager.cpp index 589338d7b3ace..fd784ab7e644f 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/manager.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/manager.cpp @@ -15,8 +15,8 @@ #include "autoware/behavior_path_lane_change_module/manager.hpp" #include "autoware/behavior_path_lane_change_module/interface.hpp" -#include "autoware/universe_utils/ros/parameter.hpp" -#include "autoware/universe_utils/ros/update_param.hpp" +#include "autoware_utils/ros/parameter.hpp" +#include "autoware_utils/ros/update_param.hpp" #include @@ -37,7 +37,7 @@ void LaneChangeModuleManager::init(rclcpp::Node * node) LCParamPtr LaneChangeModuleManager::set_params(rclcpp::Node * node, const std::string & node_name) { - using autoware::universe_utils::getOrDeclareParameter; + using autoware_utils::get_or_declare_parameter; LaneChangeParameters p{}; @@ -46,31 +46,31 @@ LCParamPtr LaneChangeModuleManager::set_params(rclcpp::Node * node, const std::s // trajectory generation { p.trajectory.max_prepare_duration = - getOrDeclareParameter(*node, parameter("trajectory.max_prepare_duration")); + get_or_declare_parameter(*node, parameter("trajectory.max_prepare_duration")); p.trajectory.min_prepare_duration = - getOrDeclareParameter(*node, parameter("trajectory.min_prepare_duration")); + get_or_declare_parameter(*node, parameter("trajectory.min_prepare_duration")); p.trajectory.lateral_jerk = - getOrDeclareParameter(*node, parameter("trajectory.lateral_jerk")); + get_or_declare_parameter(*node, parameter("trajectory.lateral_jerk")); p.trajectory.min_longitudinal_acc = - getOrDeclareParameter(*node, parameter("trajectory.min_longitudinal_acc")); + get_or_declare_parameter(*node, parameter("trajectory.min_longitudinal_acc")); p.trajectory.max_longitudinal_acc = - getOrDeclareParameter(*node, parameter("trajectory.max_longitudinal_acc")); + get_or_declare_parameter(*node, parameter("trajectory.max_longitudinal_acc")); p.trajectory.th_prepare_length_diff = - getOrDeclareParameter(*node, parameter("trajectory.th_prepare_length_diff")); + get_or_declare_parameter(*node, parameter("trajectory.th_prepare_length_diff")); p.trajectory.th_lane_changing_length_diff = - getOrDeclareParameter(*node, parameter("trajectory.th_lane_changing_length_diff")); + get_or_declare_parameter(*node, parameter("trajectory.th_lane_changing_length_diff")); p.trajectory.min_lane_changing_velocity = - getOrDeclareParameter(*node, parameter("trajectory.min_lane_changing_velocity")); + get_or_declare_parameter(*node, parameter("trajectory.min_lane_changing_velocity")); p.trajectory.lane_changing_decel_factor = - getOrDeclareParameter(*node, parameter("trajectory.lane_changing_decel_factor")); + get_or_declare_parameter(*node, parameter("trajectory.lane_changing_decel_factor")); p.trajectory.th_prepare_curvature = - getOrDeclareParameter(*node, parameter("trajectory.th_prepare_curvature")); + get_or_declare_parameter(*node, parameter("trajectory.th_prepare_curvature")); p.trajectory.lon_acc_sampling_num = - getOrDeclareParameter(*node, parameter("trajectory.lon_acc_sampling_num")); + get_or_declare_parameter(*node, parameter("trajectory.lon_acc_sampling_num")); p.trajectory.lat_acc_sampling_num = - getOrDeclareParameter(*node, parameter("trajectory.lat_acc_sampling_num")); + get_or_declare_parameter(*node, parameter("trajectory.lat_acc_sampling_num")); - const auto max_acc = getOrDeclareParameter(*node, "normal.max_acc"); + const auto max_acc = get_or_declare_parameter(*node, "normal.max_acc"); p.trajectory.min_lane_changing_velocity = std::min( p.trajectory.min_lane_changing_velocity, max_acc * p.trajectory.max_prepare_duration); @@ -86,11 +86,11 @@ LCParamPtr LaneChangeModuleManager::set_params(rclcpp::Node * node, const std::s } // lateral acceleration map - const auto lateral_acc_velocity = - getOrDeclareParameter>(*node, parameter("lateral_acceleration.velocity")); - const auto min_lateral_acc = getOrDeclareParameter>( + const auto lateral_acc_velocity = get_or_declare_parameter>( + *node, parameter("lateral_acceleration.velocity")); + const auto min_lateral_acc = get_or_declare_parameter>( *node, parameter("lateral_acceleration.min_values")); - const auto max_lateral_acc = getOrDeclareParameter>( + const auto max_lateral_acc = get_or_declare_parameter>( *node, parameter("lateral_acceleration.max_values")); if ( lateral_acc_velocity.size() != min_lateral_acc.size() || @@ -108,71 +108,74 @@ LCParamPtr LaneChangeModuleManager::set_params(rclcpp::Node * node, const std::s // turn signal p.min_length_for_turn_signal_activation = - getOrDeclareParameter(*node, parameter("min_length_for_turn_signal_activation")); + get_or_declare_parameter(*node, parameter("min_length_for_turn_signal_activation")); // lane change regulations - p.regulate_on_crosswalk = getOrDeclareParameter(*node, parameter("regulation.crosswalk")); + p.regulate_on_crosswalk = + get_or_declare_parameter(*node, parameter("regulation.crosswalk")); p.regulate_on_intersection = - getOrDeclareParameter(*node, parameter("regulation.intersection")); + get_or_declare_parameter(*node, parameter("regulation.intersection")); p.regulate_on_traffic_light = - getOrDeclareParameter(*node, parameter("regulation.traffic_light")); + get_or_declare_parameter(*node, parameter("regulation.traffic_light")); // ego vehicle stuck detection - p.th_stop_velocity = getOrDeclareParameter(*node, parameter("stuck_detection.velocity")); - p.th_stop_time = getOrDeclareParameter(*node, parameter("stuck_detection.stop_time")); + p.th_stop_velocity = + get_or_declare_parameter(*node, parameter("stuck_detection.velocity")); + p.th_stop_time = get_or_declare_parameter(*node, parameter("stuck_detection.stop_time")); // safety { p.safety.enable_loose_check_for_cancel = - getOrDeclareParameter(*node, parameter("safety_check.allow_loose_check_for_cancel")); - p.safety.enable_target_lane_bound_check = - getOrDeclareParameter(*node, parameter("safety_check.enable_target_lane_bound_check")); - p.safety.th_stopped_object_velocity = getOrDeclareParameter( + get_or_declare_parameter(*node, parameter("safety_check.allow_loose_check_for_cancel")); + p.safety.enable_target_lane_bound_check = get_or_declare_parameter( + *node, parameter("safety_check.enable_target_lane_bound_check")); + p.safety.th_stopped_object_velocity = get_or_declare_parameter( *node, parameter("safety_check.stopped_object_velocity_threshold")); p.safety.lane_expansion_left_offset = - getOrDeclareParameter(*node, parameter("safety_check.lane_expansion.left_offset")); - p.safety.lane_expansion_right_offset = - getOrDeclareParameter(*node, parameter("safety_check.lane_expansion.right_offset")); + get_or_declare_parameter(*node, parameter("safety_check.lane_expansion.left_offset")); + p.safety.lane_expansion_right_offset = get_or_declare_parameter( + *node, parameter("safety_check.lane_expansion.right_offset")); // collision check p.safety.collision_check.enable_for_prepare_phase_in_general_lanes = - getOrDeclareParameter( + get_or_declare_parameter( *node, parameter("collision_check.enable_for_prepare_phase.general_lanes")); - p.safety.collision_check.enable_for_prepare_phase_in_intersection = getOrDeclareParameter( - *node, parameter("collision_check.enable_for_prepare_phase.intersection")); - p.safety.collision_check.enable_for_prepare_phase_in_turns = getOrDeclareParameter( + p.safety.collision_check.enable_for_prepare_phase_in_intersection = + get_or_declare_parameter( + *node, parameter("collision_check.enable_for_prepare_phase.intersection")); + p.safety.collision_check.enable_for_prepare_phase_in_turns = get_or_declare_parameter( *node, parameter("collision_check.enable_for_prepare_phase.turns")); p.safety.collision_check.check_current_lane = - getOrDeclareParameter(*node, parameter("collision_check.check_current_lanes")); + get_or_declare_parameter(*node, parameter("collision_check.check_current_lanes")); p.safety.collision_check.check_other_lanes = - getOrDeclareParameter(*node, parameter("collision_check.check_other_lanes")); + get_or_declare_parameter(*node, parameter("collision_check.check_other_lanes")); p.safety.collision_check.use_all_predicted_paths = - getOrDeclareParameter(*node, parameter("collision_check.use_all_predicted_paths")); - p.safety.collision_check.prediction_time_resolution = - getOrDeclareParameter(*node, parameter("collision_check.prediction_time_resolution")); + get_or_declare_parameter(*node, parameter("collision_check.use_all_predicted_paths")); + p.safety.collision_check.prediction_time_resolution = get_or_declare_parameter( + *node, parameter("collision_check.prediction_time_resolution")); p.safety.collision_check.th_yaw_diff = - getOrDeclareParameter(*node, parameter("collision_check.yaw_diff_threshold")); + get_or_declare_parameter(*node, parameter("collision_check.yaw_diff_threshold")); p.safety.collision_check.th_incoming_object_yaw = - getOrDeclareParameter(*node, parameter("collision_check.th_incoming_object_yaw")); + get_or_declare_parameter(*node, parameter("collision_check.th_incoming_object_yaw")); // rss check auto set_rss_params = [&](auto & params, const std::string & prefix) { - params.longitudinal_distance_min_threshold = getOrDeclareParameter( + params.longitudinal_distance_min_threshold = get_or_declare_parameter( *node, parameter(prefix + ".longitudinal_distance_min_threshold")); - params.longitudinal_velocity_delta_time = getOrDeclareParameter( + params.longitudinal_velocity_delta_time = get_or_declare_parameter( *node, parameter(prefix + ".longitudinal_velocity_delta_time")); params.front_vehicle_deceleration = - getOrDeclareParameter(*node, parameter(prefix + ".expected_front_deceleration")); + get_or_declare_parameter(*node, parameter(prefix + ".expected_front_deceleration")); params.rear_vehicle_deceleration = - getOrDeclareParameter(*node, parameter(prefix + ".expected_rear_deceleration")); + get_or_declare_parameter(*node, parameter(prefix + ".expected_rear_deceleration")); params.rear_vehicle_reaction_time = - getOrDeclareParameter(*node, parameter(prefix + ".rear_vehicle_reaction_time")); - params.rear_vehicle_safety_time_margin = getOrDeclareParameter( + get_or_declare_parameter(*node, parameter(prefix + ".rear_vehicle_reaction_time")); + params.rear_vehicle_safety_time_margin = get_or_declare_parameter( *node, parameter(prefix + ".rear_vehicle_safety_time_margin")); - params.lateral_distance_max_threshold = - getOrDeclareParameter(*node, parameter(prefix + ".lateral_distance_max_threshold")); - params.extended_polygon_policy = - getOrDeclareParameter(*node, parameter(prefix + ".extended_polygon_policy")); + params.lateral_distance_max_threshold = get_or_declare_parameter( + *node, parameter(prefix + ".lateral_distance_max_threshold")); + params.extended_polygon_policy = get_or_declare_parameter( + *node, parameter(prefix + ".extended_polygon_policy")); }; set_rss_params(p.safety.rss_params, "safety_check.execution"); set_rss_params(p.safety.rss_params_for_parked, "safety_check.parked"); @@ -181,29 +184,33 @@ LCParamPtr LaneChangeModuleManager::set_params(rclcpp::Node * node, const std::s // target object types const std::string ns = "lane_change.target_object."; - p.safety.target_object_types.check_car = getOrDeclareParameter(*node, ns + "car"); - p.safety.target_object_types.check_truck = getOrDeclareParameter(*node, ns + "truck"); - p.safety.target_object_types.check_bus = getOrDeclareParameter(*node, ns + "bus"); - p.safety.target_object_types.check_trailer = getOrDeclareParameter(*node, ns + "trailer"); - p.safety.target_object_types.check_unknown = getOrDeclareParameter(*node, ns + "unknown"); - p.safety.target_object_types.check_bicycle = getOrDeclareParameter(*node, ns + "bicycle"); + p.safety.target_object_types.check_car = get_or_declare_parameter(*node, ns + "car"); + p.safety.target_object_types.check_truck = get_or_declare_parameter(*node, ns + "truck"); + p.safety.target_object_types.check_bus = get_or_declare_parameter(*node, ns + "bus"); + p.safety.target_object_types.check_trailer = + get_or_declare_parameter(*node, ns + "trailer"); + p.safety.target_object_types.check_unknown = + get_or_declare_parameter(*node, ns + "unknown"); + p.safety.target_object_types.check_bicycle = + get_or_declare_parameter(*node, ns + "bicycle"); p.safety.target_object_types.check_motorcycle = - getOrDeclareParameter(*node, ns + "motorcycle"); + get_or_declare_parameter(*node, ns + "motorcycle"); p.safety.target_object_types.check_pedestrian = - getOrDeclareParameter(*node, ns + "pedestrian"); + get_or_declare_parameter(*node, ns + "pedestrian"); } // lane change parameters - p.time_limit = getOrDeclareParameter(*node, parameter("time_limit")); - p.backward_lane_length = getOrDeclareParameter(*node, parameter("backward_lane_length")); + p.time_limit = get_or_declare_parameter(*node, parameter("time_limit")); + p.backward_lane_length = + get_or_declare_parameter(*node, parameter("backward_lane_length")); p.backward_length_buffer_for_end_of_lane = - getOrDeclareParameter(*node, parameter("backward_length_buffer_for_end_of_lane")); - p.backward_length_buffer_for_blocking_object = - getOrDeclareParameter(*node, parameter("backward_length_buffer_for_blocking_object")); + get_or_declare_parameter(*node, parameter("backward_length_buffer_for_end_of_lane")); + p.backward_length_buffer_for_blocking_object = get_or_declare_parameter( + *node, parameter("backward_length_buffer_for_blocking_object")); p.backward_length_from_intersection = - getOrDeclareParameter(*node, parameter("backward_length_from_intersection")); + get_or_declare_parameter(*node, parameter("backward_length_from_intersection")); p.enable_stopped_vehicle_buffer = - getOrDeclareParameter(*node, parameter("enable_stopped_vehicle_buffer")); + get_or_declare_parameter(*node, parameter("enable_stopped_vehicle_buffer")); if (p.backward_length_buffer_for_end_of_lane < 1.0) { RCLCPP_WARN_STREAM( @@ -212,55 +219,55 @@ LCParamPtr LaneChangeModuleManager::set_params(rclcpp::Node * node, const std::s } // lane change delay - p.delay.enable = getOrDeclareParameter(*node, parameter("delay_lane_change.enable")); + p.delay.enable = get_or_declare_parameter(*node, parameter("delay_lane_change.enable")); p.delay.check_only_parked_vehicle = - getOrDeclareParameter(*node, parameter("delay_lane_change.check_only_parked_vehicle")); + get_or_declare_parameter(*node, parameter("delay_lane_change.check_only_parked_vehicle")); p.delay.min_road_shoulder_width = - getOrDeclareParameter(*node, parameter("delay_lane_change.min_road_shoulder_width")); - p.delay.th_parked_vehicle_shift_ratio = getOrDeclareParameter( + get_or_declare_parameter(*node, parameter("delay_lane_change.min_road_shoulder_width")); + p.delay.th_parked_vehicle_shift_ratio = get_or_declare_parameter( *node, parameter("delay_lane_change.th_parked_vehicle_shift_ratio")); // trajectory generation near terminal using frenet planner - p.frenet.enable = getOrDeclareParameter(*node, parameter("frenet.enable")); - p.frenet.th_yaw_diff_deg = getOrDeclareParameter(*node, parameter("frenet.th_yaw_diff")); + p.frenet.enable = get_or_declare_parameter(*node, parameter("frenet.enable")); + p.frenet.th_yaw_diff_deg = + get_or_declare_parameter(*node, parameter("frenet.th_yaw_diff")); p.frenet.th_curvature_smoothing = - getOrDeclareParameter(*node, parameter("frenet.th_curvature_smoothing")); + get_or_declare_parameter(*node, parameter("frenet.th_curvature_smoothing")); // lane change cancel p.cancel.enable_on_prepare_phase = - getOrDeclareParameter(*node, parameter("cancel.enable_on_prepare_phase")); + get_or_declare_parameter(*node, parameter("cancel.enable_on_prepare_phase")); p.cancel.enable_on_lane_changing_phase = - getOrDeclareParameter(*node, parameter("cancel.enable_on_lane_changing_phase")); - p.cancel.delta_time = getOrDeclareParameter(*node, parameter("cancel.delta_time")); - p.cancel.duration = getOrDeclareParameter(*node, parameter("cancel.duration")); + get_or_declare_parameter(*node, parameter("cancel.enable_on_lane_changing_phase")); + p.cancel.delta_time = get_or_declare_parameter(*node, parameter("cancel.delta_time")); + p.cancel.duration = get_or_declare_parameter(*node, parameter("cancel.duration")); p.cancel.max_lateral_jerk = - getOrDeclareParameter(*node, parameter("cancel.max_lateral_jerk")); + get_or_declare_parameter(*node, parameter("cancel.max_lateral_jerk")); p.cancel.overhang_tolerance = - getOrDeclareParameter(*node, parameter("cancel.overhang_tolerance")); + get_or_declare_parameter(*node, parameter("cancel.overhang_tolerance")); p.cancel.th_unsafe_hysteresis = - getOrDeclareParameter(*node, parameter("cancel.unsafe_hysteresis_threshold")); + get_or_declare_parameter(*node, parameter("cancel.unsafe_hysteresis_threshold")); p.cancel.deceleration_sampling_num = - getOrDeclareParameter(*node, parameter("cancel.deceleration_sampling_num")); + get_or_declare_parameter(*node, parameter("cancel.deceleration_sampling_num")); // finish judge parameters p.lane_change_finish_judge_buffer = - getOrDeclareParameter(*node, parameter("lane_change_finish_judge_buffer")); + get_or_declare_parameter(*node, parameter("lane_change_finish_judge_buffer")); p.th_finish_judge_lateral_diff = - getOrDeclareParameter(*node, parameter("finish_judge_lateral_threshold")); + get_or_declare_parameter(*node, parameter("finish_judge_lateral_threshold")); const auto finish_judge_lateral_angle_deviation = - getOrDeclareParameter(*node, parameter("finish_judge_lateral_angle_deviation")); - p.th_finish_judge_yaw_diff = - autoware::universe_utils::deg2rad(finish_judge_lateral_angle_deviation); + get_or_declare_parameter(*node, parameter("finish_judge_lateral_angle_deviation")); + p.th_finish_judge_yaw_diff = autoware_utils::deg2rad(finish_judge_lateral_angle_deviation); // debug marker - p.publish_debug_marker = getOrDeclareParameter(*node, parameter("publish_debug_marker")); + p.publish_debug_marker = get_or_declare_parameter(*node, parameter("publish_debug_marker")); // terminal lane change path - p.terminal_path.enable = getOrDeclareParameter(*node, parameter("terminal_path.enable")); + p.terminal_path.enable = get_or_declare_parameter(*node, parameter("terminal_path.enable")); p.terminal_path.disable_near_goal = - getOrDeclareParameter(*node, parameter("terminal_path.disable_near_goal")); + get_or_declare_parameter(*node, parameter("terminal_path.disable_near_goal")); p.terminal_path.stop_at_boundary = - getOrDeclareParameter(*node, parameter("terminal_path.stop_at_boundary")); + get_or_declare_parameter(*node, parameter("terminal_path.stop_at_boundary")); // validation of safety check parameters // if loose check is not enabled, lane change module will keep on chattering and canceling, and @@ -312,14 +319,14 @@ std::unique_ptr LaneChangeModuleManager::createNewSceneMod void LaneChangeModuleManager::updateModuleParams(const std::vector & parameters) { - using autoware::universe_utils::updateParam; + using autoware_utils::update_param; auto p = parameters_; { const std::string ns = "lane_change."; auto time_limit = p->time_limit; - updateParam(parameters, ns + "time_limit", time_limit); + update_param(parameters, ns + "time_limit", time_limit); if (time_limit >= 10.0) { p->time_limit = time_limit; } else { @@ -329,44 +336,44 @@ void LaneChangeModuleManager::updateModuleParams(const std::vectortime_limit); } - updateParam(parameters, ns + "backward_lane_length", p->backward_lane_length); - updateParam( + update_param(parameters, ns + "backward_lane_length", p->backward_lane_length); + update_param( parameters, ns + "backward_length_buffer_for_end_of_lane", p->backward_length_buffer_for_end_of_lane); - updateParam( + update_param( parameters, ns + "backward_length_buffer_for_blocking_object", p->backward_length_buffer_for_blocking_object); - updateParam( + update_param( parameters, ns + "lane_change_finish_judge_buffer", p->lane_change_finish_judge_buffer); - updateParam( + update_param( parameters, ns + "enable_stopped_vehicle_buffer", p->enable_stopped_vehicle_buffer); - updateParam( + update_param( parameters, ns + "finish_judge_lateral_threshold", p->th_finish_judge_lateral_diff); - updateParam(parameters, ns + "publish_debug_marker", p->publish_debug_marker); - updateParam( + update_param(parameters, ns + "publish_debug_marker", p->publish_debug_marker); + update_param( parameters, ns + "min_length_for_turn_signal_activation", p->min_length_for_turn_signal_activation); } { const std::string ns = "lane_change.trajectory."; - updateParam( + update_param( parameters, ns + "max_prepare_duration", p->trajectory.max_prepare_duration); - updateParam( + update_param( parameters, ns + "min_prepare_duration", p->trajectory.min_prepare_duration); - updateParam(parameters, ns + "lateral_jerk", p->trajectory.lateral_jerk); - updateParam( + update_param(parameters, ns + "lateral_jerk", p->trajectory.lateral_jerk); + update_param( parameters, ns + "min_lane_changing_velocity", p->trajectory.min_lane_changing_velocity); - updateParam( + update_param( parameters, ns + "min_longitudinal_acc", p->trajectory.min_longitudinal_acc); - updateParam( + update_param( parameters, ns + "max_longitudinal_acc", p->trajectory.max_longitudinal_acc); - updateParam( + update_param( parameters, ns + "lane_changing_decel_factor", p->trajectory.lane_changing_decel_factor); - updateParam( + update_param( parameters, ns + "th_prepare_curvature", p->trajectory.th_prepare_curvature); int longitudinal_acc_sampling_num = p->trajectory.lon_acc_sampling_num; - updateParam(parameters, ns + "lon_acc_sampling_num", longitudinal_acc_sampling_num); + update_param(parameters, ns + "lon_acc_sampling_num", longitudinal_acc_sampling_num); if (longitudinal_acc_sampling_num > 0) { p->trajectory.lon_acc_sampling_num = longitudinal_acc_sampling_num; } else { @@ -378,7 +385,7 @@ void LaneChangeModuleManager::updateModuleParams(const std::vectortrajectory.lat_acc_sampling_num; - updateParam(parameters, ns + "lat_acc_sampling_num", lateral_acc_sampling_num); + update_param(parameters, ns + "lat_acc_sampling_num", lateral_acc_sampling_num); if (lateral_acc_sampling_num > 0) { p->trajectory.lat_acc_sampling_num = lateral_acc_sampling_num; } else { @@ -389,24 +396,24 @@ void LaneChangeModuleManager::updateModuleParams(const std::vector( + update_param( parameters, ns + "th_prepare_length_diff", p->trajectory.th_prepare_length_diff); - updateParam( + update_param( parameters, ns + "th_lane_changing_length_diff", p->trajectory.th_lane_changing_length_diff); } { const std::string ns = "lane_change.frenet."; - updateParam(parameters, ns + "enable", p->frenet.enable); - updateParam(parameters, ns + "th_yaw_diff", p->frenet.th_yaw_diff_deg); - updateParam( + update_param(parameters, ns + "enable", p->frenet.enable); + update_param(parameters, ns + "th_yaw_diff", p->frenet.th_yaw_diff_deg); + update_param( parameters, ns + "th_curvature_smoothing", p->frenet.th_curvature_smoothing); } { const std::string ns = "lane_change.safety_check.lane_expansion."; - updateParam(parameters, ns + "left_offset", p->safety.lane_expansion_left_offset); - updateParam(parameters, ns + "right_offset", p->safety.lane_expansion_right_offset); + update_param(parameters, ns + "left_offset", p->safety.lane_expansion_left_offset); + update_param(parameters, ns + "right_offset", p->safety.lane_expansion_right_offset); } { @@ -415,9 +422,9 @@ void LaneChangeModuleManager::updateModuleParams(const std::vector min_values = p->trajectory.lat_acc_map.base_min_acc; std::vector max_values = p->trajectory.lat_acc_map.base_max_acc; - updateParam>(parameters, ns + "velocity", velocity); - updateParam>(parameters, ns + "min_values", min_values); - updateParam>(parameters, ns + "max_values", max_values); + update_param>(parameters, ns + "velocity", velocity); + update_param>(parameters, ns + "min_values", min_values); + update_param>(parameters, ns + "max_values", max_values); if ( velocity.size() >= 2 && velocity.size() == min_values.size() && velocity.size() == max_values.size()) { @@ -437,30 +444,30 @@ void LaneChangeModuleManager::updateModuleParams(const std::vector( + update_param( parameters, ns + "enable_for_prepare_phase.general_lanes", p->safety.collision_check.enable_for_prepare_phase_in_general_lanes); - updateParam( + update_param( parameters, ns + "enable_for_prepare_phase.intersection", p->safety.collision_check.enable_for_prepare_phase_in_intersection); - updateParam( + update_param( parameters, ns + "enable_for_prepare_phase.turns", p->safety.collision_check.enable_for_prepare_phase_in_turns); - updateParam( + update_param( parameters, ns + "check_current_lanes", p->safety.collision_check.check_current_lane); - updateParam( + update_param( parameters, ns + "check_other_lanes", p->safety.collision_check.check_other_lanes); - updateParam( + update_param( parameters, ns + "use_all_predicted_paths", p->safety.collision_check.use_all_predicted_paths); - updateParam( + update_param( parameters, ns + "prediction_time_resolution", p->safety.collision_check.prediction_time_resolution); - updateParam( + update_param( parameters, ns + "yaw_diff_threshold", p->safety.collision_check.th_yaw_diff); auto th_incoming_object_yaw = p->safety.collision_check.th_incoming_object_yaw; - updateParam(parameters, ns + "th_incoming_object_yaw", th_incoming_object_yaw); + update_param(parameters, ns + "th_incoming_object_yaw", th_incoming_object_yaw); if (th_incoming_object_yaw >= M_PI_2) { p->safety.collision_check.th_incoming_object_yaw = th_incoming_object_yaw; } else { @@ -475,53 +482,53 @@ void LaneChangeModuleManager::updateModuleParams(const std::vector(parameters, ns + "car", p->safety.target_object_types.check_car); - updateParam(parameters, ns + "truck", p->safety.target_object_types.check_truck); - updateParam(parameters, ns + "bus", p->safety.target_object_types.check_bus); - updateParam(parameters, ns + "trailer", p->safety.target_object_types.check_trailer); - updateParam(parameters, ns + "unknown", p->safety.target_object_types.check_unknown); - updateParam(parameters, ns + "bicycle", p->safety.target_object_types.check_bicycle); - updateParam( + update_param(parameters, ns + "car", p->safety.target_object_types.check_car); + update_param(parameters, ns + "truck", p->safety.target_object_types.check_truck); + update_param(parameters, ns + "bus", p->safety.target_object_types.check_bus); + update_param(parameters, ns + "trailer", p->safety.target_object_types.check_trailer); + update_param(parameters, ns + "unknown", p->safety.target_object_types.check_unknown); + update_param(parameters, ns + "bicycle", p->safety.target_object_types.check_bicycle); + update_param( parameters, ns + "motorcycle", p->safety.target_object_types.check_motorcycle); - updateParam( + update_param( parameters, ns + "pedestrian", p->safety.target_object_types.check_pedestrian); } { const std::string ns = "lane_change.regulation."; - updateParam(parameters, ns + "crosswalk", p->regulate_on_crosswalk); - updateParam(parameters, ns + "intersection", p->regulate_on_intersection); - updateParam(parameters, ns + "traffic_light", p->regulate_on_traffic_light); + update_param(parameters, ns + "crosswalk", p->regulate_on_crosswalk); + update_param(parameters, ns + "intersection", p->regulate_on_intersection); + update_param(parameters, ns + "traffic_light", p->regulate_on_traffic_light); } { const std::string ns = "lane_change.stuck_detection."; - updateParam(parameters, ns + "velocity", p->th_stop_velocity); - updateParam(parameters, ns + "stop_time", p->th_stop_time); + update_param(parameters, ns + "velocity", p->th_stop_velocity); + update_param(parameters, ns + "stop_time", p->th_stop_time); } auto update_rss_params = [¶meters, this](const std::string & prefix, auto & params) { - using autoware::universe_utils::updateParam; - updateParam( + using autoware_utils::update_param; + update_param( parameters, prefix + "longitudinal_distance_min_threshold", params.longitudinal_distance_min_threshold); - updateParam( + update_param( parameters, prefix + "longitudinal_velocity_delta_time", params.longitudinal_velocity_delta_time); - updateParam( + update_param( parameters, prefix + "expected_front_deceleration", params.front_vehicle_deceleration); - updateParam( + update_param( parameters, prefix + "expected_rear_deceleration", params.rear_vehicle_deceleration); - updateParam( + update_param( parameters, prefix + "rear_vehicle_reaction_time", params.rear_vehicle_reaction_time); - updateParam( + update_param( parameters, prefix + "rear_vehicle_safety_time_margin", params.rear_vehicle_safety_time_margin); - updateParam( + update_param( parameters, prefix + "lateral_distance_max_threshold", params.lateral_distance_max_threshold); auto extended_polygon_policy = params.extended_polygon_policy; - updateParam( + update_param( parameters, prefix + "extended_polygon_policy", extended_polygon_policy); if (extended_polygon_policy == "rectangle" || extended_polygon_policy == "along_path") { params.extended_polygon_policy = extended_polygon_policy; @@ -541,26 +548,26 @@ void LaneChangeModuleManager::updateModuleParams(const std::vector(parameters, ns + "enable", p->delay.enable); - updateParam( + update_param(parameters, ns + "enable", p->delay.enable); + update_param( parameters, ns + "check_only_parked_vehicle", p->delay.check_only_parked_vehicle); - updateParam( + update_param( parameters, ns + "min_road_shoulder_width", p->delay.min_road_shoulder_width); - updateParam( + update_param( parameters, ns + "th_parked_vehicle_shift_ratio", p->delay.th_parked_vehicle_shift_ratio); } { const std::string ns = "lane_change.terminal_path."; - updateParam(parameters, ns + "enable", p->terminal_path.enable); - updateParam(parameters, ns + "disable_near_goal", p->terminal_path.disable_near_goal); - updateParam(parameters, ns + "stop_at_boundary", p->terminal_path.stop_at_boundary); + update_param(parameters, ns + "enable", p->terminal_path.enable); + update_param(parameters, ns + "disable_near_goal", p->terminal_path.disable_near_goal); + update_param(parameters, ns + "stop_at_boundary", p->terminal_path.stop_at_boundary); } { const std::string ns = "lane_change.cancel."; bool enable_on_prepare_phase = p->cancel.enable_on_prepare_phase; - updateParam(parameters, ns + "enable_on_prepare_phase", enable_on_prepare_phase); + update_param(parameters, ns + "enable_on_prepare_phase", enable_on_prepare_phase); if (!enable_on_prepare_phase) { RCLCPP_WARN_THROTTLE( node_->get_logger(), *node_->get_clock(), 1000, @@ -569,7 +576,7 @@ void LaneChangeModuleManager::updateModuleParams(const std::vectorcancel.enable_on_lane_changing_phase; - updateParam( + update_param( parameters, ns + "enable_on_lane_changing_phase", enable_on_lane_changing_phase); if (!enable_on_lane_changing_phase) { RCLCPP_WARN_THROTTLE( @@ -579,7 +586,7 @@ void LaneChangeModuleManager::updateModuleParams(const std::vectorcancel.deceleration_sampling_num; - updateParam(parameters, ns + "deceleration_sampling_num", deceleration_sampling_num); + update_param(parameters, ns + "deceleration_sampling_num", deceleration_sampling_num); if (deceleration_sampling_num > 0) { p->cancel.deceleration_sampling_num = deceleration_sampling_num; } else { @@ -590,11 +597,11 @@ void LaneChangeModuleManager::updateModuleParams(const std::vector(parameters, ns + "delta_time", p->cancel.delta_time); - updateParam(parameters, ns + "duration", p->cancel.duration); - updateParam(parameters, ns + "max_lateral_jerk", p->cancel.max_lateral_jerk); - updateParam(parameters, ns + "overhang_tolerance", p->cancel.overhang_tolerance); - updateParam( + update_param(parameters, ns + "delta_time", p->cancel.delta_time); + update_param(parameters, ns + "duration", p->cancel.duration); + update_param(parameters, ns + "max_lateral_jerk", p->cancel.max_lateral_jerk); + update_param(parameters, ns + "overhang_tolerance", p->cancel.overhang_tolerance); + update_param( parameters, ns + "unsafe_hysteresis_threshold", p->cancel.th_unsafe_hysteresis); } diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp index 37331560814c2..606ac2604b8ea 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp @@ -26,14 +26,14 @@ #include #include -#include -#include -#include -#include #include #include #include #include +#include +#include +#include +#include #include #include #include @@ -235,7 +235,7 @@ void NormalLaneChange::update_filtered_objects() void NormalLaneChange::updateLaneChangeStatus() { - universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + autoware_utils::ScopedTimeTrack st(__func__, *time_keeper_); const auto [found_valid_path, found_safe_path] = getSafePath(status_.lane_change_path); // Update status @@ -279,7 +279,7 @@ std::pair NormalLaneChange::getSafePath(LaneChangePath & safe_path) bool NormalLaneChange::isLaneChangeRequired() { - universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + autoware_utils::ScopedTimeTrack st(__func__, *time_keeper_); if ( !common_data_ptr_ || !common_data_ptr_->is_data_available() || @@ -436,7 +436,7 @@ BehaviorModuleOutput NormalLaneChange::getTerminalLaneChangePath() const BehaviorModuleOutput NormalLaneChange::generateOutput() { - universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + autoware_utils::ScopedTimeTrack st(__func__, *time_keeper_); if (!status_.is_valid_path) { RCLCPP_DEBUG(logger_, "No valid path found. Returning previous module's path as output."); insert_stop_point(get_current_lanes(), prev_module_output_.path); @@ -486,7 +486,7 @@ BehaviorModuleOutput NormalLaneChange::generateOutput() void NormalLaneChange::extendOutputDrivableArea(BehaviorModuleOutput & output) const { - universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + autoware_utils::ScopedTimeTrack st(__func__, *time_keeper_); const auto & dp = planner_data_->drivable_area_expansion_parameters; const auto drivable_lanes = utils::lane_change::generateDrivableLanes( @@ -506,7 +506,7 @@ void NormalLaneChange::extendOutputDrivableArea(BehaviorModuleOutput & output) c void NormalLaneChange::insert_stop_point( const lanelet::ConstLanelets & lanelets, PathWithLaneId & path, const bool is_waiting_approval) { - universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + autoware_utils::ScopedTimeTrack st(__func__, *time_keeper_); if (lanelets.empty()) { return; } @@ -628,7 +628,7 @@ void NormalLaneChange::insert_stop_point_on_current_lanes( PathWithLaneId NormalLaneChange::getReferencePath() const { - universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + autoware_utils::ScopedTimeTrack st(__func__, *time_keeper_); lanelet::ConstLanelet closest_lanelet; if (!lanelet::utils::query::getClosestLanelet( get_target_lanes(), getEgoPose(), &closest_lanelet)) { @@ -643,7 +643,7 @@ PathWithLaneId NormalLaneChange::getReferencePath() const std::optional NormalLaneChange::extendPath() { - universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + autoware_utils::ScopedTimeTrack st(__func__, *time_keeper_); const auto path = status_.lane_change_path.path; auto & target_lanes = common_data_ptr_->lanes_ptr->target; @@ -706,7 +706,7 @@ void NormalLaneChange::resetParameters() TurnSignalInfo NormalLaneChange::updateOutputTurnSignal() const { - universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + autoware_utils::ScopedTimeTrack st(__func__, *time_keeper_); const auto & current_lanes = get_current_lanes(); const auto & shift_line = status_.lane_change_path.info.shift_line; const auto & shift_path = status_.lane_change_path.shifted_path; @@ -727,7 +727,7 @@ TurnSignalInfo NormalLaneChange::updateOutputTurnSignal() const lanelet::ConstLanelets NormalLaneChange::get_lane_change_lanes( const lanelet::ConstLanelets & current_lanes) const { - universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + autoware_utils::ScopedTimeTrack st(__func__, *time_keeper_); if (current_lanes.empty()) { return {}; } @@ -952,7 +952,7 @@ lane_change::TargetObjects NormalLaneChange::get_target_objects( FilteredLanesObjects NormalLaneChange::filter_objects() const { - universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + autoware_utils::ScopedTimeTrack st(__func__, *time_keeper_); auto objects = *planner_data_->dynamic_object; utils::path_safety_checker::filterObjectsByClass( objects, lane_change_parameters_->safety.target_object_types); @@ -1542,7 +1542,7 @@ PathSafetyStatus NormalLaneChange::evaluateApprovedPathWithUnsafeHysteresis( bool NormalLaneChange::isValidPath(const PathWithLaneId & path) const { - universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + autoware_utils::ScopedTimeTrack st(__func__, *time_keeper_); const auto & route_handler = planner_data_->route_handler; const auto & dp = planner_data_->drivable_area_expansion_parameters; @@ -1576,14 +1576,14 @@ bool NormalLaneChange::isValidPath(const PathWithLaneId & path) const bool NormalLaneChange::isRequiredStop(const bool is_trailing_object) { - universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + autoware_utils::ScopedTimeTrack st(__func__, *time_keeper_); return common_data_ptr_->transient_data.is_ego_near_current_terminal_start && isAbleToStopSafely() && is_trailing_object; } bool NormalLaneChange::calcAbortPath() { - universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + autoware_utils::ScopedTimeTrack st(__func__, *time_keeper_); const auto & route_handler = getRouteHandler(); const auto & common_param = getCommonParam(); const auto current_velocity = @@ -1729,7 +1729,7 @@ PathSafetyStatus NormalLaneChange::isLaneChangePathSafe( const utils::path_safety_checker::RSSparams & rss_params, CollisionCheckDebugMap & debug_data) const { - universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + autoware_utils::ScopedTimeTrack st(__func__, *time_keeper_); constexpr auto is_safe = true; constexpr auto is_object_behind_ego = true; @@ -1841,7 +1841,7 @@ bool NormalLaneChange::is_colliding( double NormalLaneChange::get_max_velocity_for_safety_check() const { - universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + autoware_utils::ScopedTimeTrack st(__func__, *time_keeper_); const auto external_velocity_limit_ptr = planner_data_->external_limit_max_velocity; if (external_velocity_limit_ptr) { return std::min( @@ -1853,7 +1853,7 @@ double NormalLaneChange::get_max_velocity_for_safety_check() const bool NormalLaneChange::is_ego_stuck() const { - universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + autoware_utils::ScopedTimeTrack st(__func__, *time_keeper_); const auto & lc_param_ptr = common_data_ptr_->lc_param_ptr; if (std::abs(common_data_ptr_->get_ego_speed()) > lc_param_ptr->th_stop_velocity) { @@ -1912,7 +1912,7 @@ void NormalLaneChange::set_stop_pose( void NormalLaneChange::updateStopTime() { - universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + autoware_utils::ScopedTimeTrack st(__func__, *time_keeper_); const auto current_vel = getEgoVelocity(); if (std::abs(current_vel) > lane_change_parameters_->th_stop_velocity) { diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/calculation.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/calculation.cpp index a0130fcd27041..577f0fd32614b 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/calculation.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/calculation.cpp @@ -59,11 +59,11 @@ double calc_stopping_distance(const LCParamPtr & lc_param_ptr) double calc_dist_to_last_fit_width( const lanelet::ConstLanelets & lanelets, const lanelet::BasicPolygon2d & lanelet_polygon, - const universe_utils::LineString2d & line_string, const Pose & src_pose, + const autoware_utils::LineString2d & line_string, const Pose & src_pose, const BehaviorPathPlannerParameters & bpp_param, const double margin) { const double buffer_distance = 0.5 * bpp_param.vehicle_width + margin; - universe_utils::MultiPolygon2d center_line_polygon; + autoware_utils::MultiPolygon2d center_line_polygon; namespace strategy = boost::geometry::strategy::buffer; boost::geometry::buffer( line_string, center_line_polygon, strategy::distance_symmetric(buffer_distance), @@ -72,7 +72,7 @@ double calc_dist_to_last_fit_width( if (center_line_polygon.empty()) return 0.0; - std::vector intersection_points; + std::vector intersection_points; boost::geometry::intersection(lanelet_polygon, center_line_polygon, intersection_points); if (intersection_points.empty()) { @@ -97,11 +97,11 @@ double calc_dist_to_last_fit_width( const auto & current_lanes_polygon = common_data_ptr->lanes_polygon_ptr->current; const auto & bpp_param = *common_data_ptr->bpp_param_ptr; - universe_utils::LineString2d line_string; + autoware_utils::LineString2d line_string; line_string.reserve(path.points.size() - 1); std::for_each(path.points.begin() + 1, path.points.end(), [&line_string](const auto & point) { const auto & position = point.point.pose.position; - boost::geometry::append(line_string, universe_utils::Point2d(position.x, position.y)); + boost::geometry::append(line_string, autoware_utils::Point2d(position.x, position.y)); }); const auto & src_pose = path.points.front().point.pose; @@ -120,10 +120,10 @@ double calc_dist_to_last_fit_width( if (center_line.size() <= 1) return 0.0; - universe_utils::LineString2d line_string; + autoware_utils::LineString2d line_string; line_string.reserve(center_line.size() - 1); std::for_each(center_line.begin() + 1, center_line.end(), [&line_string](const auto & point) { - boost::geometry::append(line_string, universe_utils::Point2d(point.x(), point.y())); + boost::geometry::append(line_string, autoware_utils::Point2d(point.x(), point.y())); }); return calc_dist_to_last_fit_width( diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/markers.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/markers.cpp index 465835ff5fbb4..4741a594e2a64 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/markers.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/markers.cpp @@ -16,9 +16,9 @@ #include "autoware/behavior_path_planner_common/marker_utils/utils.hpp" #include -#include -#include #include +#include +#include #include #include @@ -37,8 +37,8 @@ namespace marker_utils::lane_change_markers { -using autoware::universe_utils::createDefaultMarker; -using autoware::universe_utils::createMarkerScale; +using autoware_utils::create_default_marker; +using autoware_utils::create_marker_scale; using geometry_msgs::msg::Point; MarkerArray showAllValidLaneChangePath( @@ -64,9 +64,9 @@ MarkerArray showAllValidLaneChangePath( std::string ns_with_idx = ns + "[" + std::to_string(idx) + "]"; const auto & color = colors.at(idx); const auto & points = lc_path.path.points; - auto marker = createDefaultMarker( - "map", current_time, ns_with_idx, ++id, Marker::LINE_STRIP, createMarkerScale(0.1, 0.1, 0.0), - color); + auto marker = create_default_marker( + "map", current_time, ns_with_idx, ++id, Marker::LINE_STRIP, + create_marker_scale(0.1, 0.1, 0.0), color); marker.points.reserve(points.size()); for (const auto & point : points) { @@ -74,9 +74,9 @@ MarkerArray showAllValidLaneChangePath( } const auto & info = lc_path.info; - auto text_marker = createDefaultMarker( + auto text_marker = create_default_marker( "map", current_time, ns_with_idx, ++id, visualization_msgs::msg::Marker::TEXT_VIEW_FACING, - createMarkerScale(0.1, 0.1, 0.8), colors::yellow()); + create_marker_scale(0.1, 0.1, 0.8), colors::yellow()); const auto prep_idx = points.size() / 4; text_marker.pose = points.at(prep_idx).point.pose; text_marker.pose.position.z += 2.0; @@ -114,18 +114,18 @@ MarkerArray createLaneChangingVirtualWallMarker( MarkerArray marker_array{}; marker_array.markers.reserve(2); { - auto wall_marker = createDefaultMarker( + auto wall_marker = create_default_marker( "map", now, ns + "virtual_wall", id, visualization_msgs::msg::Marker::CUBE, - createMarkerScale(0.1, 5.0, 2.0), colors::green()); + create_marker_scale(0.1, 5.0, 2.0), colors::green()); wall_marker.pose = lane_changing_pose; wall_marker.pose.position.z += 1.0; marker_array.markers.push_back(wall_marker); } { - auto text_marker = createDefaultMarker( + auto text_marker = create_default_marker( "map", now, ns + "_text", id, visualization_msgs::msg::Marker::TEXT_VIEW_FACING, - createMarkerScale(0.0, 0.0, 1.0), colors::white()); + create_marker_scale(0.0, 0.0, 1.0), colors::white()); text_marker.pose = lane_changing_pose; text_marker.pose.position.z += 2.0; text_marker.text = module_name; @@ -172,9 +172,9 @@ MarkerArray showExecutionInfo( const geometry_msgs::msg::Pose & ego_pose) { auto default_text_marker = [&]() { - return createDefaultMarker( + return create_default_marker( "map", rclcpp::Clock{RCL_ROS_TIME}.now(), "execution_info", 0, Marker::TEXT_VIEW_FACING, - createMarkerScale(0.5, 0.5, 0.5), colors::white()); + create_marker_scale(0.5, 0.5, 0.5), colors::white()); }; MarkerArray marker_array; @@ -200,10 +200,10 @@ MarkerArray ShowLaneChangeMetricsInfo( { MarkerArray marker_array; - auto text_marker = createDefaultMarker( + auto text_marker = create_default_marker( "map", rclcpp::Clock{RCL_ROS_TIME}.now(), "sampling_metrics", 0, Marker::TEXT_VIEW_FACING, - createMarkerScale(0.6, 0.6, 0.6), colors::yellow()); - text_marker.pose = autoware::universe_utils::calcOffsetPose(pose, 10.0, 15.0, 0.0); + create_marker_scale(0.6, 0.6, 0.6), colors::yellow()); + text_marker.pose = autoware_utils::calc_offset_pose(pose, 10.0, 15.0, 0.0); if (!debug_data.lane_change_metrics.empty()) { text_marker.text = @@ -293,7 +293,7 @@ MarkerArray createDebugMarkerArray( MarkerArray debug_marker; const auto add = [&debug_marker](const MarkerArray & added) { - autoware::universe_utils::appendMarkerArray(added, &debug_marker); + autoware_utils::append_marker_array(added, &debug_marker); }; if (!scene_debug_data.execution_area.points.empty()) { diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/path.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/path.cpp index 5d16ffb32eb54..a7e96394db041 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/path.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/path.cpp @@ -21,8 +21,8 @@ #include "autoware/behavior_path_planner_common/utils/utils.hpp" #include -#include #include +#include #include #include #include @@ -53,8 +53,8 @@ using autoware::frenet_planner::SamplingParameters; using autoware::route_handler::Direction; using autoware::sampler_common::FrenetPoint; using autoware::sampler_common::transform::Spline2D; -using autoware::universe_utils::LineString2d; -using autoware::universe_utils::Point2d; +using autoware_utils::LineString2d; +using autoware_utils::Point2d; using geometry_msgs::msg::Pose; template @@ -158,8 +158,8 @@ std::optional exceed_yaw_threshold( const auto & p1 = std::prev(prepare.end() - 1)->point.pose; const auto & p2 = std::next(lane_changing.begin())->point.pose; - const auto yaw_diff_rad = std::abs(autoware::universe_utils::normalizeRadian( - tf2::getYaw(p1.orientation) - tf2::getYaw(p2.orientation))); + const auto yaw_diff_rad = std::abs( + autoware_utils::normalize_radian(tf2::getYaw(p1.orientation) - tf2::getYaw(p2.orientation))); if (yaw_diff_rad > yaw_th_rad) { return yaw_diff_rad; } @@ -244,11 +244,11 @@ std::vector calc_curvatures( points | ranges::views::drop(nearest_segment_idx), points | ranges::views::drop(nearest_segment_idx + 1), points | ranges::views::drop(nearest_segment_idx + 2))) { - const auto point1 = autoware::universe_utils::getPoint(p1); - const auto point2 = autoware::universe_utils::getPoint(p2); - const auto point3 = autoware::universe_utils::getPoint(p3); + const auto point1 = autoware_utils::get_point(p1); + const auto point2 = autoware_utils::get_point(p2); + const auto point3 = autoware_utils::get_point(p3); - curvatures.push_back(autoware::universe_utils::calcCurvature(point1, point2, point3)); + curvatures.push_back(autoware_utils::calc_curvature(point1, point2, point3)); } return curvatures; @@ -486,7 +486,7 @@ LaneChangePath construct_candidate_path( point.lane_ids = target_lane_reference_path.points.at(*nearest_idx).lane_ids; } - constexpr auto yaw_diff_th = autoware::universe_utils::deg2rad(5.0); + constexpr auto yaw_diff_th = autoware_utils::deg2rad(5.0); if ( const auto yaw_diff_opt = exceed_yaw_threshold(prepare_segment, shifted_path.path, yaw_diff_th)) { @@ -510,7 +510,7 @@ std::vector generate_frenet_candidates( const std::vector & prep_metrics) { std::vector trajectory_groups; - universe_utils::StopWatch sw; + autoware_utils::StopWatch sw; const auto & transient_data = common_data_ptr->transient_data; const auto & current_lanes = common_data_ptr->lanes_ptr->current; @@ -551,7 +551,7 @@ std::vector generate_frenet_candidates( double ref_s = 0.0; for (auto it = target_lane_reference_path.points.begin(); std::next(it) != target_lane_reference_path.points.end(); ++it) { - ref_s += universe_utils::calcDistance2d(*it, *std::next(it)); + ref_s += autoware_utils::calc_distance2d(*it, *std::next(it)); target_ref_path_sums.push_back(ref_s); } @@ -678,8 +678,8 @@ std::optional get_candidate_path( const auto th_yaw_diff_deg = common_data_ptr->lc_param_ptr->frenet.th_yaw_diff_deg; if ( const auto yaw_diff_opt = exceed_yaw_threshold( - prepare_segment, shifted_path.path, autoware::universe_utils::deg2rad(th_yaw_diff_deg))) { - const auto yaw_diff_deg = autoware::universe_utils::rad2deg(yaw_diff_opt.value()); + prepare_segment, shifted_path.path, autoware_utils::deg2rad(th_yaw_diff_deg))) { + const auto yaw_diff_deg = autoware_utils::rad2deg(yaw_diff_opt.value()); const auto err_msg = fmt::format( "Excessive yaw difference {yaw_diff:2.2f}[deg]. The threshold is {th_yaw_diff:2.2f}[deg].", fmt::arg("yaw_diff", yaw_diff_deg), fmt::arg("th_yaw_diff", th_yaw_diff_deg)); @@ -739,11 +739,10 @@ void append_target_ref_to_candidate( for (const auto & [p2, p3] : ranges::views::zip( points | ranges::views::drop(lc_end_idx + 1), points | ranges::views::drop(lc_end_idx + 2))) { - const auto point1 = autoware::universe_utils::getPoint(candidate_path.back().point.pose); - const auto point2 = autoware::universe_utils::getPoint(p2); - const auto point3 = autoware::universe_utils::getPoint(p3); - const auto curvature = - std::abs(autoware::universe_utils::calcCurvature(point1, point2, point3)); + const auto point1 = autoware_utils::get_point(candidate_path.back().point.pose); + const auto point2 = autoware_utils::get_point(p2); + const auto point3 = autoware_utils::get_point(p3); + const auto curvature = std::abs(autoware_utils::calc_curvature(point1, point2, point3)); if (curvature < th_curvature_smoothing) { candidate_path.push_back(p2); } diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp index 1f87bbd644009..a7b808f1f6877 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp @@ -22,22 +22,22 @@ #include "autoware/behavior_path_planner_common/utils/traffic_light_utils.hpp" #include "autoware/behavior_path_planner_common/utils/utils.hpp" #include "autoware/object_recognition_utils/predicted_path_utils.hpp" -#include "autoware/universe_utils/math/unit_conversion.hpp" +#include "autoware_utils/math/unit_conversion.hpp" // for the geometry types #include -#include +#include // for the svg mapper #include #include #include #include -#include -#include -#include #include #include #include +#include +#include +#include #include #include #include @@ -70,12 +70,12 @@ namespace autoware::behavior_path_planner::utils::lane_change { using autoware::route_handler::RouteHandler; -using autoware::universe_utils::LineString2d; -using autoware::universe_utils::Point2d; -using autoware::universe_utils::Polygon2d; using autoware_internal_planning_msgs::msg::PathWithLaneId; using autoware_perception_msgs::msg::ObjectClassification; using autoware_perception_msgs::msg::PredictedObjects; +using autoware_utils::LineString2d; +using autoware_utils::Point2d; +using autoware_utils::Polygon2d; using behavior_path_planner::lane_change::PathType; using geometry_msgs::msg::Pose; @@ -436,7 +436,7 @@ bool isParkedObject( const auto & obj_pose = object.initial_pose; const auto & obj_shape = object.shape; - const auto obj_poly = autoware::universe_utils::toPolygon2d(obj_pose, obj_shape); + const auto obj_poly = autoware_utils::to_polygon2d(obj_pose, obj_shape); const auto obj_point = obj_pose.position; double max_dist_to_bound = std::numeric_limits::lowest(); @@ -555,7 +555,7 @@ ExtendedPredictedObject transform( t += time_resolution) { const auto obj_pose = autoware::object_recognition_utils::calcInterpolatedPose(path, t); if (obj_pose) { - const auto obj_polygon = autoware::universe_utils::toPolygon2d(*obj_pose, object.shape); + const auto obj_polygon = autoware_utils::to_polygon2d(*obj_pose, object.shape); extended_object.predicted_paths.at(i).path.emplace_back( t, *obj_pose, obj_vel_norm, obj_polygon); } @@ -595,7 +595,7 @@ Polygon2d get_ego_footprint(const Pose & ego_pose, const VehicleInfo & ego_info) const auto base_to_rear = ego_info.rear_overhang_m; const auto width = ego_info.vehicle_width_m; - return autoware::universe_utils::toFootprint(ego_pose, base_to_front, base_to_rear, width); + return autoware_utils::to_footprint(ego_pose, base_to_front, base_to_rear, width); } Point getEgoFrontVertex( @@ -603,7 +603,7 @@ Point getEgoFrontVertex( { const double lon_offset = ego_info.wheel_base_m + ego_info.front_overhang_m; const double lat_offset = 0.5 * (left ? ego_info.vehicle_width_m : -ego_info.vehicle_width_m); - return autoware::universe_utils::calcOffsetPose(ego_pose, lon_offset, lat_offset, 0.0).position; + return autoware_utils::calc_offset_pose(ego_pose, lon_offset, lat_offset, 0.0).position; } bool is_within_intersection( @@ -713,8 +713,7 @@ bool is_ahead_of_ego( const auto & current_footprint = common_data_ptr->transient_data.current_footprint.outer(); auto ego_min_dist_to_end = std::numeric_limits::max(); for (const auto & ego_edge_point : current_footprint) { - const auto ego_edge = - autoware::universe_utils::createPoint(ego_edge_point.x(), ego_edge_point.y(), 0.0); + const auto ego_edge = autoware_utils::create_point(ego_edge_point.x(), ego_edge_point.y(), 0.0); const auto dist_to_end = autoware::motion_utils::calcSignedArcLength( path.points, ego_edge, path.points.back().point.pose.position); ego_min_dist_to_end = std::min(dist_to_end, ego_min_dist_to_end); @@ -722,7 +721,7 @@ bool is_ahead_of_ego( auto current_min_dist_to_end = std::numeric_limits::max(); for (const auto & polygon_p : object.initial_polygon.outer()) { - const auto obj_p = autoware::universe_utils::createPoint(polygon_p.x(), polygon_p.y(), 0.0); + const auto obj_p = autoware_utils::create_point(polygon_p.x(), polygon_p.y(), 0.0); const auto dist_ego_to_obj = autoware::motion_utils::calcSignedArcLength( path.points, obj_p, path.points.back().point.pose.position); current_min_dist_to_end = std::min(dist_ego_to_obj, current_min_dist_to_end); @@ -750,7 +749,7 @@ bool is_before_terminal( } for (const auto & polygon_p : object.initial_polygon.outer()) { - const auto obj_p = autoware::universe_utils::createPoint(polygon_p.x(), polygon_p.y(), 0.0); + const auto obj_p = autoware_utils::create_point(polygon_p.x(), polygon_p.y(), 0.0); const auto dist_obj_to_terminal = autoware::motion_utils::calcSignedArcLength(path.points, obj_p, terminal_position); current_max_dist = std::max(dist_obj_to_terminal, current_max_dist); @@ -763,10 +762,10 @@ double calc_angle_to_lanelet_segment(const lanelet::ConstLanelets & lanelets, co lanelet::ConstLanelet closest_lanelet; if (!lanelet::utils::query::getClosestLanelet(lanelets, pose, &closest_lanelet)) { - return autoware::universe_utils::deg2rad(180); + return autoware_utils::deg2rad(180); } const auto closest_pose = lanelet::utils::getClosestCenterPose(closest_lanelet, pose.position); - return std::abs(autoware::universe_utils::calcYawDeviation(closest_pose, pose)); + return std::abs(autoware_utils::calc_yaw_deviation(closest_pose, pose)); } double get_distance_to_next_regulatory_element( @@ -864,7 +863,7 @@ std::vector get_line_string_paths(const ExtendedPredictedObject & const auto & path = predicted_path.path; line_string.reserve(path.size()); for (const auto & path_point : path) { - const auto point = universe_utils::fromMsg(path_point.pose.position).to_2d(); + const auto point = autoware_utils::from_msg(path_point.pose.position).to_2d(); line_string.push_back(point); } diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/test/test_lane_change_utils.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/test/test_lane_change_utils.cpp index 147a53672f30a..78317e93717d2 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/test/test_lane_change_utils.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/test/test_lane_change_utils.cpp @@ -13,8 +13,8 @@ // limitations under the License. #include "autoware/behavior_path_lane_change_module/structs/data.hpp" -#include -#include +#include +#include #include #include @@ -24,16 +24,16 @@ constexpr double epsilon = 1e-6; TEST(BehaviorPathPlanningLaneChangeUtilsTest, projectCurrentPoseToTarget) { geometry_msgs::msg::Pose ego_pose; - const auto ego_yaw = autoware::universe_utils::deg2rad(0.0); - ego_pose.orientation = autoware::universe_utils::createQuaternionFromYaw(ego_yaw); - ego_pose.position = autoware::universe_utils::createPoint(0, 0, 0); + const auto ego_yaw = autoware_utils::deg2rad(0.0); + ego_pose.orientation = autoware_utils::create_quaternion_from_yaw(ego_yaw); + ego_pose.position = autoware_utils::create_point(0, 0, 0); geometry_msgs::msg::Pose obj_pose; - const auto obj_yaw = autoware::universe_utils::deg2rad(0.0); - obj_pose.orientation = autoware::universe_utils::createQuaternionFromYaw(obj_yaw); - obj_pose.position = autoware::universe_utils::createPoint(-4, 3, 0); + const auto obj_yaw = autoware_utils::deg2rad(0.0); + obj_pose.orientation = autoware_utils::create_quaternion_from_yaw(obj_yaw); + obj_pose.position = autoware_utils::create_point(-4, 3, 0); - const auto result = autoware::universe_utils::inverseTransformPose(obj_pose, ego_pose); + const auto result = autoware_utils::inverse_transform_pose(obj_pose, ego_pose); EXPECT_NEAR(result.position.x, -4, epsilon); EXPECT_NEAR(result.position.y, 3, epsilon); diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner/include/autoware/behavior_path_planner/behavior_path_planner_node.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner/include/autoware/behavior_path_planner/behavior_path_planner_node.hpp index 281d6e345d51a..215a9ee3bbb10 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner/include/autoware/behavior_path_planner/behavior_path_planner_node.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner/include/autoware/behavior_path_planner/behavior_path_planner_node.hpp @@ -17,13 +17,13 @@ #include "autoware/behavior_path_planner_common/data_manager.hpp" #include "autoware/behavior_path_planner_common/interface/scene_module_interface.hpp" -#include "autoware/universe_utils/ros/debug_publisher.hpp" -#include "autoware/universe_utils/ros/logger_level_configure.hpp" +#include "autoware_utils/ros/debug_publisher.hpp" +#include "autoware_utils/ros/logger_level_configure.hpp" #include "planner_manager.hpp" #include -#include -#include +#include +#include #include #include @@ -73,7 +73,7 @@ using tier4_planning_msgs::msg::LateralOffset; using tier4_planning_msgs::msg::RerouteAvailability; using visualization_msgs::msg::Marker; using visualization_msgs::msg::MarkerArray; -using DebugPublisher = autoware::universe_utils::DebugPublisher; +using DebugPublisher = autoware_utils::DebugPublisher; class BehaviorPathPlannerNode : public rclcpp::Node { @@ -88,32 +88,31 @@ class BehaviorPathPlannerNode : public rclcpp::Node private: // subscriber - autoware::universe_utils::InterProcessPollingSubscriber< - LaneletRoute, universe_utils::polling_policy::Newest> + autoware_utils::InterProcessPollingSubscriber< + LaneletRoute, autoware_utils::polling_policy::Newest> route_subscriber_{this, "~/input/route", rclcpp::QoS{1}.transient_local()}; - autoware::universe_utils::InterProcessPollingSubscriber< - LaneletMapBin, universe_utils::polling_policy::Newest> + autoware_utils::InterProcessPollingSubscriber< + LaneletMapBin, autoware_utils::polling_policy::Newest> vector_map_subscriber_{this, "~/input/vector_map", rclcpp::QoS{1}.transient_local()}; - autoware::universe_utils::InterProcessPollingSubscriber velocity_subscriber_{ + autoware_utils::InterProcessPollingSubscriber velocity_subscriber_{ this, "~/input/odometry"}; - autoware::universe_utils::InterProcessPollingSubscriber + autoware_utils::InterProcessPollingSubscriber acceleration_subscriber_{this, "~/input/accel"}; - autoware::universe_utils::InterProcessPollingSubscriber scenario_subscriber_{ + autoware_utils::InterProcessPollingSubscriber scenario_subscriber_{ this, "~/input/scenario"}; - autoware::universe_utils::InterProcessPollingSubscriber perception_subscriber_{ + autoware_utils::InterProcessPollingSubscriber perception_subscriber_{ this, "~/input/perception"}; - autoware::universe_utils::InterProcessPollingSubscriber occupancy_grid_subscriber_{ + autoware_utils::InterProcessPollingSubscriber occupancy_grid_subscriber_{ this, "~/input/occupancy_grid_map"}; - autoware::universe_utils::InterProcessPollingSubscriber costmap_subscriber_{ + autoware_utils::InterProcessPollingSubscriber costmap_subscriber_{ this, "~/input/costmap"}; - autoware::universe_utils::InterProcessPollingSubscriber - traffic_signals_subscriber_{this, "~/input/traffic_signals"}; - autoware::universe_utils::InterProcessPollingSubscriber lateral_offset_subscriber_{ + autoware_utils::InterProcessPollingSubscriber traffic_signals_subscriber_{ + this, "~/input/traffic_signals"}; + autoware_utils::InterProcessPollingSubscriber lateral_offset_subscriber_{ this, "~/input/lateral_offset"}; - autoware::universe_utils::InterProcessPollingSubscriber - operation_mode_subscriber_{ - this, "/system/operation_mode/state", rclcpp::QoS{1}.transient_local()}; - autoware::universe_utils::InterProcessPollingSubscriber + autoware_utils::InterProcessPollingSubscriber operation_mode_subscriber_{ + this, "/system/operation_mode/state", rclcpp::QoS{1}.transient_local()}; + autoware_utils::InterProcessPollingSubscriber external_limit_max_velocity_subscriber_{this, "/planning/scenario_planning/max_velocity"}; // publisher @@ -235,9 +234,9 @@ class BehaviorPathPlannerNode : public rclcpp::Node const std::shared_ptr & path_candidate_ptr, const bool is_ready, const std::shared_ptr & planner_data); - std::unique_ptr logger_configure_; + std::unique_ptr logger_configure_; - std::unique_ptr published_time_publisher_; + std::unique_ptr published_time_publisher_; }; } // namespace autoware::behavior_path_planner diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner/include/autoware/behavior_path_planner/planner_manager.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner/include/autoware/behavior_path_planner/planner_manager.hpp index a8ee2f90e5f09..3f4843c1cf33d 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner/include/autoware/behavior_path_planner/planner_manager.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner/include/autoware/behavior_path_planner/planner_manager.hpp @@ -18,10 +18,10 @@ #include "autoware/behavior_path_planner_common/interface/scene_module_interface.hpp" #include "autoware/behavior_path_planner_common/interface/scene_module_manager_interface.hpp" #include "autoware/behavior_path_planner_common/interface/scene_module_visitor.hpp" -#include "autoware/universe_utils/ros/debug_publisher.hpp" -#include "autoware/universe_utils/system/stop_watch.hpp" +#include "autoware_utils/ros/debug_publisher.hpp" +#include "autoware_utils/system/stop_watch.hpp" -#include +#include #include #include @@ -41,11 +41,11 @@ namespace autoware::behavior_path_planner { -using autoware::universe_utils::StopWatch; using autoware_internal_planning_msgs::msg::PathWithLaneId; +using autoware_utils::StopWatch; using SceneModulePtr = std::shared_ptr; using SceneModuleManagerPtr = std::shared_ptr; -using DebugPublisher = autoware::universe_utils::DebugPublisher; +using DebugPublisher = autoware_utils::DebugPublisher; using DebugDoubleMsg = autoware_internal_debug_msgs::msg::Float64Stamped; using DebugStringMsg = autoware_internal_debug_msgs::msg::StringStamped; diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner/package.xml b/planning/behavior_path_planner/autoware_behavior_path_planner/package.xml index 3b5b28101ff3a..48da34558bf4a 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner/package.xml +++ b/planning/behavior_path_planner/autoware_behavior_path_planner/package.xml @@ -52,7 +52,7 @@ autoware_planning_msgs autoware_planning_test_manager autoware_signal_processing - autoware_universe_utils + autoware_utils autoware_vehicle_info_utils autoware_vehicle_msgs behaviortree_cpp_v3 diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner/src/behavior_path_planner_node.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner/src/behavior_path_planner_node.cpp index 0ac3f17fcb36a..ef95c4fdbe90d 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner/src/behavior_path_planner_node.cpp @@ -17,7 +17,7 @@ #include "autoware/behavior_path_planner_common/utils/path_utils.hpp" #include "autoware/motion_utils/trajectory/conversion.hpp" -#include +#include #include #include @@ -129,9 +129,8 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod this, get_clock(), period_ns, std::bind(&BehaviorPathPlannerNode::run, this)); } - logger_configure_ = std::make_unique(this); - published_time_publisher_ = - std::make_unique(this); + logger_configure_ = std::make_unique(this); + published_time_publisher_ = std::make_unique(this); } std::vector BehaviorPathPlannerNode::getWaitingApprovalModules() @@ -162,7 +161,7 @@ void BehaviorPathPlannerNode::takeData() { // route { - const auto msg = route_subscriber_.takeData(); + const auto msg = route_subscriber_.take_data(); if (msg) { if (msg->segments.empty()) { RCLCPP_ERROR(get_logger(), "input route is empty. ignored"); @@ -174,7 +173,7 @@ void BehaviorPathPlannerNode::takeData() } // map { - const auto msg = vector_map_subscriber_.takeData(); + const auto msg = vector_map_subscriber_.take_data(); if (msg) { map_ptr_ = msg; has_received_map_ = true; @@ -182,70 +181,70 @@ void BehaviorPathPlannerNode::takeData() } // velocity { - const auto msg = velocity_subscriber_.takeData(); + const auto msg = velocity_subscriber_.take_data(); if (msg) { planner_data_->self_odometry = msg; } } // acceleration { - const auto msg = acceleration_subscriber_.takeData(); + const auto msg = acceleration_subscriber_.take_data(); if (msg) { planner_data_->self_acceleration = msg; } } // scenario { - const auto msg = scenario_subscriber_.takeData(); + const auto msg = scenario_subscriber_.take_data(); if (msg) { current_scenario_ = msg; } } // perception { - const auto msg = perception_subscriber_.takeData(); + const auto msg = perception_subscriber_.take_data(); if (msg) { planner_data_->dynamic_object = msg; } } // occupancy_grid { - const auto msg = occupancy_grid_subscriber_.takeData(); + const auto msg = occupancy_grid_subscriber_.take_data(); if (msg) { planner_data_->occupancy_grid = msg; } } // costmap { - const auto msg = costmap_subscriber_.takeData(); + const auto msg = costmap_subscriber_.take_data(); if (msg) { planner_data_->costmap = msg; } } // traffic_signal { - const auto msg = traffic_signals_subscriber_.takeData(); + const auto msg = traffic_signals_subscriber_.take_data(); if (msg) { onTrafficSignals(msg); } } // lateral_offset { - const auto msg = lateral_offset_subscriber_.takeData(); + const auto msg = lateral_offset_subscriber_.take_data(); if (msg) { onLateralOffset(msg); } } // operation_mode { - const auto msg = operation_mode_subscriber_.takeData(); + const auto msg = operation_mode_subscriber_.take_data(); if (msg) { planner_data_->operation_mode = msg; } } // external_velocity_limiter { - const auto msg = external_limit_max_velocity_subscriber_.takeData(); + const auto msg = external_limit_max_velocity_subscriber_.take_data(); if (msg) { planner_data_->external_limit_max_velocity = msg; } @@ -416,7 +415,7 @@ void BehaviorPathPlannerNode::run() if ( output.modified_goal && /* has changed modified goal */ ( - !planner_data_->prev_modified_goal || autoware::universe_utils::calcDistance2d( + !planner_data_->prev_modified_goal || autoware_utils::calc_distance2d( planner_data_->prev_modified_goal->pose.position, output.modified_goal->pose.position) > 0.01)) { PoseWithUuidStamped modified_goal = *(output.modified_goal); @@ -509,29 +508,27 @@ void BehaviorPathPlannerNode::publish_turn_signal_debug_data(const TurnSignalDeb constexpr double scale_x = 1.0; constexpr double scale_y = 1.0; constexpr double scale_z = 1.0; - const auto scale = autoware::universe_utils::createMarkerScale(scale_x, scale_y, scale_z); - const auto desired_section_color = - autoware::universe_utils::createMarkerColor(0.0, 1.0, 0.0, 0.999); - const auto required_section_color = - autoware::universe_utils::createMarkerColor(1.0, 0.0, 1.0, 0.999); + const auto scale = autoware_utils::create_marker_scale(scale_x, scale_y, scale_z); + const auto desired_section_color = autoware_utils::create_marker_color(0.0, 1.0, 0.0, 0.999); + const auto required_section_color = autoware_utils::create_marker_color(1.0, 0.0, 1.0, 0.999); // intersection turn signal info { const auto & turn_signal_info = debug_data.intersection_turn_signal_info; - auto desired_start_marker = autoware::universe_utils::createDefaultMarker( + auto desired_start_marker = autoware_utils::create_default_marker( "map", current_time, "intersection_turn_signal_desired_start", 0L, Marker::SPHERE, scale, desired_section_color); - auto desired_end_marker = autoware::universe_utils::createDefaultMarker( + auto desired_end_marker = autoware_utils::create_default_marker( "map", current_time, "intersection_turn_signal_desired_end", 0L, Marker::SPHERE, scale, desired_section_color); desired_start_marker.pose = turn_signal_info.desired_start_point; desired_end_marker.pose = turn_signal_info.desired_end_point; - auto required_start_marker = autoware::universe_utils::createDefaultMarker( + auto required_start_marker = autoware_utils::create_default_marker( "map", current_time, "intersection_turn_signal_required_start", 0L, Marker::SPHERE, scale, required_section_color); - auto required_end_marker = autoware::universe_utils::createDefaultMarker( + auto required_end_marker = autoware_utils::create_default_marker( "map", current_time, "intersection_turn_signal_required_end", 0L, Marker::SPHERE, scale, required_section_color); required_start_marker.pose = turn_signal_info.required_start_point; @@ -547,19 +544,19 @@ void BehaviorPathPlannerNode::publish_turn_signal_debug_data(const TurnSignalDeb { const auto & turn_signal_info = debug_data.behavior_turn_signal_info; - auto desired_start_marker = autoware::universe_utils::createDefaultMarker( + auto desired_start_marker = autoware_utils::create_default_marker( "map", current_time, "behavior_turn_signal_desired_start", 0L, Marker::CUBE, scale, desired_section_color); - auto desired_end_marker = autoware::universe_utils::createDefaultMarker( + auto desired_end_marker = autoware_utils::create_default_marker( "map", current_time, "behavior_turn_signal_desired_end", 0L, Marker::CUBE, scale, desired_section_color); desired_start_marker.pose = turn_signal_info.desired_start_point; desired_end_marker.pose = turn_signal_info.desired_end_point; - auto required_start_marker = autoware::universe_utils::createDefaultMarker( + auto required_start_marker = autoware_utils::create_default_marker( "map", current_time, "behavior_turn_signal_required_start", 0L, Marker::CUBE, scale, required_section_color); - auto required_end_marker = autoware::universe_utils::createDefaultMarker( + auto required_end_marker = autoware_utils::create_default_marker( "map", current_time, "behavior_turn_signal_required_end", 0L, Marker::CUBE, scale, required_section_color); required_start_marker.pose = turn_signal_info.required_start_point; @@ -585,18 +582,18 @@ void BehaviorPathPlannerNode::publish_bounds(const PathWithLaneId & path) constexpr double color_a = 0.999; const auto current_time = path.header.stamp; - auto left_marker = autoware::universe_utils::createDefaultMarker( + auto left_marker = autoware_utils::create_default_marker( "map", current_time, "left_bound", 0L, Marker::LINE_STRIP, - autoware::universe_utils::createMarkerScale(scale_x, scale_y, scale_z), - autoware::universe_utils::createMarkerColor(color_r, color_g, color_b, color_a)); + autoware_utils::create_marker_scale(scale_x, scale_y, scale_z), + autoware_utils::create_marker_color(color_r, color_g, color_b, color_a)); for (const auto lb : path.left_bound) { left_marker.points.push_back(lb); } - auto right_marker = autoware::universe_utils::createDefaultMarker( + auto right_marker = autoware_utils::create_default_marker( "map", current_time, "right_bound", 0L, Marker::LINE_STRIP, - autoware::universe_utils::createMarkerScale(scale_x, scale_y, scale_z), - autoware::universe_utils::createMarkerColor(color_r, color_g, color_b, color_a)); + autoware_utils::create_marker_scale(scale_x, scale_y, scale_z), + autoware_utils::create_marker_color(color_r, color_g, color_b, color_a)); for (const auto rb : path.right_bound) { right_marker.points.push_back(rb); } @@ -779,7 +776,7 @@ void BehaviorPathPlannerNode::onLateralOffset(const LateralOffset::ConstSharedPt SetParametersResult BehaviorPathPlannerNode::onSetParam( const std::vector & parameters) { - using autoware::universe_utils::updateParam; + using autoware_utils::update_param; rcl_interfaces::msg::SetParametersResult result; @@ -795,73 +792,73 @@ SetParametersResult BehaviorPathPlannerNode::onSetParam( // Drivable area expansion parameters using drivable_area_expansion::DrivableAreaExpansionParameters; const std::lock_guard lock(mutex_pd_); // for planner_data_ - updateParam( + update_param( parameters, DrivableAreaExpansionParameters::DRIVABLE_AREA_RIGHT_BOUND_OFFSET_PARAM, planner_data_->drivable_area_expansion_parameters.drivable_area_right_bound_offset); - updateParam( + update_param( parameters, DrivableAreaExpansionParameters::DRIVABLE_AREA_LEFT_BOUND_OFFSET_PARAM, planner_data_->drivable_area_expansion_parameters.drivable_area_left_bound_offset); - updateParam( + update_param( parameters, DrivableAreaExpansionParameters::DRIVABLE_AREA_TYPES_TO_SKIP_PARAM, planner_data_->drivable_area_expansion_parameters.drivable_area_types_to_skip); - updateParam( + update_param( parameters, DrivableAreaExpansionParameters::ENABLED_PARAM, planner_data_->drivable_area_expansion_parameters.enabled); - updateParam( + update_param( parameters, DrivableAreaExpansionParameters::AVOID_DYN_OBJECTS_PARAM, planner_data_->drivable_area_expansion_parameters.avoid_dynamic_objects); - updateParam( + update_param( parameters, DrivableAreaExpansionParameters::AVOID_LINESTRING_TYPES_PARAM, planner_data_->drivable_area_expansion_parameters.avoid_linestring_types); - updateParam( + update_param( parameters, DrivableAreaExpansionParameters::AVOID_LINESTRING_DIST_PARAM, planner_data_->drivable_area_expansion_parameters.avoid_linestring_dist); - updateParam( + update_param( parameters, DrivableAreaExpansionParameters::EGO_EXTRA_FRONT_OVERHANG, planner_data_->drivable_area_expansion_parameters.extra_front_overhang); - updateParam( + update_param( parameters, DrivableAreaExpansionParameters::EGO_EXTRA_WHEELBASE, planner_data_->drivable_area_expansion_parameters.extra_wheelbase); - updateParam( + update_param( parameters, DrivableAreaExpansionParameters::EGO_EXTRA_WIDTH, planner_data_->drivable_area_expansion_parameters.extra_width); - updateParam( + update_param( parameters, DrivableAreaExpansionParameters::DYN_OBJECTS_EXTRA_OFFSET_FRONT, planner_data_->drivable_area_expansion_parameters.dynamic_objects_extra_front_offset); - updateParam( + update_param( parameters, DrivableAreaExpansionParameters::DYN_OBJECTS_EXTRA_OFFSET_REAR, planner_data_->drivable_area_expansion_parameters.dynamic_objects_extra_rear_offset); - updateParam( + update_param( parameters, DrivableAreaExpansionParameters::DYN_OBJECTS_EXTRA_OFFSET_LEFT, planner_data_->drivable_area_expansion_parameters.dynamic_objects_extra_left_offset); - updateParam( + update_param( parameters, DrivableAreaExpansionParameters::DYN_OBJECTS_EXTRA_OFFSET_RIGHT, planner_data_->drivable_area_expansion_parameters.dynamic_objects_extra_right_offset); - updateParam( + update_param( parameters, DrivableAreaExpansionParameters::MAX_EXP_DIST_PARAM, planner_data_->drivable_area_expansion_parameters.max_expansion_distance); - updateParam( + update_param( parameters, DrivableAreaExpansionParameters::MAX_PATH_ARC_LENGTH_PARAM, planner_data_->drivable_area_expansion_parameters.max_path_arc_length); - updateParam( + update_param( parameters, DrivableAreaExpansionParameters::RESAMPLE_INTERVAL_PARAM, planner_data_->drivable_area_expansion_parameters.resample_interval); - updateParam( + update_param( parameters, DrivableAreaExpansionParameters::MAX_REUSE_DEVIATION_PARAM, planner_data_->drivable_area_expansion_parameters.max_reuse_deviation); - updateParam( + update_param( parameters, DrivableAreaExpansionParameters::SMOOTHING_CURVATURE_WINDOW_PARAM, planner_data_->drivable_area_expansion_parameters.curvature_average_window); - updateParam( + update_param( parameters, DrivableAreaExpansionParameters::SMOOTHING_MAX_BOUND_RATE_PARAM, planner_data_->drivable_area_expansion_parameters.max_bound_rate); - updateParam( + update_param( parameters, DrivableAreaExpansionParameters::SMOOTHING_ARC_LENGTH_RANGE_PARAM, planner_data_->drivable_area_expansion_parameters.arc_length_range); - updateParam( + update_param( parameters, DrivableAreaExpansionParameters::MIN_BOUND_INTERVAL, planner_data_->drivable_area_expansion_parameters.min_bound_interval); - updateParam( + update_param( parameters, DrivableAreaExpansionParameters::PRINT_RUNTIME_PARAM, planner_data_->drivable_area_expansion_parameters.print_runtime); } catch (const rclcpp::exceptions::InvalidParameterTypeException & e) { diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner/src/planner_manager.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner/src/planner_manager.cpp index 322209d46732b..2df6bc4bc1a5b 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner/src/planner_manager.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner/src/planner_manager.cpp @@ -17,8 +17,8 @@ #include "autoware/behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp" #include "autoware/behavior_path_planner_common/utils/path_utils.hpp" #include "autoware/behavior_path_planner_common/utils/utils.hpp" -#include "autoware/universe_utils/ros/debug_publisher.hpp" -#include "autoware/universe_utils/system/stop_watch.hpp" +#include "autoware_utils/ros/debug_publisher.hpp" +#include "autoware_utils/system/stop_watch.hpp" #include #include @@ -281,10 +281,10 @@ void PlannerManager::publishDebugRootReferencePath( { using visualization_msgs::msg::Marker; MarkerArray array; - Marker m = autoware::universe_utils::createDefaultMarker( + Marker m = autoware_utils::create_default_marker( "map", clock_.now(), "root_reference_path", 0UL, Marker::LINE_STRIP, - autoware::universe_utils::createMarkerScale(1.0, 1.0, 1.0), - autoware::universe_utils::createMarkerColor(1.0, 0.0, 0.0, 1.0)); + autoware_utils::create_marker_scale(1.0, 1.0, 1.0), + autoware_utils::create_marker_color(1.0, 0.0, 0.0, 1.0)); for (const auto & p : reference_path.path.points) m.points.push_back(p.point.pose.position); array.markers.push_back(m); m.points.clear(); diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/data_manager.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/data_manager.hpp index 9ebf5bc1d3b41..f19793ea417a3 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/data_manager.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/data_manager.hpp @@ -102,7 +102,7 @@ struct DrivableAreaInfo struct Obstacle { geometry_msgs::msg::Pose pose; - autoware::universe_utils::Polygon2d poly; + autoware_utils::Polygon2d poly; bool is_left{true}; }; std::vector drivable_lanes{}; diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/interface/scene_module_interface.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/interface/scene_module_interface.hpp index bc419e878b91f..b2b0ff6184005 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/interface/scene_module_interface.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/interface/scene_module_interface.hpp @@ -28,10 +28,10 @@ #include #include #include -#include -#include -#include -#include +#include +#include +#include +#include #include #include @@ -56,9 +56,9 @@ using autoware::objects_of_interest_marker_interface::ColorName; using autoware::objects_of_interest_marker_interface::ObjectsOfInterestMarkerInterface; using autoware::planning_factor_interface::PlanningFactorInterface; using autoware::rtc_interface::RTCInterface; -using autoware::universe_utils::calcOffsetPose; -using autoware::universe_utils::generateUUID; using autoware_internal_planning_msgs::msg::PathWithLaneId; +using autoware_utils::calc_offset_pose; +using autoware_utils::generate_uuid; using tier4_planning_msgs::msg::AvoidanceDebugMsgArray; using tier4_planning_msgs::msg::PlanningFactor; using tier4_planning_msgs::msg::SafetyFactorArray; @@ -91,10 +91,10 @@ class SceneModuleInterface objects_of_interest_marker_interface_ptr_map_( std::move(objects_of_interest_marker_interface_ptr_map)), planning_factor_interface_{planning_factor_interface}, - time_keeper_(std::make_shared()) + time_keeper_(std::make_shared()) { for (const auto & [module_name, ptr] : rtc_interface_ptr_map_) { - uuid_map_.emplace(module_name, generateUUID()); + uuid_map_.emplace(module_name, generate_uuid()); } } @@ -220,7 +220,7 @@ class SceneModuleInterface previous_module_output_ = previous_module_output; } - std::shared_ptr getTimeKeeper() const { return time_keeper_; } + std::shared_ptr getTimeKeeper() const { return time_keeper_; } /** * @brief set planner data @@ -264,7 +264,7 @@ class SceneModuleInterface const auto & base_link2front = planner_data_->parameters.base_link2front; return PoseWithDetail( - calcOffsetPose(stop_pose_.value().pose, base_link2front, 0.0, 0.0), + calc_offset_pose(stop_pose_.value().pose, base_link2front, 0.0, 0.0), stop_pose_.value().detail); } @@ -276,7 +276,7 @@ class SceneModuleInterface const auto & base_link2front = planner_data_->parameters.base_link2front; return PoseWithDetail( - calcOffsetPose(slow_pose_.value().pose, base_link2front, 0.0, 0.0), + calc_offset_pose(slow_pose_.value().pose, base_link2front, 0.0, 0.0), slow_pose_.value().detail); } @@ -288,7 +288,7 @@ class SceneModuleInterface const auto & base_link2front = planner_data_->parameters.base_link2front; return PoseWithDetail( - calcOffsetPose(dead_pose_.value().pose, base_link2front, 0.0, 0.0), + calc_offset_pose(dead_pose_.value().pose, base_link2front, 0.0, 0.0), dead_pose_.value().detail); } @@ -629,7 +629,7 @@ class SceneModuleInterface mutable MarkerArray drivable_lanes_marker_; - mutable std::shared_ptr time_keeper_; + mutable std::shared_ptr time_keeper_; }; } // namespace autoware::behavior_path_planner diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/interface/scene_module_manager_interface.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/interface/scene_module_manager_interface.hpp index 446829d1d0f9d..607f61263d1af 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/interface/scene_module_manager_interface.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/interface/scene_module_manager_interface.hpp @@ -18,7 +18,7 @@ #include "autoware/behavior_path_planner_common/data_manager.hpp" #include "autoware/behavior_path_planner_common/interface/scene_module_interface.hpp" -#include "autoware/universe_utils/ros/parameter.hpp" +#include "autoware_utils/ros/parameter.hpp" #include #include @@ -39,7 +39,7 @@ namespace autoware::behavior_path_planner using autoware::motion_utils::createDeadLineVirtualWallMarker; using autoware::motion_utils::createSlowDownVirtualWallMarker; using autoware::motion_utils::createStopVirtualWallMarker; -using autoware::universe_utils::toHexString; +using autoware_utils::to_hex_string; using unique_identifier_msgs::msg::UUID; using SceneModulePtr = std::shared_ptr; using SceneModuleObserver = std::weak_ptr; @@ -106,7 +106,7 @@ class SceneModuleManagerInterface void publishVirtualWall() const { - using autoware::universe_utils::appendMarkerArray; + using autoware_utils::append_marker_array; MarkerArray markers{}; @@ -133,12 +133,12 @@ class SceneModuleManagerInterface const auto text = m.lock()->name() + (detail.empty() ? "" : " (" + detail + ")"); const auto virtual_wall = create_virtual_wall( opt_pose.value().pose, text, rclcpp::Clock().now(), marker_id, 0.0, "", true); - appendMarkerArray(virtual_wall, &markers); + append_marker_array(virtual_wall, &markers); } } const auto module_specific_wall = m.lock()->getModuleVirtualWall(); - appendMarkerArray(module_specific_wall, &markers); + append_marker_array(module_specific_wall, &markers); m.lock()->resetWallPoses(); } @@ -148,7 +148,7 @@ class SceneModuleManagerInterface void publishMarker() const { - using autoware::universe_utils::appendMarkerArray; + using autoware_utils::append_marker_array; MarkerArray info_markers{}; MarkerArray debug_markers{}; @@ -181,9 +181,9 @@ class SceneModuleManagerInterface } if (observers_.empty() && idle_module_ptr_ != nullptr) { - appendMarkerArray(idle_module_ptr_->getInfoMarkers(), &info_markers); - appendMarkerArray(idle_module_ptr_->getDebugMarkers(), &debug_markers); - appendMarkerArray(idle_module_ptr_->getDrivableLanesMarkers(), &drivable_lanes_markers); + append_marker_array(idle_module_ptr_->getInfoMarkers(), &info_markers); + append_marker_array(idle_module_ptr_->getDebugMarkers(), &debug_markers); + append_marker_array(idle_module_ptr_->getDrivableLanesMarkers(), &drivable_lanes_markers); } pub_info_marker_->publish(info_markers); @@ -262,7 +262,7 @@ class SceneModuleManagerInterface rclcpp::Publisher::SharedPtr pub_drivable_lanes_; - rclcpp::Publisher::SharedPtr pub_processing_time_; + rclcpp::Publisher::SharedPtr pub_processing_time_; std::string name_; diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/marker_utils/colors.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/marker_utils/colors.hpp index f2318e9d5c1ad..09b0dd2b382df 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/marker_utils/colors.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/marker_utils/colors.hpp @@ -14,7 +14,7 @@ #ifndef AUTOWARE__BEHAVIOR_PATH_PLANNER_COMMON__MARKER_UTILS__COLORS_HPP_ #define AUTOWARE__BEHAVIOR_PATH_PLANNER_COMMON__MARKER_UTILS__COLORS_HPP_ -#include "autoware/universe_utils/ros/marker_helper.hpp" +#include "autoware_utils/ros/marker_helper.hpp" #include "std_msgs/msg/detail/color_rgba__struct.hpp" @@ -26,62 +26,62 @@ using std_msgs::msg::ColorRGBA; inline ColorRGBA red(float a = 0.99) { - return autoware::universe_utils::createMarkerColor(1., 0., 0., a); + return autoware_utils::create_marker_color(1., 0., 0., a); } inline ColorRGBA green(float a = 0.99) { - return autoware::universe_utils::createMarkerColor(0., 1., 0., a); + return autoware_utils::create_marker_color(0., 1., 0., a); } inline ColorRGBA blue(float a = 0.99) { - return autoware::universe_utils::createMarkerColor(0., 0., 1., a); + return autoware_utils::create_marker_color(0., 0., 1., a); } inline ColorRGBA yellow(float a = 0.99) { - return autoware::universe_utils::createMarkerColor(1., 1., 0., a); + return autoware_utils::create_marker_color(1., 1., 0., a); } inline ColorRGBA aqua(float a = 0.99) { - return autoware::universe_utils::createMarkerColor(0., 1., 1., a); + return autoware_utils::create_marker_color(0., 1., 1., a); } inline ColorRGBA magenta(float a = 0.99) { - return autoware::universe_utils::createMarkerColor(1., 0., 1., a); + return autoware_utils::create_marker_color(1., 0., 1., a); } inline ColorRGBA medium_orchid(float a = 0.99) { - return autoware::universe_utils::createMarkerColor(0.729, 0.333, 0.827, a); + return autoware_utils::create_marker_color(0.729, 0.333, 0.827, a); } inline ColorRGBA light_pink(float a = 0.99) { - return autoware::universe_utils::createMarkerColor(1., 0.713, 0.756, a); + return autoware_utils::create_marker_color(1., 0.713, 0.756, a); } inline ColorRGBA light_yellow(float a = 0.99) { - return autoware::universe_utils::createMarkerColor(1., 1., 0.878, a); + return autoware_utils::create_marker_color(1., 1., 0.878, a); } inline ColorRGBA light_steel_blue(float a = 0.99) { - return autoware::universe_utils::createMarkerColor(0.690, 0.768, 0.870, a); + return autoware_utils::create_marker_color(0.690, 0.768, 0.870, a); } inline ColorRGBA white(float a = 0.99) { - return autoware::universe_utils::createMarkerColor(1., 1., 1., a); + return autoware_utils::create_marker_color(1., 1., 1., a); } inline ColorRGBA grey(float a = 0.99) { - return autoware::universe_utils::createMarkerColor(.5, .5, .5, a); + return autoware_utils::create_marker_color(.5, .5, .5, a); } inline std::vector colors_list(float a = 0.99) diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/marker_utils/utils.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/marker_utils/utils.hpp index 8305f9268d23d..fe5ecc74a6943 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/marker_utils/utils.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/marker_utils/utils.hpp @@ -40,10 +40,10 @@ using autoware::behavior_path_planner::utils::path_safety_checker::CollisionChec using autoware::behavior_path_planner::utils::path_safety_checker::CollisionCheckDebugPair; using autoware::behavior_path_planner::utils::path_safety_checker::ExtendedPredictedObject; using autoware::behavior_path_planner::utils::path_safety_checker::ExtendedPredictedObjects; -using autoware::universe_utils::Polygon2d; using autoware_internal_planning_msgs::msg::PathWithLaneId; using autoware_perception_msgs::msg::PredictedObjects; using autoware_perception_msgs::msg::PredictedPath; +using autoware_utils::Polygon2d; using geometry_msgs::msg::Point; using geometry_msgs::msg::Polygon; using geometry_msgs::msg::Pose; diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/turn_signal_decider.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/turn_signal_decider.hpp index 067333edfcd11..5d6b1e7e50a6a 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/turn_signal_decider.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/turn_signal_decider.hpp @@ -15,14 +15,14 @@ #ifndef AUTOWARE__BEHAVIOR_PATH_PLANNER_COMMON__TURN_SIGNAL_DECIDER_HPP_ #define AUTOWARE__BEHAVIOR_PATH_PLANNER_COMMON__TURN_SIGNAL_DECIDER_HPP_ -#include "autoware/universe_utils/geometry/boost_polygon_utils.hpp" +#include "autoware_utils/geometry/boost_polygon_utils.hpp" #include #include #include -#include -#include #include +#include +#include #include #include @@ -240,8 +240,8 @@ class TurnSignalDecider const ShiftedPath & path, const ShiftLine & shift_line, const lanelet::ConstLanelets & lanes, const autoware::vehicle_info_utils::VehicleInfo & vehicle_info) const { - using autoware::universe_utils::pose2transform; - using autoware::universe_utils::transformVector; + using autoware_utils::pose2transform; + using autoware_utils::transform_vector; using boost::geometry::intersects; const auto footprint = vehicle_info.createFootprint(); @@ -250,7 +250,7 @@ class TurnSignalDecider return std::any_of(start_itr, end_itr, [&footprint, &lanes](const auto & point) { const auto transform = pose2transform(point.point.pose); - const auto shifted_vehicle_footprint = transformVector(footprint, transform); + const auto shifted_vehicle_footprint = transform_vector(footprint, transform); auto check_for_vehicle_and_bound_intersection = [&shifted_vehicle_footprint](const auto & lane) { diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/drivable_area_expansion/footprints.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/drivable_area_expansion/footprints.hpp index aea3818053171..415db5c343cbf 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/drivable_area_expansion/footprints.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/drivable_area_expansion/footprints.hpp @@ -18,7 +18,7 @@ #include "autoware/behavior_path_planner_common/utils/drivable_area_expansion/parameters.hpp" #include "autoware/behavior_path_planner_common/utils/drivable_area_expansion/types.hpp" -#include +#include #include #include diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/drivable_area_expansion/types.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/drivable_area_expansion/types.hpp index 93fa80519f183..2621a12b2fe80 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/drivable_area_expansion/types.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/drivable_area_expansion/types.hpp @@ -15,7 +15,7 @@ #ifndef AUTOWARE__BEHAVIOR_PATH_PLANNER_COMMON__UTILS__DRIVABLE_AREA_EXPANSION__TYPES_HPP_ #define AUTOWARE__BEHAVIOR_PATH_PLANNER_COMMON__UTILS__DRIVABLE_AREA_EXPANSION__TYPES_HPP_ -#include "autoware/universe_utils/geometry/boost_geometry.hpp" +#include "autoware_utils/geometry/boost_geometry.hpp" #include #include @@ -36,13 +36,13 @@ using autoware_planning_msgs::msg::PathPoint; using geometry_msgs::msg::Point; using geometry_msgs::msg::Pose; -using autoware::universe_utils::LineString2d; -using autoware::universe_utils::MultiLineString2d; -using autoware::universe_utils::MultiPoint2d; -using autoware::universe_utils::MultiPolygon2d; -using autoware::universe_utils::Point2d; -using autoware::universe_utils::Polygon2d; -using autoware::universe_utils::Segment2d; +using autoware_utils::LineString2d; +using autoware_utils::MultiLineString2d; +using autoware_utils::MultiPoint2d; +using autoware_utils::MultiPolygon2d; +using autoware_utils::Point2d; +using autoware_utils::Polygon2d; +using autoware_utils::Segment2d; using SegmentRtree = boost::geometry::index::rtree>; diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.hpp index b632185367de4..54ef9c8074b61 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.hpp @@ -15,7 +15,7 @@ #ifndef AUTOWARE__BEHAVIOR_PATH_PLANNER_COMMON__UTILS__OCCUPANCY_GRID_BASED_COLLISION_DETECTOR__OCCUPANCY_GRID_BASED_COLLISION_DETECTOR_HPP_ #define AUTOWARE__BEHAVIOR_PATH_PLANNER_COMMON__UTILS__OCCUPANCY_GRID_BASED_COLLISION_DETECTOR__OCCUPANCY_GRID_BASED_COLLISION_DETECTOR_HPP_ -#include +#include #include #include diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp index 88bd1e09af480..451bef9c420ef 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp @@ -115,7 +115,7 @@ bool isPolygonOverlapLanelet( const double yaw_threshold); bool isPolygonOverlapLanelet( - const PredictedObject & object, const autoware::universe_utils::Polygon2d & lanelet_polygon); + const PredictedObject & object, const autoware_utils::Polygon2d & lanelet_polygon); bool isPolygonOverlapLanelet( const PredictedObject & object, const lanelet::BasicPolygon2d & lanelet_polygon); diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp index 795dc6d145190..aeda0c680778b 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp @@ -15,8 +15,8 @@ #ifndef AUTOWARE__BEHAVIOR_PATH_PLANNER_COMMON__UTILS__PATH_SAFETY_CHECKER__PATH_SAFETY_CHECKER_PARAMETERS_HPP_ // NOLINT #define AUTOWARE__BEHAVIOR_PATH_PLANNER_COMMON__UTILS__PATH_SAFETY_CHECKER__PATH_SAFETY_CHECKER_PARAMETERS_HPP_ // NOLINT -#include -#include +#include +#include #include #include @@ -33,10 +33,10 @@ namespace autoware::behavior_path_planner::utils::path_safety_checker { -using autoware::universe_utils::Polygon2d; using autoware_perception_msgs::msg::ObjectClassification; using autoware_perception_msgs::msg::PredictedObject; using autoware_perception_msgs::msg::Shape; +using autoware_utils::Polygon2d; using geometry_msgs::msg::Pose; using geometry_msgs::msg::Twist; @@ -101,7 +101,7 @@ struct ExtendedPredictedObject return (max_elem != object.classification.end()) ? max_elem->label : ObjectClassification::UNKNOWN; }); - initial_polygon = autoware::universe_utils::toPolygon2d(initial_pose, shape); + initial_polygon = autoware_utils::to_polygon2d(initial_pose, shape); } }; using ExtendedPredictedObjects = std::vector; diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp index cd9a7672949f1..04f70ffbe63e5 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp @@ -18,9 +18,9 @@ #include "autoware/behavior_path_planner_common/data_manager.hpp" #include "autoware/behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" -#include -#include -#include +#include +#include +#include #include #include @@ -37,13 +37,13 @@ namespace autoware::behavior_path_planner::utils::path_safety_checker { using autoware::behavior_path_planner::utils::path_safety_checker::CollisionCheckDebug; -using autoware::universe_utils::calcYawDeviation; -using autoware::universe_utils::Point2d; -using autoware::universe_utils::Polygon2d; using autoware::vehicle_info_utils::VehicleInfo; using autoware_perception_msgs::msg::PredictedObject; using autoware_perception_msgs::msg::PredictedPath; using autoware_perception_msgs::msg::Shape; +using autoware_utils::calc_yaw_deviation; +using autoware_utils::Point2d; +using autoware_utils::Polygon2d; using geometry_msgs::msg::Pose; using geometry_msgs::msg::Twist; diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_shifter/path_shifter.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_shifter/path_shifter.hpp index f46aa8ad7dea3..5185d03d97b17 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_shifter/path_shifter.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_shifter/path_shifter.hpp @@ -15,7 +15,7 @@ #ifndef AUTOWARE__BEHAVIOR_PATH_PLANNER_COMMON__UTILS__PATH_SHIFTER__PATH_SHIFTER_HPP_ #define AUTOWARE__BEHAVIOR_PATH_PLANNER_COMMON__UTILS__PATH_SHIFTER__PATH_SHIFTER_HPP_ -#include +#include #include #include @@ -29,16 +29,16 @@ #include namespace autoware::behavior_path_planner { -using autoware::universe_utils::generateUUID; using autoware_internal_planning_msgs::msg::PathPointWithLaneId; using autoware_internal_planning_msgs::msg::PathWithLaneId; +using autoware_utils::generate_uuid; using geometry_msgs::msg::Point; using geometry_msgs::msg::Pose; using unique_identifier_msgs::msg::UUID; struct ShiftLine { - ShiftLine() : id(generateUUID()) {} + ShiftLine() : id(generate_uuid()) {} Pose start{}; // shift start point in absolute coordinate Pose end{}; // shift start point in absolute coordinate diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/utils.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/utils.hpp index 30f1ef6510cf9..bf0c024fd584d 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/utils.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/utils.hpp @@ -19,7 +19,7 @@ #include "autoware/motion_utils/trajectory/trajectory.hpp" #include -#include +#include #include #include @@ -47,10 +47,10 @@ using autoware_perception_msgs::msg::PredictedObjects; using autoware_perception_msgs::msg::PredictedPath; using autoware::route_handler::RouteHandler; -using autoware::universe_utils::LinearRing2d; -using autoware::universe_utils::Polygon2d; using autoware_internal_planning_msgs::msg::PathPointWithLaneId; using autoware_internal_planning_msgs::msg::PathWithLaneId; +using autoware_utils::LinearRing2d; +using autoware_utils::Polygon2d; using geometry_msgs::msg::Point; using geometry_msgs::msg::Pose; using geometry_msgs::msg::Vector3; @@ -178,7 +178,7 @@ bool checkCollisionWithExtraStoppingMargin( * @return Has collision or not */ bool checkCollisionBetweenPathFootprintsAndObjects( - const autoware::universe_utils::LinearRing2d & vehicle_footprint, const PathWithLaneId & ego_path, + const autoware_utils::LinearRing2d & vehicle_footprint, const PathWithLaneId & ego_path, const PredictedObjects & dynamic_objects, const double margin); /** @@ -186,7 +186,7 @@ bool checkCollisionBetweenPathFootprintsAndObjects( * @return Has collision or not */ bool checkCollisionBetweenFootprintAndObjects( - const autoware::universe_utils::LinearRing2d & vehicle_footprint, const Pose & ego_pose, + const autoware_utils::LinearRing2d & vehicle_footprint, const Pose & ego_pose, const PredictedObjects & dynamic_objects, const double margin); /** diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/package.xml b/planning/behavior_path_planner/autoware_behavior_path_planner_common/package.xml index 0a3d919ead23b..cbb513c6730a7 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/package.xml +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/package.xml @@ -57,7 +57,7 @@ autoware_route_handler autoware_rtc_interface autoware_traffic_light_utils - autoware_universe_utils + autoware_utils autoware_vehicle_info_utils geometry_msgs magic_enum diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/interface/scene_module_manager_interface.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/interface/scene_module_manager_interface.cpp index 11be3b28a7f94..a5dc98629e537 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/interface/scene_module_manager_interface.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/interface/scene_module_manager_interface.cpp @@ -23,23 +23,23 @@ namespace autoware::behavior_path_planner void SceneModuleManagerInterface::initInterface( rclcpp::Node * node, const std::vector & rtc_types) { - using autoware::universe_utils::getOrDeclareParameter; + using autoware_utils::get_or_declare_parameter; // init manager configuration { std::string ns = name_ + "."; try { - config_.enable_rtc = getOrDeclareParameter(*node, "enable_all_modules_auto_mode") + config_.enable_rtc = get_or_declare_parameter(*node, "enable_all_modules_auto_mode") ? false - : getOrDeclareParameter(*node, ns + "enable_rtc"); + : get_or_declare_parameter(*node, ns + "enable_rtc"); } catch (const std::exception & e) { - config_.enable_rtc = getOrDeclareParameter(*node, ns + "enable_rtc"); + config_.enable_rtc = get_or_declare_parameter(*node, ns + "enable_rtc"); } - config_.enable_simultaneous_execution_as_approved_module = - getOrDeclareParameter(*node, ns + "enable_simultaneous_execution_as_approved_module"); - config_.enable_simultaneous_execution_as_candidate_module = - getOrDeclareParameter(*node, ns + "enable_simultaneous_execution_as_candidate_module"); + config_.enable_simultaneous_execution_as_approved_module = get_or_declare_parameter( + *node, ns + "enable_simultaneous_execution_as_approved_module"); + config_.enable_simultaneous_execution_as_candidate_module = get_or_declare_parameter( + *node, ns + "enable_simultaneous_execution_as_candidate_module"); } // init rtc configuration @@ -59,7 +59,7 @@ void SceneModuleManagerInterface::initInterface( pub_debug_marker_ = node->create_publisher("~/debug/" + name_, 20); pub_virtual_wall_ = node->create_publisher("~/virtual_wall/" + name_, 20); pub_drivable_lanes_ = node->create_publisher("~/drivable_lanes/" + name_, 20); - pub_processing_time_ = node->create_publisher( + pub_processing_time_ = node->create_publisher( "~/processing_time/" + name_, 20); } diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/marker_utils/utils.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/marker_utils/utils.cpp index 7029c15e45b6e..b0bb72e3994a3 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/marker_utils/utils.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/marker_utils/utils.cpp @@ -18,9 +18,9 @@ #include "autoware/behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" #include "autoware/behavior_path_planner_common/utils/path_utils.hpp" -#include -#include -#include +#include +#include +#include #include #include @@ -33,12 +33,12 @@ namespace marker_utils { using autoware::behavior_path_planner::utils::calcPathArcLengthArray; -using autoware::universe_utils::calcOffsetPose; -using autoware::universe_utils::createDefaultMarker; -using autoware::universe_utils::createMarkerColor; -using autoware::universe_utils::createMarkerOrientation; -using autoware::universe_utils::createMarkerScale; -using autoware::universe_utils::createPoint; +using autoware_utils::calc_offset_pose; +using autoware_utils::create_default_marker; +using autoware_utils::create_marker_color; +using autoware_utils::create_marker_orientation; +using autoware_utils::create_marker_scale; +using autoware_utils::create_point; using std_msgs::msg::ColorRGBA; using visualization_msgs::msg::Marker; @@ -51,13 +51,13 @@ void addFootprintMarker( const double base_to_rear = vehicle_info.rear_overhang_m; marker.points.push_back( - autoware::universe_utils::calcOffsetPose(pose, base_to_front, -half_width, 0.0).position); + autoware_utils::calc_offset_pose(pose, base_to_front, -half_width, 0.0).position); marker.points.push_back( - autoware::universe_utils::calcOffsetPose(pose, base_to_front, half_width, 0.0).position); + autoware_utils::calc_offset_pose(pose, base_to_front, half_width, 0.0).position); marker.points.push_back( - autoware::universe_utils::calcOffsetPose(pose, -base_to_rear, half_width, 0.0).position); + autoware_utils::calc_offset_pose(pose, -base_to_rear, half_width, 0.0).position); marker.points.push_back( - autoware::universe_utils::calcOffsetPose(pose, -base_to_rear, -half_width, 0.0).position); + autoware_utils::calc_offset_pose(pose, -base_to_rear, -half_width, 0.0).position); marker.points.push_back(marker.points.front()); } @@ -68,9 +68,9 @@ MarkerArray createFootprintMarkerArray( const auto current_time = rclcpp::Clock{RCL_ROS_TIME}.now(); MarkerArray msg; - Marker marker = createDefaultMarker( - "map", current_time, ns, id, Marker::LINE_STRIP, createMarkerScale(0.2, 0.2, 0.2), - createMarkerColor(r, g, b, 0.999)); + Marker marker = create_default_marker( + "map", current_time, ns, id, Marker::LINE_STRIP, create_marker_scale(0.2, 0.2, 0.2), + create_marker_color(r, g, b, 0.999)); marker.lifetime = rclcpp::Duration::from_seconds(1.5); MarkerArray marker_array; @@ -84,9 +84,9 @@ MarkerArray createPointsMarkerArray( const std::vector & points, const std::string & ns, const int32_t id, const float r, const float g, const float b) { - auto marker = createDefaultMarker( + auto marker = create_default_marker( "map", rclcpp::Clock{RCL_ROS_TIME}.now(), ns, id, Marker::LINE_STRIP, - createMarkerScale(0.1, 0.0, 0.0), createMarkerColor(r, g, b, 0.999)); + create_marker_scale(0.1, 0.0, 0.0), create_marker_color(r, g, b, 0.999)); for (const auto & point : points) { marker.points.push_back(point); @@ -103,9 +103,9 @@ MarkerArray createPoseMarkerArray( { MarkerArray msg; - Marker marker = createDefaultMarker( + Marker marker = create_default_marker( "map", rclcpp::Clock{RCL_ROS_TIME}.now(), ns, id, Marker::ARROW, - createMarkerScale(0.7, 0.3, 0.3), createMarkerColor(r, g, b, 0.999)); + create_marker_scale(0.7, 0.3, 0.3), create_marker_color(r, g, b, 0.999)); marker.pose = pose; msg.markers.push_back(marker); @@ -126,13 +126,13 @@ MarkerArray createPathMarkerArray( const auto reserve_size = path.points.size() + static_cast(path.points.size() / 10); msg.markers.reserve(reserve_size); - Marker marker = createDefaultMarker( - "map", current_time, ns, 0L, Marker::ARROW, createMarkerScale(0.2, 0.1, 0.3), - createMarkerColor(r, g, b, 0.999)); + Marker marker = create_default_marker( + "map", current_time, ns, 0L, Marker::ARROW, create_marker_scale(0.2, 0.1, 0.3), + create_marker_color(r, g, b, 0.999)); - Marker marker_text = createDefaultMarker( - "map", current_time, ns, 0L, Marker::TEXT_VIEW_FACING, createMarkerScale(0.2, 0.1, 0.3), - createMarkerColor(1, 1, 1, 0.999)); + Marker marker_text = create_default_marker( + "map", current_time, ns, 0L, Marker::TEXT_VIEW_FACING, create_marker_scale(0.2, 0.1, 0.3), + create_marker_color(1, 1, 1, 0.999)); for (const auto & p : path.points) { marker.id = uid + i++; @@ -168,30 +168,30 @@ MarkerArray createShiftLineMarkerArray( double current_shift = base_shift; for (const auto & sp : shift_lines_local) { // ROS_ERROR("sp: s = (%f, %f), g = (%f, %f)", sp.start.x, sp.start.y, sp.end.x, sp.end.y); - Marker basic_marker = createDefaultMarker( - "map", current_time, ns, 0L, Marker::CUBE, createMarkerScale(0.5, 0.5, 0.5), - createMarkerColor(r, g, b, 0.5)); - basic_marker.pose.orientation = createMarkerOrientation(0, 0, 0, 1.0); + Marker basic_marker = create_default_marker( + "map", current_time, ns, 0L, Marker::CUBE, create_marker_scale(0.5, 0.5, 0.5), + create_marker_color(r, g, b, 0.5)); + basic_marker.pose.orientation = create_marker_orientation(0, 0, 0, 1.0); { // start point auto marker_s = basic_marker; marker_s.id = ++id; marker_s.pose = sp.start; - marker_s.pose = calcOffsetPose(marker_s.pose, 0.0, current_shift, 0.0); + marker_s.pose = calc_offset_pose(marker_s.pose, 0.0, current_shift, 0.0); msg.markers.push_back(marker_s); // end point auto marker_e = basic_marker; marker_e.id = ++id; marker_e.pose = sp.end; - marker_e.pose = calcOffsetPose(marker_e.pose, 0.0, sp.end_shift_length, 0.0); + marker_e.pose = calc_offset_pose(marker_e.pose, 0.0, sp.end_shift_length, 0.0); msg.markers.push_back(marker_e); // start-to-end line auto marker_l = basic_marker; marker_l.id = ++id; marker_l.type = Marker::LINE_STRIP; - marker_l.scale = createMarkerScale(w, 0.0, 0.0); + marker_l.scale = create_marker_scale(w, 0.0, 0.0); marker_l.points.reserve(2); marker_l.points.push_back(marker_s.pose.position); marker_l.points.push_back(marker_e.pose.position); @@ -213,15 +213,15 @@ MarkerArray createShiftLengthMarkerArray( MarkerArray msg; - Marker marker = createDefaultMarker( + Marker marker = create_default_marker( "map", rclcpp::Clock{RCL_ROS_TIME}.now(), ns, 0L, Marker::LINE_STRIP, - createMarkerScale(0.1, 0.0, 0.0), createMarkerColor(r, g, b, 0.9)); - marker.pose.orientation = autoware::universe_utils::createMarkerOrientation(0, 0, 0, 1.0); + create_marker_scale(0.1, 0.0, 0.0), create_marker_color(r, g, b, 0.9)); + marker.pose.orientation = autoware_utils::create_marker_orientation(0, 0, 0, 1.0); marker.points.reserve(shift_distance.size()); for (size_t i = 0; i < shift_distance.size(); ++i) { const auto p = - calcOffsetPose(reference.points.at(i).point.pose, 0.0, shift_distance.at(i), 0.0); + calc_offset_pose(reference.points.at(i).point.pose, 0.0, shift_distance.at(i), 0.0); marker.points.push_back(p.position); } @@ -244,15 +244,16 @@ MarkerArray createShiftGradMarkerArray( MarkerArray msg; - Marker marker = createDefaultMarker( + Marker marker = create_default_marker( "map", rclcpp::Clock{RCL_ROS_TIME}.now(), ns, 0L, Marker::TEXT_VIEW_FACING, - createMarkerScale(0.1, 0.1, 0.1), createMarkerColor(r, g, b, 0.9)); + create_marker_scale(0.1, 0.1, 0.1), create_marker_color(r, g, b, 0.9)); for (size_t i = 0; i < grad.size(); ++i) { std::ostringstream string_stream; string_stream << "Grad:" << grad.at(i); marker.text = string_stream.str(); - marker.pose = calcOffsetPose(reference.points.at(i).point.pose, 0.0, shift_distance.at(i), 0.0); + marker.pose = + calc_offset_pose(reference.points.at(i).point.pose, 0.0, shift_distance.at(i), 0.0); marker.id = i; msg.markers.push_back(marker); @@ -269,18 +270,18 @@ MarkerArray createLaneletsAreaMarkerArray( MarkerArray msg; for (const auto & lanelet : lanelets) { - Marker marker = createDefaultMarker( + Marker marker = create_default_marker( "map", current_time, ns, static_cast(lanelet.id()), Marker::LINE_STRIP, - createMarkerScale(0.1, 0.0, 0.0), createMarkerColor(r, g, b, 0.999)); - marker.pose.orientation = autoware::universe_utils::createMarkerOrientation(0, 0, 0, 1.0); + create_marker_scale(0.1, 0.0, 0.0), create_marker_color(r, g, b, 0.999)); + marker.pose.orientation = autoware_utils::create_marker_orientation(0, 0, 0, 1.0); if (!lanelet.polygon3d().empty()) { marker.points.reserve(lanelet.polygon3d().size() + 1); } for (const auto & p : lanelet.polygon3d()) { - marker.points.push_back(createPoint(p.x(), p.y(), p.z())); + marker.points.push_back(create_point(p.x(), p.y(), p.z())); } if (!marker.points.empty()) { @@ -297,18 +298,18 @@ MarkerArray createPolygonMarkerArray( const Polygon & polygon, std::string && ns, const int64_t & lane_id, const float & r, const float & g, const float & b, const float & w) { - Marker marker = createDefaultMarker( + Marker marker = create_default_marker( "map", rclcpp::Clock{RCL_ROS_TIME}.now(), ns, static_cast(lane_id), Marker::LINE_STRIP, - createMarkerScale(w, 0.0, 0.0), createMarkerColor(r, g, b, 0.8)); + create_marker_scale(w, 0.0, 0.0), create_marker_color(r, g, b, 0.8)); - marker.pose.orientation = autoware::universe_utils::createMarkerOrientation(0, 0, 0, 1.0); + marker.pose.orientation = autoware_utils::create_marker_orientation(0, 0, 0, 1.0); if (!polygon.points.empty()) { marker.points.reserve(polygon.points.size() + 1); } for (const auto & p : polygon.points) { - marker.points.push_back(createPoint(p.x, p.y, p.z)); + marker.points.push_back(create_point(p.x, p.y, p.z)); } if (!marker.points.empty()) { marker.points.push_back(marker.points.front()); @@ -324,9 +325,9 @@ MarkerArray createObjectsMarkerArray( const PredictedObjects & objects, std::string && ns, const int64_t & lane_id, const float & r, const float & g, const float & b) { - Marker marker = createDefaultMarker( + Marker marker = create_default_marker( "map", rclcpp::Clock{RCL_ROS_TIME}.now(), ns, 0L, Marker::CUBE, - createMarkerScale(3.0, 1.0, 1.0), createMarkerColor(r, g, b, 0.8)); + create_marker_scale(3.0, 1.0, 1.0), create_marker_color(r, g, b, 0.8)); int32_t uid = bitShift(lane_id); int32_t i{0}; @@ -352,9 +353,10 @@ MarkerArray createDrivableLanesMarkerArray( const auto get_lanelet_marker = [&ns, &i](const auto & lanelet, const auto r, const auto g, const auto b) { - auto marker = createDefaultMarker( + auto marker = create_default_marker( "map", rclcpp::Clock{RCL_ROS_TIME}.now(), ns, bitShift(lanelet.id()) + i++, - Marker::LINE_STRIP, createMarkerScale(0.1, 0.0, 0.0), createMarkerColor(r, g, b, 0.999)); + Marker::LINE_STRIP, create_marker_scale(0.1, 0.0, 0.0), + create_marker_color(r, g, b, 0.999)); if (lanelet.polygon3d().empty()) { return marker; @@ -363,7 +365,7 @@ MarkerArray createDrivableLanesMarkerArray( marker.points.reserve(lanelet.polygon3d().size() + 1); for (const auto & p : lanelet.polygon3d()) { - marker.points.push_back(createPoint(p.x(), p.y(), p.z())); + marker.points.push_back(create_point(p.x(), p.y(), p.z())); } marker.points.push_back(marker.points.front()); @@ -403,9 +405,9 @@ MarkerArray createPredictedPathMarkerArray( const auto & path = predicted_path.path; - Marker marker = createDefaultMarker( - "map", current_time, ns, id, Marker::LINE_STRIP, createMarkerScale(0.1, 0.1, 0.1), - createMarkerColor(r, g, b, 0.999)); + Marker marker = create_default_marker( + "map", current_time, ns, id, Marker::LINE_STRIP, create_marker_scale(0.1, 0.1, 0.1), + create_marker_color(r, g, b, 0.999)); marker.lifetime = rclcpp::Duration::from_seconds(1.5); MarkerArray marker_array; @@ -431,26 +433,29 @@ MarkerArray showPolygon(const CollisionCheckDebugMap & obj_debug_vec, std::strin const auto now = rclcpp::Clock{RCL_ROS_TIME}.now(); constexpr float line_scale_val{0.2}; - const auto line_marker_scale = createMarkerScale(line_scale_val, line_scale_val, line_scale_val); + const auto line_marker_scale = + create_marker_scale(line_scale_val, line_scale_val, line_scale_val); auto default_line_marker = [&](const auto & color = colors::green()) { - return createDefaultMarker("map", now, ns, ++id, Marker::LINE_STRIP, line_marker_scale, color); + return create_default_marker( + "map", now, ns, ++id, Marker::LINE_STRIP, line_marker_scale, color); }; constexpr float text_scale_val{1.5}; - const auto text_marker_scale = createMarkerScale(text_scale_val, text_scale_val, text_scale_val); + const auto text_marker_scale = + create_marker_scale(text_scale_val, text_scale_val, text_scale_val); auto default_text_marker = [&]() { - return createDefaultMarker( + return create_default_marker( "map", now, ns + "_text", ++id, visualization_msgs::msg::Marker::TEXT_VIEW_FACING, text_marker_scale, colors::white()); }; auto default_cube_marker = [&](const auto & width, const auto & depth, const auto & color = colors::green()) { - return createDefaultMarker( + return create_default_marker( "map", now, ns + "_cube", ++id, visualization_msgs::msg::Marker::CUBE, - createMarkerScale(width, depth, 1.0), color); + create_marker_scale(width, depth, 1.0), color); }; MarkerArray marker_array; @@ -468,7 +473,7 @@ MarkerArray showPolygon(const CollisionCheckDebugMap & obj_debug_vec, std::strin polygon_marker = default_line_marker(color); polygon_marker.points.reserve(polygon.outer().size()); for (const auto & p : polygon.outer()) { - polygon_marker.points.push_back(createPoint(p.x(), p.y(), poly_z)); + polygon_marker.points.push_back(create_point(p.x(), p.y(), poly_z)); } }; @@ -502,9 +507,9 @@ MarkerArray showPredictedPath(const CollisionCheckDebugMap & obj_debug_vec, std: { int32_t id{0}; const auto current_time{rclcpp::Clock{RCL_ROS_TIME}.now()}; - const auto arrow_marker_scale = createMarkerScale(1.0, 0.3, 0.3); + const auto arrow_marker_scale = create_marker_scale(1.0, 0.3, 0.3); const auto default_arrow_marker = [&](const auto & color) { - return createDefaultMarker( + return create_default_marker( "map", current_time, ns, ++id, Marker::ARROW, arrow_marker_scale, color); }; @@ -547,9 +552,9 @@ MarkerArray showSafetyCheckInfo(const CollisionCheckDebugMap & obj_debug_vec, st { int32_t id{0}; auto default_text_marker = [&]() { - return createDefaultMarker( + return create_default_marker( "map", rclcpp::Clock{RCL_ROS_TIME}.now(), ns, ++id, Marker::TEXT_VIEW_FACING, - createMarkerScale(0.5, 0.5, 0.5), colors::aqua()); + create_marker_scale(0.5, 0.5, 0.5), colors::aqua()); }; MarkerArray marker_array; @@ -593,9 +598,9 @@ MarkerArray showFilteredObjects( auto default_cube_marker = [&](const auto & width, const auto & depth, const auto & color = colors::green()) { - return createDefaultMarker( + return create_default_marker( "map", now, ns + "_cube", ++id, visualization_msgs::msg::Marker::CUBE, - createMarkerScale(width, depth, 1.0), color); + create_marker_scale(width, depth, 1.0), color); }; MarkerArray marker_array; diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/turn_signal_decider.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/turn_signal_decider.cpp index d513017c221cf..bbe4e5b9dbbd7 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/turn_signal_decider.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/turn_signal_decider.cpp @@ -20,11 +20,11 @@ #include #include #include -#include -#include -#include #include #include +#include +#include +#include #include #include @@ -600,9 +600,8 @@ geometry_msgs::msg::Pose TurnSignalDecider::get_required_end_point( const double terminal_yaw = tf2::getYaw(resampled_centerline.back().orientation); for (size_t i = 0; i < resampled_centerline.size(); ++i) { const double yaw = tf2::getYaw(resampled_centerline.at(i).orientation); - const double yaw_diff = autoware::universe_utils::normalizeRadian(yaw - terminal_yaw); - if ( - std::fabs(yaw_diff) < autoware::universe_utils::deg2rad(intersection_angle_threshold_deg_)) { + const double yaw_diff = autoware_utils::normalize_radian(yaw - terminal_yaw); + if (std::fabs(yaw_diff) < autoware_utils::deg2rad(intersection_angle_threshold_deg_)) { return resampled_centerline.at(i); } } @@ -659,9 +658,9 @@ void TurnSignalDecider::initialize_intersection_info() geometry_msgs::msg::Quaternion TurnSignalDecider::calc_orientation( const Point & src_point, const Point & dst_point) { - const double pitch = autoware::universe_utils::calcElevationAngle(src_point, dst_point); - const double yaw = autoware::universe_utils::calcAzimuthAngle(src_point, dst_point); - return autoware::universe_utils::createQuaternionFromRPY(0.0, pitch, yaw); + const double pitch = autoware_utils::calc_elevation_angle(src_point, dst_point); + const double yaw = autoware_utils::calc_azimuth_angle(src_point, dst_point); + return autoware_utils::create_quaternion_from_rpy(0.0, pitch, yaw); } std::pair TurnSignalDecider::getBehaviorTurnSignalInfo( @@ -673,7 +672,7 @@ std::pair TurnSignalDecider::getBehaviorTurnSignalInfo( const bool override_ego_stopped_check, const bool is_pull_out, const bool is_lane_change, const bool is_pull_over) const { - using autoware::universe_utils::getPose; + using autoware_utils::get_pose; const auto & p = parameters; const auto & rh = route_handler; @@ -727,8 +726,8 @@ std::pair TurnSignalDecider::getBehaviorTurnSignalInfo( const auto relative_shift_length = end_shift_length - start_shift_length; - const auto p_path_start = getPose(path.path.points.front()); - const auto p_path_end = getPose(path.path.points.back()); + const auto p_path_start = get_pose(path.path.points.front()); + const auto p_path_end = get_pose(path.path.points.back()); // If shift length is shorter than the threshold, it does not need to turn on blinkers if (std::fabs(relative_shift_length) < p.turn_signal_shift_length_threshold) { diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/drivable_area_expansion/drivable_area_expansion.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/drivable_area_expansion/drivable_area_expansion.cpp index 1076af002fb28..59ddbc297c511 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/drivable_area_expansion/drivable_area_expansion.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/drivable_area_expansion/drivable_area_expansion.cpp @@ -24,9 +24,9 @@ #include #include #include -#include -#include -#include +#include +#include +#include #include @@ -57,8 +57,8 @@ void reuse_previous_poses( const auto ego_is_behind = prev_poses.size() > 1 && autoware::motion_utils::calcLongitudinalOffsetToSegment(prev_poses, 0, ego_point) < 0.0; - const auto ego_is_far = !prev_poses.empty() && autoware::universe_utils::calcDistance2d( - ego_point, prev_poses.front()) < 0.0; + const auto ego_is_far = + !prev_poses.empty() && autoware_utils::calc_distance2d(ego_point, prev_poses.front()) < 0.0; // make sure the reused points are not behind the current original drivable area LineString2d left_bound; LineString2d right_bound; @@ -170,7 +170,7 @@ void apply_arc_length_range_smoothing( auto arc_length = boost::geometry::distance( bound_projections[path_idx].point, convert_point(bound[bound_idx + 1])); const auto update_arc_length_and_bound_expansions = [&](auto idx) { - arc_length += autoware::universe_utils::calcDistance2d(bound[idx - 1], bound[idx]); + arc_length += autoware_utils::calc_distance2d(bound[idx - 1], bound[idx]); bound_expansions[idx] = std::max(bound_expansions[idx], original_expansions[bound_idx]); }; for (auto up_bound_idx = bound_idx + 2; up_bound_idx < bound.size(); ++up_bound_idx) { @@ -209,7 +209,7 @@ void apply_bound_change_rate_limit( if (distances.empty()) return; const auto apply_max_vel = [&](auto & exp, const auto from, const auto to) { if (exp[from] > exp[to]) { - const auto arc_length = autoware::universe_utils::calcDistance2d(bound[from], bound[to]); + const auto arc_length = autoware_utils::calc_distance2d(bound[from], bound[to]); const auto smoothed_dist = exp[from] - arc_length * max_rate; exp[to] = std::max(exp[to], smoothed_dist); } @@ -299,12 +299,11 @@ void expand_bound( for (auto idx = 1LU; idx < bound.size(); ++idx) { bool is_intersecting = false; for (auto succ_idx = idx + 1; succ_idx < bound.size(); ++succ_idx) { - const auto intersection = autoware::universe_utils::intersect( - bound[idx - 1], bound[idx], bound[succ_idx - 1], bound[succ_idx]); + const auto intersection = + autoware_utils::intersect(bound[idx - 1], bound[idx], bound[succ_idx - 1], bound[succ_idx]); if ( - intersection && - autoware::universe_utils::calcDistance2d(*intersection, bound[idx - 1]) > 1e-3 && - autoware::universe_utils::calcDistance2d(*intersection, bound[idx]) > 1e-3) { + intersection && autoware_utils::calc_distance2d(*intersection, bound[idx - 1]) > 1e-3 && + autoware_utils::calc_distance2d(*intersection, bound[idx]) > 1e-3) { idx = succ_idx; is_intersecting = true; } @@ -392,8 +391,8 @@ void add_bound_point(std::vector & bound, const Pose & pose, const double new_point.y = nearest_projection.point.y(); new_point.z = bound[nearest_idx].z; if ( - universe_utils::calcDistance2d(new_point, bound[nearest_idx]) > min_bound_interval && - universe_utils::calcDistance2d(new_point, bound[nearest_idx + 1]) > min_bound_interval) { + autoware_utils::calc_distance2d(new_point, bound[nearest_idx]) > min_bound_interval && + autoware_utils::calc_distance2d(new_point, bound[nearest_idx + 1]) > min_bound_interval) { bound.insert(bound.begin() + nearest_idx + 1, new_point); } } @@ -414,7 +413,7 @@ void expand_drivable_area( { // skip if no bounds or not enough points to calculate path curvature if (path.points.size() < 3 || path.left_bound.empty() || path.right_bound.empty()) return; - autoware::universe_utils::StopWatch stop_watch; + autoware_utils::StopWatch stop_watch; stop_watch.tic("overall"); stop_watch.tic("preprocessing"); const auto & params = planner_data->drivable_area_expansion_parameters; diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/drivable_area_expansion/footprints.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/drivable_area_expansion/footprints.cpp index 10356150fc689..6aadd37dda8f5 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/drivable_area_expansion/footprints.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/drivable_area_expansion/footprints.cpp @@ -16,8 +16,8 @@ #include "autoware/behavior_path_planner_common/utils/drivable_area_expansion/parameters.hpp" -#include -#include +#include +#include #include @@ -37,8 +37,7 @@ Polygon2d create_footprint(const geometry_msgs::msg::Pose & pose, const Polygon2 { const auto angle = tf2::getYaw(pose.orientation); return translate_polygon( - autoware::universe_utils::rotatePolygon(base_footprint, angle), pose.position.x, - pose.position.y); + autoware_utils::rotate_polygon(base_footprint, angle), pose.position.x, pose.position.y); } MultiPolygon2d create_object_footprints( diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/drivable_area_expansion/static_drivable_area.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/drivable_area_expansion/static_drivable_area.cpp index 218d94316bfd8..eba505f1f9cd8 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/drivable_area_expansion/static_drivable_area.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/drivable_area_expansion/static_drivable_area.cpp @@ -17,11 +17,11 @@ #include "autoware/motion_utils/trajectory/trajectory.hpp" #include -#include -#include #include #include #include +#include +#include #include @@ -63,8 +63,8 @@ std::vector removeSharpPoints(const std::vector & points) const auto product = std::inner_product(vec_1to2.begin(), vec_1to2.end(), vec_3to2.begin(), 0.0); - const auto dist_1to2 = autoware::universe_utils::calcDistance3d(p1, p2); - const auto dist_3to2 = autoware::universe_utils::calcDistance3d(p3, p2); + const auto dist_1to2 = autoware_utils::calc_distance3d(p1, p2); + const auto dist_3to2 = autoware_utils::calc_distance3d(p3, p2); constexpr double epsilon = 1e-3; @@ -90,22 +90,22 @@ template size_t findNearestSegmentIndexFromLateralDistance( const std::vector & points, const geometry_msgs::msg::Point & target_point) { - using autoware::universe_utils::calcAzimuthAngle; - using autoware::universe_utils::calcDistance2d; - using autoware::universe_utils::normalizeRadian; + using autoware_utils::calc_azimuth_angle; + using autoware_utils::calc_distance2d; + using autoware_utils::normalize_radian; std::optional closest_idx{std::nullopt}; double min_lateral_dist = std::numeric_limits::max(); for (size_t seg_idx = 0; seg_idx < points.size() - 1; ++seg_idx) { const double lon_dist = autoware::motion_utils::calcLongitudinalOffsetToSegment(points, seg_idx, target_point); - const double segment_length = calcDistance2d(points.at(seg_idx), points.at(seg_idx + 1)); + const double segment_length = calc_distance2d(points.at(seg_idx), points.at(seg_idx + 1)); const double lat_dist = [&]() { if (lon_dist < 0.0) { - return calcDistance2d(points.at(seg_idx), target_point); + return calc_distance2d(points.at(seg_idx), target_point); } if (segment_length < lon_dist) { - return calcDistance2d(points.at(seg_idx + 1), target_point); + return calc_distance2d(points.at(seg_idx + 1), target_point); } return std::abs(autoware::motion_utils::calcLateralOffset(points, target_point, seg_idx)); }(); @@ -127,28 +127,28 @@ size_t findNearestSegmentIndexFromLateralDistance( const std::vector & points, const geometry_msgs::msg::Pose & target_point, const double yaw_threshold) { - using autoware::universe_utils::calcAzimuthAngle; - using autoware::universe_utils::calcDistance2d; - using autoware::universe_utils::normalizeRadian; + using autoware_utils::calc_azimuth_angle; + using autoware_utils::calc_distance2d; + using autoware_utils::normalize_radian; std::optional closest_idx{std::nullopt}; double min_lateral_dist = std::numeric_limits::max(); for (size_t seg_idx = 0; seg_idx < points.size() - 1; ++seg_idx) { const auto base_yaw = tf2::getYaw(target_point.orientation); const auto yaw = - normalizeRadian(calcAzimuthAngle(points.at(seg_idx), points.at(seg_idx + 1)) - base_yaw); + normalize_radian(calc_azimuth_angle(points.at(seg_idx), points.at(seg_idx + 1)) - base_yaw); if (yaw_threshold < std::abs(yaw)) { continue; } const double lon_dist = autoware::motion_utils::calcLongitudinalOffsetToSegment( points, seg_idx, target_point.position); - const double segment_length = calcDistance2d(points.at(seg_idx), points.at(seg_idx + 1)); + const double segment_length = calc_distance2d(points.at(seg_idx), points.at(seg_idx + 1)); const double lat_dist = [&]() { if (lon_dist < 0.0) { - return calcDistance2d(points.at(seg_idx), target_point.position); + return calc_distance2d(points.at(seg_idx), target_point.position); } if (segment_length < lon_dist) { - return calcDistance2d(points.at(seg_idx + 1), target_point.position); + return calc_distance2d(points.at(seg_idx + 1), target_point.position); } return std::abs( autoware::motion_utils::calcLateralOffset(points, target_point.position, seg_idx)); @@ -253,8 +253,7 @@ std::optional> intersectBound( const size_t end_idx = static_cast(std::min( static_cast(bound.size()) - 1, static_cast(std::max(seg_idx1, seg_idx2)) + 1 + 5)); for (int i = start_idx; i < static_cast(end_idx); ++i) { - const auto intersect_point = - autoware::universe_utils::intersect(p1, p2, bound.at(i), bound.at(i + 1)); + const auto intersect_point = autoware_utils::intersect(p1, p2, bound.at(i), bound.at(i + 1)); if (intersect_point) { std::pair result; result.first = static_cast(i); @@ -270,20 +269,20 @@ double calcSquaredDistanceFromPointToSegment( const geometry_msgs::msg::Point & segment_end_point, const geometry_msgs::msg::Point & target_point) { - using autoware::universe_utils::calcSquaredDistance2d; + using autoware_utils::calc_squared_distance2d; const auto & a = segment_start_point; const auto & b = segment_end_point; const auto & p = target_point; const double dot_val = (b.x - a.x) * (p.x - a.x) + (b.y - a.y) * (p.y - a.y); - const double squared_segment_length = calcSquaredDistance2d(a, b); + const double squared_segment_length = calc_squared_distance2d(a, b); if (0 <= dot_val && dot_val <= squared_segment_length) { - return calcSquaredDistance2d(p, a) - dot_val * dot_val / squared_segment_length; + return calc_squared_distance2d(p, a) - dot_val * dot_val / squared_segment_length; } // target_point is outside the segment. - return std::min(calcSquaredDistance2d(a, p), calcSquaredDistance2d(b, p)); + return std::min(calc_squared_distance2d(a, p), calc_squared_distance2d(b, p)); } PolygonPoint transformBoundFrenetCoordinate( @@ -445,7 +444,7 @@ std::vector concatenateTwoPolygons( double min_dist_to_intersection = std::numeric_limits::max(); PolygonPoint closest_intersect_point; for (size_t i = 0; i < get_in_poly().size() - 1; ++i) { - const auto intersection = autoware::universe_utils::intersect( + const auto intersection = autoware_utils::intersect( get_out_poly().at(curr_idx).point, get_out_poly().at(next_idx).point, get_in_poly().at(i).point, get_in_poly().at(i + 1).point); if (!intersection) { @@ -462,7 +461,7 @@ std::vector concatenateTwoPolygons( const auto intersect_point = PolygonPoint{*intersection, 0, 0.0, 0.0}; const double dist_to_intersection = - autoware::universe_utils::calcDistance2d(get_out_poly().at(curr_idx).point, *intersection); + autoware_utils::calc_distance2d(get_out_poly().at(curr_idx).point, *intersection); if (dist_to_intersection < min_dist_to_intersection) { closest_idx = i; min_dist_to_intersection = dist_to_intersection; @@ -842,7 +841,7 @@ void generateDrivableArea( PathWithLaneId & path, const double vehicle_length, const double offset, const bool is_driving_forward) { - using autoware::universe_utils::calcOffsetPose; + using autoware_utils::calc_offset_pose; // remove path points which is close to the previous point PathWithLaneId resampled_path{}; @@ -866,7 +865,7 @@ void generateDrivableArea( // add last point of path if enough far from the one of resampled path constexpr double th_last_point_distance = 0.3; if ( - autoware::universe_utils::calcDistance2d( + autoware_utils::calc_distance2d( resampled_path.points.back().point.pose.position, path.points.back().point.pose.position) > th_last_point_distance) { resampled_path.points.push_back(path.points.back()); @@ -878,8 +877,8 @@ void generateDrivableArea( for (const auto & point : resampled_path.points) { const auto & pose = point.point.pose; - const auto left_point = calcOffsetPose(pose, 0, offset, 0); - const auto right_point = calcOffsetPose(pose, 0, -offset, 0); + const auto left_point = calc_offset_pose(pose, 0, offset, 0); + const auto right_point = calc_offset_pose(pose, 0, -offset, 0); left_bound.push_back(left_point.position); right_bound.push_back(right_point.position); @@ -888,33 +887,33 @@ void generateDrivableArea( if (is_driving_forward) { // add backward offset point to bound const Pose first_point = - calcOffsetPose(resampled_path.points.front().point.pose, -vehicle_length, 0, 0); - const Pose left_first_point = calcOffsetPose(first_point, 0, offset, 0); - const Pose right_first_point = calcOffsetPose(first_point, 0, -offset, 0); + calc_offset_pose(resampled_path.points.front().point.pose, -vehicle_length, 0, 0); + const Pose left_first_point = calc_offset_pose(first_point, 0, offset, 0); + const Pose right_first_point = calc_offset_pose(first_point, 0, -offset, 0); left_bound.insert(left_bound.begin(), left_first_point.position); right_bound.insert(right_bound.begin(), right_first_point.position); // add forward offset point to bound const Pose last_point = - calcOffsetPose(resampled_path.points.back().point.pose, vehicle_length, 0, 0); - const Pose left_last_point = calcOffsetPose(last_point, 0, offset, 0); - const Pose right_last_point = calcOffsetPose(last_point, 0, -offset, 0); + calc_offset_pose(resampled_path.points.back().point.pose, vehicle_length, 0, 0); + const Pose left_last_point = calc_offset_pose(last_point, 0, offset, 0); + const Pose right_last_point = calc_offset_pose(last_point, 0, -offset, 0); left_bound.push_back(left_last_point.position); right_bound.push_back(right_last_point.position); } else { // add forward offset point to bound const Pose first_point = - calcOffsetPose(resampled_path.points.front().point.pose, vehicle_length, 0, 0); - const Pose left_first_point = calcOffsetPose(first_point, 0, offset, 0); - const Pose right_first_point = calcOffsetPose(first_point, 0, -offset, 0); + calc_offset_pose(resampled_path.points.front().point.pose, vehicle_length, 0, 0); + const Pose left_first_point = calc_offset_pose(first_point, 0, offset, 0); + const Pose right_first_point = calc_offset_pose(first_point, 0, -offset, 0); left_bound.insert(left_bound.begin(), left_first_point.position); right_bound.insert(right_bound.begin(), right_first_point.position); // add backward offset point to bound const Pose last_point = - calcOffsetPose(resampled_path.points.back().point.pose, -vehicle_length, 0, 0); - const Pose left_last_point = calcOffsetPose(last_point, 0, offset, 0); - const Pose right_last_point = calcOffsetPose(last_point, 0, -offset, 0); + calc_offset_pose(resampled_path.points.back().point.pose, -vehicle_length, 0, 0); + const Pose left_last_point = calc_offset_pose(last_point, 0, offset, 0); + const Pose right_last_point = calc_offset_pose(last_point, 0, -offset, 0); left_bound.push_back(left_last_point.position); right_bound.push_back(right_last_point.position); } @@ -939,7 +938,7 @@ void generateDrivableArea( p_line.push_back(p2); bool intersection_found = false; for (size_t j = i + 2; j < bound.size() - 1; j++) { - const double distance = autoware::universe_utils::calcDistance2d(bound.at(i), bound.at(j)); + const double distance = autoware_utils::calc_distance2d(bound.at(i), bound.at(j)); if (distance > intersection_check_distance) { break; } @@ -1038,7 +1037,7 @@ void extractObstaclesFromDrivableArea( std::vector edge_points; for (int i = 0; i < static_cast(obstacle.poly.outer().size()) - 1; ++i) { // NOTE: There is a duplicated points - edge_points.push_back(autoware::universe_utils::createPoint( + edge_points.push_back(autoware_utils::create_point( obstacle.poly.outer().at(i).x(), obstacle.poly.outer().at(i).y(), path.points.at(nearest_path_idx).point.pose.position.z)); } @@ -1301,9 +1300,9 @@ std::pair, bool> getBoundWithFreeSpaceAreas( const std::vector & other_side_bound, const std::shared_ptr planner_data, const bool is_left) { - using autoware::universe_utils::getPose; - using autoware::universe_utils::pose2transform; - using autoware::universe_utils::transformVector; + using autoware_utils::get_pose; + using autoware_utils::pose2transform; + using autoware_utils::transform_vector; using lanelet::utils::to2D; using lanelet::utils::conversion::toGeomMsgPt; using lanelet::utils::conversion::toLaneletPoint; @@ -1353,7 +1352,7 @@ std::pair, bool> getBoundWithFreeSpaceAreas( } const auto footprint = planner_data->parameters.vehicle_info.createFootprint(); - const auto vehicle_polygon = transformVector(footprint, pose2transform(ego_pose)); + const auto vehicle_polygon = transform_vector(footprint, pose2transform(ego_pose)); const auto is_driving_freespace = !boost::geometry::disjoint(vehicle_polygon, to2D(polygon).basicPolygon()); @@ -1381,12 +1380,12 @@ std::pair, bool> getBoundWithFreeSpaceAreas( return bound; } - const auto p_offset = autoware::universe_utils::calcOffsetPose( + const auto p_offset = autoware_utils::calc_offset_pose( ego_pose, (trim_behind_bound ? -100.0 : 100.0), (is_left ? 0.1 : -0.1), 0.0); std::vector ret; for (size_t i = 1; i < bound.size(); ++i) { - const auto intersect = autoware::universe_utils::intersect( + const auto intersect = autoware_utils::intersect( ego_pose.position, p_offset.position, toGeomMsgPt(bound.at(i - 1)), toGeomMsgPt(bound.at(i))); @@ -1485,17 +1484,16 @@ std::vector postProcess( return original_bound; } - const auto addPoints = - [](const lanelet::ConstLineString3d & points, std::vector & bound) { - for (const auto & bound_p : points) { - const auto cp = lanelet::utils::conversion::toGeomMsgPt(bound_p); - if ( - bound.empty() || - autoware::universe_utils::calcDistance2d(cp, bound.back()) > overlap_threshold) { - bound.push_back(cp); - } + const auto addPoints = []( + const lanelet::ConstLineString3d & points, + std::vector & bound) { + for (const auto & bound_p : points) { + const auto cp = lanelet::utils::conversion::toGeomMsgPt(bound_p); + if (bound.empty() || autoware_utils::calc_distance2d(cp, bound.back()) > overlap_threshold) { + bound.push_back(cp); } - }; + } + }; const auto has_overlap = [&](const lanelet::ConstLanelet & lane, const lanelet::ConstLanelets & ignore_lanelets = {}) { @@ -1595,17 +1593,14 @@ std::vector postProcess( // Insert middle points for (size_t i = start_idx + 1; i <= goal_idx; ++i) { const auto & next_point = tmp_bound.at(i); - const double dist = - autoware::universe_utils::calcDistance2d(processed_bound.back(), next_point); + const double dist = autoware_utils::calc_distance2d(processed_bound.back(), next_point); if (dist > overlap_threshold) { processed_bound.push_back(next_point); } } // Insert a goal point - if ( - autoware::universe_utils::calcDistance2d(processed_bound.back(), goal_point) > - overlap_threshold) { + if (autoware_utils::calc_distance2d(processed_bound.back(), goal_point) > overlap_threshold) { processed_bound.push_back(goal_point); } diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.cpp index 640659ed6caab..d43b8b0f75b53 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.cpp @@ -14,21 +14,21 @@ #include "autoware/behavior_path_planner_common/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.hpp" -#include -#include +#include +#include #include namespace autoware::behavior_path_planner { -using autoware::universe_utils::createQuaternionFromYaw; -using autoware::universe_utils::normalizeRadian; -using autoware::universe_utils::transformPose; +using autoware_utils::create_quaternion_from_yaw; +using autoware_utils::normalize_radian; +using autoware_utils::transform_pose; int discretize_angle(const double theta, const int theta_size) { const double one_angle_range = 2.0 * M_PI / theta_size; - return static_cast(std::rint(normalizeRadian(theta, 0.0) / one_angle_range)) % theta_size; + return static_cast(std::rint(normalize_radian(theta, 0.0) / one_angle_range)) % theta_size; } IndexXYT pose2index( @@ -51,7 +51,7 @@ geometry_msgs::msg::Pose index2pose( const double one_angle_range = 2.0 * M_PI / theta_size; const double yaw = index.theta * one_angle_range; - pose_local.orientation = createQuaternionFromYaw(yaw); + pose_local.orientation = create_quaternion_from_yaw(yaw); return pose_local; } @@ -65,7 +65,7 @@ geometry_msgs::msg::Pose global2local( geometry_msgs::msg::TransformStamped transform; transform.transform = tf2::toMsg(tf_origin.inverse()); - return transformPose(pose_global, transform); + return transform_pose(pose_global, transform); } void OccupancyGridBasedCollisionDetector::setMap(const nav_msgs::msg::OccupancyGrid & costmap) diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/parking_departure/geometric_parallel_parking.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/parking_departure/geometric_parallel_parking.cpp index bef35d5b7d187..3b59263530a16 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/parking_departure/geometric_parallel_parking.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/parking_departure/geometric_parallel_parking.cpp @@ -18,10 +18,10 @@ #include #include #include -#include -#include #include #include +#include +#include #include @@ -34,12 +34,12 @@ #include #include -using autoware::universe_utils::calcDistance2d; -using autoware::universe_utils::calcOffsetPose; -using autoware::universe_utils::inverseTransformPoint; -using autoware::universe_utils::normalizeRadian; -using autoware::universe_utils::transformPose; using autoware_internal_planning_msgs::msg::PathWithLaneId; +using autoware_utils::calc_distance2d; +using autoware_utils::calc_offset_pose; +using autoware_utils::inverse_transform_point; +using autoware_utils::normalize_radian; +using autoware_utils::transform_pose; using geometry_msgs::msg::Point; using geometry_msgs::msg::Pose; using lanelet::utils::getArcCoordinates; @@ -135,10 +135,10 @@ std::vector GeometricParallelParking::generatePullOverPaths( // check the continuity of straight path and arc path const Pose & road_path_last_pose = straight_path.points.back().point.pose; const Pose & arc_path_first_pose = arc_paths.front().points.front().point.pose; - const double yaw_diff = std::abs(autoware::universe_utils::normalizeRadian( + const double yaw_diff = std::abs(autoware_utils::normalize_radian( tf2::getYaw(road_path_last_pose.orientation) - tf2::getYaw(arc_path_first_pose.orientation))); - const double distance = calcDistance2d(road_path_last_pose, arc_path_first_pose); - if (yaw_diff > autoware::universe_utils::deg2rad(5.0) || distance > 0.1) { + const double distance = calc_distance2d(road_path_last_pose, arc_path_first_pose); + if (yaw_diff > autoware_utils::deg2rad(5.0) || distance > 0.1) { return std::vector{}; } @@ -165,7 +165,7 @@ bool GeometricParallelParking::planPullOver( const auto & common_params = planner_data_->parameters; const double end_pose_offset = is_forward ? -parameters_.after_forward_parking_straight_distance : parameters_.after_backward_parking_straight_distance; - const Pose arc_end_pose = calcOffsetPose(goal_pose, end_pose_offset, 0, 0); + const Pose arc_end_pose = calc_offset_pose(goal_pose, end_pose_offset, 0, 0); // not plan after parking has started if (isParking()) { @@ -285,10 +285,10 @@ bool GeometricParallelParking::planPullOut( // check the continuity of straight path and arc path const Pose & road_path_first_pose = road_center_line_path.points.front().point.pose; const Pose & arc_path_last_pose = arc_paths.back().points.back().point.pose; - const double yaw_diff = std::abs(autoware::universe_utils::normalizeRadian( + const double yaw_diff = std::abs(autoware_utils::normalize_radian( tf2::getYaw(road_path_first_pose.orientation) - tf2::getYaw(arc_path_last_pose.orientation))); - const double distance = calcDistance2d(road_path_first_pose, arc_path_last_pose); - if (yaw_diff > autoware::universe_utils::deg2rad(5.0) || distance > 0.1) { + const double distance = calc_distance2d(road_path_first_pose, arc_path_last_pose); + if (yaw_diff > autoware_utils::deg2rad(5.0) || distance > 0.1) { continue; } @@ -339,7 +339,7 @@ std::optional GeometricParallelParking::calcStartPose( // Assuming parallel poses, calculate the approximate start pose on the centerline from the goal // pose - const Pose approximate_start_pose = calcOffsetPose(goal_pose, dx, -arc_coordinates.distance, 0); + const Pose approximate_start_pose = calc_offset_pose(goal_pose, dx, -arc_coordinates.distance, 0); lanelet::ConstLanelet closest_road_lane{}; // Calculate start pose on the centerline, then offset it. @@ -388,17 +388,17 @@ std::vector GeometricParallelParking::planOneTrial( const auto & common_params = planner_data_->parameters; const auto & route_handler = planner_data_->route_handler; - const Pose arc_end_pose = calcOffsetPose(goal_pose, end_pose_offset, 0, 0); + const Pose arc_end_pose = calc_offset_pose(goal_pose, end_pose_offset, 0, 0); const double self_yaw = tf2::getYaw(start_pose.orientation); const double goal_yaw = tf2::getYaw(arc_end_pose.orientation); - const double psi = normalizeRadian(self_yaw - goal_yaw); + const double psi = normalize_radian(self_yaw - goal_yaw); - const Pose C_far = left_side_parking ? calcOffsetPose(arc_end_pose, 0, -R_E_far, 0) - : calcOffsetPose(arc_end_pose, 0, R_E_far, 0); - const double d_C_far_Einit = calcDistance2d(C_far, start_pose); + const Pose C_far = left_side_parking ? calc_offset_pose(arc_end_pose, 0, -R_E_far, 0) + : calc_offset_pose(arc_end_pose, 0, R_E_far, 0); + const double d_C_far_Einit = calc_distance2d(C_far, start_pose); - const Point C_far_goal_coords = inverseTransformPoint(C_far.position, arc_end_pose); - const Point self_point_goal_coords = inverseTransformPoint(start_pose.position, arc_end_pose); + const Point C_far_goal_coords = inverse_transform_point(C_far.position, arc_end_pose); + const Point self_point_goal_coords = inverse_transform_point(start_pose.position, arc_end_pose); const double alpha = left_side_parking @@ -414,7 +414,7 @@ std::vector GeometricParallelParking::planOneTrial( // combine road and shoulder lanes // cut the road lanes up to start_pose to prevent unintended processing for overlapped lane lanelet::ConstLanelets lanes{}; - autoware::universe_utils::Point2d start_point2d(start_pose.position.x, start_pose.position.y); + autoware_utils::Point2d start_point2d(start_pose.position.x, start_pose.position.y); for (const auto & lane : road_lanes) { if (boost::geometry::within(start_point2d, lane.polygon2d().basicPolygon())) { lanes.push_back(lane); @@ -447,8 +447,8 @@ std::vector GeometricParallelParking::planOneTrial( } // Generate arc path(first turn -> second turn) - const Pose C_near = left_side_parking ? calcOffsetPose(start_pose, 0, R_E_near, 0) - : calcOffsetPose(start_pose, 0, -R_E_near, 0); + const Pose C_near = left_side_parking ? calc_offset_pose(start_pose, 0, R_E_near, 0) + : calc_offset_pose(start_pose, 0, -R_E_near, 0); const double theta_near = std::acos( (std::pow(R_E_near, 2) + std::pow(R_E_near + R_E_far, 2) - std::pow(d_C_far_Einit, 2)) / @@ -468,16 +468,16 @@ std::vector GeometricParallelParking::planOneTrial( PathWithLaneId path_turn_first = left_side_parking ? generateArcPathWithHeader( - C_near, R_E_near, -M_PI_2, normalizeRadian(-M_PI_2 + theta_near), is_forward, is_forward) + C_near, R_E_near, -M_PI_2, normalize_radian(-M_PI_2 + theta_near), is_forward, is_forward) : generateArcPathWithHeader( - C_near, R_E_near, M_PI_2, normalizeRadian(M_PI_2 + theta_near), !is_forward, is_forward); + C_near, R_E_near, M_PI_2, normalize_radian(M_PI_2 + theta_near), !is_forward, is_forward); PathWithLaneId path_turn_second = left_side_parking ? generateArcPathWithHeader( - C_far, R_E_far, normalizeRadian(psi + M_PI_2 + theta_near), M_PI_2, + C_far, R_E_far, normalize_radian(psi + M_PI_2 + theta_near), M_PI_2, !is_forward, is_forward) : generateArcPathWithHeader( - C_far, R_E_far, normalizeRadian(psi - M_PI_2 + theta_near), -M_PI_2, + C_far, R_E_far, normalize_radian(psi - M_PI_2 + theta_near), -M_PI_2, is_forward, is_forward); // Need to add straight path to last right_turning for parking in parallel @@ -586,7 +586,7 @@ PathWithLaneId GeometricParallelParking::generateArcPath( // insert the last point exactly const auto p = generateArcPathPoint(center, radius, end_yaw, is_left_turn, is_forward); constexpr double min_dist = 0.01; - if (path.points.empty() || calcDistance2d(path.points.back(), p) > min_dist) { + if (path.points.empty() || calc_distance2d(path.points.back(), p) > min_dist) { path.points.push_back(p); } @@ -605,15 +605,15 @@ PathPointWithLaneId GeometricParallelParking::generateArcPathPoint( // set orientation tf2::Quaternion quat; if ((is_left_turn && !is_forward) || (!is_left_turn && is_forward)) { - quat.setRPY(0, 0, normalizeRadian(yaw - M_PI_2)); + quat.setRPY(0, 0, normalize_radian(yaw - M_PI_2)); } else { - quat.setRPY(0, 0, normalizeRadian(yaw + M_PI_2)); + quat.setRPY(0, 0, normalize_radian(yaw + M_PI_2)); } pose_center_coords.orientation = tf2::toMsg(quat); // get pose in map coords PathPointWithLaneId p{}; - p.point.pose = transformPose(pose_center_coords, center); + p.point.pose = transform_pose(pose_center_coords, center); return p; } diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_safety_checker/objects_filtering.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_safety_checker/objects_filtering.cpp index 7e021233bbfe2..f21d750940144 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_safety_checker/objects_filtering.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_safety_checker/objects_filtering.cpp @@ -18,8 +18,8 @@ #include "autoware/object_recognition_utils/predicted_path_utils.hpp" #include -#include -#include +#include +#include #include @@ -88,8 +88,7 @@ bool isCentroidWithinLanelet( } const auto closest_pose = lanelet::utils::getClosestCenterPose(lanelet, object_pose.position); - return std::abs(autoware::universe_utils::calcYawDeviation(closest_pose, object_pose)) < - yaw_threshold; + return std::abs(autoware_utils::calc_yaw_deviation(closest_pose, object_pose)) < yaw_threshold; } bool isPolygonOverlapLanelet( @@ -102,21 +101,20 @@ bool isPolygonOverlapLanelet( const auto & object_pose = object.kinematics.initial_pose_with_covariance.pose; const auto closest_pose = lanelet::utils::getClosestCenterPose(lanelet, object_pose.position); - return std::abs(autoware::universe_utils::calcYawDeviation(closest_pose, object_pose)) < - yaw_threshold; + return std::abs(autoware_utils::calc_yaw_deviation(closest_pose, object_pose)) < yaw_threshold; } bool isPolygonOverlapLanelet( - const PredictedObject & object, const autoware::universe_utils::Polygon2d & lanelet_polygon) + const PredictedObject & object, const autoware_utils::Polygon2d & lanelet_polygon) { - const auto object_polygon = autoware::universe_utils::toPolygon2d(object); + const auto object_polygon = autoware_utils::to_polygon2d(object); return !boost::geometry::disjoint(lanelet_polygon, object_polygon); } bool isPolygonOverlapLanelet( const PredictedObject & object, const lanelet::BasicPolygon2d & lanelet_polygon) { - const auto object_polygon = autoware::universe_utils::toPolygon2d(object); + const auto object_polygon = autoware_utils::to_polygon2d(object); return !boost::geometry::disjoint(lanelet_polygon, object_polygon); } @@ -358,7 +356,7 @@ ExtendedPredictedObject transform( for (double t = 0.0; t < safety_check_time_horizon + 1e-3; t += safety_check_time_resolution) { const auto obj_pose = autoware::object_recognition_utils::calcInterpolatedPose(path, t); if (obj_pose) { - const auto obj_polygon = autoware::universe_utils::toPolygon2d(*obj_pose, object.shape); + const auto obj_polygon = autoware_utils::to_polygon2d(*obj_pose, object.shape); extended_object.predicted_paths[i].path.emplace_back( t, *obj_pose, obj_velocity, obj_polygon); } diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_safety_checker/safety_check.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_safety_checker/safety_check.cpp index b448ae50492ac..217cd53f21dbe 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_safety_checker/safety_check.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_safety_checker/safety_check.cpp @@ -17,8 +17,8 @@ #include "autoware/behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp" #include "autoware/interpolation/linear_interpolation.hpp" #include "autoware/motion_utils/trajectory/trajectory.hpp" -#include "autoware/universe_utils/geometry/boost_polygon_utils.hpp" -#include "autoware/universe_utils/ros/uuid_helper.hpp" +#include "autoware_utils/geometry/boost_polygon_utils.hpp" +#include "autoware_utils/ros/uuid_helper.hpp" #include #include @@ -44,7 +44,7 @@ using autoware::motion_utils::calcLongitudinalOffsetPoint; using autoware::motion_utils::calcLongitudinalOffsetToSegment; using autoware::motion_utils::findNearestIndex; using autoware::motion_utils::findNearestSegmentIndex; -using autoware::universe_utils::calcDistance2d; +using autoware_utils::calc_distance2d; void appendPointToPolygon(Polygon2d & polygon, const geometry_msgs::msg::Point & geom_point) { @@ -59,21 +59,20 @@ bool isTargetObjectOncoming( const geometry_msgs::msg::Pose & vehicle_pose, const geometry_msgs::msg::Pose & object_pose, const double angle_threshold) { - return std::abs(calcYawDeviation(vehicle_pose, object_pose)) > angle_threshold; + return std::abs(calc_yaw_deviation(vehicle_pose, object_pose)) > angle_threshold; } bool isTargetObjectFront( const geometry_msgs::msg::Pose & ego_pose, const Polygon2d & obj_polygon, const double base_to_front) { - const auto ego_offset_pose = - autoware::universe_utils::calcOffsetPose(ego_pose, base_to_front, 0.0, 0.0); + const auto ego_offset_pose = autoware_utils::calc_offset_pose(ego_pose, base_to_front, 0.0, 0.0); // check all edges in the polygon const auto & obj_polygon_outer = obj_polygon.outer(); for (const auto & obj_edge : obj_polygon_outer) { - const auto obj_point = autoware::universe_utils::createPoint(obj_edge.x(), obj_edge.y(), 0.0); - if (autoware::universe_utils::calcLongitudinalDeviation(ego_offset_pose, obj_point) > 0.0) { + const auto obj_point = autoware_utils::create_point(obj_edge.x(), obj_edge.y(), 0.0); + if (autoware_utils::calc_longitudinal_deviation(ego_offset_pose, obj_point) > 0.0) { return true; } } @@ -103,13 +102,13 @@ Polygon2d createExtendedPolygon( } const auto p1 = - autoware::universe_utils::calcOffsetPose(base_link_pose, forward_lon_offset, lat_offset, 0.0); + autoware_utils::calc_offset_pose(base_link_pose, forward_lon_offset, lat_offset, 0.0); const auto p2 = - autoware::universe_utils::calcOffsetPose(base_link_pose, forward_lon_offset, -lat_offset, 0.0); + autoware_utils::calc_offset_pose(base_link_pose, forward_lon_offset, -lat_offset, 0.0); const auto p3 = - autoware::universe_utils::calcOffsetPose(base_link_pose, backward_lon_offset, -lat_offset, 0.0); + autoware_utils::calc_offset_pose(base_link_pose, backward_lon_offset, -lat_offset, 0.0); const auto p4 = - autoware::universe_utils::calcOffsetPose(base_link_pose, backward_lon_offset, lat_offset, 0.0); + autoware_utils::calc_offset_pose(base_link_pose, backward_lon_offset, lat_offset, 0.0); Polygon2d polygon; polygon.outer().reserve(5); @@ -118,9 +117,8 @@ Polygon2d createExtendedPolygon( appendPointToPolygon(polygon, p3.position); appendPointToPolygon(polygon, p4.position); appendPointToPolygon(polygon, p1.position); - return autoware::universe_utils::isClockwise(polygon) - ? polygon - : autoware::universe_utils::inverseClockwise(polygon); + return autoware_utils::is_clockwise(polygon) ? polygon + : autoware_utils::inverse_clockwise(polygon); } Polygon2d createExtendedPolygon( @@ -139,8 +137,8 @@ Polygon2d createExtendedPolygon( double min_y = std::numeric_limits::max(); const auto obj_polygon_outer = obj_polygon.outer(); for (const auto & polygon_p : obj_polygon_outer) { - const auto obj_p = autoware::universe_utils::createPoint(polygon_p.x(), polygon_p.y(), 0.0); - const auto transformed_p = autoware::universe_utils::inverseTransformPoint(obj_p, obj_pose); + const auto obj_p = autoware_utils::create_point(polygon_p.x(), polygon_p.y(), 0.0); + const auto transformed_p = autoware_utils::inverse_transform_point(obj_p, obj_pose); max_x = std::max(transformed_p.x, max_x); min_x = std::min(transformed_p.x, min_x); @@ -162,13 +160,13 @@ Polygon2d createExtendedPolygon( } const auto p1 = - autoware::universe_utils::calcOffsetPose(obj_pose, forward_lon_offset, left_lat_offset, 0.0); + autoware_utils::calc_offset_pose(obj_pose, forward_lon_offset, left_lat_offset, 0.0); const auto p2 = - autoware::universe_utils::calcOffsetPose(obj_pose, forward_lon_offset, right_lat_offset, 0.0); + autoware_utils::calc_offset_pose(obj_pose, forward_lon_offset, right_lat_offset, 0.0); const auto p3 = - autoware::universe_utils::calcOffsetPose(obj_pose, backward_lon_offset, right_lat_offset, 0.0); + autoware_utils::calc_offset_pose(obj_pose, backward_lon_offset, right_lat_offset, 0.0); const auto p4 = - autoware::universe_utils::calcOffsetPose(obj_pose, backward_lon_offset, left_lat_offset, 0.0); + autoware_utils::calc_offset_pose(obj_pose, backward_lon_offset, left_lat_offset, 0.0); Polygon2d polygon; polygon.outer().reserve(5); @@ -177,9 +175,8 @@ Polygon2d createExtendedPolygon( appendPointToPolygon(polygon, p3.position); appendPointToPolygon(polygon, p4.position); appendPointToPolygon(polygon, p1.position); - return autoware::universe_utils::isClockwise(polygon) - ? polygon - : autoware::universe_utils::inverseClockwise(polygon); + return autoware_utils::is_clockwise(polygon) ? polygon + : autoware_utils::inverse_clockwise(polygon); } Polygon2d create_extended_polygon_along_path( @@ -218,50 +215,49 @@ Polygon2d create_extended_polygon_along_path( Polygon2d polygon; { - const auto p_offset = autoware::universe_utils::calcOffsetPose( - base_link_pose, backward_lon_offset, lat_offset, 0.0); + const auto p_offset = + autoware_utils::calc_offset_pose(base_link_pose, backward_lon_offset, lat_offset, 0.0); appendPointToPolygon(polygon, p_offset.position); } for (size_t i = start_idx + 1; i < end_idx + 1; ++i) { - const auto p = autoware::universe_utils::getPose(planned_path.points.at(i)); - const auto p_offset = autoware::universe_utils::calcOffsetPose(p, 0.0, lat_offset, 0.0); + const auto p = autoware_utils::get_pose(planned_path.points.at(i)); + const auto p_offset = autoware_utils::calc_offset_pose(p, 0.0, lat_offset, 0.0); appendPointToPolygon(polygon, p_offset.position); } { - const auto p_offset = autoware::universe_utils::calcOffsetPose( - lon_offset_pose.value(), base_to_front, lat_offset, 0.0); + const auto p_offset = + autoware_utils::calc_offset_pose(lon_offset_pose.value(), base_to_front, lat_offset, 0.0); appendPointToPolygon(polygon, p_offset.position); } { - const auto p_offset = autoware::universe_utils::calcOffsetPose( - lon_offset_pose.value(), base_to_front, -lat_offset, 0.0); + const auto p_offset = + autoware_utils::calc_offset_pose(lon_offset_pose.value(), base_to_front, -lat_offset, 0.0); appendPointToPolygon(polygon, p_offset.position); } for (size_t i = end_idx; i > start_idx; --i) { - const auto p = autoware::universe_utils::getPose(planned_path.points.at(i)); - const auto p_offset = autoware::universe_utils::calcOffsetPose(p, 0.0, -lat_offset, 0.0); + const auto p = autoware_utils::get_pose(planned_path.points.at(i)); + const auto p_offset = autoware_utils::calc_offset_pose(p, 0.0, -lat_offset, 0.0); appendPointToPolygon(polygon, p_offset.position); } { - const auto p_offset = autoware::universe_utils::calcOffsetPose( - base_link_pose, backward_lon_offset, -lat_offset, 0.0); + const auto p_offset = + autoware_utils::calc_offset_pose(base_link_pose, backward_lon_offset, -lat_offset, 0.0); appendPointToPolygon(polygon, p_offset.position); } { - const auto p_offset = autoware::universe_utils::calcOffsetPose( - base_link_pose, backward_lon_offset, lat_offset, 0.0); + const auto p_offset = + autoware_utils::calc_offset_pose(base_link_pose, backward_lon_offset, lat_offset, 0.0); appendPointToPolygon(polygon, p_offset.position); } - return autoware::universe_utils::isClockwise(polygon) - ? polygon - : autoware::universe_utils::inverseClockwise(polygon); + return autoware_utils::is_clockwise(polygon) ? polygon + : autoware_utils::inverse_clockwise(polygon); } std::vector createExtendedPolygonsFromPoseWithVelocityStamped( @@ -277,8 +273,7 @@ std::vector createExtendedPolygonsFromPoseWithVelocityStamped( const double base_to_rear = vehicle_info.rear_overhang_m + backward_margin; const double width = vehicle_info.vehicle_width_m + lat_margin * 2; - const auto polygon = - autoware::universe_utils::toFootprint(pose, base_to_front, base_to_rear, width); + const auto polygon = autoware_utils::to_footprint(pose, base_to_front, base_to_rear, width); polygons.push_back(polygon); } @@ -345,7 +340,7 @@ std::optional calc_interpolated_pose_with_velocity( const double time_step = pt.time - prev_pt.time; const double ratio = std::clamp(offset / time_step, 0.0, 1.0); const auto interpolated_pose = - autoware::universe_utils::calcInterpolatedPose(prev_pt.pose, pt.pose, ratio, false); + autoware_utils::calc_interpolated_pose(prev_pt.pose, pt.pose, ratio, false); const double interpolated_velocity = autoware::interpolation::lerp(prev_pt.velocity, pt.velocity, ratio); return PoseWithVelocityStamped{relative_time, interpolated_pose, interpolated_velocity}; @@ -373,8 +368,7 @@ get_interpolated_pose_with_velocity_and_polygon_stamped( const auto & pose = interpolation_result->pose; const auto & velocity = interpolation_result->velocity; - const auto ego_polygon = - autoware::universe_utils::toFootprint(pose, base_to_front, base_to_rear, width); + const auto ego_polygon = autoware_utils::to_footprint(pose, base_to_front, base_to_rear, width); return PoseWithVelocityAndPolygonStamped{current_time, pose, velocity, ego_polygon}; } @@ -403,7 +397,7 @@ get_interpolated_pose_with_velocity_and_polygon_stamped( const auto & pose = interpolation_result->pose; const auto & velocity = interpolation_result->velocity; - const auto obj_polygon = autoware::universe_utils::toPolygon2d(pose, shape); + const auto obj_polygon = autoware_utils::to_polygon2d(pose, shape); return PoseWithVelocityAndPolygonStamped{current_time, pose, velocity, obj_polygon}; } @@ -481,8 +475,8 @@ std::vector filterPredictedPathAfterTargetPose( const auto target_idx = std::min_element(path.begin(), path.end(), [&target_pose](const auto & a, const auto & b) { - return calcDistance2d(a.pose.position, target_pose.position) < - calcDistance2d(b.pose.position, target_pose.position); + return calc_distance2d(a.pose.position, target_pose.position) < + calc_distance2d(b.pose.position, target_pose.position); }); std::copy(target_idx, path.end(), std::back_inserter(filtered_path)); @@ -621,7 +615,7 @@ std::vector get_collided_polygons( const double ego_yaw = tf2::getYaw(ego_pose.orientation); const double object_yaw = tf2::getYaw(obj_pose.orientation); - const double yaw_difference = autoware::universe_utils::normalizeRadian(ego_yaw - object_yaw); + const double yaw_difference = autoware_utils::normalize_radian(ego_yaw - object_yaw); if (std::abs(yaw_difference) > yaw_difference_th) continue; // check intersects @@ -716,10 +710,10 @@ CollisionCheckDebugPair createObjectDebug(const ExtendedPredictedObject & obj) { CollisionCheckDebug debug; debug.current_obj_pose = obj.initial_pose; - debug.extended_obj_polygon = autoware::universe_utils::toPolygon2d(obj.initial_pose, obj.shape); + debug.extended_obj_polygon = autoware_utils::to_polygon2d(obj.initial_pose, obj.shape); debug.obj_shape = obj.shape; debug.current_twist = obj.initial_twist; - return {autoware::universe_utils::toBoostUUID(obj.uuid), debug}; + return {autoware_utils::to_boost_uuid(obj.uuid), debug}; } void updateCollisionCheckDebugMap( @@ -799,10 +793,10 @@ std::pair checkObjectsCollisionRough( const Point ego_point = offset_point ? offset_point.value() : points.at(findNearestIndex(points, object_point)).point.pose.position; - return autoware::universe_utils::calcDistance2d(ego_point, object_point); + return autoware_utils::calc_distance2d(ego_point, object_point); } const Point ego_point = points.at(findNearestIndex(points, object_point)).point.pose.position; - return autoware::universe_utils::calcDistance2d(ego_point, object_point); + return autoware_utils::calc_distance2d(ego_point, object_point); }); // calculate min and max length from object center to edge @@ -873,10 +867,10 @@ double calculateRoughDistanceToObjects( const Point ego_point = offset_point ? offset_point.value() : points.at(findNearestIndex(points, object_point)).point.pose.position; - return std::max(calcDistance2d(ego_point, object_point) - object_length - ego_length, 0.0); + return std::max(calc_distance2d(ego_point, object_point) - object_length - ego_length, 0.0); } const Point ego_point = points.at(findNearestIndex(points, object_point)).point.pose.position; - return std::max(calcDistance2d(ego_point, object_point) - object_length - ego_length, 0.0); + return std::max(calc_distance2d(ego_point, object_point) - object_length - ego_length, 0.0); }); min_distance = std::min(min_distance, distance); } @@ -891,7 +885,7 @@ tier4_planning_msgs::msg::SafetyFactorArray to_safety_factor_array( tier4_planning_msgs::msg::SafetyFactor safety_factor; safety_factor.type = tier4_planning_msgs::msg::SafetyFactor::OBJECT; safety_factor.is_safe = data.is_safe; - safety_factor.object_id = autoware::universe_utils::toUUIDMsg(uuid); + safety_factor.object_id = autoware_utils::to_uuid_msg(uuid); safety_factor.points.push_back(data.current_obj_pose.position); safety_factors.factors.push_back(safety_factor); } diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_utils.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_utils.cpp index 8195b49ae662c..30e091c4bf6fa 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_utils.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_utils.cpp @@ -20,9 +20,9 @@ #include #include #include -#include #include #include +#include #include @@ -50,8 +50,7 @@ std::vector calcPathArcLengthArray( out.push_back(sum); for (size_t i = bounded_start + 1; i < bounded_end; ++i) { - sum += autoware::universe_utils::calcDistance2d( - path.points.at(i).point, path.points.at(i - 1).point); + sum += autoware_utils::calc_distance2d(path.points.at(i).point, path.points.at(i - 1).point); out.push_back(sum); } return out; @@ -165,12 +164,12 @@ size_t getIdxByArclength( throw std::runtime_error("[getIdxByArclength] path points must be > 0"); } - using autoware::universe_utils::calcDistance2d; + using autoware_utils::calc_distance2d; double sum_length = 0.0; if (signed_arc >= 0.0) { for (size_t i = target_idx; i < path.points.size() - 1; ++i) { const auto next_i = i + 1; - sum_length += calcDistance2d(path.points.at(i), path.points.at(next_i)); + sum_length += calc_distance2d(path.points.at(i), path.points.at(next_i)); if (sum_length > signed_arc) { return next_i; } @@ -179,7 +178,7 @@ size_t getIdxByArclength( } for (size_t i = target_idx; i > 0; --i) { const auto next_i = i - 1; - sum_length -= calcDistance2d(path.points.at(i), path.points.at(next_i)); + sum_length -= calc_distance2d(path.points.at(i), path.points.at(next_i)); if (sum_length < signed_arc) { return next_i; } @@ -331,12 +330,11 @@ std::vector spline_two_points( std::vector interpolatePose( const Pose & start_pose, const Pose & end_pose, const double resample_interval) { - using autoware::universe_utils::calcAzimuthAngle; + using autoware_utils::calc_azimuth_angle; std::vector interpolated_poses{}; // output - const double distance = - autoware::universe_utils::calcDistance2d(start_pose.position, end_pose.position); + const double distance = autoware_utils::calc_distance2d(start_pose.position, end_pose.position); const std::vector base_s{0.0, distance}; const std::vector base_x{start_pose.position.x, end_pose.position.x}; const std::vector base_y{start_pose.position.y, end_pose.position.y}; @@ -363,11 +361,11 @@ std::vector interpolatePose( // insert orientation for (size_t i = 0; i < interpolated_poses.size(); ++i) { - const double yaw = calcAzimuthAngle( + const double yaw = calc_azimuth_angle( interpolated_poses.at(i).position, i < interpolated_poses.size() - 1 ? interpolated_poses.at(i + 1).position : end_pose.position); - interpolated_poses.at(i).orientation = autoware::universe_utils::createQuaternionFromYaw(yaw); + interpolated_poses.at(i).orientation = autoware_utils::create_quaternion_from_yaw(yaw); } return interpolated_poses; @@ -388,7 +386,7 @@ Pose getUnshiftedEgoPose(const Pose & ego_pose, const ShiftedPath & prev_path) auto unshifted_pose = autoware::motion_utils::calcInterpolatedPoint(prev_path.path, ego_pose).point.pose; - unshifted_pose = autoware::universe_utils::calcOffsetPose( + unshifted_pose = autoware_utils::calc_offset_pose( unshifted_pose, 0.0, -prev_path.shift_length.at(closest_idx), 0.0); unshifted_pose.orientation = ego_pose.orientation; diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/traffic_light_utils.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/traffic_light_utils.cpp index 305519279a9ea..5c3b75813c237 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/traffic_light_utils.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/traffic_light_utils.cpp @@ -106,8 +106,7 @@ std::optional calcDistanceToRedTrafficLight( const auto x = 0.5 * (stop_line->front().x() + stop_line->back().x()); const auto y = 0.5 * (stop_line->front().y() + stop_line->back().y()); const auto z = 0.5 * (stop_line->front().z() + stop_line->back().z()); - return calcSignedArcLength( - path.points, ego_pos, autoware::universe_utils::createPoint(x, y, z)); + return calcSignedArcLength(path.points, ego_pos, autoware_utils::create_point(x, y, z)); } } diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/utils.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/utils.cpp index 79686e989c8a9..3ecc1803637c0 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/utils.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/utils.cpp @@ -17,13 +17,13 @@ #include "autoware/motion_utils/trajectory/path_with_lane_id.hpp" #include -#include -#include -#include -#include #include #include #include +#include +#include +#include +#include #include @@ -75,10 +75,10 @@ double calcInterpolatedVelocity( namespace autoware::behavior_path_planner::utils { -using autoware::universe_utils::LineString2d; -using autoware::universe_utils::Point2d; using autoware_perception_msgs::msg::ObjectClassification; using autoware_perception_msgs::msg::Shape; +using autoware_utils::LineString2d; +using autoware_utils::Point2d; using geometry_msgs::msg::PoseWithCovarianceStamped; std::optional getPolygonByPoint( @@ -102,8 +102,8 @@ double l2Norm(const Vector3 vector) } bool checkCollisionBetweenPathFootprintsAndObjects( - const autoware::universe_utils::LinearRing2d & local_vehicle_footprint, - const PathWithLaneId & ego_path, const PredictedObjects & dynamic_objects, const double margin) + const autoware_utils::LinearRing2d & local_vehicle_footprint, const PathWithLaneId & ego_path, + const PredictedObjects & dynamic_objects, const double margin) { for (const auto & p : ego_path.points) { if (checkCollisionBetweenFootprintAndObjects( @@ -115,14 +115,14 @@ bool checkCollisionBetweenPathFootprintsAndObjects( } bool checkCollisionBetweenFootprintAndObjects( - const autoware::universe_utils::LinearRing2d & local_vehicle_footprint, const Pose & ego_pose, + const autoware_utils::LinearRing2d & local_vehicle_footprint, const Pose & ego_pose, const PredictedObjects & dynamic_objects, const double margin) { - const auto vehicle_footprint = autoware::universe_utils::transformVector( - local_vehicle_footprint, autoware::universe_utils::pose2transform(ego_pose)); + const auto vehicle_footprint = autoware_utils::transform_vector( + local_vehicle_footprint, autoware_utils::pose2transform(ego_pose)); for (const auto & object : dynamic_objects.objects) { - const auto obj_polygon = autoware::universe_utils::toPolygon2d(object); + const auto obj_polygon = autoware_utils::to_polygon2d(object); const double distance = boost::geometry::distance(obj_polygon, vehicle_footprint); if (distance < margin) return true; } @@ -133,18 +133,18 @@ double calcLateralDistanceFromEgoToObject( const Pose & ego_pose, const double vehicle_width, const PredictedObject & dynamic_object) { double min_distance = std::numeric_limits::max(); - const auto obj_polygon = autoware::universe_utils::toPolygon2d(dynamic_object); + const auto obj_polygon = autoware_utils::to_polygon2d(dynamic_object); const auto vehicle_left_pose = - autoware::universe_utils::calcOffsetPose(ego_pose, 0, vehicle_width / 2, 0); + autoware_utils::calc_offset_pose(ego_pose, 0, vehicle_width / 2, 0); const auto vehicle_right_pose = - autoware::universe_utils::calcOffsetPose(ego_pose, 0, -vehicle_width / 2, 0); + autoware_utils::calc_offset_pose(ego_pose, 0, -vehicle_width / 2, 0); for (const auto & p : obj_polygon.outer()) { - const auto point = autoware::universe_utils::createPoint(p.x(), p.y(), 0.0); + const auto point = autoware_utils::create_point(p.x(), p.y(), 0.0); const double signed_distance_from_left = - autoware::universe_utils::calcLateralDeviation(vehicle_left_pose, point); + autoware_utils::calc_lateral_deviation(vehicle_left_pose, point); const double signed_distance_from_right = - autoware::universe_utils::calcLateralDeviation(vehicle_right_pose, point); + autoware_utils::calc_lateral_deviation(vehicle_right_pose, point); if (signed_distance_from_left < 0.0 && signed_distance_from_right > 0.0) { // point is between left and right @@ -163,21 +163,19 @@ double calc_longitudinal_distance_from_ego_to_object( const PredictedObject & dynamic_object) { double min_distance = std::numeric_limits::max(); - const auto obj_polygon = autoware::universe_utils::toPolygon2d(dynamic_object); - const auto vehicle_front_pose = - autoware::universe_utils::calcOffsetPose(ego_pose, base_link2front, 0, 0); - const auto vehicle_rear_pose = - autoware::universe_utils::calcOffsetPose(ego_pose, base_link2rear, 0, 0); + const auto obj_polygon = autoware_utils::to_polygon2d(dynamic_object); + const auto vehicle_front_pose = autoware_utils::calc_offset_pose(ego_pose, base_link2front, 0, 0); + const auto vehicle_rear_pose = autoware_utils::calc_offset_pose(ego_pose, base_link2rear, 0, 0); for (const auto & p : obj_polygon.outer()) { - const auto point = autoware::universe_utils::createPoint(p.x(), p.y(), 0.0); + const auto point = autoware_utils::create_point(p.x(), p.y(), 0.0); // forward is positive const double signed_distance_from_front = - autoware::universe_utils::calcLongitudinalDeviation(vehicle_front_pose, point); + autoware_utils::calc_longitudinal_deviation(vehicle_front_pose, point); // backward is positive const double signed_distance_from_rear = - -autoware::universe_utils::calcLongitudinalDeviation(vehicle_rear_pose, point); + -autoware_utils::calc_longitudinal_deviation(vehicle_rear_pose, point); if (signed_distance_from_front < 0.0 && signed_distance_from_rear < 0.0) { // point is between front and rear @@ -221,8 +219,7 @@ std::optional findIndexOutOfGoalSearchRange( for (size_t i = 0; i < points.size(); ++i) { const auto & lane_ids = points.at(i).lane_ids; - const double dist_to_goal = - autoware::universe_utils::calcDistance2d(points.at(i).point.pose, goal); + const double dist_to_goal = autoware_utils::calc_distance2d(points.at(i).point.pose, goal); const bool is_goal_lane_id_in_point = std::find(lane_ids.begin(), lane_ids.end(), goal_lane_id) != lane_ids.end(); if (dist_to_goal < max_dist && dist_to_goal < min_dist && is_goal_lane_id_in_point) { @@ -239,7 +236,7 @@ std::optional findIndexOutOfGoalSearchRange( // find index out of goal search range size_t min_dist_out_of_range_index = min_dist_index; for (int i = min_dist_index; 0 <= i; --i) { - const double dist = autoware::universe_utils::calcDistance2d(points.at(i).point, goal); + const double dist = autoware_utils::calc_distance2d(points.at(i).point, goal); min_dist_out_of_range_index = i; if (max_dist < dist) { break; @@ -275,7 +272,7 @@ bool set_goal( PathPointWithLaneId pre_refined_goal{}; constexpr double goal_to_pre_goal_distance = -1.0; pre_refined_goal.point.pose = - autoware::universe_utils::calcOffsetPose(goal, goal_to_pre_goal_distance, 0.0, 0.0); + autoware_utils::calc_offset_pose(goal, goal_to_pre_goal_distance, 0.0, 0.0); const size_t closest_seg_idx_for_pre_goal = findNearestSegmentIndex(input.points, pre_refined_goal.point.pose, 3.0, M_PI_4); pre_refined_goal.point.pose.position.z = @@ -404,8 +401,7 @@ bool isInLaneletWithYawThreshold( { const double pose_yaw = tf2::getYaw(current_pose.orientation); const double lanelet_angle = lanelet::utils::getLaneletAngle(lanelet, current_pose.position); - const double angle_diff = - std::abs(autoware::universe_utils::normalizeRadian(lanelet_angle - pose_yaw)); + const double angle_diff = std::abs(autoware_utils::normalize_radian(lanelet_angle - pose_yaw)); return (angle_diff < std::abs(yaw_threshold)) && lanelet::utils::isInLanelet(current_pose, lanelet, radius); @@ -432,7 +428,7 @@ bool isEgoOutOfRoute( } // If ego vehicle is over goal on goal lane, return true - const double yaw_threshold = autoware::universe_utils::deg2rad(90); + const double yaw_threshold = autoware_utils::deg2rad(90); if ( closest_road_lane.id() == goal_lane.id() && isInLaneletWithYawThreshold(self_pose, goal_lane, yaw_threshold)) { @@ -484,7 +480,7 @@ bool isEgoWithinOriginalLane( const lanelet::BasicPolygon2d & lane_polygon, const Pose & current_pose, const BehaviorPathPlannerParameters & common_param, const double outer_margin) { - const auto vehicle_poly = autoware::universe_utils::toFootprint( + const auto vehicle_poly = autoware_utils::to_footprint( current_pose, common_param.base_link2front, common_param.base_link2rear, common_param.vehicle_width); @@ -765,16 +761,16 @@ std::optional getSignedDistanceFromBoundary( rear_left.y = vehicle_width / 2; front_left.x = base_link2front; front_left.y = vehicle_width / 2; - rear_corner_point = autoware::universe_utils::transformPoint(rear_left, vehicle_pose); - front_corner_point = autoware::universe_utils::transformPoint(front_left, vehicle_pose); + rear_corner_point = autoware_utils::transform_point(rear_left, vehicle_pose); + front_corner_point = autoware_utils::transform_point(front_left, vehicle_pose); } else { Point front_right, rear_right; rear_right.x = -base_link2rear; rear_right.y = -vehicle_width / 2; front_right.x = base_link2front; front_right.y = -vehicle_width / 2; - rear_corner_point = autoware::universe_utils::transformPoint(rear_right, vehicle_pose); - front_corner_point = autoware::universe_utils::transformPoint(front_right, vehicle_pose); + rear_corner_point = autoware_utils::transform_point(rear_right, vehicle_pose); + front_corner_point = autoware_utils::transform_point(front_right, vehicle_pose); } const auto combined_lane = lanelet::utils::combineLaneletsShape(lanelets); @@ -798,18 +794,16 @@ std::optional getSignedDistanceFromBoundary( const Point p1 = lanelet::utils::conversion::toGeomMsgPt(bound_line_2d[i]); const Point p2 = lanelet::utils::conversion::toGeomMsgPt(bound_line_2d[i + 1]); - const Point inverse_p1 = - autoware::universe_utils::inverseTransformPoint(p1, vehicle_corner_pose); - const Point inverse_p2 = - autoware::universe_utils::inverseTransformPoint(p2, vehicle_corner_pose); + const Point inverse_p1 = autoware_utils::inverse_transform_point(p1, vehicle_corner_pose); + const Point inverse_p2 = autoware_utils::inverse_transform_point(p2, vehicle_corner_pose); const double dx_p1 = inverse_p1.x; const double dx_p2 = inverse_p2.x; const double dy_p1 = inverse_p1.y; const double dy_p2 = inverse_p2.y; // Calculate the Euclidean distances between vehicle's corner and the current and next points. - const double distance1 = autoware::universe_utils::calcDistance2d(p1, vehicle_corner_point); - const double distance2 = autoware::universe_utils::calcDistance2d(p2, vehicle_corner_point); + const double distance1 = autoware_utils::calc_distance2d(p1, vehicle_corner_point); + const double distance2 = autoware_utils::calc_distance2d(p2, vehicle_corner_point); // If one of the bound points is behind and the other is in front of the vehicle corner point // and any of these points is closer than the current minimum distance, @@ -862,9 +856,9 @@ std::optional getSignedDistanceFromBoundary( bound_pose.orientation = vehicle_pose.orientation; const Point inverse_rear_point = - autoware::universe_utils::inverseTransformPoint(rear_corner_point, bound_pose); + autoware_utils::inverse_transform_point(rear_corner_point, bound_pose); const Point inverse_front_point = - autoware::universe_utils::inverseTransformPoint(front_corner_point, bound_pose); + autoware_utils::inverse_transform_point(front_corner_point, bound_pose); const double dx_rear = inverse_rear_point.x; const double dx_front = inverse_front_point.x; const double dy_rear = inverse_rear_point.y; @@ -905,9 +899,8 @@ Polygon2d toPolygon2d(const lanelet::ConstLanelet & lanelet) } polygon.outer().push_back(polygon.outer().front()); - return autoware::universe_utils::isClockwise(polygon) - ? polygon - : autoware::universe_utils::inverseClockwise(polygon); + return autoware_utils::is_clockwise(polygon) ? polygon + : autoware_utils::inverse_clockwise(polygon); } Polygon2d toPolygon2d(const lanelet::BasicPolygon2d & polygon) @@ -918,9 +911,7 @@ Polygon2d toPolygon2d(const lanelet::BasicPolygon2d & polygon) } ret.outer().push_back(ret.outer().front()); - return autoware::universe_utils::isClockwise(ret) - ? ret - : autoware::universe_utils::inverseClockwise(ret); + return autoware_utils::is_clockwise(ret) ? ret : autoware_utils::inverse_clockwise(ret); } // TODO(Horibe) There is a similar function in route_handler. diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/test/test_footprints.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/test/test_footprints.cpp index debb8e0de6cce..c98be1b663e99 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/test/test_footprints.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/test/test_footprints.cpp @@ -19,8 +19,8 @@ #include using autoware::test_utils::createPose; -using autoware::universe_utils::Point2d; -using autoware::universe_utils::Polygon2d; +using autoware_utils::Point2d; +using autoware_utils::Polygon2d; constexpr auto epsilon = 1e-6; diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/test/test_objects_filtering.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/test/test_objects_filtering.cpp index d2827f1cf8cb4..b1e0dafc6b713 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/test/test_objects_filtering.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/test/test_objects_filtering.cpp @@ -17,8 +17,8 @@ #include "autoware/behavior_path_planner_common/utils/utils.hpp" #include -#include #include +#include #include @@ -43,8 +43,8 @@ using autoware_planning_msgs::msg::Trajectory; using autoware::test_utils::createPose; using autoware::test_utils::generateTrajectory; using autoware::test_utils::make_lanelet; -using autoware::universe_utils::createPoint; -using autoware::universe_utils::createVector3; +using autoware_utils::create_point; +using autoware_utils::create_vector3; constexpr double epsilon = 1e-6; @@ -59,7 +59,7 @@ PredictedObject create_bounding_box_object( const std::vector & classification = std::vector()) { PredictedObject object; - object.object_id = autoware::universe_utils::generateUUID(); + object.object_id = autoware_utils::generate_uuid(); object.kinematics.initial_pose_with_covariance.pose = pose; object.kinematics.initial_twist_with_covariance.twist.linear = velocity; object.shape.type = autoware_perception_msgs::msg::Shape::BOUNDING_BOX; @@ -114,7 +114,7 @@ TEST(BehaviorPathPlanningObjectsFiltering, position_filter) { using autoware::behavior_path_planner::utils::path_safety_checker::filter::position_filter; - auto current_pos = createPoint(0.0, 0.0, 0.0); + auto current_pos = create_point(0.0, 0.0, 0.0); PredictedObject object = create_bounding_box_object(createPose(10.0, 0.0, 0.0, 0.0, 0.0, 0.0)); auto straight_trajectory = generateTrajectory(20, 1.0); double forward_distance = 20.0; @@ -143,8 +143,8 @@ TEST(BehaviorPathPlanningObjectsFiltering, is_within_circle) { using autoware::behavior_path_planner::utils::path_safety_checker::filter::is_within_circle; - auto object_pos = createPoint(0.0, 0.0, 0.0); - auto ref_point = createPoint(0.0, 0.0, 0.0); + auto object_pos = create_point(0.0, 0.0, 0.0); + auto ref_point = create_point(0.0, 0.0, 0.0); double search_radius = 1.0; EXPECT_TRUE(is_within_circle(object_pos, ref_point, search_radius)); @@ -206,7 +206,7 @@ TEST(BehaviorPathPlanningObjectsFiltering, filterObjects) { using autoware::behavior_path_planner::utils::path_safety_checker::filterObjects; using autoware::behavior_path_planner::utils::path_safety_checker::ObjectsFilteringParams; - using autoware::universe_utils::createVector3; + using autoware_utils::create_vector3; std::shared_ptr objects = std::make_shared(); std::shared_ptr route_handler = @@ -221,7 +221,7 @@ TEST(BehaviorPathPlanningObjectsFiltering, filterObjects) current_lanes.push_back(route_handler->getLaneletsFromId(1000)); current_lanes.push_back(route_handler->getLaneletsFromId(1010)); - auto current_pose = createPoint(360.22, 600.51, 0.0); + auto current_pose = create_point(360.22, 600.51, 0.0); EXPECT_TRUE( filterObjects(objects, route_handler, current_lanes, current_pose, params).objects.empty()); @@ -231,10 +231,10 @@ TEST(BehaviorPathPlanningObjectsFiltering, filterObjects) classification.probability = 1.0; auto target_object = create_bounding_box_object( - createPose(360.22, 605.51, 0.0, 0.0, 0.0, 0.0), createVector3(1.0, 1.0, 0.0)); + createPose(360.22, 605.51, 0.0, 0.0, 0.0, 0.0), create_vector3(1.0, 1.0, 0.0)); target_object.classification.push_back(classification); auto other_object = create_bounding_box_object( - createPose(370.22, 600.51, 0.0, 0.0, 0.0, 0.0), createVector3(1.0, 1.0, 0.0)); + createPose(370.22, 600.51, 0.0, 0.0, 0.0, 0.0), create_vector3(1.0, 1.0, 0.0)); other_object.classification.push_back(classification); objects->objects.push_back(target_object); @@ -251,9 +251,9 @@ TEST(BehaviorPathPlanningObjectsFiltering, filterObjectsByVelocity) PredictedObjects objects; auto slow_obj = create_bounding_box_object( - createPose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0), createVector3(2.0, 0.0, 0.0)); + createPose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0), create_vector3(2.0, 0.0, 0.0)); auto fast_obj = create_bounding_box_object( - createPose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0), createVector3(10.0, 0.0, 0.0)); + createPose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0), create_vector3(10.0, 0.0, 0.0)); double vel_thr = 5.0; objects.objects.push_back(slow_obj); @@ -280,7 +280,7 @@ TEST(BehaviorPathPlanningObjectsFiltering, filterObjectsByPosition) using autoware::behavior_path_planner::utils::path_safety_checker::filterObjectsByPosition; using autoware::behavior_path_planner::utils::path_safety_checker::filterObjectsWithinRadius; - auto current_pos = createPoint(0.0, 0.0, 0.0); + auto current_pos = create_point(0.0, 0.0, 0.0); auto straight_path = trajectory_to_path_with_lane_id(generateTrajectory(20, 1.0)); double forward_distance = 10.0; double backward_distance = 1.0; @@ -388,7 +388,7 @@ TEST(BehaviorPathPlanningObjectsFiltering, getPredictedPathFromObj) pose.position.x = initial_pose.position.x + time * velocity; pose.position.y = initial_pose.position.y; PoseWithVelocityAndPolygonStamped obj_pose_with_poly( - time, pose, velocity, autoware::universe_utils::toPolygon2d(pose, shape)); + time, pose, velocity, autoware_utils::to_polygon2d(pose, shape)); path.push_back(obj_pose_with_poly); } return path; @@ -464,10 +464,10 @@ TEST(BehaviorPathPlanningObjectsFiltering, createPredictedPath) TEST(BehaviorPathPlanningObjectsFiltering, transform) { using autoware::behavior_path_planner::utils::path_safety_checker::transform; - auto velocity = autoware::universe_utils::createVector3(2.0, 0.0, 0.0); + auto velocity = autoware_utils::create_vector3(2.0, 0.0, 0.0); auto obj = create_bounding_box_object( - createPose(2.0, 0.0, 0.0, 0.0, 0.0, 0.0), createVector3(2.0, 0.0, 0.0), 2.0, 1.0); + createPose(2.0, 0.0, 0.0, 0.0, 0.0, 0.0), create_vector3(2.0, 0.0, 0.0), 2.0, 1.0); autoware_perception_msgs::msg::PredictedPath predicted_path; auto straight_path = trajectory_to_predicted_path(generateTrajectory(5, 1.0)); straight_path.confidence = 0.6; @@ -525,19 +525,19 @@ TEST(BehaviorPathPlanningObjectsFiltering, filterObjectsByClass) ObjectClassification classification; autoware::behavior_path_planner::utils::path_safety_checker::ObjectTypesToCheck types_to_check; - car.object_id = autoware::universe_utils::generateUUID(); + car.object_id = autoware_utils::generate_uuid(); classification.label = ObjectClassification::Type::CAR; classification.probability = 1.0; car.classification.push_back(classification); objects.objects.push_back(car); - truck.object_id = autoware::universe_utils::generateUUID(); + truck.object_id = autoware_utils::generate_uuid(); classification.label = ObjectClassification::Type::TRUCK; classification.probability = 1.0; truck.classification.push_back(classification); objects.objects.push_back(truck); - pedestrian.object_id = autoware::universe_utils::generateUUID(); + pedestrian.object_id = autoware_utils::generate_uuid(); classification.label = ObjectClassification::Type::PEDESTRIAN; classification.probability = 1.0; pedestrian.classification.push_back(classification); @@ -560,7 +560,7 @@ TEST(BehaviorPathPlanningObjectsFiltering, createTargetObjectsOnLane) { using autoware::behavior_path_planner::utils::path_safety_checker::createTargetObjectsOnLane; using autoware::behavior_path_planner::utils::path_safety_checker::ObjectsFilteringParams; - using autoware::universe_utils::createVector3; + using autoware_utils::create_vector3; PredictedObjects objects; std::shared_ptr route_handler = diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/test/test_parking_departure_utils.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/test/test_parking_departure_utils.cpp index 18da6d1565657..9f0a33bd6329d 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/test/test_parking_departure_utils.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/test/test_parking_departure_utils.cpp @@ -176,7 +176,7 @@ TEST(BehaviorPathPlanningParkingDepartureUtil, initializeCollisionCheckDebugMap) using autoware::behavior_path_planner::utils::parking_departure::initializeCollisionCheckDebugMap; autoware::behavior_path_planner::CollisionCheckDebugMap debug_map; - auto uuid1 = autoware::universe_utils::toBoostUUID(autoware::universe_utils::generateUUID()); + auto uuid1 = autoware_utils::to_boost_uuid(autoware_utils::generate_uuid()); autoware::behavior_path_planner::utils::path_safety_checker::CollisionCheckDebug debug_info; debug_map[uuid1] = debug_info; diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/test/test_path_shifter.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/test/test_path_shifter.cpp index eacd1b086cd3b..c2e081745878a 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/test/test_path_shifter.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/test/test_path_shifter.cpp @@ -14,8 +14,8 @@ #include "autoware/behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" -#include #include +#include #include @@ -350,8 +350,8 @@ TEST_F(PathShifterTest, generate) // Condition: next index of start index would be end index std::vector lines; ShiftLine line; - line.start.position = universe_utils::createPoint(0.0, 0.0, 0.0); - line.end.position = universe_utils::createPoint(1.1, 0.0, 0.0); + line.start.position = autoware_utils::create_point(0.0, 0.0, 0.0); + line.end.position = autoware_utils::create_point(1.1, 0.0, 0.0); lines.push_back(line); path_shifter_.setShiftLines(lines); EXPECT_FALSE(path_shifter_.generate(&shifted_path)); diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/test/test_safety_check.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/test/test_safety_check.cpp index 9ba540b4a9792..cf5cf25497dfe 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/test/test_safety_check.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/test/test_safety_check.cpp @@ -15,9 +15,9 @@ #include "autoware/behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" #include "autoware/behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp" -#include -#include #include +#include +#include #include #include @@ -46,9 +46,9 @@ using autoware::behavior_path_planner::utils::path_safety_checker::PredictedPath using autoware::behavior_path_planner::utils::path_safety_checker::RSSparams; using autoware::test_utils::createPose; using autoware::test_utils::generateTrajectory; -using autoware::universe_utils::Polygon2d; using autoware_internal_planning_msgs::msg::PathWithLaneId; using autoware_perception_msgs::msg::Shape; +using autoware_utils::Polygon2d; using geometry_msgs::msg::Pose; std::vector create_test_path() @@ -88,7 +88,7 @@ std::vector create_path_with_velocity_and_pol double time = static_cast(i) * interval; pose.position.x = initial_pose.position.x + time * velocity; PoseWithVelocityAndPolygonStamped obj_pose_with_poly( - time, pose, velocity, autoware::universe_utils::toPolygon2d(pose, shape)); + time, pose, velocity, autoware_utils::to_polygon2d(pose, shape)); predicted_path.push_back(obj_pose_with_poly); } @@ -117,7 +117,7 @@ ExtendedPredictedObject create_extended_predicted_object(Pose pose, float confid shape.dimensions.y = 1.0; object.initial_pose = pose; object.shape = shape; - object.initial_polygon = autoware::universe_utils::toPolygon2d(pose, shape); + object.initial_polygon = autoware_utils::to_polygon2d(pose, shape); object.predicted_paths.push_back(create_predicted_path_with_polygon(pose, confidence)); return object; @@ -134,12 +134,12 @@ TEST(BehaviorPathPlanningSafetyUtilsTest, isTargetObjectOncoming) EXPECT_FALSE(isTargetObjectOncoming(vehicle_pose, object_pose)); // Condition: facing each other - object_pose.orientation = autoware::universe_utils::createQuaternionFromYaw(M_PI); + object_pose.orientation = autoware_utils::create_quaternion_from_yaw(M_PI); EXPECT_TRUE(isTargetObjectOncoming(vehicle_pose, object_pose)); // Condition: Narrow angle threshold double angle_threshold = 0.75 * M_PI; - object_pose.orientation = autoware::universe_utils::createQuaternionFromYaw(M_PI_2); + object_pose.orientation = autoware_utils::create_quaternion_from_yaw(M_PI_2); EXPECT_FALSE(isTargetObjectOncoming(vehicle_pose, object_pose, angle_threshold)); } @@ -153,20 +153,17 @@ TEST(BehaviorPathPlanningSafetyUtilsTest, isTargetObjectFront) shape.type = autoware_perception_msgs::msg::Shape::BOUNDING_BOX; shape.dimensions.x = 5.0; shape.dimensions.y = 2.0; - auto obj_polygon = - autoware::universe_utils::toPolygon2d(createPose(10.0, 0.0, 0.0, 0.0, 0.0, 0.0), shape); + auto obj_polygon = autoware_utils::to_polygon2d(createPose(10.0, 0.0, 0.0, 0.0, 0.0, 0.0), shape); // Condition: object in front EXPECT_TRUE(isTargetObjectFront(ego_pose, obj_polygon, base_to_front)); // Condition: object behind - obj_polygon = - autoware::universe_utils::toPolygon2d(createPose(-10.0, 0.0, 0.0, 0.0, 0.0, 0.0), shape); + obj_polygon = autoware_utils::to_polygon2d(createPose(-10.0, 0.0, 0.0, 0.0, 0.0, 0.0), shape); EXPECT_FALSE(isTargetObjectFront(ego_pose, obj_polygon, base_to_front)); // Condition: object overlapping - obj_polygon = - autoware::universe_utils::toPolygon2d(createPose(3.0, 0.0, 0.0, 0.0, 0.0, 0.0), shape); + obj_polygon = autoware_utils::to_polygon2d(createPose(3.0, 0.0, 0.0, 0.0, 0.0, 0.0), shape); EXPECT_TRUE(isTargetObjectFront(ego_pose, obj_polygon, base_to_front)); } @@ -182,8 +179,8 @@ TEST(BehaviorPathPlanningSafetyUtilsTest, createExtendedEgoPolygon) { Pose ego_pose; - ego_pose.position = autoware::universe_utils::createPoint(0.0, 0.0, 0.0); - ego_pose.orientation = autoware::universe_utils::createQuaternionFromYaw(0.0); + ego_pose.position = autoware_utils::create_point(0.0, 0.0, 0.0); + ego_pose.orientation = autoware_utils::create_quaternion_from_yaw(0.0); const double lon_length = 10.0; const double lat_margin = 2.0; @@ -210,8 +207,8 @@ TEST(BehaviorPathPlanningSafetyUtilsTest, createExtendedEgoPolygon) { Pose ego_pose; - ego_pose.position = autoware::universe_utils::createPoint(3.0, 4.0, 0.0); - ego_pose.orientation = autoware::universe_utils::createQuaternionFromYaw(0.0); + ego_pose.position = autoware_utils::create_point(3.0, 4.0, 0.0); + ego_pose.orientation = autoware_utils::create_quaternion_from_yaw(0.0); const double lon_length = 10.0; const double lat_margin = 2.0; @@ -238,9 +235,8 @@ TEST(BehaviorPathPlanningSafetyUtilsTest, createExtendedEgoPolygon) { Pose ego_pose; - ego_pose.position = autoware::universe_utils::createPoint(0.0, 0.0, 0.0); - ego_pose.orientation = - autoware::universe_utils::createQuaternionFromYaw(autoware::universe_utils::deg2rad(60)); + ego_pose.position = autoware_utils::create_point(0.0, 0.0, 0.0); + ego_pose.orientation = autoware_utils::create_quaternion_from_yaw(autoware_utils::deg2rad(60)); const double lon_length = 10.0; const double lat_margin = 2.0; @@ -269,13 +265,13 @@ TEST(BehaviorPathPlanningSafetyUtilsTest, createExtendedEgoPolygon) TEST(BehaviorPathPlanningSafetyUtilsTest, createExtendedObjPolygon) { using autoware::behavior_path_planner::utils::path_safety_checker::createExtendedPolygon; - using autoware::universe_utils::createPoint; - using autoware::universe_utils::createQuaternionFromYaw; + using autoware_utils::create_point; + using autoware_utils::create_quaternion_from_yaw; { Pose obj_pose; - obj_pose.position = createPoint(0.0, 0.0, 0.0); - obj_pose.orientation = autoware::universe_utils::createQuaternionFromYaw(0.0); + obj_pose.position = create_point(0.0, 0.0, 0.0); + obj_pose.orientation = autoware_utils::create_quaternion_from_yaw(0.0); Shape shape; shape.type = autoware_perception_msgs::msg::Shape::POLYGON; @@ -298,7 +294,7 @@ TEST(BehaviorPathPlanningSafetyUtilsTest, createExtendedObjPolygon) CollisionCheckDebug debug; PoseWithVelocityAndPolygonStamped obj_pose_with_poly( - 0.0, obj_pose, 0.0, autoware::universe_utils::toPolygon2d(obj_pose, shape)); + 0.0, obj_pose, 0.0, autoware_utils::to_polygon2d(obj_pose, shape)); const auto polygon = createExtendedPolygon(obj_pose_with_poly, lon_length, lat_margin, is_stopped_object, debug); @@ -622,17 +618,14 @@ TEST(BehaviorPathPlanningSafetyUtilsTest, checkPolygonsIntersects) shape.dimensions.x = 5.0; shape.dimensions.y = 2.0; - poly_1.push_back( - autoware::universe_utils::toPolygon2d(createPose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0), shape)); - poly_2.push_back( - autoware::universe_utils::toPolygon2d(createPose(10.0, 2.0, 0.0, 0.0, 0.0, 0.0), shape)); + poly_1.push_back(autoware_utils::to_polygon2d(createPose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0), shape)); + poly_2.push_back(autoware_utils::to_polygon2d(createPose(10.0, 2.0, 0.0, 0.0, 0.0, 0.0), shape)); // Condition: no collision EXPECT_FALSE(checkPolygonsIntersects(poly_1, poly_2)); // Condition: collide - poly_2.push_back( - autoware::universe_utils::toPolygon2d(createPose(1.0, 2.0, 0.0, 0.0, 0.0, 0.0), shape)); + poly_2.push_back(autoware_utils::to_polygon2d(createPose(1.0, 2.0, 0.0, 0.0, 0.0, 0.0), shape)); EXPECT_TRUE(checkPolygonsIntersects(poly_1, poly_2)); } diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/test/test_traffic_light_utils.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/test/test_traffic_light_utils.cpp index 300050211ff1e..66fd44a0050e6 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/test/test_traffic_light_utils.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/test/test_traffic_light_utils.cpp @@ -18,7 +18,7 @@ #include "autoware_test_utils/autoware_test_utils.hpp" #include "autoware_test_utils/mock_data_parser.hpp" -#include +#include #include #include @@ -97,7 +97,7 @@ class TrafficLightTest : public ::testing::Test { nav_msgs::msg::Odometry odometry; odometry.pose.pose = planner_data_->self_odometry->pose.pose; - odometry.twist.twist.linear = autoware::universe_utils::createVector3(0.0, 0.0, 0.0); + odometry.twist.twist.linear = autoware_utils::create_vector3(0.0, 0.0, 0.0); planner_data_->self_odometry = std::make_shared(odometry); } diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/test/test_turn_signal.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/test/test_turn_signal.cpp index fd17ca91ff269..2155277ea6f7c 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/test/test_turn_signal.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/test/test_turn_signal.cpp @@ -14,7 +14,7 @@ #include "autoware/behavior_path_planner_common/turn_signal_decider.hpp" #include "autoware/motion_utils/trajectory/trajectory.hpp" -#include +#include #include "autoware_planning_msgs/msg/path_point.hpp" #include @@ -26,10 +26,10 @@ using autoware::behavior_path_planner::PathWithLaneId; using autoware::behavior_path_planner::Pose; using autoware::behavior_path_planner::TurnSignalDecider; using autoware::behavior_path_planner::TurnSignalInfo; -using autoware::universe_utils::createPoint; -using autoware::universe_utils::createQuaternionFromYaw; using autoware_internal_planning_msgs::msg::PathPointWithLaneId; using autoware_planning_msgs::msg::PathPoint; +using autoware_utils::create_point; +using autoware_utils::create_quaternion_from_yaw; using autoware_vehicle_msgs::msg::HazardLightsCommand; using autoware_vehicle_msgs::msg::TurnIndicatorsCommand; using geometry_msgs::msg::Point; @@ -82,25 +82,25 @@ TEST(BehaviorPathPlanningTurnSignal, Condition1) TurnSignalInfo intersection_signal_info; intersection_signal_info.turn_signal.command = TurnIndicatorsCommand::ENABLE_LEFT; - intersection_signal_info.desired_start_point.position = createPoint(0.0, 0.0, 0.0); - intersection_signal_info.desired_start_point.orientation = createQuaternionFromYaw(0.0); - intersection_signal_info.desired_end_point.position = createPoint(65.0, 0.0, 0.0); - intersection_signal_info.desired_end_point.orientation = createQuaternionFromYaw(0.0); - intersection_signal_info.required_start_point.position = createPoint(35.0, 0.0, 0.0); - intersection_signal_info.required_start_point.orientation = createQuaternionFromYaw(0.0); - intersection_signal_info.required_end_point.position = createPoint(48.0, 0.0, 0.0); - intersection_signal_info.required_end_point.orientation = createQuaternionFromYaw(0.0); + intersection_signal_info.desired_start_point.position = create_point(0.0, 0.0, 0.0); + intersection_signal_info.desired_start_point.orientation = create_quaternion_from_yaw(0.0); + intersection_signal_info.desired_end_point.position = create_point(65.0, 0.0, 0.0); + intersection_signal_info.desired_end_point.orientation = create_quaternion_from_yaw(0.0); + intersection_signal_info.required_start_point.position = create_point(35.0, 0.0, 0.0); + intersection_signal_info.required_start_point.orientation = create_quaternion_from_yaw(0.0); + intersection_signal_info.required_end_point.position = create_point(48.0, 0.0, 0.0); + intersection_signal_info.required_end_point.orientation = create_quaternion_from_yaw(0.0); TurnSignalInfo behavior_signal_info; behavior_signal_info.turn_signal.command = TurnIndicatorsCommand::ENABLE_RIGHT; - behavior_signal_info.desired_start_point.position = createPoint(5.0, 0.0, 0.0); - behavior_signal_info.desired_start_point.orientation = createQuaternionFromYaw(0.0); - behavior_signal_info.desired_end_point.position = createPoint(70.0, 0.0, 0.0); - behavior_signal_info.desired_end_point.orientation = createQuaternionFromYaw(0.0); - behavior_signal_info.required_start_point.position = createPoint(45.0, 0.0, 0.0); - behavior_signal_info.required_start_point.orientation = createQuaternionFromYaw(0.0); - behavior_signal_info.required_end_point.position = createPoint(50.0, 0.0, 0.0); - behavior_signal_info.required_end_point.orientation = createQuaternionFromYaw(0.0); + behavior_signal_info.desired_start_point.position = create_point(5.0, 0.0, 0.0); + behavior_signal_info.desired_start_point.orientation = create_quaternion_from_yaw(0.0); + behavior_signal_info.desired_end_point.position = create_point(70.0, 0.0, 0.0); + behavior_signal_info.desired_end_point.orientation = create_quaternion_from_yaw(0.0); + behavior_signal_info.required_start_point.position = create_point(45.0, 0.0, 0.0); + behavior_signal_info.required_start_point.orientation = create_quaternion_from_yaw(0.0); + behavior_signal_info.required_end_point.position = create_point(50.0, 0.0, 0.0); + behavior_signal_info.required_end_point.orientation = create_quaternion_from_yaw(0.0); // current pose on the behavior desired start { @@ -231,25 +231,25 @@ TEST(BehaviorPathPlanningTurnSignal, Condition2) TurnSignalInfo intersection_signal_info; intersection_signal_info.turn_signal.command = TurnIndicatorsCommand::ENABLE_LEFT; - intersection_signal_info.desired_start_point.position = createPoint(0.0, 0.0, 0.0); - intersection_signal_info.desired_start_point.orientation = createQuaternionFromYaw(0.0); - intersection_signal_info.desired_end_point.position = createPoint(65.0, 0.0, 0.0); - intersection_signal_info.desired_end_point.orientation = createQuaternionFromYaw(0.0); - intersection_signal_info.required_start_point.position = createPoint(35.0, 0.0, 0.0); - intersection_signal_info.required_start_point.orientation = createQuaternionFromYaw(0.0); - intersection_signal_info.required_end_point.position = createPoint(50.0, 0.0, 0.0); - intersection_signal_info.required_end_point.orientation = createQuaternionFromYaw(0.0); + intersection_signal_info.desired_start_point.position = create_point(0.0, 0.0, 0.0); + intersection_signal_info.desired_start_point.orientation = create_quaternion_from_yaw(0.0); + intersection_signal_info.desired_end_point.position = create_point(65.0, 0.0, 0.0); + intersection_signal_info.desired_end_point.orientation = create_quaternion_from_yaw(0.0); + intersection_signal_info.required_start_point.position = create_point(35.0, 0.0, 0.0); + intersection_signal_info.required_start_point.orientation = create_quaternion_from_yaw(0.0); + intersection_signal_info.required_end_point.position = create_point(50.0, 0.0, 0.0); + intersection_signal_info.required_end_point.orientation = create_quaternion_from_yaw(0.0); TurnSignalInfo behavior_signal_info; behavior_signal_info.turn_signal.command = TurnIndicatorsCommand::ENABLE_RIGHT; - behavior_signal_info.desired_start_point.position = createPoint(5.0, 0.0, 0.0); - behavior_signal_info.desired_start_point.orientation = createQuaternionFromYaw(0.0); - behavior_signal_info.desired_end_point.position = createPoint(70.0, 0.0, 0.0); - behavior_signal_info.desired_end_point.orientation = createQuaternionFromYaw(0.0); - behavior_signal_info.required_start_point.position = createPoint(40.0, 0.0, 0.0); - behavior_signal_info.required_start_point.orientation = createQuaternionFromYaw(0.0); - behavior_signal_info.required_end_point.position = createPoint(45.0, 0.0, 0.0); - behavior_signal_info.required_end_point.orientation = createQuaternionFromYaw(0.0); + behavior_signal_info.desired_start_point.position = create_point(5.0, 0.0, 0.0); + behavior_signal_info.desired_start_point.orientation = create_quaternion_from_yaw(0.0); + behavior_signal_info.desired_end_point.position = create_point(70.0, 0.0, 0.0); + behavior_signal_info.desired_end_point.orientation = create_quaternion_from_yaw(0.0); + behavior_signal_info.required_start_point.position = create_point(40.0, 0.0, 0.0); + behavior_signal_info.required_start_point.orientation = create_quaternion_from_yaw(0.0); + behavior_signal_info.required_end_point.position = create_point(45.0, 0.0, 0.0); + behavior_signal_info.required_end_point.orientation = create_quaternion_from_yaw(0.0); // current pose on the behavior desired start { @@ -344,25 +344,25 @@ TEST(BehaviorPathPlanningTurnSignal, Condition3) TurnSignalInfo intersection_signal_info; intersection_signal_info.turn_signal.command = TurnIndicatorsCommand::ENABLE_LEFT; - intersection_signal_info.desired_start_point.position = createPoint(0.0, 0.0, 0.0); - intersection_signal_info.desired_start_point.orientation = createQuaternionFromYaw(0.0); - intersection_signal_info.desired_end_point.position = createPoint(65.0, 0.0, 0.0); - intersection_signal_info.desired_end_point.orientation = createQuaternionFromYaw(0.0); - intersection_signal_info.required_start_point.position = createPoint(35.0, 0.0, 0.0); - intersection_signal_info.required_start_point.orientation = createQuaternionFromYaw(0.0); - intersection_signal_info.required_end_point.position = createPoint(50.0, 0.0, 0.0); - intersection_signal_info.required_end_point.orientation = createQuaternionFromYaw(0.0); + intersection_signal_info.desired_start_point.position = create_point(0.0, 0.0, 0.0); + intersection_signal_info.desired_start_point.orientation = create_quaternion_from_yaw(0.0); + intersection_signal_info.desired_end_point.position = create_point(65.0, 0.0, 0.0); + intersection_signal_info.desired_end_point.orientation = create_quaternion_from_yaw(0.0); + intersection_signal_info.required_start_point.position = create_point(35.0, 0.0, 0.0); + intersection_signal_info.required_start_point.orientation = create_quaternion_from_yaw(0.0); + intersection_signal_info.required_end_point.position = create_point(50.0, 0.0, 0.0); + intersection_signal_info.required_end_point.orientation = create_quaternion_from_yaw(0.0); TurnSignalInfo behavior_signal_info; behavior_signal_info.turn_signal.command = TurnIndicatorsCommand::ENABLE_RIGHT; - behavior_signal_info.desired_start_point.position = createPoint(5.0, 0.0, 0.0); - behavior_signal_info.desired_start_point.orientation = createQuaternionFromYaw(0.0); - behavior_signal_info.desired_end_point.position = createPoint(70.0, 0.0, 0.0); - behavior_signal_info.desired_end_point.orientation = createQuaternionFromYaw(0.0); - behavior_signal_info.required_start_point.position = createPoint(30.0, 0.0, 0.0); - behavior_signal_info.required_start_point.orientation = createQuaternionFromYaw(0.0); - behavior_signal_info.required_end_point.position = createPoint(45.0, 0.0, 0.0); - behavior_signal_info.required_end_point.orientation = createQuaternionFromYaw(0.0); + behavior_signal_info.desired_start_point.position = create_point(5.0, 0.0, 0.0); + behavior_signal_info.desired_start_point.orientation = create_quaternion_from_yaw(0.0); + behavior_signal_info.desired_end_point.position = create_point(70.0, 0.0, 0.0); + behavior_signal_info.desired_end_point.orientation = create_quaternion_from_yaw(0.0); + behavior_signal_info.required_start_point.position = create_point(30.0, 0.0, 0.0); + behavior_signal_info.required_start_point.orientation = create_quaternion_from_yaw(0.0); + behavior_signal_info.required_end_point.position = create_point(45.0, 0.0, 0.0); + behavior_signal_info.required_end_point.orientation = create_quaternion_from_yaw(0.0); // current pose on the behavior desired start { diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/test/test_utils.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/test/test_utils.cpp index 94ffaeba61711..aab7ddf0e08f7 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/test/test_utils.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/test/test_utils.cpp @@ -26,12 +26,12 @@ #include #include -using autoware::universe_utils::Point2d; using autoware_internal_planning_msgs::msg::PathPointWithLaneId; using autoware_internal_planning_msgs::msg::PathWithLaneId; using autoware_perception_msgs::msg::PredictedObject; using autoware_perception_msgs::msg::PredictedObjects; using autoware_planning_msgs::msg::Trajectory; +using autoware_utils::Point2d; using ObjectClassification = autoware_perception_msgs::msg::ObjectClassification; using autoware::behavior_path_planner::PlannerData; using autoware_planning_msgs::msg::LaneletRoute; @@ -85,11 +85,11 @@ TEST_F(BehaviorPathPlanningUtilTest, l2Norm) { using autoware::behavior_path_planner::utils::l2Norm; - geometry_msgs::msg::Vector3 vector = autoware::universe_utils::createVector3(0.0, 0.0, 0.0); + geometry_msgs::msg::Vector3 vector = autoware_utils::create_vector3(0.0, 0.0, 0.0); auto norm = l2Norm(vector); EXPECT_DOUBLE_EQ(norm, 0.0); - vector = autoware::universe_utils::createVector3(1.0, 2.0, 2.0); + vector = autoware_utils::create_vector3(1.0, 2.0, 2.0); norm = l2Norm(vector); EXPECT_DOUBLE_EQ(norm, 3.0); } @@ -98,7 +98,7 @@ TEST_F(BehaviorPathPlanningUtilTest, checkCollisionBetweenPathFootprintsAndObjec { using autoware::behavior_path_planner::utils::checkCollisionBetweenPathFootprintsAndObjects; - autoware::universe_utils::LinearRing2d base_footprint = { + autoware_utils::LinearRing2d base_footprint = { Point2d{1.0, 1.0}, Point2d{1.0, -1.0}, Point2d{-1.0, -1.0}, Point2d{-1.0, 1.0}, Point2d{1.0, -1.0}}; double margin = 0.2; @@ -132,7 +132,7 @@ TEST_F(BehaviorPathPlanningUtilTest, checkCollisionBetweenFootprintAndObjects) using autoware::behavior_path_planner::utils::checkCollisionBetweenFootprintAndObjects; auto ego_pose = createPose(1.0, 1.0, 0.0, 0.0, 0.0, 0.0); - autoware::universe_utils::LinearRing2d base_footprint = { + autoware_utils::LinearRing2d base_footprint = { Point2d{1.0, 1.0}, Point2d{1.0, -1.0}, Point2d{-1.0, -1.0}, Point2d{-1.0, 1.0}, Point2d{1.0, -1.0}}; double margin = 0.2; diff --git a/planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/include/autoware/behavior_path_sampling_planner_module/sampling_planner_module.hpp b/planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/include/autoware/behavior_path_sampling_planner_module/sampling_planner_module.hpp index 6f8f3e9e94642..b3f1edc8464be 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/include/autoware/behavior_path_sampling_planner_module/sampling_planner_module.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/include/autoware/behavior_path_sampling_planner_module/sampling_planner_module.hpp @@ -24,11 +24,6 @@ #include "autoware/behavior_path_sampling_planner_module/sampling_planner_parameters.hpp" #include "autoware/behavior_path_sampling_planner_module/util.hpp" #include "autoware/motion_utils/trajectory/path_with_lane_id.hpp" -#include "autoware/universe_utils/geometry/boost_geometry.hpp" -#include "autoware/universe_utils/geometry/boost_polygon_utils.hpp" -#include "autoware/universe_utils/math/constants.hpp" -#include "autoware/universe_utils/ros/update_param.hpp" -#include "autoware/universe_utils/system/stop_watch.hpp" #include "autoware_bezier_sampler/bezier_sampling.hpp" #include "autoware_frenet_planner/frenet_planner.hpp" #include "autoware_lanelet2_extension/utility/query.hpp" @@ -38,6 +33,11 @@ #include "autoware_sampler_common/constraints/soft_constraint.hpp" #include "autoware_sampler_common/structures.hpp" #include "autoware_sampler_common/transform/spline_transform.hpp" +#include "autoware_utils/geometry/boost_geometry.hpp" +#include "autoware_utils/geometry/boost_polygon_utils.hpp" +#include "autoware_utils/math/constants.hpp" +#include "autoware_utils/ros/update_param.hpp" +#include "autoware_utils/system/stop_watch.hpp" #include "autoware_vehicle_info_utils/vehicle_info_utils.hpp" #include "rclcpp/rclcpp.hpp" @@ -75,8 +75,8 @@ struct SamplingPlannerDebugData { std::vector sampled_candidates{}; size_t previous_sampled_candidates_nb = 0UL; - std::vector obstacles{}; - std::vector footprints{}; + std::vector obstacles{}; + std::vector footprints{}; }; class SamplingPlannerModule : public SceneModuleInterface { @@ -206,7 +206,7 @@ class SamplingPlannerModule : public SceneModuleInterface autoware::motion_utils::findNearestIndex(prev_module_reference_path->points, ego_pose); if (!nearest_index) return false; auto toYaw = [](const geometry_msgs::msg::Quaternion & quat) -> double { - const auto rpy = autoware::universe_utils::getRPY(quat); + const auto rpy = autoware_utils::get_rpy(quat); return rpy.z; }; const auto quat = prev_module_reference_path->points[*nearest_index].point.pose.orientation; diff --git a/planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/include/autoware/behavior_path_sampling_planner_module/sampling_planner_parameters.hpp b/planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/include/autoware/behavior_path_sampling_planner_module/sampling_planner_parameters.hpp index f778eef981a92..7b81f132d48c1 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/include/autoware/behavior_path_sampling_planner_module/sampling_planner_parameters.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/include/autoware/behavior_path_sampling_planner_module/sampling_planner_parameters.hpp @@ -21,11 +21,11 @@ #include namespace autoware::behavior_path_planner { -using autoware::universe_utils::LinearRing2d; -using autoware::universe_utils::MultiPoint2d; -using autoware::universe_utils::MultiPolygon2d; -using autoware::universe_utils::Point2d; -using autoware::universe_utils::Polygon2d; +using autoware_utils::LinearRing2d; +using autoware_utils::MultiPoint2d; +using autoware_utils::MultiPolygon2d; +using autoware_utils::Point2d; +using autoware_utils::Polygon2d; struct SamplingPlannerParameters { // constraints.hard diff --git a/planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/include/autoware/behavior_path_sampling_planner_module/util.hpp b/planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/include/autoware/behavior_path_sampling_planner_module/util.hpp index 7ca21f08879fa..e7e240e66ee10 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/include/autoware/behavior_path_sampling_planner_module/util.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/include/autoware/behavior_path_sampling_planner_module/util.hpp @@ -17,8 +17,8 @@ #include "autoware_sampler_common/structures.hpp" #include "autoware_sampler_common/transform/spline_transform.hpp" -#include -#include +#include +#include #include @@ -101,7 +101,7 @@ inline autoware::sampler_common::State getInitialState( { autoware::sampler_common::State initial_state; Point2d initial_state_pose{pose.position.x, pose.position.y}; - const auto rpy = autoware::universe_utils::getRPY(pose.orientation); + const auto rpy = autoware_utils::get_rpy(pose.orientation); initial_state.pose = initial_state_pose; initial_state.frenet = reference_spline.frenet({pose.position.x, pose.position.y}); initial_state.heading = rpy.z; diff --git a/planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/package.xml b/planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/package.xml index d91c9d4c45138..da4f6085f683e 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/package.xml +++ b/planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/package.xml @@ -25,7 +25,7 @@ autoware_planning_test_manager autoware_route_handler autoware_signal_processing - autoware_universe_utils + autoware_utils autoware_vehicle_info_utils autoware_vehicle_msgs geometry_msgs diff --git a/planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/src/manager.cpp b/planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/src/manager.cpp index b2b86433002cc..1e9cbd1c29a91 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/src/manager.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/src/manager.cpp @@ -14,7 +14,7 @@ #include "autoware/behavior_path_sampling_planner_module/manager.hpp" -#include "autoware/universe_utils/ros/update_param.hpp" +#include "autoware_utils/ros/update_param.hpp" #include @@ -77,48 +77,48 @@ void SamplingPlannerModuleManager::init(rclcpp::Node * node) void SamplingPlannerModuleManager::updateModuleParams( [[maybe_unused]] const std::vector & parameters) { - using autoware::universe_utils::updateParam; + using autoware_utils::update_param; auto & p = parameters_; { std::string ns{"constraints.hard"}; - updateParam(parameters, ns + ".max_curvature", p->max_curvature); - updateParam(parameters, ns + ".min_curvature", p->min_curvature); + update_param(parameters, ns + ".max_curvature", p->max_curvature); + update_param(parameters, ns + ".min_curvature", p->min_curvature); ns = std::string{"constraints.soft"}; - updateParam(parameters, ns + ".lateral_deviation_weight", p->lateral_deviation_weight); - updateParam(parameters, ns + ".length_weight", p->length_weight); - updateParam(parameters, ns + ".curvature_weight", p->curvature_weight); - updateParam>(parameters, ns + ".weights", p->weights); + update_param(parameters, ns + ".lateral_deviation_weight", p->lateral_deviation_weight); + update_param(parameters, ns + ".length_weight", p->length_weight); + update_param(parameters, ns + ".curvature_weight", p->curvature_weight); + update_param>(parameters, ns + ".weights", p->weights); } { std::string ns{"sampling"}; - updateParam(parameters, ns + ".enable_frenet", p->enable_frenet); - updateParam(parameters, ns + ".enable_bezier", p->enable_bezier); - updateParam(parameters, ns + ".resolution", p->resolution); + update_param(parameters, ns + ".enable_frenet", p->enable_frenet); + update_param(parameters, ns + ".enable_bezier", p->enable_bezier); + update_param(parameters, ns + ".resolution", p->resolution); - updateParam( + update_param( parameters, ns + ".previous_path_reuse_points_nb", p->previous_path_reuse_points_nb); - updateParam( + update_param( parameters, ns + ".nb_target_lateral_positions", p->nb_target_lateral_positions); - updateParam>(parameters, ns + ".target_lengths", p->target_lengths); + update_param>(parameters, ns + ".target_lengths", p->target_lengths); - updateParam>( + update_param>( parameters, ns + ".target_lateral_positions", p->target_lateral_positions); ns += ".frenet"; - updateParam>( + update_param>( parameters, ns + ".target_lateral_velocities", p->target_lateral_velocities); - updateParam>( + update_param>( parameters, ns + ".target_lateral_accelerations", p->target_lateral_accelerations); } { std::string ns{"preprocessing"}; - updateParam(parameters, ns + ".force_zero_initial_deviation", p->force_zero_deviation); - updateParam(parameters, ns + ".force_zero_initial_heading", p->force_zero_heading); - updateParam(parameters, ns + ".smooth_reference_trajectory", p->smooth_reference); + update_param(parameters, ns + ".force_zero_initial_deviation", p->force_zero_deviation); + update_param(parameters, ns + ".force_zero_initial_heading", p->force_zero_heading); + update_param(parameters, ns + ".smooth_reference_trajectory", p->smooth_reference); } std::for_each(observers_.begin(), observers_.end(), [&](const auto & observer) { diff --git a/planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/src/sampling_planner_module.cpp b/planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/src/sampling_planner_module.cpp index 5bb57f17ce813..2bb47ae5aac69 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/src/sampling_planner_module.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/src/sampling_planner_module.cpp @@ -28,10 +28,10 @@ namespace autoware::behavior_path_planner using autoware::motion_utils::calcSignedArcLength; using autoware::motion_utils::findNearestIndex; using autoware::motion_utils::findNearestSegmentIndex; -using autoware::universe_utils::calcDistance2d; -using autoware::universe_utils::calcOffsetPose; -using autoware::universe_utils::getPoint; -using autoware::universe_utils::Point2d; +using autoware_utils::calc_distance2d; +using autoware_utils::calc_offset_pose; +using autoware_utils::get_point; +using autoware_utils::Point2d; using geometry_msgs::msg::Point; namespace bg = boost::geometry; @@ -113,7 +113,7 @@ SamplingPlannerModule::SamplingPlannerModule( // [[maybe_unused]] const SoftConstraintsInputs & input_data) -> double { // if (path.points.empty()) return 0.0; // const auto & goal_pose_yaw = - // autoware::universe_utils::getRPY(input_data.goal_pose.orientation).z; const auto & + // autoware_utils::getRPY(input_data.goal_pose.orientation).z; const auto & // last_point_yaw = path.yaws.back(); const double angle_difference = std::abs(last_point_yaw // - goal_pose_yaw); return angle_difference / (3.141519 / 4.0); // }); @@ -374,9 +374,9 @@ void SamplingPlannerModule::prepareConstraints( size_t i = 0; for (const auto & o : predicted_objects->objects) { if (o.kinematics.initial_twist_with_covariance.twist.linear.x < 0.5) { - const auto polygon = autoware::universe_utils::toPolygon2d(o); + const auto polygon = autoware_utils::to_polygon2d(o); constraints.obstacle_polygons.push_back(polygon); - const auto box = boost::geometry::return_envelope(polygon); + const auto box = boost::geometry::return_envelope(polygon); constraints.rtree.insert(std::make_pair(box, i)); } i++; diff --git a/planning/behavior_path_planner/autoware_behavior_path_side_shift_module/package.xml b/planning/behavior_path_planner/autoware_behavior_path_side_shift_module/package.xml index 5b40fe1d74169..186d5a962cd6c 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_side_shift_module/package.xml +++ b/planning/behavior_path_planner/autoware_behavior_path_side_shift_module/package.xml @@ -22,7 +22,7 @@ autoware_behavior_path_planner_common autoware_motion_utils autoware_planning_msgs - autoware_universe_utils + autoware_utils geometry_msgs pluginlib rclcpp diff --git a/planning/behavior_path_planner/autoware_behavior_path_side_shift_module/src/manager.cpp b/planning/behavior_path_planner/autoware_behavior_path_side_shift_module/src/manager.cpp index 2e8d7e98c11cf..1be1501f3d008 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_side_shift_module/src/manager.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_side_shift_module/src/manager.cpp @@ -14,7 +14,7 @@ #include "autoware/behavior_path_side_shift_module/manager.hpp" -#include "autoware/universe_utils/ros/update_param.hpp" +#include "autoware_utils/ros/update_param.hpp" #include @@ -48,12 +48,12 @@ void SideShiftModuleManager::init(rclcpp::Node * node) void SideShiftModuleManager::updateModuleParams( [[maybe_unused]] const std::vector & parameters) { - using autoware::universe_utils::updateParam; + using autoware_utils::update_param; [[maybe_unused]] auto p = parameters_; [[maybe_unused]] const std::string ns = "side_shift."; - // updateParam(parameters, ns + ..., ...); + // update_param(parameters, ns + ..., ...); std::for_each(observers_.begin(), observers_.end(), [&p](const auto & observer) { if (!observer.expired()) observer.lock()->updateModuleParams(p); diff --git a/planning/behavior_path_planner/autoware_behavior_path_side_shift_module/src/scene.cpp b/planning/behavior_path_planner/autoware_behavior_path_side_shift_module/src/scene.cpp index 9536c1ba17336..7040a3b85dffa 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_side_shift_module/src/scene.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_side_shift_module/src/scene.cpp @@ -33,8 +33,8 @@ namespace autoware::behavior_path_planner using autoware::motion_utils::calcSignedArcLength; using autoware::motion_utils::findNearestIndex; using autoware::motion_utils::findNearestSegmentIndex; -using autoware::universe_utils::calcDistance2d; -using autoware::universe_utils::getPoint; +using autoware_utils::calc_distance2d; +using autoware_utils::get_point; using geometry_msgs::msg::Point; SideShiftModule::SideShiftModule( @@ -198,8 +198,7 @@ void SideShiftModule::updateData() const auto longest_dist_to_shift_line = [&]() { double max_dist = 0.0; for (const auto & pnt : path_shifter_.getShiftLines()) { - max_dist = - std::max(max_dist, autoware::universe_utils::calcDistance2d(getEgoPose(), pnt.start)); + max_dist = std::max(max_dist, autoware_utils::calc_distance2d(getEgoPose(), pnt.start)); } return max_dist; }(); @@ -422,7 +421,7 @@ PathWithLaneId SideShiftModule::extendBackwardLength(const PathWithLaneId & orig const auto longest_dist_to_shift_point = [&]() { double max_dist = 0.0; for (const auto & pnt : path_shifter_.getShiftLines()) { - max_dist = std::max(max_dist, calcDistance2d(getEgoPose(), pnt.start)); + max_dist = std::max(max_dist, calc_distance2d(getEgoPose(), pnt.start)); } return max_dist; }(); @@ -433,8 +432,8 @@ PathWithLaneId SideShiftModule::extendBackwardLength(const PathWithLaneId & orig const auto & prev_reference = getPreviousModuleOutput().path; const size_t orig_ego_idx = findNearestIndex(original_path.points, getEgoPose().position); - const size_t prev_ego_idx = - findNearestSegmentIndex(prev_reference.points, getPoint(original_path.points.at(orig_ego_idx))); + const size_t prev_ego_idx = findNearestSegmentIndex( + prev_reference.points, get_point(original_path.points.at(orig_ego_idx))); size_t clip_idx = 0; for (size_t i = 0; i < prev_ego_idx; ++i) { @@ -467,7 +466,7 @@ void SideShiftModule::setDebugMarkersVisualization() const debug_marker_.markers.clear(); const auto add = [this](const MarkerArray & added) { - autoware::universe_utils::appendMarkerArray(added, &debug_marker_); + autoware_utils::append_marker_array(added, &debug_marker_); }; const auto add_shift_line_marker = [this, add]( diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/freespace_pull_out.hpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/freespace_pull_out.hpp index 9b2c8fa22a9d4..3ea076518266f 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/freespace_pull_out.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/freespace_pull_out.hpp @@ -16,7 +16,7 @@ #define AUTOWARE__BEHAVIOR_PATH_START_PLANNER_MODULE__FREESPACE_PULL_OUT_HPP_ #include "autoware/behavior_path_start_planner_module/pull_out_planner_base.hpp" -#include "autoware/universe_utils/system/time_keeper.hpp" +#include "autoware_utils/system/time_keeper.hpp" #include #include diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/geometric_pull_out.hpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/geometric_pull_out.hpp index bc8c6001ba067..a45397a920aba 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/geometric_pull_out.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/geometric_pull_out.hpp @@ -18,7 +18,7 @@ #include "autoware/behavior_path_planner_common/utils/parking_departure/geometric_parallel_parking.hpp" #include "autoware/behavior_path_start_planner_module/pull_out_path.hpp" #include "autoware/behavior_path_start_planner_module/pull_out_planner_base.hpp" -#include "autoware/universe_utils/system/time_keeper.hpp" +#include "autoware_utils/system/time_keeper.hpp" #include @@ -33,8 +33,8 @@ class GeometricPullOut : public PullOutPlannerBase public: explicit GeometricPullOut( rclcpp::Node & node, const StartPlannerParameters & parameters, - std::shared_ptr time_keeper = - std::make_shared()); + std::shared_ptr time_keeper = + std::make_shared()); PlannerType getPlannerType() const override { return PlannerType::GEOMETRIC; }; std::optional plan( diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/pull_out_planner_base.hpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/pull_out_planner_base.hpp index 934234b3d4b4f..d026f184ebd0c 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/pull_out_planner_base.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/pull_out_planner_base.hpp @@ -20,7 +20,7 @@ #include "autoware/behavior_path_start_planner_module/data_structs.hpp" #include "autoware/behavior_path_start_planner_module/pull_out_path.hpp" #include "autoware/behavior_path_start_planner_module/util.hpp" -#include "autoware/universe_utils/system/time_keeper.hpp" +#include "autoware_utils/system/time_keeper.hpp" #include #include @@ -31,8 +31,8 @@ namespace autoware::behavior_path_planner { -using autoware::universe_utils::LinearRing2d; using autoware_internal_planning_msgs::msg::PathWithLaneId; +using autoware_utils::LinearRing2d; using geometry_msgs::msg::Pose; class PullOutPlannerBase @@ -40,8 +40,8 @@ class PullOutPlannerBase public: explicit PullOutPlannerBase( rclcpp::Node & node, const StartPlannerParameters & parameters, - std::shared_ptr time_keeper = - std::make_shared()) + std::shared_ptr time_keeper = + std::make_shared()) : parameters_{parameters}, vehicle_info_{autoware::vehicle_info_utils::VehicleInfoUtils(node).getVehicleInfo()}, vehicle_footprint_{vehicle_info_.createFootprint()}, @@ -71,7 +71,7 @@ class PullOutPlannerBase LinearRing2d vehicle_footprint_; double collision_check_margin_; - mutable std::shared_ptr time_keeper_; + mutable std::shared_ptr time_keeper_; }; } // namespace autoware::behavior_path_planner diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/shift_pull_out.hpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/shift_pull_out.hpp index 84337a0a037f4..81e7005a9f062 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/shift_pull_out.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/shift_pull_out.hpp @@ -17,7 +17,7 @@ #include "autoware/behavior_path_start_planner_module/pull_out_path.hpp" #include "autoware/behavior_path_start_planner_module/pull_out_planner_base.hpp" -#include "autoware/universe_utils/system/time_keeper.hpp" +#include "autoware_utils/system/time_keeper.hpp" #include @@ -35,8 +35,8 @@ class ShiftPullOut : public PullOutPlannerBase public: explicit ShiftPullOut( rclcpp::Node & node, const StartPlannerParameters & parameters, - std::shared_ptr time_keeper = - std::make_shared()); + std::shared_ptr time_keeper = + std::make_shared()); PlannerType getPlannerType() const override { return PlannerType::SHIFT; }; std::optional plan( diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/package.xml b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/package.xml index 674f6a7f53981..cdc8940a41e85 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/package.xml +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/package.xml @@ -25,7 +25,7 @@ autoware_freespace_planning_algorithms autoware_motion_utils autoware_rtc_interface - autoware_universe_utils + autoware_utils pluginlib rclcpp visualization_msgs diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/data_structs.cpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/data_structs.cpp index f613b9345ce42..8010418c3a32d 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/data_structs.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/data_structs.cpp @@ -16,7 +16,7 @@ #include "autoware/behavior_path_start_planner_module/manager.hpp" -#include +#include #include #include @@ -26,170 +26,173 @@ namespace autoware::behavior_path_planner StartPlannerParameters StartPlannerParameters::init(rclcpp::Node & node) { - using autoware::universe_utils::getOrDeclareParameter; + using autoware_utils::get_or_declare_parameter; StartPlannerParameters p; { const std::string ns = "start_planner."; - p.th_arrived_distance = getOrDeclareParameter(node, ns + "th_arrived_distance"); - p.th_stopped_velocity = getOrDeclareParameter(node, ns + "th_stopped_velocity"); - p.th_stopped_time = getOrDeclareParameter(node, ns + "th_stopped_time"); + p.th_arrived_distance = get_or_declare_parameter(node, ns + "th_arrived_distance"); + p.th_stopped_velocity = get_or_declare_parameter(node, ns + "th_stopped_velocity"); + p.th_stopped_time = get_or_declare_parameter(node, ns + "th_stopped_time"); p.prepare_time_before_start = - getOrDeclareParameter(node, ns + "prepare_time_before_start"); + get_or_declare_parameter(node, ns + "prepare_time_before_start"); p.th_distance_to_middle_of_the_road = - getOrDeclareParameter(node, ns + "th_distance_to_middle_of_the_road"); - p.skip_rear_vehicle_check = getOrDeclareParameter(node, ns + "skip_rear_vehicle_check"); + get_or_declare_parameter(node, ns + "th_distance_to_middle_of_the_road"); + p.skip_rear_vehicle_check = + get_or_declare_parameter(node, ns + "skip_rear_vehicle_check"); p.extra_width_margin_for_rear_obstacle = - getOrDeclareParameter(node, ns + "extra_width_margin_for_rear_obstacle"); + get_or_declare_parameter(node, ns + "extra_width_margin_for_rear_obstacle"); p.collision_check_margins = - getOrDeclareParameter>(node, ns + "collision_check_margins"); + get_or_declare_parameter>(node, ns + "collision_check_margins"); p.collision_check_margin_from_front_object = - getOrDeclareParameter(node, ns + "collision_check_margin_from_front_object"); + get_or_declare_parameter(node, ns + "collision_check_margin_from_front_object"); p.th_moving_object_velocity = - getOrDeclareParameter(node, ns + "th_moving_object_velocity"); + get_or_declare_parameter(node, ns + "th_moving_object_velocity"); p.center_line_path_interval = - getOrDeclareParameter(node, ns + "center_line_path_interval"); + get_or_declare_parameter(node, ns + "center_line_path_interval"); // shift pull out - p.enable_shift_pull_out = getOrDeclareParameter(node, ns + "enable_shift_pull_out"); + p.enable_shift_pull_out = get_or_declare_parameter(node, ns + "enable_shift_pull_out"); p.check_shift_path_lane_departure = - getOrDeclareParameter(node, ns + "check_shift_path_lane_departure"); + get_or_declare_parameter(node, ns + "check_shift_path_lane_departure"); p.allow_check_shift_path_lane_departure_override = - getOrDeclareParameter(node, ns + "allow_check_shift_path_lane_departure_override"); + get_or_declare_parameter(node, ns + "allow_check_shift_path_lane_departure_override"); p.shift_collision_check_distance_from_end = - getOrDeclareParameter(node, ns + "shift_collision_check_distance_from_end"); + get_or_declare_parameter(node, ns + "shift_collision_check_distance_from_end"); p.minimum_shift_pull_out_distance = - getOrDeclareParameter(node, ns + "minimum_shift_pull_out_distance"); + get_or_declare_parameter(node, ns + "minimum_shift_pull_out_distance"); p.lateral_acceleration_sampling_num = - getOrDeclareParameter(node, ns + "lateral_acceleration_sampling_num"); - p.lateral_jerk = getOrDeclareParameter(node, ns + "lateral_jerk"); - p.maximum_lateral_acc = getOrDeclareParameter(node, ns + "maximum_lateral_acc"); - p.minimum_lateral_acc = getOrDeclareParameter(node, ns + "minimum_lateral_acc"); - p.maximum_curvature = getOrDeclareParameter(node, ns + "maximum_curvature"); + get_or_declare_parameter(node, ns + "lateral_acceleration_sampling_num"); + p.lateral_jerk = get_or_declare_parameter(node, ns + "lateral_jerk"); + p.maximum_lateral_acc = get_or_declare_parameter(node, ns + "maximum_lateral_acc"); + p.minimum_lateral_acc = get_or_declare_parameter(node, ns + "minimum_lateral_acc"); + p.maximum_curvature = get_or_declare_parameter(node, ns + "maximum_curvature"); p.end_pose_curvature_threshold = - getOrDeclareParameter(node, ns + "end_pose_curvature_threshold"); + get_or_declare_parameter(node, ns + "end_pose_curvature_threshold"); p.maximum_longitudinal_deviation = - getOrDeclareParameter(node, ns + "maximum_longitudinal_deviation"); + get_or_declare_parameter(node, ns + "maximum_longitudinal_deviation"); // geometric pull out p.enable_geometric_pull_out = - getOrDeclareParameter(node, ns + "enable_geometric_pull_out"); + get_or_declare_parameter(node, ns + "enable_geometric_pull_out"); p.geometric_collision_check_distance_from_end = - getOrDeclareParameter(node, ns + "geometric_collision_check_distance_from_end"); - p.divide_pull_out_path = getOrDeclareParameter(node, ns + "divide_pull_out_path"); + get_or_declare_parameter(node, ns + "geometric_collision_check_distance_from_end"); + p.divide_pull_out_path = get_or_declare_parameter(node, ns + "divide_pull_out_path"); p.parallel_parking_parameters.pull_out_velocity = - getOrDeclareParameter(node, ns + "geometric_pull_out_velocity"); + get_or_declare_parameter(node, ns + "geometric_pull_out_velocity"); p.parallel_parking_parameters.pull_out_arc_path_interval = - getOrDeclareParameter(node, ns + "arc_path_interval"); + get_or_declare_parameter(node, ns + "arc_path_interval"); p.parallel_parking_parameters.pull_out_lane_departure_margin = - getOrDeclareParameter(node, ns + "lane_departure_margin"); + get_or_declare_parameter(node, ns + "lane_departure_margin"); p.lane_departure_check_expansion_margin = - getOrDeclareParameter(node, ns + "lane_departure_check_expansion_margin"); + get_or_declare_parameter(node, ns + "lane_departure_check_expansion_margin"); p.parallel_parking_parameters.pull_out_max_steer_angle = - getOrDeclareParameter(node, ns + "pull_out_max_steer_angle"); // 15deg + get_or_declare_parameter(node, ns + "pull_out_max_steer_angle"); // 15deg p.parallel_parking_parameters.center_line_path_interval = p.center_line_path_interval; // for geometric parallel parking // search start pose backward - p.search_priority = getOrDeclareParameter( + p.search_priority = get_or_declare_parameter( node, ns + "search_priority"); // "efficient_path" or "short_back_distance" - p.enable_back = getOrDeclareParameter(node, ns + "enable_back"); - p.backward_velocity = getOrDeclareParameter(node, ns + "backward_velocity"); - p.max_back_distance = getOrDeclareParameter(node, ns + "max_back_distance"); + p.enable_back = get_or_declare_parameter(node, ns + "enable_back"); + p.backward_velocity = get_or_declare_parameter(node, ns + "backward_velocity"); + p.max_back_distance = get_or_declare_parameter(node, ns + "max_back_distance"); p.backward_search_resolution = - getOrDeclareParameter(node, ns + "backward_search_resolution"); + get_or_declare_parameter(node, ns + "backward_search_resolution"); p.backward_path_update_duration = - getOrDeclareParameter(node, ns + "backward_path_update_duration"); + get_or_declare_parameter(node, ns + "backward_path_update_duration"); p.ignore_distance_from_lane_end = - getOrDeclareParameter(node, ns + "ignore_distance_from_lane_end"); + get_or_declare_parameter(node, ns + "ignore_distance_from_lane_end"); // stop condition p.maximum_deceleration_for_stop = - getOrDeclareParameter(node, ns + "stop_condition.maximum_deceleration_for_stop"); + get_or_declare_parameter(node, ns + "stop_condition.maximum_deceleration_for_stop"); p.maximum_jerk_for_stop = - getOrDeclareParameter(node, ns + "stop_condition.maximum_jerk_for_stop"); + get_or_declare_parameter(node, ns + "stop_condition.maximum_jerk_for_stop"); } { const std::string ns = "start_planner.object_types_to_check_for_path_generation."; p.object_types_to_check_for_path_generation.check_car = - getOrDeclareParameter(node, ns + "check_car"); + get_or_declare_parameter(node, ns + "check_car"); p.object_types_to_check_for_path_generation.check_truck = - getOrDeclareParameter(node, ns + "check_truck"); + get_or_declare_parameter(node, ns + "check_truck"); p.object_types_to_check_for_path_generation.check_bus = - getOrDeclareParameter(node, ns + "check_bus"); + get_or_declare_parameter(node, ns + "check_bus"); p.object_types_to_check_for_path_generation.check_trailer = - getOrDeclareParameter(node, ns + "check_trailer"); + get_or_declare_parameter(node, ns + "check_trailer"); p.object_types_to_check_for_path_generation.check_unknown = - getOrDeclareParameter(node, ns + "check_unknown"); + get_or_declare_parameter(node, ns + "check_unknown"); p.object_types_to_check_for_path_generation.check_bicycle = - getOrDeclareParameter(node, ns + "check_bicycle"); + get_or_declare_parameter(node, ns + "check_bicycle"); p.object_types_to_check_for_path_generation.check_motorcycle = - getOrDeclareParameter(node, ns + "check_motorcycle"); + get_or_declare_parameter(node, ns + "check_motorcycle"); p.object_types_to_check_for_path_generation.check_pedestrian = - getOrDeclareParameter(node, ns + "check_pedestrian"); + get_or_declare_parameter(node, ns + "check_pedestrian"); } // freespace planner general params { const std::string ns = "start_planner.freespace_planner."; - p.enable_freespace_planner = getOrDeclareParameter(node, ns + "enable_freespace_planner"); + p.enable_freespace_planner = + get_or_declare_parameter(node, ns + "enable_freespace_planner"); p.freespace_planner_algorithm = - getOrDeclareParameter(node, ns + "freespace_planner_algorithm"); + get_or_declare_parameter(node, ns + "freespace_planner_algorithm"); p.end_pose_search_start_distance = - getOrDeclareParameter(node, ns + "end_pose_search_start_distance"); + get_or_declare_parameter(node, ns + "end_pose_search_start_distance"); p.end_pose_search_end_distance = - getOrDeclareParameter(node, ns + "end_pose_search_end_distance"); + get_or_declare_parameter(node, ns + "end_pose_search_end_distance"); p.end_pose_search_interval = - getOrDeclareParameter(node, ns + "end_pose_search_interval"); - p.freespace_planner_velocity = getOrDeclareParameter(node, ns + "velocity"); - p.vehicle_shape_margin = getOrDeclareParameter(node, ns + "vehicle_shape_margin"); + get_or_declare_parameter(node, ns + "end_pose_search_interval"); + p.freespace_planner_velocity = get_or_declare_parameter(node, ns + "velocity"); + p.vehicle_shape_margin = get_or_declare_parameter(node, ns + "vehicle_shape_margin"); p.freespace_planner_common_parameters.time_limit = - getOrDeclareParameter(node, ns + "time_limit"); + get_or_declare_parameter(node, ns + "time_limit"); p.freespace_planner_common_parameters.max_turning_ratio = - getOrDeclareParameter(node, ns + "max_turning_ratio"); + get_or_declare_parameter(node, ns + "max_turning_ratio"); p.freespace_planner_common_parameters.turning_steps = - getOrDeclareParameter(node, ns + "turning_steps"); + get_or_declare_parameter(node, ns + "turning_steps"); } // freespace planner search config { const std::string ns = "start_planner.freespace_planner.search_configs."; p.freespace_planner_common_parameters.theta_size = - getOrDeclareParameter(node, ns + "theta_size"); + get_or_declare_parameter(node, ns + "theta_size"); p.freespace_planner_common_parameters.angle_goal_range = - getOrDeclareParameter(node, ns + "angle_goal_range"); + get_or_declare_parameter(node, ns + "angle_goal_range"); p.freespace_planner_common_parameters.curve_weight = - getOrDeclareParameter(node, ns + "curve_weight"); + get_or_declare_parameter(node, ns + "curve_weight"); p.freespace_planner_common_parameters.reverse_weight = - getOrDeclareParameter(node, ns + "reverse_weight"); + get_or_declare_parameter(node, ns + "reverse_weight"); p.freespace_planner_common_parameters.lateral_goal_range = - getOrDeclareParameter(node, ns + "lateral_goal_range"); + get_or_declare_parameter(node, ns + "lateral_goal_range"); p.freespace_planner_common_parameters.longitudinal_goal_range = - getOrDeclareParameter(node, ns + "longitudinal_goal_range"); + get_or_declare_parameter(node, ns + "longitudinal_goal_range"); } // freespace planner costmap configs { const std::string ns = "start_planner.freespace_planner.costmap_configs."; p.freespace_planner_common_parameters.obstacle_threshold = - getOrDeclareParameter(node, ns + "obstacle_threshold"); + get_or_declare_parameter(node, ns + "obstacle_threshold"); } // freespace planner astar { const std::string ns = "start_planner.freespace_planner.astar."; p.astar_parameters.search_method = - getOrDeclareParameter(node, ns + "search_method"); + get_or_declare_parameter(node, ns + "search_method"); p.astar_parameters.only_behind_solutions = - getOrDeclareParameter(node, ns + "only_behind_solutions"); - p.astar_parameters.use_back = getOrDeclareParameter(node, ns + "use_back"); + get_or_declare_parameter(node, ns + "only_behind_solutions"); + p.astar_parameters.use_back = get_or_declare_parameter(node, ns + "use_back"); p.astar_parameters.distance_heuristic_weight = - getOrDeclareParameter(node, ns + "distance_heuristic_weight"); + get_or_declare_parameter(node, ns + "distance_heuristic_weight"); } // freespace planner rrtstar { const std::string ns = "start_planner.freespace_planner.rrtstar."; - p.rrt_star_parameters.enable_update = getOrDeclareParameter(node, ns + "enable_update"); + p.rrt_star_parameters.enable_update = + get_or_declare_parameter(node, ns + "enable_update"); p.rrt_star_parameters.use_informed_sampling = - getOrDeclareParameter(node, ns + "use_informed_sampling"); + get_or_declare_parameter(node, ns + "use_informed_sampling"); p.rrt_star_parameters.max_planning_time = - getOrDeclareParameter(node, ns + "max_planning_time"); + get_or_declare_parameter(node, ns + "max_planning_time"); p.rrt_star_parameters.neighbor_radius = - getOrDeclareParameter(node, ns + "neighbor_radius"); - p.rrt_star_parameters.margin = getOrDeclareParameter(node, ns + "margin"); + get_or_declare_parameter(node, ns + "neighbor_radius"); + p.rrt_star_parameters.margin = get_or_declare_parameter(node, ns + "margin"); } const std::string base_ns = "start_planner.path_safety_check."; @@ -197,142 +200,142 @@ StartPlannerParameters StartPlannerParameters::init(rclcpp::Node & node) { const std::string ego_path_ns = base_ns + "ego_predicted_path."; p.ego_predicted_path_params.min_velocity = - getOrDeclareParameter(node, ego_path_ns + "min_velocity"); + get_or_declare_parameter(node, ego_path_ns + "min_velocity"); p.ego_predicted_path_params.acceleration = - getOrDeclareParameter(node, ego_path_ns + "min_acceleration"); + get_or_declare_parameter(node, ego_path_ns + "min_acceleration"); p.ego_predicted_path_params.time_horizon_for_front_object = - getOrDeclareParameter(node, ego_path_ns + "time_horizon_for_front_object"); + get_or_declare_parameter(node, ego_path_ns + "time_horizon_for_front_object"); p.ego_predicted_path_params.time_horizon_for_rear_object = - getOrDeclareParameter(node, ego_path_ns + "time_horizon_for_rear_object"); + get_or_declare_parameter(node, ego_path_ns + "time_horizon_for_rear_object"); p.ego_predicted_path_params.time_resolution = - getOrDeclareParameter(node, ego_path_ns + "time_resolution"); + get_or_declare_parameter(node, ego_path_ns + "time_resolution"); p.ego_predicted_path_params.delay_until_departure = - getOrDeclareParameter(node, ego_path_ns + "delay_until_departure"); + get_or_declare_parameter(node, ego_path_ns + "delay_until_departure"); } // ObjectFilteringParams const std::string obj_filter_ns = base_ns + "target_filtering."; { p.objects_filtering_params.safety_check_time_horizon = - getOrDeclareParameter(node, obj_filter_ns + "safety_check_time_horizon"); + get_or_declare_parameter(node, obj_filter_ns + "safety_check_time_horizon"); p.objects_filtering_params.safety_check_time_resolution = - getOrDeclareParameter(node, obj_filter_ns + "safety_check_time_resolution"); + get_or_declare_parameter(node, obj_filter_ns + "safety_check_time_resolution"); p.objects_filtering_params.object_check_forward_distance = - getOrDeclareParameter(node, obj_filter_ns + "object_check_forward_distance"); + get_or_declare_parameter(node, obj_filter_ns + "object_check_forward_distance"); p.objects_filtering_params.object_check_backward_distance = - getOrDeclareParameter(node, obj_filter_ns + "object_check_backward_distance"); + get_or_declare_parameter(node, obj_filter_ns + "object_check_backward_distance"); p.objects_filtering_params.ignore_object_velocity_threshold = - getOrDeclareParameter(node, obj_filter_ns + "ignore_object_velocity_threshold"); + get_or_declare_parameter(node, obj_filter_ns + "ignore_object_velocity_threshold"); p.objects_filtering_params.include_opposite_lane = - getOrDeclareParameter(node, obj_filter_ns + "include_opposite_lane"); + get_or_declare_parameter(node, obj_filter_ns + "include_opposite_lane"); p.objects_filtering_params.invert_opposite_lane = - getOrDeclareParameter(node, obj_filter_ns + "invert_opposite_lane"); + get_or_declare_parameter(node, obj_filter_ns + "invert_opposite_lane"); p.objects_filtering_params.check_all_predicted_path = - getOrDeclareParameter(node, obj_filter_ns + "check_all_predicted_path"); + get_or_declare_parameter(node, obj_filter_ns + "check_all_predicted_path"); p.objects_filtering_params.use_all_predicted_path = - getOrDeclareParameter(node, obj_filter_ns + "use_all_predicted_path"); + get_or_declare_parameter(node, obj_filter_ns + "use_all_predicted_path"); p.objects_filtering_params.use_predicted_path_outside_lanelet = - getOrDeclareParameter(node, obj_filter_ns + "use_predicted_path_outside_lanelet"); + get_or_declare_parameter(node, obj_filter_ns + "use_predicted_path_outside_lanelet"); } // ObjectTypesToCheck { const std::string obj_types_ns = obj_filter_ns + "object_types_to_check."; p.objects_filtering_params.object_types_to_check.check_car = - getOrDeclareParameter(node, obj_types_ns + "check_car"); + get_or_declare_parameter(node, obj_types_ns + "check_car"); p.objects_filtering_params.object_types_to_check.check_truck = - getOrDeclareParameter(node, obj_types_ns + "check_truck"); + get_or_declare_parameter(node, obj_types_ns + "check_truck"); p.objects_filtering_params.object_types_to_check.check_bus = - getOrDeclareParameter(node, obj_types_ns + "check_bus"); + get_or_declare_parameter(node, obj_types_ns + "check_bus"); p.objects_filtering_params.object_types_to_check.check_trailer = - getOrDeclareParameter(node, obj_types_ns + "check_trailer"); + get_or_declare_parameter(node, obj_types_ns + "check_trailer"); p.objects_filtering_params.object_types_to_check.check_unknown = - getOrDeclareParameter(node, obj_types_ns + "check_unknown"); + get_or_declare_parameter(node, obj_types_ns + "check_unknown"); p.objects_filtering_params.object_types_to_check.check_bicycle = - getOrDeclareParameter(node, obj_types_ns + "check_bicycle"); + get_or_declare_parameter(node, obj_types_ns + "check_bicycle"); p.objects_filtering_params.object_types_to_check.check_motorcycle = - getOrDeclareParameter(node, obj_types_ns + "check_motorcycle"); + get_or_declare_parameter(node, obj_types_ns + "check_motorcycle"); p.objects_filtering_params.object_types_to_check.check_pedestrian = - getOrDeclareParameter(node, obj_types_ns + "check_pedestrian"); + get_or_declare_parameter(node, obj_types_ns + "check_pedestrian"); } // ObjectLaneConfiguration { const std::string obj_lane_ns = obj_filter_ns + "object_lane_configuration."; p.objects_filtering_params.object_lane_configuration.check_current_lane = - getOrDeclareParameter(node, obj_lane_ns + "check_current_lane"); + get_or_declare_parameter(node, obj_lane_ns + "check_current_lane"); p.objects_filtering_params.object_lane_configuration.check_right_lane = - getOrDeclareParameter(node, obj_lane_ns + "check_right_side_lane"); + get_or_declare_parameter(node, obj_lane_ns + "check_right_side_lane"); p.objects_filtering_params.object_lane_configuration.check_left_lane = - getOrDeclareParameter(node, obj_lane_ns + "check_left_side_lane"); + get_or_declare_parameter(node, obj_lane_ns + "check_left_side_lane"); p.objects_filtering_params.object_lane_configuration.check_shoulder_lane = - getOrDeclareParameter(node, obj_lane_ns + "check_shoulder_lane"); + get_or_declare_parameter(node, obj_lane_ns + "check_shoulder_lane"); p.objects_filtering_params.object_lane_configuration.check_other_lane = - getOrDeclareParameter(node, obj_lane_ns + "check_other_lane"); + get_or_declare_parameter(node, obj_lane_ns + "check_other_lane"); } // SafetyCheckParams const std::string safety_check_ns = base_ns + "safety_check_params."; { p.safety_check_params.enable_safety_check = - getOrDeclareParameter(node, safety_check_ns + "enable_safety_check"); + get_or_declare_parameter(node, safety_check_ns + "enable_safety_check"); p.safety_check_params.hysteresis_factor_expand_rate = - getOrDeclareParameter(node, safety_check_ns + "hysteresis_factor_expand_rate"); + get_or_declare_parameter(node, safety_check_ns + "hysteresis_factor_expand_rate"); p.safety_check_params.backward_path_length = - getOrDeclareParameter(node, safety_check_ns + "backward_path_length"); + get_or_declare_parameter(node, safety_check_ns + "backward_path_length"); p.safety_check_params.forward_path_length = - getOrDeclareParameter(node, safety_check_ns + "forward_path_length"); + get_or_declare_parameter(node, safety_check_ns + "forward_path_length"); p.safety_check_params.publish_debug_marker = - getOrDeclareParameter(node, safety_check_ns + "publish_debug_marker"); - p.safety_check_params.collision_check_yaw_diff_threshold = - getOrDeclareParameter(node, safety_check_ns + "collision_check_yaw_diff_threshold"); + get_or_declare_parameter(node, safety_check_ns + "publish_debug_marker"); + p.safety_check_params.collision_check_yaw_diff_threshold = get_or_declare_parameter( + node, safety_check_ns + "collision_check_yaw_diff_threshold"); } // RSSparams { const std::string rss_ns = safety_check_ns + "rss_params."; p.safety_check_params.rss_params.rear_vehicle_reaction_time = - getOrDeclareParameter(node, rss_ns + "rear_vehicle_reaction_time"); + get_or_declare_parameter(node, rss_ns + "rear_vehicle_reaction_time"); p.safety_check_params.rss_params.rear_vehicle_safety_time_margin = - getOrDeclareParameter(node, rss_ns + "rear_vehicle_safety_time_margin"); + get_or_declare_parameter(node, rss_ns + "rear_vehicle_safety_time_margin"); p.safety_check_params.rss_params.lateral_distance_max_threshold = - getOrDeclareParameter(node, rss_ns + "lateral_distance_max_threshold"); + get_or_declare_parameter(node, rss_ns + "lateral_distance_max_threshold"); p.safety_check_params.rss_params.longitudinal_distance_min_threshold = - getOrDeclareParameter(node, rss_ns + "longitudinal_distance_min_threshold"); + get_or_declare_parameter(node, rss_ns + "longitudinal_distance_min_threshold"); p.safety_check_params.rss_params.longitudinal_velocity_delta_time = - getOrDeclareParameter(node, rss_ns + "longitudinal_velocity_delta_time"); + get_or_declare_parameter(node, rss_ns + "longitudinal_velocity_delta_time"); p.safety_check_params.rss_params.extended_polygon_policy = - getOrDeclareParameter(node, rss_ns + "extended_polygon_policy"); + get_or_declare_parameter(node, rss_ns + "extended_polygon_policy"); } // surround moving obstacle check { const std::string surround_moving_obstacle_check_ns = "start_planner.surround_moving_obstacle_check."; p.search_radius = - getOrDeclareParameter(node, surround_moving_obstacle_check_ns + "search_radius"); - p.th_moving_obstacle_velocity = getOrDeclareParameter( + get_or_declare_parameter(node, surround_moving_obstacle_check_ns + "search_radius"); + p.th_moving_obstacle_velocity = get_or_declare_parameter( node, surround_moving_obstacle_check_ns + "th_moving_obstacle_velocity"); // ObjectTypesToCheck { const std::string obj_types_ns = surround_moving_obstacle_check_ns + "object_types_to_check."; p.surround_moving_obstacles_type_to_check.check_car = - getOrDeclareParameter(node, obj_types_ns + "check_car"); + get_or_declare_parameter(node, obj_types_ns + "check_car"); p.surround_moving_obstacles_type_to_check.check_truck = - getOrDeclareParameter(node, obj_types_ns + "check_truck"); + get_or_declare_parameter(node, obj_types_ns + "check_truck"); p.surround_moving_obstacles_type_to_check.check_bus = - getOrDeclareParameter(node, obj_types_ns + "check_bus"); + get_or_declare_parameter(node, obj_types_ns + "check_bus"); p.surround_moving_obstacles_type_to_check.check_trailer = - getOrDeclareParameter(node, obj_types_ns + "check_trailer"); + get_or_declare_parameter(node, obj_types_ns + "check_trailer"); p.surround_moving_obstacles_type_to_check.check_unknown = - getOrDeclareParameter(node, obj_types_ns + "check_unknown"); + get_or_declare_parameter(node, obj_types_ns + "check_unknown"); p.surround_moving_obstacles_type_to_check.check_bicycle = - getOrDeclareParameter(node, obj_types_ns + "check_bicycle"); + get_or_declare_parameter(node, obj_types_ns + "check_bicycle"); p.surround_moving_obstacles_type_to_check.check_motorcycle = - getOrDeclareParameter(node, obj_types_ns + "check_motorcycle"); + get_or_declare_parameter(node, obj_types_ns + "check_motorcycle"); p.surround_moving_obstacles_type_to_check.check_pedestrian = - getOrDeclareParameter(node, obj_types_ns + "check_pedestrian"); + get_or_declare_parameter(node, obj_types_ns + "check_pedestrian"); } } // debug { const std::string debug_ns = "start_planner.debug."; - p.print_debug_info = getOrDeclareParameter(node, debug_ns + "print_debug_info"); + p.print_debug_info = get_or_declare_parameter(node, debug_ns + "print_debug_info"); } return p; diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/freespace_pull_out.cpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/freespace_pull_out.cpp index 6893c7d11d09d..3fb11fb0e383d 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/freespace_pull_out.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/freespace_pull_out.cpp @@ -86,7 +86,7 @@ std::optional FreespacePullOut::plan( const size_t index = std::distance(last_path.points.begin(), it); if (index == 0) continue; const double distance = - autoware::universe_utils::calcDistance2d(end_pose.position, it->point.pose.position); + autoware_utils::calc_distance2d(end_pose.position, it->point.pose.position); if (distance < th_end_distance) { last_path.points.erase(it, last_path.points.end()); break; diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/geometric_pull_out.cpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/geometric_pull_out.cpp index ed09e0c0a33d6..b5bfa59b9900c 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/geometric_pull_out.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/geometric_pull_out.cpp @@ -18,7 +18,7 @@ #include "autoware/behavior_path_planner_common/utils/path_utils.hpp" #include "autoware/behavior_path_planner_common/utils/utils.hpp" #include "autoware/behavior_path_start_planner_module/util.hpp" -#include "autoware/universe_utils/geometry/boost_polygon_utils.hpp" +#include "autoware_utils/geometry/boost_polygon_utils.hpp" #include @@ -27,8 +27,8 @@ #include using autoware::motion_utils::findNearestIndex; -using autoware::universe_utils::calcDistance2d; -using autoware::universe_utils::calcOffsetPose; +using autoware_utils::calc_distance2d; +using autoware_utils::calc_offset_pose; using lanelet::utils::getArcCoordinates; namespace autoware::behavior_path_planner { @@ -36,7 +36,7 @@ using start_planner_utils::getPullOutLanes; GeometricPullOut::GeometricPullOut( rclcpp::Node & node, const StartPlannerParameters & parameters, - std::shared_ptr time_keeper) + std::shared_ptr time_keeper) : PullOutPlannerBase{node, parameters, time_keeper}, parallel_parking_parameters_{parameters.parallel_parking_parameters} { diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/manager.cpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/manager.cpp index 9ea51d56ee1cc..6d2e0878b4d4f 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/manager.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/manager.cpp @@ -14,7 +14,7 @@ #include "autoware/behavior_path_start_planner_module/manager.hpp" -#include "autoware/universe_utils/ros/update_param.hpp" +#include "autoware_utils/ros/update_param.hpp" #include @@ -46,149 +46,152 @@ void StartPlannerModuleManager::init(rclcpp::Node * node) void StartPlannerModuleManager::updateModuleParams( [[maybe_unused]] const std::vector & parameters) { - using autoware::universe_utils::updateParam; + using autoware_utils::update_param; auto & p = parameters_; { const std::string ns = "start_planner."; - updateParam(parameters, ns + "th_arrived_distance", p->th_arrived_distance); - updateParam(parameters, ns + "th_stopped_velocity", p->th_stopped_velocity); - updateParam(parameters, ns + "th_stopped_time", p->th_stopped_time); - updateParam(parameters, ns + "prepare_time_before_start", p->prepare_time_before_start); - updateParam( + update_param(parameters, ns + "th_arrived_distance", p->th_arrived_distance); + update_param(parameters, ns + "th_stopped_velocity", p->th_stopped_velocity); + update_param(parameters, ns + "th_stopped_time", p->th_stopped_time); + update_param( + parameters, ns + "prepare_time_before_start", p->prepare_time_before_start); + update_param( parameters, ns + "th_distance_to_middle_of_the_road", p->th_distance_to_middle_of_the_road); - updateParam(parameters, ns + "skip_rear_vehicle_check", p->skip_rear_vehicle_check); - updateParam( + update_param(parameters, ns + "skip_rear_vehicle_check", p->skip_rear_vehicle_check); + update_param( parameters, ns + "extra_width_margin_for_rear_obstacle", p->extra_width_margin_for_rear_obstacle); - updateParam>( + update_param>( parameters, ns + "collision_check_margins", p->collision_check_margins); - updateParam( + update_param( parameters, ns + "collision_check_margin_from_front_object", p->collision_check_margin_from_front_object); - updateParam(parameters, ns + "th_moving_object_velocity", p->th_moving_object_velocity); + update_param( + parameters, ns + "th_moving_object_velocity", p->th_moving_object_velocity); const std::string obj_types_ns = ns + "object_types_to_check_for_path_generation."; { - updateParam( + update_param( parameters, obj_types_ns + "check_car", p->object_types_to_check_for_path_generation.check_car); - updateParam( + update_param( parameters, obj_types_ns + "check_truck", p->object_types_to_check_for_path_generation.check_truck); - updateParam( + update_param( parameters, obj_types_ns + "check_bus", p->object_types_to_check_for_path_generation.check_bus); - updateParam( + update_param( parameters, obj_types_ns + "check_trailer", p->object_types_to_check_for_path_generation.check_trailer); - updateParam( + update_param( parameters, obj_types_ns + "check_unknown", p->object_types_to_check_for_path_generation.check_unknown); - updateParam( + update_param( parameters, obj_types_ns + "check_bicycle", p->object_types_to_check_for_path_generation.check_bicycle); - updateParam( + update_param( parameters, obj_types_ns + "check_motorcycle", p->object_types_to_check_for_path_generation.check_motorcycle); - updateParam( + update_param( parameters, obj_types_ns + "check_pedestrian", p->object_types_to_check_for_path_generation.check_pedestrian); } - updateParam(parameters, ns + "center_line_path_interval", p->center_line_path_interval); - updateParam(parameters, ns + "enable_shift_pull_out", p->enable_shift_pull_out); - updateParam( + update_param( + parameters, ns + "center_line_path_interval", p->center_line_path_interval); + update_param(parameters, ns + "enable_shift_pull_out", p->enable_shift_pull_out); + update_param( parameters, ns + "shift_collision_check_distance_from_end", p->shift_collision_check_distance_from_end); - updateParam( + update_param( parameters, ns + "minimum_shift_pull_out_distance", p->minimum_shift_pull_out_distance); - updateParam( + update_param( parameters, ns + "lateral_acceleration_sampling_num", p->lateral_acceleration_sampling_num); - updateParam(parameters, ns + "lateral_jerk", p->lateral_jerk); - updateParam(parameters, ns + "maximum_lateral_acc", p->maximum_lateral_acc); - updateParam(parameters, ns + "minimum_lateral_acc", p->minimum_lateral_acc); - updateParam(parameters, ns + "maximum_curvature", p->maximum_curvature); - updateParam( + update_param(parameters, ns + "lateral_jerk", p->lateral_jerk); + update_param(parameters, ns + "maximum_lateral_acc", p->maximum_lateral_acc); + update_param(parameters, ns + "minimum_lateral_acc", p->minimum_lateral_acc); + update_param(parameters, ns + "maximum_curvature", p->maximum_curvature); + update_param( parameters, ns + "end_pose_curvature_threshold", p->end_pose_curvature_threshold); - updateParam( + update_param( parameters, ns + "maximum_longitudinal_deviation", p->maximum_longitudinal_deviation); - updateParam(parameters, ns + "enable_geometric_pull_out", p->enable_geometric_pull_out); - updateParam(parameters, ns + "divide_pull_out_path", p->divide_pull_out_path); - updateParam( + update_param(parameters, ns + "enable_geometric_pull_out", p->enable_geometric_pull_out); + update_param(parameters, ns + "divide_pull_out_path", p->divide_pull_out_path); + update_param( parameters, ns + "arc_path_interval", p->parallel_parking_parameters.pull_out_arc_path_interval); - updateParam( + update_param( parameters, ns + "lane_departure_margin", p->parallel_parking_parameters.pull_out_lane_departure_margin); - updateParam( + update_param( parameters, ns + "lane_departure_check_expansion_margin", p->lane_departure_check_expansion_margin); - updateParam( + update_param( parameters, ns + "pull_out_max_steer_angle", p->parallel_parking_parameters.pull_out_max_steer_angle); - updateParam(parameters, ns + "enable_back", p->enable_back); - updateParam(parameters, ns + "backward_velocity", p->backward_velocity); - updateParam( + update_param(parameters, ns + "enable_back", p->enable_back); + update_param(parameters, ns + "backward_velocity", p->backward_velocity); + update_param( parameters, ns + "geometric_pull_out_velocity", p->parallel_parking_parameters.pull_out_velocity); - updateParam( + update_param( parameters, ns + "geometric_collision_check_distance_from_end", p->geometric_collision_check_distance_from_end); - updateParam( + update_param( parameters, ns + "check_shift_path_lane_departure", p->check_shift_path_lane_departure); - updateParam( + update_param( parameters, ns + "allow_check_shift_path_lane_departure_override", p->allow_check_shift_path_lane_departure_override); - updateParam(parameters, ns + "search_priority", p->search_priority); - updateParam(parameters, ns + "max_back_distance", p->max_back_distance); - updateParam( + update_param(parameters, ns + "search_priority", p->search_priority); + update_param(parameters, ns + "max_back_distance", p->max_back_distance); + update_param( parameters, ns + "backward_search_resolution", p->backward_search_resolution); - updateParam( + update_param( parameters, ns + "backward_path_update_duration", p->backward_path_update_duration); - updateParam( + update_param( parameters, ns + "ignore_distance_from_lane_end", p->ignore_distance_from_lane_end); - updateParam( + update_param( parameters, ns + "stop_condition.maximum_deceleration_for_stop", p->maximum_deceleration_for_stop); - updateParam( + update_param( parameters, ns + "stop_condition.maximum_jerk_for_stop", p->maximum_jerk_for_stop); } { const std::string ns = "start_planner.freespace_planner."; - updateParam(parameters, ns + "enable_freespace_planner", p->enable_freespace_planner); - updateParam( + update_param(parameters, ns + "enable_freespace_planner", p->enable_freespace_planner); + update_param( parameters, ns + "freespace_planner_algorithm", p->freespace_planner_algorithm); - updateParam( + update_param( parameters, ns + "end_pose_search_start_distance", p->end_pose_search_start_distance); - updateParam( + update_param( parameters, ns + "end_pose_search_end_distance", p->end_pose_search_end_distance); - updateParam(parameters, ns + "end_pose_search_interval", p->end_pose_search_interval); - updateParam(parameters, ns + "velocity", p->freespace_planner_velocity); - updateParam(parameters, ns + "vehicle_shape_margin", p->vehicle_shape_margin); - updateParam( + update_param(parameters, ns + "end_pose_search_interval", p->end_pose_search_interval); + update_param(parameters, ns + "velocity", p->freespace_planner_velocity); + update_param(parameters, ns + "vehicle_shape_margin", p->vehicle_shape_margin); + update_param( parameters, ns + "time_limit", p->freespace_planner_common_parameters.time_limit); - updateParam( + update_param( parameters, ns + "max_turning_ratio", p->freespace_planner_common_parameters.max_turning_ratio); - updateParam( + update_param( parameters, ns + "turning_steps", p->freespace_planner_common_parameters.turning_steps); } { const std::string ns = "start_planner.freespace_planner.search_configs."; - updateParam( + update_param( parameters, ns + "theta_size", p->freespace_planner_common_parameters.theta_size); - updateParam( + update_param( parameters, ns + "angle_goal_range", p->freespace_planner_common_parameters.angle_goal_range); - updateParam( + update_param( parameters, ns + "curve_weight", p->freespace_planner_common_parameters.curve_weight); - updateParam( + update_param( parameters, ns + "reverse_weight", p->freespace_planner_common_parameters.reverse_weight); - updateParam( + update_param( parameters, ns + "lateral_goal_range", p->freespace_planner_common_parameters.lateral_goal_range); - updateParam( + update_param( parameters, ns + "longitudinal_goal_range", p->freespace_planner_common_parameters.longitudinal_goal_range); } @@ -196,7 +199,7 @@ void StartPlannerModuleManager::updateModuleParams( { const std::string ns = "start_planner.freespace_planner.costmap_configs."; - updateParam( + update_param( parameters, ns + "obstacle_threshold", p->freespace_planner_common_parameters.obstacle_threshold); } @@ -204,205 +207,206 @@ void StartPlannerModuleManager::updateModuleParams( { const std::string ns = "start_planner.freespace_planner.astar."; - updateParam(parameters, ns + "search_method", p->astar_parameters.search_method); - updateParam(parameters, ns + "use_back", p->astar_parameters.use_back); - updateParam( + update_param(parameters, ns + "search_method", p->astar_parameters.search_method); + update_param(parameters, ns + "use_back", p->astar_parameters.use_back); + update_param( parameters, ns + "only_behind_solutions", p->astar_parameters.only_behind_solutions); - updateParam( + update_param( parameters, ns + "distance_heuristic_weight", p->astar_parameters.distance_heuristic_weight); } { const std::string ns = "start_planner.freespace_planner.rrtstar."; - updateParam(parameters, ns + "enable_update", p->rrt_star_parameters.enable_update); - updateParam( + update_param(parameters, ns + "enable_update", p->rrt_star_parameters.enable_update); + update_param( parameters, ns + "use_informed_sampling", p->rrt_star_parameters.use_informed_sampling); - updateParam( + update_param( parameters, ns + "max_planning_time", p->rrt_star_parameters.max_planning_time); - updateParam(parameters, ns + "neighbor_radius", p->rrt_star_parameters.neighbor_radius); - updateParam(parameters, ns + "margin", p->rrt_star_parameters.margin); + update_param( + parameters, ns + "neighbor_radius", p->rrt_star_parameters.neighbor_radius); + update_param(parameters, ns + "margin", p->rrt_star_parameters.margin); } const std::string base_ns = "start_planner.path_safety_check."; const std::string ego_path_ns = base_ns + "ego_predicted_path."; { - updateParam( + update_param( parameters, ego_path_ns + "min_velocity", p->ego_predicted_path_params.min_velocity); - updateParam( + update_param( parameters, ego_path_ns + "min_acceleration", p->ego_predicted_path_params.acceleration); - updateParam( + update_param( parameters, ego_path_ns + "time_horizon_for_front_object", p->ego_predicted_path_params.time_horizon_for_front_object); - updateParam( + update_param( parameters, ego_path_ns + "time_horizon_for_rear_object", p->ego_predicted_path_params.time_horizon_for_rear_object); - updateParam( + update_param( parameters, ego_path_ns + "time_resolution", p->ego_predicted_path_params.time_resolution); - updateParam( + update_param( parameters, ego_path_ns + "delay_until_departure", p->ego_predicted_path_params.delay_until_departure); } const std::string obj_filter_ns = base_ns + "target_filtering."; { - updateParam( + update_param( parameters, obj_filter_ns + "safety_check_time_horizon", p->objects_filtering_params.safety_check_time_horizon); - updateParam( + update_param( parameters, obj_filter_ns + "safety_check_time_resolution", p->objects_filtering_params.safety_check_time_resolution); - updateParam( + update_param( parameters, obj_filter_ns + "object_check_forward_distance", p->objects_filtering_params.object_check_forward_distance); - updateParam( + update_param( parameters, obj_filter_ns + "object_check_backward_distance", p->objects_filtering_params.object_check_backward_distance); - updateParam( + update_param( parameters, obj_filter_ns + "ignore_object_velocity_threshold", p->objects_filtering_params.ignore_object_velocity_threshold); - updateParam( + update_param( parameters, obj_filter_ns + "include_opposite_lane", p->objects_filtering_params.include_opposite_lane); - updateParam( + update_param( parameters, obj_filter_ns + "invert_opposite_lane", p->objects_filtering_params.invert_opposite_lane); - updateParam( + update_param( parameters, obj_filter_ns + "check_all_predicted_path", p->objects_filtering_params.check_all_predicted_path); - updateParam( + update_param( parameters, obj_filter_ns + "use_all_predicted_path", p->objects_filtering_params.use_all_predicted_path); - updateParam( + update_param( parameters, obj_filter_ns + "use_predicted_path_outside_lanelet", p->objects_filtering_params.use_predicted_path_outside_lanelet); } { const std::string obj_types_ns = obj_filter_ns + "object_types_to_check."; - updateParam( + update_param( parameters, obj_types_ns + "check_car", p->objects_filtering_params.object_types_to_check.check_car); - updateParam( + update_param( parameters, obj_types_ns + "check_truck", p->objects_filtering_params.object_types_to_check.check_truck); - updateParam( + update_param( parameters, obj_types_ns + "check_bus", p->objects_filtering_params.object_types_to_check.check_bus); - updateParam( + update_param( parameters, obj_types_ns + "check_trailer", p->objects_filtering_params.object_types_to_check.check_trailer); - updateParam( + update_param( parameters, obj_types_ns + "check_unknown", p->objects_filtering_params.object_types_to_check.check_unknown); - updateParam( + update_param( parameters, obj_types_ns + "check_bicycle", p->objects_filtering_params.object_types_to_check.check_bicycle); - updateParam( + update_param( parameters, obj_types_ns + "check_motorcycle", p->objects_filtering_params.object_types_to_check.check_motorcycle); - updateParam( + update_param( parameters, obj_types_ns + "check_pedestrian", p->objects_filtering_params.object_types_to_check.check_pedestrian); } const std::string obj_lane_ns = obj_filter_ns + "object_lane_configuration."; { - updateParam( + update_param( parameters, obj_lane_ns + "check_current_lane", p->objects_filtering_params.object_lane_configuration.check_current_lane); - updateParam( + update_param( parameters, obj_lane_ns + "check_right_side_lane", p->objects_filtering_params.object_lane_configuration.check_right_lane); - updateParam( + update_param( parameters, obj_lane_ns + "check_left_side_lane", p->objects_filtering_params.object_lane_configuration.check_left_lane); - updateParam( + update_param( parameters, obj_lane_ns + "check_shoulder_lane", p->objects_filtering_params.object_lane_configuration.check_shoulder_lane); - updateParam( + update_param( parameters, obj_lane_ns + "check_other_lane", p->objects_filtering_params.object_lane_configuration.check_other_lane); } const std::string safety_check_ns = base_ns + "safety_check_params."; { - updateParam( + update_param( parameters, safety_check_ns + "enable_safety_check", p->safety_check_params.enable_safety_check); - updateParam( + update_param( parameters, safety_check_ns + "hysteresis_factor_expand_rate", p->safety_check_params.hysteresis_factor_expand_rate); - updateParam( + update_param( parameters, safety_check_ns + "backward_path_length", p->safety_check_params.backward_path_length); - updateParam( + update_param( parameters, safety_check_ns + "forward_path_length", p->safety_check_params.forward_path_length); - updateParam( + update_param( parameters, safety_check_ns + "publish_debug_marker", p->safety_check_params.publish_debug_marker); - updateParam( + update_param( parameters, safety_check_ns + "collision_check_yaw_diff_threshold", p->safety_check_params.collision_check_yaw_diff_threshold); } { const std::string rss_ns = safety_check_ns + "rss_params."; - updateParam( + update_param( parameters, rss_ns + "rear_vehicle_reaction_time", p->safety_check_params.rss_params.rear_vehicle_reaction_time); - updateParam( + update_param( parameters, rss_ns + "rear_vehicle_safety_time_margin", p->safety_check_params.rss_params.rear_vehicle_safety_time_margin); - updateParam( + update_param( parameters, rss_ns + "lateral_distance_max_threshold", p->safety_check_params.rss_params.lateral_distance_max_threshold); - updateParam( + update_param( parameters, rss_ns + "longitudinal_distance_min_threshold", p->safety_check_params.rss_params.longitudinal_distance_min_threshold); - updateParam( + update_param( parameters, rss_ns + "longitudinal_velocity_delta_time", p->safety_check_params.rss_params.longitudinal_velocity_delta_time); - updateParam( + update_param( parameters, rss_ns + "extended_polygon_policy", p->safety_check_params.rss_params.extended_polygon_policy); } { const std::string surround_moving_obstacle_check_ns = "start_planner.surround_moving_obstacle_check."; - updateParam( + update_param( parameters, surround_moving_obstacle_check_ns + "search_radius", p->search_radius); - updateParam( + update_param( parameters, surround_moving_obstacle_check_ns + "th_moving_obstacle_velocity", p->th_moving_obstacle_velocity); // ObjectTypesToCheck { std::string obj_types_ns = surround_moving_obstacle_check_ns + "object_types_to_check."; - updateParam( + update_param( parameters, obj_types_ns + "check_car", p->surround_moving_obstacles_type_to_check.check_car); - updateParam( + update_param( parameters, obj_types_ns + "check_truck", p->surround_moving_obstacles_type_to_check.check_truck); - updateParam( + update_param( parameters, obj_types_ns + "check_bus", p->surround_moving_obstacles_type_to_check.check_bus); - updateParam( + update_param( parameters, obj_types_ns + "check_trailer", p->surround_moving_obstacles_type_to_check.check_trailer); - updateParam( + update_param( parameters, obj_types_ns + "check_unknown", p->surround_moving_obstacles_type_to_check.check_unknown); - updateParam( + update_param( parameters, obj_types_ns + "check_bicycle", p->surround_moving_obstacles_type_to_check.check_bicycle); - updateParam( + update_param( parameters, obj_types_ns + "check_motorcycle", p->surround_moving_obstacles_type_to_check.check_motorcycle); - updateParam( + update_param( parameters, obj_types_ns + "check_pedestrian", p->surround_moving_obstacles_type_to_check.check_pedestrian); } @@ -410,7 +414,7 @@ void StartPlannerModuleManager::updateModuleParams( { const std::string debug_ns = "start_planner.debug."; - updateParam(parameters, debug_ns + "print_debug_info", p->print_debug_info); + update_param(parameters, debug_ns + "print_debug_info", p->print_debug_info); } std::for_each(observers_.begin(), observers_.end(), [&p](const auto & observer) { diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/pull_out_planner_base.cpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/pull_out_planner_base.cpp index a475b3ad582cb..790ffe9ec92e6 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/pull_out_planner_base.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/pull_out_planner_base.cpp @@ -23,7 +23,7 @@ bool PullOutPlannerBase::isPullOutPathCollided( const std::shared_ptr & planner_data, double collision_check_distance_from_end) const { - universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + autoware_utils::ScopedTimeTrack st(__func__, *time_keeper_); // check for collisions const auto & dynamic_objects = planner_data->dynamic_object; diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/shift_pull_out.cpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/shift_pull_out.cpp index 16a3d4ffd842a..9d65e9defe58b 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/shift_pull_out.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/shift_pull_out.cpp @@ -20,7 +20,7 @@ #include "autoware/behavior_path_planner_common/utils/utils.hpp" #include "autoware/behavior_path_start_planner_module/util.hpp" #include "autoware/motion_utils/trajectory/path_with_lane_id.hpp" -#include "autoware/universe_utils/geometry/boost_polygon_utils.hpp" +#include "autoware_utils/geometry/boost_polygon_utils.hpp" #include #include @@ -32,8 +32,8 @@ #include using autoware::motion_utils::findNearestIndex; -using autoware::universe_utils::calcDistance2d; -using autoware::universe_utils::calcOffsetPose; +using autoware_utils::calc_distance2d; +using autoware_utils::calc_offset_pose; using lanelet::utils::getArcCoordinates; namespace autoware::behavior_path_planner { @@ -41,7 +41,7 @@ using start_planner_utils::getPullOutLanes; ShiftPullOut::ShiftPullOut( rclcpp::Node & node, const StartPlannerParameters & parameters, - std::shared_ptr time_keeper) + std::shared_ptr time_keeper) : PullOutPlannerBase{node, parameters, time_keeper} { autoware::lane_departure_checker::Param lane_departure_checker_params; @@ -75,13 +75,13 @@ std::optional ShiftPullOut::plan( const auto lanelet_map_ptr = planner_data->route_handler->getLaneletMapPtr(); std::vector fused_id_start_to_end{}; - std::optional fused_polygon_start_to_end = std::nullopt; + std::optional fused_polygon_start_to_end = std::nullopt; std::vector fused_id_crop_points{}; - std::optional fused_polygon_crop_points = std::nullopt; + std::optional fused_polygon_crop_points = std::nullopt; // get safe path for (auto & pull_out_path : pull_out_paths) { - universe_utils::ScopedTimeTrack st("get safe path", *time_keeper_); + autoware_utils::ScopedTimeTrack st("get safe path", *time_keeper_); // shift path is not separate but only one. auto & shift_path = pull_out_path.partial_paths.front(); @@ -238,7 +238,7 @@ std::vector ShiftPullOut::calcPullOutPaths( const Pose & start_pose, const Pose & goal_pose, const BehaviorPathPlannerParameters & behavior_path_parameters) { - universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + autoware_utils::ScopedTimeTrack st(__func__, *time_keeper_); std::vector candidate_paths{}; diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/start_planner_module.cpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/start_planner_module.cpp index bff77fb141470..e973e7069d153 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/start_planner_module.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/start_planner_module.cpp @@ -48,7 +48,7 @@ using autoware::behavior_path_planner::utils::parking_departure::initializeColli using autoware::behavior_path_planner::utils::path_safety_checker::ExtendedPredictedObject; using autoware::motion_utils::calcLateralOffset; using autoware::motion_utils::calcLongitudinalOffsetPose; -using autoware::universe_utils::calcOffsetPose; +using autoware_utils::calc_offset_pose; // set as macro so that calling function name will be printed. // debug print is heavy. turn on only when debugging. @@ -205,7 +205,7 @@ void StartPlannerModule::updateObjectsFilteringParams( void StartPlannerModule::updateData() { - universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + autoware_utils::ScopedTimeTrack st(__func__, *time_keeper_); // The method PlannerManager::run() calls SceneModuleInterface::setData and // SceneModuleInterface::setPreviousModuleOutput() before this module's run() method is called @@ -271,8 +271,7 @@ bool StartPlannerModule::hasFinishedBackwardDriving() const { // check ego car is close enough to pull out start pose and stopped const auto current_pose = planner_data_->self_odometry->pose.pose; - const auto distance = - autoware::universe_utils::calcDistance2d(current_pose, status_.pull_out_start_pose); + const auto distance = autoware_utils::calc_distance2d(current_pose, status_.pull_out_start_pose); const bool is_near = distance < parameters_->th_arrived_distance; const double ego_vel = utils::l2Norm(planner_data_->self_odometry->twist.twist.linear); @@ -380,7 +379,7 @@ bool StartPlannerModule::isCurrentPoseOnMiddleOfTheRoad() const bool StartPlannerModule::isPreventingRearVehicleFromPassingThrough() const { - universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + autoware_utils::ScopedTimeTrack st(__func__, *time_keeper_); // prepare poses for preventing check // - current_pose @@ -411,7 +410,7 @@ bool StartPlannerModule::isPreventingRearVehicleFromPassingThrough() const bool StartPlannerModule::isPreventingRearVehicleFromPassingThrough(const Pose & ego_pose) const { - universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + autoware_utils::ScopedTimeTrack st(__func__, *time_keeper_); const auto & dynamic_object = planner_data_->dynamic_object; const auto & route_handler = planner_data_->route_handler; @@ -429,7 +428,7 @@ bool StartPlannerModule::isPreventingRearVehicleFromPassingThrough(const Pose & boundary_line.begin(), boundary_line.end(), [&boundary_path](const auto & boundary_point) { const double x = boundary_point.x(); const double y = boundary_point.y(); - boundary_path.push_back(autoware::universe_utils::createPoint(x, y, 0.0)); + boundary_path.push_back(autoware_utils::create_point(x, y, 0.0)); }); return std::fabs(calcLateralOffset(boundary_path, search_pose.position)); @@ -442,7 +441,7 @@ bool StartPlannerModule::isPreventingRearVehicleFromPassingThrough(const Pose & autoware::motion_utils::findNearestSegmentIndex(centerline_path.points, start_pose); if (!start_pose_nearest_segment_index) return false; - const auto start_pose_point_msg = autoware::universe_utils::createPoint( + const auto start_pose_point_msg = autoware_utils::create_point( start_pose.position.x, start_pose.position.y, start_pose.position.z); const auto starting_pose_lateral_offset = autoware::motion_utils::calcLateralOffset( centerline_path.points, start_pose_point_msg, start_pose_nearest_segment_index.value()); @@ -457,8 +456,8 @@ bool StartPlannerModule::isPreventingRearVehicleFromPassingThrough(const Pose & geometry_msgs::msg::Pose & ego_overhang_point_as_pose, const bool ego_is_merging_from_the_left) -> std::optional> { const auto local_vehicle_footprint = vehicle_info_.createFootprint(); - const auto vehicle_footprint = autoware::universe_utils::transformVector( - local_vehicle_footprint, autoware::universe_utils::pose2transform(ego_pose)); + const auto vehicle_footprint = autoware_utils::transform_vector( + local_vehicle_footprint, autoware_utils::pose2transform(ego_pose)); double smallest_lateral_gap_between_ego_and_border = std::numeric_limits::max(); double corresponding_lateral_gap_with_other_lane_bound = std::numeric_limits::max(); @@ -569,7 +568,7 @@ bool StartPlannerModule::isPreventingRearVehicleFromPassingThrough(const Pose & bool StartPlannerModule::isCloseToOriginalStartPose() const { const Pose start_pose = planner_data_->route_handler->getOriginalStartPose(); - return autoware::universe_utils::calcDistance2d( + return autoware_utils::calc_distance2d( start_pose.position, planner_data_->self_odometry->pose.pose.position) > parameters_->th_arrived_distance; } @@ -577,7 +576,7 @@ bool StartPlannerModule::isCloseToOriginalStartPose() const bool StartPlannerModule::hasArrivedAtGoal() const { const Pose goal_pose = planner_data_->route_handler->getGoalPose(); - return autoware::universe_utils::calcDistance2d( + return autoware_utils::calc_distance2d( goal_pose.position, planner_data_->self_odometry->pose.pose.position) < parameters_->th_arrived_distance; } @@ -662,7 +661,7 @@ bool StartPlannerModule::canTransitSuccessState() BehaviorModuleOutput StartPlannerModule::plan() { - universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + autoware_utils::ScopedTimeTrack st(__func__, *time_keeper_); if (isWaitingApproval()) { clearWaitingApproval(); @@ -680,7 +679,7 @@ BehaviorModuleOutput StartPlannerModule::plan() } const auto path = std::invoke([&]() { - universe_utils::ScopedTimeTrack st2("plan path", *time_keeper_); + autoware_utils::ScopedTimeTrack st2("plan path", *time_keeper_); if (!status_.driving_forward && !status_.backward_driving_complete) { return status_.backward_path; @@ -798,7 +797,7 @@ PathWithLaneId StartPlannerModule::getFullPath() const BehaviorModuleOutput StartPlannerModule::planWaitingApproval() { - universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + autoware_utils::ScopedTimeTrack st(__func__, *time_keeper_); updatePullOutStatus(); @@ -894,7 +893,7 @@ void StartPlannerModule::planWithPriority( const std::vector & start_pose_candidates, const Pose & refined_start_pose, const Pose & goal_pose, const std::string & search_priority) { - universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + autoware_utils::ScopedTimeTrack st(__func__, *time_keeper_); if (start_pose_candidates.empty()) return; @@ -903,7 +902,7 @@ void StartPlannerModule::planWithPriority( std::vector debug_data_vector; { // create a scope for the scoped time track - universe_utils::ScopedTimeTrack st2("findPullOutPaths", *time_keeper_); + autoware_utils::ScopedTimeTrack st2("findPullOutPaths", *time_keeper_); for (const auto & collision_check_margin : parameters_->collision_check_margins) { for (const auto & [index, planner] : order_priority) { @@ -925,7 +924,7 @@ void StartPlannerModule::planWithPriority( PriorityOrder StartPlannerModule::determinePriorityOrder( const std::string & search_priority, const size_t start_pose_candidates_num) { - universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + autoware_utils::ScopedTimeTrack st(__func__, *time_keeper_); PriorityOrder order_priority; if (search_priority == "efficient_path") { @@ -955,7 +954,7 @@ bool StartPlannerModule::findPullOutPath( // if start_pose_candidate is far from refined_start_pose, backward driving is necessary constexpr double epsilon = 0.01; const double backwards_distance = - autoware::universe_utils::calcDistance2d(start_pose_candidate, refined_start_pose); + autoware_utils::calc_distance2d(start_pose_candidate, refined_start_pose); const bool backward_is_unnecessary = backwards_distance < epsilon; planner->setCollisionCheckMargin(collision_check_margin); @@ -1016,7 +1015,7 @@ PathWithLaneId StartPlannerModule::generateStopPath() const { const auto & current_pose = planner_data_->self_odometry->pose.pose; constexpr double dummy_path_distance = 1.0; - const auto moved_pose = calcOffsetPose(current_pose, dummy_path_distance, 0, 0); + const auto moved_pose = calc_offset_pose(current_pose, dummy_path_distance, 0, 0); // convert Pose to PathPointWithLaneId with 0 velocity. auto toPathPointWithLaneId = [this](const Pose & pose) { @@ -1096,7 +1095,7 @@ std::vector StartPlannerModule::generateDrivableLanes( void StartPlannerModule::updatePullOutStatus() { - universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + autoware_utils::ScopedTimeTrack st(__func__, *time_keeper_); // skip updating if enough time has not passed for preventing chattering between back and // start_planner @@ -1160,13 +1159,13 @@ void StartPlannerModule::updateStatusAfterBackwardDriving() // To enable approval of the forward path, the RTC status is removed. removeRTCStatus(); for (auto & itr : uuid_map_) { - itr.second = generateUUID(); + itr.second = generate_uuid(); } } PathWithLaneId StartPlannerModule::calcBackwardPathFromStartPose() const { - universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + autoware_utils::ScopedTimeTrack st(__func__, *time_keeper_); const Pose start_pose = planner_data_->route_handler->getOriginalStartPose(); const auto pull_out_lanes = start_planner_utils::getPullOutLanes( @@ -1186,7 +1185,8 @@ PathWithLaneId StartPlannerModule::calcBackwardPathFromStartPose() const // shift all path points laterally to align with the start pose for (auto & path_point : path.points) { - path_point.point.pose = calcOffsetPose(path_point.point.pose, 0, arc_position_pose.distance, 0); + path_point.point.pose = + calc_offset_pose(path_point.point.pose, 0, arc_position_pose.distance, 0); } return path; @@ -1195,7 +1195,7 @@ PathWithLaneId StartPlannerModule::calcBackwardPathFromStartPose() const std::vector StartPlannerModule::searchPullOutStartPoseCandidates( const PathWithLaneId & back_path_from_start_pose) const { - universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + autoware_utils::ScopedTimeTrack st(__func__, *time_keeper_); std::vector pull_out_start_pose_candidates{}; const auto start_pose = planner_data_->route_handler->getOriginalStartPose(); @@ -1261,7 +1261,7 @@ PredictedObjects StartPlannerModule::filterStopObjectsInPullOutLanes( const double velocity_threshold, const double object_check_forward_distance, const double object_check_backward_distance) const { - universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + autoware_utils::ScopedTimeTrack st(__func__, *time_keeper_); const auto stop_objects = utils::path_safety_checker::filterObjectsByVelocity( *planner_data_->dynamic_object, velocity_threshold); @@ -1290,7 +1290,7 @@ PredictedObjects StartPlannerModule::filterStopObjectsInPullOutLanes( bool StartPlannerModule::hasReachedFreespaceEnd() const { const auto & current_pose = planner_data_->self_odometry->pose.pose; - return autoware::universe_utils::calcDistance2d(current_pose, status_.pull_out_path.end_pose) < + return autoware_utils::calc_distance2d(current_pose, status_.pull_out_path.end_pose) < parameters_->th_arrived_distance; } @@ -1332,8 +1332,8 @@ bool StartPlannerModule::hasFinishedCurrentPath() const auto current_path = getCurrentPath(); const auto current_path_end = current_path.points.back(); const auto self_pose = planner_data_->self_odometry->pose.pose; - const bool is_near_target = autoware::universe_utils::calcDistance2d( - current_path_end, self_pose) < parameters_->th_arrived_distance; + const bool is_near_target = + autoware_utils::calc_distance2d(current_path_end, self_pose) < parameters_->th_arrived_distance; return is_near_target && isStopped(); } @@ -1412,7 +1412,7 @@ TurnSignalInfo StartPlannerModule::calcTurnSignalInfo() bool StartPlannerModule::isSafePath() const { - universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + autoware_utils::ScopedTimeTrack st(__func__, *time_keeper_); // TODO(Sugahara): should safety check for backward path @@ -1691,9 +1691,9 @@ void SceneModuleVisitor::visitStartPlannerModule(const StartPlannerModule * modu void StartPlannerModule::setDebugData() { - using autoware::universe_utils::createDefaultMarker; - using autoware::universe_utils::createMarkerColor; - using autoware::universe_utils::createMarkerScale; + using autoware_utils::create_default_marker; + using autoware_utils::create_marker_color; + using autoware_utils::create_marker_scale; using lanelet::visualization::laneletsAsTriangleMarkerArray; using marker_utils::addFootprintMarker; using marker_utils::createFootprintMarkerArray; @@ -1706,18 +1706,18 @@ void StartPlannerModule::setDebugData() using marker_utils::showSafetyCheckInfo; using visualization_msgs::msg::Marker; - const auto red_color = createMarkerColor(1.0, 0.0, 0.0, 0.999); - const auto cyan_color = createMarkerColor(0.0, 1.0, 1.0, 0.2); - const auto pink_color = createMarkerColor(1.0, 0.5, 0.5, 0.35); - const auto purple_color = createMarkerColor(1.0, 0.0, 1.0, 0.99); - const auto white_color = createMarkerColor(1.0, 1.0, 1.0, 0.99); + const auto red_color = create_marker_color(1.0, 0.0, 0.0, 0.999); + const auto cyan_color = create_marker_color(0.0, 1.0, 1.0, 0.2); + const auto pink_color = create_marker_color(1.0, 0.5, 0.5, 0.35); + const auto purple_color = create_marker_color(1.0, 0.0, 1.0, 0.99); + const auto white_color = create_marker_color(1.0, 1.0, 1.0, 0.99); const auto life_time = rclcpp::Duration::from_seconds(1.5); auto add = [&](MarkerArray added, MarkerArray & target_marker_array) { for (auto & marker : added.markers) { marker.lifetime = life_time; } - autoware::universe_utils::appendMarkerArray(added, &target_marker_array); + autoware_utils::append_marker_array(added, &target_marker_array); }; debug_marker_.markers.clear(); @@ -1755,9 +1755,9 @@ void StartPlannerModule::setDebugData() createPoseMarkerArray( *collision_check_end_pose, "static_collision_check_end_pose", 0, 1.0, 0.0, 0.0), info_marker_); - auto marker = createDefaultMarker( + auto marker = create_default_marker( "map", rclcpp::Clock{RCL_ROS_TIME}.now(), "static_collision_check_end_polygon", 0, - Marker::LINE_STRIP, createMarkerScale(0.1, 0.1, 0.1), red_color); + Marker::LINE_STRIP, create_marker_scale(0.1, 0.1, 0.1), red_color); addFootprintMarker(marker, *collision_check_end_pose, vehicle_info_); marker.lifetime = life_time; info_marker_.markers.push_back(marker); @@ -1767,12 +1767,12 @@ void StartPlannerModule::setDebugData() { MarkerArray start_pose_footprint_marker_array{}; MarkerArray start_pose_text_marker_array{}; - Marker footprint_marker = createDefaultMarker( + Marker footprint_marker = create_default_marker( "map", rclcpp::Clock{RCL_ROS_TIME}.now(), "start_pose_candidates", 0, Marker::LINE_STRIP, - createMarkerScale(0.2, 0.2, 0.2), purple_color); - Marker text_marker = createDefaultMarker( + create_marker_scale(0.2, 0.2, 0.2), purple_color); + Marker text_marker = create_default_marker( "map", rclcpp::Clock{RCL_ROS_TIME}.now(), "start_pose_candidates_idx", 0, - visualization_msgs::msg::Marker::TEXT_VIEW_FACING, createMarkerScale(0.3, 0.3, 0.3), + visualization_msgs::msg::Marker::TEXT_VIEW_FACING, create_marker_scale(0.3, 0.3, 0.3), purple_color); footprint_marker.lifetime = rclcpp::Duration::from_seconds(1.5); text_marker.lifetime = rclcpp::Duration::from_seconds(1.5); @@ -1794,9 +1794,9 @@ void StartPlannerModule::setDebugData() // visualize the footprint from pull_out_start pose to pull_out_end pose along the path { MarkerArray pull_out_path_footprint_marker_array{}; - Marker pull_out_path_footprint_marker = createDefaultMarker( + Marker pull_out_path_footprint_marker = create_default_marker( "map", rclcpp::Clock{RCL_ROS_TIME}.now(), "shift_path_footprint", 0, Marker::LINE_STRIP, - createMarkerScale(0.2, 0.2, 0.2), pink_color); + create_marker_scale(0.2, 0.2, 0.2), pink_color); pull_out_path_footprint_marker.lifetime = rclcpp::Duration::from_seconds(1.5); PathWithLaneId path_shift_start_to_end{}; const auto shift_path = status_.pull_out_path.partial_paths.front(); @@ -1849,9 +1849,9 @@ void StartPlannerModule::setDebugData() // visualize estimated_stop_pose for isPreventingRearVehicleFromPassingThrough() if (debug_data_.estimated_stop_pose.has_value()) { - auto footprint_marker = createDefaultMarker( + auto footprint_marker = create_default_marker( "map", rclcpp::Clock{RCL_ROS_TIME}.now(), "estimated_stop_pose", 0, Marker::LINE_STRIP, - createMarkerScale(0.2, 0.2, 0.2), purple_color); + create_marker_scale(0.2, 0.2, 0.2), purple_color); footprint_marker.lifetime = rclcpp::Duration::from_seconds(1.5); addFootprintMarker(footprint_marker, debug_data_.estimated_stop_pose.value(), vehicle_info_); debug_marker_.markers.push_back(footprint_marker); @@ -1871,9 +1871,9 @@ void StartPlannerModule::setDebugData() { visualization_msgs::msg::MarkerArray planner_type_marker_array{}; const auto color = status_.found_pull_out_path ? white_color : red_color; - auto marker = createDefaultMarker( + auto marker = create_default_marker( header.frame_id, header.stamp, "planner_type", 0, - visualization_msgs::msg::Marker::TEXT_VIEW_FACING, createMarkerScale(0.0, 0.0, 1.0), color); + visualization_msgs::msg::Marker::TEXT_VIEW_FACING, create_marker_scale(0.0, 0.0, 1.0), color); marker.pose = status_.pull_out_start_pose; if (!status_.driving_forward) { marker.text = "BACK -> "; diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/util.cpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/util.cpp index d386665c181d6..819252b7e3d15 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/util.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/util.cpp @@ -19,8 +19,8 @@ #include "autoware/behavior_path_planner_common/utils/utils.hpp" #include -#include #include +#include #include #include @@ -65,7 +65,7 @@ PathWithLaneId getBackwardPath( const double lateral_distance_to_shoulder_center = current_pose_arc_coords.distance; for (size_t i = 0; i < backward_path.points.size(); ++i) { auto & p = backward_path.points.at(i).point.pose; - p = autoware::universe_utils::calcOffsetPose(p, 0, lateral_distance_to_shoulder_center, 0); + p = autoware_utils::calc_offset_pose(p, 0, lateral_distance_to_shoulder_center, 0); } } diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/helper.hpp b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/helper.hpp index 1575d764fffd0..6dd8658be0bc7 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/helper.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/helper.hpp @@ -527,7 +527,7 @@ class AvoidanceHelper (std::sqrt(v0 * v0 + 2.0 * p->max_acceleration * x_end) - v0) / p->max_acceleration; const auto v_end = v0 + p->max_acceleration * t_end; - max_v_point_ = std::make_pair(getPose(path.points.back()), v_end); + max_v_point_ = std::make_pair(autoware_utils::get_pose(path.points.back()), v_end); } void reset() diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/parameter_helper.hpp b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/parameter_helper.hpp index 7d4e9f2ae69eb..37231477e165b 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/parameter_helper.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/parameter_helper.hpp @@ -14,7 +14,7 @@ #ifndef AUTOWARE__BEHAVIOR_PATH_STATIC_OBSTACLE_AVOIDANCE_MODULE__PARAMETER_HELPER_HPP_ #define AUTOWARE__BEHAVIOR_PATH_STATIC_OBSTACLE_AVOIDANCE_MODULE__PARAMETER_HELPER_HPP_ -#include "autoware/universe_utils/ros/parameter.hpp" +#include "autoware_utils/ros/parameter.hpp" #include #include @@ -26,8 +26,8 @@ namespace autoware::behavior_path_planner { -using autoware::universe_utils::getOrDeclareParameter; using autoware_perception_msgs::msg::ObjectClassification; +using autoware_utils::get_or_declare_parameter; AvoidanceParameters getParameter(rclcpp::Node * node) { @@ -36,41 +36,43 @@ AvoidanceParameters getParameter(rclcpp::Node * node) { const std::string ns = "avoidance."; p.resample_interval_for_planning = - getOrDeclareParameter(*node, ns + "resample_interval_for_planning"); + get_or_declare_parameter(*node, ns + "resample_interval_for_planning"); p.resample_interval_for_output = - getOrDeclareParameter(*node, ns + "resample_interval_for_output"); + get_or_declare_parameter(*node, ns + "resample_interval_for_output"); p.path_generation_method = - getOrDeclareParameter(*node, ns + "path_generation_method"); + get_or_declare_parameter(*node, ns + "path_generation_method"); } // drivable area { const std::string ns = "avoidance."; - p.use_lane_type = getOrDeclareParameter(*node, ns + "use_lane_type"); - p.use_intersection_areas = getOrDeclareParameter(*node, ns + "use_intersection_areas"); + p.use_lane_type = get_or_declare_parameter(*node, ns + "use_lane_type"); + p.use_intersection_areas = get_or_declare_parameter(*node, ns + "use_intersection_areas"); p.use_hatched_road_markings = - getOrDeclareParameter(*node, ns + "use_hatched_road_markings"); - p.use_freespace_areas = getOrDeclareParameter(*node, ns + "use_freespace_areas"); + get_or_declare_parameter(*node, ns + "use_hatched_road_markings"); + p.use_freespace_areas = get_or_declare_parameter(*node, ns + "use_freespace_areas"); } // target object { const auto get_object_param = [&](std::string && ns) { ObjectParameter param{}; - param.moving_speed_threshold = getOrDeclareParameter(*node, ns + "th_moving_speed"); - param.moving_time_threshold = getOrDeclareParameter(*node, ns + "th_moving_time"); - param.max_expand_ratio = getOrDeclareParameter(*node, ns + "max_expand_ratio"); + param.moving_speed_threshold = + get_or_declare_parameter(*node, ns + "th_moving_speed"); + param.moving_time_threshold = get_or_declare_parameter(*node, ns + "th_moving_time"); + param.max_expand_ratio = get_or_declare_parameter(*node, ns + "max_expand_ratio"); param.envelope_buffer_margin = - getOrDeclareParameter(*node, ns + "envelope_buffer_margin"); + get_or_declare_parameter(*node, ns + "envelope_buffer_margin"); param.lateral_soft_margin = - getOrDeclareParameter(*node, ns + "lateral_margin.soft_margin"); + get_or_declare_parameter(*node, ns + "lateral_margin.soft_margin"); param.lateral_hard_margin = - getOrDeclareParameter(*node, ns + "lateral_margin.hard_margin"); - param.lateral_hard_margin_for_parked_vehicle = - getOrDeclareParameter(*node, ns + "lateral_margin.hard_margin_for_parked_vehicle"); - param.longitudinal_margin = getOrDeclareParameter(*node, ns + "longitudinal_margin"); + get_or_declare_parameter(*node, ns + "lateral_margin.hard_margin"); + param.lateral_hard_margin_for_parked_vehicle = get_or_declare_parameter( + *node, ns + "lateral_margin.hard_margin_for_parked_vehicle"); + param.longitudinal_margin = + get_or_declare_parameter(*node, ns + "longitudinal_margin"); param.th_error_eclipse_long_radius = - getOrDeclareParameter(*node, ns + "th_error_eclipse_long_radius"); + get_or_declare_parameter(*node, ns + "th_error_eclipse_long_radius"); return param; }; @@ -87,9 +89,9 @@ AvoidanceParameters getParameter(rclcpp::Node * node) p.object_parameters.emplace(ObjectClassification::UNKNOWN, get_object_param(ns + "unknown.")); p.lower_distance_for_polygon_expansion = - getOrDeclareParameter(*node, ns + "lower_distance_for_polygon_expansion"); + get_or_declare_parameter(*node, ns + "lower_distance_for_polygon_expansion"); p.upper_distance_for_polygon_expansion = - getOrDeclareParameter(*node, ns + "upper_distance_for_polygon_expansion"); + get_or_declare_parameter(*node, ns + "upper_distance_for_polygon_expansion"); } // target filtering @@ -99,7 +101,7 @@ AvoidanceParameters getParameter(rclcpp::Node * node) return; } p.object_parameters.at(object_type).is_avoidance_target = - getOrDeclareParameter(*node, ns); + get_or_declare_parameter(*node, ns); }; const std::string ns = "avoidance.target_filtering."; @@ -113,64 +115,64 @@ AvoidanceParameters getParameter(rclcpp::Node * node) set_target_flag(ObjectClassification::UNKNOWN, ns + "target_type.unknown"); p.object_check_goal_distance = - getOrDeclareParameter(*node, ns + "object_check_goal_distance"); + get_or_declare_parameter(*node, ns + "object_check_goal_distance"); p.object_check_return_pose_distance = - getOrDeclareParameter(*node, ns + "object_check_return_pose_distance"); + get_or_declare_parameter(*node, ns + "object_check_return_pose_distance"); p.object_check_yaw_deviation = - getOrDeclareParameter(*node, ns + "intersection.yaw_deviation"); + get_or_declare_parameter(*node, ns + "intersection.yaw_deviation"); p.object_last_seen_threshold = - getOrDeclareParameter(*node, ns + "max_compensation_time"); + get_or_declare_parameter(*node, ns + "max_compensation_time"); } { const std::string ns = "avoidance.target_filtering.parked_vehicle."; p.threshold_distance_object_is_on_center = - getOrDeclareParameter(*node, ns + "th_offset_from_centerline"); + get_or_declare_parameter(*node, ns + "th_offset_from_centerline"); p.object_check_shiftable_ratio = - getOrDeclareParameter(*node, ns + "th_shiftable_ratio"); + get_or_declare_parameter(*node, ns + "th_shiftable_ratio"); p.object_check_min_road_shoulder_width = - getOrDeclareParameter(*node, ns + "min_road_shoulder_width"); + get_or_declare_parameter(*node, ns + "min_road_shoulder_width"); } { const std::string ns = "avoidance.target_filtering.merging_vehicle."; - p.th_overhang_distance = getOrDeclareParameter(*node, ns + "th_overhang_distance"); + p.th_overhang_distance = get_or_declare_parameter(*node, ns + "th_overhang_distance"); } { const std::string ns = "avoidance.target_filtering.avoidance_for_ambiguous_vehicle."; - p.policy_ambiguous_vehicle = getOrDeclareParameter(*node, ns + "policy"); - p.wait_and_see_target_behaviors = - getOrDeclareParameter>(*node, ns + "wait_and_see.target_behaviors"); + p.policy_ambiguous_vehicle = get_or_declare_parameter(*node, ns + "policy"); + p.wait_and_see_target_behaviors = get_or_declare_parameter>( + *node, ns + "wait_and_see.target_behaviors"); p.wait_and_see_th_closest_distance = - getOrDeclareParameter(*node, ns + "wait_and_see.th_closest_distance"); + get_or_declare_parameter(*node, ns + "wait_and_see.th_closest_distance"); p.time_threshold_for_ambiguous_vehicle = - getOrDeclareParameter(*node, ns + "condition.th_stopped_time"); + get_or_declare_parameter(*node, ns + "condition.th_stopped_time"); p.distance_threshold_for_ambiguous_vehicle = - getOrDeclareParameter(*node, ns + "condition.th_moving_distance"); + get_or_declare_parameter(*node, ns + "condition.th_moving_distance"); p.object_ignore_section_traffic_light_in_front_distance = - getOrDeclareParameter(*node, ns + "ignore_area.traffic_light.front_distance"); + get_or_declare_parameter(*node, ns + "ignore_area.traffic_light.front_distance"); p.object_ignore_section_crosswalk_in_front_distance = - getOrDeclareParameter(*node, ns + "ignore_area.crosswalk.front_distance"); + get_or_declare_parameter(*node, ns + "ignore_area.crosswalk.front_distance"); p.object_ignore_section_crosswalk_behind_distance = - getOrDeclareParameter(*node, ns + "ignore_area.crosswalk.behind_distance"); + get_or_declare_parameter(*node, ns + "ignore_area.crosswalk.behind_distance"); } { const std::string ns = "avoidance.target_filtering.freespace."; p.freespace_condition_th_stopped_time = - getOrDeclareParameter(*node, ns + "condition.th_stopped_time"); + get_or_declare_parameter(*node, ns + "condition.th_stopped_time"); } { const std::string ns = "avoidance.target_filtering.detection_area."; - p.use_static_detection_area = getOrDeclareParameter(*node, ns + "static"); + p.use_static_detection_area = get_or_declare_parameter(*node, ns + "static"); p.object_check_min_forward_distance = - getOrDeclareParameter(*node, ns + "min_forward_distance"); + get_or_declare_parameter(*node, ns + "min_forward_distance"); p.object_check_max_forward_distance = - getOrDeclareParameter(*node, ns + "max_forward_distance"); + get_or_declare_parameter(*node, ns + "max_forward_distance"); p.object_check_backward_distance = - getOrDeclareParameter(*node, ns + "backward_distance"); + get_or_declare_parameter(*node, ns + "backward_distance"); } // safety check general params @@ -180,7 +182,7 @@ AvoidanceParameters getParameter(rclcpp::Node * node) return; } p.object_parameters.at(object_type).is_safety_check_target = - getOrDeclareParameter(*node, ns); + get_or_declare_parameter(*node, ns); }; const std::string ns = "avoidance.safety_check."; @@ -193,81 +195,83 @@ AvoidanceParameters getParameter(rclcpp::Node * node) set_target_flag(ObjectClassification::MOTORCYCLE, ns + "target_type.motorcycle"); set_target_flag(ObjectClassification::UNKNOWN, ns + "target_type.unknown"); - p.enable_safety_check = getOrDeclareParameter(*node, ns + "enable"); - p.check_current_lane = getOrDeclareParameter(*node, ns + "check_current_lane"); - p.check_shift_side_lane = getOrDeclareParameter(*node, ns + "check_shift_side_lane"); - p.check_other_side_lane = getOrDeclareParameter(*node, ns + "check_other_side_lane"); + p.enable_safety_check = get_or_declare_parameter(*node, ns + "enable"); + p.check_current_lane = get_or_declare_parameter(*node, ns + "check_current_lane"); + p.check_shift_side_lane = get_or_declare_parameter(*node, ns + "check_shift_side_lane"); + p.check_other_side_lane = get_or_declare_parameter(*node, ns + "check_other_side_lane"); p.check_unavoidable_object = - getOrDeclareParameter(*node, ns + "check_unavoidable_object"); - p.check_other_object = getOrDeclareParameter(*node, ns + "check_other_object"); + get_or_declare_parameter(*node, ns + "check_unavoidable_object"); + p.check_other_object = get_or_declare_parameter(*node, ns + "check_other_object"); p.check_all_predicted_path = - getOrDeclareParameter(*node, ns + "check_all_predicted_path"); + get_or_declare_parameter(*node, ns + "check_all_predicted_path"); p.safety_check_backward_distance = - getOrDeclareParameter(*node, ns + "safety_check_backward_distance"); + get_or_declare_parameter(*node, ns + "safety_check_backward_distance"); p.hysteresis_factor_expand_rate = - getOrDeclareParameter(*node, ns + "hysteresis_factor_expand_rate"); + get_or_declare_parameter(*node, ns + "hysteresis_factor_expand_rate"); p.hysteresis_factor_safe_count = - getOrDeclareParameter(*node, ns + "hysteresis_factor_safe_count"); + get_or_declare_parameter(*node, ns + "hysteresis_factor_safe_count"); p.collision_check_yaw_diff_threshold = - getOrDeclareParameter(*node, ns + "collision_check_yaw_diff_threshold"); + get_or_declare_parameter(*node, ns + "collision_check_yaw_diff_threshold"); } // safety check predicted path params { const std::string ns = "avoidance.safety_check."; p.ego_predicted_path_params.min_velocity = - getOrDeclareParameter(*node, ns + "min_velocity"); + get_or_declare_parameter(*node, ns + "min_velocity"); p.ego_predicted_path_params.max_velocity = - getOrDeclareParameter(*node, ns + "max_velocity"); - p.ego_predicted_path_params.acceleration = - getOrDeclareParameter(*node, "avoidance.constraints.longitudinal.max_acceleration"); + get_or_declare_parameter(*node, ns + "max_velocity"); + p.ego_predicted_path_params.acceleration = get_or_declare_parameter( + *node, "avoidance.constraints.longitudinal.max_acceleration"); p.ego_predicted_path_params.time_horizon_for_front_object = - getOrDeclareParameter(*node, ns + "time_horizon_for_front_object"); + get_or_declare_parameter(*node, ns + "time_horizon_for_front_object"); p.ego_predicted_path_params.time_horizon_for_rear_object = - getOrDeclareParameter(*node, ns + "time_horizon_for_rear_object"); + get_or_declare_parameter(*node, ns + "time_horizon_for_rear_object"); p.ego_predicted_path_params.time_resolution = - getOrDeclareParameter(*node, ns + "time_resolution"); + get_or_declare_parameter(*node, ns + "time_resolution"); p.ego_predicted_path_params.delay_until_departure = - getOrDeclareParameter(*node, ns + "delay_until_departure"); + get_or_declare_parameter(*node, ns + "delay_until_departure"); } // safety check rss params { const std::string ns = "avoidance.safety_check."; p.rss_params.extended_polygon_policy = - getOrDeclareParameter(*node, ns + "extended_polygon_policy"); + get_or_declare_parameter(*node, ns + "extended_polygon_policy"); p.rss_params.longitudinal_distance_min_threshold = - getOrDeclareParameter(*node, ns + "longitudinal_distance_min_threshold"); + get_or_declare_parameter(*node, ns + "longitudinal_distance_min_threshold"); p.rss_params.longitudinal_velocity_delta_time = - getOrDeclareParameter(*node, ns + "longitudinal_velocity_delta_time"); + get_or_declare_parameter(*node, ns + "longitudinal_velocity_delta_time"); p.rss_params.front_vehicle_deceleration = - getOrDeclareParameter(*node, ns + "expected_front_deceleration"); + get_or_declare_parameter(*node, ns + "expected_front_deceleration"); p.rss_params.rear_vehicle_deceleration = - getOrDeclareParameter(*node, ns + "expected_rear_deceleration"); + get_or_declare_parameter(*node, ns + "expected_rear_deceleration"); p.rss_params.rear_vehicle_reaction_time = - getOrDeclareParameter(*node, ns + "rear_vehicle_reaction_time"); + get_or_declare_parameter(*node, ns + "rear_vehicle_reaction_time"); p.rss_params.rear_vehicle_safety_time_margin = - getOrDeclareParameter(*node, ns + "rear_vehicle_safety_time_margin"); + get_or_declare_parameter(*node, ns + "rear_vehicle_safety_time_margin"); p.rss_params.lateral_distance_max_threshold = - getOrDeclareParameter(*node, ns + "lateral_distance_max_threshold"); + get_or_declare_parameter(*node, ns + "lateral_distance_max_threshold"); } // avoidance maneuver (lateral) { const std::string ns = "avoidance.avoidance.lateral."; p.soft_drivable_bound_margin = - getOrDeclareParameter(*node, ns + "soft_drivable_bound_margin"); + get_or_declare_parameter(*node, ns + "soft_drivable_bound_margin"); p.hard_drivable_bound_margin = - getOrDeclareParameter(*node, ns + "hard_drivable_bound_margin"); - p.lateral_execution_threshold = getOrDeclareParameter(*node, ns + "th_avoid_execution"); + get_or_declare_parameter(*node, ns + "hard_drivable_bound_margin"); + p.lateral_execution_threshold = + get_or_declare_parameter(*node, ns + "th_avoid_execution"); p.lateral_small_shift_threshold = - getOrDeclareParameter(*node, ns + "th_small_shift_length"); - p.max_right_shift_length = getOrDeclareParameter(*node, ns + "max_right_shift_length"); - p.max_left_shift_length = getOrDeclareParameter(*node, ns + "max_left_shift_length"); + get_or_declare_parameter(*node, ns + "th_small_shift_length"); + p.max_right_shift_length = + get_or_declare_parameter(*node, ns + "max_right_shift_length"); + p.max_left_shift_length = get_or_declare_parameter(*node, ns + "max_left_shift_length"); p.max_deviation_from_lane = - getOrDeclareParameter(*node, ns + "max_deviation_from_lane"); + get_or_declare_parameter(*node, ns + "max_deviation_from_lane"); p.ratio_for_return_shift_approval = - getOrDeclareParameter(*node, ns + "ratio_for_return_shift_approval"); + get_or_declare_parameter(*node, ns + "ratio_for_return_shift_approval"); if (p.ratio_for_return_shift_approval < 0.0 || p.ratio_for_return_shift_approval > 1.0) { throw std::domain_error( "ratio_for_return_shift_approval should be within range of 0.0 to 1.0"); @@ -277,59 +281,60 @@ AvoidanceParameters getParameter(rclcpp::Node * node) // avoidance maneuver (longitudinal) { const std::string ns = "avoidance.avoidance.longitudinal."; - p.min_prepare_time = getOrDeclareParameter(*node, ns + "min_prepare_time"); - p.max_prepare_time = getOrDeclareParameter(*node, ns + "max_prepare_time"); - p.min_prepare_distance = getOrDeclareParameter(*node, ns + "min_prepare_distance"); - p.min_slow_down_speed = getOrDeclareParameter(*node, ns + "min_slow_down_speed"); - p.buf_slow_down_speed = getOrDeclareParameter(*node, ns + "buf_slow_down_speed"); + p.min_prepare_time = get_or_declare_parameter(*node, ns + "min_prepare_time"); + p.max_prepare_time = get_or_declare_parameter(*node, ns + "max_prepare_time"); + p.min_prepare_distance = get_or_declare_parameter(*node, ns + "min_prepare_distance"); + p.min_slow_down_speed = get_or_declare_parameter(*node, ns + "min_slow_down_speed"); + p.buf_slow_down_speed = get_or_declare_parameter(*node, ns + "buf_slow_down_speed"); p.nominal_avoidance_speed = - getOrDeclareParameter(*node, ns + "nominal_avoidance_speed"); - p.consider_front_overhang = getOrDeclareParameter(*node, ns + "consider_front_overhang"); - p.consider_rear_overhang = getOrDeclareParameter(*node, ns + "consider_rear_overhang"); + get_or_declare_parameter(*node, ns + "nominal_avoidance_speed"); + p.consider_front_overhang = + get_or_declare_parameter(*node, ns + "consider_front_overhang"); + p.consider_rear_overhang = get_or_declare_parameter(*node, ns + "consider_rear_overhang"); } // avoidance maneuver (return shift dead line) { const std::string ns = "avoidance.avoidance.return_dead_line."; - p.enable_dead_line_for_goal = getOrDeclareParameter(*node, ns + "goal.enable"); + p.enable_dead_line_for_goal = get_or_declare_parameter(*node, ns + "goal.enable"); p.enable_dead_line_for_traffic_light = - getOrDeclareParameter(*node, ns + "traffic_light.enable"); - p.dead_line_buffer_for_goal = getOrDeclareParameter(*node, ns + "goal.buffer"); + get_or_declare_parameter(*node, ns + "traffic_light.enable"); + p.dead_line_buffer_for_goal = get_or_declare_parameter(*node, ns + "goal.buffer"); p.dead_line_buffer_for_traffic_light = - getOrDeclareParameter(*node, ns + "traffic_light.buffer"); + get_or_declare_parameter(*node, ns + "traffic_light.buffer"); } // cancel { const std::string ns = "avoidance.cancel."; - p.enable_cancel_maneuver = getOrDeclareParameter(*node, ns + "enable"); + p.enable_cancel_maneuver = get_or_declare_parameter(*node, ns + "enable"); p.force_deactivate_duration_time = - getOrDeclareParameter(*node, ns + "force.duration_time"); + get_or_declare_parameter(*node, ns + "force.duration_time"); } // yield { const std::string ns = "avoidance.yield."; - p.enable_yield_maneuver = getOrDeclareParameter(*node, ns + "enable"); + p.enable_yield_maneuver = get_or_declare_parameter(*node, ns + "enable"); p.enable_yield_maneuver_during_shifting = - getOrDeclareParameter(*node, ns + "enable_during_shifting"); + get_or_declare_parameter(*node, ns + "enable_during_shifting"); } // stop { const std::string ns = "avoidance.stop."; - p.stop_max_distance = getOrDeclareParameter(*node, ns + "max_distance"); - p.stop_buffer = getOrDeclareParameter(*node, ns + "stop_buffer"); + p.stop_max_distance = get_or_declare_parameter(*node, ns + "max_distance"); + p.stop_buffer = get_or_declare_parameter(*node, ns + "stop_buffer"); } // policy { const std::string ns = "avoidance.policy."; - p.policy_approval = getOrDeclareParameter(*node, ns + "make_approval_request"); - p.policy_deceleration = getOrDeclareParameter(*node, ns + "deceleration"); - p.policy_lateral_margin = getOrDeclareParameter(*node, ns + "lateral_margin"); + p.policy_approval = get_or_declare_parameter(*node, ns + "make_approval_request"); + p.policy_deceleration = get_or_declare_parameter(*node, ns + "deceleration"); + p.policy_lateral_margin = get_or_declare_parameter(*node, ns + "lateral_margin"); p.use_shorten_margin_immediately = - getOrDeclareParameter(*node, ns + "use_shorten_margin_immediately"); + get_or_declare_parameter(*node, ns + "use_shorten_margin_immediately"); if (p.policy_approval != "per_shift_line" && p.policy_approval != "per_avoidance_maneuver") { throw std::domain_error("invalid policy. please select 'best_effort' or 'reliable'."); @@ -347,25 +352,25 @@ AvoidanceParameters getParameter(rclcpp::Node * node) // constraints (longitudinal) { const std::string ns = "avoidance.constraints.longitudinal."; - p.nominal_deceleration = getOrDeclareParameter(*node, ns + "nominal_deceleration"); - p.nominal_jerk = getOrDeclareParameter(*node, ns + "nominal_jerk"); - p.max_deceleration = getOrDeclareParameter(*node, ns + "max_deceleration"); - p.max_jerk = getOrDeclareParameter(*node, ns + "max_jerk"); - p.max_acceleration = getOrDeclareParameter(*node, ns + "max_acceleration"); + p.nominal_deceleration = get_or_declare_parameter(*node, ns + "nominal_deceleration"); + p.nominal_jerk = get_or_declare_parameter(*node, ns + "nominal_jerk"); + p.max_deceleration = get_or_declare_parameter(*node, ns + "max_deceleration"); + p.max_jerk = get_or_declare_parameter(*node, ns + "max_jerk"); + p.max_acceleration = get_or_declare_parameter(*node, ns + "max_acceleration"); p.min_velocity_to_limit_max_acceleration = - getOrDeclareParameter(*node, ns + "min_velocity_to_limit_max_acceleration"); + get_or_declare_parameter(*node, ns + "min_velocity_to_limit_max_acceleration"); } // constraints (lateral) { const std::string ns = "avoidance.constraints.lateral."; - p.velocity_map = getOrDeclareParameter>(*node, ns + "velocity"); + p.velocity_map = get_or_declare_parameter>(*node, ns + "velocity"); p.lateral_max_accel_map = - getOrDeclareParameter>(*node, ns + "max_accel_values"); + get_or_declare_parameter>(*node, ns + "max_accel_values"); p.lateral_min_jerk_map = - getOrDeclareParameter>(*node, ns + "min_jerk_values"); + get_or_declare_parameter>(*node, ns + "min_jerk_values"); p.lateral_max_jerk_map = - getOrDeclareParameter>(*node, ns + "max_jerk_values"); + get_or_declare_parameter>(*node, ns + "max_jerk_values"); if (p.velocity_map.empty()) { throw std::domain_error("invalid velocity map."); @@ -387,29 +392,29 @@ AvoidanceParameters getParameter(rclcpp::Node * node) // shift line pipeline { const std::string ns = "avoidance.shift_line_pipeline."; - p.quantize_size = getOrDeclareParameter(*node, ns + "trim.quantize_size"); - p.th_similar_grad_1 = getOrDeclareParameter(*node, ns + "trim.th_similar_grad_1"); - p.th_similar_grad_2 = getOrDeclareParameter(*node, ns + "trim.th_similar_grad_2"); - p.th_similar_grad_3 = getOrDeclareParameter(*node, ns + "trim.th_similar_grad_3"); + p.quantize_size = get_or_declare_parameter(*node, ns + "trim.quantize_size"); + p.th_similar_grad_1 = get_or_declare_parameter(*node, ns + "trim.th_similar_grad_1"); + p.th_similar_grad_2 = get_or_declare_parameter(*node, ns + "trim.th_similar_grad_2"); + p.th_similar_grad_3 = get_or_declare_parameter(*node, ns + "trim.th_similar_grad_3"); } // debug { const std::string ns = "avoidance.debug."; p.enable_other_objects_marker = - getOrDeclareParameter(*node, ns + "enable_other_objects_marker"); + get_or_declare_parameter(*node, ns + "enable_other_objects_marker"); p.enable_other_objects_info = - getOrDeclareParameter(*node, ns + "enable_other_objects_info"); + get_or_declare_parameter(*node, ns + "enable_other_objects_info"); p.enable_detection_area_marker = - getOrDeclareParameter(*node, ns + "enable_detection_area_marker"); + get_or_declare_parameter(*node, ns + "enable_detection_area_marker"); p.enable_drivable_bound_marker = - getOrDeclareParameter(*node, ns + "enable_drivable_bound_marker"); + get_or_declare_parameter(*node, ns + "enable_drivable_bound_marker"); p.enable_safety_check_marker = - getOrDeclareParameter(*node, ns + "enable_safety_check_marker"); + get_or_declare_parameter(*node, ns + "enable_safety_check_marker"); p.enable_shift_line_marker = - getOrDeclareParameter(*node, ns + "enable_shift_line_marker"); - p.enable_lane_marker = getOrDeclareParameter(*node, ns + "enable_lane_marker"); - p.enable_misc_marker = getOrDeclareParameter(*node, ns + "enable_misc_marker"); + get_or_declare_parameter(*node, ns + "enable_shift_line_marker"); + p.enable_lane_marker = get_or_declare_parameter(*node, ns + "enable_lane_marker"); + p.enable_misc_marker = get_or_declare_parameter(*node, ns + "enable_misc_marker"); } return p; diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/scene.hpp b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/scene.hpp index 46b6388fd5119..406c5ce924a5b 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/scene.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/scene.hpp @@ -184,9 +184,9 @@ class StaticObstacleAvoidanceModule : public SceneModuleInterface } if (candidate_registered) { - uuid_map_.at("left") = generateUUID(); - uuid_map_.at("right") = generateUUID(); - candidate_uuid_ = generateUUID(); + uuid_map_.at("left") = generate_uuid(); + uuid_map_.at("right") = generate_uuid(); + candidate_uuid_ = generate_uuid(); } } diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/type_alias.hpp b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/type_alias.hpp index 96cc3f3bfc6aa..e2ee86849f148 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/type_alias.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/type_alias.hpp @@ -18,11 +18,11 @@ #include #include #include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include #include #include @@ -57,22 +57,22 @@ using tier4_planning_msgs::msg::AvoidanceDebugMsgArray; using tier4_rtc_msgs::msg::State; // tier4 utils functions -using autoware::universe_utils::appendMarkerArray; -using autoware::universe_utils::calcDistance2d; -using autoware::universe_utils::calcLateralDeviation; -using autoware::universe_utils::calcOffsetPose; -using autoware::universe_utils::calcYawDeviation; -using autoware::universe_utils::createDefaultMarker; -using autoware::universe_utils::createMarkerColor; -using autoware::universe_utils::createMarkerScale; -using autoware::universe_utils::createPoint; -using autoware::universe_utils::createQuaternionFromRPY; -using autoware::universe_utils::getPoint; -using autoware::universe_utils::getPose; -using autoware::universe_utils::Point2d; -using autoware::universe_utils::Polygon2d; -using autoware::universe_utils::pose2transform; -using autoware::universe_utils::toHexString; +using autoware_utils::append_marker_array; +using autoware_utils::calc_distance2d; +using autoware_utils::calc_lateral_deviation; +using autoware_utils::calc_offset_pose; +using autoware_utils::calc_yaw_deviation; +using autoware_utils::create_default_marker; +using autoware_utils::create_marker_color; +using autoware_utils::create_marker_scale; +using autoware_utils::create_point; +using autoware_utils::create_quaternion_from_rpy; +using autoware_utils::get_point; +using autoware_utils::get_pose; +using autoware_utils::Point2d; +using autoware_utils::Polygon2d; +using autoware_utils::pose2transform; +using autoware_utils::to_hex_string; } // namespace autoware::behavior_path_planner #endif // AUTOWARE__BEHAVIOR_PATH_STATIC_OBSTACLE_AVOIDANCE_MODULE__TYPE_ALIAS_HPP_ diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/package.xml b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/package.xml index 6dd7138b4aa24..d9ed8a07bc9b5 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/package.xml +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/package.xml @@ -32,7 +32,7 @@ autoware_rtc_interface autoware_signal_processing autoware_test_utils - autoware_universe_utils + autoware_utils autoware_vehicle_info_utils geometry_msgs magic_enum diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/debug.cpp b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/debug.cpp index e7177768be3dd..8afa580f9c5f2 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/debug.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/debug.cpp @@ -57,11 +57,11 @@ MarkerArray createObjectsCubeMarkerArray( }; for (const auto & object : objects) { - auto marker = createDefaultMarker( + auto marker = create_default_marker( "map", rclcpp::Clock{RCL_ROS_TIME}.now(), ns, 0L, Marker::CUBE, scale, color); if (is_small_object(object.object)) { - marker.scale = createMarkerScale(0.5, 0.5, 1.5); + marker.scale = create_marker_scale(0.5, 0.5, 1.5); marker.type = Marker::CYLINDER; } @@ -78,12 +78,12 @@ MarkerArray createObjectPolygonMarkerArray(const ObjectDataArray & objects, std: MarkerArray msg; for (const auto & object : objects) { - auto marker = createDefaultMarker( + auto marker = create_default_marker( "map", rclcpp::Clock{RCL_ROS_TIME}.now(), ns, 0L, Marker::LINE_STRIP, - createMarkerScale(0.1, 0.0, 0.0), createMarkerColor(1.0, 1.0, 1.0, 0.999)); + create_marker_scale(0.1, 0.0, 0.0), create_marker_color(1.0, 1.0, 1.0, 0.999)); for (const auto & p : object.envelope_poly.outer()) { - marker.points.push_back(createPoint(p.x(), p.y(), object.getPosition().z)); + marker.points.push_back(create_point(p.x(), p.y(), object.getPosition().z)); } marker.points.push_back(marker.points.front()); @@ -104,9 +104,9 @@ MarkerArray createToDrivableBoundDistance(const ObjectDataArray & objects, std:: } { - auto marker = createDefaultMarker( + auto marker = create_default_marker( "map", rclcpp::Clock{RCL_ROS_TIME}.now(), ns, 0L, Marker::LINE_STRIP, - createMarkerScale(0.1, 0.0, 0.0), createMarkerColor(1.0, 0.0, 0.42, 0.999)); + create_marker_scale(0.1, 0.0, 0.0), create_marker_color(1.0, 0.0, 0.42, 0.999)); marker.points.push_back(object.narrowest_place.value().first); marker.points.push_back(object.narrowest_place.value().second); @@ -115,9 +115,9 @@ MarkerArray createToDrivableBoundDistance(const ObjectDataArray & objects, std:: } { - auto marker = createDefaultMarker( + auto marker = create_default_marker( "map", rclcpp::Clock{RCL_ROS_TIME}.now(), ns, 0L, Marker::TEXT_VIEW_FACING, - createMarkerScale(0.5, 0.5, 0.5), createMarkerColor(1.0, 1.0, 0.0, 1.0)); + create_marker_scale(0.5, 0.5, 0.5), create_marker_color(1.0, 1.0, 0.0, 1.0)); marker.pose.position = object.narrowest_place.value().second; std::ostringstream string_stream; @@ -135,9 +135,9 @@ MarkerArray createObjectInfoMarkerArray( { MarkerArray msg; - auto marker = createDefaultMarker( + auto marker = create_default_marker( "map", rclcpp::Clock{RCL_ROS_TIME}.now(), ns, 0L, Marker::TEXT_VIEW_FACING, - createMarkerScale(0.5, 0.5, 0.5), createMarkerColor(1.0, 1.0, 0.0, 1.0)); + create_marker_scale(0.5, 0.5, 0.5), create_marker_color(1.0, 1.0, 0.0, 1.0)); for (const auto & object : objects) { if (verbose) { @@ -154,8 +154,8 @@ MarkerArray createObjectInfoMarkerArray( << "move_time:" << object.move_time << " [s]\n" << "stop_time:" << object.stop_time << " [s]\n"; marker.text = string_stream.str(); - marker.color = createMarkerColor(1.0, 1.0, 0.0, 0.999); - marker.scale = createMarkerScale(0.5, 0.5, 0.5); + marker.color = create_marker_color(1.0, 1.0, 0.0, 0.999); + marker.scale = create_marker_scale(0.5, 0.5, 0.5); marker.ns = ns; msg.markers.push_back(marker); } @@ -168,8 +168,8 @@ MarkerArray createObjectInfoMarkerArray( string_stream << magic_enum::enum_name(object.info) << (object.is_parked ? "(PARKED)" : ""); string_stream << (object.is_ambiguous ? "(WAIT AND SEE)" : ""); marker.text = string_stream.str(); - marker.color = createMarkerColor(1.0, 1.0, 1.0, 0.999); - marker.scale = createMarkerScale(0.6, 0.6, 0.6); + marker.color = create_marker_color(1.0, 1.0, 1.0, 0.999); + marker.scale = create_marker_scale(0.6, 0.6, 0.6); marker.ns = ns + "_reason"; msg.markers.push_back(marker); } @@ -184,7 +184,7 @@ MarkerArray createOverhangLaneletMarkerArray(const ObjectDataArray & objects, st msg.markers.reserve(objects.size()); for (const auto & object : objects) { - appendMarkerArray( + append_marker_array( marker_utils::createLaneletsAreaMarkerArray( {object.overhang_lanelet}, std::string(ns), 0.0, 0.0, 1.0), &msg); @@ -198,16 +198,16 @@ MarkerArray avoidableObjectsMarkerArray(const ObjectDataArray & objects, std::st MarkerArray msg; msg.markers.reserve(objects.size() * 5); - appendMarkerArray( + append_marker_array( createObjectsCubeMarkerArray( - objects, ns + "_cube", createMarkerScale(3.0, 1.5, 1.5), - createMarkerColor(1.0, 1.0, 0.0, 0.8)), + objects, ns + "_cube", create_marker_scale(3.0, 1.5, 1.5), + create_marker_color(1.0, 1.0, 0.0, 0.8)), &msg); - appendMarkerArray(createObjectInfoMarkerArray(objects, ns + "_info", true), &msg); - appendMarkerArray(createObjectPolygonMarkerArray(objects, ns + "_envelope_polygon"), &msg); - appendMarkerArray(createToDrivableBoundDistance(objects, ns + "_to_drivable_bound"), &msg); - appendMarkerArray(createOverhangLaneletMarkerArray(objects, ns + "_overhang_lanelet"), &msg); + append_marker_array(createObjectInfoMarkerArray(objects, ns + "_info", true), &msg); + append_marker_array(createObjectPolygonMarkerArray(objects, ns + "_envelope_polygon"), &msg); + append_marker_array(createToDrivableBoundDistance(objects, ns + "_to_drivable_bound"), &msg); + append_marker_array(createOverhangLaneletMarkerArray(objects, ns + "_overhang_lanelet"), &msg); return msg; } @@ -217,16 +217,16 @@ MarkerArray unAvoidableObjectsMarkerArray(const ObjectDataArray & objects, std:: MarkerArray msg; msg.markers.reserve(objects.size() * 5); - appendMarkerArray( + append_marker_array( createObjectsCubeMarkerArray( - objects, ns + "_cube", createMarkerScale(3.0, 1.5, 1.5), - createMarkerColor(1.0, 0.0, 0.0, 0.8)), + objects, ns + "_cube", create_marker_scale(3.0, 1.5, 1.5), + create_marker_color(1.0, 0.0, 0.0, 0.8)), &msg); - appendMarkerArray(createObjectInfoMarkerArray(objects, ns + "_info", true), &msg); - appendMarkerArray(createObjectPolygonMarkerArray(objects, ns + "_envelope_polygon"), &msg); - appendMarkerArray(createToDrivableBoundDistance(objects, ns + "_to_drivable_bound"), &msg); - appendMarkerArray(createOverhangLaneletMarkerArray(objects, ns + "_overhang_lanelet"), &msg); + append_marker_array(createObjectInfoMarkerArray(objects, ns + "_info", true), &msg); + append_marker_array(createObjectPolygonMarkerArray(objects, ns + "_envelope_polygon"), &msg); + append_marker_array(createToDrivableBoundDistance(objects, ns + "_to_drivable_bound"), &msg); + append_marker_array(createOverhangLaneletMarkerArray(objects, ns + "_overhang_lanelet"), &msg); return msg; } @@ -253,38 +253,38 @@ MarkerArray createTurnSignalMarkerArray(const TurnSignalInfo & turn_signal_info, size_t i = 0; { - auto marker = createDefaultMarker( + auto marker = create_default_marker( "map", rclcpp::Clock{RCL_ROS_TIME}.now(), ns, i++, Marker::ARROW, - createMarkerScale(0.6, 0.3, 0.3), createMarkerColor(0.0, 0.0, 1.0, 0.999)); + create_marker_scale(0.6, 0.3, 0.3), create_marker_color(0.0, 0.0, 1.0, 0.999)); marker.pose = turn_signal_info.desired_start_point; - marker.pose = calcOffsetPose(marker.pose, 0.0, 0.0, 0.0, yaw_offset); + marker.pose = calc_offset_pose(marker.pose, 0.0, 0.0, 0.0, yaw_offset); msg.markers.push_back(marker); } { - auto marker = createDefaultMarker( + auto marker = create_default_marker( "map", rclcpp::Clock{RCL_ROS_TIME}.now(), ns, i++, Marker::ARROW, - createMarkerScale(0.6, 0.3, 0.3), createMarkerColor(0.0, 0.0, 1.0, 0.999)); + create_marker_scale(0.6, 0.3, 0.3), create_marker_color(0.0, 0.0, 1.0, 0.999)); marker.pose = turn_signal_info.desired_end_point; - marker.pose = calcOffsetPose(marker.pose, 0.0, 0.0, 0.0, yaw_offset); + marker.pose = calc_offset_pose(marker.pose, 0.0, 0.0, 0.0, yaw_offset); msg.markers.push_back(marker); } { - auto marker = createDefaultMarker( + auto marker = create_default_marker( "map", rclcpp::Clock{RCL_ROS_TIME}.now(), ns, i++, Marker::ARROW, - createMarkerScale(0.8, 0.4, 0.4), createMarkerColor(1.0, 0.0, 0.0, 0.999)); + create_marker_scale(0.8, 0.4, 0.4), create_marker_color(1.0, 0.0, 0.0, 0.999)); marker.pose = turn_signal_info.required_start_point; - marker.pose = calcOffsetPose(marker.pose, 0.0, 0.0, 0.0, yaw_offset); + marker.pose = calc_offset_pose(marker.pose, 0.0, 0.0, 0.0, yaw_offset); msg.markers.push_back(marker); } { - auto marker = createDefaultMarker( + auto marker = create_default_marker( "map", rclcpp::Clock{RCL_ROS_TIME}.now(), ns, i++, Marker::ARROW, - createMarkerScale(0.8, 0.4, 0.4), createMarkerColor(1.0, 0.0, 0.0, 0.999)); + create_marker_scale(0.8, 0.4, 0.4), create_marker_color(1.0, 0.0, 0.0, 0.999)); marker.pose = turn_signal_info.required_end_point; - marker.pose = calcOffsetPose(marker.pose, 0.0, 0.0, 0.0, yaw_offset); + marker.pose = calc_offset_pose(marker.pose, 0.0, 0.0, 0.0, yaw_offset); msg.markers.push_back(marker); } @@ -304,9 +304,9 @@ MarkerArray createAvoidLineMarkerArray( return msg; } - auto marker = createDefaultMarker( + auto marker = create_default_marker( "map", rclcpp::Clock{RCL_ROS_TIME}.now(), ns, 0L, Marker::CUBE, - createMarkerScale(0.1, 0.1, 0.1), createMarkerColor(r, g, b, 0.9)); + create_marker_scale(0.1, 0.1, 0.1), create_marker_color(r, g, b, 0.9)); int32_t shift_line_id{0}; int32_t marker_id{0}; @@ -316,9 +316,9 @@ MarkerArray createAvoidLineMarkerArray( auto m = marker; m.id = marker_id++; m.type = Marker::LINE_STRIP; - m.scale = createMarkerScale(w, 0.0, 0.0); - m.points.push_back(calcOffsetPose(s.start, 0.0, s.start_shift_length, 0.0).position); - m.points.push_back(calcOffsetPose(s.end, 0.0, s.end_shift_length, 0.0).position); + m.scale = create_marker_scale(w, 0.0, 0.0); + m.points.push_back(calc_offset_pose(s.start, 0.0, s.start_shift_length, 0.0).position); + m.points.push_back(calc_offset_pose(s.end, 0.0, s.end_shift_length, 0.0).position); msg.markers.push_back(m); } @@ -327,8 +327,8 @@ MarkerArray createAvoidLineMarkerArray( auto m = marker; m.id = marker_id++; m.type = Marker::CUBE; - m.scale = createMarkerScale(0.2, 0.2, 0.2); - m.pose = calcOffsetPose(s.start, 0.0, s.start_shift_length, 0.0); + m.scale = create_marker_scale(0.2, 0.2, 0.2); + m.pose = calc_offset_pose(s.start, 0.0, s.start_shift_length, 0.0); msg.markers.push_back(m); } @@ -337,8 +337,8 @@ MarkerArray createAvoidLineMarkerArray( auto m = marker; m.id = marker_id++; m.type = Marker::CUBE; - m.scale = createMarkerScale(0.2, 0.2, 0.2); - m.pose = calcOffsetPose(s.end, 0.0, s.end_shift_length, 0.0); + m.scale = create_marker_scale(0.2, 0.2, 0.2); + m.pose = calc_offset_pose(s.end, 0.0, s.end_shift_length, 0.0); msg.markers.push_back(m); } @@ -349,8 +349,8 @@ MarkerArray createAvoidLineMarkerArray( string_stream << "(S):" << shift_line_id; m.id = marker_id++; m.type = Marker::TEXT_VIEW_FACING; - m.scale = createMarkerScale(0.3, 0.3, 0.3); - m.pose = calcOffsetPose(s.start, 0.0, s.start_shift_length + 0.3, 0.0); + m.scale = create_marker_scale(0.3, 0.3, 0.3); + m.pose = calc_offset_pose(s.start, 0.0, s.start_shift_length + 0.3, 0.0); m.text = string_stream.str(); msg.markers.push_back(m); } @@ -362,8 +362,8 @@ MarkerArray createAvoidLineMarkerArray( string_stream << "(E):" << shift_line_id; m.id = marker_id++; m.type = Marker::TEXT_VIEW_FACING; - m.scale = createMarkerScale(0.3, 0.3, 0.3); - m.pose = calcOffsetPose(s.end, 0.0, s.end_shift_length - 0.3, 0.0); + m.scale = create_marker_scale(0.3, 0.3, 0.3); + m.pose = calc_offset_pose(s.end, 0.0, s.end_shift_length - 0.3, 0.0); m.text = string_stream.str(); msg.markers.push_back(m); } @@ -389,8 +389,8 @@ MarkerArray createTargetObjectsMarkerArray(const ObjectDataArray & objects, cons MarkerArray msg; msg.markers.reserve(objects.size() * 4); - appendMarkerArray(avoidableObjectsMarkerArray(avoidable, "avoidable_" + ns), &msg); - appendMarkerArray(unAvoidableObjectsMarkerArray(unavoidable, "unavoidable_" + ns), &msg); + append_marker_array(avoidableObjectsMarkerArray(avoidable, "avoidable_" + ns), &msg); + append_marker_array(unAvoidableObjectsMarkerArray(unavoidable, "unavoidable_" + ns), &msg); return msg; } @@ -419,14 +419,14 @@ MarkerArray createOtherObjectsMarkerArray( std::string ns = string_stream.str(); transform(ns.begin(), ns.end(), ns.begin(), tolower); - appendMarkerArray( + append_marker_array( createObjectsCubeMarkerArray( - filtered_objects, "others_" + ns + "_cube", createMarkerScale(3.0, 1.5, 1.5), - createMarkerColor(0.5, 0.5, 0.5, 0.8)), + filtered_objects, "others_" + ns + "_cube", create_marker_scale(3.0, 1.5, 1.5), + create_marker_color(0.5, 0.5, 0.5, 0.8)), &msg); - appendMarkerArray( + append_marker_array( createObjectInfoMarkerArray(filtered_objects, "others_" + ns + "_info", verbose), &msg); - appendMarkerArray( + append_marker_array( createOverhangLaneletMarkerArray(filtered_objects, "others_" + ns + "_overhang_lanelet"), &msg); return msg; @@ -447,9 +447,9 @@ MarkerArray createAmbiguousObjectsMarkerArray( } { - auto marker = createDefaultMarker( + auto marker = create_default_marker( "map", rclcpp::Clock{RCL_ROS_TIME}.now(), "ambiguous_target", 0L, Marker::ARROW, - createMarkerScale(0.5, 1.0, 1.0), createMarkerColor(1.0, 1.0, 0.0, 0.999)); + create_marker_scale(0.5, 1.0, 1.0), create_marker_color(1.0, 1.0, 0.0, 0.999)); Point src, dst; src = object.getPosition(); @@ -465,10 +465,10 @@ MarkerArray createAmbiguousObjectsMarkerArray( } { - auto marker = createDefaultMarker( + auto marker = create_default_marker( "map", rclcpp::Clock{RCL_ROS_TIME}.now(), "ambiguous_target_text", 0L, - Marker::TEXT_VIEW_FACING, createMarkerScale(0.5, 0.5, 0.5), - createMarkerColor(1.0, 1.0, 0.0, 1.0)); + Marker::TEXT_VIEW_FACING, create_marker_scale(0.5, 0.5, 0.5), + create_marker_color(1.0, 1.0, 0.0, 1.0)); marker.id = uuidToInt32(object.object.object_id); marker.pose = object.getPose(); @@ -476,15 +476,15 @@ MarkerArray createAmbiguousObjectsMarkerArray( std::ostringstream string_stream; string_stream << "SHOULD AVOID?"; marker.text = string_stream.str(); - marker.color = createMarkerColor(1.0, 1.0, 0.0, 0.999); - marker.scale = createMarkerScale(0.8, 0.8, 0.8); + marker.color = create_marker_color(1.0, 1.0, 0.0, 0.999); + marker.scale = create_marker_scale(0.8, 0.8, 0.8); msg.markers.push_back(marker); } { - auto marker = createDefaultMarker( + auto marker = create_default_marker( "map", rclcpp::Clock{RCL_ROS_TIME}.now(), "request_text", 0L, Marker::TEXT_VIEW_FACING, - createMarkerScale(0.5, 0.5, 0.5), createMarkerColor(1.0, 1.0, 0.0, 1.0)); + create_marker_scale(0.5, 0.5, 0.5), create_marker_color(1.0, 1.0, 0.0, 1.0)); marker.id = uuidToInt32(object.object.object_id); marker.pose = ego_pose; @@ -492,8 +492,8 @@ MarkerArray createAmbiguousObjectsMarkerArray( std::ostringstream string_stream; string_stream << "SYSTEM REQUESTS OPERATOR SUPPORT."; marker.text = string_stream.str(); - marker.color = createMarkerColor(1.0, 1.0, 0.0, 0.999); - marker.scale = createMarkerScale(0.8, 0.8, 0.8); + marker.color = create_marker_color(1.0, 1.0, 0.0, 0.999); + marker.scale = create_marker_scale(0.8, 0.8, 0.8); msg.markers.push_back(marker); } @@ -511,10 +511,10 @@ MarkerArray createStopTargetObjectMarkerArray(const AvoidancePlanningData & data return msg; } - appendMarkerArray( + append_marker_array( createObjectsCubeMarkerArray( - {data.stop_target_object.value()}, "stop_target", createMarkerScale(3.4, 1.9, 1.9), - createMarkerColor(1.0, 0.0, 0.42, 0.5)), + {data.stop_target_object.value()}, "stop_target", create_marker_scale(3.4, 1.9, 1.9), + create_marker_color(1.0, 0.0, 0.42, 0.5)), &msg); return msg; @@ -529,9 +529,9 @@ MarkerArray createDrivableBounds( // right bound { - auto marker = createDefaultMarker( - "map", current_time, ns + "_right", 0L, Marker::LINE_STRIP, createMarkerScale(0.4, 0.0, 0.0), - createMarkerColor(r, g, b, 0.999)); + auto marker = create_default_marker( + "map", current_time, ns + "_right", 0L, Marker::LINE_STRIP, + create_marker_scale(0.4, 0.0, 0.0), create_marker_color(r, g, b, 0.999)); for (const auto & p : data.right_bound) { marker.points.push_back(p); @@ -542,9 +542,9 @@ MarkerArray createDrivableBounds( // left bound { - auto marker = createDefaultMarker( - "map", current_time, ns + "_left", 0L, Marker::LINE_STRIP, createMarkerScale(0.4, 0.0, 0.0), - createMarkerColor(r, g, b, 0.999)); + auto marker = create_default_marker( + "map", current_time, ns + "_left", 0L, Marker::LINE_STRIP, create_marker_scale(0.4, 0.0, 0.0), + create_marker_color(r, g, b, 0.999)); for (const auto & p : data.left_bound) { marker.points.push_back(p); @@ -580,7 +580,7 @@ MarkerArray createDebugMarkerArray( const auto & path = data.reference_path; - const auto add = [&msg](const MarkerArray & added) { appendMarkerArray(added, &msg); }; + const auto add = [&msg](const MarkerArray & added) { append_marker_array(added, &msg); }; const auto addAvoidLine = [&](const AvoidLineArray & al_arr, const auto & ns, auto r, auto g, auto b, double w = 0.05) { @@ -684,11 +684,11 @@ MarkerArray createDebugMarkerArray( if (parameters->enable_lane_marker) { add(laneletsAsTriangleMarkerArray( "drivable_lanes", transformToLanelets(data.drivable_lanes), - createMarkerColor(0.16, 1.0, 0.69, 0.2))); + create_marker_color(0.16, 1.0, 0.69, 0.2))); add(laneletsAsTriangleMarkerArray( - "current_lanes", data.current_lanelets, createMarkerColor(1.0, 1.0, 1.0, 0.2))); + "current_lanes", data.current_lanelets, create_marker_color(1.0, 1.0, 1.0, 0.2))); add(laneletsAsTriangleMarkerArray( - "safety_check_lanes", debug.safety_check_lanes, createMarkerColor(1.0, 0.0, 0.42, 0.2))); + "safety_check_lanes", debug.safety_check_lanes, create_marker_color(1.0, 0.0, 0.42, 0.2))); } // misc @@ -737,16 +737,16 @@ std::string toStrInfo(const autoware::behavior_path_planner::AvoidLineArray & ap } std::string toStrInfo(const autoware::behavior_path_planner::AvoidLine & ap) { - using autoware::universe_utils::toHexString; + using autoware_utils::to_hex_string; std::stringstream pids; for (const auto pid : ap.parent_ids) { - pids << toHexString(pid) << ", "; + pids << to_hex_string(pid) << ", "; } const auto & ps = ap.start.position; const auto & pe = ap.end.position; std::stringstream ss; - ss << "id = " << toHexString(ap.id) << ", shift length: " << ap.end_shift_length + ss << "id = " << to_hex_string(ap.id) << ", shift length: " << ap.end_shift_length << ", start_idx: " << ap.start_idx << ", end_idx: " << ap.end_idx << ", start_dist = " << ap.start_longitudinal << ", end_dist = " << ap.end_longitudinal << ", start_shift_length: " << ap.start_shift_length << ", start: (" << ps.x << ", " << ps.y diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/manager.cpp b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/manager.cpp index 41d54999658c2..7d64147ffcb46 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/manager.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/manager.cpp @@ -15,8 +15,8 @@ #include "autoware/behavior_path_static_obstacle_avoidance_module/manager.hpp" #include "autoware/behavior_path_static_obstacle_avoidance_module/parameter_helper.hpp" -#include "autoware/universe_utils/ros/parameter.hpp" -#include "autoware/universe_utils/ros/update_param.hpp" +#include "autoware_utils/ros/parameter.hpp" +#include "autoware_utils/ros/update_param.hpp" #include @@ -28,8 +28,8 @@ namespace autoware::behavior_path_planner { void StaticObstacleAvoidanceModuleManager::init(rclcpp::Node * node) { - using autoware::universe_utils::getOrDeclareParameter; using autoware_perception_msgs::msg::ObjectClassification; + using autoware_utils::get_or_declare_parameter; // init manager interface initInterface(node, {"left", "right"}); @@ -42,25 +42,25 @@ void StaticObstacleAvoidanceModuleManager::init(rclcpp::Node * node) void StaticObstacleAvoidanceModuleManager::updateModuleParams( const std::vector & parameters) { - using autoware::universe_utils::updateParam; using autoware_perception_msgs::msg::ObjectClassification; + using autoware_utils::update_param; auto p = parameters_; const auto update_object_param = [&p, ¶meters]( const auto & semantic, const std::string & ns) { auto & config = p->object_parameters.at(semantic); - updateParam(parameters, ns + "th_moving_speed", config.moving_speed_threshold); - updateParam(parameters, ns + "th_moving_time", config.moving_time_threshold); - updateParam(parameters, ns + "max_expand_ratio", config.max_expand_ratio); - updateParam(parameters, ns + "envelope_buffer_margin", config.envelope_buffer_margin); - updateParam(parameters, ns + "lateral_margin.soft_margin", config.lateral_soft_margin); - updateParam(parameters, ns + "lateral_margin.hard_margin", config.lateral_hard_margin); - updateParam( + update_param(parameters, ns + "th_moving_speed", config.moving_speed_threshold); + update_param(parameters, ns + "th_moving_time", config.moving_time_threshold); + update_param(parameters, ns + "max_expand_ratio", config.max_expand_ratio); + update_param(parameters, ns + "envelope_buffer_margin", config.envelope_buffer_margin); + update_param(parameters, ns + "lateral_margin.soft_margin", config.lateral_soft_margin); + update_param(parameters, ns + "lateral_margin.hard_margin", config.lateral_hard_margin); + update_param( parameters, ns + "lateral_margin.hard_margin_for_parked_vehicle", config.lateral_hard_margin_for_parked_vehicle); - updateParam(parameters, ns + "longitudinal_margin", config.longitudinal_margin); - updateParam( + update_param(parameters, ns + "longitudinal_margin", config.longitudinal_margin); + update_param( parameters, ns + "th_error_eclipse_long_radius", config.th_error_eclipse_long_radius); }; @@ -75,10 +75,10 @@ void StaticObstacleAvoidanceModuleManager::updateModuleParams( update_object_param(ObjectClassification::BICYCLE, ns + "bicycle."); update_object_param(ObjectClassification::UNKNOWN, ns + "unknown."); - updateParam( + update_param( parameters, ns + "lower_distance_for_polygon_expansion", p->lower_distance_for_polygon_expansion); - updateParam( + update_param( parameters, ns + "upper_distance_for_polygon_expansion", p->upper_distance_for_polygon_expansion); } @@ -88,7 +88,7 @@ void StaticObstacleAvoidanceModuleManager::updateModuleParams( if (p->object_parameters.count(object_type) == 0) { return; } - updateParam(parameters, ns, p->object_parameters.at(object_type).is_avoidance_target); + update_param(parameters, ns, p->object_parameters.at(object_type).is_avoidance_target); }; const std::string ns = "avoidance.target_filtering."; @@ -101,106 +101,107 @@ void StaticObstacleAvoidanceModuleManager::updateModuleParams( set_target_flag(ObjectClassification::MOTORCYCLE, ns + "target_type.motorcycle"); set_target_flag(ObjectClassification::UNKNOWN, ns + "target_type.unknown"); - updateParam( + update_param( parameters, ns + "object_check_goal_distance", p->object_check_goal_distance); - updateParam( + update_param( parameters, ns + "object_check_return_pose_distance", p->object_check_return_pose_distance); - updateParam(parameters, ns + "max_compensation_time", p->object_last_seen_threshold); + update_param(parameters, ns + "max_compensation_time", p->object_last_seen_threshold); } { const std::string ns = "avoidance.target_filtering.parked_vehicle."; - updateParam( + update_param( parameters, ns + "th_offset_from_centerline", p->threshold_distance_object_is_on_center); - updateParam(parameters, ns + "th_shiftable_ratio", p->object_check_shiftable_ratio); - updateParam( + update_param(parameters, ns + "th_shiftable_ratio", p->object_check_shiftable_ratio); + update_param( parameters, ns + "min_road_shoulder_width", p->object_check_min_road_shoulder_width); } { const std::string ns = "avoidance.target_filtering.detection_area."; - updateParam(parameters, ns + "static", p->use_static_detection_area); - updateParam( + update_param(parameters, ns + "static", p->use_static_detection_area); + update_param( parameters, ns + "min_forward_distance", p->object_check_min_forward_distance); - updateParam( + update_param( parameters, ns + "max_forward_distance", p->object_check_max_forward_distance); - updateParam(parameters, ns + "backward_distance", p->object_check_backward_distance); + update_param(parameters, ns + "backward_distance", p->object_check_backward_distance); } { const std::string ns = "avoidance.target_filtering.merging_vehicle."; - updateParam(parameters, ns + "th_overhang_distance", p->th_overhang_distance); + update_param(parameters, ns + "th_overhang_distance", p->th_overhang_distance); } { const std::string ns = "avoidance.avoidance.lateral.avoidance_for_ambiguous_vehicle."; - updateParam(parameters, ns + "policy", p->policy_ambiguous_vehicle); - updateParam( + update_param(parameters, ns + "policy", p->policy_ambiguous_vehicle); + update_param( parameters, ns + "wait_and_see.th_closest_distance", p->wait_and_see_th_closest_distance); - updateParam( + update_param( parameters, ns + "condition.th_stopped_time", p->time_threshold_for_ambiguous_vehicle); - updateParam( + update_param( parameters, ns + "condition.th_moving_distance", p->distance_threshold_for_ambiguous_vehicle); - updateParam( + update_param( parameters, ns + "ignore_area.traffic_light.front_distance", p->object_ignore_section_traffic_light_in_front_distance); - updateParam( + update_param( parameters, ns + "ignore_area.crosswalk.front_distance", p->object_ignore_section_crosswalk_in_front_distance); - updateParam( + update_param( parameters, ns + "ignore_area.crosswalk.behind_distance", p->object_ignore_section_crosswalk_behind_distance); } { const std::string ns = "avoidance.target_filtering.freespace."; - updateParam( + update_param( parameters, ns + "condition.th_stopped_time", p->freespace_condition_th_stopped_time); } { const std::string ns = "avoidance.target_filtering.intersection."; - updateParam(parameters, ns + "yaw_deviation", p->object_check_yaw_deviation); + update_param(parameters, ns + "yaw_deviation", p->object_check_yaw_deviation); } { const std::string ns = "avoidance.avoidance.lateral."; - updateParam(parameters, ns + "th_avoid_execution", p->lateral_execution_threshold); - updateParam(parameters, ns + "th_small_shift_length", p->lateral_small_shift_threshold); - updateParam( + update_param(parameters, ns + "th_avoid_execution", p->lateral_execution_threshold); + update_param( + parameters, ns + "th_small_shift_length", p->lateral_small_shift_threshold); + update_param( parameters, ns + "soft_drivable_bound_margin", p->soft_drivable_bound_margin); - updateParam( + update_param( parameters, ns + "hard_drivable_bound_margin", p->hard_drivable_bound_margin); } { const std::string ns = "avoidance.avoidance.longitudinal."; - updateParam(parameters, ns + "min_prepare_time", p->min_prepare_time); - updateParam(parameters, ns + "max_prepare_time", p->max_prepare_time); - updateParam(parameters, ns + "min_prepare_distance", p->min_prepare_distance); - updateParam(parameters, ns + "min_slow_down_speed", p->min_slow_down_speed); - updateParam(parameters, ns + "buf_slow_down_speed", p->buf_slow_down_speed); - updateParam(parameters, ns + "consider_front_overhang", p->consider_front_overhang); - updateParam(parameters, ns + "consider_rear_overhang", p->consider_rear_overhang); + update_param(parameters, ns + "min_prepare_time", p->min_prepare_time); + update_param(parameters, ns + "max_prepare_time", p->max_prepare_time); + update_param(parameters, ns + "min_prepare_distance", p->min_prepare_distance); + update_param(parameters, ns + "min_slow_down_speed", p->min_slow_down_speed); + update_param(parameters, ns + "buf_slow_down_speed", p->buf_slow_down_speed); + update_param(parameters, ns + "consider_front_overhang", p->consider_front_overhang); + update_param(parameters, ns + "consider_rear_overhang", p->consider_rear_overhang); } { const std::string ns = "avoidance.cancel."; - updateParam(parameters, ns + "force.duration_time", p->force_deactivate_duration_time); + update_param(parameters, ns + "force.duration_time", p->force_deactivate_duration_time); } { const std::string ns = "avoidance.stop."; - updateParam(parameters, ns + "max_distance", p->stop_max_distance); - updateParam(parameters, ns + "stop_buffer", p->stop_buffer); + update_param(parameters, ns + "max_distance", p->stop_max_distance); + update_param(parameters, ns + "stop_buffer", p->stop_buffer); } { const std::string ns = "avoidance.policy."; - updateParam(parameters, ns + "make_approval_request", p->policy_approval); - updateParam(parameters, ns + "deceleration", p->policy_deceleration); - updateParam(parameters, ns + "lateral_margin", p->policy_lateral_margin); - updateParam( + update_param(parameters, ns + "make_approval_request", p->policy_approval); + update_param(parameters, ns + "deceleration", p->policy_deceleration); + update_param(parameters, ns + "lateral_margin", p->policy_lateral_margin); + update_param( parameters, ns + "use_shorten_margin_immediately", p->use_shorten_margin_immediately); if (p->policy_approval != "per_shift_line" && p->policy_approval != "per_avoidance_maneuver") { @@ -226,13 +227,13 @@ void StaticObstacleAvoidanceModuleManager::updateModuleParams( const std::string ns = "avoidance.constrains.lateral."; std::vector velocity_map; - updateParam>(parameters, ns + "velocity", velocity_map); + update_param>(parameters, ns + "velocity", velocity_map); std::vector lateral_max_accel_map; - updateParam>(parameters, ns + "max_accel_values", lateral_max_accel_map); + update_param>(parameters, ns + "max_accel_values", lateral_max_accel_map); std::vector lateral_min_jerk_map; - updateParam>(parameters, ns + "min_jerk_values", lateral_min_jerk_map); + update_param>(parameters, ns + "min_jerk_values", lateral_min_jerk_map); std::vector lateral_max_jerk_map; - updateParam>(parameters, ns + "max_jerk_values", lateral_max_jerk_map); + update_param>(parameters, ns + "max_jerk_values", lateral_max_jerk_map); if (!velocity_map.empty()) { p->velocity_map = velocity_map; @@ -254,37 +255,38 @@ void StaticObstacleAvoidanceModuleManager::updateModuleParams( { const std::string ns = "avoidance.constraints.longitudinal."; - updateParam(parameters, ns + "nominal_deceleration", p->nominal_deceleration); - updateParam(parameters, ns + "nominal_jerk", p->nominal_jerk); - updateParam(parameters, ns + "max_deceleration", p->max_deceleration); - updateParam(parameters, ns + "max_jerk", p->max_jerk); - updateParam(parameters, ns + "max_acceleration", p->max_acceleration); - updateParam( + update_param(parameters, ns + "nominal_deceleration", p->nominal_deceleration); + update_param(parameters, ns + "nominal_jerk", p->nominal_jerk); + update_param(parameters, ns + "max_deceleration", p->max_deceleration); + update_param(parameters, ns + "max_jerk", p->max_jerk); + update_param(parameters, ns + "max_acceleration", p->max_acceleration); + update_param( parameters, ns + "min_velocity_to_limit_max_acceleration", p->min_velocity_to_limit_max_acceleration); } { const std::string ns = "avoidance.shift_line_pipeline."; - updateParam(parameters, ns + "trim.quantize_size", p->quantize_size); - updateParam(parameters, ns + "trim.th_similar_grad_1", p->th_similar_grad_1); - updateParam(parameters, ns + "trim.th_similar_grad_2", p->th_similar_grad_2); - updateParam(parameters, ns + "trim.th_similar_grad_3", p->th_similar_grad_3); + update_param(parameters, ns + "trim.quantize_size", p->quantize_size); + update_param(parameters, ns + "trim.th_similar_grad_1", p->th_similar_grad_1); + update_param(parameters, ns + "trim.th_similar_grad_2", p->th_similar_grad_2); + update_param(parameters, ns + "trim.th_similar_grad_3", p->th_similar_grad_3); } { const std::string ns = "avoidance.debug."; - updateParam( + update_param( parameters, ns + "enable_other_objects_marker", p->enable_other_objects_marker); - updateParam(parameters, ns + "enable_other_objects_info", p->enable_other_objects_info); - updateParam( + update_param(parameters, ns + "enable_other_objects_info", p->enable_other_objects_info); + update_param( parameters, ns + "enable_detection_area_marker", p->enable_detection_area_marker); - updateParam( + update_param( parameters, ns + "enable_drivable_bound_marker", p->enable_drivable_bound_marker); - updateParam(parameters, ns + "enable_safety_check_marker", p->enable_safety_check_marker); - updateParam(parameters, ns + "enable_shift_line_marker", p->enable_shift_line_marker); - updateParam(parameters, ns + "enable_lane_marker", p->enable_lane_marker); - updateParam(parameters, ns + "enable_misc_marker", p->enable_misc_marker); + update_param( + parameters, ns + "enable_safety_check_marker", p->enable_safety_check_marker); + update_param(parameters, ns + "enable_shift_line_marker", p->enable_shift_line_marker); + update_param(parameters, ns + "enable_lane_marker", p->enable_lane_marker); + update_param(parameters, ns + "enable_misc_marker", p->enable_misc_marker); } std::for_each(observers_.begin(), observers_.end(), [&p](const auto & observer) { diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/scene.cpp b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/scene.cpp index e0f96c3ab2ed1..948d0cdd58b4b 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/scene.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/scene.cpp @@ -24,10 +24,10 @@ #include "autoware/behavior_path_static_obstacle_avoidance_module/debug.hpp" #include "autoware/behavior_path_static_obstacle_avoidance_module/utils.hpp" -#include -#include #include #include +#include +#include #include #include @@ -186,7 +186,8 @@ bool StaticObstacleAvoidanceModule::canTransitSuccessState() constexpr double THRESHOLD = 1.0; const auto is_further_than_threshold = - calcDistance2d(getEgoPose(), getPose(data.reference_path.points.back())) > THRESHOLD; + calc_distance2d(getEgoPose(), autoware_utils::get_pose(data.reference_path.points.back())) > + THRESHOLD; if (is_further_than_threshold && arrived_path_end_) { RCLCPP_WARN(getLogger(), "Reach path end point. Exit."); return true; @@ -199,7 +200,7 @@ bool StaticObstacleAvoidanceModule::canTransitSuccessState() void StaticObstacleAvoidanceModule::fillFundamentalData( AvoidancePlanningData & data, DebugData & debug) { - universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + autoware_utils::ScopedTimeTrack st(__func__, *time_keeper_); // reference pose data.reference_pose = utils::getUnshiftedEgoPose(getEgoPose(), helper_->getPreviousSplineShiftPath()); @@ -327,7 +328,7 @@ void StaticObstacleAvoidanceModule::fillFundamentalData( void StaticObstacleAvoidanceModule::fillAvoidanceTargetObjects( AvoidancePlanningData & data, DebugData & debug) const { - universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + autoware_utils::ScopedTimeTrack st(__func__, *time_keeper_); using utils::static_obstacle_avoidance::filterTargetObjects; using utils::static_obstacle_avoidance::separateObjectsByPath; using utils::static_obstacle_avoidance::updateRoadShoulderDistance; @@ -367,7 +368,7 @@ void StaticObstacleAvoidanceModule::fillAvoidanceTargetObjects( std::vector debug_info_array; const auto append = [&](const auto & o) { AvoidanceDebugMsg debug_info; - debug_info.object_id = toHexString(o.object.object_id); + debug_info.object_id = to_hex_string(o.object.object_id); debug_info.longitudinal_distance = o.longitudinal; debug_info.lateral_distance_from_centerline = o.to_centerline; debug_info_array.push_back(debug_info); @@ -381,7 +382,7 @@ void StaticObstacleAvoidanceModule::fillAvoidanceTargetObjects( void StaticObstacleAvoidanceModule::fillAvoidanceTargetData(ObjectDataArray & objects) const { - universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + autoware_utils::ScopedTimeTrack st(__func__, *time_keeper_); using utils::static_obstacle_avoidance::fillAvoidanceNecessity; using utils::static_obstacle_avoidance::fillObjectStoppableJudge; @@ -398,7 +399,7 @@ void StaticObstacleAvoidanceModule::fillAvoidanceTargetData(ObjectDataArray & ob ObjectData StaticObstacleAvoidanceModule::createObjectData( const AvoidancePlanningData & data, const PredictedObject & object) const { - universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + autoware_utils::ScopedTimeTrack st(__func__, *time_keeper_); using boost::geometry::return_centroid; const auto & path_points = data.reference_path.points; @@ -416,7 +417,7 @@ ObjectData StaticObstacleAvoidanceModule::createObjectData( const auto lower = parameters_->lower_distance_for_polygon_expansion; const auto upper = parameters_->upper_distance_for_polygon_expansion; const auto clamp = - std::clamp(calcDistance2d(getEgoPose(), object_pose) - lower, 0.0, upper) / upper; + std::clamp(calc_distance2d(getEgoPose(), object_pose) - lower, 0.0, upper) / upper; object_data.distance_factor = object_parameter.max_expand_ratio * clamp + 1.0; // Calc envelop polygon. @@ -431,7 +432,7 @@ ObjectData StaticObstacleAvoidanceModule::createObjectData( object_data, stopped_objects_, parameters_); // Calc lateral deviation from path to target object. - object_data.direction = calcLateralDeviation(object_closest_pose, object_pose.position) > 0.0 + object_data.direction = calc_lateral_deviation(object_closest_pose, object_pose.position) > 0.0 ? Direction::LEFT : Direction::RIGHT; @@ -440,7 +441,7 @@ ObjectData StaticObstacleAvoidanceModule::createObjectData( bool StaticObstacleAvoidanceModule::canYieldManeuver(const AvoidancePlanningData & data) const { - universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + autoware_utils::ScopedTimeTrack st(__func__, *time_keeper_); // transit yield maneuver only when the avoidance maneuver is not initiated. if (helper_->isShifted()) { RCLCPP_DEBUG(getLogger(), "avoidance maneuver already initiated."); @@ -494,7 +495,7 @@ bool StaticObstacleAvoidanceModule::canYieldManeuver(const AvoidancePlanningData void StaticObstacleAvoidanceModule::fillShiftLine( AvoidancePlanningData & data, DebugData & debug) const { - universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + autoware_utils::ScopedTimeTrack st(__func__, *time_keeper_); auto path_shifter = path_shifter_; /** @@ -549,7 +550,7 @@ void StaticObstacleAvoidanceModule::fillShiftLine( void StaticObstacleAvoidanceModule::fillEgoStatus( AvoidancePlanningData & data, [[maybe_unused]] DebugData & debug) const { - universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + autoware_utils::ScopedTimeTrack st(__func__, *time_keeper_); data.state = getCurrentModuleState(data); /** @@ -701,7 +702,7 @@ void StaticObstacleAvoidanceModule::fillEgoStatus( void StaticObstacleAvoidanceModule::fillDebugData( const AvoidancePlanningData & data, [[maybe_unused]] DebugData & debug) const { - universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + autoware_utils::ScopedTimeTrack st(__func__, *time_keeper_); if (!data.stop_target_object) { return; } @@ -735,7 +736,7 @@ void StaticObstacleAvoidanceModule::fillDebugData( data.reference_path.points, getEgoPosition(), o_front.longitudinal - total_avoid_distance); dead_pose_ = dead_pose_opt.has_value() ? PoseWithDetail(dead_pose_opt.value()) - : PoseWithDetail(getPose(data.reference_path.points.front())); + : PoseWithDetail(autoware_utils::get_pose(data.reference_path.points.front())); } void StaticObstacleAvoidanceModule::updateEgoBehavior( @@ -781,7 +782,7 @@ void StaticObstacleAvoidanceModule::updateEgoBehavior( bool StaticObstacleAvoidanceModule::isSafePath( ShiftedPath & shifted_path, [[maybe_unused]] DebugData & debug) const { - universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + autoware_utils::ScopedTimeTrack st(__func__, *time_keeper_); const auto & p = planner_data_->parameters; if (force_deactivated_) { @@ -847,8 +848,7 @@ bool StaticObstacleAvoidanceModule::isSafePath( for (const auto & object : safety_check_target_objects) { auto current_debug_data = utils::path_safety_checker::createObjectDebug(object); - const auto obj_polygon = - autoware::universe_utils::toPolygon2d(object.initial_pose, object.shape); + const auto obj_polygon = autoware_utils::to_polygon2d(object.initial_pose, object.shape); const auto is_object_front = utils::path_safety_checker::isTargetObjectFront( getEgoPose(), obj_polygon, p.vehicle_info.max_longitudinal_offset_m); @@ -894,7 +894,7 @@ bool StaticObstacleAvoidanceModule::isSafePath( PathWithLaneId StaticObstacleAvoidanceModule::extendBackwardLength( const PathWithLaneId & original_path) const { - universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + autoware_utils::ScopedTimeTrack st(__func__, *time_keeper_); const auto previous_path = helper_->getPreviousReferencePath(); const auto longest_dist_to_shift_point = [&]() { @@ -917,7 +917,7 @@ PathWithLaneId StaticObstacleAvoidanceModule::extendBackwardLength( const size_t orig_ego_idx = planner_data_->findEgoIndex(original_path.points); const auto prev_ego_idx = autoware::motion_utils::findNearestSegmentIndex( - previous_path.points, getPose(original_path.points.at(orig_ego_idx)), + previous_path.points, autoware_utils::get_pose(original_path.points.at(orig_ego_idx)), std::numeric_limits::max(), planner_data_->parameters.ego_nearest_yaw_threshold); if (!prev_ego_idx) { return original_path; @@ -926,8 +926,8 @@ PathWithLaneId StaticObstacleAvoidanceModule::extendBackwardLength( size_t clip_idx = 0; double accumulated_length = 0.0; for (size_t i = prev_ego_idx.value(); i > 0; i--) { - accumulated_length += autoware::universe_utils::calcDistance2d( - previous_path.points.at(i - 1), previous_path.points.at(i)); + accumulated_length += + autoware_utils::calc_distance2d(previous_path.points.at(i - 1), previous_path.points.at(i)); if (accumulated_length > backward_length) { clip_idx = i; break; @@ -1064,7 +1064,7 @@ auto StaticObstacleAvoidanceModule::getTurnSignal( BehaviorModuleOutput StaticObstacleAvoidanceModule::plan() { - universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + autoware_utils::ScopedTimeTrack st(__func__, *time_keeper_); const auto & data = avoid_data_; resetPathCandidate(); @@ -1180,7 +1180,7 @@ BehaviorModuleOutput StaticObstacleAvoidanceModule::plan() CandidateOutput StaticObstacleAvoidanceModule::planCandidate() const { - universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + autoware_utils::ScopedTimeTrack st(__func__, *time_keeper_); const auto & data = avoid_data_; CandidateOutput output; @@ -1224,7 +1224,7 @@ CandidateOutput StaticObstacleAvoidanceModule::planCandidate() const BehaviorModuleOutput StaticObstacleAvoidanceModule::planWaitingApproval() { - universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + autoware_utils::ScopedTimeTrack st(__func__, *time_keeper_); BehaviorModuleOutput out = plan(); if (path_shifter_.getShiftLines().empty()) { @@ -1268,9 +1268,9 @@ void StaticObstacleAvoidanceModule::updatePathShifter(const AvoidLineArray & shi {uuid_map_.at("right"), sl_front.start, sl_back.end, relative_longitudinal}); } - uuid_map_.at("left") = generateUUID(); - uuid_map_.at("right") = generateUUID(); - candidate_uuid_ = generateUUID(); + uuid_map_.at("left") = generate_uuid(); + uuid_map_.at("right") = generate_uuid(); + candidate_uuid_ = generate_uuid(); lockNewModuleLaunch(); } @@ -1281,7 +1281,7 @@ void StaticObstacleAvoidanceModule::updatePathShifter(const AvoidLineArray & shi void StaticObstacleAvoidanceModule::addNewShiftLines( PathShifter & path_shifter, const AvoidLineArray & new_shift_lines) const { - universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + autoware_utils::ScopedTimeTrack st(__func__, *time_keeper_); ShiftLineArray future = utils::static_obstacle_avoidance::toShiftLineArray(new_shift_lines); size_t min_start_idx = std::numeric_limits::max(); @@ -1348,7 +1348,7 @@ void StaticObstacleAvoidanceModule::addNewShiftLines( bool StaticObstacleAvoidanceModule::isValidShiftLine( const AvoidLineArray & shift_lines, const PathShifter & shifter) const { - universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + autoware_utils::ScopedTimeTrack st(__func__, *time_keeper_); if (shift_lines.empty()) { return true; } @@ -1401,7 +1401,7 @@ bool StaticObstacleAvoidanceModule::isValidShiftLine( const auto left_bound = lanelet::utils::to2D(toLineString3d(avoid_data_.left_bound)); const auto right_bound = lanelet::utils::to2D(toLineString3d(avoid_data_.right_bound)); for (size_t i = start_idx; i <= end_idx; ++i) { - const auto p = getPoint(path.points.at(i)); + const auto p = get_point(path.points.at(i)); lanelet::BasicPoint2d basic_point{p.x, p.y}; const auto shift_length = proposed_shift_path.shift_length.at(i); @@ -1424,7 +1424,7 @@ bool StaticObstacleAvoidanceModule::isValidShiftLine( void StaticObstacleAvoidanceModule::updateData() { - universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + autoware_utils::ScopedTimeTrack st(__func__, *time_keeper_); using utils::static_obstacle_avoidance::toShiftedPath; helper_->setData(planner_data_); @@ -1515,14 +1515,14 @@ void StaticObstacleAvoidanceModule::initRTCStatus() { left_shift_array_.clear(); right_shift_array_.clear(); - uuid_map_.at("left") = generateUUID(); - uuid_map_.at("right") = generateUUID(); - candidate_uuid_ = generateUUID(); + uuid_map_.at("left") = generate_uuid(); + uuid_map_.at("right") = generate_uuid(); + candidate_uuid_ = generate_uuid(); } void StaticObstacleAvoidanceModule::updateRTCData() { - universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + autoware_utils::ScopedTimeTrack st(__func__, *time_keeper_); const auto & data = avoid_data_; updateRegisteredRTCStatus(helper_->getPreviousSplineShiftPath().path); @@ -1558,16 +1558,16 @@ void StaticObstacleAvoidanceModule::updateRTCData() void StaticObstacleAvoidanceModule::updateInfoMarker(const AvoidancePlanningData & data) const { - universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + autoware_utils::ScopedTimeTrack st(__func__, *time_keeper_); using utils::static_obstacle_avoidance::createAmbiguousObjectsMarkerArray; using utils::static_obstacle_avoidance::createStopTargetObjectMarkerArray; using utils::static_obstacle_avoidance::createTargetObjectsMarkerArray; info_marker_.markers.clear(); - appendMarkerArray( + append_marker_array( createTargetObjectsMarkerArray(data.target_objects, "target_objects"), &info_marker_); - appendMarkerArray(createStopTargetObjectMarkerArray(data), &info_marker_); - appendMarkerArray( + append_marker_array(createStopTargetObjectMarkerArray(data), &info_marker_); + append_marker_array( createAmbiguousObjectsMarkerArray( data.target_objects, getEgoPose(), parameters_->policy_ambiguous_vehicle), &info_marker_); @@ -1577,7 +1577,7 @@ void StaticObstacleAvoidanceModule::updateDebugMarker( const BehaviorModuleOutput & output, const AvoidancePlanningData & data, const PathShifter & shifter, const DebugData & debug) const { - universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + autoware_utils::ScopedTimeTrack st(__func__, *time_keeper_); debug_marker_.markers.clear(); debug_marker_ = utils::static_obstacle_avoidance::createDebugMarkerArray( output, data, shifter, debug, parameters_); @@ -1602,7 +1602,7 @@ void StaticObstacleAvoidanceModule::updateAvoidanceDebugData( double StaticObstacleAvoidanceModule::calcDistanceToStopLine(const ObjectData & object) const { - universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + autoware_utils::ScopedTimeTrack st(__func__, *time_keeper_); const auto & p = parameters_; const auto & vehicle_width = planner_data_->parameters.vehicle_width; @@ -1644,7 +1644,7 @@ double StaticObstacleAvoidanceModule::calcDistanceToStopLine(const ObjectData & void StaticObstacleAvoidanceModule::insertReturnDeadLine( const bool use_constraints_for_decel, ShiftedPath & shifted_path) const { - universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + autoware_utils::ScopedTimeTrack st(__func__, *time_keeper_); const auto & data = avoid_data_; if (data.to_return_point > planner_data_->parameters.forward_path_length) { @@ -1736,7 +1736,7 @@ void StaticObstacleAvoidanceModule::insertReturnDeadLine( void StaticObstacleAvoidanceModule::insertWaitPoint( const bool use_constraints_for_decel, ShiftedPath & shifted_path) const { - universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + autoware_utils::ScopedTimeTrack st(__func__, *time_keeper_); const auto & data = avoid_data_; // If avoidance path is NOT valid, don't insert any stop points. @@ -1790,7 +1790,7 @@ void StaticObstacleAvoidanceModule::insertWaitPoint( void StaticObstacleAvoidanceModule::insertStopPoint( const bool use_constraints_for_decel, ShiftedPath & shifted_path) const { - universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + autoware_utils::ScopedTimeTrack st(__func__, *time_keeper_); const auto & data = avoid_data_; if (data.safe) { @@ -1838,7 +1838,7 @@ void StaticObstacleAvoidanceModule::insertStopPoint( void StaticObstacleAvoidanceModule::insertPrepareVelocity(ShiftedPath & shifted_path) const { - universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + autoware_utils::ScopedTimeTrack st(__func__, *time_keeper_); const auto & data = avoid_data_; // If avoidance path is NOT safe, don't insert any slow down sections. @@ -1953,7 +1953,7 @@ void StaticObstacleAvoidanceModule::insertPrepareVelocity(ShiftedPath & shifted_ void StaticObstacleAvoidanceModule::insertAvoidanceVelocity(ShiftedPath & shifted_path) const { - universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + autoware_utils::ScopedTimeTrack st(__func__, *time_keeper_); const auto & data = avoid_data_; // do nothing if no shift line is approved. diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/shift_line_generator.cpp b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/shift_line_generator.cpp index 52d5ee49c9a68..3cd64aecccfc0 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/shift_line_generator.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/shift_line_generator.cpp @@ -354,7 +354,7 @@ AvoidOutlines ShiftLineGenerator::generateAvoidOutline( al_avoid.end_longitudinal = to_shift_end; // misc - al_avoid.id = generateUUID(); + al_avoid.id = generate_uuid(); al_avoid.object = o; al_avoid.object_on_right = utils::static_obstacle_avoidance::isOnRight(o); } @@ -380,7 +380,7 @@ AvoidOutlines ShiftLineGenerator::generateAvoidOutline( al_return.end_shift_length = 0.0; // misc - al_return.id = generateUUID(); + al_return.id = generate_uuid(); al_return.object = o; al_return.object_on_right = utils::static_obstacle_avoidance::isOnRight(o); } @@ -399,7 +399,7 @@ AvoidOutlines ShiftLineGenerator::generateAvoidOutline( return true; } const bool has_object_near_goal = - autoware::universe_utils::calcDistance2d(goal_pose.position, o.getPosition()) < + autoware_utils::calc_distance2d(goal_pose.position, o.getPosition()) < parameters_->object_check_goal_distance; return has_object_near_goal; }(); @@ -579,7 +579,7 @@ void ShiftLineGenerator::generateTotalShiftLine( avoid_lines.front().start_longitudinal; for (size_t i = data.ego_closest_path_index; i <= avoid_lines.front().start_idx; ++i) { - sl.shift_line.at(i) = helper_->getLinearShift(getPoint(path.points.at(i))); + sl.shift_line.at(i) = helper_->getLinearShift(get_point(path.points.at(i))); sl.shift_line_grad.at(i) = grad_first_shift_line; } @@ -658,7 +658,7 @@ AvoidLineArray ShiftLineGenerator::extractShiftLinesFromLine( found_first_start = true; } else { setEndData(al, shift, p, i, arcs.at(i)); - al.id = generateUUID(); + al.id = generate_uuid(); merged_avoid_lines.push_back(al); setStartData(al, 0.0, p, i, arcs.at(i)); // start length is overwritten later. } @@ -668,7 +668,7 @@ AvoidLineArray ShiftLineGenerator::extractShiftLinesFromLine( const auto & p = path.points.at(N - 1).point.pose; const auto shift = sl.shift_line.at(N - 1); setEndData(al, shift, p, N - 1, arcs.at(N - 1)); - al.id = generateUUID(); + al.id = generate_uuid(); merged_avoid_lines.push_back(al); } @@ -714,7 +714,7 @@ AvoidOutlines ShiftLineGenerator::applyMergeProcess( continue; } - const auto merged_shift_line = merge(return_line.value(), avoid_line, generateUUID()); + const auto merged_shift_line = merge(return_line.value(), avoid_line, generate_uuid()); if (!helper_->isComfortable(AvoidLineArray{merged_shift_line})) { ret.push_back(outlines.at(i)); @@ -763,7 +763,7 @@ AvoidOutlines ShiftLineGenerator::applyFillGapProcess( if (outline.middle_lines.empty()) { const auto new_line = outline.return_line.has_value() - ? fill(outline.avoid_line, outline.return_line.value(), generateUUID()) + ? fill(outline.avoid_line, outline.return_line.value(), generate_uuid()) : outline.avoid_line; outline.middle_lines.push_back(new_line); debug.step1_filled_shift_line.push_back(new_line); @@ -772,7 +772,7 @@ AvoidOutlines ShiftLineGenerator::applyFillGapProcess( helper_->alignShiftLinesOrder(outline.middle_lines, false); if (outline.avoid_line.end_longitudinal < outline.middle_lines.front().start_longitudinal) { - const auto new_line = fill(outline.avoid_line, outline.middle_lines.front(), generateUUID()); + const auto new_line = fill(outline.avoid_line, outline.middle_lines.front(), generate_uuid()); outline.middle_lines.push_back(new_line); debug.step1_filled_shift_line.push_back(new_line); } @@ -783,7 +783,7 @@ AvoidOutlines ShiftLineGenerator::applyFillGapProcess( outline.return_line.has_value() && outline.middle_lines.back().end_longitudinal < outline.return_line->start_longitudinal) { const auto new_line = - fill(outline.middle_lines.back(), outline.return_line.value(), generateUUID()); + fill(outline.middle_lines.back(), outline.return_line.value(), generate_uuid()); outline.middle_lines.push_back(new_line); debug.step1_filled_shift_line.push_back(new_line); } @@ -818,7 +818,7 @@ AvoidLineArray ShiftLineGenerator::applyFillGapProcess( ego_line, helper_->getEgoLinearShift(), data.reference_pose, data.ego_closest_path_index, 0.0); - const auto new_line = fill(ego_line, sorted.front(), generateUUID()); + const auto new_line = fill(ego_line, sorted.front(), generate_uuid()); ret.push_back(new_line); debug.step1_front_shift_line.push_back(new_line); } @@ -831,7 +831,7 @@ AvoidLineArray ShiftLineGenerator::applyFillGapProcess( continue; } - const auto new_line = fill(sorted.at(i), sorted.at(i + 1), generateUUID()); + const auto new_line = fill(sorted.at(i), sorted.at(i + 1), generate_uuid()); ret.push_back(new_line); debug.step1_front_shift_line.push_back(new_line); } @@ -1060,7 +1060,7 @@ AvoidLineArray ShiftLineGenerator::addReturnShiftLine( if (utils::isAllowedGoalModification(data_->route_handler)) { const auto has_object_near_goal = std::any_of(data.target_objects.begin(), data.target_objects.end(), [&](const auto & o) { - return autoware::universe_utils::calcDistance2d( + return autoware_utils::calc_distance2d( data_->route_handler->getGoalPose().position, o.getPosition()) < parameters_->object_check_goal_distance; }); @@ -1130,7 +1130,7 @@ AvoidLineArray ShiftLineGenerator::addReturnShiftLine( if (utils::isAllowedGoalModification(data_->route_handler)) { const bool has_last_shift_near_goal = std::any_of(data.target_objects.begin(), data.target_objects.end(), [&](const auto & o) { - return autoware::universe_utils::calcDistance2d(last_sl.end.position, o.getPosition()) < + return autoware_utils::calc_distance2d(last_sl.end.position, o.getPosition()) < parameters_->object_check_goal_distance; }); if (has_last_shift_near_goal) { @@ -1230,7 +1230,7 @@ AvoidLineArray ShiftLineGenerator::addReturnShiftLine( // shift point for prepare distance: from last shift to return-start point. if (nominal_prepare_distance > last_sl_distance) { AvoidLine al; - al.id = generateUUID(); + al.id = generate_uuid(); al.start_idx = last_sl.end_idx; al.start = last_sl.end; al.start_longitudinal = arclength_from_ego.at(al.start_idx); @@ -1247,7 +1247,7 @@ AvoidLineArray ShiftLineGenerator::addReturnShiftLine( // shift point for return to center line { AvoidLine al; - al.id = generateUUID(); + al.id = generate_uuid(); al.start_idx = utils::static_obstacle_avoidance::findPathIndexFromArclength( arclength_from_ego, prepare_distance_scaled); al.start = data.reference_path.points.at(al.start_idx).point.pose; diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/utils.cpp b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/utils.cpp index 8c09cd2e011f9..1c998268c73cd 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/utils.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/utils.cpp @@ -58,8 +58,7 @@ geometry_msgs::msg::Point32 createPoint32(const double x, const double y, const return p; } -geometry_msgs::msg::Polygon toMsg( - const autoware::universe_utils::Polygon2d & polygon, const double z) +geometry_msgs::msg::Polygon toMsg(const autoware_utils::Polygon2d & polygon, const double z) { geometry_msgs::msg::Polygon ret; for (const auto & p : polygon.outer()) { @@ -385,7 +384,7 @@ bool isParallelToEgoLane(const ObjectData & object, const double threshold) { const auto closest_pose = lanelet::utils::getClosestCenterPose(object.overhang_lanelet, object.getPosition()); - const auto yaw_deviation = std::abs(calcYawDeviation(closest_pose, object.getPose())); + const auto yaw_deviation = std::abs(calc_yaw_deviation(closest_pose, object.getPose())); return yaw_deviation < threshold || yaw_deviation > M_PI - threshold; } @@ -394,7 +393,7 @@ bool isMergingToEgoLane(const ObjectData & object) { const auto closest_pose = lanelet::utils::getClosestCenterPose(object.overhang_lanelet, object.getPosition()); - const auto yaw_deviation = calcYawDeviation(closest_pose, object.getPose()); + const auto yaw_deviation = calc_yaw_deviation(closest_pose, object.getPose()); if (isOnRight(object)) { if (yaw_deviation < 0.0 && -1.0 * M_PI_2 < yaw_deviation) { @@ -659,7 +658,7 @@ bool isNeverAvoidanceTarget( return true; } - const auto object_polygon = autoware::universe_utils::toPolygon2d(object.object); + const auto object_polygon = autoware_utils::to_polygon2d(object.object); const auto is_disjoint_right_lane = boost::geometry::disjoint(object_polygon, right_lane.value().polygon2d().basicPolygon()); if (is_disjoint_right_lane) { @@ -691,7 +690,7 @@ bool isNeverAvoidanceTarget( return true; } - const auto object_polygon = autoware::universe_utils::toPolygon2d(object.object); + const auto object_polygon = autoware_utils::to_polygon2d(object.object); const auto is_disjoint_left_lane = boost::geometry::disjoint(object_polygon, left_lane.value().polygon2d().basicPolygon()); if (is_disjoint_left_lane) { @@ -973,7 +972,7 @@ bool isSatisfiedWithVehicleCondition( } const auto is_moving_distance_longer_than_threshold = - calcDistance2d(object.init_pose, object.getPose()) > + calc_distance2d(object.init_pose, object.getPose()) > parameters->distance_threshold_for_ambiguous_vehicle; if (is_moving_distance_longer_than_threshold) { object.info = ObjectInfo::AMBIGUOUS_STOPPED_VEHICLE; @@ -1077,7 +1076,7 @@ double getRoadShoulderDistance( ObjectData & object, const AvoidancePlanningData & data, const std::shared_ptr & planner_data) { - using autoware::universe_utils::Point2d; + using autoware_utils::Point2d; using lanelet::utils::to2D; const auto object_closest_index = @@ -1103,26 +1102,27 @@ double getRoadShoulderDistance( for (size_t i = 1; i < bound.size(); i++) { { const auto p2 = - calcOffsetPose(p_tmp, 0.0, (isOnRight(object) ? 100.0 : -100.0), 0.0).position; + calc_offset_pose(p_tmp, 0.0, (isOnRight(object) ? 100.0 : -100.0), 0.0).position; const auto opt_intersect = - autoware::universe_utils::intersect(p1.second, p2, bound.at(i - 1), bound.at(i)); + autoware_utils::intersect(p1.second, p2, bound.at(i - 1), bound.at(i)); if (opt_intersect.has_value()) { intersects.emplace_back( - calcDistance2d(p1.second, opt_intersect.value()), p1.second, opt_intersect.value()); + calc_distance2d(p1.second, opt_intersect.value()), p1.second, opt_intersect.value()); break; } } { const auto p2 = - calcOffsetPose(p_tmp, 0.0, (isOnRight(object) ? -0.5 : 0.5) * envelope_polygon_width, 0.0) + calc_offset_pose( + p_tmp, 0.0, (isOnRight(object) ? -0.5 : 0.5) * envelope_polygon_width, 0.0) .position; const auto opt_intersect = - autoware::universe_utils::intersect(p1.second, p2, bound.at(i - 1), bound.at(i)); + autoware_utils::intersect(p1.second, p2, bound.at(i - 1), bound.at(i)); if (opt_intersect.has_value()) { intersects.emplace_back( - -1.0 * calcDistance2d(p1.second, opt_intersect.value()), p1.second, + -1.0 * calc_distance2d(p1.second, opt_intersect.value()), p1.second, opt_intersect.value()); break; } @@ -1168,8 +1168,8 @@ bool isWithinLanes( { const auto & rh = planner_data->route_handler; const auto & ego_pose = planner_data->self_odometry->pose.pose; - const auto transform = autoware::universe_utils::pose2transform(ego_pose); - const auto footprint = autoware::universe_utils::transformVector( + const auto transform = autoware_utils::pose2transform(ego_pose); + const auto footprint = autoware_utils::transform_vector( planner_data->parameters.vehicle_info.createFootprint(), transform); if (!closest_lanelet.has_value()) { @@ -1325,7 +1325,7 @@ void fillLongitudinalAndLengthByClosestEnvelopeFootprint( double min_distance = std::numeric_limits::max(); double max_distance = std::numeric_limits::lowest(); for (const auto & p : obj.envelope_poly.outer()) { - const auto point = autoware::universe_utils::createPoint(p.x(), p.y(), 0.0); + const auto point = autoware_utils::create_point(p.x(), p.y(), 0.0); // TODO(someone): search around first position where the ego should avoid the object. const double arc_length = autoware::motion_utils::calcSignedArcLength(path.points, ego_pos, point); @@ -1342,10 +1342,11 @@ std::vector> calcEnvelopeOverhangDistance( std::vector> overhang_points{}; for (const auto & p : object_data.envelope_poly.outer()) { - const auto point = autoware::universe_utils::createPoint(p.x(), p.y(), 0.0); + const auto point = autoware_utils::create_point(p.x(), p.y(), 0.0); // TODO(someone): search around first position where the ego should avoid the object. const auto idx = autoware::motion_utils::findNearestIndex(path.points, point); - const auto lateral = calcLateralDeviation(getPose(path.points.at(idx)), point); + const auto lateral = + calc_lateral_deviation(autoware_utils::get_pose(path.points.at(idx)), point); overhang_points.emplace_back(lateral, point); } std::sort(overhang_points.begin(), overhang_points.end(), [&](const auto & a, const auto & b) { @@ -1378,9 +1379,9 @@ Polygon2d createEnvelopePolygon( const Polygon2d & object_polygon, const Pose & closest_pose, const double envelope_buffer) { namespace bg = boost::geometry; - using autoware::universe_utils::expandPolygon; - using autoware::universe_utils::Point2d; - using autoware::universe_utils::Polygon2d; + using autoware_utils::expand_polygon; + using autoware_utils::Point2d; + using autoware_utils::Polygon2d; using Box = bg::model::box; const auto toPolygon2d = [](const geometry_msgs::msg::Polygon & polygon) { @@ -1394,7 +1395,7 @@ Polygon2d createEnvelopePolygon( }; Pose pose_2d = closest_pose; - pose_2d.orientation = createQuaternionFromRPY(0.0, 0.0, tf2::getYaw(closest_pose.orientation)); + pose_2d.orientation = create_quaternion_from_rpy(0.0, 0.0, tf2::getYaw(closest_pose.orientation)); TransformStamped geometry_tf{}; geometry_tf.transform = pose2transform(pose_2d); @@ -1417,14 +1418,14 @@ Polygon2d createEnvelopePolygon( tf2::doTransform( toMsg(envelope_poly, closest_pose.position.z), envelope_ros_polygon, geometry_tf); - const auto expanded_polygon = expandPolygon(toPolygon2d(envelope_ros_polygon), envelope_buffer); + const auto expanded_polygon = expand_polygon(toPolygon2d(envelope_ros_polygon), envelope_buffer); return expanded_polygon; } Polygon2d createEnvelopePolygon( const ObjectData & object_data, const Pose & closest_pose, const double envelope_buffer) { - const auto object_polygon = autoware::universe_utils::toPolygon2d(object_data.object); + const auto object_polygon = autoware_utils::to_polygon2d(object_data.object); return createEnvelopePolygon(object_polygon, closest_pose, envelope_buffer); } @@ -1455,8 +1456,7 @@ std::vector generateObstaclePolygonsForDrivableArea( // generate obstacle polygon const double diff_poly_buffer = object.avoid_margin.value() - object_parameter.envelope_buffer_margin - vehicle_width / 2.0; - const auto obj_poly = - autoware::universe_utils::expandPolygon(object.envelope_poly, diff_poly_buffer); + const auto obj_poly = autoware_utils::expand_polygon(object.envelope_poly, diff_poly_buffer); obstacles_for_drivable_area.push_back({object.getPose(), obj_poly, !isOnRight(object)}); } return obstacles_for_drivable_area; @@ -1541,7 +1541,7 @@ void insertDecelPoint( insertVelocity(path, velocity); - p_out = PoseWithDetail(getPose(path.points.at(insert_idx.value()))); + p_out = PoseWithDetail(autoware_utils::get_pose(path.points.at(insert_idx.value()))); } void fillObjectEnvelopePolygon( @@ -1603,7 +1603,7 @@ void fillObjectEnvelopePolygon( const auto multi_step_envelope_poly = createEnvelopePolygon(unions.front(), closest_pose, 0.0); - const auto object_polygon = autoware::universe_utils::toPolygon2d(object_data.object); + const auto object_polygon = autoware_utils::to_polygon2d(object_data.object); const auto object_polygon_area = boost::geometry::area(object_polygon); const auto envelope_polygon_area = boost::geometry::area(multi_step_envelope_poly); @@ -1773,7 +1773,7 @@ void compensateLostTargetObjects( const auto similar_pos_obj = std::find_if( data.target_objects.begin(), data.target_objects.end(), [&object](const auto & o) { constexpr auto POS_THR = 1.5; - return calcDistance2d(object.getPose(), o.getPose()) < POS_THR; + return calc_distance2d(object.getPose(), o.getPose()) < POS_THR; }); // same id object is not detected, but object is found around registered. update registered. @@ -2088,11 +2088,11 @@ AvoidLineArray combineRawShiftLinesWithUniqueCheck( { // TODO(Horibe) parametrize const auto isSimilar = [](const AvoidLine & a, const AvoidLine & b) { - using autoware::universe_utils::calcDistance2d; - if (calcDistance2d(a.start, b.start) > 1.0) { + using autoware_utils::calc_distance2d; + if (calc_distance2d(a.start, b.start) > 1.0) { return false; } - if (calcDistance2d(a.end, b.end) > 1.0) { + if (calc_distance2d(a.end, b.end) > 1.0) { return false; } if (std::abs(a.end_shift_length - b.end_shift_length) > 0.5) { @@ -2395,7 +2395,7 @@ std::pair separateObjectsByPath( const auto objects = planner_data->dynamic_object->objects; std::for_each(objects.begin(), objects.end(), [&](const auto & object) { - const auto obj_polygon = autoware::universe_utils::toPolygon2d(object); + const auto obj_polygon = autoware_utils::to_polygon2d(object); if (!within_detection_area(obj_polygon)) { other_objects.objects.push_back(object); } else { diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/test/test_utils.cpp b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/test/test_utils.cpp index ffccd2d64c189..83fc9e285b9e1 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/test/test_utils.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/test/test_utils.cpp @@ -17,8 +17,8 @@ #include "autoware/behavior_path_static_obstacle_avoidance_module/helper.hpp" #include "autoware/behavior_path_static_obstacle_avoidance_module/type_alias.hpp" #include "autoware/behavior_path_static_obstacle_avoidance_module/utils.hpp" -#include "autoware/universe_utils/math/unit_conversion.hpp" #include "autoware_test_utils/autoware_test_utils.hpp" +#include "autoware_utils/math/unit_conversion.hpp" #include #include @@ -36,11 +36,11 @@ using autoware::behavior_path_planner::AvoidanceParameters; using autoware::behavior_path_planner::ObjectData; using autoware::behavior_path_planner::helper::static_obstacle_avoidance::AvoidanceHelper; using autoware::route_handler::Direction; -using autoware::universe_utils::createPoint; -using autoware::universe_utils::createQuaternionFromRPY; -using autoware::universe_utils::createVector3; -using autoware::universe_utils::deg2rad; -using autoware::universe_utils::generateUUID; +using autoware_utils::create_point; +using autoware_utils::create_quaternion_from_rpy; +using autoware_utils::create_vector3; +using autoware_utils::deg2rad; +using autoware_utils::generate_uuid; constexpr double epsilon = 1e-6; @@ -63,9 +63,9 @@ auto get_planner_data() -> std::shared_ptr // set odometry. Odometry odometry; odometry.pose.pose = geometry_msgs::build() - .position(createPoint(0.0, 0.0, 0.0)) - .orientation(createQuaternionFromRPY(0.0, 0.0, 0.0)); - odometry.twist.twist.linear = createVector3(0.0, 0.0, 0.0); + .position(create_point(0.0, 0.0, 0.0)) + .orientation(create_quaternion_from_rpy(0.0, 0.0, 0.0)); + odometry.twist.twist.linear = create_vector3(0.0, 0.0, 0.0); planner_data.self_odometry = std::make_shared(odometry); @@ -189,8 +189,8 @@ TEST(TestUtils, getObjectBehavior) object_data.direction = Direction::LEFT; object_data.object.kinematics.initial_pose_with_covariance.pose = geometry_msgs::build() - .position(createPoint(0.0, 0.1, 0.0)) - .orientation(createQuaternionFromRPY(0.0, 0.0, 0.0)); + .position(create_point(0.0, 0.1, 0.0)) + .orientation(create_quaternion_from_rpy(0.0, 0.0, 0.0)); EXPECT_EQ( filtering_utils::getObjectBehavior(object_data, parameters), ObjectData::Behavior::NONE); @@ -202,8 +202,8 @@ TEST(TestUtils, getObjectBehavior) object_data.direction = Direction::RIGHT; object_data.object.kinematics.initial_pose_with_covariance.pose = geometry_msgs::build() - .position(createPoint(0.0, -0.1, 0.0)) - .orientation(createQuaternionFromRPY(0.0, 0.0, deg2rad(170))); + .position(create_point(0.0, -0.1, 0.0)) + .orientation(create_quaternion_from_rpy(0.0, 0.0, deg2rad(170))); EXPECT_EQ( filtering_utils::getObjectBehavior(object_data, parameters), ObjectData::Behavior::NONE); @@ -215,8 +215,8 @@ TEST(TestUtils, getObjectBehavior) object_data.direction = Direction::LEFT; object_data.object.kinematics.initial_pose_with_covariance.pose = geometry_msgs::build() - .position(createPoint(0.0, 0.1, 0.0)) - .orientation(createQuaternionFromRPY(0.0, 0.0, deg2rad(30))); + .position(create_point(0.0, 0.1, 0.0)) + .orientation(create_quaternion_from_rpy(0.0, 0.0, deg2rad(30))); EXPECT_EQ( filtering_utils::getObjectBehavior(object_data, parameters), ObjectData::Behavior::DEVIATING); @@ -228,8 +228,8 @@ TEST(TestUtils, getObjectBehavior) object_data.direction = Direction::RIGHT; object_data.object.kinematics.initial_pose_with_covariance.pose = geometry_msgs::build() - .position(createPoint(0.0, -0.1, 0.0)) - .orientation(createQuaternionFromRPY(0.0, 0.0, deg2rad(30))); + .position(create_point(0.0, -0.1, 0.0)) + .orientation(create_quaternion_from_rpy(0.0, 0.0, deg2rad(30))); EXPECT_EQ( filtering_utils::getObjectBehavior(object_data, parameters), ObjectData::Behavior::MERGING); @@ -241,8 +241,8 @@ TEST(TestUtils, getObjectBehavior) object_data.direction = Direction::LEFT; object_data.object.kinematics.initial_pose_with_covariance.pose = geometry_msgs::build() - .position(createPoint(0.0, 0.1, 0.0)) - .orientation(createQuaternionFromRPY(0.0, 0.0, deg2rad(-150))); + .position(create_point(0.0, 0.1, 0.0)) + .orientation(create_quaternion_from_rpy(0.0, 0.0, deg2rad(-150))); EXPECT_EQ( filtering_utils::getObjectBehavior(object_data, parameters), ObjectData::Behavior::DEVIATING); @@ -254,8 +254,8 @@ TEST(TestUtils, getObjectBehavior) object_data.direction = Direction::RIGHT; object_data.object.kinematics.initial_pose_with_covariance.pose = geometry_msgs::build() - .position(createPoint(0.0, -0.1, 0.0)) - .orientation(createQuaternionFromRPY(0.0, 0.0, deg2rad(-150))); + .position(create_point(0.0, -0.1, 0.0)) + .orientation(create_quaternion_from_rpy(0.0, 0.0, deg2rad(-150))); EXPECT_EQ( filtering_utils::getObjectBehavior(object_data, parameters), ObjectData::Behavior::MERGING); @@ -267,8 +267,8 @@ TEST(TestUtils, getObjectBehavior) object_data.direction = Direction::LEFT; object_data.object.kinematics.initial_pose_with_covariance.pose = geometry_msgs::build() - .position(createPoint(0.0, 0.1, 0.0)) - .orientation(createQuaternionFromRPY(0.0, 0.0, deg2rad(-30))); + .position(create_point(0.0, 0.1, 0.0)) + .orientation(create_quaternion_from_rpy(0.0, 0.0, deg2rad(-30))); EXPECT_EQ( filtering_utils::getObjectBehavior(object_data, parameters), ObjectData::Behavior::MERGING); @@ -280,8 +280,8 @@ TEST(TestUtils, getObjectBehavior) object_data.direction = Direction::RIGHT; object_data.object.kinematics.initial_pose_with_covariance.pose = geometry_msgs::build() - .position(createPoint(0.0, -0.1, 0.0)) - .orientation(createQuaternionFromRPY(0.0, 0.0, deg2rad(-30))); + .position(create_point(0.0, -0.1, 0.0)) + .orientation(create_quaternion_from_rpy(0.0, 0.0, deg2rad(-30))); EXPECT_EQ( filtering_utils::getObjectBehavior(object_data, parameters), ObjectData::Behavior::DEVIATING); @@ -293,8 +293,8 @@ TEST(TestUtils, getObjectBehavior) object_data.direction = Direction::LEFT; object_data.object.kinematics.initial_pose_with_covariance.pose = geometry_msgs::build() - .position(createPoint(0.0, 0.1, 0.0)) - .orientation(createQuaternionFromRPY(0.0, 0.0, deg2rad(150))); + .position(create_point(0.0, 0.1, 0.0)) + .orientation(create_quaternion_from_rpy(0.0, 0.0, deg2rad(150))); EXPECT_EQ( filtering_utils::getObjectBehavior(object_data, parameters), ObjectData::Behavior::MERGING); @@ -306,8 +306,8 @@ TEST(TestUtils, getObjectBehavior) object_data.direction = Direction::RIGHT; object_data.object.kinematics.initial_pose_with_covariance.pose = geometry_msgs::build() - .position(createPoint(0.0, -0.1, 0.0)) - .orientation(createQuaternionFromRPY(0.0, 0.0, deg2rad(150))); + .position(create_point(0.0, -0.1, 0.0)) + .orientation(create_quaternion_from_rpy(0.0, 0.0, deg2rad(150))); EXPECT_EQ( filtering_utils::getObjectBehavior(object_data, parameters), ObjectData::Behavior::DEVIATING); @@ -322,8 +322,8 @@ TEST(TestUtils, isNoNeedAvoidanceBehavior) 50L, 1.0, 20.0, 0.0, 0.0, std::numeric_limits::max()); const auto object_pose = geometry_msgs::build() - .position(createPoint(0.0, 0.0, 0.0)) - .orientation(createQuaternionFromRPY(0.0, 0.0, deg2rad(45))); + .position(create_point(0.0, 0.0, 0.0)) + .orientation(create_quaternion_from_rpy(0.0, 0.0, deg2rad(45))); const auto nearest_path_pose = path.points.at(autoware::motion_utils::findNearestIndex(path.points, object_pose.position)) @@ -332,10 +332,10 @@ TEST(TestUtils, isNoNeedAvoidanceBehavior) const auto shape = autoware_perception_msgs::build() .type(Shape::BOUNDING_BOX) .footprint(geometry_msgs::msg::Polygon{}) - .dimensions(createVector3(2.8284271247461901, 1.41421356237309505, 2.0)); + .dimensions(create_vector3(2.8284271247461901, 1.41421356237309505, 2.0)); - auto object_data = - create_test_object(object_pose, createVector3(0.0, 0.0, 0.0), shape, ObjectClassification::CAR); + auto object_data = create_test_object( + object_pose, create_vector3(0.0, 0.0, 0.0), shape, ObjectClassification::CAR); // object is NOT avoidable. but there is possibility that the ego has to avoid it. { @@ -348,8 +348,8 @@ TEST(TestUtils, isNoNeedAvoidanceBehavior) { object_data.object.kinematics.initial_pose_with_covariance.pose = geometry_msgs::build() - .position(createPoint(2.5, 3.6, 0.0)) - .orientation(createQuaternionFromRPY(0.0, 0.0, deg2rad(45))); + .position(create_point(2.5, 3.6, 0.0)) + .orientation(create_quaternion_from_rpy(0.0, 0.0, deg2rad(45))); object_data.direction = Direction::LEFT; object_data.avoid_margin = 2.0; object_data.envelope_poly = createEnvelopePolygon(object_data, nearest_path_pose, 0.0); @@ -363,8 +363,8 @@ TEST(TestUtils, isNoNeedAvoidanceBehavior) { object_data.object.kinematics.initial_pose_with_covariance.pose = geometry_msgs::build() - .position(createPoint(2.5, 3.4, 0.0)) - .orientation(createQuaternionFromRPY(0.0, 0.0, deg2rad(45))); + .position(create_point(2.5, 3.4, 0.0)) + .orientation(create_quaternion_from_rpy(0.0, 0.0, deg2rad(45))); object_data.direction = Direction::LEFT; object_data.avoid_margin = 2.0; object_data.envelope_poly = createEnvelopePolygon(object_data, nearest_path_pose, 0.0); @@ -378,8 +378,8 @@ TEST(TestUtils, isNoNeedAvoidanceBehavior) { object_data.object.kinematics.initial_pose_with_covariance.pose = geometry_msgs::build() - .position(createPoint(2.5, 2.9, 0.0)) - .orientation(createQuaternionFromRPY(0.0, 0.0, deg2rad(45))); + .position(create_point(2.5, 2.9, 0.0)) + .orientation(create_quaternion_from_rpy(0.0, 0.0, deg2rad(45))); object_data.direction = Direction::LEFT; object_data.avoid_margin = 2.0; object_data.envelope_poly = createEnvelopePolygon(object_data, nearest_path_pose, 0.0); @@ -518,7 +518,7 @@ TEST(TestUtils, isSatisfiedWithCommonCondition) .probability(1.0)); EXPECT_FALSE(filtering_utils::isSatisfiedWithCommonCondition( - object_data, path, forward_detection_range, 4.0, createPoint(0.0, 0.0, 0.0), false, + object_data, path, forward_detection_range, 4.0, create_point(0.0, 0.0, 0.0), false, parameters)); EXPECT_EQ(object_data.info, ObjectInfo::IS_NOT_TARGET_OBJECT); } @@ -532,7 +532,7 @@ TEST(TestUtils, isSatisfiedWithCommonCondition) .probability(1.0)); EXPECT_FALSE(filtering_utils::isSatisfiedWithCommonCondition( - object_data, path, forward_detection_range, 4.0, createPoint(0.0, 0.0, 0.0), false, + object_data, path, forward_detection_range, 4.0, create_point(0.0, 0.0, 0.0), false, parameters)); EXPECT_EQ(object_data.info, ObjectInfo::IS_NOT_TARGET_OBJECT); } @@ -547,7 +547,7 @@ TEST(TestUtils, isSatisfiedWithCommonCondition) object_data.move_time = 0.6; EXPECT_FALSE(filtering_utils::isSatisfiedWithCommonCondition( - object_data, path, forward_detection_range, 4.0, createPoint(0.0, 0.0, 0.0), false, + object_data, path, forward_detection_range, 4.0, create_point(0.0, 0.0, 0.0), false, parameters)); EXPECT_EQ(object_data.info, ObjectInfo::MOVING_OBJECT); } @@ -555,8 +555,8 @@ TEST(TestUtils, isSatisfiedWithCommonCondition) // object behind the ego. { const auto object_pose = geometry_msgs::build() - .position(createPoint(2.5, 1.0, 0.0)) - .orientation(createQuaternionFromRPY(0.0, 0.0, deg2rad(45))); + .position(create_point(2.5, 1.0, 0.0)) + .orientation(create_quaternion_from_rpy(0.0, 0.0, deg2rad(45))); ObjectData object_data; object_data.object.shape.type = autoware_perception_msgs::msg::Shape::BOUNDING_BOX; @@ -578,7 +578,7 @@ TEST(TestUtils, isSatisfiedWithCommonCondition) object_data.envelope_poly = createEnvelopePolygon(object_data, pose, margin); EXPECT_FALSE(filtering_utils::isSatisfiedWithCommonCondition( - object_data, path, forward_detection_range, 4.0, createPoint(8.0, 0.5, 0.0), false, + object_data, path, forward_detection_range, 4.0, create_point(8.0, 0.5, 0.0), false, parameters)); EXPECT_EQ(object_data.info, ObjectInfo::FURTHER_THAN_THRESHOLD); } @@ -586,8 +586,8 @@ TEST(TestUtils, isSatisfiedWithCommonCondition) // farther than detection range. { const auto object_pose = geometry_msgs::build() - .position(createPoint(7.5, 1.0, 0.0)) - .orientation(createQuaternionFromRPY(0.0, 0.0, deg2rad(45))); + .position(create_point(7.5, 1.0, 0.0)) + .orientation(create_quaternion_from_rpy(0.0, 0.0, deg2rad(45))); ObjectData object_data; object_data.object.shape.type = autoware_perception_msgs::msg::Shape::BOUNDING_BOX; @@ -609,7 +609,7 @@ TEST(TestUtils, isSatisfiedWithCommonCondition) object_data.envelope_poly = createEnvelopePolygon(object_data, pose, margin); EXPECT_FALSE(filtering_utils::isSatisfiedWithCommonCondition( - object_data, path, forward_detection_range, 4.0, createPoint(0.0, 0.0, 0.0), false, + object_data, path, forward_detection_range, 4.0, create_point(0.0, 0.0, 0.0), false, parameters)); EXPECT_EQ(object_data.info, ObjectInfo::FURTHER_THAN_THRESHOLD); } @@ -617,8 +617,8 @@ TEST(TestUtils, isSatisfiedWithCommonCondition) // farther than goal position. { const auto object_pose = geometry_msgs::build() - .position(createPoint(7.0, 1.0, 0.0)) - .orientation(createQuaternionFromRPY(0.0, 0.0, deg2rad(45))); + .position(create_point(7.0, 1.0, 0.0)) + .orientation(create_quaternion_from_rpy(0.0, 0.0, deg2rad(45))); ObjectData object_data; object_data.object.shape.type = autoware_perception_msgs::msg::Shape::BOUNDING_BOX; @@ -640,7 +640,7 @@ TEST(TestUtils, isSatisfiedWithCommonCondition) object_data.envelope_poly = createEnvelopePolygon(object_data, pose, margin); EXPECT_FALSE(filtering_utils::isSatisfiedWithCommonCondition( - object_data, path, forward_detection_range, 4.0, createPoint(0.0, 0.0, 0.0), false, + object_data, path, forward_detection_range, 4.0, create_point(0.0, 0.0, 0.0), false, parameters)); EXPECT_EQ(object_data.info, ObjectInfo::FURTHER_THAN_GOAL); } @@ -648,8 +648,8 @@ TEST(TestUtils, isSatisfiedWithCommonCondition) // within detection range. { const auto object_pose = geometry_msgs::build() - .position(createPoint(4.5, 1.0, 0.0)) - .orientation(createQuaternionFromRPY(0.0, 0.0, deg2rad(45))); + .position(create_point(4.5, 1.0, 0.0)) + .orientation(create_quaternion_from_rpy(0.0, 0.0, deg2rad(45))); ObjectData object_data; object_data.object.shape.type = autoware_perception_msgs::msg::Shape::BOUNDING_BOX; @@ -671,7 +671,7 @@ TEST(TestUtils, isSatisfiedWithCommonCondition) object_data.envelope_poly = createEnvelopePolygon(object_data, pose, margin); EXPECT_FALSE(filtering_utils::isSatisfiedWithCommonCondition( - object_data, path, forward_detection_range, 6.4, createPoint(0.0, 0.0, 0.0), false, + object_data, path, forward_detection_range, 6.4, create_point(0.0, 0.0, 0.0), false, parameters)); EXPECT_EQ(object_data.info, ObjectInfo::TOO_NEAR_TO_GOAL); } @@ -679,8 +679,8 @@ TEST(TestUtils, isSatisfiedWithCommonCondition) // within detection range. { const auto object_pose = geometry_msgs::build() - .position(createPoint(4.5, 1.0, 0.0)) - .orientation(createQuaternionFromRPY(0.0, 0.0, deg2rad(45))); + .position(create_point(4.5, 1.0, 0.0)) + .orientation(create_quaternion_from_rpy(0.0, 0.0, deg2rad(45))); ObjectData object_data; object_data.object.shape.type = autoware_perception_msgs::msg::Shape::BOUNDING_BOX; @@ -702,15 +702,15 @@ TEST(TestUtils, isSatisfiedWithCommonCondition) object_data.envelope_poly = createEnvelopePolygon(object_data, pose, margin); EXPECT_TRUE(filtering_utils::isSatisfiedWithCommonCondition( - object_data, path, forward_detection_range, 6.6, createPoint(0.0, 0.0, 0.0), false, + object_data, path, forward_detection_range, 6.6, create_point(0.0, 0.0, 0.0), false, parameters)); } // within detection range. { const auto object_pose = geometry_msgs::build() - .position(createPoint(4.5, 1.0, 0.0)) - .orientation(createQuaternionFromRPY(0.0, 0.0, deg2rad(45))); + .position(create_point(4.5, 1.0, 0.0)) + .orientation(create_quaternion_from_rpy(0.0, 0.0, deg2rad(45))); ObjectData object_data; object_data.object.shape.type = autoware_perception_msgs::msg::Shape::BOUNDING_BOX; @@ -732,7 +732,7 @@ TEST(TestUtils, isSatisfiedWithCommonCondition) object_data.envelope_poly = createEnvelopePolygon(object_data, pose, margin); EXPECT_TRUE(filtering_utils::isSatisfiedWithCommonCondition( - object_data, path, forward_detection_range, 4.0, createPoint(0.0, 0.0, 0.0), true, + object_data, path, forward_detection_range, 4.0, create_point(0.0, 0.0, 0.0), true, parameters)); } } @@ -819,7 +819,7 @@ TEST(TestUtils, insertDecelPoint) auto path = autoware::test_utils::generateTrajectory( 50L, 1.0, 20.0, 0.0, 0.0, std::numeric_limits::max()); - const auto ego_position = createPoint(2.5, 0.5, 0.0); + const auto ego_position = create_point(2.5, 0.5, 0.0); constexpr double offset = 100.0; constexpr double velocity = 1.0; @@ -837,7 +837,7 @@ TEST(TestUtils, insertDecelPoint) auto path = autoware::test_utils::generateTrajectory( 50L, 1.0, 20.0, 0.0, 0.0, std::numeric_limits::max()); - const auto ego_position = createPoint(3.5, 0.5, 0.0); + const auto ego_position = create_point(3.5, 0.5, 0.0); constexpr double offset = 3.0; constexpr double velocity = 1.0; @@ -861,18 +861,18 @@ TEST(TestUtils, DISABLED_fillObjectMovingTime) const auto parameters = get_parameters(); const auto object_pose = geometry_msgs::build() - .position(createPoint(0.0, 0.0, 0.0)) - .orientation(createQuaternionFromRPY(0.0, 0.0, deg2rad(45))); + .position(create_point(0.0, 0.0, 0.0)) + .orientation(create_quaternion_from_rpy(0.0, 0.0, deg2rad(45))); const auto shape = autoware_perception_msgs::build() .type(Shape::BOUNDING_BOX) .footprint(geometry_msgs::msg::Polygon{}) - .dimensions(createVector3(2.0, 1.0, 2.0)); + .dimensions(create_vector3(2.0, 1.0, 2.0)); - const auto uuid = generateUUID(); + const auto uuid = generate_uuid(); auto old_object = create_test_object( - object_pose, createVector3(0.0, 0.0, 0.0), shape, ObjectClassification::TRUCK); + object_pose, create_vector3(0.0, 0.0, 0.0), shape, ObjectClassification::TRUCK); // set previous object data. { @@ -881,19 +881,19 @@ TEST(TestUtils, DISABLED_fillObjectMovingTime) old_object.last_stop = rclcpp::Clock{RCL_ROS_TIME}.now(); old_object.move_time = 0.0; old_object.object.kinematics.initial_twist_with_covariance.twist.linear = - createVector3(0.0, 0.0, 0.0); + create_vector3(0.0, 0.0, 0.0); } rclcpp::sleep_for(490ms); auto new_object = create_test_object( - object_pose, createVector3(0.0, 0.0, 0.0), shape, ObjectClassification::TRUCK); + object_pose, create_vector3(0.0, 0.0, 0.0), shape, ObjectClassification::TRUCK); // find new stop object { - new_object.object.object_id = generateUUID(); + new_object.object.object_id = generate_uuid(); new_object.object.kinematics.initial_twist_with_covariance.twist.linear = - createVector3(0.99, 0.0, 0.0); + create_vector3(0.99, 0.0, 0.0); ObjectDataArray buffer{}; fillObjectMovingTime(new_object, buffer, parameters); @@ -904,9 +904,9 @@ TEST(TestUtils, DISABLED_fillObjectMovingTime) // find new move object { - new_object.object.object_id = generateUUID(); + new_object.object.object_id = generate_uuid(); new_object.object.kinematics.initial_twist_with_covariance.twist.linear = - createVector3(1.01, 0.0, 0.0); + create_vector3(1.01, 0.0, 0.0); ObjectDataArray buffer{}; fillObjectMovingTime(new_object, buffer, parameters); @@ -918,7 +918,7 @@ TEST(TestUtils, DISABLED_fillObjectMovingTime) { new_object.object.object_id = uuid; new_object.object.kinematics.initial_twist_with_covariance.twist.linear = - createVector3(1.01, 0.0, 0.0); + create_vector3(1.01, 0.0, 0.0); ObjectDataArray buffer{old_object}; fillObjectMovingTime(new_object, buffer, parameters); @@ -932,7 +932,7 @@ TEST(TestUtils, DISABLED_fillObjectMovingTime) { new_object.object.object_id = uuid; new_object.object.kinematics.initial_twist_with_covariance.twist.linear = - createVector3(0.99, 0.0, 0.0); + create_vector3(0.99, 0.0, 0.0); ObjectDataArray buffer{old_object}; fillObjectMovingTime(new_object, buffer, parameters); @@ -948,7 +948,7 @@ TEST(TestUtils, DISABLED_fillObjectMovingTime) { new_object.object.object_id = uuid; new_object.object.kinematics.initial_twist_with_covariance.twist.linear = - createVector3(1.01, 0.0, 0.0); + create_vector3(1.01, 0.0, 0.0); ObjectDataArray buffer{old_object}; fillObjectMovingTime(new_object, buffer, parameters); @@ -965,8 +965,8 @@ TEST(TestUtils, calcEnvelopeOverhangDistance) 50L, 1.0, 20.0, 0.0, 0.0, std::numeric_limits::max()); const auto object_pose = geometry_msgs::build() - .position(createPoint(0.0, 0.0, 0.0)) - .orientation(createQuaternionFromRPY(0.0, 0.0, deg2rad(45))); + .position(create_point(0.0, 0.0, 0.0)) + .orientation(create_quaternion_from_rpy(0.0, 0.0, deg2rad(45))); const auto nearest_path_pose = path.points.at(autoware::motion_utils::findNearestIndex(path.points, object_pose.position)) @@ -975,16 +975,16 @@ TEST(TestUtils, calcEnvelopeOverhangDistance) const auto shape = autoware_perception_msgs::build() .type(Shape::BOUNDING_BOX) .footprint(geometry_msgs::msg::Polygon{}) - .dimensions(createVector3(2.8284271247461901, 1.41421356237309505, 2.0)); + .dimensions(create_vector3(2.8284271247461901, 1.41421356237309505, 2.0)); auto object_data = create_test_object( - object_pose, createVector3(0.0, 0.0, 0.0), shape, ObjectClassification::TRUCK); + object_pose, create_vector3(0.0, 0.0, 0.0), shape, ObjectClassification::TRUCK); { object_data.object.kinematics.initial_pose_with_covariance.pose = geometry_msgs::build() - .position(createPoint(2.5, 1.0, 0.0)) - .orientation(createQuaternionFromRPY(0.0, 0.0, deg2rad(45))); + .position(create_point(2.5, 1.0, 0.0)) + .orientation(create_quaternion_from_rpy(0.0, 0.0, deg2rad(45))); object_data.direction = Direction::LEFT; object_data.envelope_poly = createEnvelopePolygon(object_data, nearest_path_pose, 0.0); @@ -1001,8 +1001,8 @@ TEST(TestUtils, calcEnvelopeOverhangDistance) { object_data.object.kinematics.initial_pose_with_covariance.pose = geometry_msgs::build() - .position(createPoint(2.5, -1.0, 0.0)) - .orientation(createQuaternionFromRPY(0.0, 0.0, deg2rad(45))); + .position(create_point(2.5, -1.0, 0.0)) + .orientation(create_quaternion_from_rpy(0.0, 0.0, deg2rad(45))); object_data.direction = Direction::RIGHT; object_data.envelope_poly = createEnvelopePolygon(object_data, nearest_path_pose, 0.0); @@ -1025,8 +1025,8 @@ TEST(TestUtils, createEnvelopePolygon) constexpr double margin = 0.353553390593273762; const auto pose = geometry_msgs::build() - .position(createPoint(0.0, 0.0, 0.0)) - .orientation(createQuaternionFromRPY(0.0, 0.0, deg2rad(45))); + .position(create_point(0.0, 0.0, 0.0)) + .orientation(create_quaternion_from_rpy(0.0, 0.0, deg2rad(45))); const auto output = createEnvelopePolygon(footprint, pose, margin); @@ -1063,16 +1063,16 @@ TEST(TestUtils, generateObstaclePolygonsForDrivableArea) ObjectClassification::TRUCK, create_params(envelope_buffer, 0.5, lateral_soft_margin)); const auto object_pose = geometry_msgs::build() - .position(createPoint(0.0, 0.0, 0.0)) - .orientation(createQuaternionFromRPY(0.0, 0.0, deg2rad(45))); + .position(create_point(0.0, 0.0, 0.0)) + .orientation(create_quaternion_from_rpy(0.0, 0.0, deg2rad(45))); const auto shape = autoware_perception_msgs::build() .type(Shape::BOUNDING_BOX) .footprint(geometry_msgs::msg::Polygon{}) - .dimensions(createVector3(2.0, 1.0, 2.0)); + .dimensions(create_vector3(2.0, 1.0, 2.0)); auto object_data = create_test_object( - object_pose, createVector3(0.0, 0.0, 0.0), shape, ObjectClassification::TRUCK); + object_pose, create_vector3(0.0, 0.0, 0.0), shape, ObjectClassification::TRUCK); // empty { @@ -1151,8 +1151,8 @@ TEST(TestUtils, fillLongitudinalAndLengthByClosestEnvelopeFootprint) 50L, 1.0, 20.0, 0.0, 0.0, std::numeric_limits::max()); const auto object_pose = geometry_msgs::build() - .position(createPoint(2.5, 1.0, 0.0)) - .orientation(createQuaternionFromRPY(0.0, 0.0, deg2rad(45))); + .position(create_point(2.5, 1.0, 0.0)) + .orientation(create_quaternion_from_rpy(0.0, 0.0, deg2rad(45))); const auto nearest_path_pose = path.points.at(autoware::motion_utils::findNearestIndex(path.points, object_pose.position)) @@ -1161,15 +1161,15 @@ TEST(TestUtils, fillLongitudinalAndLengthByClosestEnvelopeFootprint) const auto shape = autoware_perception_msgs::build() .type(Shape::BOUNDING_BOX) .footprint(geometry_msgs::msg::Polygon{}) - .dimensions(createVector3(2.8284271247461901, 1.41421356237309505, 2.0)); + .dimensions(create_vector3(2.8284271247461901, 1.41421356237309505, 2.0)); auto object_data = create_test_object( - object_pose, createVector3(0.0, 0.0, 0.0), shape, ObjectClassification::TRUCK); + object_pose, create_vector3(0.0, 0.0, 0.0), shape, ObjectClassification::TRUCK); object_data.direction = Direction::LEFT; object_data.envelope_poly = createEnvelopePolygon(object_data, nearest_path_pose, 0.0); fillLongitudinalAndLengthByClosestEnvelopeFootprint( - path, createPoint(0.0, 0.0, 0.0), object_data); + path, create_point(0.0, 0.0, 0.0), object_data); EXPECT_NEAR(object_data.longitudinal, 1.0, epsilon); EXPECT_NEAR(object_data.length, 3.0, epsilon); } @@ -1181,11 +1181,11 @@ TEST(TestUtils, fillObjectEnvelopePolygon) const auto parameters = get_parameters(); - const auto uuid = generateUUID(); + const auto uuid = generate_uuid(); const auto object_pose = geometry_msgs::build() - .position(createPoint(2.5, 1.0, 0.0)) - .orientation(createQuaternionFromRPY(0.0, 0.0, deg2rad(45))); + .position(create_point(2.5, 1.0, 0.0)) + .orientation(create_quaternion_from_rpy(0.0, 0.0, deg2rad(45))); const auto nearest_path_pose = path.points.at(autoware::motion_utils::findNearestIndex(path.points, object_pose.position)) @@ -1194,10 +1194,10 @@ TEST(TestUtils, fillObjectEnvelopePolygon) const auto shape = autoware_perception_msgs::build() .type(Shape::BOUNDING_BOX) .footprint(geometry_msgs::msg::Polygon{}) - .dimensions(createVector3(2.8284271247461901, 1.41421356237309505, 2.0)); + .dimensions(create_vector3(2.8284271247461901, 1.41421356237309505, 2.0)); auto stored_object = create_test_object( - object_pose, createVector3(0.0, 0.0, 0.0), shape, ObjectClassification::TRUCK); + object_pose, create_vector3(0.0, 0.0, 0.0), shape, ObjectClassification::TRUCK); stored_object.object.object_id = uuid; stored_object.envelope_poly = createEnvelopePolygon(stored_object, nearest_path_pose, 0.0); stored_object.object.kinematics.initial_pose_with_covariance = @@ -1211,13 +1211,13 @@ TEST(TestUtils, fillObjectEnvelopePolygon) stored_object.direction = Direction::LEFT; auto object_data = create_test_object( - object_pose, createVector3(0.0, 0.0, 0.0), shape, ObjectClassification::TRUCK); + object_pose, create_vector3(0.0, 0.0, 0.0), shape, ObjectClassification::TRUCK); // new object. { ObjectDataArray stored_objects{}; - object_data.object.object_id = generateUUID(); + object_data.object.object_id = generate_uuid(); object_data.object.kinematics.initial_pose_with_covariance = geometry_msgs::build() .pose(object_pose) @@ -1245,8 +1245,8 @@ TEST(TestUtils, fillObjectEnvelopePolygon) ObjectDataArray stored_objects{stored_object}; const auto new_pose = geometry_msgs::build() - .position(createPoint(3.0, 0.5, 0.0)) - .orientation(createQuaternionFromRPY(0.0, 0.0, deg2rad(45))); + .position(create_point(3.0, 0.5, 0.0)) + .orientation(create_quaternion_from_rpy(0.0, 0.0, deg2rad(45))); object_data.object.object_id = uuid; object_data.object.kinematics.initial_pose_with_covariance = @@ -1275,8 +1275,8 @@ TEST(TestUtils, fillObjectEnvelopePolygon) ObjectDataArray stored_objects{stored_object}; const auto new_pose = geometry_msgs::build() - .position(createPoint(3.0, 0.5, 0.0)) - .orientation(createQuaternionFromRPY(0.0, 0.0, deg2rad(45))); + .position(create_point(3.0, 0.5, 0.0)) + .orientation(create_quaternion_from_rpy(0.0, 0.0, deg2rad(45))); object_data.object.object_id = uuid; object_data.object.kinematics.initial_pose_with_covariance = @@ -1303,7 +1303,7 @@ TEST(TestUtils, fillObjectEnvelopePolygon) // threshold. { auto huge_covariance_object = create_test_object( - object_pose, createVector3(0.0, 0.0, 0.0), shape, ObjectClassification::TRUCK); + object_pose, create_vector3(0.0, 0.0, 0.0), shape, ObjectClassification::TRUCK); huge_covariance_object.object.object_id = uuid; huge_covariance_object.object.kinematics.initial_pose_with_covariance = geometry_msgs::build() @@ -1321,8 +1321,8 @@ TEST(TestUtils, fillObjectEnvelopePolygon) ObjectDataArray stored_objects{huge_covariance_object}; const auto new_pose = geometry_msgs::build() - .position(createPoint(3.0, 0.5, 0.0)) - .orientation(createQuaternionFromRPY(0.0, 0.0, deg2rad(45))); + .position(create_point(3.0, 0.5, 0.0)) + .orientation(create_quaternion_from_rpy(0.0, 0.0, deg2rad(45))); object_data.object.object_id = uuid; object_data.object.kinematics.initial_pose_with_covariance = @@ -1381,7 +1381,7 @@ TEST(TestUtils, compensateLostTargetObjects) const auto planner_data = get_planner_data(); - const auto uuid = generateUUID(); + const auto uuid = generate_uuid(); const auto init_time = rclcpp::Clock{RCL_ROS_TIME}.now(); @@ -1390,8 +1390,8 @@ TEST(TestUtils, compensateLostTargetObjects) stored_object.last_seen = init_time; stored_object.object.kinematics.initial_pose_with_covariance.pose = geometry_msgs::build() - .position(createPoint(1.0, 1.0, 0.0)) - .orientation(createQuaternionFromRPY(0.0, 0.0, 0.0)); + .position(create_point(1.0, 1.0, 0.0)) + .orientation(create_quaternion_from_rpy(0.0, 0.0, 0.0)); rclcpp::sleep_for(100ms); @@ -1402,12 +1402,12 @@ TEST(TestUtils, compensateLostTargetObjects) ObjectDataArray stored_objects{}; ObjectData new_object; - new_object.object.object_id = generateUUID(); + new_object.object.object_id = generate_uuid(); new_object.last_seen = now; new_object.object.kinematics.initial_pose_with_covariance.pose = geometry_msgs::build() - .position(createPoint(2.0, 5.0, 0.0)) - .orientation(createQuaternionFromRPY(0.0, 0.0, 0.0)); + .position(create_point(2.0, 5.0, 0.0)) + .orientation(create_quaternion_from_rpy(0.0, 0.0, 0.0)); AvoidancePlanningData avoidance_planning_data; avoidance_planning_data.target_objects = {new_object}; @@ -1448,8 +1448,8 @@ TEST(TestUtils, compensateLostTargetObjects) detected_object.last_seen = now; detected_object.object.kinematics.initial_pose_with_covariance.pose = geometry_msgs::build() - .position(createPoint(1.0, 1.0, 0.0)) - .orientation(createQuaternionFromRPY(0.0, 0.0, 0.0)); + .position(create_point(1.0, 1.0, 0.0)) + .orientation(create_quaternion_from_rpy(0.0, 0.0, 0.0)); AvoidancePlanningData avoidance_planning_data; avoidance_planning_data.target_objects = {detected_object}; @@ -1468,12 +1468,12 @@ TEST(TestUtils, compensateLostTargetObjects) ObjectDataArray stored_objects{stored_object}; ObjectData detected_object; - detected_object.object.object_id = generateUUID(); + detected_object.object.object_id = generate_uuid(); detected_object.last_seen = now; detected_object.object.kinematics.initial_pose_with_covariance.pose = geometry_msgs::build() - .position(createPoint(1.1, 1.1, 0.0)) - .orientation(createQuaternionFromRPY(0.0, 0.0, 0.0)); + .position(create_point(1.1, 1.1, 0.0)) + .orientation(create_quaternion_from_rpy(0.0, 0.0, 0.0)); AvoidancePlanningData avoidance_planning_data; avoidance_planning_data.target_objects = {detected_object}; @@ -1492,12 +1492,12 @@ TEST(TestUtils, compensateLostTargetObjects) ObjectDataArray stored_objects{stored_object}; ObjectData detected_object; - detected_object.object.object_id = generateUUID(); + detected_object.object.object_id = generate_uuid(); detected_object.last_seen = now; detected_object.object.kinematics.initial_pose_with_covariance.pose = geometry_msgs::build() - .position(createPoint(3.0, 3.0, 0.0)) - .orientation(createQuaternionFromRPY(0.0, 0.0, 0.0)); + .position(create_point(3.0, 3.0, 0.0)) + .orientation(create_quaternion_from_rpy(0.0, 0.0, 0.0)); AvoidancePlanningData avoidance_planning_data; avoidance_planning_data.target_objects = {detected_object}; @@ -1530,8 +1530,8 @@ TEST(TestUtils, compensateLostTargetObjects) TEST(TestUtils, calcErrorEclipseLongRadius) { const auto pose = geometry_msgs::build() - .position(createPoint(3.0, 3.0, 0.0)) - .orientation(createQuaternionFromRPY(0.0, 0.0, 0.0)); + .position(create_point(3.0, 3.0, 0.0)) + .orientation(create_quaternion_from_rpy(0.0, 0.0, 0.0)); const auto pose_with_covariance = geometry_msgs::build().pose(pose).covariance( {5.0, 4.0, 0.0, 0.0, 0.0, 0.0, 4.0, 5.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/include/autoware/behavior_velocity_blind_spot_module/scene.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/include/autoware/behavior_velocity_blind_spot_module/scene.hpp index b22b5aeca0842..5c201de9ab88d 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/include/autoware/behavior_velocity_blind_spot_module/scene.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/include/autoware/behavior_velocity_blind_spot_module/scene.hpp @@ -75,7 +75,7 @@ class BlindSpotModule : public SceneModuleInterfaceWithRTC const int64_t module_id, const int64_t lane_id, const TurnDirection turn_direction, const std::shared_ptr planner_data, const PlannerParam & planner_param, const rclcpp::Logger logger, const rclcpp::Clock::SharedPtr clock, - const std::shared_ptr time_keeper, + const std::shared_ptr time_keeper, const std::shared_ptr planning_factor_interface); diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/include/autoware/behavior_velocity_blind_spot_module/util.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/include/autoware/behavior_velocity_blind_spot_module/util.hpp index 4931c94b1931f..7ad2e09a68920 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/include/autoware/behavior_velocity_blind_spot_module/util.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/include/autoware/behavior_velocity_blind_spot_module/util.hpp @@ -16,7 +16,7 @@ #define AUTOWARE__BEHAVIOR_VELOCITY_BLIND_SPOT_MODULE__UTIL_HPP_ #include -#include +#include #include @@ -54,7 +54,7 @@ std::optional generateInterpolatedPathInfo( std::optional getFirstPointIntersectsLineByFootprint( const lanelet::ConstLineString2d & line, const InterpolatedPathInfo & interpolated_path_info, - const autoware::universe_utils::LinearRing2d & footprint, const double vehicle_length); + const autoware_utils::LinearRing2d & footprint, const double vehicle_length); std::optional getSiblingStraightLanelet( const lanelet::Lanelet assigned_lane, diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/package.xml b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/package.xml index 8c2f96728abc1..e1a4235052cb1 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/package.xml +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/package.xml @@ -26,7 +26,7 @@ autoware_planning_msgs autoware_route_handler autoware_test_utils - autoware_universe_utils + autoware_utils geometry_msgs pluginlib rclcpp diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/debug.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/debug.cpp index 2a6f4825ba414..7f7b94fcd99ea 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/debug.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/debug.cpp @@ -17,7 +17,7 @@ #include #include #include -#include +#include #include @@ -28,10 +28,10 @@ namespace autoware::behavior_velocity_planner { -using autoware::universe_utils::appendMarkerArray; -using autoware::universe_utils::createMarkerColor; -using autoware::universe_utils::createMarkerOrientation; -using autoware::universe_utils::createMarkerScale; +using autoware_utils::append_marker_array; +using autoware_utils::create_marker_color; +using autoware_utils::create_marker_orientation; +using autoware_utils::create_marker_scale; namespace { @@ -53,9 +53,9 @@ visualization_msgs::msg::MarkerArray createLaneletPolygonsMarkerArray( marker.lifetime = rclcpp::Duration::from_seconds(0.3); marker.type = visualization_msgs::msg::Marker::LINE_STRIP; marker.action = visualization_msgs::msg::Marker::ADD; - marker.pose.orientation = createMarkerOrientation(0, 0, 0, 1.0); - marker.scale = createMarkerScale(0.1, 0.0, 0.0); - marker.color = createMarkerColor(r, g, b, 0.999); + marker.pose.orientation = create_marker_orientation(0, 0, 0, 1.0); + marker.scale = create_marker_scale(0.1, 0.0, 0.0); + marker.color = create_marker_color(r, g, b, 0.999); for (const auto & p : polygon) { geometry_msgs::msg::Point point; point.x = p.x(); @@ -95,13 +95,13 @@ visualization_msgs::msg::MarkerArray BlindSpotModule::createDebugMarkerArray() const auto now = this->clock_->now(); if (debug_data_.detection_area) { - appendMarkerArray( + append_marker_array( createLaneletPolygonsMarkerArray( {debug_data_.detection_area.value()}, "detection_area", module_id_, 0.5, 0.0, 0.0), &debug_marker_array, now); } - appendMarkerArray( + append_marker_array( debug::createObjectsMarkerArray( debug_data_.conflicting_targets, "conflicting_targets", module_id_, now, 0.99, 0.4, 0.0), &debug_marker_array, now); diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/manager.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/manager.cpp index 76fb7dabc68a7..9313a6fad48d8 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/manager.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/manager.cpp @@ -61,7 +61,7 @@ void BlindSpotModuleManager::launchNewModules( registerModule(std::make_shared( module_id, lane_id, turn_direction, planner_data_, planner_param_, logger_.get_child("blind_spot_module"), clock_, time_keeper_, planning_factor_interface_)); - generateUUID(module_id); + generate_uuid(module_id); updateRTCStatus( getUUID(module_id), true, State::WAITING_FOR_EXECUTION, std::numeric_limits::lowest(), path.header.stamp); diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/parameter.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/parameter.cpp index 0984de6f73262..eafe94eb8e972 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/parameter.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/parameter.cpp @@ -14,7 +14,7 @@ #include "autoware/behavior_velocity_blind_spot_module/parameter.hpp" -#include +#include #include @@ -23,23 +23,24 @@ namespace autoware::behavior_velocity_planner PlannerParam PlannerParam::init(rclcpp::Node & node, const std::string & ns) { - using autoware::universe_utils::getOrDeclareParameter; + using autoware_utils::get_or_declare_parameter; PlannerParam param; - param.use_pass_judge_line = getOrDeclareParameter(node, ns + ".use_pass_judge_line"); - param.stop_line_margin = getOrDeclareParameter(node, ns + ".stop_line_margin"); + param.use_pass_judge_line = get_or_declare_parameter(node, ns + ".use_pass_judge_line"); + param.stop_line_margin = get_or_declare_parameter(node, ns + ".stop_line_margin"); param.backward_detection_length = - getOrDeclareParameter(node, ns + ".backward_detection_length"); + get_or_declare_parameter(node, ns + ".backward_detection_length"); param.ignore_width_from_center_line = - getOrDeclareParameter(node, ns + ".ignore_width_from_center_line"); - param.adjacent_extend_width = getOrDeclareParameter(node, ns + ".adjacent_extend_width"); + get_or_declare_parameter(node, ns + ".ignore_width_from_center_line"); + param.adjacent_extend_width = + get_or_declare_parameter(node, ns + ".adjacent_extend_width"); param.opposite_adjacent_extend_width = - getOrDeclareParameter(node, ns + ".opposite_adjacent_extend_width"); + get_or_declare_parameter(node, ns + ".opposite_adjacent_extend_width"); param.max_future_movement_time = - getOrDeclareParameter(node, ns + ".max_future_movement_time"); - param.ttc_min = getOrDeclareParameter(node, ns + ".ttc_min"); - param.ttc_max = getOrDeclareParameter(node, ns + ".ttc_max"); + get_or_declare_parameter(node, ns + ".max_future_movement_time"); + param.ttc_min = get_or_declare_parameter(node, ns + ".ttc_min"); + param.ttc_max = get_or_declare_parameter(node, ns + ".ttc_max"); param.ttc_ego_minimal_velocity = - getOrDeclareParameter(node, ns + ".ttc_ego_minimal_velocity"); + get_or_declare_parameter(node, ns + ".ttc_ego_minimal_velocity"); return param; } } // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/scene.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/scene.cpp index 2bdbb249c20d9..31b4419fffeef 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/scene.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/scene.cpp @@ -41,7 +41,7 @@ BlindSpotModule::BlindSpotModule( const int64_t module_id, const int64_t lane_id, const TurnDirection turn_direction, const std::shared_ptr planner_data, const PlannerParam & planner_param, const rclcpp::Logger logger, const rclcpp::Clock::SharedPtr clock, - const std::shared_ptr time_keeper, + const std::shared_ptr time_keeper, const std::shared_ptr planning_factor_interface) : SceneModuleInterfaceWithRTC(module_id, logger, clock, time_keeper, planning_factor_interface), @@ -191,7 +191,7 @@ static std::optional getDuplicatedPointIdx( const auto & p = path.points.at(i).point.pose.position; constexpr double min_dist = 0.001; - if (autoware::universe_utils::calcDistance2d(p, point) < min_dist) { + if (autoware_utils::calc_distance2d(p, point) < min_dist) { return i; } } diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/util.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/util.cpp index 73255d0df69fb..c0e55de2f1f4f 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/util.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/util.cpp @@ -90,7 +90,7 @@ std::optional generateInterpolatedPathInfo( std::optional getFirstPointIntersectsLineByFootprint( const lanelet::ConstLineString2d & line, const InterpolatedPathInfo & interpolated_path_info, - const autoware::universe_utils::LinearRing2d & footprint, const double vehicle_length) + const autoware_utils::LinearRing2d & footprint, const double vehicle_length) { const auto & path_ip = interpolated_path_info.path; const auto [lane_start, lane_end] = interpolated_path_info.lane_id_interval.value(); @@ -100,8 +100,8 @@ std::optional getFirstPointIntersectsLineByFootprint( const auto line2d = line.basicLineString(); for (auto i = start; i <= lane_end; ++i) { const auto & base_pose = path_ip.points.at(i).point.pose; - const auto path_footprint = autoware::universe_utils::transformVector( - footprint, autoware::universe_utils::pose2transform(base_pose)); + const auto path_footprint = + autoware_utils::transform_vector(footprint, autoware_utils::pose2transform(base_pose)); if (boost::geometry::intersects(path_footprint, line2d)) { return std::make_optional(i); } diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/package.xml b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/package.xml index 0f0b45c2d8940..0c9dfc39c354b 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/package.xml +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/package.xml @@ -32,7 +32,7 @@ autoware_perception_msgs autoware_planning_msgs autoware_route_handler - autoware_universe_utils + autoware_utils autoware_vehicle_info_utils eigen geometry_msgs diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/debug.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/debug.cpp index 0ec5a2a944059..80d0c411d14db 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/debug.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/debug.cpp @@ -16,20 +16,20 @@ #include #include -#include -#include +#include +#include #include namespace autoware::behavior_velocity_planner { -using autoware::universe_utils::appendMarkerArray; -using autoware::universe_utils::calcOffsetPose; -using autoware::universe_utils::createDefaultMarker; -using autoware::universe_utils::createMarkerColor; -using autoware::universe_utils::createMarkerScale; -using autoware::universe_utils::createPoint; +using autoware_utils::append_marker_array; +using autoware_utils::calc_offset_pose; +using autoware_utils::create_default_marker; +using autoware_utils::create_marker_color; +using autoware_utils::create_marker_scale; +using autoware_utils::create_point; using visualization_msgs::msg::Marker; namespace @@ -46,9 +46,9 @@ visualization_msgs::msg::MarkerArray createCrosswalkMarkers( const auto & point = std::get<1>(collision_point); const auto & state = std::get<2>(collision_point); - auto marker = createDefaultMarker( + auto marker = create_default_marker( "map", now, "collision point state", uid + i++, Marker::TEXT_VIEW_FACING, - createMarkerScale(0.0, 0.0, 1.0), createMarkerColor(1.0, 1.0, 1.0, 0.999)); + create_marker_scale(0.0, 0.0, 1.0), create_marker_color(1.0, 1.0, 1.0, 0.999)); std::ostringstream string_stream; string_stream << std::fixed << std::setprecision(2); string_stream << "(module, ttc, ttv, state)=(" << module_id << " , " @@ -78,26 +78,26 @@ visualization_msgs::msg::MarkerArray createCrosswalkMarkers( } if (debug_data.range_near_point) { - auto marker = createDefaultMarker( - "map", now, "attention range near", uid, Marker::POINTS, createMarkerScale(0.25, 0.25, 0.0), - createMarkerColor(0.0, 0.0, 1.0, 0.999)); + auto marker = create_default_marker( + "map", now, "attention range near", uid, Marker::POINTS, create_marker_scale(0.25, 0.25, 0.0), + create_marker_color(0.0, 0.0, 1.0, 0.999)); marker.points.push_back(*debug_data.range_near_point); msg.markers.push_back(marker); } if (debug_data.range_far_point) { - auto marker = createDefaultMarker( - "map", now, "attention range far", uid, Marker::POINTS, createMarkerScale(0.25, 0.25, 0.0), - createMarkerColor(1.0, 1.0, 1.0, 0.999)); + auto marker = create_default_marker( + "map", now, "attention range far", uid, Marker::POINTS, create_marker_scale(0.25, 0.25, 0.0), + create_marker_color(1.0, 1.0, 1.0, 0.999)); marker.points.push_back(*debug_data.range_far_point); msg.markers.push_back(marker); } for (size_t i = 0; i < debug_data.obj_polygons.size(); ++i) { const auto & points = debug_data.obj_polygons.at(i); - auto marker = createDefaultMarker( - "map", now, "object polygon", uid + i, Marker::LINE_STRIP, createMarkerScale(0.1, 0.0, 0.0), - createMarkerColor(1.0, 0.0, 1.0, 0.999)); + auto marker = create_default_marker( + "map", now, "object polygon", uid + i, Marker::LINE_STRIP, create_marker_scale(0.1, 0.0, 0.0), + create_marker_color(1.0, 0.0, 1.0, 0.999)); marker.points = points; // marker.points.push_back(marker.points.front()); msg.markers.push_back(marker); @@ -105,9 +105,9 @@ visualization_msgs::msg::MarkerArray createCrosswalkMarkers( for (size_t i = 0; i < debug_data.ego_polygons.size(); ++i) { const auto & points = debug_data.ego_polygons.at(i); - auto marker = createDefaultMarker( - "map", now, "vehicle polygon", uid + i, Marker::LINE_STRIP, createMarkerScale(0.1, 0.0, 0.0), - createMarkerColor(1.0, 1.0, 0.0, 0.999)); + auto marker = create_default_marker( + "map", now, "vehicle polygon", uid + i, Marker::LINE_STRIP, + create_marker_scale(0.1, 0.0, 0.0), create_marker_color(1.0, 1.0, 0.0, 0.999)); marker.points = points; // marker.points.push_back(marker.points.front()); msg.markers.push_back(marker); @@ -115,23 +115,23 @@ visualization_msgs::msg::MarkerArray createCrosswalkMarkers( // Crosswalk polygon if (!debug_data.crosswalk_polygon.empty()) { - auto marker = createDefaultMarker( - "map", now, "crosswalk polygon", uid, Marker::LINE_STRIP, createMarkerScale(0.1, 0.0, 0.0), - createMarkerColor(1.0, 1.0, 1.0, 0.999)); + auto marker = create_default_marker( + "map", now, "crosswalk polygon", uid, Marker::LINE_STRIP, create_marker_scale(0.1, 0.0, 0.0), + create_marker_color(1.0, 1.0, 1.0, 0.999)); for (const auto & p : debug_data.crosswalk_polygon) { - marker.points.push_back(createPoint(p.x, p.y, p.z)); + marker.points.push_back(create_point(p.x, p.y, p.z)); } marker.points.push_back(marker.points.front()); - marker.color = debug_data.ignore_crosswalk ? createMarkerColor(1.0, 1.0, 1.0, 0.999) - : createMarkerColor(1.0, 0.0, 0.0, 0.999); + marker.color = debug_data.ignore_crosswalk ? create_marker_color(1.0, 1.0, 1.0, 0.999) + : create_marker_color(1.0, 0.0, 0.0, 0.999); msg.markers.push_back(marker); } // Collision point if (!debug_data.collision_points.empty()) { - auto marker = createDefaultMarker( - "map", now, "collision point", uid, Marker::POINTS, createMarkerScale(0.25, 0.25, 0.0), - createMarkerColor(1.0, 0.0, 1.0, 0.999)); + auto marker = create_default_marker( + "map", now, "collision point", uid, Marker::POINTS, create_marker_scale(0.25, 0.25, 0.0), + create_marker_color(1.0, 0.0, 1.0, 0.999)); for (const auto & collision_point : debug_data.collision_points) { marker.points.push_back(std::get<1>(collision_point).collision_point); } @@ -140,20 +140,20 @@ visualization_msgs::msg::MarkerArray createCrosswalkMarkers( // Slow point if (!debug_data.slow_poses.empty()) { - auto marker = createDefaultMarker( - "map", now, "slow point", uid, Marker::POINTS, createMarkerScale(0.25, 0.25, 0.0), - createMarkerColor(1.0, 1.0, 0.0, 0.999)); + auto marker = create_default_marker( + "map", now, "slow point", uid, Marker::POINTS, create_marker_scale(0.25, 0.25, 0.0), + create_marker_color(1.0, 1.0, 0.0, 0.999)); for (const auto & p : debug_data.slow_poses) { - marker.points.push_back(createPoint(p.position.x, p.position.y, p.position.z)); + marker.points.push_back(create_point(p.position.x, p.position.y, p.position.z)); } msg.markers.push_back(marker); } // Stop factor point if (!debug_data.stop_factor_points.empty()) { - auto marker = createDefaultMarker( - "map", now, "stop factor point", uid, Marker::POINTS, createMarkerScale(0.25, 0.25, 0.0), - createMarkerColor(0.0, 0.0, 1.0, 0.999)); + auto marker = create_default_marker( + "map", now, "stop factor point", uid, Marker::POINTS, create_marker_scale(0.25, 0.25, 0.0), + create_marker_color(0.0, 0.0, 1.0, 0.999)); for (const auto & p : debug_data.stop_factor_points) { marker.points.push_back(p); } @@ -162,31 +162,31 @@ visualization_msgs::msg::MarkerArray createCrosswalkMarkers( // Stop point if (!debug_data.stop_poses.empty()) { - auto marker = createDefaultMarker( - "map", now, "stop point", uid, Marker::POINTS, createMarkerScale(0.25, 0.25, 0.0), - createMarkerColor(1.0, 0.0, 0.0, 0.999)); + auto marker = create_default_marker( + "map", now, "stop point", uid, Marker::POINTS, create_marker_scale(0.25, 0.25, 0.0), + create_marker_color(1.0, 0.0, 0.0, 0.999)); for (const auto & p : debug_data.stop_poses) { - marker.points.push_back(createPoint(p.position.x, p.position.y, p.position.z)); + marker.points.push_back(create_point(p.position.x, p.position.y, p.position.z)); } msg.markers.push_back(marker); } if (!debug_data.occlusion_detection_areas.empty()) { - auto marker = createDefaultMarker( + auto marker = create_default_marker( "map", now, "occlusion_detection_areas", uid, Marker::LINE_LIST, - createMarkerScale(0.25, 0.25, 0.0), createMarkerColor(1.0, 1.0, 1.0, 0.5)); + create_marker_scale(0.25, 0.25, 0.0), create_marker_color(1.0, 1.0, 1.0, 0.5)); for (const auto & area : debug_data.occlusion_detection_areas) { for (auto i = 0UL; i + 1 < area.size(); ++i) { const auto & p1 = area[i]; const auto & p2 = area[i + 1]; - marker.points.push_back(createPoint(p1.x(), p1.y(), 0.0)); - marker.points.push_back(createPoint(p2.x(), p2.y(), 0.0)); + marker.points.push_back(create_point(p1.x(), p1.y(), 0.0)); + marker.points.push_back(create_point(p2.x(), p2.y(), 0.0)); } } msg.markers.push_back(marker); - marker = createDefaultMarker( - "map", now, "crosswalk_origin", uid, Marker::SPHERE, createMarkerScale(0.25, 0.25, 0.25), - createMarkerColor(1.0, 1.0, 1.0, 0.5)); + marker = create_default_marker( + "map", now, "crosswalk_origin", uid, Marker::SPHERE, create_marker_scale(0.25, 0.25, 0.25), + create_marker_color(1.0, 1.0, 1.0, 0.5)); marker.pose.position = debug_data.crosswalk_origin; msg.markers.push_back(marker); } @@ -204,19 +204,19 @@ autoware::motion_utils::VirtualWalls CrosswalkModule::createVirtualWalls() wall.style = autoware::motion_utils::VirtualWallType::stop; for (const auto & p : debug_data_.stop_poses) { - wall.pose = calcOffsetPose(p, debug_data_.base_link2front, 0.0, 0.0); + wall.pose = calc_offset_pose(p, debug_data_.base_link2front, 0.0, 0.0); virtual_walls.push_back(wall); } wall.style = autoware::motion_utils::VirtualWallType::slowdown; wall.text += debug_data_.virtual_wall_suffix; for (const auto & p : debug_data_.slow_poses) { - wall.pose = calcOffsetPose(p, debug_data_.base_link2front, 0.0, 0.0); + wall.pose = calc_offset_pose(p, debug_data_.base_link2front, 0.0, 0.0); virtual_walls.push_back(wall); } wall.style = autoware::motion_utils::VirtualWallType::pass; wall.text += debug_data_.virtual_wall_suffix; for (const auto & p : debug_data_.pass_poses) { - wall.pose = calcOffsetPose(p, debug_data_.base_link2front, 0.0, 0.0); + wall.pose = calc_offset_pose(p, debug_data_.base_link2front, 0.0, 0.0); virtual_walls.push_back(wall); } @@ -227,7 +227,7 @@ visualization_msgs::msg::MarkerArray CrosswalkModule::createDebugMarkerArray() { visualization_msgs::msg::MarkerArray debug_marker_array; - appendMarkerArray( + append_marker_array( createCrosswalkMarkers(debug_data_, this->clock_->now(), module_id_), &debug_marker_array); return debug_marker_array; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/manager.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/manager.cpp index 5bfd31b75976a..410fb51100924 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/manager.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/manager.cpp @@ -15,7 +15,7 @@ #include "manager.hpp" #include -#include +#include #include #include @@ -27,7 +27,7 @@ namespace autoware::behavior_velocity_planner { -using autoware::universe_utils::getOrDeclareParameter; +using autoware_utils::get_or_declare_parameter; using lanelet::autoware::Crosswalk; CrosswalkModuleManager::CrosswalkModuleManager(rclcpp::Node & node) @@ -38,121 +38,126 @@ CrosswalkModuleManager::CrosswalkModuleManager(rclcpp::Node & node) // for crosswalk parameters auto & cp = crosswalk_planner_param_; - cp.show_processing_time = getOrDeclareParameter(node, ns + ".common.show_processing_time"); + cp.show_processing_time = + get_or_declare_parameter(node, ns + ".common.show_processing_time"); // param for input data cp.traffic_light_state_timeout = - getOrDeclareParameter(node, ns + ".common.traffic_light_state_timeout"); + get_or_declare_parameter(node, ns + ".common.traffic_light_state_timeout"); // param for stop position cp.stop_distance_from_crosswalk = - getOrDeclareParameter(node, ns + ".stop_position.stop_distance_from_crosswalk"); - cp.stop_distance_from_object_preferred = - getOrDeclareParameter(node, ns + ".stop_position.stop_distance_from_object_preferred"); + get_or_declare_parameter(node, ns + ".stop_position.stop_distance_from_crosswalk"); + cp.stop_distance_from_object_preferred = get_or_declare_parameter( + node, ns + ".stop_position.stop_distance_from_object_preferred"); cp.stop_distance_from_object_limit = - getOrDeclareParameter(node, ns + ".stop_position.stop_distance_from_object_limit"); + get_or_declare_parameter(node, ns + ".stop_position.stop_distance_from_object_limit"); cp.stop_position_threshold = - getOrDeclareParameter(node, ns + ".stop_position.stop_position_threshold"); + get_or_declare_parameter(node, ns + ".stop_position.stop_position_threshold"); cp.min_acc_preferred = - getOrDeclareParameter(node, ns + ".stop_position.min_acc_preferred"); + get_or_declare_parameter(node, ns + ".stop_position.min_acc_preferred"); cp.min_jerk_preferred = - getOrDeclareParameter(node, ns + ".stop_position.min_jerk_preferred"); + get_or_declare_parameter(node, ns + ".stop_position.min_jerk_preferred"); // param for restart suppression cp.min_dist_to_stop_for_restart_suppression = - getOrDeclareParameter(node, ns + ".restart_suppression.min_distance_to_stop"); + get_or_declare_parameter(node, ns + ".restart_suppression.min_distance_to_stop"); cp.max_dist_to_stop_for_restart_suppression = - getOrDeclareParameter(node, ns + ".restart_suppression.max_distance_to_stop"); + get_or_declare_parameter(node, ns + ".restart_suppression.max_distance_to_stop"); // param for ego velocity cp.min_slow_down_velocity = - getOrDeclareParameter(node, ns + ".slow_down.min_slow_down_velocity"); - cp.max_slow_down_jerk = getOrDeclareParameter(node, ns + ".slow_down.max_slow_down_jerk"); + get_or_declare_parameter(node, ns + ".slow_down.min_slow_down_velocity"); + cp.max_slow_down_jerk = + get_or_declare_parameter(node, ns + ".slow_down.max_slow_down_jerk"); cp.max_slow_down_accel = - getOrDeclareParameter(node, ns + ".slow_down.max_slow_down_accel"); - cp.no_relax_velocity = getOrDeclareParameter(node, ns + ".slow_down.no_relax_velocity"); + get_or_declare_parameter(node, ns + ".slow_down.max_slow_down_accel"); + cp.no_relax_velocity = + get_or_declare_parameter(node, ns + ".slow_down.no_relax_velocity"); // param for stuck vehicle cp.enable_stuck_check_in_intersection = - getOrDeclareParameter(node, ns + ".stuck_vehicle.enable_stuck_check_in_intersection"); + get_or_declare_parameter(node, ns + ".stuck_vehicle.enable_stuck_check_in_intersection"); cp.stuck_vehicle_velocity = - getOrDeclareParameter(node, ns + ".stuck_vehicle.stuck_vehicle_velocity"); + get_or_declare_parameter(node, ns + ".stuck_vehicle.stuck_vehicle_velocity"); cp.max_stuck_vehicle_lateral_offset = - getOrDeclareParameter(node, ns + ".stuck_vehicle.max_stuck_vehicle_lateral_offset"); + get_or_declare_parameter(node, ns + ".stuck_vehicle.max_stuck_vehicle_lateral_offset"); cp.required_clearance = - getOrDeclareParameter(node, ns + ".stuck_vehicle.required_clearance"); - cp.min_acc_for_stuck_vehicle = getOrDeclareParameter(node, ns + ".stuck_vehicle.min_acc"); + get_or_declare_parameter(node, ns + ".stuck_vehicle.required_clearance"); + cp.min_acc_for_stuck_vehicle = + get_or_declare_parameter(node, ns + ".stuck_vehicle.min_acc"); cp.max_jerk_for_stuck_vehicle = - getOrDeclareParameter(node, ns + ".stuck_vehicle.max_jerk"); + get_or_declare_parameter(node, ns + ".stuck_vehicle.max_jerk"); cp.min_jerk_for_stuck_vehicle = - getOrDeclareParameter(node, ns + ".stuck_vehicle.min_jerk"); + get_or_declare_parameter(node, ns + ".stuck_vehicle.min_jerk"); // param for pass judge logic cp.ego_pass_first_margin_x = - getOrDeclareParameter>(node, ns + ".pass_judge.ego_pass_first_margin_x"); + get_or_declare_parameter>(node, ns + ".pass_judge.ego_pass_first_margin_x"); cp.ego_pass_first_margin_y = - getOrDeclareParameter>(node, ns + ".pass_judge.ego_pass_first_margin_y"); + get_or_declare_parameter>(node, ns + ".pass_judge.ego_pass_first_margin_y"); cp.ego_pass_first_additional_margin = - getOrDeclareParameter(node, ns + ".pass_judge.ego_pass_first_additional_margin"); + get_or_declare_parameter(node, ns + ".pass_judge.ego_pass_first_additional_margin"); cp.ego_pass_later_margin_x = - getOrDeclareParameter>(node, ns + ".pass_judge.ego_pass_later_margin_x"); + get_or_declare_parameter>(node, ns + ".pass_judge.ego_pass_later_margin_x"); cp.ego_pass_later_margin_y = - getOrDeclareParameter>(node, ns + ".pass_judge.ego_pass_later_margin_y"); + get_or_declare_parameter>(node, ns + ".pass_judge.ego_pass_later_margin_y"); cp.ego_pass_later_additional_margin = - getOrDeclareParameter(node, ns + ".pass_judge.ego_pass_later_additional_margin"); + get_or_declare_parameter(node, ns + ".pass_judge.ego_pass_later_additional_margin"); cp.ego_min_assumed_speed = - getOrDeclareParameter(node, ns + ".pass_judge.ego_min_assumed_speed"); + get_or_declare_parameter(node, ns + ".pass_judge.ego_min_assumed_speed"); cp.min_acc_for_no_stop_decision = - getOrDeclareParameter(node, ns + ".pass_judge.no_stop_decision.min_acc"); + get_or_declare_parameter(node, ns + ".pass_judge.no_stop_decision.min_acc"); cp.min_jerk_for_no_stop_decision = - getOrDeclareParameter(node, ns + ".pass_judge.no_stop_decision.min_jerk"); - cp.overrun_threshold_length_for_no_stop_decision = getOrDeclareParameter( + get_or_declare_parameter(node, ns + ".pass_judge.no_stop_decision.min_jerk"); + cp.overrun_threshold_length_for_no_stop_decision = get_or_declare_parameter( node, ns + ".pass_judge.no_stop_decision.overrun_threshold_length"); cp.stop_object_velocity = - getOrDeclareParameter(node, ns + ".pass_judge.stop_object_velocity_threshold"); + get_or_declare_parameter(node, ns + ".pass_judge.stop_object_velocity_threshold"); cp.min_object_velocity = - getOrDeclareParameter(node, ns + ".pass_judge.min_object_velocity"); + get_or_declare_parameter(node, ns + ".pass_judge.min_object_velocity"); cp.disable_yield_for_new_stopped_object = - getOrDeclareParameter(node, ns + ".pass_judge.disable_yield_for_new_stopped_object"); - cp.distance_set_for_no_intention_to_walk = getOrDeclareParameter>( + get_or_declare_parameter(node, ns + ".pass_judge.disable_yield_for_new_stopped_object"); + cp.distance_set_for_no_intention_to_walk = get_or_declare_parameter>( node, ns + ".pass_judge.distance_set_for_no_intention_to_walk"); - cp.timeout_set_for_no_intention_to_walk = getOrDeclareParameter>( + cp.timeout_set_for_no_intention_to_walk = get_or_declare_parameter>( node, ns + ".pass_judge.timeout_set_for_no_intention_to_walk"); cp.timeout_ego_stop_for_yield = - getOrDeclareParameter(node, ns + ".pass_judge.timeout_ego_stop_for_yield"); + get_or_declare_parameter(node, ns + ".pass_judge.timeout_ego_stop_for_yield"); // param for target area & object cp.crosswalk_attention_range = - getOrDeclareParameter(node, ns + ".object_filtering.crosswalk_attention_range"); - cp.vehicle_object_cross_angle_threshold = getOrDeclareParameter( + get_or_declare_parameter(node, ns + ".object_filtering.crosswalk_attention_range"); + cp.vehicle_object_cross_angle_threshold = get_or_declare_parameter( node, ns + ".object_filtering.vehicle_object_cross_angle_threshold"); cp.look_unknown = - getOrDeclareParameter(node, ns + ".object_filtering.target_object.unknown"); + get_or_declare_parameter(node, ns + ".object_filtering.target_object.unknown"); cp.look_bicycle = - getOrDeclareParameter(node, ns + ".object_filtering.target_object.bicycle"); + get_or_declare_parameter(node, ns + ".object_filtering.target_object.bicycle"); cp.look_motorcycle = - getOrDeclareParameter(node, ns + ".object_filtering.target_object.motorcycle"); + get_or_declare_parameter(node, ns + ".object_filtering.target_object.motorcycle"); cp.look_pedestrian = - getOrDeclareParameter(node, ns + ".object_filtering.target_object.pedestrian"); + get_or_declare_parameter(node, ns + ".object_filtering.target_object.pedestrian"); // param for occlusions - cp.occlusion_enable = getOrDeclareParameter(node, ns + ".occlusion.enable"); + cp.occlusion_enable = get_or_declare_parameter(node, ns + ".occlusion.enable"); cp.occlusion_occluded_object_velocity = - getOrDeclareParameter(node, ns + ".occlusion.occluded_object_velocity"); + get_or_declare_parameter(node, ns + ".occlusion.occluded_object_velocity"); cp.occlusion_slow_down_velocity = - getOrDeclareParameter(node, ns + ".occlusion.slow_down_velocity"); - cp.occlusion_time_buffer = getOrDeclareParameter(node, ns + ".occlusion.time_buffer"); - cp.occlusion_min_size = getOrDeclareParameter(node, ns + ".occlusion.min_size"); - cp.occlusion_free_space_max = getOrDeclareParameter(node, ns + ".occlusion.free_space_max"); - cp.occlusion_occupied_min = getOrDeclareParameter(node, ns + ".occlusion.occupied_min"); + get_or_declare_parameter(node, ns + ".occlusion.slow_down_velocity"); + cp.occlusion_time_buffer = get_or_declare_parameter(node, ns + ".occlusion.time_buffer"); + cp.occlusion_min_size = get_or_declare_parameter(node, ns + ".occlusion.min_size"); + cp.occlusion_free_space_max = + get_or_declare_parameter(node, ns + ".occlusion.free_space_max"); + cp.occlusion_occupied_min = get_or_declare_parameter(node, ns + ".occlusion.occupied_min"); cp.occlusion_ignore_with_traffic_light = - getOrDeclareParameter(node, ns + ".occlusion.ignore_with_traffic_light"); + get_or_declare_parameter(node, ns + ".occlusion.ignore_with_traffic_light"); cp.occlusion_ignore_behind_predicted_objects = - getOrDeclareParameter(node, ns + ".occlusion.ignore_behind_predicted_objects"); + get_or_declare_parameter(node, ns + ".occlusion.ignore_behind_predicted_objects"); cp.occlusion_ignore_velocity_thresholds.resize( autoware_perception_msgs::msg::ObjectClassification::PEDESTRIAN + 1, - getOrDeclareParameter(node, ns + ".occlusion.ignore_velocity_thresholds.default")); + get_or_declare_parameter(node, ns + ".occlusion.ignore_velocity_thresholds.default")); const auto get_label = [](const std::string & s) { if (s == "CAR") return autoware_perception_msgs::msg::ObjectClassification::CAR; if (s == "TRUCK") return autoware_perception_msgs::msg::ObjectClassification::TRUCK; @@ -163,15 +168,15 @@ CrosswalkModuleManager::CrosswalkModuleManager(rclcpp::Node & node) if (s == "PEDESTRIAN") return autoware_perception_msgs::msg::ObjectClassification::PEDESTRIAN; return autoware_perception_msgs::msg::ObjectClassification::UNKNOWN; }; - const auto custom_labels = getOrDeclareParameter>( + const auto custom_labels = get_or_declare_parameter>( node, ns + ".occlusion.ignore_velocity_thresholds.custom_labels"); - const auto custom_velocities = getOrDeclareParameter>( + const auto custom_velocities = get_or_declare_parameter>( node, ns + ".occlusion.ignore_velocity_thresholds.custom_thresholds"); for (auto idx = 0UL; idx < std::min(custom_labels.size(), custom_velocities.size()); ++idx) { cp.occlusion_ignore_velocity_thresholds[get_label(custom_labels[idx])] = custom_velocities[idx]; } cp.occlusion_extra_objects_size = - getOrDeclareParameter(node, ns + ".occlusion.extra_predicted_objects_size"); + get_or_declare_parameter(node, ns + ".occlusion.extra_predicted_objects_size"); } void CrosswalkModuleManager::launchNewModules(const PathWithLaneId & path) @@ -194,7 +199,7 @@ void CrosswalkModuleManager::launchNewModules(const PathWithLaneId & path) registerModule(std::make_shared( node_, road_lanelet_id, crosswalk_lanelet_id, reg_elem_id, lanelet_map_ptr, p, logger, clock_, time_keeper_, planning_factor_interface_)); - generateUUID(crosswalk_lanelet_id); + generate_uuid(crosswalk_lanelet_id); updateRTCStatus( getUUID(crosswalk_lanelet_id), true, State::WAITING_FOR_EXECUTION, std::numeric_limits::lowest(), path.header.stamp); diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/occluded_crosswalk.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/occluded_crosswalk.cpp index dc4f2dc79110b..cf526c3e8da44 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/occluded_crosswalk.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/occluded_crosswalk.cpp @@ -15,7 +15,7 @@ #include "occluded_crosswalk.hpp" #include -#include +#include #include #include @@ -119,7 +119,7 @@ void clear_occlusions_behind_objects( object.kinematics.initial_pose_with_covariance.pose.position.y}; if (lanelet::geometry::distance2d(grid_map_position, object_position) < range) { lanelet::BasicPoints2d edge_points; - const auto object_polygon = autoware::universe_utils::toPolygon2d(object); + const auto object_polygon = autoware_utils::to_polygon2d(object); for (const auto & edge_point : object_polygon.outer()) edge_points.push_back(edge_point); std::sort(edge_points.begin(), edge_points.end(), angle_cmp); // points.push_back(interpolate_point({object_position, edge_point}, 10.0 * range)); diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp index 43f2f6617e5d0..c2d56e86b16b1 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp @@ -21,9 +21,9 @@ #include #include #include -#include -#include -#include +#include +#include +#include #include #include @@ -52,11 +52,11 @@ using autoware::motion_utils::calcSignedArcLength; using autoware::motion_utils::calcSignedArcLengthPartialSum; using autoware::motion_utils::findNearestSegmentIndex; using autoware::motion_utils::resamplePath; -using autoware::universe_utils::createPoint; -using autoware::universe_utils::getPose; -using autoware::universe_utils::Point2d; -using autoware::universe_utils::Polygon2d; -using autoware::universe_utils::toHexString; +using autoware_utils::create_point; +using autoware_utils::get_pose; +using autoware_utils::Point2d; +using autoware_utils::Polygon2d; +using autoware_utils::to_hex_string; namespace { @@ -74,7 +74,7 @@ std::vector toGeometryPointVector( { std::vector points; for (const auto & p : polygon.outer()) { - points.push_back(createPoint(p.x(), p.y(), z)); + points.push_back(create_point(p.x(), p.y(), z)); } return points; } @@ -85,7 +85,7 @@ void offsetPolygon2d( { for (const auto & polygon_point : polygon.points) { const auto offset_pos = - autoware::universe_utils::calcOffsetPose(origin_point, polygon_point.x, polygon_point.y, 0.0) + autoware_utils::calc_offset_pose(origin_point, polygon_point.x, polygon_point.y, 0.0) .position; offset_polygon.outer().push_back(Point2d(offset_pos.x, offset_pos.y)); } @@ -98,8 +98,7 @@ Polygon2d createMultiStepPolygon( { Polygon2d multi_step_polygon{}; for (size_t i = start_idx; i <= end_idx; ++i) { - offsetPolygon2d( - autoware::universe_utils::getPose(obj_path_points.at(i)), polygon, multi_step_polygon); + offsetPolygon2d(autoware_utils::get_pose(obj_path_points.at(i)), polygon, multi_step_polygon); } Polygon2d hull_multi_step_polygon{}; @@ -176,7 +175,7 @@ CrosswalkModule::CrosswalkModule( const std::optional & reg_elem_id, const lanelet::LaneletMapPtr & lanelet_map_ptr, const PlannerParam & planner_param, const rclcpp::Logger & logger, const rclcpp::Clock::SharedPtr clock, - const std::shared_ptr time_keeper, + const std::shared_ptr time_keeper, const std::shared_ptr planning_factor_interface) : SceneModuleInterfaceWithRTC(module_id, logger, clock, time_keeper, planning_factor_interface), @@ -223,7 +222,7 @@ bool CrosswalkModule::modifyPathVelocity(PathWithLaneId * path) // Initialize debug data debug_data_ = DebugData(planner_data_); for (const auto & p : crosswalk_.polygon2d().basicPolygon()) { - debug_data_.crosswalk_polygon.push_back(createPoint(p.x(), p.y(), ego_pos.z)); + debug_data_.crosswalk_polygon.push_back(create_point(p.x(), p.y(), ego_pos.z)); } recordTime(1); @@ -315,7 +314,7 @@ std::optional CrosswalkModule::checkStopForCrosswalkUsers( const PathWithLaneId & ego_path, const PathWithLaneId & sparse_resample_path, const geometry_msgs::msg::Point & first_path_point_on_crosswalk, const geometry_msgs::msg::Point & last_path_point_on_crosswalk, - const std::optional & default_stop_pose_opt) + const std::optional & default_stop_pose) { const auto & ego_pos = planner_data_->current_odometry->pose.position; @@ -329,8 +328,8 @@ std::optional CrosswalkModule::checkStopForCrosswalkUsers( // Update object state // This exceptional handling should be done in update(), but is compromised by history const double dist_default_stop = - default_stop_pose_opt.has_value() - ? calcSignedArcLength(ego_path.points, ego_pos, default_stop_pose_opt->position) + default_stop_pose.has_value() + ? calcSignedArcLength(ego_path.points, ego_pos, default_stop_pose->position) : 0.0; updateObjectState( dist_default_stop, sparse_resample_path, crosswalk_attention_range, attention_area); @@ -362,7 +361,7 @@ std::optional CrosswalkModule::checkStopForCrosswalkUsers( } const auto decided_stop_pose_opt = - calcStopPose(ego_path, dist_nearest_cp.value(), default_stop_pose_opt); + calcStopPose(ego_path, dist_nearest_cp.value(), default_stop_pose); if (!decided_stop_pose_opt.has_value()) { return {}; } @@ -506,7 +505,7 @@ void CrosswalkModule::insertDecelPointWithDebugInfo( return; } - debug_data_.first_stop_pose = getPose(*stop_pose); + debug_data_.first_stop_pose = get_pose(*stop_pose); if (std::abs(target_velocity) < 1e-3) { debug_data_.stop_poses.push_back(*stop_pose); @@ -651,19 +650,18 @@ std::pair CrosswalkModule::clampAttentionRangeByNeighborCrosswal } std::optional CrosswalkModule::findEgoPassageDirectionAlongPath( - const PathWithLaneId & path) const + const PathWithLaneId & sparse_resample_path) const { auto findIntersectPoint = [&](const lanelet::ConstLineString3d line) -> std::optional { const auto line_start = - autoware::universe_utils::createPoint(line.front().x(), line.front().y(), line.front().z()); + autoware_utils::create_point(line.front().x(), line.front().y(), line.front().z()); const auto line_end = - autoware::universe_utils::createPoint(line.back().x(), line.back().y(), line.back().z()); - for (unsigned i = 0; i < path.points.size() - 1; ++i) { - const auto & start = path.points.at(i).point.pose.position; - const auto & end = path.points.at(i + 1).point.pose.position; - if (const auto intersect = - autoware::universe_utils::intersect(line_start, line_end, start, end); + autoware_utils::create_point(line.back().x(), line.back().y(), line.back().z()); + for (unsigned i = 0; i < sparse_resample_path.points.size() - 1; ++i) { + const auto & start = sparse_resample_path.points.at(i).point.pose.position; + const auto & end = sparse_resample_path.points.at(i + 1).point.pose.position; + if (const auto intersect = autoware_utils::intersect(line_start, line_end, start, end); intersect.has_value()) { return intersect; } @@ -683,19 +681,18 @@ std::optional CrosswalkModule::findEgoPassageDirectionAlongPath( std::optional CrosswalkModule::findObjectPassageDirectionAlongVehicleLane( const autoware_perception_msgs::msg::PredictedPath & path) const { - using autoware::universe_utils::Segment2d; + using autoware_utils::Segment2d; auto findIntersectPoint = [&](const lanelet::ConstLineString3d line) -> std::optional> { const auto line_start = - autoware::universe_utils::createPoint(line.front().x(), line.front().y(), line.front().z()); + autoware_utils::create_point(line.front().x(), line.front().y(), line.front().z()); const auto line_end = - autoware::universe_utils::createPoint(line.back().x(), line.back().y(), line.back().z()); + autoware_utils::create_point(line.back().x(), line.back().y(), line.back().z()); for (unsigned i = 0; i < path.path.size() - 1; ++i) { const auto & start = path.path.at(i).position; const auto & end = path.path.at(i + 1).position; - if (const auto intersect = - autoware::universe_utils::intersect(line_start, line_end, start, end); + if (const auto intersect = autoware_utils::intersect(line_start, line_end, start, end); intersect.has_value()) { return std::make_optional(std::make_pair(i, intersect.value())); } @@ -773,7 +770,7 @@ std::optional CrosswalkModule::getCollisionPoint( // Calculate nearest collision point among collisions with a predicted path Point2d boost_intersection_center_point; bg::centroid(multi_step_intersection_polygons.front(), boost_intersection_center_point); - const auto intersection_center_point = createPoint( + const auto intersection_center_point = create_point( boost_intersection_center_point.x(), boost_intersection_center_point.y(), ego_pos.z); const auto dist_ego2cp = @@ -1135,7 +1132,7 @@ void CrosswalkModule::updateObjectState( // Update object state object_info_manager_.init(); for (const auto & object : objects_ptr->objects) { - const auto obj_uuid = toHexString(object.object_id); + const auto obj_uuid = to_hex_string(object.object_id); const auto & obj_pos = object.kinematics.initial_pose_with_covariance.pose.position; const auto & obj_vel = object.kinematics.initial_twist_with_covariance.twist.linear; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp index 167917263318d..7b85b07ee114f 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp @@ -18,9 +18,9 @@ #include "autoware/behavior_velocity_crosswalk_module/util.hpp" #include -#include -#include #include +#include +#include #include #include @@ -50,13 +50,13 @@ namespace autoware::behavior_velocity_planner { namespace bg = boost::geometry; -using autoware::universe_utils::Polygon2d; -using autoware::universe_utils::StopWatch; using autoware_internal_planning_msgs::msg::PathWithLaneId; using autoware_perception_msgs::msg::ObjectClassification; using autoware_perception_msgs::msg::PredictedObject; using autoware_perception_msgs::msg::PredictedObjects; using autoware_perception_msgs::msg::TrafficLightElement; +using autoware_utils::Polygon2d; +using autoware_utils::StopWatch; using lanelet::autoware::Crosswalk; namespace @@ -295,8 +295,7 @@ class CrosswalkModule : public SceneModuleInterfaceWithRTC const bool is_object_away_from_path = !attention_area.outer().empty() && - boost::geometry::distance( - autoware::universe_utils::fromMsg(position).to_2d(), attention_area) > 0.5; + boost::geometry::distance(autoware_utils::from_msg(position).to_2d(), attention_area) > 0.5; // add new object if (objects.count(uuid) == 0) { @@ -355,7 +354,7 @@ class CrosswalkModule : public SceneModuleInterfaceWithRTC const std::optional & reg_elem_id, const lanelet::LaneletMapPtr & lanelet_map_ptr, const PlannerParam & planner_param, const rclcpp::Logger & logger, const rclcpp::Clock::SharedPtr clock, - const std::shared_ptr time_keeper, + const std::shared_ptr time_keeper, const std::shared_ptr planning_factor_interface); diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/util.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/util.cpp index 6951ead97ccea..f900da80dbab0 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/util.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/util.cpp @@ -17,9 +17,9 @@ #include #include #include -#include -#include #include +#include +#include #include @@ -50,9 +50,9 @@ namespace autoware::behavior_velocity_planner { namespace bg = boost::geometry; using autoware::motion_utils::calcSignedArcLength; -using autoware::universe_utils::createPoint; -using autoware::universe_utils::Line2d; -using autoware::universe_utils::Point2d; +using autoware_utils::create_point; +using autoware_utils::Line2d; +using autoware_utils::Point2d; std::vector> getCrosswalksOnPath( const geometry_msgs::msg::Pose & current_pose, @@ -144,10 +144,10 @@ getPathEndPointsOnCrosswalk( const auto compare = [&](const Point2d & p1, const Point2d & p2) { const auto dist_l1 = - calcSignedArcLength(ego_path.points, size_t(0), createPoint(p1.x(), p1.y(), ego_pos.z)); + calcSignedArcLength(ego_path.points, size_t(0), create_point(p1.x(), p1.y(), ego_pos.z)); const auto dist_l2 = - calcSignedArcLength(ego_path.points, size_t(0), createPoint(p2.x(), p2.y(), ego_pos.z)); + calcSignedArcLength(ego_path.points, size_t(0), create_point(p2.x(), p2.y(), ego_pos.z)); return dist_l1 < dist_l2; }; @@ -161,8 +161,8 @@ getPathEndPointsOnCrosswalk( const auto & front_intersects = intersects.front(); const auto & back_intersects = intersects.back(); return std::make_pair( - createPoint(front_intersects.x(), front_intersects.y(), ego_pos.z), - createPoint(back_intersects.x(), back_intersects.y(), ego_pos.z)); + create_point(front_intersects.x(), front_intersects.y(), ego_pos.z), + create_point(back_intersects.x(), back_intersects.y(), ego_pos.z)); } std::vector getLinestringIntersects( @@ -195,20 +195,20 @@ std::vector getLinestringIntersects( const auto compare = [&](const Point2d & p1, const Point2d & p2) { const auto dist_l1 = - calcSignedArcLength(ego_path.points, size_t(0), createPoint(p1.x(), p1.y(), ego_pos.z)); + calcSignedArcLength(ego_path.points, size_t(0), create_point(p1.x(), p1.y(), ego_pos.z)); const auto dist_l2 = - calcSignedArcLength(ego_path.points, size_t(0), createPoint(p2.x(), p2.y(), ego_pos.z)); + calcSignedArcLength(ego_path.points, size_t(0), create_point(p2.x(), p2.y(), ego_pos.z)); return dist_l1 < dist_l2; }; std::sort(intersects.begin(), intersects.end(), compare); - // convert autoware::universe_utils::Point2d to geometry::msg::Point + // convert autoware_utils::Point2d to geometry::msg::Point std::vector geometry_points; for (const auto & p : intersects) { - geometry_points.push_back(createPoint(p.x(), p.y(), ego_pos.z)); + geometry_points.push_back(create_point(p.x(), p.y(), ego_pos.z)); } return geometry_points; } diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/package.xml b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/package.xml index aff8d6238f4fb..ae693191d61c0 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/package.xml +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/package.xml @@ -23,7 +23,7 @@ autoware_motion_utils autoware_planning_msgs autoware_route_handler - autoware_universe_utils + autoware_utils eigen geometry_msgs libboost-dev diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/debug.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/debug.cpp index 87df2a620f717..4279e73cb9ada 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/debug.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/debug.cpp @@ -17,8 +17,8 @@ #include #include #include -#include -#include +#include +#include #ifdef ROS_DISTRO_GALACTIC #include @@ -31,10 +31,10 @@ namespace autoware::behavior_velocity_planner { -using autoware::universe_utils::appendMarkerArray; -using autoware::universe_utils::createDefaultMarker; -using autoware::universe_utils::createMarkerColor; -using autoware::universe_utils::createMarkerScale; +using autoware_utils::append_marker_array; +using autoware_utils::create_default_marker; +using autoware_utils::create_marker_color; +using autoware_utils::create_marker_scale; using std_msgs::msg::ColorRGBA; namespace @@ -71,10 +71,10 @@ visualization_msgs::msg::MarkerArray createCorrespondenceMarkerArray( // ID { - auto marker = createDefaultMarker( + auto marker = create_default_marker( "map", now, "detection_area_id", detection_area_reg_elem.id(), - visualization_msgs::msg::Marker::TEXT_VIEW_FACING, createMarkerScale(0.0, 0.0, 1.0), - createMarkerColor(1.0, 1.0, 1.0, 0.999)); + visualization_msgs::msg::Marker::TEXT_VIEW_FACING, create_marker_scale(0.0, 0.0, 1.0), + create_marker_color(1.0, 1.0, 1.0, 0.999)); marker.lifetime = rclcpp::Duration::from_seconds(0.5); for (const auto & detection_area : detection_area_reg_elem.detectionAreas()) { @@ -90,10 +90,10 @@ visualization_msgs::msg::MarkerArray createCorrespondenceMarkerArray( // Polygon { - auto marker = createDefaultMarker( + auto marker = create_default_marker( "map", now, "detection_area_polygon", detection_area_reg_elem.id(), - visualization_msgs::msg::Marker::LINE_LIST, createMarkerScale(0.1, 0.0, 0.0), - createMarkerColor(marker_color.r, marker_color.g, marker_color.b, marker_color.a)); + visualization_msgs::msg::Marker::LINE_LIST, create_marker_scale(0.1, 0.0, 0.0), + create_marker_color(marker_color.r, marker_color.g, marker_color.b, marker_color.a)); marker.lifetime = rclcpp::Duration::from_seconds(0.5); for (const auto & detection_area : detection_area_reg_elem.detectionAreas()) { @@ -116,10 +116,10 @@ visualization_msgs::msg::MarkerArray createCorrespondenceMarkerArray( // Polygon to StopLine { - auto marker = createDefaultMarker( + auto marker = create_default_marker( "map", now, "detection_area_correspondence", detection_area_reg_elem.id(), - visualization_msgs::msg::Marker::LINE_LIST, createMarkerScale(0.1, 0.0, 0.0), - createMarkerColor(marker_color.r, marker_color.g, marker_color.b, marker_color.a)); + visualization_msgs::msg::Marker::LINE_LIST, create_marker_scale(0.1, 0.0, 0.0), + create_marker_color(marker_color.r, marker_color.g, marker_color.b, marker_color.a)); marker.lifetime = rclcpp::Duration::from_seconds(0.5); for (const auto & detection_area : detection_area_reg_elem.detectionAreas()) { @@ -150,10 +150,10 @@ visualization_msgs::msg::MarkerArray DetectionAreaModule::createDebugMarkerArray marker_color.b = 0.0; marker_color.a = 1.0; - appendMarkerArray( + append_marker_array( createCorrespondenceMarkerArray(detection_area_reg_elem_, now, marker_color), &wall_marker); - appendMarkerArray( + append_marker_array( debug::createPointsMarkerArray( debug_data_.obstacle_points, "obstacles", module_id_, now, 0.6, 0.6, 0.6, 1.0, 0.0, 0.0), &wall_marker, now); @@ -164,7 +164,7 @@ visualization_msgs::msg::MarkerArray DetectionAreaModule::createDebugMarkerArray marker_color.b = 0.0; marker_color.a = 1.0; - appendMarkerArray( + append_marker_array( createCorrespondenceMarkerArray(detection_area_reg_elem_, now, marker_color), &wall_marker); } @@ -179,13 +179,13 @@ autoware::motion_utils::VirtualWalls DetectionAreaModule::createVirtualWalls() wall.style = autoware::motion_utils::VirtualWallType::stop; for (const auto & p : debug_data_.stop_poses) { - wall.pose = autoware::universe_utils::calcOffsetPose(p, debug_data_.base_link2front, 0.0, 0.0); + wall.pose = autoware_utils::calc_offset_pose(p, debug_data_.base_link2front, 0.0, 0.0); virtual_walls.push_back(wall); } wall.style = autoware::motion_utils::VirtualWallType::deadline; for (const auto & p : debug_data_.dead_line_poses) { - wall.pose = autoware::universe_utils::calcOffsetPose(p, debug_data_.base_link2front, 0.0, 0.0); + wall.pose = autoware_utils::calc_offset_pose(p, debug_data_.base_link2front, 0.0, 0.0); virtual_walls.push_back(wall); } return virtual_walls; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/manager.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/manager.cpp index 2bdd1decac60f..ecec3f384099e 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/manager.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/manager.cpp @@ -15,8 +15,8 @@ #include "manager.hpp" #include -#include #include +#include #include @@ -30,25 +30,27 @@ namespace autoware::behavior_velocity_planner { -using autoware::universe_utils::getOrDeclareParameter; +using autoware_utils::get_or_declare_parameter; using lanelet::autoware::DetectionArea; DetectionAreaModuleManager::DetectionAreaModuleManager(rclcpp::Node & node) : SceneModuleManagerInterface(node, getModuleName()) { const std::string ns(DetectionAreaModuleManager::getModuleName()); - planner_param_.stop_margin = getOrDeclareParameter(node, ns + ".stop_margin"); - planner_param_.use_dead_line = getOrDeclareParameter(node, ns + ".use_dead_line"); - planner_param_.dead_line_margin = getOrDeclareParameter(node, ns + ".dead_line_margin"); + planner_param_.stop_margin = get_or_declare_parameter(node, ns + ".stop_margin"); + planner_param_.use_dead_line = get_or_declare_parameter(node, ns + ".use_dead_line"); + planner_param_.dead_line_margin = + get_or_declare_parameter(node, ns + ".dead_line_margin"); planner_param_.use_pass_judge_line = - getOrDeclareParameter(node, ns + ".use_pass_judge_line"); - planner_param_.state_clear_time = getOrDeclareParameter(node, ns + ".state_clear_time"); + get_or_declare_parameter(node, ns + ".use_pass_judge_line"); + planner_param_.state_clear_time = + get_or_declare_parameter(node, ns + ".state_clear_time"); planner_param_.hold_stop_margin_distance = - getOrDeclareParameter(node, ns + ".hold_stop_margin_distance"); + get_or_declare_parameter(node, ns + ".hold_stop_margin_distance"); planner_param_.distance_to_judge_over_stop_line = - getOrDeclareParameter(node, ns + ".distance_to_judge_over_stop_line"); + get_or_declare_parameter(node, ns + ".distance_to_judge_over_stop_line"); planner_param_.suppress_pass_judge_when_stopping = - getOrDeclareParameter(node, ns + ".suppress_pass_judge_when_stopping"); + get_or_declare_parameter(node, ns + ".suppress_pass_judge_when_stopping"); } void DetectionAreaModuleManager::launchNewModules( diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/scene.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/scene.cpp index 8641ce19e4ae5..b46b3ca4e36bc 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/scene.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/scene.cpp @@ -37,7 +37,7 @@ DetectionAreaModule::DetectionAreaModule( const lanelet::autoware::DetectionArea & detection_area_reg_elem, const PlannerParam & planner_param, const rclcpp::Logger & logger, const rclcpp::Clock::SharedPtr clock, - const std::shared_ptr time_keeper, + const std::shared_ptr time_keeper, const std::shared_ptr planning_factor_interface) : SceneModuleInterface(module_id, logger, clock, time_keeper, planning_factor_interface), diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/scene.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/scene.hpp index 16df809e7ff62..334cfc1587c92 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/scene.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/scene.hpp @@ -69,7 +69,7 @@ class DetectionAreaModule : public SceneModuleInterface const lanelet::autoware::DetectionArea & detection_area_reg_elem, const PlannerParam & planner_param, const rclcpp::Logger & logger, const rclcpp::Clock::SharedPtr clock, - const std::shared_ptr time_keeper, + const std::shared_ptr time_keeper, const std::shared_ptr planning_factor_interface); diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/utils.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/utils.cpp index 0d721fc81bcee..3d26969cdb88d 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/utils.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/utils.cpp @@ -16,8 +16,8 @@ #include #include -#include #include +#include #include #include @@ -98,7 +98,7 @@ std::pair get_smallest_enclosing_circle( namespace autoware::behavior_velocity_planner::detection_area { -universe_utils::LineString2d get_stop_line_geometry2d( +autoware_utils::LineString2d get_stop_line_geometry2d( const lanelet::autoware::DetectionArea & detection_area, const double extend_length) { const auto stop_line = detection_area.stopLine(); @@ -117,7 +117,7 @@ std::vector get_obstacle_points( (circle.first.y() - p.y) * (circle.first.y() - p.y); if (squared_dist <= circle.second) { if (boost::geometry::within(Point2d{p.x, p.y}, poly.basicPolygon())) { - obstacle_points.push_back(autoware::universe_utils::createPoint(p.x, p.y, p.z)); + obstacle_points.push_back(autoware_utils::create_point(p.x, p.y, p.z)); // get all obstacle point becomes high computation cost so skip if any point is found break; } diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/utils.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/utils.hpp index 3a0ba0172cbd1..67ba6348d5a3b 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/utils.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/utils.hpp @@ -15,8 +15,8 @@ #ifndef UTILS_HPP_ #define UTILS_HPP_ -#include #include +#include #include #include @@ -36,7 +36,7 @@ namespace autoware::behavior_velocity_planner::detection_area /// @param [in] detection_area detection area /// @param [in] extend_length [m] extension length to add on each edge of the stop line /// @return extended stop line -universe_utils::LineString2d get_stop_line_geometry2d( +autoware_utils::LineString2d get_stop_line_geometry2d( const lanelet::autoware::DetectionArea & detection_area, const double extend_length); /// @brief get the obstacle points found inside a detection area diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/debug.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/debug.cpp index 2c9a2c30b11b8..f2fbace68d33f 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/debug.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/debug.cpp @@ -18,7 +18,7 @@ #include #include #include -#include +#include #include @@ -35,10 +35,10 @@ namespace { -using autoware::universe_utils::appendMarkerArray; -using autoware::universe_utils::createMarkerColor; -using autoware::universe_utils::createMarkerOrientation; -using autoware::universe_utils::createMarkerScale; +using autoware_utils::append_marker_array; +using autoware_utils::create_marker_color; +using autoware_utils::create_marker_orientation; +using autoware_utils::create_marker_scale; visualization_msgs::msg::MarkerArray createLaneletPolygonsMarkerArray( const std::vector & polygons, const std::string & ns, @@ -57,9 +57,9 @@ visualization_msgs::msg::MarkerArray createLaneletPolygonsMarkerArray( marker.lifetime = rclcpp::Duration::from_seconds(0.3); marker.type = visualization_msgs::msg::Marker::LINE_STRIP; marker.action = visualization_msgs::msg::Marker::ADD; - marker.pose.orientation = createMarkerOrientation(0, 0, 0, 1.0); - marker.scale = createMarkerScale(0.1, 0.0, 0.0); - marker.color = createMarkerColor(r, g, b, 0.999); + marker.pose.orientation = create_marker_orientation(0, 0, 0, 1.0); + marker.scale = create_marker_scale(0.1, 0.0, 0.0); + marker.color = create_marker_color(r, g, b, 0.999); for (const auto & p : polygon) { geometry_msgs::msg::Point point; point.x = p.x(); @@ -89,9 +89,9 @@ visualization_msgs::msg::MarkerArray createPoseMarkerArray( marker_line.lifetime = rclcpp::Duration::from_seconds(0.3); marker_line.type = visualization_msgs::msg::Marker::LINE_STRIP; marker_line.action = visualization_msgs::msg::Marker::ADD; - marker_line.pose.orientation = createMarkerOrientation(0, 0, 0, 1.0); - marker_line.scale = createMarkerScale(0.2, 0.0, 0.0); - marker_line.color = createMarkerColor(r, g, b, 0.999); + marker_line.pose.orientation = create_marker_orientation(0, 0, 0, 1.0); + marker_line.scale = create_marker_scale(0.2, 0.0, 0.0); + marker_line.color = create_marker_color(r, g, b, 0.999); const double yaw = tf2::getYaw(pose.orientation); @@ -131,7 +131,7 @@ visualization_msgs::msg::MarkerArray createArrowLineMarkerArray( arrow.y = 1.0; arrow.z = 1.0; marker.scale = arrow; - marker.color = createMarkerColor(r, g, b, 0.999); + marker.color = create_marker_color(r, g, b, 0.999); marker.points.push_back(point_start); marker.points.push_back(point_end); @@ -153,7 +153,7 @@ visualization_msgs::msg::MarkerArray createLineMarkerArray( marker.type = visualization_msgs::msg::Marker::LINE_STRIP; marker.action = visualization_msgs::msg::Marker::ADD; marker.scale.x = 0.1; - marker.color = createMarkerColor(r, g, b, 0.999); + marker.color = create_marker_color(r, g, b, 0.999); marker.points.push_back(point_start); marker.points.push_back(point_end); @@ -209,10 +209,10 @@ constexpr std::tuple light_blue() namespace autoware::behavior_velocity_planner { -using autoware::universe_utils::appendMarkerArray; -using autoware::universe_utils::createMarkerColor; -using autoware::universe_utils::createMarkerOrientation; -using autoware::universe_utils::createMarkerScale; +using autoware_utils::append_marker_array; +using autoware_utils::create_marker_color; +using autoware_utils::create_marker_orientation; +using autoware_utils::create_marker_scale; visualization_msgs::msg::MarkerArray IntersectionModule::createDebugMarkerArray() { @@ -221,14 +221,14 @@ visualization_msgs::msg::MarkerArray IntersectionModule::createDebugMarkerArray( const auto now = this->clock_->now(); if (debug_data_.attention_area) { - appendMarkerArray( + append_marker_array( ::createLaneletPolygonsMarkerArray( debug_data_.attention_area.value(), "attention_area", lane_id_, 0.0, 1.0, 0.0), &debug_marker_array); } if (debug_data_.occlusion_attention_area) { - appendMarkerArray( + append_marker_array( ::createLaneletPolygonsMarkerArray( debug_data_.occlusion_attention_area.value(), "occlusion_attention_area", lane_id_, 0.917, 0.568, 0.596), @@ -236,14 +236,14 @@ visualization_msgs::msg::MarkerArray IntersectionModule::createDebugMarkerArray( } if (debug_data_.adjacent_area) { - appendMarkerArray( + append_marker_array( ::createLaneletPolygonsMarkerArray( debug_data_.adjacent_area.value(), "adjacent_area", lane_id_, 0.913, 0.639, 0.149), &debug_marker_array); } if (debug_data_.first_attention_area) { - appendMarkerArray( + append_marker_array( ::createLaneletPolygonsMarkerArray( {debug_data_.first_attention_area.value()}, "first_attention_area", lane_id_, 1, 0.647, 0.0), @@ -251,7 +251,7 @@ visualization_msgs::msg::MarkerArray IntersectionModule::createDebugMarkerArray( } if (debug_data_.second_attention_area) { - appendMarkerArray( + append_marker_array( ::createLaneletPolygonsMarkerArray( {debug_data_.second_attention_area.value()}, "second_attention_area", lane_id_, 1, 0.647, 0.0), @@ -259,7 +259,7 @@ visualization_msgs::msg::MarkerArray IntersectionModule::createDebugMarkerArray( } if (debug_data_.stuck_vehicle_detect_area) { - appendMarkerArray( + append_marker_array( debug::createPolygonMarkerArray( debug_data_.stuck_vehicle_detect_area.value(), "stuck_vehicle_detect_area", lane_id_, now, 0.3, 0.0, 0.0, 0.0, 0.5, 0.5), @@ -267,7 +267,7 @@ visualization_msgs::msg::MarkerArray IntersectionModule::createDebugMarkerArray( } if (debug_data_.yield_stuck_detect_area) { - appendMarkerArray( + append_marker_array( ::createLaneletPolygonsMarkerArray( debug_data_.yield_stuck_detect_area.value(), "yield_stuck_detect_area", lane_id_, 0.6588235, 0.34509, 0.6588235), @@ -275,14 +275,14 @@ visualization_msgs::msg::MarkerArray IntersectionModule::createDebugMarkerArray( } if (debug_data_.ego_lane) { - appendMarkerArray( + append_marker_array( ::createLaneletPolygonsMarkerArray( {debug_data_.ego_lane.value()}, "ego_lane", lane_id_, 1, 0.647, 0.0), &debug_marker_array, now); } if (debug_data_.candidate_collision_ego_lane_polygon) { - appendMarkerArray( + append_marker_array( debug::createPolygonMarkerArray( debug_data_.candidate_collision_ego_lane_polygon.value(), "candidate_collision_ego_lane_polygon", module_id_, now, 0.3, 0.0, 0.0, 0.5, 0.0, 0.0), @@ -294,43 +294,43 @@ visualization_msgs::msg::MarkerArray IntersectionModule::createDebugMarkerArray( static constexpr auto yellow = ::yellow(); static constexpr auto red = ::red(); static constexpr auto light_blue = ::light_blue(); - appendMarkerArray( + append_marker_array( debug::createObjectsMarkerArray( debug_data_.safe_under_traffic_control_targets, "safe_under_traffic_control_targets", module_id_, now, std::get<0>(light_blue), std::get<1>(light_blue), std::get<2>(light_blue)), &debug_marker_array, now); - appendMarkerArray( + append_marker_array( debug::createObjectsMarkerArray( debug_data_.unsafe_targets, "unsafe_targets", module_id_, now, std::get<0>(green), std::get<1>(green), std::get<2>(green)), &debug_marker_array, now); - appendMarkerArray( + append_marker_array( debug::createObjectsMarkerArray( debug_data_.misjudge_targets, "misjudge_targets", module_id_, now, std::get<0>(yellow), std::get<1>(yellow), std::get<2>(yellow)), &debug_marker_array, now); - appendMarkerArray( + append_marker_array( debug::createObjectsMarkerArray( debug_data_.too_late_detect_targets, "too_late_detect_targets", module_id_, now, std::get<0>(red), std::get<1>(red), std::get<2>(red)), &debug_marker_array, now); - appendMarkerArray( + append_marker_array( debug::createObjectsMarkerArray( debug_data_.parked_targets, "parked_targets", module_id_, now, std::get<0>(white), std::get<1>(white), std::get<2>(white)), &debug_marker_array, now); - appendMarkerArray( + append_marker_array( debug::createObjectsMarkerArray( debug_data_.stuck_targets, "stuck_targets", module_id_, now, std::get<0>(white), std::get<1>(white), std::get<2>(white)), &debug_marker_array, now); - appendMarkerArray( + append_marker_array( debug::createObjectsMarkerArray( debug_data_.yield_stuck_targets, "yield_stuck_targets", module_id_, now, std::get<0>(white), std::get<1>(white), std::get<2>(white)), @@ -339,7 +339,7 @@ visualization_msgs::msg::MarkerArray IntersectionModule::createDebugMarkerArray( if (debug_data_.first_pass_judge_wall_pose) { const double r = debug_data_.passed_first_pass_judge ? 1.0 : 0.0; const double g = debug_data_.passed_first_pass_judge ? 0.0 : 1.0; - appendMarkerArray( + append_marker_array( ::createPoseMarkerArray( debug_data_.first_pass_judge_wall_pose.value(), "first_pass_judge_wall_pose", module_id_, r, g, 0.0), @@ -349,7 +349,7 @@ visualization_msgs::msg::MarkerArray IntersectionModule::createDebugMarkerArray( if (debug_data_.second_pass_judge_wall_pose) { const double r = debug_data_.passed_second_pass_judge ? 1.0 : 0.0; const double g = debug_data_.passed_second_pass_judge ? 0.0 : 1.0; - appendMarkerArray( + append_marker_array( ::createPoseMarkerArray( debug_data_.second_pass_judge_wall_pose.value(), "second_pass_judge_wall_pose", module_id_, r, g, 0.0), @@ -358,7 +358,7 @@ visualization_msgs::msg::MarkerArray IntersectionModule::createDebugMarkerArray( for (size_t j = 0; j < debug_data_.occlusion_polygons.size(); ++j) { const auto & p = debug_data_.occlusion_polygons.at(j); - appendMarkerArray( + append_marker_array( debug::createPolygonMarkerArray( p, "occlusion_polygons", lane_id_ + j, now, 0.3, 0.0, 0.0, 1.0, 0.0, 0.0), &debug_marker_array, now); @@ -366,7 +366,7 @@ visualization_msgs::msg::MarkerArray IntersectionModule::createDebugMarkerArray( if (debug_data_.nearest_occlusion_projection) { const auto [point_start, point_end] = debug_data_.nearest_occlusion_projection.value(); - appendMarkerArray( + append_marker_array( ::createArrowLineMarkerArray( point_start, point_end, "nearest_occlusion_projection", lane_id_, 0.5, 0.5, 0.0), &debug_marker_array, now); @@ -382,7 +382,7 @@ visualization_msgs::msg::MarkerArray IntersectionModule::createDebugMarkerArray( geometry_msgs::build().x(p2.x).y(p2.y).z(p2.z)); poly.points.push_back( geometry_msgs::build().x(p3.x).y(p3.y).z(p3.z)); - appendMarkerArray( + append_marker_array( debug::createPolygonMarkerArray( poly, "nearest_occlusion_triangle", lane_id_, now, 0.3, 0.0, 0.0, std::get<0>(color), std::get<1>(color), std::get<2>(color)), @@ -399,7 +399,7 @@ visualization_msgs::msg::MarkerArray IntersectionModule::createDebugMarkerArray( tl_point_point.z = tl_point.z(); const auto tl_color = (color == GREEN) ? green : (color == YELLOW ? yellow : red); const auto [r, g, b] = tl_color; - appendMarkerArray( + append_marker_array( ::createLineMarkerArray( ego.position, tl_point_point, "intersection_traffic_light", lane_id_, r, g, b), &debug_marker_array, now); @@ -466,7 +466,7 @@ visualization_msgs::msg::MarkerArray MergeFromPrivateRoadModule::createDebugMark int32_t uid = autoware::behavior_velocity_planner::planning_utils::bitShift(module_id_); const auto now = this->clock_->now(); if (state == StateMachine::State::STOP) { - appendMarkerArray( + append_marker_array( ::createPoseMarkerArray(debug_data_.stop_point_pose, "stop_point_pose", uid, 1.0, 0.0, 0.0), &debug_marker_array, now); } diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/intersection_lanelets.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/intersection_lanelets.cpp index 943eef03fc6ce..123c763d0b239 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/intersection_lanelets.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/intersection_lanelets.cpp @@ -26,7 +26,7 @@ namespace autoware::behavior_velocity_planner void IntersectionLanelets::update( const bool is_prioritized, const InterpolatedPathInfo & interpolated_path_info, - const autoware::universe_utils::LinearRing2d & footprint, const double vehicle_length, + const autoware_utils::LinearRing2d & footprint, const double vehicle_length, lanelet::routing::RoutingGraphPtr routing_graph_ptr) { is_prioritized_ = is_prioritized; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/intersection_lanelets.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/intersection_lanelets.hpp index f8110475e2782..9c3347ec48b39 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/intersection_lanelets.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/intersection_lanelets.hpp @@ -17,7 +17,7 @@ #include "interpolated_path_info.hpp" -#include +#include #include #include @@ -41,7 +41,7 @@ struct IntersectionLanelets */ void update( const bool is_prioritized, const InterpolatedPathInfo & interpolated_path_info, - const autoware::universe_utils::LinearRing2d & footprint, const double vehicle_length, + const autoware_utils::LinearRing2d & footprint, const double vehicle_length, lanelet::routing::RoutingGraphPtr routing_graph_ptr); const lanelet::ConstLanelets & attention() const diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/manager.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/manager.cpp index eeedb921b293d..fb3ab94ba5b8b 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/manager.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/manager.cpp @@ -16,8 +16,8 @@ #include #include -#include #include +#include #include @@ -30,7 +30,7 @@ namespace autoware::behavior_velocity_planner { -using autoware::universe_utils::getOrDeclareParameter; +using autoware_utils::get_or_declare_parameter; IntersectionModuleManager::IntersectionModuleManager(rclcpp::Node & node) : SceneModuleManagerInterfaceWithRTC( @@ -46,25 +46,25 @@ IntersectionModuleManager::IntersectionModuleManager(rclcpp::Node & node) // common { ip.common.attention_area_length = - getOrDeclareParameter(node, ns + ".common.attention_area_length"); + get_or_declare_parameter(node, ns + ".common.attention_area_length"); ip.common.attention_area_margin = - getOrDeclareParameter(node, ns + ".common.attention_area_margin"); + get_or_declare_parameter(node, ns + ".common.attention_area_margin"); ip.common.attention_area_angle_threshold = - getOrDeclareParameter(node, ns + ".common.attention_area_angle_threshold"); + get_or_declare_parameter(node, ns + ".common.attention_area_angle_threshold"); ip.common.use_intersection_area = - getOrDeclareParameter(node, ns + ".common.use_intersection_area"); + get_or_declare_parameter(node, ns + ".common.use_intersection_area"); ip.common.default_stopline_margin = - getOrDeclareParameter(node, ns + ".common.default_stopline_margin"); + get_or_declare_parameter(node, ns + ".common.default_stopline_margin"); ip.common.stopline_overshoot_margin = - getOrDeclareParameter(node, ns + ".common.stopline_overshoot_margin"); + get_or_declare_parameter(node, ns + ".common.stopline_overshoot_margin"); ip.common.path_interpolation_ds = - getOrDeclareParameter(node, ns + ".common.path_interpolation_ds"); - ip.common.max_accel = getOrDeclareParameter(node, ns + ".common.max_accel"); - ip.common.max_jerk = getOrDeclareParameter(node, ns + ".common.max_jerk"); + get_or_declare_parameter(node, ns + ".common.path_interpolation_ds"); + ip.common.max_accel = get_or_declare_parameter(node, ns + ".common.max_accel"); + ip.common.max_jerk = get_or_declare_parameter(node, ns + ".common.max_jerk"); ip.common.delay_response_time = - getOrDeclareParameter(node, ns + ".common.delay_response_time"); - ip.common.enable_pass_judge_before_default_stopline = - getOrDeclareParameter(node, ns + ".common.enable_pass_judge_before_default_stopline"); + get_or_declare_parameter(node, ns + ".common.delay_response_time"); + ip.common.enable_pass_judge_before_default_stopline = get_or_declare_parameter( + node, ns + ".common.enable_pass_judge_before_default_stopline"); } // stuck @@ -72,39 +72,39 @@ IntersectionModuleManager::IntersectionModuleManager(rclcpp::Node & node) // target_type { ip.stuck_vehicle.target_type.car = - getOrDeclareParameter(node, ns + ".stuck_vehicle.target_type.car"); + get_or_declare_parameter(node, ns + ".stuck_vehicle.target_type.car"); ip.stuck_vehicle.target_type.bus = - getOrDeclareParameter(node, ns + ".stuck_vehicle.target_type.bus"); + get_or_declare_parameter(node, ns + ".stuck_vehicle.target_type.bus"); ip.stuck_vehicle.target_type.truck = - getOrDeclareParameter(node, ns + ".stuck_vehicle.target_type.truck"); + get_or_declare_parameter(node, ns + ".stuck_vehicle.target_type.truck"); ip.stuck_vehicle.target_type.trailer = - getOrDeclareParameter(node, ns + ".stuck_vehicle.target_type.trailer"); + get_or_declare_parameter(node, ns + ".stuck_vehicle.target_type.trailer"); ip.stuck_vehicle.target_type.motorcycle = - getOrDeclareParameter(node, ns + ".stuck_vehicle.target_type.motorcycle"); + get_or_declare_parameter(node, ns + ".stuck_vehicle.target_type.motorcycle"); ip.stuck_vehicle.target_type.bicycle = - getOrDeclareParameter(node, ns + ".stuck_vehicle.target_type.bicycle"); + get_or_declare_parameter(node, ns + ".stuck_vehicle.target_type.bicycle"); ip.stuck_vehicle.target_type.unknown = - getOrDeclareParameter(node, ns + ".stuck_vehicle.target_type.unknown"); + get_or_declare_parameter(node, ns + ".stuck_vehicle.target_type.unknown"); } // turn_direction { ip.stuck_vehicle.turn_direction.left = - getOrDeclareParameter(node, ns + ".stuck_vehicle.turn_direction.left"); + get_or_declare_parameter(node, ns + ".stuck_vehicle.turn_direction.left"); ip.stuck_vehicle.turn_direction.right = - getOrDeclareParameter(node, ns + ".stuck_vehicle.turn_direction.right"); + get_or_declare_parameter(node, ns + ".stuck_vehicle.turn_direction.right"); ip.stuck_vehicle.turn_direction.straight = - getOrDeclareParameter(node, ns + ".stuck_vehicle.turn_direction.straight"); + get_or_declare_parameter(node, ns + ".stuck_vehicle.turn_direction.straight"); } ip.stuck_vehicle.use_stuck_stopline = - getOrDeclareParameter(node, ns + ".stuck_vehicle.use_stuck_stopline"); + get_or_declare_parameter(node, ns + ".stuck_vehicle.use_stuck_stopline"); ip.stuck_vehicle.stuck_vehicle_detect_dist = - getOrDeclareParameter(node, ns + ".stuck_vehicle.stuck_vehicle_detect_dist"); - ip.stuck_vehicle.stuck_vehicle_velocity_threshold = - getOrDeclareParameter(node, ns + ".stuck_vehicle.stuck_vehicle_velocity_threshold"); + get_or_declare_parameter(node, ns + ".stuck_vehicle.stuck_vehicle_detect_dist"); + ip.stuck_vehicle.stuck_vehicle_velocity_threshold = get_or_declare_parameter( + node, ns + ".stuck_vehicle.stuck_vehicle_velocity_threshold"); ip.stuck_vehicle.disable_against_private_lane = - getOrDeclareParameter(node, ns + ".stuck_vehicle.disable_against_private_lane"); + get_or_declare_parameter(node, ns + ".stuck_vehicle.disable_against_private_lane"); } // yield_stuck @@ -112,139 +112,140 @@ IntersectionModuleManager::IntersectionModuleManager(rclcpp::Node & node) // target_type { ip.yield_stuck.target_type.car = - getOrDeclareParameter(node, ns + ".yield_stuck.target_type.car"); + get_or_declare_parameter(node, ns + ".yield_stuck.target_type.car"); ip.yield_stuck.target_type.bus = - getOrDeclareParameter(node, ns + ".yield_stuck.target_type.bus"); + get_or_declare_parameter(node, ns + ".yield_stuck.target_type.bus"); ip.yield_stuck.target_type.truck = - getOrDeclareParameter(node, ns + ".yield_stuck.target_type.truck"); + get_or_declare_parameter(node, ns + ".yield_stuck.target_type.truck"); ip.yield_stuck.target_type.trailer = - getOrDeclareParameter(node, ns + ".yield_stuck.target_type.trailer"); + get_or_declare_parameter(node, ns + ".yield_stuck.target_type.trailer"); ip.yield_stuck.target_type.motorcycle = - getOrDeclareParameter(node, ns + ".yield_stuck.target_type.motorcycle"); + get_or_declare_parameter(node, ns + ".yield_stuck.target_type.motorcycle"); ip.yield_stuck.target_type.bicycle = - getOrDeclareParameter(node, ns + ".yield_stuck.target_type.bicycle"); + get_or_declare_parameter(node, ns + ".yield_stuck.target_type.bicycle"); ip.yield_stuck.target_type.unknown = - getOrDeclareParameter(node, ns + ".yield_stuck.target_type.unknown"); + get_or_declare_parameter(node, ns + ".yield_stuck.target_type.unknown"); } // turn_direction { ip.yield_stuck.turn_direction.left = - getOrDeclareParameter(node, ns + ".yield_stuck.turn_direction.left"); + get_or_declare_parameter(node, ns + ".yield_stuck.turn_direction.left"); ip.yield_stuck.turn_direction.right = - getOrDeclareParameter(node, ns + ".yield_stuck.turn_direction.right"); + get_or_declare_parameter(node, ns + ".yield_stuck.turn_direction.right"); ip.yield_stuck.turn_direction.straight = - getOrDeclareParameter(node, ns + ".yield_stuck.turn_direction.straight"); + get_or_declare_parameter(node, ns + ".yield_stuck.turn_direction.straight"); } ip.yield_stuck.distance_threshold = - getOrDeclareParameter(node, ns + ".yield_stuck.distance_threshold"); + get_or_declare_parameter(node, ns + ".yield_stuck.distance_threshold"); } // collision_detection { - ip.collision_detection.consider_wrong_direction_vehicle = getOrDeclareParameter( + ip.collision_detection.consider_wrong_direction_vehicle = get_or_declare_parameter( node, ns + ".collision_detection.consider_wrong_direction_vehicle"); - ip.collision_detection.collision_detection_hold_time = getOrDeclareParameter( + ip.collision_detection.collision_detection_hold_time = get_or_declare_parameter( node, ns + ".collision_detection.collision_detection_hold_time"); - ip.collision_detection.min_predicted_path_confidence = getOrDeclareParameter( + ip.collision_detection.min_predicted_path_confidence = get_or_declare_parameter( node, ns + ".collision_detection.min_predicted_path_confidence"); // target_type { ip.collision_detection.target_type.car = - getOrDeclareParameter(node, ns + ".collision_detection.target_type.car"); + get_or_declare_parameter(node, ns + ".collision_detection.target_type.car"); ip.collision_detection.target_type.bus = - getOrDeclareParameter(node, ns + ".collision_detection.target_type.bus"); + get_or_declare_parameter(node, ns + ".collision_detection.target_type.bus"); ip.collision_detection.target_type.truck = - getOrDeclareParameter(node, ns + ".collision_detection.target_type.truck"); + get_or_declare_parameter(node, ns + ".collision_detection.target_type.truck"); ip.collision_detection.target_type.trailer = - getOrDeclareParameter(node, ns + ".collision_detection.target_type.trailer"); + get_or_declare_parameter(node, ns + ".collision_detection.target_type.trailer"); ip.collision_detection.target_type.motorcycle = - getOrDeclareParameter(node, ns + ".collision_detection.target_type.motorcycle"); + get_or_declare_parameter(node, ns + ".collision_detection.target_type.motorcycle"); ip.collision_detection.target_type.bicycle = - getOrDeclareParameter(node, ns + ".collision_detection.target_type.bicycle"); + get_or_declare_parameter(node, ns + ".collision_detection.target_type.bicycle"); ip.collision_detection.target_type.unknown = - getOrDeclareParameter(node, ns + ".collision_detection.target_type.unknown"); + get_or_declare_parameter(node, ns + ".collision_detection.target_type.unknown"); } // velocity_profile { - ip.collision_detection.velocity_profile.use_upstream = getOrDeclareParameter( + ip.collision_detection.velocity_profile.use_upstream = get_or_declare_parameter( node, ns + ".collision_detection.velocity_profile.use_upstream"); ip.collision_detection.velocity_profile.minimum_upstream_velocity = - getOrDeclareParameter( + get_or_declare_parameter( node, ns + ".collision_detection.velocity_profile.minimum_upstream_velocity"); - ip.collision_detection.velocity_profile.default_velocity = getOrDeclareParameter( + ip.collision_detection.velocity_profile.default_velocity = get_or_declare_parameter( node, ns + ".collision_detection.velocity_profile.default_velocity"); ip.collision_detection.velocity_profile.minimum_default_velocity = - getOrDeclareParameter( + get_or_declare_parameter( node, ns + ".collision_detection.velocity_profile.minimum_default_velocity"); } // fully_prioritized { ip.collision_detection.fully_prioritized.collision_start_margin_time = - getOrDeclareParameter( + get_or_declare_parameter( node, ns + ".collision_detection.fully_prioritized.collision_start_margin_time"); ip.collision_detection.fully_prioritized.collision_end_margin_time = - getOrDeclareParameter( + get_or_declare_parameter( node, ns + ".collision_detection.fully_prioritized.collision_end_margin_time"); } // partially_prioritized { ip.collision_detection.partially_prioritized.collision_start_margin_time = - getOrDeclareParameter( + get_or_declare_parameter( node, ns + ".collision_detection.partially_prioritized.collision_start_margin_time"); ip.collision_detection.partially_prioritized.collision_end_margin_time = - getOrDeclareParameter( + get_or_declare_parameter( node, ns + ".collision_detection.partially_prioritized.collision_end_margin_time"); } // not_prioritized { ip.collision_detection.not_prioritized.collision_start_margin_time = - getOrDeclareParameter( + get_or_declare_parameter( node, ns + ".collision_detection.not_prioritized.collision_start_margin_time"); ip.collision_detection.not_prioritized.collision_end_margin_time = - getOrDeclareParameter( + get_or_declare_parameter( node, ns + ".collision_detection.not_prioritized.collision_end_margin_time"); } // yield_on_green_traffic_light { ip.collision_detection.yield_on_green_traffic_light.distance_to_assigned_lanelet_start = - getOrDeclareParameter( + get_or_declare_parameter( node, ns + ".collision_detection.yield_on_green_traffic_light.distance_to_assigned_lanelet_start"); - ip.collision_detection.yield_on_green_traffic_light.duration = getOrDeclareParameter( - node, ns + ".collision_detection.yield_on_green_traffic_light.duration"); + ip.collision_detection.yield_on_green_traffic_light.duration = + get_or_declare_parameter( + node, ns + ".collision_detection.yield_on_green_traffic_light.duration"); ip.collision_detection.yield_on_green_traffic_light.object_dist_to_stopline = - getOrDeclareParameter( + get_or_declare_parameter( node, ns + ".collision_detection.yield_on_green_traffic_light.object_dist_to_stopline"); } // ignore_on_amber_traffic_light, ignore_on_red_traffic_light { ip.collision_detection.ignore_on_amber_traffic_light.object_expected_deceleration.car = - getOrDeclareParameter( + get_or_declare_parameter( node, ns + ".collision_detection.ignore_on_amber_traffic_light.object_expected_deceleration.car"); ip.collision_detection.ignore_on_amber_traffic_light.object_expected_deceleration.bike = - getOrDeclareParameter( + get_or_declare_parameter( node, ns + ".collision_detection.ignore_on_amber_traffic_light.object_expected_deceleration.bike"); ip.collision_detection.ignore_on_red_traffic_light.object_margin_to_path = - getOrDeclareParameter( + get_or_declare_parameter( node, ns + ".collision_detection.ignore_on_red_traffic_light.object_margin_to_path"); } ip.collision_detection.avoid_collision_by_acceleration.object_time_margin_to_collision_point = - getOrDeclareParameter( + get_or_declare_parameter( node, ns + ".collision_detection.avoid_collision_by_acceleration.object_time_margin_to_" "collision_point"); @@ -252,48 +253,48 @@ IntersectionModuleManager::IntersectionModuleManager(rclcpp::Node & node) // occlusion { - ip.occlusion.enable = getOrDeclareParameter(node, ns + ".occlusion.enable"); + ip.occlusion.enable = get_or_declare_parameter(node, ns + ".occlusion.enable"); ip.occlusion.occlusion_attention_area_length = - getOrDeclareParameter(node, ns + ".occlusion.occlusion_attention_area_length"); + get_or_declare_parameter(node, ns + ".occlusion.occlusion_attention_area_length"); ip.occlusion.free_space_max = - getOrDeclareParameter(node, ns + ".occlusion.free_space_max"); - ip.occlusion.occupied_min = getOrDeclareParameter(node, ns + ".occlusion.occupied_min"); + get_or_declare_parameter(node, ns + ".occlusion.free_space_max"); + ip.occlusion.occupied_min = get_or_declare_parameter(node, ns + ".occlusion.occupied_min"); ip.occlusion.denoise_kernel = - getOrDeclareParameter(node, ns + ".occlusion.denoise_kernel"); - ip.occlusion.attention_lane_crop_curvature_threshold = getOrDeclareParameter( + get_or_declare_parameter(node, ns + ".occlusion.denoise_kernel"); + ip.occlusion.attention_lane_crop_curvature_threshold = get_or_declare_parameter( node, ns + ".occlusion.attention_lane_crop_curvature_threshold"); - ip.occlusion.attention_lane_curvature_calculation_ds = getOrDeclareParameter( + ip.occlusion.attention_lane_curvature_calculation_ds = get_or_declare_parameter( node, ns + ".occlusion.attention_lane_curvature_calculation_ds"); // creep_during_peeking { ip.occlusion.creep_during_peeking.enable = - getOrDeclareParameter(node, ns + ".occlusion.creep_during_peeking.enable"); - ip.occlusion.creep_during_peeking.creep_velocity = - getOrDeclareParameter(node, ns + ".occlusion.creep_during_peeking.creep_velocity"); + get_or_declare_parameter(node, ns + ".occlusion.creep_during_peeking.enable"); + ip.occlusion.creep_during_peeking.creep_velocity = get_or_declare_parameter( + node, ns + ".occlusion.creep_during_peeking.creep_velocity"); } ip.occlusion.peeking_offset = - getOrDeclareParameter(node, ns + ".occlusion.peeking_offset"); - ip.occlusion.occlusion_required_clearance_distance = - getOrDeclareParameter(node, ns + ".occlusion.occlusion_required_clearance_distance"); + get_or_declare_parameter(node, ns + ".occlusion.peeking_offset"); + ip.occlusion.occlusion_required_clearance_distance = get_or_declare_parameter( + node, ns + ".occlusion.occlusion_required_clearance_distance"); ip.occlusion.possible_object_bbox = - getOrDeclareParameter>(node, ns + ".occlusion.possible_object_bbox"); - ip.occlusion.ignore_parked_vehicle_speed_threshold = - getOrDeclareParameter(node, ns + ".occlusion.ignore_parked_vehicle_speed_threshold"); + get_or_declare_parameter>(node, ns + ".occlusion.possible_object_bbox"); + ip.occlusion.ignore_parked_vehicle_speed_threshold = get_or_declare_parameter( + node, ns + ".occlusion.ignore_parked_vehicle_speed_threshold"); ip.occlusion.occlusion_detection_hold_time = - getOrDeclareParameter(node, ns + ".occlusion.occlusion_detection_hold_time"); + get_or_declare_parameter(node, ns + ".occlusion.occlusion_detection_hold_time"); ip.occlusion.temporal_stop_time_before_peeking = - getOrDeclareParameter(node, ns + ".occlusion.temporal_stop_time_before_peeking"); + get_or_declare_parameter(node, ns + ".occlusion.temporal_stop_time_before_peeking"); ip.occlusion.temporal_stop_before_attention_area = - getOrDeclareParameter(node, ns + ".occlusion.temporal_stop_before_attention_area"); - ip.occlusion.creep_velocity_without_traffic_light = - getOrDeclareParameter(node, ns + ".occlusion.creep_velocity_without_traffic_light"); - ip.occlusion.static_occlusion_with_traffic_light_timeout = getOrDeclareParameter( + get_or_declare_parameter(node, ns + ".occlusion.temporal_stop_before_attention_area"); + ip.occlusion.creep_velocity_without_traffic_light = get_or_declare_parameter( + node, ns + ".occlusion.creep_velocity_without_traffic_light"); + ip.occlusion.static_occlusion_with_traffic_light_timeout = get_or_declare_parameter( node, ns + ".occlusion.static_occlusion_with_traffic_light_timeout"); } - ip.debug.ttc = getOrDeclareParameter>(node, ns + ".debug.ttc"); + ip.debug.ttc = get_or_declare_parameter>(node, ns + ".debug.ttc"); decision_state_pub_ = node.create_publisher("~/debug/intersection/decision_state", 1); @@ -340,7 +341,7 @@ void IntersectionModuleManager::launchNewModules( module_id, lane_id, planner_data_, intersection_param_, associative_ids, turn_direction, has_traffic_light, node_, logger_.get_child("intersection_module"), clock_, time_keeper_, planning_factor_interface_); - generateUUID(module_id); + generate_uuid(module_id); /* set RTC status as non_occluded status initially */ const UUID uuid = getUUID(new_module->getModuleId()); const auto occlusion_uuid = new_module->getOcclusionUUID(); @@ -474,13 +475,14 @@ MergeFromPrivateModuleManager::MergeFromPrivateModuleManager(rclcpp::Node & node { const std::string ns(MergeFromPrivateModuleManager::getModuleName()); auto & mp = merge_from_private_area_param_; - mp.stop_duration_sec = getOrDeclareParameter(node, ns + ".stop_duration_sec"); + mp.stop_duration_sec = get_or_declare_parameter(node, ns + ".stop_duration_sec"); mp.attention_area_length = node.get_parameter("intersection.common.attention_area_length").as_double(); - mp.stopline_margin = getOrDeclareParameter(node, ns + ".stopline_margin"); + mp.stopline_margin = get_or_declare_parameter(node, ns + ".stopline_margin"); mp.path_interpolation_ds = node.get_parameter("intersection.common.path_interpolation_ds").as_double(); - mp.stop_distance_threshold = getOrDeclareParameter(node, ns + ".stop_distance_threshold"); + mp.stop_distance_threshold = + get_or_declare_parameter(node, ns + ".stop_distance_threshold"); } void MergeFromPrivateModuleManager::launchNewModules( diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/object_manager.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/object_manager.cpp index 9fd5ba4ab7d5c..6c363eb46f93a 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/object_manager.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/object_manager.cpp @@ -14,9 +14,9 @@ #include "object_manager.hpp" -#include -#include // for toPolygon2d #include +#include +#include // for toPolygon2d #include #include @@ -40,15 +40,15 @@ std::string to_string(const unique_identifier_msgs::msg::UUID & uuid) return ss.str(); } -autoware::universe_utils::Polygon2d createOneStepPolygon( +autoware_utils::Polygon2d createOneStepPolygon( const geometry_msgs::msg::Pose & prev_pose, const geometry_msgs::msg::Pose & next_pose, const autoware_perception_msgs::msg::Shape & shape) { namespace bg = boost::geometry; - const auto prev_poly = autoware::universe_utils::toPolygon2d(prev_pose, shape); - const auto next_poly = autoware::universe_utils::toPolygon2d(next_pose, shape); + const auto prev_poly = autoware_utils::to_polygon2d(prev_pose, shape); + const auto next_poly = autoware_utils::to_polygon2d(next_pose, shape); - autoware::universe_utils::Polygon2d one_step_poly; + autoware_utils::Polygon2d one_step_poly; for (const auto & point : prev_poly.outer()) { one_step_poly.outer().push_back(point); } @@ -58,7 +58,7 @@ autoware::universe_utils::Polygon2d createOneStepPolygon( bg::correct(one_step_poly); - autoware::universe_utils::Polygon2d convex_one_step_poly; + autoware_utils::Polygon2d convex_one_step_poly; bg::convex_hull(one_step_poly, convex_one_step_poly); return convex_one_step_poly; @@ -162,13 +162,13 @@ bool ObjectInfo::can_stop_before_ego_lane( const auto stopline = stopline_opt_.value(); const auto stopline_p1 = stopline.front(); const auto stopline_p2 = stopline.back(); - const autoware::universe_utils::Point2d stopline_mid{ + const autoware_utils::Point2d stopline_mid{ (stopline_p1.x() + stopline_p2.x()) / 2.0, (stopline_p1.y() + stopline_p2.y()) / 2.0}; const auto attention_lane_end = attention_lanelet.centerline().back(); - const autoware::universe_utils::LineString2d attention_lane_later_part( - {autoware::universe_utils::Point2d{stopline_mid.x(), stopline_mid.y()}, - autoware::universe_utils::Point2d{attention_lane_end.x(), attention_lane_end.y()}}); - std::vector ego_collision_points; + const autoware_utils::LineString2d attention_lane_later_part( + {autoware_utils::Point2d{stopline_mid.x(), stopline_mid.y()}, + autoware_utils::Point2d{attention_lane_end.x(), attention_lane_end.y()}}); + std::vector ego_collision_points; bg::intersection( attention_lane_later_part, ego_lane.centerline2d().basicLineString(), ego_collision_points); if (ego_collision_points.empty()) { diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection.cpp index 1ce7da981f516..7e3faa8f30ff7 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection.cpp @@ -19,11 +19,11 @@ #include // for toGeomPoly #include #include -#include // for toPolygon2d -#include -#include #include #include +#include // for toPolygon2d +#include +#include #include @@ -50,7 +50,7 @@ IntersectionModule::IntersectionModule( const PlannerParam & planner_param, const std::set & associative_ids, const std::string & turn_direction, const bool has_traffic_light, rclcpp::Node & node, const rclcpp::Logger logger, const rclcpp::Clock::SharedPtr clock, - const std::shared_ptr time_keeper, + const std::shared_ptr time_keeper, const std::shared_ptr planning_factor_interface) : SceneModuleInterfaceWithRTC(module_id, logger, clock, time_keeper, planning_factor_interface), @@ -59,7 +59,7 @@ IntersectionModule::IntersectionModule( associative_ids_(associative_ids), turn_direction_(turn_direction), has_traffic_light_(has_traffic_light), - occlusion_uuid_(autoware::universe_utils::generateUUID()) + occlusion_uuid_(autoware_utils::generate_uuid()) { { collision_state_machine_.setMarginTime( diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection.hpp index 19eb60c1e7361..db93497df353c 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection.hpp @@ -306,7 +306,7 @@ class IntersectionModule : public SceneModuleInterfaceWithRTC const PlannerParam & planner_param, const std::set & associative_ids, const std::string & turn_direction, const bool has_traffic_light, rclcpp::Node & node, const rclcpp::Logger logger, const rclcpp::Clock::SharedPtr clock, - const std::shared_ptr time_keeper, + const std::shared_ptr time_keeper, const std::shared_ptr planning_factor_interface); diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection_collision.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection_collision.cpp index f727fe3d66951..94928c68e815a 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection_collision.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection_collision.cpp @@ -18,9 +18,9 @@ #include // for toGeomPoly #include // for smoothPath #include -#include // for toPolygon2d -#include #include +#include // for toPolygon2d +#include #include #include @@ -118,8 +118,7 @@ void IntersectionModule::updateObjectInfoManagerArea() if (!intersection_area) { return false; } - return bg::within( - autoware::universe_utils::Point2d{obj_pos.x, obj_pos.y}, intersection_area.value()); + return bg::within(autoware_utils::Point2d{obj_pos.x, obj_pos.y}, intersection_area.value()); }(); std::optional attention_lanelet{std::nullopt}; std::optional stopline{std::nullopt}; @@ -320,8 +319,7 @@ void IntersectionModule::updateObjectInfoManagerCollision( bool collision_detected = false; for (auto i = begin; i <= end; ++i) { if (bg::intersects( - polygon, - autoware::universe_utils::toPolygon2d(object_path.at(i), predicted_object.shape))) { + polygon, autoware_utils::to_polygon2d(object_path.at(i), predicted_object.shape))) { collision_detected = true; break; } @@ -500,13 +498,13 @@ std::string IntersectionModule::generateDetectionBlameDiagnosis( "judge line({2} seconds before from now) given the estimated current velocity {3}[m/s]. " "ego was at x = {4}, y = {5} when it passed the 1st pass judge line so it is the fault " "of detection side that failed to detect around {6}[m] range at that time.\n", - past_position.x, // 0 - past_position.y, // 1 - time_diff, // 2 - object_info->observed_velocity(), // 3 - passed_1st_judge_line_pose.position.x, // 4 - passed_1st_judge_line_pose.position.y, // 5 - autoware::universe_utils::calcDistance2d(passed_1st_judge_line_pose, past_position) // 6 + past_position.x, // 0 + past_position.y, // 1 + time_diff, // 2 + object_info->observed_velocity(), // 3 + passed_1st_judge_line_pose.position.x, // 4 + passed_1st_judge_line_pose.position.y, // 5 + autoware_utils::calc_distance2d(passed_1st_judge_line_pose, past_position) // 6 ); } } @@ -538,13 +536,13 @@ std::string IntersectionModule::generateDetectionBlameDiagnosis( "judge line({2} seconds before from now) given the estimated current velocity {3}[m/s]. " "ego was at x = {4}, y = {5} when it passed the 2nd pass judge line so it is the fault " "of detection side that failed to detect around {6}[m] range at that time.\n", - past_position.x, // 0 - past_position.y, // 1 - time_diff, // 2 - object_info->observed_velocity(), // 3 - passed_2nd_judge_line_pose.position.x, // 4 - passed_2nd_judge_line_pose.position.y, // 5 - autoware::universe_utils::calcDistance2d(passed_2nd_judge_line_pose, past_position) // 6 + past_position.x, // 0 + past_position.y, // 1 + time_diff, // 2 + object_info->observed_velocity(), // 3 + passed_2nd_judge_line_pose.position.x, // 4 + passed_2nd_judge_line_pose.position.y, // 5 + autoware_utils::calc_distance2d(passed_2nd_judge_line_pose, past_position) // 6 ); } } @@ -624,10 +622,10 @@ std::string IntersectionModule::generateEgoRiskEvasiveDiagnosis( const auto [begin, end] = unsafe_interval.interval_position; const auto &p1 = unsafe_interval.path.at(begin).position, p2 = unsafe_interval.path.at(end).position; - const auto collision_pos = autoware::universe_utils::createPoint( - (p1.x + p2.x) / 2, (p1.y + p2.y) / 2, (p1.z + p2.z) / 2); + const auto collision_pos = + autoware_utils::create_point((p1.x + p2.x) / 2, (p1.y + p2.y) / 2, (p1.z + p2.z) / 2); const auto object_dist_to_margin_point = - autoware::universe_utils::calcDistance2d( + autoware_utils::calc_distance2d( object_info->predicted_object().kinematics.initial_pose_with_covariance.pose.position, collision_pos) - planner_param_.collision_detection.avoid_collision_by_acceleration @@ -790,8 +788,7 @@ std::optional IntersectionModule::checkAngleForTargetLanelets( } const double ll_angle = lanelet::utils::getLaneletAngle(ll, pose.position); const double pose_angle = tf2::getYaw(pose.orientation); - const double angle_diff = - autoware::universe_utils::normalizeRadian(ll_angle - pose_angle, -M_PI); + const double angle_diff = autoware_utils::normalize_radian(ll_angle - pose_angle, -M_PI); if (consider_wrong_direction_vehicle) { if (std::fabs(angle_diff) > 1.57 || std::fabs(angle_diff) < detection_area_angle_thr) { return std::make_optional(i); @@ -929,7 +926,7 @@ IntersectionModule::TimeDistanceArray IntersectionModule::calcIntersectionPassin const auto & p1 = smoothed_reference_path.points.at(i); const auto & p2 = smoothed_reference_path.points.at(i + 1); - const double dist = autoware::universe_utils::calcDistance2d(p1, p2); + const double dist = autoware_utils::calc_distance2d(p1, p2); dist_sum += dist; // use average velocity between p1 and p2 diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection_occlusion.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection_occlusion.cpp index 6ec447bdea0af..64ebc766921f8 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection_occlusion.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection_occlusion.cpp @@ -15,8 +15,8 @@ #include "scene_intersection.hpp" #include "util.hpp" -#include // for toPolygon2d -#include +#include // for toPolygon2d +#include #include #include @@ -156,7 +156,7 @@ IntersectionModule::OcclusionType IntersectionModule::detectOcclusion( auto findCommonCvPolygons = [&](const auto & area2d, std::vector> & cv_polygons) -> void { - autoware::universe_utils::Polygon2d area2d_poly; + autoware_utils::Polygon2d area2d_poly; for (const auto & p : area2d) { area2d_poly.outer().emplace_back(p.x(), p.y()); } @@ -245,7 +245,7 @@ IntersectionModule::OcclusionType IntersectionModule::detectOcclusion( std::vector> blocking_polygons; for (const auto & blocking_attention_object_info : blocking_attention_objects) { const Polygon2d obj_poly = - autoware::universe_utils::toPolygon2d(blocking_attention_object_info->predicted_object()); + autoware_utils::to_polygon2d(blocking_attention_object_info->predicted_object()); findCommonCvPolygons(obj_poly.outer(), blocking_polygons); } for (const auto & blocking_polygon : blocking_polygons) { @@ -326,8 +326,7 @@ IntersectionModule::OcclusionType IntersectionModule::detectOcclusion( for (auto i = lane_start_idx + 1; i <= lane_end_idx; i++) { const auto path_linestring_point = path_ip.points.at(i).point.pose.position; if ( - autoware::universe_utils::calcDistance2d( - prev_path_linestring_point, path_linestring_point) < + autoware_utils::calc_distance2d(prev_path_linestring_point, path_linestring_point) < 1.0 /* rough tick for computational cost */) { continue; } @@ -408,9 +407,9 @@ IntersectionModule::OcclusionType IntersectionModule::detectOcclusion( min_dist = acc_dist; nearest_occlusion_point = { division_index, std::distance(division.begin(), point_it), - autoware::universe_utils::createPoint(point_it->x(), point_it->y(), origin.z), - autoware::universe_utils::createPoint(projection_it->x(), projection_it->y(), origin.z), - autoware::universe_utils::createPoint( + autoware_utils::create_point(point_it->x(), point_it->y(), origin.z), + autoware_utils::create_point(projection_it->x(), projection_it->y(), origin.z), + autoware_utils::create_point( projection_it->x(), projection_it->y(), origin.z) /* initialize with projection point at first*/}; found_min_dist_for_this_division = true; @@ -419,7 +418,7 @@ IntersectionModule::OcclusionType IntersectionModule::detectOcclusion( // division previously in this iteration, and the iterated cells are still OCCLUDED since // then nearest_occlusion_point.visible_end = - autoware::universe_utils::createPoint(point_it->x(), point_it->y(), origin.z); + autoware_utils::create_point(point_it->x(), point_it->y(), origin.z); } } is_prev_occluded = (pixel == OCCLUDED); @@ -442,8 +441,7 @@ IntersectionModule::OcclusionType IntersectionModule::detectOcclusion( nearest_occlusion_point.visible_end.x, nearest_occlusion_point.visible_end.y); bg::correct(ego_occlusion_triangle); for (const auto & attention_object_info : object_info_manager_.allObjects()) { - const auto obj_poly = - autoware::universe_utils::toPolygon2d(attention_object_info->predicted_object()); + const auto obj_poly = autoware_utils::to_polygon2d(attention_object_info->predicted_object()); if (bg::intersects(obj_poly, ego_occlusion_triangle)) { debug_data_.static_occlusion = false; return DynamicallyOccluded{min_dist}; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection_prepare_data.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection_prepare_data.cpp index 25355863f8bd9..700c979009fe7 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection_prepare_data.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection_prepare_data.cpp @@ -19,7 +19,6 @@ #include // for planning_utils:: #include #include -#include #include // for lanelet::autoware::RoadMarking #include #include @@ -83,7 +82,7 @@ lanelet::ConstLanelet generatePathLanelet( for (size_t i = start_idx; i <= end_idx; ++i) { const auto & p = path.points.at(i).point.pose; const auto & p_prev = path.points.at(prev_idx).point.pose; - if (i != start_idx && autoware::universe_utils::calcDistance2d(p_prev, p) < interval) { + if (i != start_idx && autoware_utils::calc_distance2d(p_prev, p) < interval) { continue; } prev_idx = i; @@ -286,7 +285,7 @@ Result IntersectionModule::prepare const bool approached_assigned_lane = autoware::motion_utils::calcSignedArcLength( path->points, closest_idx, - autoware::universe_utils::createPoint( + autoware_utils::create_point( assigned_lane_begin_point.x(), assigned_lane_begin_point.y(), assigned_lane_begin_point.z())) < planner_param_.collision_detection.yield_on_green_traffic_light @@ -390,8 +389,8 @@ std::optional IntersectionModule::generateIntersectionSto std::optional first_footprint_attention_centerline_ip_opt = std::nullopt; for (auto i = std::get<0>(lane_interval_ip); i < std::get<1>(lane_interval_ip); ++i) { const auto & base_pose = path_ip.points.at(i).point.pose; - const auto path_footprint = autoware::universe_utils::transformVector( - local_footprint, autoware::universe_utils::pose2transform(base_pose)); + const auto path_footprint = + autoware_utils::transform_vector(local_footprint, autoware_utils::pose2transform(base_pose)); if (bg::intersects(path_footprint, first_attention_lane_centerline.basicLineString())) { // NOTE: maybe consideration of braking dist is necessary first_footprint_attention_centerline_ip_opt = i; @@ -432,8 +431,8 @@ std::optional IntersectionModule::generateIntersectionSto // NOTE: if footprints[0] is already inside the attention area, invalid { const auto & base_pose0 = path_ip.points.at(default_stopline_ip).point.pose; - const auto path_footprint0 = autoware::universe_utils::transformVector( - local_footprint, autoware::universe_utils::pose2transform(base_pose0)); + const auto path_footprint0 = + autoware_utils::transform_vector(local_footprint, autoware_utils::pose2transform(base_pose0)); if (bg::intersects( path_footprint0, lanelet::utils::to2D(first_attention_area).basicPolygon())) { occlusion_peeking_line_valid = false; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection_stuck.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection_stuck.cpp index 87619ea121aff..62d713dff960a 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection_stuck.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection_stuck.cpp @@ -17,8 +17,8 @@ #include // for toGeomPoly #include -#include #include +#include #include #include @@ -295,7 +295,7 @@ bool IntersectionModule::checkStuckVehicleInIntersection(const PathLanelets & pa } // check if the footprint is in the stuck detect area - const auto obj_footprint = autoware::universe_utils::toPolygon2d(object); + const auto obj_footprint = autoware_utils::to_polygon2d(object); // NOTE: in order not to stop too much const bool is_in_stuck_area = bg::within( to_bg2d(object.kinematics.initial_pose_with_covariance.pose.position), diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_merge_from_private_road.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_merge_from_private_road.cpp index 458ebd37e93f7..f968e75e248bb 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_merge_from_private_road.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_merge_from_private_road.cpp @@ -19,9 +19,9 @@ #include #include #include -#include #include #include +#include #include #include @@ -43,7 +43,7 @@ MergeFromPrivateRoadModule::MergeFromPrivateRoadModule( [[maybe_unused]] std::shared_ptr planner_data, const PlannerParam & planner_param, const std::set & associative_ids, const rclcpp::Logger logger, const rclcpp::Clock::SharedPtr clock, - const std::shared_ptr time_keeper, + const std::shared_ptr time_keeper, const std::shared_ptr planning_factor_interface) : SceneModuleInterface(module_id, logger, clock, time_keeper, planning_factor_interface), @@ -57,7 +57,7 @@ MergeFromPrivateRoadModule::MergeFromPrivateRoadModule( static std::optional getFirstConflictingLanelet( const lanelet::ConstLanelets & conflicting_lanelets, const InterpolatedPathInfo & interpolated_path_info, - const autoware::universe_utils::LinearRing2d & footprint, const double vehicle_length) + const autoware_utils::LinearRing2d & footprint, const double vehicle_length) { const auto & path_ip = interpolated_path_info.path; const auto [lane_start, end] = interpolated_path_info.lane_id_interval.value(); @@ -67,8 +67,8 @@ static std::optional getFirstConflictingLanelet( for (size_t i = start; i <= end; ++i) { const auto & pose = path_ip.points.at(i).point.pose; - const auto path_footprint = autoware::universe_utils::transformVector( - footprint, autoware::universe_utils::pose2transform(pose)); + const auto path_footprint = + autoware_utils::transform_vector(footprint, autoware_utils::pose2transform(pose)); for (const auto & conflicting_lanelet : conflicting_lanelets) { const auto polygon_2d = conflicting_lanelet.polygon2d().basicPolygon(); const bool intersects = bg::intersects(polygon_2d, path_footprint); diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_merge_from_private_road.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_merge_from_private_road.hpp index 1630acb501de4..e5849a8995cd4 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_merge_from_private_road.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_merge_from_private_road.hpp @@ -62,7 +62,7 @@ class MergeFromPrivateRoadModule : public SceneModuleInterface const int64_t module_id, const int64_t lane_id, std::shared_ptr planner_data, const PlannerParam & planner_param, const std::set & associative_ids, const rclcpp::Logger logger, const rclcpp::Clock::SharedPtr clock, - const std::shared_ptr time_keeper, + const std::shared_ptr time_keeper, const std::shared_ptr planning_factor_interface); diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/util.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/util.cpp index 3234e3a10f8aa..c6d826878fac4 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/util.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/util.cpp @@ -20,8 +20,8 @@ #include #include #include -#include #include +#include #include #include @@ -55,7 +55,7 @@ static std::optional getDuplicatedPointIdx( const auto & p = path.points.at(i).point.pose.position; constexpr double min_dist = 0.001; - if (autoware::universe_utils::calcDistance2d(p, point) < min_dist) { + if (autoware_utils::calc_distance2d(p, point) < min_dist) { return i; } } @@ -137,7 +137,7 @@ std::optional> findLaneIdsInterval( std::optional getFirstPointInsidePolygonByFootprint( const lanelet::CompoundPolygon3d & polygon, const InterpolatedPathInfo & interpolated_path_info, - const autoware::universe_utils::LinearRing2d & footprint, const double vehicle_length) + const autoware_utils::LinearRing2d & footprint, const double vehicle_length) { const auto & path_ip = interpolated_path_info.path; const auto [lane_start, lane_end] = interpolated_path_info.lane_id_interval.value(); @@ -147,8 +147,8 @@ std::optional getFirstPointInsidePolygonByFootprint( const auto area_2d = lanelet::utils::to2D(polygon).basicPolygon(); for (auto i = start; i <= lane_end; ++i) { const auto & base_pose = path_ip.points.at(i).point.pose; - const auto path_footprint = autoware::universe_utils::transformVector( - footprint, autoware::universe_utils::pose2transform(base_pose)); + const auto path_footprint = + autoware_utils::transform_vector(footprint, autoware_utils::pose2transform(base_pose)); if (bg::intersects(path_footprint, area_2d)) { return std::make_optional(i); } @@ -161,7 +161,7 @@ std::optional & polygons, const InterpolatedPathInfo & interpolated_path_info, - const autoware::universe_utils::LinearRing2d & footprint, const double vehicle_length) + const autoware_utils::LinearRing2d & footprint, const double vehicle_length) { const auto & path_ip = interpolated_path_info.path; const auto [lane_start, lane_end] = interpolated_path_info.lane_id_interval.value(); @@ -171,8 +171,8 @@ getFirstPointInsidePolygonsByFootprint( for (size_t i = start; i <= lane_end; ++i) { const auto & pose = path_ip.points.at(i).point.pose; - const auto path_footprint = autoware::universe_utils::transformVector( - footprint, autoware::universe_utils::pose2transform(pose)); + const auto path_footprint = + autoware_utils::transform_vector(footprint, autoware_utils::pose2transform(pose)); for (size_t j = 0; j < polygons.size(); ++j) { const auto area_2d = lanelet::utils::to2D(polygons.at(j)).basicPolygon(); const bool is_in_polygon = bg::intersects(area_2d, path_footprint); @@ -343,7 +343,7 @@ bool isOverTargetIndex( return static_cast(closest_idx > target_idx); } -std::optional getIntersectionArea( +std::optional getIntersectionArea( lanelet::ConstLanelet assigned_lane, lanelet::LaneletMapConstPtr lanelet_map_ptr) { const std::string area_id_str = assigned_lane.attributeOr("intersection_area", "else"); diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/util.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/util.hpp index e8f48a901a641..8224ee4e9a3fd 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/util.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/util.hpp @@ -17,7 +17,7 @@ #include "interpolated_path_info.hpp" -#include +#include #include #include @@ -79,7 +79,7 @@ bool isOverTargetIndex( const autoware_internal_planning_msgs::msg::PathWithLaneId & path, const size_t closest_idx, const geometry_msgs::msg::Pose & current_pose, const size_t target_idx); -std::optional getIntersectionArea( +std::optional getIntersectionArea( lanelet::ConstLanelet assigned_lane, lanelet::LaneletMapConstPtr lanelet_map_ptr); /** @@ -120,7 +120,7 @@ void retrievePathsBackward( */ std::optional getFirstPointInsidePolygonByFootprint( const lanelet::CompoundPolygon3d & polygon, const InterpolatedPathInfo & interpolated_path_info, - const autoware::universe_utils::LinearRing2d & footprint, const double vehicle_length); + const autoware_utils::LinearRing2d & footprint, const double vehicle_length); /** * @brief find the index of the first point where vehicle footprint intersects with the given @@ -131,7 +131,7 @@ std::optional & polygons, const InterpolatedPathInfo & interpolated_path_info, - const autoware::universe_utils::LinearRing2d & footprint, const double vehicle_length); + const autoware_utils::LinearRing2d & footprint, const double vehicle_length); std::vector getPolygon3dFromLanelets( const lanelet::ConstLanelets & ll_vec); diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/package.xml b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/package.xml index 190583f583981..1ebda53e27f2d 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/package.xml +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/package.xml @@ -24,7 +24,7 @@ autoware_motion_utils autoware_planning_msgs autoware_route_handler - autoware_universe_utils + autoware_utils eigen geometry_msgs libboost-dev diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/src/debug.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/src/debug.cpp index c0c04a0949ce8..6d69a1d067f10 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/src/debug.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/src/debug.cpp @@ -16,18 +16,18 @@ #include #include -#include -#include +#include +#include #include namespace autoware::behavior_velocity_planner { -using autoware::universe_utils::appendMarkerArray; -using autoware::universe_utils::createDefaultMarker; -using autoware::universe_utils::createMarkerColor; -using autoware::universe_utils::createMarkerScale; -using autoware::universe_utils::createPoint; +using autoware_utils::append_marker_array; +using autoware_utils::create_default_marker; +using autoware_utils::create_marker_color; +using autoware_utils::create_marker_scale; +using autoware_utils::create_point; using visualization_msgs::msg::Marker; namespace @@ -41,11 +41,11 @@ visualization_msgs::msg::MarkerArray createNoDrivableLaneMarkers( // No Drivable Lane Polygon if (!debug_data.no_drivable_lane_polygon.empty()) { - auto marker = createDefaultMarker( + auto marker = create_default_marker( "map", now, "no_drivable_lane polygon", uid, Marker::LINE_STRIP, - createMarkerScale(0.1, 0.0, 0.0), createMarkerColor(1.0, 1.0, 1.0, 0.999)); + create_marker_scale(0.1, 0.0, 0.0), create_marker_color(1.0, 1.0, 1.0, 0.999)); for (const auto & p : debug_data.no_drivable_lane_polygon) { - marker.points.push_back(createPoint(p.x, p.y, p.z)); + marker.points.push_back(create_point(p.x, p.y, p.z)); } marker.points.push_back(marker.points.front()); msg.markers.push_back(marker); @@ -53,16 +53,16 @@ visualization_msgs::msg::MarkerArray createNoDrivableLaneMarkers( // Path - polygon intersection points { - auto marker = createDefaultMarker( + auto marker = create_default_marker( "map", now, "path_polygon intersection points", uid, Marker::POINTS, - createMarkerScale(0.25, 0.25, 0.0), createMarkerColor(1.0, 0.0, 0.0, 0.999)); + create_marker_scale(0.25, 0.25, 0.0), create_marker_color(1.0, 0.0, 0.0, 0.999)); const auto & p_first = debug_data.path_polygon_intersection.first_intersection_point; if (p_first) { - marker.points.push_back(createPoint(p_first->x, p_first->y, p_first->z)); + marker.points.push_back(create_point(p_first->x, p_first->y, p_first->z)); } const auto & p_second = debug_data.path_polygon_intersection.second_intersection_point; if (p_second) { - marker.points.push_back(createPoint(p_second->x, p_second->y, p_second->z)); + marker.points.push_back(create_point(p_second->x, p_second->y, p_second->z)); } if (!marker.points.empty()) msg.markers.push_back(marker); } @@ -96,7 +96,7 @@ visualization_msgs::msg::MarkerArray NoDrivableLaneModule::createDebugMarkerArra visualization_msgs::msg::MarkerArray debug_marker_array; const auto now = this->clock_->now(); - appendMarkerArray( + append_marker_array( createNoDrivableLaneMarkers(debug_data_, this->clock_->now(), module_id_), &debug_marker_array); return debug_marker_array; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/src/manager.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/src/manager.cpp index 48a7b435b0e36..1053f7bb8287a 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/src/manager.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/src/manager.cpp @@ -15,21 +15,21 @@ #include "manager.hpp" #include -#include +#include #include #include namespace autoware::behavior_velocity_planner { -using autoware::universe_utils::getOrDeclareParameter; +using autoware_utils::get_or_declare_parameter; NoDrivableLaneModuleManager::NoDrivableLaneModuleManager(rclcpp::Node & node) : SceneModuleManagerInterface(node, getModuleName()) { const std::string ns(NoDrivableLaneModuleManager::getModuleName()); - planner_param_.stop_margin = getOrDeclareParameter(node, ns + ".stop_margin"); - planner_param_.print_debug_info = getOrDeclareParameter(node, ns + ".print_debug_info"); + planner_param_.stop_margin = get_or_declare_parameter(node, ns + ".stop_margin"); + planner_param_.print_debug_info = get_or_declare_parameter(node, ns + ".print_debug_info"); } void NoDrivableLaneModuleManager::launchNewModules( diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/src/scene.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/src/scene.cpp index bbca1bf2d6481..0fdda1f54924c 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/src/scene.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/src/scene.cpp @@ -15,7 +15,7 @@ #include "scene.hpp" #include "autoware/motion_utils/trajectory/trajectory.hpp" -#include "autoware/universe_utils/geometry/geometry.hpp" +#include "autoware_utils/geometry/geometry.hpp" #include "util.hpp" #include @@ -25,12 +25,12 @@ namespace autoware::behavior_velocity_planner { -using autoware::universe_utils::createPoint; +using autoware_utils::create_point; NoDrivableLaneModule::NoDrivableLaneModule( const int64_t module_id, const int64_t lane_id, const PlannerParam & planner_param, const rclcpp::Logger logger, const rclcpp::Clock::SharedPtr clock, - const std::shared_ptr time_keeper, + const std::shared_ptr time_keeper, const std::shared_ptr planning_factor_interface) : SceneModuleInterface(module_id, logger, clock, time_keeper, planning_factor_interface), @@ -158,7 +158,7 @@ void NoDrivableLaneModule::handle_approaching_state(PathWithLaneId * path) } geometry_msgs::msg::Point stop_point = - autoware::universe_utils::getPoint(path->points.at(target_point_idx).point); + autoware_utils::get_point(path->points.at(target_point_idx).point); const auto & op_stop_pose = planning_utils::insertStopPoint(stop_point, target_segment_idx, *path); @@ -215,7 +215,7 @@ void NoDrivableLaneModule::handle_inside_no_drivable_lane_state(PathWithLaneId * // Get stop point and stop factor { - const auto & stop_pose = autoware::universe_utils::getPose(path->points.at(0)); + const auto & stop_pose = autoware_utils::get_pose(path->points.at(0)); planning_factor_interface_->add( path->points, planner_data_->current_odometry->pose, stop_pose, stop_pose, tier4_planning_msgs::msg::PlanningFactor::STOP, tier4_planning_msgs::msg::SafetyFactorArray{}, @@ -276,7 +276,7 @@ void NoDrivableLaneModule::initialize_debug_data( debug_data_.path_polygon_intersection = path_no_drivable_lane_polygon_intersection; for (const auto & p : no_drivable_lane.polygon2d().basicPolygon()) { - debug_data_.no_drivable_lane_polygon.push_back(createPoint(p.x(), p.y(), ego_pos.z)); + debug_data_.no_drivable_lane_polygon.push_back(create_point(p.x(), p.y(), ego_pos.z)); } } diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/src/scene.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/src/scene.hpp index 17df17f5ed84e..ce6c31b4c9647 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/src/scene.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/src/scene.hpp @@ -56,7 +56,7 @@ class NoDrivableLaneModule : public SceneModuleInterface NoDrivableLaneModule( const int64_t module_id, const int64_t lane_id, const PlannerParam & planner_param, const rclcpp::Logger logger, const rclcpp::Clock::SharedPtr clock, - const std::shared_ptr time_keeper, + const std::shared_ptr time_keeper, const std::shared_ptr planning_factor_interface); diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/src/util.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/src/util.cpp index 3ca6dccd77e6d..a3c960c641275 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/src/util.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/src/util.cpp @@ -19,7 +19,7 @@ #include #include -#include +#include #include @@ -35,7 +35,7 @@ using Polygon = bg::model::polygon; using Line = bg::model::linestring; using autoware::motion_utils::calcSignedArcLength; -using autoware::universe_utils::createPoint; +using autoware_utils::create_point; PathWithNoDrivableLanePolygonIntersection getPathIntersectionWithNoDrivableLanePolygon( const PathWithLaneId & ego_path, const lanelet::BasicPolygon2d & polygon, @@ -68,10 +68,10 @@ PathWithNoDrivableLanePolygonIntersection getPathIntersectionWithNoDrivableLaneP const auto compare = [&](const Point & p1, const Point & p2) { const auto dist_l1 = - calcSignedArcLength(ego_path.points, size_t(0), createPoint(p1.x(), p1.y(), ego_pos.z)); + calcSignedArcLength(ego_path.points, size_t(0), create_point(p1.x(), p1.y(), ego_pos.z)); const auto dist_l2 = - calcSignedArcLength(ego_path.points, size_t(0), createPoint(p2.x(), p2.y(), ego_pos.z)); + calcSignedArcLength(ego_path.points, size_t(0), create_point(p2.x(), p2.y(), ego_pos.z)); return dist_l1 < dist_l2; }; @@ -99,10 +99,10 @@ PathWithNoDrivableLanePolygonIntersection getPathIntersectionWithNoDrivableLaneP const auto & p = intersects.at(0); if (is_last_path_point_inside_polygon) { path_no_drivable_lane_polygon_intersection.first_intersection_point = - createPoint(p.x(), p.y(), ego_pos.z); + create_point(p.x(), p.y(), ego_pos.z); } else if (path_no_drivable_lane_polygon_intersection.is_first_path_point_inside_polygon) { path_no_drivable_lane_polygon_intersection.second_intersection_point = - createPoint(p.x(), p.y(), ego_pos.z); + create_point(p.x(), p.y(), ego_pos.z); } else { // do nothing } @@ -111,9 +111,9 @@ PathWithNoDrivableLanePolygonIntersection getPathIntersectionWithNoDrivableLaneP const auto & p0 = intersects.at(0); const auto & p1 = intersects.at(1); path_no_drivable_lane_polygon_intersection.first_intersection_point = - createPoint(p0.x(), p0.y(), ego_pos.z); + create_point(p0.x(), p0.y(), ego_pos.z); path_no_drivable_lane_polygon_intersection.second_intersection_point = - createPoint(p1.x(), p1.y(), ego_pos.z); + create_point(p1.x(), p1.y(), ego_pos.z); } else { // do nothing } diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/package.xml b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/package.xml index b9af47f6729c9..de3092635f131 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/package.xml +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/package.xml @@ -25,7 +25,7 @@ autoware_motion_utils autoware_perception_msgs autoware_planning_msgs - autoware_universe_utils + autoware_utils autoware_vehicle_info_utils geometry_msgs libboost-dev diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/debug.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/debug.cpp index b9509e308d1fc..af281acc9cb15 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/debug.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/debug.cpp @@ -18,8 +18,8 @@ #include #include #include -#include -#include +#include +#include #include #include @@ -30,10 +30,10 @@ namespace { const double marker_lifetime = 0.2; using DebugData = no_stopping_area::DebugData; -using autoware::universe_utils::appendMarkerArray; -using autoware::universe_utils::createDefaultMarker; -using autoware::universe_utils::createMarkerColor; -using autoware::universe_utils::createMarkerScale; +using autoware_utils::append_marker_array; +using autoware_utils::create_default_marker; +using autoware_utils::create_marker_color; +using autoware_utils::create_marker_scale; lanelet::BasicPoint3d get_centroid_point(const lanelet::BasicPolygon3d & poly) { @@ -60,10 +60,10 @@ visualization_msgs::msg::MarkerArray create_lanelet_info_marker_array( // ID { - auto marker = createDefaultMarker( + auto marker = create_default_marker( "map", now, "no_stopping_area_id", static_cast(no_stopping_area_reg_elem.id()), - visualization_msgs::msg::Marker::TEXT_VIEW_FACING, createMarkerScale(0.0, 0.0, 1.0), - createMarkerColor(1.0, 1.0, 1.0, 0.999)); + visualization_msgs::msg::Marker::TEXT_VIEW_FACING, create_marker_scale(0.0, 0.0, 1.0), + create_marker_color(1.0, 1.0, 1.0, 0.999)); marker.lifetime = rclcpp::Duration::from_seconds(marker_lifetime); for (const auto & detection_area : no_stopping_area_reg_elem.noStoppingAreas()) { @@ -79,10 +79,10 @@ visualization_msgs::msg::MarkerArray create_lanelet_info_marker_array( // Polygon { - auto marker = createDefaultMarker( + auto marker = create_default_marker( "map", now, "no_stopping_area_polygon", static_cast(no_stopping_area_reg_elem.id()), - visualization_msgs::msg::Marker::LINE_LIST, createMarkerScale(0.1, 0.0, 0.0), - createMarkerColor(0.1, 0.1, 1.0, 0.500)); + visualization_msgs::msg::Marker::LINE_LIST, create_marker_scale(0.1, 0.0, 0.0), + create_marker_color(0.1, 0.1, 1.0, 0.500)); marker.lifetime = rclcpp::Duration::from_seconds(marker_lifetime); for (const auto & no_stopping_area : no_stopping_area_reg_elem.noStoppingAreas()) { @@ -107,11 +107,11 @@ visualization_msgs::msg::MarkerArray create_lanelet_info_marker_array( if (stop_line) { const auto stop_line_center_point = (stop_line.value().front().basicPoint() + stop_line.value().back().basicPoint()) / 2; - auto marker = createDefaultMarker( + auto marker = create_default_marker( "map", now, "no_stopping_area_correspondence", static_cast(no_stopping_area_reg_elem.id()), - visualization_msgs::msg::Marker::LINE_STRIP, createMarkerScale(0.1, 0.0, 0.0), - createMarkerColor(0.1, 0.1, 1.0, 0.500)); + visualization_msgs::msg::Marker::LINE_STRIP, create_marker_scale(0.1, 0.0, 0.0), + create_marker_color(0.1, 0.1, 1.0, 0.500)); marker.lifetime = rclcpp::Duration::from_seconds(marker_lifetime); for (const auto & detection_area : no_stopping_area_reg_elem.noStoppingAreas()) { const auto poly = detection_area.basicPolygon(); @@ -133,24 +133,24 @@ visualization_msgs::msg::MarkerArray NoStoppingAreaModule::createDebugMarkerArra visualization_msgs::msg::MarkerArray debug_marker_array; const rclcpp::Time now = clock_->now(); - appendMarkerArray( + append_marker_array( create_lanelet_info_marker_array(no_stopping_area_reg_elem_, now), &debug_marker_array, now); if (!debug_data_.stuck_points.empty()) { - appendMarkerArray( + append_marker_array( debug::createPointsMarkerArray( debug_data_.stuck_points, "stuck_points", module_id_, now, 0.3, 0.3, 0.3, 1.0, 1.0, 0.0), &debug_marker_array, now); } if (!debug_data_.stuck_vehicle_detect_area.points.empty()) { - appendMarkerArray( + append_marker_array( debug::createPolygonMarkerArray( debug_data_.stuck_vehicle_detect_area, "stuck_vehicle_detect_area", module_id_, now, 0.1, 0.1, 0.1, 1.0, 1.0, 0.0), &debug_marker_array, now); } if (!debug_data_.stop_line_detect_area.points.empty()) { - appendMarkerArray( + append_marker_array( debug::createPolygonMarkerArray( debug_data_.stop_line_detect_area, "stop_line_detect_area", module_id_, now, 0.1, 0.1, 0.1, 1.0, 1.0, 0.0), @@ -167,7 +167,7 @@ autoware::motion_utils::VirtualWalls NoStoppingAreaModule::createVirtualWalls() wall.text = "no_stopping_area"; wall.style = autoware::motion_utils::VirtualWallType::stop; for (const auto & p : debug_data_.stop_poses) { - wall.pose = autoware::universe_utils::calcOffsetPose(p, debug_data_.base_link2front, 0.0, 0.0); + wall.pose = autoware_utils::calc_offset_pose(p, debug_data_.base_link2front, 0.0, 0.0); virtual_walls.push_back(wall); } return virtual_walls; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/manager.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/manager.cpp index 0598bbb4f0d26..c948e372638cb 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/manager.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/manager.cpp @@ -15,7 +15,7 @@ #include "manager.hpp" #include -#include +#include #include #include @@ -25,7 +25,7 @@ namespace autoware::behavior_velocity_planner { -using autoware::universe_utils::getOrDeclareParameter; +using autoware_utils::get_or_declare_parameter; using lanelet::autoware::NoStoppingArea; NoStoppingAreaModuleManager::NoStoppingAreaModuleManager(rclcpp::Node & node) @@ -35,14 +35,14 @@ NoStoppingAreaModuleManager::NoStoppingAreaModuleManager(rclcpp::Node & node) const std::string ns(NoStoppingAreaModuleManager::getModuleName()); auto & pp = planner_param_; const auto & vi = autoware::vehicle_info_utils::VehicleInfoUtils(node).getVehicleInfo(); - pp.state_clear_time = getOrDeclareParameter(node, ns + ".state_clear_time"); - pp.stuck_vehicle_vel_thr = getOrDeclareParameter(node, ns + ".stuck_vehicle_vel_thr"); - pp.stop_margin = getOrDeclareParameter(node, ns + ".stop_margin"); - pp.dead_line_margin = getOrDeclareParameter(node, ns + ".dead_line_margin"); - pp.stop_line_margin = getOrDeclareParameter(node, ns + ".stop_line_margin"); - pp.detection_area_length = getOrDeclareParameter(node, ns + ".detection_area_length"); + pp.state_clear_time = get_or_declare_parameter(node, ns + ".state_clear_time"); + pp.stuck_vehicle_vel_thr = get_or_declare_parameter(node, ns + ".stuck_vehicle_vel_thr"); + pp.stop_margin = get_or_declare_parameter(node, ns + ".stop_margin"); + pp.dead_line_margin = get_or_declare_parameter(node, ns + ".dead_line_margin"); + pp.stop_line_margin = get_or_declare_parameter(node, ns + ".stop_line_margin"); + pp.detection_area_length = get_or_declare_parameter(node, ns + ".detection_area_length"); pp.stuck_vehicle_front_margin = - getOrDeclareParameter(node, ns + ".stuck_vehicle_front_margin"); + get_or_declare_parameter(node, ns + ".stuck_vehicle_front_margin"); pp.path_expand_width = vi.vehicle_width_m * 0.5; } @@ -61,7 +61,7 @@ void NoStoppingAreaModuleManager::launchNewModules( registerModule(std::make_shared( module_id, lane_id, *m.first, planner_param_, logger_.get_child("no_stopping_area_module"), clock_, time_keeper_, planning_factor_interface_)); - generateUUID(module_id); + generate_uuid(module_id); updateRTCStatus( getUUID(module_id), true, State::WAITING_FOR_EXECUTION, std::numeric_limits::lowest(), path.header.stamp); diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.cpp index 14f4f7b62c1f7..ee262339148a7 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.cpp @@ -20,8 +20,8 @@ #include #include #include -#include -#include +#include +#include #include #include @@ -42,7 +42,7 @@ NoStoppingAreaModule::NoStoppingAreaModule( const lanelet::autoware::NoStoppingArea & no_stopping_area_reg_elem, const PlannerParam & planner_param, const rclcpp::Logger & logger, const rclcpp::Clock::SharedPtr clock, - const std::shared_ptr time_keeper, + const std::shared_ptr time_keeper, const std::shared_ptr planning_factor_interface) : SceneModuleInterfaceWithRTC(module_id, logger, clock, time_keeper, planning_factor_interface), @@ -175,7 +175,7 @@ bool NoStoppingAreaModule::check_stuck_vehicles_in_no_stopping_area( continue; // not stop vehicle } // check if the footprint is in the stuck detect area - const Polygon2d obj_footprint = autoware::universe_utils::toPolygon2d(object); + const Polygon2d obj_footprint = autoware_utils::to_polygon2d(object); const bool is_in_stuck_area = !bg::disjoint(obj_footprint, poly); if (is_in_stuck_area) { RCLCPP_DEBUG(logger_, "stuck vehicle found."); diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.hpp index 58ea2a320f5bd..8a7f93931899d 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.hpp @@ -58,7 +58,7 @@ class NoStoppingAreaModule : public SceneModuleInterfaceWithRTC const lanelet::autoware::NoStoppingArea & no_stopping_area_reg_elem, const PlannerParam & planner_param, const rclcpp::Logger & logger, const rclcpp::Clock::SharedPtr clock, - const std::shared_ptr time_keeper, + const std::shared_ptr time_keeper, const std::shared_ptr planning_factor_interface); diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/utils.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/utils.cpp index ca9627bc1a057..7a3be8277fff0 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/utils.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/utils.cpp @@ -17,7 +17,7 @@ #include #include #include -#include +#include #include #include @@ -78,7 +78,7 @@ std::optional generate_stop_line( std::vector collision_points; boost::geometry::intersection(area_poly, line, collision_points); if (!collision_points.empty()) { - const double yaw = autoware::universe_utils::calcAzimuthAngle(p0, p1); + const double yaw = autoware_utils::calc_azimuth_angle(p0, p1); const double w = ego_width; const double l = stop_line_margin; stop_line.emplace_back( @@ -159,7 +159,7 @@ Polygon2d generate_ego_no_stopping_area_lane_polygon( } const auto no_stopping_area = no_stopping_area_reg_elem.noStoppingAreas().front(); for (size_t i = ego_area_start_idx; i < pp.size() - 1; ++i) { - dist_from_start_sum += autoware::universe_utils::calcDistance2d(pp.at(i), pp.at(i - 1)); + dist_from_start_sum += autoware_utils::calc_distance2d(pp.at(i), pp.at(i - 1)); const auto & p = pp.at(i).point.pose.position; // TODO(someone): within will skip points on the edge of polygons so some points can be skipped // depending on the interpolation @@ -186,12 +186,12 @@ Polygon2d generate_ego_no_stopping_area_lane_polygon( // decide end idx with extract distance size_t ego_area_end_idx = ego_area_start_idx; for (size_t i = ego_area_start_idx; i < pp.size() - 1; ++i) { - dist_from_start_sum += autoware::universe_utils::calcDistance2d(pp.at(i), pp.at(i - 1)); + dist_from_start_sum += autoware_utils::calc_distance2d(pp.at(i), pp.at(i - 1)); const auto & p = pp.at(i).point.pose.position; // TODO(someone): within will skip points on the edge of polygons so some points can be skipped // depending on the interpolation if (!bg::within(Point2d{p.x, p.y}, lanelet::utils::to2D(no_stopping_area).basicPolygon())) { - dist_from_area_sum += autoware::universe_utils::calcDistance2d(pp.at(i), pp.at(i - 1)); + dist_from_area_sum += autoware_utils::calc_distance2d(pp.at(i), pp.at(i - 1)); } if (dist_from_start_sum > max_polygon_length || dist_from_area_sum > margin) { break; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/utils.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/utils.hpp index d545e1618fe71..bf3f7486ec353 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/utils.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/utils.hpp @@ -17,8 +17,8 @@ #include #include -#include #include +#include #include #include @@ -33,7 +33,7 @@ namespace autoware::behavior_velocity_planner::no_stopping_area { using PathIndexWithPose = std::pair; // front index, pose -using PathIndexWithPoint2d = std::pair; // front index, point2d +using PathIndexWithPoint2d = std::pair; // front index, point2d using PathIndexWithOffset = std::pair; // front index, offset struct PassJudge @@ -99,7 +99,7 @@ void insert_stop_point( * @param ego_width [m] width of ego * @param stop_line_margin [m] margin to keep between the stop line and the no stopping areas **/ -std::optional generate_stop_line( +std::optional generate_stop_line( const autoware_internal_planning_msgs::msg::PathWithLaneId & path, const lanelet::ConstPolygons3d & no_stopping_areas, const double ego_width, const double stop_line_margin); @@ -157,7 +157,7 @@ bool check_stop_lines_in_no_stopping_area( * @param vehicle_width [m] width of the ego vehicle * @return generated stop line */ -std::optional get_stop_line_geometry2d( +std::optional get_stop_line_geometry2d( const autoware_internal_planning_msgs::msg::PathWithLaneId & path, const lanelet::autoware::NoStoppingArea & no_stopping_area_reg_elem, const double stop_line_margin, const double stop_line_extend_length, const double vehicle_width); diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/test/test_utils.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/test/test_utils.cpp index d1747aafb4c78..a980fcf611eff 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/test/test_utils.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/test/test_utils.cpp @@ -15,8 +15,8 @@ #include "../src/utils.hpp" #include -#include #include +#include #include #include @@ -225,7 +225,7 @@ TEST(NoStoppingAreaTest, generateEgoNoStoppingAreaLanePolygon) { using autoware::behavior_velocity_planner::no_stopping_area:: generate_ego_no_stopping_area_lane_polygon; - using autoware::universe_utils::Point2d; + using autoware_utils::Point2d; geometry_msgs::msg::Pose ego_pose; // ego at (0,0) ego_pose.position.x = 0.0; ego_pose.position.y = 0.0; @@ -250,7 +250,7 @@ TEST(NoStoppingAreaTest, generateEgoNoStoppingAreaLanePolygon) const auto lane_poly = generate_ego_no_stopping_area_lane_polygon( path, ego_pose, *no_stopping_area_reg_elem, margin, max_polygon_length, path_expand_width, logger, clock); - autoware::universe_utils::Polygon2d simplified_poly; + autoware_utils::Polygon2d simplified_poly; EXPECT_FALSE(lane_poly.outer().empty()); EXPECT_TRUE(lane_poly.inners().empty()); // simplify the polygon to make it easier to check @@ -271,7 +271,7 @@ TEST(NoStoppingAreaTest, generateEgoNoStoppingAreaLanePolygon) const auto lane_poly = generate_ego_no_stopping_area_lane_polygon( path, ego_pose, *no_stopping_area_reg_elem, big_margin, max_polygon_length, path_expand_width, logger, clock); - autoware::universe_utils::Polygon2d simplified_poly; + autoware_utils::Polygon2d simplified_poly; EXPECT_FALSE(lane_poly.outer().empty()); EXPECT_TRUE(lane_poly.inners().empty()); // simplify the polygon to make it easier to check @@ -289,7 +289,7 @@ TEST(NoStoppingAreaTest, generateEgoNoStoppingAreaLanePolygon) const auto lane_poly = generate_ego_no_stopping_area_lane_polygon( path, ego_pose, *no_stopping_area_reg_elem, margin, small_polygon_length, path_expand_width, logger, clock); - autoware::universe_utils::Polygon2d simplified_poly; + autoware_utils::Polygon2d simplified_poly; EXPECT_FALSE(lane_poly.outer().empty()); EXPECT_TRUE(lane_poly.inners().empty()); // simplify the polygon to make it easier to check @@ -358,7 +358,7 @@ TEST(NoStoppingAreaTest, checkStopLinesInNoStoppingArea) { using autoware::behavior_velocity_planner::no_stopping_area::check_stop_lines_in_no_stopping_area; autoware_internal_planning_msgs::msg::PathWithLaneId path; - autoware::universe_utils::Polygon2d poly; + autoware_utils::Polygon2d poly; autoware::behavior_velocity_planner::no_stopping_area::DebugData debug_data; // empty inputs diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/package.xml b/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/package.xml index c4b26150a6670..31e1f21b35f88 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/package.xml +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/package.xml @@ -24,7 +24,7 @@ autoware_perception_msgs autoware_planning_msgs autoware_route_handler - autoware_universe_utils + autoware_utils autoware_vehicle_info_utils geometry_msgs grid_map_ros diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/debug.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/debug.cpp index 5760b6de66a71..41d360ee9eace 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/debug.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/debug.cpp @@ -18,7 +18,7 @@ #include #include #include -#include +#include #include #include @@ -30,12 +30,12 @@ namespace { using builtin_interfaces::msg::Time; using BasicPolygons = std::vector; -using autoware::universe_utils::appendMarkerArray; -using autoware::universe_utils::calcOffsetPose; -using autoware::universe_utils::createMarkerColor; -using autoware::universe_utils::createMarkerOrientation; -using autoware::universe_utils::createMarkerPosition; -using autoware::universe_utils::createMarkerScale; +using autoware_utils::append_marker_array; +using autoware_utils::calc_offset_pose; +using autoware_utils::create_marker_color; +using autoware_utils::create_marker_orientation; +using autoware_utils::create_marker_position; +using autoware_utils::create_marker_scale; using occlusion_spot_utils::PossibleCollisionInfo; using visualization_msgs::msg::Marker; using visualization_msgs::msg::MarkerArray; @@ -48,8 +48,8 @@ std::vector makeDebugInfoMarker( debug_marker.header.frame_id = "map"; debug_marker.id = id; debug_marker.action = Marker::ADD; - debug_marker.pose.position = createMarkerPosition(0.0, 0.0, 0.0); - debug_marker.pose.orientation = createMarkerOrientation(0, 0, 0, 1.0); + debug_marker.pose.position = create_marker_position(0.0, 0.0, 0.0); + debug_marker.pose.orientation = create_marker_orientation(0, 0, 0, 1.0); debug_marker.lifetime = rclcpp::Duration::from_seconds(0.5); const auto & pc = possible_collision; // for collision point with margin @@ -57,8 +57,8 @@ std::vector makeDebugInfoMarker( debug_marker.ns = "collision_point"; debug_marker.type = Marker::CYLINDER; debug_marker.pose = pc.collision_with_margin.pose; - debug_marker.scale = createMarkerScale(0.5, 0.5, 0.5); - debug_marker.color = createMarkerColor(1.0, 0.0, 0.0, 0.5); + debug_marker.scale = create_marker_scale(0.5, 0.5, 0.5); + debug_marker.color = create_marker_color(1.0, 0.0, 0.0, 0.5); debug_markers.push_back(debug_marker); } // cylinder at collision_point point @@ -66,8 +66,8 @@ std::vector makeDebugInfoMarker( debug_marker.ns = "collision_point_with_margin"; debug_marker.type = Marker::CYLINDER; debug_marker.pose = pc.collision_pose; - debug_marker.scale = createMarkerScale(0.5, 0.5, 0.5); - debug_marker.color = createMarkerColor(1.0, 0.5, 0.0, 0.5); + debug_marker.scale = create_marker_scale(0.5, 0.5, 0.5); + debug_marker.color = create_marker_color(1.0, 0.5, 0.0, 0.5); debug_markers.push_back(debug_marker); } @@ -76,8 +76,8 @@ std::vector makeDebugInfoMarker( debug_marker.ns = "obstacle"; debug_marker.type = Marker::CYLINDER; debug_marker.pose.position = pc.obstacle_info.position; - debug_marker.color = createMarkerColor(0.5, 0.5, 0.5, 0.5); - debug_marker.scale = createMarkerScale(0.8, 0.8, 1.5); + debug_marker.color = create_marker_color(0.5, 0.5, 0.5, 0.5); + debug_marker.scale = create_marker_scale(0.8, 0.8, 1.5); debug_markers.push_back(debug_marker); } @@ -85,8 +85,8 @@ std::vector makeDebugInfoMarker( { debug_marker.ns = "from_obj_to_collision"; debug_marker.type = Marker::ARROW; - debug_marker.scale = createMarkerScale(0.05, 0.2, 0.5); - debug_marker.color = createMarkerColor(0.1, 0.1, 0.1, 0.5); + debug_marker.scale = create_marker_scale(0.05, 0.2, 0.5); + debug_marker.color = create_marker_color(0.1, 0.1, 0.1, 0.5); debug_marker.points = {pc.obstacle_info.position, pc.intersection_pose.position}; debug_markers.push_back(debug_marker); } @@ -97,7 +97,7 @@ std::vector makeDebugInfoMarker( debug_marker.type = Marker::TEXT_VIEW_FACING; debug_marker.pose = pc.collision_with_margin.pose; debug_marker.scale.z = 1.0; - debug_marker.color = createMarkerColor(1.0, 1.0, 0.0, 1.0); + debug_marker.color = create_marker_color(1.0, 1.0, 0.0, 1.0); std::ostringstream string_stream; auto r = [](const double v) { return std::round(v * 100.0) / 100.0; }; const double len = r(pc.arc_lane_dist_at_collision.length); @@ -140,15 +140,15 @@ MarkerArray makePolygonMarker( debug_marker.id = planning_utils::bitShift(id); debug_marker.type = Marker::LINE_STRIP; debug_marker.action = Marker::ADD; - debug_marker.pose.position = createMarkerPosition(0.0, 0.0, 0); - debug_marker.pose.orientation = createMarkerOrientation(0, 0, 0, 1.0); - debug_marker.scale = createMarkerScale(0.1, 0.1, 0.1); - debug_marker.color = createMarkerColor(1.0, 1.0, 1.0, 0.5); + debug_marker.pose.position = create_marker_position(0.0, 0.0, 0); + debug_marker.pose.orientation = create_marker_orientation(0, 0, 0, 1.0); + debug_marker.scale = create_marker_scale(0.1, 0.1, 0.1); + debug_marker.color = create_marker_color(1.0, 1.0, 1.0, 0.5); debug_marker.lifetime = rclcpp::Duration::from_seconds(0.5); debug_marker.ns = ns; for (const auto & poly : polygons) { for (const auto & p : poly) { - geometry_msgs::msg::Point point = createMarkerPosition(p.x(), p.y(), z + 0.5); + geometry_msgs::msg::Point point = create_marker_position(p.x(), p.y(), z + 0.5); debug_marker.points.push_back(point); } debug_markers.markers.push_back(debug_marker); @@ -168,15 +168,15 @@ MarkerArray makeSlicePolygonMarker( debug_marker.id = planning_utils::bitShift(id); debug_marker.type = Marker::LINE_STRIP; debug_marker.action = Marker::ADD; - debug_marker.pose.position = createMarkerPosition(0.0, 0.0, 0); - debug_marker.pose.orientation = createMarkerOrientation(0, 0, 0, 1.0); - debug_marker.scale = createMarkerScale(0.1, 0.1, 0.1); - debug_marker.color = createMarkerColor(1.0, 0.0, 1.0, 0.3); + debug_marker.pose.position = create_marker_position(0.0, 0.0, 0); + debug_marker.pose.orientation = create_marker_orientation(0, 0, 0, 1.0); + debug_marker.scale = create_marker_scale(0.1, 0.1, 0.1); + debug_marker.color = create_marker_color(1.0, 0.0, 1.0, 0.3); debug_marker.lifetime = rclcpp::Duration::from_seconds(0.1); debug_marker.ns = ns; for (const auto & slice : slices) { for (const auto & p : slice.outer()) { - geometry_msgs::msg::Point point = createMarkerPosition(p.x(), p.y(), z); + geometry_msgs::msg::Point point = create_marker_position(p.x(), p.y(), z); debug_marker.points.push_back(point); } debug_markers.markers.push_back(debug_marker); @@ -192,21 +192,21 @@ MarkerArray OcclusionSpotModule::createDebugMarkerArray() const auto now = this->clock_->now(); MarkerArray debug_marker_array; if (!debug_data_.possible_collisions.empty()) { - appendMarkerArray(makeDebugInfoMarkers(debug_data_), &debug_marker_array, now); + append_marker_array(makeDebugInfoMarkers(debug_data_), &debug_marker_array, now); } if (!debug_data_.detection_area_polygons.empty()) { - appendMarkerArray( + append_marker_array( makeSlicePolygonMarker( debug_data_.detection_area_polygons, "detection_area", module_id_, debug_data_.z), &debug_marker_array, now); } if (!debug_data_.close_partition.empty() && param_.is_show_occlusion) { - appendMarkerArray( + append_marker_array( makePolygonMarker(debug_data_.close_partition, "close_partition", module_id_, debug_data_.z), &debug_marker_array, now); } if (!debug_data_.occlusion_points.empty()) { - appendMarkerArray( + append_marker_array( debug::createPointsMarkerArray( debug_data_.occlusion_points, "occlusion", module_id_, now, 0.5, 0.5, 0.5, 1.0, 0.0, 0.0), &debug_marker_array, now); @@ -222,7 +222,7 @@ autoware::motion_utils::VirtualWalls OcclusionSpotModule::createVirtualWalls() wall.style = autoware::motion_utils::VirtualWallType::slowdown; for (size_t id = 0; id < debug_data_.debug_poses.size(); id++) { wall.pose = - calcOffsetPose(debug_data_.debug_poses.at(id), debug_data_.baselink_to_front, 0.0, 0.0); + calc_offset_pose(debug_data_.debug_poses.at(id), debug_data_.baselink_to_front, 0.0, 0.0); virtual_walls.push_back(wall); } return virtual_walls; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/grid_utils.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/grid_utils.cpp index b3e9b2678189f..41f1b1ad02622 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/grid_utils.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/grid_utils.cpp @@ -96,13 +96,13 @@ std::optional generateOcclusionPolygon( const Polygon2d & occupancy_poly, const Point2d & origin, const Point2d & min_theta_pos, const Point2d & max_theta_pos, const double ray_max_length = 100.0) { - using autoware::universe_utils::normalizeRadian; + using autoware_utils::normalize_radian; const double origin_x = origin.x(); const double origin_y = origin.y(); const double min_theta = - normalizeRadian(std::atan2(min_theta_pos.y() - origin_y, min_theta_pos.x() - origin_x), 0.0); + normalize_radian(std::atan2(min_theta_pos.y() - origin_y, min_theta_pos.x() - origin_x), 0.0); const double max_theta = - normalizeRadian(std::atan2(max_theta_pos.y() - origin_y, max_theta_pos.x() - origin_x), 0.0); + normalize_radian(std::atan2(max_theta_pos.y() - origin_y, max_theta_pos.x() - origin_x), 0.0); LineString2d theta_min_ray = { origin, {origin_x + ray_max_length * std::cos(min_theta), @@ -135,13 +135,13 @@ std::optional generateOcclusionPolygon( Polygon2d generateOccupancyPolygon(const nav_msgs::msg::MapMetaData & info, const double r = 100) { - using autoware::universe_utils::calcOffsetPose; + using autoware_utils::calc_offset_pose; // generate occupancy polygon from grid origin Polygon2d poly; // create counter clockwise poly - poly.outer().emplace_back(to_bg2d(calcOffsetPose(info.origin, 0, 0, 0).position)); - poly.outer().emplace_back(to_bg2d(calcOffsetPose(info.origin, r, 0, 0).position)); - poly.outer().emplace_back(to_bg2d(calcOffsetPose(info.origin, r, r, 0).position)); - poly.outer().emplace_back(to_bg2d(calcOffsetPose(info.origin, 0, r, 0).position)); + poly.outer().emplace_back(to_bg2d(calc_offset_pose(info.origin, 0, 0, 0).position)); + poly.outer().emplace_back(to_bg2d(calc_offset_pose(info.origin, r, 0, 0).position)); + poly.outer().emplace_back(to_bg2d(calc_offset_pose(info.origin, r, r, 0).position)); + poly.outer().emplace_back(to_bg2d(calc_offset_pose(info.origin, 0, r, 0).position)); boost::geometry::correct(poly); return poly; @@ -156,7 +156,7 @@ std::pair calcEdgePoint(const Polygon2d & foot_print, const Poin for (size_t i = 0; i < foot_print.outer().size(); i++) { const auto & f = foot_print.outer().at(i); PolarCoordinates polar = toPolarCoordinates(origin, f); - const double theta_norm = autoware::universe_utils::normalizeRadian(polar.theta, 0.0); + const double theta_norm = autoware_utils::normalize_radian(polar.theta, 0.0); if (theta_norm < min_theta) { min_theta = theta_norm; min_idx = i; @@ -184,7 +184,7 @@ std::optional generateOccupiedPolygon( Point transformFromMap2Grid(const TransformStamped & geom_tf_map2grid, const Point2d & p) { - Point geom_pt = autoware::universe_utils::createPoint(p.x(), p.y(), 0); + Point geom_pt = autoware_utils::create_point(p.x(), p.y(), 0); Point transformed_geom_pt; // from map coordinate to occupancy grid coordinate tf2::doTransform(geom_pt, transformed_geom_pt, geom_tf_map2grid); diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/grid_utils.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/grid_utils.hpp index 78abb19b3fdbe..3d6b74cdc9fc0 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/grid_utils.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/grid_utils.hpp @@ -18,8 +18,8 @@ #include #include #include -#include -#include +#include +#include #include #include #include @@ -55,11 +55,11 @@ using geometry_msgs::msg::Pose; using geometry_msgs::msg::PoseStamped; namespace grid_utils { -using autoware::universe_utils::LineString2d; -using autoware::universe_utils::Point2d; -using autoware::universe_utils::Polygon2d; using autoware_perception_msgs::msg::PredictedObject; using autoware_perception_msgs::msg::PredictedObjects; +using autoware_utils::LineString2d; +using autoware_utils::Point2d; +using autoware_utils::Polygon2d; using geometry_msgs::msg::Point; using geometry_msgs::msg::TransformStamped; using nav_msgs::msg::MapMetaData; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/manager.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/manager.cpp index 88d4a4397c73e..cb79d039dc9ac 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/manager.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/manager.cpp @@ -17,7 +17,7 @@ #include "scene_occlusion_spot.hpp" #include -#include +#include #include @@ -27,7 +27,7 @@ namespace autoware::behavior_velocity_planner { -using autoware::universe_utils::getOrDeclareParameter; +using autoware_utils::get_or_declare_parameter; using occlusion_spot_utils::DETECTION_METHOD; using occlusion_spot_utils::PASS_JUDGE; @@ -38,7 +38,8 @@ OcclusionSpotModuleManager::OcclusionSpotModuleManager(rclcpp::Node & node) auto & pp = planner_param_; // for detection type { - const std::string method = getOrDeclareParameter(node, ns + ".detection_method"); + const std::string method = + get_or_declare_parameter(node, ns + ".detection_method"); if (method == "occupancy_grid") { // module id 0 pp.detection_method = DETECTION_METHOD::OCCUPANCY_GRID; module_id_ = DETECTION_METHOD::OCCUPANCY_GRID; @@ -52,7 +53,7 @@ OcclusionSpotModuleManager::OcclusionSpotModuleManager(rclcpp::Node & node) } // for passable judgement { - const std::string pass_judge = getOrDeclareParameter(node, ns + ".pass_judge"); + const std::string pass_judge = get_or_declare_parameter(node, ns + ".pass_judge"); if (pass_judge == "current_velocity") { pp.pass_judge = PASS_JUDGE::CURRENT_VELOCITY; } else if (pass_judge == "smooth_velocity") { @@ -62,52 +63,56 @@ OcclusionSpotModuleManager::OcclusionSpotModuleManager(rclcpp::Node & node) "[behavior_velocity]: occlusion spot pass judge method has invalid argument"}; } } - pp.use_object_info = getOrDeclareParameter(node, ns + ".use_object_info"); + pp.use_object_info = get_or_declare_parameter(node, ns + ".use_object_info"); pp.use_moving_object_ray_cast = - getOrDeclareParameter(node, ns + ".use_moving_object_ray_cast"); - pp.use_partition_lanelet = getOrDeclareParameter(node, ns + ".use_partition_lanelet"); - pp.pedestrian_vel = getOrDeclareParameter(node, ns + ".pedestrian_vel"); - pp.pedestrian_radius = getOrDeclareParameter(node, ns + ".pedestrian_radius"); + get_or_declare_parameter(node, ns + ".use_moving_object_ray_cast"); + pp.use_partition_lanelet = get_or_declare_parameter(node, ns + ".use_partition_lanelet"); + pp.pedestrian_vel = get_or_declare_parameter(node, ns + ".pedestrian_vel"); + pp.pedestrian_radius = get_or_declare_parameter(node, ns + ".pedestrian_radius"); // debug - pp.is_show_occlusion = getOrDeclareParameter(node, ns + ".debug.is_show_occlusion"); - pp.is_show_cv_window = getOrDeclareParameter(node, ns + ".debug.is_show_cv_window"); + pp.is_show_occlusion = get_or_declare_parameter(node, ns + ".debug.is_show_occlusion"); + pp.is_show_cv_window = get_or_declare_parameter(node, ns + ".debug.is_show_cv_window"); pp.is_show_processing_time = - getOrDeclareParameter(node, ns + ".debug.is_show_processing_time"); + get_or_declare_parameter(node, ns + ".debug.is_show_processing_time"); // threshold pp.detection_area_offset = - getOrDeclareParameter(node, ns + ".threshold.detection_area_offset"); + get_or_declare_parameter(node, ns + ".threshold.detection_area_offset"); pp.detection_area_length = - getOrDeclareParameter(node, ns + ".threshold.detection_area_length"); - pp.stuck_vehicle_vel = getOrDeclareParameter(node, ns + ".threshold.stuck_vehicle_vel"); - pp.lateral_distance_thr = getOrDeclareParameter(node, ns + ".threshold.lateral_distance"); - pp.dist_thr = getOrDeclareParameter(node, ns + ".threshold.search_dist"); - pp.angle_thr = getOrDeclareParameter(node, ns + ".threshold.search_angle"); + get_or_declare_parameter(node, ns + ".threshold.detection_area_length"); + pp.stuck_vehicle_vel = + get_or_declare_parameter(node, ns + ".threshold.stuck_vehicle_vel"); + pp.lateral_distance_thr = + get_or_declare_parameter(node, ns + ".threshold.lateral_distance"); + pp.dist_thr = get_or_declare_parameter(node, ns + ".threshold.search_dist"); + pp.angle_thr = get_or_declare_parameter(node, ns + ".threshold.search_angle"); // ego additional velocity config - pp.v.safety_ratio = getOrDeclareParameter(node, ns + ".motion.safety_ratio"); - pp.v.safe_margin = getOrDeclareParameter(node, ns + ".motion.safe_margin"); - pp.v.max_slow_down_jerk = getOrDeclareParameter(node, ns + ".motion.max_slow_down_jerk"); + pp.v.safety_ratio = get_or_declare_parameter(node, ns + ".motion.safety_ratio"); + pp.v.safe_margin = get_or_declare_parameter(node, ns + ".motion.safe_margin"); + pp.v.max_slow_down_jerk = + get_or_declare_parameter(node, ns + ".motion.max_slow_down_jerk"); pp.v.max_slow_down_accel = - getOrDeclareParameter(node, ns + ".motion.max_slow_down_accel"); - pp.v.non_effective_jerk = getOrDeclareParameter(node, ns + ".motion.non_effective_jerk"); + get_or_declare_parameter(node, ns + ".motion.max_slow_down_accel"); + pp.v.non_effective_jerk = + get_or_declare_parameter(node, ns + ".motion.non_effective_jerk"); pp.v.non_effective_accel = - getOrDeclareParameter(node, ns + ".motion.non_effective_acceleration"); + get_or_declare_parameter(node, ns + ".motion.non_effective_acceleration"); pp.v.min_allowed_velocity = - getOrDeclareParameter(node, ns + ".motion.min_allowed_velocity"); + get_or_declare_parameter(node, ns + ".motion.min_allowed_velocity"); // detection_area param pp.detection_area.min_occlusion_spot_size = - getOrDeclareParameter(node, ns + ".detection_area.min_occlusion_spot_size"); + get_or_declare_parameter(node, ns + ".detection_area.min_occlusion_spot_size"); pp.detection_area.min_longitudinal_offset = - getOrDeclareParameter(node, ns + ".detection_area.min_longitudinal_offset"); + get_or_declare_parameter(node, ns + ".detection_area.min_longitudinal_offset"); pp.detection_area.max_lateral_distance = - getOrDeclareParameter(node, ns + ".detection_area.max_lateral_distance"); + get_or_declare_parameter(node, ns + ".detection_area.max_lateral_distance"); pp.detection_area.slice_length = - getOrDeclareParameter(node, ns + ".detection_area.slice_length"); + get_or_declare_parameter(node, ns + ".detection_area.slice_length"); // occupancy grid param - pp.grid.free_space_max = getOrDeclareParameter(node, ns + ".grid.free_space_max"); - pp.grid.occupied_min = getOrDeclareParameter(node, ns + ".grid.occupied_min"); + pp.grid.free_space_max = get_or_declare_parameter(node, ns + ".grid.free_space_max"); + pp.grid.occupied_min = get_or_declare_parameter(node, ns + ".grid.occupied_min"); const auto vehicle_info = autoware::vehicle_info_utils::VehicleInfoUtils(node).getVehicleInfo(); pp.baselink_to_front = vehicle_info.max_longitudinal_offset_m; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/occlusion_spot_utils.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/occlusion_spot_utils.cpp index f8174eb327acd..378a4eeda0f18 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/occlusion_spot_utils.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/occlusion_spot_utils.cpp @@ -19,9 +19,9 @@ #include #include #include -#include -#include -#include +#include +#include +#include #include #include @@ -42,7 +42,7 @@ namespace occlusion_spot_utils Polygon2d toFootprintPolygon(const PredictedObject & object, const double scale = 1.0) { const Pose & obj_pose = object.kinematics.initial_pose_with_covariance.pose; - Polygon2d obj_footprint = autoware::universe_utils::toPolygon2d(object); + Polygon2d obj_footprint = autoware_utils::to_polygon2d(object); // upscale foot print for noise obj_footprint = upScalePolygon(obj_pose.position, obj_footprint, scale); return obj_footprint; @@ -127,7 +127,7 @@ void calcSlowDownPointsForPossibleCollision( const double dist_to_col = possible_collisions.at(collision_index).arc_lane_dist_at_collision.length; dist_along_next_path_point += - autoware::universe_utils::calcDistance2d(p_prev.pose.position, p_next.pose.position); + autoware_utils::calc_distance2d(p_prev.pose.position, p_next.pose.position); // process if nearest possible collision is between current and next path point if (dist_along_path_point < dist_to_col) { for (; collision_index < possible_collisions.size(); collision_index++) { @@ -174,8 +174,7 @@ void clipPathByLength( double length_sum = 0; clipped.points.emplace_back(path.points.front()); for (int i = 1; i < static_cast(path.points.size()); i++) { - length_sum += - autoware::universe_utils::calcDistance2d(path.points.at(i - 1), path.points.at(i)); + length_sum += autoware_utils::calc_distance2d(path.points.at(i - 1), path.points.at(i)); if (length_sum > max_length) return; clipped.points.emplace_back(path.points.at(i)); } @@ -250,7 +249,7 @@ void categorizeVehicles( lanelet::ArcCoordinates getOcclusionPoint( const PredictedObject & obj, const lanelet::ConstLineString2d & ll_string) { - const auto poly = autoware::universe_utils::toPolygon2d(obj); + const auto poly = autoware_utils::to_polygon2d(obj); std::deque arcs; for (const auto & p : poly.outer()) { lanelet::BasicPoint2d obj_p = {p.x(), p.y()}; @@ -376,7 +375,7 @@ std::vector filterVehiclesByDetectionArea( // stuck points by predicted objects for (const auto & object : objs) { // check if the footprint is in the stuck detect area - const auto obj_footprint = autoware::universe_utils::toPolygon2d(object); + const auto obj_footprint = autoware_utils::to_polygon2d(object); for (const auto & p : polys) { if (!bg::disjoint(obj_footprint, p)) { filtered_obj.emplace_back(object); @@ -404,8 +403,8 @@ bool generatePossibleCollisionsFromGridMap( param.detection_area.min_occlusion_spot_size); if (param.is_show_occlusion) { for (const auto & op : occlusion_spot_positions) { - Point p = autoware::universe_utils::createPoint( - op[0], op[1], path.points.at(0).point.pose.position.z); + Point p = + autoware_utils::create_point(op[0], op[1], path.points.at(0).point.pose.position.z); debug_data.occlusion_points.emplace_back(p); } } diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/scene_occlusion_spot.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/scene_occlusion_spot.cpp index 935b1ec125024..c396edfdb3c4f 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/scene_occlusion_spot.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/scene_occlusion_spot.cpp @@ -65,7 +65,7 @@ OcclusionSpotModule::OcclusionSpotModule( const int64_t module_id, const std::shared_ptr & planner_data, const PlannerParam & planner_param, const rclcpp::Logger & logger, const rclcpp::Clock::SharedPtr clock, - const std::shared_ptr time_keeper, + const std::shared_ptr time_keeper, const std::shared_ptr planning_factor_interface) : SceneModuleInterface(module_id, logger, clock, time_keeper, planning_factor_interface), diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/scene_occlusion_spot.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/scene_occlusion_spot.hpp index 8642137b83831..ffd126747cbe2 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/scene_occlusion_spot.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/scene_occlusion_spot.hpp @@ -19,7 +19,7 @@ #include #include -#include +#include #include #include @@ -49,7 +49,7 @@ class OcclusionSpotModule : public SceneModuleInterface const int64_t module_id, const std::shared_ptr & planner_data, const PlannerParam & planner_param, const rclcpp::Logger & logger, const rclcpp::Clock::SharedPtr clock, - const std::shared_ptr time_keeper, + const std::shared_ptr time_keeper, const std::shared_ptr planning_factor_interface); @@ -64,7 +64,7 @@ class OcclusionSpotModule : public SceneModuleInterface private: // Parameter PlannerParam param_; - autoware::universe_utils::StopWatch stop_watch_; + autoware_utils::StopWatch stop_watch_; std::vector partition_lanelets_; protected: diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/test/src/test_grid_utils.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/test/src/test_grid_utils.cpp index 73fba3969442e..458012ab10f05 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/test/src/test_grid_utils.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/test/src/test_grid_utils.cpp @@ -16,7 +16,7 @@ #include "utils.hpp" #include -#include +#include #include @@ -87,7 +87,7 @@ TEST(compareTime, polygon_vs_line_iterator) } } const grid_map::Matrix & grid_data = grid["layer"]; - autoware::universe_utils::StopWatch stop_watch; + autoware_utils::StopWatch stop_watch; stop_watch.tic("processing_time"); size_t count = 0; [[maybe_unused]] double time = 0; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/include/autoware/behavior_velocity_planner/node.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/include/autoware/behavior_velocity_planner/node.hpp index 2f267131ef1be..02529fd12c20b 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/include/autoware/behavior_velocity_planner/node.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/include/autoware/behavior_velocity_planner/node.hpp @@ -16,11 +16,11 @@ #define AUTOWARE__BEHAVIOR_VELOCITY_PLANNER__NODE_HPP_ #include "autoware/behavior_velocity_planner/planner_manager.hpp" -#include "autoware/universe_utils/ros/logger_level_configure.hpp" -#include "autoware/universe_utils/ros/polling_subscriber.hpp" +#include "autoware_utils/ros/logger_level_configure.hpp" +#include "autoware_utils/ros/polling_subscriber.hpp" #include -#include +#include #include #include @@ -62,35 +62,32 @@ class BehaviorVelocityPlannerNode : public rclcpp::Node trigger_sub_path_with_lane_id_; // polling subscribers - autoware::universe_utils::InterProcessPollingSubscriber< - autoware_perception_msgs::msg::PredictedObjects> + autoware_utils::InterProcessPollingSubscriber sub_predicted_objects_{this, "~/input/dynamic_objects"}; - autoware::universe_utils::InterProcessPollingSubscriber + autoware_utils::InterProcessPollingSubscriber sub_no_ground_pointcloud_{ - this, "~/input/no_ground_pointcloud", autoware::universe_utils::SingleDepthSensorQoS()}; + this, "~/input/no_ground_pointcloud", autoware_utils::single_depth_sensor_qos()}; - autoware::universe_utils::InterProcessPollingSubscriber - sub_vehicle_odometry_{this, "~/input/vehicle_odometry"}; + autoware_utils::InterProcessPollingSubscriber sub_vehicle_odometry_{ + this, "~/input/vehicle_odometry"}; - autoware::universe_utils::InterProcessPollingSubscriber< - geometry_msgs::msg::AccelWithCovarianceStamped> + autoware_utils::InterProcessPollingSubscriber sub_acceleration_{this, "~/input/accel"}; - autoware::universe_utils::InterProcessPollingSubscriber< + autoware_utils::InterProcessPollingSubscriber< autoware_perception_msgs::msg::TrafficLightGroupArray> sub_traffic_signals_{this, "~/input/traffic_signals"}; - autoware::universe_utils::InterProcessPollingSubscriber - sub_occupancy_grid_{this, "~/input/occupancy_grid"}; + autoware_utils::InterProcessPollingSubscriber sub_occupancy_grid_{ + this, "~/input/occupancy_grid"}; - autoware::universe_utils::InterProcessPollingSubscriber< - LaneletMapBin, universe_utils::polling_policy::Newest> + autoware_utils::InterProcessPollingSubscriber< + LaneletMapBin, autoware_utils::polling_policy::Newest> sub_lanelet_map_{this, "~/input/vector_map", rclcpp::QoS{1}.transient_local()}; - autoware::universe_utils::InterProcessPollingSubscriber - sub_external_velocity_limit_{ - this, "~/input/external_velocity_limit_mps", rclcpp::QoS{1}.transient_local()}; + autoware_utils::InterProcessPollingSubscriber sub_external_velocity_limit_{ + this, "~/input/external_velocity_limit_mps", rclcpp::QoS{1}.transient_local()}; void onTrigger( const autoware_internal_planning_msgs::msg::PathWithLaneId::ConstSharedPtr input_path_msg); @@ -137,9 +134,9 @@ class BehaviorVelocityPlannerNode : public rclcpp::Node const autoware_internal_planning_msgs::msg::PathWithLaneId::ConstSharedPtr input_path_msg, const PlannerData & planner_data); - std::unique_ptr logger_configure_; + std::unique_ptr logger_configure_; - std::unique_ptr published_time_publisher_; + std::unique_ptr published_time_publisher_; static constexpr int logger_throttle_interval = 3000; }; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/package.xml b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/package.xml index d68937727773f..6a1d026b88a06 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/package.xml +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/package.xml @@ -40,7 +40,7 @@ autoware_perception_msgs autoware_planning_msgs autoware_route_handler - autoware_universe_utils + autoware_utils autoware_velocity_smoother diagnostic_msgs eigen diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/node.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/node.cpp index d48151acf3b93..c80c27eeacbd5 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/node.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/node.cpp @@ -17,10 +17,10 @@ #include #include #include -#include -#include #include #include +#include +#include #include #include @@ -106,9 +106,8 @@ BehaviorVelocityPlannerNode::BehaviorVelocityPlannerNode(const rclcpp::NodeOptio planner_manager_.launchScenePlugin(*this, name); } - logger_configure_ = std::make_unique(this); - published_time_publisher_ = - std::make_unique(this); + logger_configure_ = std::make_unique(this); + published_time_publisher_ = std::make_unique(this); } void BehaviorVelocityPlannerNode::onLoadPlugin( @@ -155,7 +154,7 @@ void BehaviorVelocityPlannerNode::processNoGroundPointCloud( Eigen::Affine3f affine = tf2::transformToEigen(transform.transform).cast(); pcl::PointCloud::Ptr pc_transformed(new pcl::PointCloud); if (!pc.empty()) { - autoware::universe_utils::transformPointCloud(pc, *pc_transformed, affine); + autoware_utils::transform_pointcloud(pc, *pc_transformed, affine); } planner_data_.no_ground_pointcloud = pc_transformed; @@ -237,7 +236,7 @@ bool BehaviorVelocityPlannerNode::processData(rclcpp::Clock clock) }; const auto & getData = [&logData](auto & dest, auto & sub, const std::string & data_type = "") { - const auto temp = sub.takeData(); + const auto temp = sub.take_data(); if (temp) { dest = temp; return true; @@ -250,7 +249,7 @@ bool BehaviorVelocityPlannerNode::processData(rclcpp::Clock clock) is_ready &= getData(planner_data_.predicted_objects, sub_predicted_objects_, "predicted_objects"); is_ready &= getData(planner_data_.occupancy_grid, sub_occupancy_grid_, "occupancy_grid"); - const auto odometry = sub_vehicle_odometry_.takeData(); + const auto odometry = sub_vehicle_odometry_.take_data(); if (odometry) { processOdometry(odometry); } else { @@ -258,7 +257,7 @@ bool BehaviorVelocityPlannerNode::processData(rclcpp::Clock clock) is_ready = false; } - const auto no_ground_pointcloud = sub_no_ground_pointcloud_.takeData(); + const auto no_ground_pointcloud = sub_no_ground_pointcloud_.take_data(); if (no_ground_pointcloud) { processNoGroundPointCloud(no_ground_pointcloud); } else { @@ -266,18 +265,18 @@ bool BehaviorVelocityPlannerNode::processData(rclcpp::Clock clock) is_ready = false; } - const auto map_data = sub_lanelet_map_.takeData(); + const auto map_data = sub_lanelet_map_.take_data(); if (map_data) { planner_data_.route_handler_ = std::make_shared(*map_data); } // planner_data_.external_velocity_limit is std::optional type variable. - const auto external_velocity_limit = sub_external_velocity_limit_.takeData(); + const auto external_velocity_limit = sub_external_velocity_limit_.take_data(); if (external_velocity_limit) { planner_data_.external_velocity_limit = *external_velocity_limit; } - const auto traffic_signals = sub_traffic_signals_.takeData(); + const auto traffic_signals = sub_traffic_signals_.take_data(); if (traffic_signals) processTrafficSignals(traffic_signals); return is_ready; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/scene_module_interface.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/scene_module_interface.hpp index 37c1d07f62a40..2340770c62396 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/scene_module_interface.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/scene_module_interface.hpp @@ -21,10 +21,10 @@ #include #include #include -#include -#include -#include -#include +#include +#include +#include +#include #include #include @@ -50,11 +50,11 @@ namespace autoware::behavior_velocity_planner using autoware::objects_of_interest_marker_interface::ColorName; using autoware::objects_of_interest_marker_interface::ObjectsOfInterestMarkerInterface; -using autoware::universe_utils::DebugPublisher; -using autoware::universe_utils::getOrDeclareParameter; -using autoware::universe_utils::StopWatch; using autoware_internal_debug_msgs::msg::Float64Stamped; using autoware_internal_planning_msgs::msg::PathWithLaneId; +using autoware_utils::DebugPublisher; +using autoware_utils::get_or_declare_parameter; +using autoware_utils::StopWatch; using builtin_interfaces::msg::Time; using unique_identifier_msgs::msg::UUID; @@ -76,7 +76,7 @@ class SceneModuleInterface public: explicit SceneModuleInterface( const int64_t module_id, rclcpp::Logger logger, rclcpp::Clock::SharedPtr clock, - const std::shared_ptr time_keeper, + const std::shared_ptr time_keeper, const std::shared_ptr planning_factor_interface); virtual ~SceneModuleInterface() = default; @@ -102,7 +102,7 @@ class SceneModuleInterface rclcpp::Clock::SharedPtr clock_; std::shared_ptr planner_data_; std::vector objects_of_interest_; - mutable std::shared_ptr time_keeper_; + mutable std::shared_ptr time_keeper_; std::shared_ptr planning_factor_interface_; void setObjectsOfInterestData( @@ -141,10 +141,10 @@ class SceneModuleManagerInterface processing_time_publisher_ = std::make_shared(&node, "~/debug"); - pub_processing_time_detail_ = node.create_publisher( + pub_processing_time_detail_ = node.create_publisher( "~/debug/processing_time_detail_ms/" + std::string(module_name), 1); - time_keeper_ = std::make_shared(pub_processing_time_detail_); + time_keeper_ = std::make_shared(pub_processing_time_detail_); } virtual ~SceneModuleManagerInterface() = default; @@ -169,7 +169,7 @@ class SceneModuleManagerInterface protected: virtual void modifyPathVelocity(autoware_internal_planning_msgs::msg::PathWithLaneId * path) { - universe_utils::ScopedTimeTrack st( + autoware_utils::ScopedTimeTrack st( "SceneModuleManagerInterface::modifyPathVelocity", *time_keeper_); StopWatch stop_watch; stop_watch.tic("Total"); @@ -264,9 +264,9 @@ class SceneModuleManagerInterface std::shared_ptr processing_time_publisher_; - rclcpp::Publisher::SharedPtr pub_processing_time_detail_; + rclcpp::Publisher::SharedPtr pub_processing_time_detail_; - std::shared_ptr time_keeper_; + std::shared_ptr time_keeper_; std::shared_ptr planning_factor_interface_; }; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/utilization/arc_lane_util.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/utilization/arc_lane_util.hpp index 056952bd4fb9d..ce84a4512bf33 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/utilization/arc_lane_util.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/utilization/arc_lane_util.hpp @@ -16,7 +16,7 @@ #define AUTOWARE__BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__ARC_LANE_UTIL_HPP_ #include -#include +#include #include @@ -29,7 +29,7 @@ namespace autoware::behavior_velocity_planner { -inline geometry_msgs::msg::Point convertToGeomPoint(const autoware::universe_utils::Point2d & p) +inline geometry_msgs::msg::Point convertToGeomPoint(const autoware_utils::Point2d & p) { geometry_msgs::msg::Point geom_p; geom_p.x = p.x(); @@ -40,9 +40,8 @@ inline geometry_msgs::msg::Point convertToGeomPoint(const autoware::universe_uti namespace arc_lane_utils { -using PathIndexWithPose = std::pair; // front index, pose -using PathIndexWithPoint2d = - std::pair; // front index, point2d +using PathIndexWithPose = std::pair; // front index, pose +using PathIndexWithPoint2d = std::pair; // front index, point2d using PathIndexWithPoint = std::pair; // front index, point2d using PathIndexWithOffset = std::pair; // front index, offset @@ -60,10 +59,9 @@ std::optional findCollisionSegment( const geometry_msgs::msg::Point & stop_line_p2) { for (size_t i = 0; i < path.points.size() - 1; ++i) { - const auto & p1 = - autoware::universe_utils::getPoint(path.points.at(i)); // Point before collision point + const auto & p1 = autoware_utils::get_point(path.points.at(i)); // Point before collision point const auto & p2 = - autoware::universe_utils::getPoint(path.points.at(i + 1)); // Point after collision point + autoware_utils::get_point(path.points.at(i + 1)); // Point after collision point const auto collision_point = checkCollision(p1, p2, stop_line_p1, stop_line_p2); @@ -92,7 +90,7 @@ std::optional findForwardOffsetSegment( double sum_length = 0.0; for (size_t i = base_idx; i < path.points.size() - 1; ++i) { const double segment_length = - autoware::universe_utils::calcDistance2d(path.points.at(i), path.points.at(i + 1)); + autoware_utils::calc_distance2d(path.points.at(i), path.points.at(i + 1)); // If it's over offset point, return front index and remain offset length /** @@ -117,8 +115,7 @@ std::optional findBackwardOffsetSegment( double sum_length = 0.0; const auto start = static_cast(base_idx) - 1; for (std::int32_t i = start; i >= 0; --i) { - sum_length += - autoware::universe_utils::calcDistance2d(path.points.at(i), path.points.at(i + 1)); + sum_length += autoware_utils::calc_distance2d(path.points.at(i), path.points.at(i + 1)); // If it's over offset point, return front index and remain offset length /** @@ -146,13 +143,13 @@ std::optional findOffsetSegment( return findForwardOffsetSegment( path, collision_idx, offset_length + - autoware::universe_utils::calcDistance2d(path.points.at(collision_idx), collision_point)); + autoware_utils::calc_distance2d(path.points.at(collision_idx), collision_point)); } return findBackwardOffsetSegment( path, collision_idx + 1, -offset_length + - autoware::universe_utils::calcDistance2d(path.points.at(collision_idx + 1), collision_point)); + autoware_utils::calc_distance2d(path.points.at(collision_idx + 1), collision_point)); } std::optional findOffsetSegment( @@ -186,8 +183,8 @@ geometry_msgs::msg::Pose calcTargetPose(const T & path, const PathIndexWithOffse target_pose.position.x = target_point_2d.x(); target_pose.position.y = target_point_2d.y(); target_pose.position.z = interpolated_z; - const double yaw = autoware::universe_utils::calcAzimuthAngle(p_front, p_back); - target_pose.orientation = autoware::universe_utils::createQuaternionFromYaw(yaw); + const double yaw = autoware_utils::calc_azimuth_angle(p_front, p_back); + target_pose.orientation = autoware_utils::create_quaternion_from_yaw(yaw); return target_pose; } diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/utilization/boost_geometry_helper.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/utilization/boost_geometry_helper.hpp index 7598550220e05..cdbf22368bca5 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/utilization/boost_geometry_helper.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/utilization/boost_geometry_helper.hpp @@ -15,7 +15,7 @@ #ifndef AUTOWARE__BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__BOOST_GEOMETRY_HELPER_HPP_ #define AUTOWARE__BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__BOOST_GEOMETRY_HELPER_HPP_ -#include +#include #include #include @@ -50,9 +50,9 @@ namespace autoware::behavior_velocity_planner { namespace bg = boost::geometry; -using Point2d = autoware::universe_utils::Point2d; -using LineString2d = autoware::universe_utils::LineString2d; -using Polygon2d = autoware::universe_utils::Polygon2d; +using Point2d = autoware_utils::Point2d; +using LineString2d = autoware_utils::LineString2d; +using Polygon2d = autoware_utils::Polygon2d; template Point2d to_bg2d(const T & p) diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/utilization/util.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/utilization/util.hpp index 76743b819c041..6535905926ee9 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/utilization/util.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/utilization/util.hpp @@ -15,7 +15,7 @@ #ifndef AUTOWARE__BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__UTIL_HPP_ #define AUTOWARE__BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__UTIL_HPP_ -#include "autoware/universe_utils/geometry/boost_geometry.hpp" +#include "autoware_utils/geometry/boost_geometry.hpp" #include #include @@ -62,9 +62,9 @@ struct TrafficSignalStamped }; using Pose = geometry_msgs::msg::Pose; -using Point2d = autoware::universe_utils::Point2d; -using LineString2d = autoware::universe_utils::LineString2d; -using Polygon2d = autoware::universe_utils::Polygon2d; +using Point2d = autoware_utils::Point2d; +using LineString2d = autoware_utils::LineString2d; +using Polygon2d = autoware_utils::Polygon2d; using BasicPolygons2d = std::vector; using Polygons2d = std::vector; using autoware_internal_planning_msgs::msg::PathPointWithLaneId; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/package.xml b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/package.xml index a66faa2e0a303..b3dc6733e8350 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/package.xml +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/package.xml @@ -30,7 +30,7 @@ autoware_planning_factor_interface autoware_planning_msgs autoware_route_handler - autoware_universe_utils + autoware_utils autoware_vehicle_info_utils autoware_velocity_smoother diagnostic_msgs diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/scene_module_interface.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/scene_module_interface.cpp index b4e0457343df8..789b5ee4127e5 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/scene_module_interface.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/scene_module_interface.cpp @@ -14,7 +14,7 @@ #include #include -#include +#include #include #include @@ -26,7 +26,7 @@ namespace autoware::behavior_velocity_planner SceneModuleInterface::SceneModuleInterface( const int64_t module_id, rclcpp::Logger logger, rclcpp::Clock::SharedPtr clock, - const std::shared_ptr time_keeper, + const std::shared_ptr time_keeper, const std::shared_ptr planning_factor_interface) : module_id_(module_id), diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/utilization/arc_lane_util.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/utilization/arc_lane_util.cpp index 525b4947d152b..24a2d4d7eb992 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/utilization/arc_lane_util.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/utilization/arc_lane_util.cpp @@ -13,7 +13,7 @@ // limitations under the License. #include -#include +#include #include diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/utilization/debug.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/utilization/debug.cpp index 120faf15f6ad5..36319f3b43238 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/utilization/debug.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/utilization/debug.cpp @@ -14,15 +14,15 @@ #include #include -#include +#include #include #include namespace autoware::behavior_velocity_planner::debug { -using autoware::universe_utils::createDefaultMarker; -using autoware::universe_utils::createMarkerColor; -using autoware::universe_utils::createMarkerScale; +using autoware_utils::create_default_marker; +using autoware_utils::create_marker_color; +using autoware_utils::create_marker_scale; visualization_msgs::msg::MarkerArray createPolygonMarkerArray( const geometry_msgs::msg::Polygon & polygon, const std::string & ns, const int64_t module_id, @@ -31,10 +31,11 @@ visualization_msgs::msg::MarkerArray createPolygonMarkerArray( { visualization_msgs::msg::MarkerArray msg; { - auto marker = createDefaultMarker( + auto marker = create_default_marker( "map", now, ns, static_cast(module_id), visualization_msgs::msg::Marker::LINE_STRIP, - createMarkerScale(static_cast(x), static_cast(y), static_cast(z)), - createMarkerColor(static_cast(r), static_cast(g), static_cast(b), 0.8f)); + create_marker_scale(static_cast(x), static_cast(y), static_cast(z)), + create_marker_color( + static_cast(r), static_cast(g), static_cast(b), 0.8f)); marker.lifetime = rclcpp::Duration::from_seconds(0.3); for (const auto & p : polygon.points) { @@ -63,21 +64,21 @@ visualization_msgs::msg::MarkerArray createPathMarkerArray( for (size_t i = 0; i < path.points.size(); ++i) { const auto & p = path.points.at(i); - auto marker = createDefaultMarker( + auto marker = create_default_marker( "map", now, ns, static_cast(planning_utils::bitShift(lane_id) + i), visualization_msgs::msg::Marker::ARROW, - createMarkerScale(static_cast(x), static_cast(y), static_cast(z)), - createMarkerColor( + create_marker_scale(static_cast(x), static_cast(y), static_cast(z)), + create_marker_color( static_cast(r), static_cast(g), static_cast(b), 0.999f)); marker.lifetime = rclcpp::Duration::from_seconds(0.3); marker.pose = p.point.pose; if (std::find(p.lane_ids.begin(), p.lane_ids.end(), lane_id) != p.lane_ids.end()) { // if p.lane_ids has lane_id - marker.color = createMarkerColor( + marker.color = create_marker_color( static_cast(r), static_cast(g), static_cast(b), 0.999f); } else { - marker.color = createMarkerColor(0.5, 0.5, 0.5, 0.999); + marker.color = create_marker_color(0.5, 0.5, 0.5, 0.999); } msg.markers.push_back(marker); } @@ -91,9 +92,9 @@ visualization_msgs::msg::MarkerArray createObjectsMarkerArray( { visualization_msgs::msg::MarkerArray msg; - auto marker = createDefaultMarker( - "map", now, ns, 0, visualization_msgs::msg::Marker::CUBE, createMarkerScale(3.0, 1.0, 1.0), - createMarkerColor(static_cast(r), static_cast(g), static_cast(b), 0.8f)); + auto marker = create_default_marker( + "map", now, ns, 0, visualization_msgs::msg::Marker::CUBE, create_marker_scale(3.0, 1.0, 1.0), + create_marker_color(static_cast(r), static_cast(g), static_cast(b), 0.8f)); marker.lifetime = rclcpp::Duration::from_seconds(1.0); for (size_t i = 0; i < objects.objects.size(); ++i) { @@ -114,9 +115,10 @@ visualization_msgs::msg::MarkerArray createPointsMarkerArray( { visualization_msgs::msg::MarkerArray msg; - auto marker = createDefaultMarker( - "map", now, ns, 0, visualization_msgs::msg::Marker::SPHERE, createMarkerScale(x, y, z), - createMarkerColor(static_cast(r), static_cast(g), static_cast(b), 0.999f)); + auto marker = create_default_marker( + "map", now, ns, 0, visualization_msgs::msg::Marker::SPHERE, create_marker_scale(x, y, z), + create_marker_color( + static_cast(r), static_cast(g), static_cast(b), 0.999f)); marker.lifetime = rclcpp::Duration::from_seconds(0.3); for (size_t i = 0; i < points.size(); ++i) { marker.id = static_cast(i + planning_utils::bitShift(module_id)); diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/utilization/path_utilization.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/utilization/path_utilization.cpp index 445aebe6002ab..38f74429998f2 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/utilization/path_utilization.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/utilization/path_utilization.cpp @@ -15,7 +15,7 @@ #include #include #include -#include +#include #include #include @@ -77,7 +77,7 @@ autoware_planning_msgs::msg::Path interpolatePath( v.push_back(path_point.longitudinal_velocity_mps); if (idx != 0) { const auto path_point_prev = path.points.at(idx - 1); - s += autoware::universe_utils::calcDistance2d(path_point_prev.pose, path_point.pose); + s += autoware_utils::calc_distance2d(path_point_prev.pose, path_point.pose); } if (s > path_len) { break; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/utilization/util.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/utilization/util.cpp index 6b20496d956eb..a11ca779b2c16 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/utilization/util.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/utilization/util.cpp @@ -16,8 +16,8 @@ #include "autoware/behavior_velocity_planner_common/utilization/boost_geometry_helper.hpp" #include "autoware/motion_utils/trajectory/trajectory.hpp" -#include "autoware/universe_utils/geometry/geometry.hpp" #include "autoware_lanelet2_extension/utility/query.hpp" +#include "autoware_utils/geometry/geometry.hpp" #include @@ -49,10 +49,8 @@ size_t calcPointIndexFromSegmentIndex( const size_t prev_point_idx = seg_idx; const size_t next_point_idx = seg_idx + 1; - const double prev_dist = - autoware::universe_utils::calcDistance2d(point, points.at(prev_point_idx)); - const double next_dist = - autoware::universe_utils::calcDistance2d(point, points.at(next_point_idx)); + const double prev_dist = autoware_utils::calc_distance2d(point, points.at(prev_point_idx)); + const double next_dist = autoware_utils::calc_distance2d(point, points.at(next_point_idx)); if (prev_dist < next_dist) { return prev_point_idx; @@ -66,7 +64,7 @@ PathPoint getLerpPathPointWithLaneId(const PathPoint p0, const PathPoint p1, con { auto lerp = [](const double a, const double b, const double t) { return a + t * (b - a); }; PathPoint p; - p.pose = autoware::universe_utils::calcInterpolatedPose(p0, p1, ratio); + p.pose = autoware_utils::calc_interpolated_pose(p0, p1, ratio); const double v = lerp(p0.longitudinal_velocity_mps, p1.longitudinal_velocity_mps, ratio); p.longitudinal_velocity_mps = static_cast(v); return p; @@ -88,7 +86,7 @@ geometry_msgs::msg::Pose transformRelCoordinate2D( res.position.y = ((-1.0) * std::sin(yaw) * trans_p.x) + (std::cos(yaw) * trans_p.y); res.position.z = target.position.z - origin.position.z; res.orientation = - autoware::universe_utils::createQuaternionFromYaw(tf2::getYaw(target.orientation) - yaw); + autoware_utils::create_quaternion_from_yaw(tf2::getYaw(target.orientation) - yaw); return res; } @@ -98,9 +96,9 @@ geometry_msgs::msg::Pose transformRelCoordinate2D( namespace autoware::behavior_velocity_planner::planning_utils { using autoware::motion_utils::calcSignedArcLength; -using autoware::universe_utils::calcDistance2d; -using autoware::universe_utils::calcOffsetPose; using autoware_planning_msgs::msg::PathPoint; +using autoware_utils::calc_distance2d; +using autoware_utils::calc_offset_pose; size_t calcSegmentIndexFromPointIndex( const std::vector & points, @@ -127,7 +125,7 @@ size_t calcSegmentIndexFromPointIndex( Point2d calculateOffsetPoint2d( const geometry_msgs::msg::Pose & pose, const double offset_x, const double offset_y) { - return to_bg2d(calcOffsetPose(pose, offset_x, offset_y, 0.0)); + return to_bg2d(calc_offset_pose(pose, offset_x, offset_y, 0.0)); } bool createDetectionAreaPolygons( @@ -165,7 +163,7 @@ bool createDetectionAreaPolygons( if (dist_to_nearest > eps) { // interpolate ego point const auto & pp = path.points; - const double ds = calcDistance2d(pp.at(target_seg_idx), pp.at(target_seg_idx + 1)); + const double ds = calc_distance2d(pp.at(target_seg_idx), pp.at(target_seg_idx + 1)); const double dist_to_target_seg = calcSignedArcLength(path.points, target_seg_idx, target_pose.position, target_seg_idx); const double ratio = dist_to_target_seg / ds; @@ -186,7 +184,7 @@ bool createDetectionAreaPolygons( LineString2d right_outer_bound = {calculateOffsetPoint2d(p0.pose, min_len, -offset_right - eps)}; for (size_t s = first_idx; s <= max_index; s++) { const auto p1 = path.points.at(s).point; - const double ds = calcDistance2d(p0, p1); + const double ds = calc_distance2d(p0, p1); dist_sum += ds; length += ds; // calculate the distance that obstacles can move until ego reach the trajectory point @@ -276,7 +274,7 @@ void insertVelocity( int max_idx = std::min(static_cast(insert_index + 1), static_cast(path.points.size() - 1)); for (int i = min_idx; i <= max_idx; i++) { - if (calcDistance2d(path.points.at(static_cast(i)), path_point) < min_distance) { + if (calc_distance2d(path.points.at(static_cast(i)), path_point) < min_distance) { path.points.at(i).point.longitudinal_velocity_mps = v; already_has_path_point = true; insert_index = static_cast(i); @@ -312,7 +310,7 @@ geometry_msgs::msg::Pose getAheadPose( for (size_t i = start_idx; i < path.points.size() - 1; ++i) { const geometry_msgs::msg::Pose p0 = path.points.at(i).point.pose; const geometry_msgs::msg::Pose p1 = path.points.at(i + 1).point.pose; - curr_dist += autoware::universe_utils::calcDistance2d(p0, p1); + curr_dist += autoware_utils::calc_distance2d(p0, p1); if (curr_dist > ahead_dist) { const double dl = std::max(curr_dist - prev_dist, 0.0001 /* avoid 0 divide */); const double w_p0 = (curr_dist - ahead_dist) / dl; @@ -625,7 +623,7 @@ std::optional insertDecelPoint( output.points.at(i).point.longitudinal_velocity_mps = std::min(original_velocity, target_velocity); } - return autoware::universe_utils::getPose(output.points.at(insert_idx.value())); + return autoware_utils::get_pose(output.points.at(insert_idx.value())); } // TODO(murooka): remove this function for u-turn and crossing-path @@ -641,7 +639,7 @@ std::optional insertStopPoint( return {}; } - return autoware::universe_utils::getPose(output.points.at(insert_idx.value())); + return autoware_utils::get_pose(output.points.at(insert_idx.value())); } std::optional insertStopPoint( @@ -654,7 +652,7 @@ std::optional insertStopPoint( return {}; } - return autoware::universe_utils::getPose(output.points.at(insert_idx.value())); + return autoware_utils::get_pose(output.points.at(insert_idx.value())); } std::set getAssociativeIntersectionLanelets( diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/test/src/test_trajectory_utils.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/test/src/test_trajectory_utils.cpp index 283e54c01403b..ae4d24812011b 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/test/src/test_trajectory_utils.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/test/src/test_trajectory_utils.cpp @@ -77,7 +77,7 @@ TEST(smoothPath, nominal) planner_data->velocity_smoother_ = std::make_shared( - *node, std::make_shared()); + *node, std::make_shared()); // Input path PathWithLaneId in_path; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_rtc_interface/include/autoware/behavior_velocity_rtc_interface/scene_module_interface_with_rtc.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_rtc_interface/include/autoware/behavior_velocity_rtc_interface/scene_module_interface_with_rtc.hpp index 84a9efdab8ba9..540ae52026f0b 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_rtc_interface/include/autoware/behavior_velocity_rtc_interface/scene_module_interface_with_rtc.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_rtc_interface/include/autoware/behavior_velocity_rtc_interface/scene_module_interface_with_rtc.hpp @@ -18,7 +18,7 @@ #include #include #include -#include +#include #include #include @@ -41,8 +41,8 @@ namespace autoware::behavior_velocity_planner { using autoware::rtc_interface::RTCInterface; -using autoware::universe_utils::getOrDeclareParameter; using autoware_internal_planning_msgs::msg::PathWithLaneId; +using autoware_utils::get_or_declare_parameter; using builtin_interfaces::msg::Time; using tier4_rtc_msgs::msg::Module; using tier4_rtc_msgs::msg::State; @@ -53,7 +53,7 @@ class SceneModuleInterfaceWithRTC : public SceneModuleInterface public: explicit SceneModuleInterfaceWithRTC( const int64_t module_id, rclcpp::Logger logger, rclcpp::Clock::SharedPtr clock, - const std::shared_ptr time_keeper, + const std::shared_ptr time_keeper, const std::shared_ptr planning_factor_interface); virtual ~SceneModuleInterfaceWithRTC() = default; @@ -117,7 +117,7 @@ class SceneModuleManagerInterfaceWithRTC UUID getUUID(const int64_t & module_id) const; - void generateUUID(const int64_t & module_id); + void generate_uuid(const int64_t & module_id); void removeUUID(const int64_t & module_id); @@ -131,11 +131,11 @@ class SceneModuleManagerInterfaceWithRTC bool enable_rtc = true; try { - enable_rtc = getOrDeclareParameter(node, "enable_all_modules_auto_mode") + enable_rtc = get_or_declare_parameter(node, "enable_all_modules_auto_mode") ? false - : getOrDeclareParameter(node, param_name); + : get_or_declare_parameter(node, param_name); } catch (const std::exception & e) { - enable_rtc = getOrDeclareParameter(node, param_name); + enable_rtc = get_or_declare_parameter(node, param_name); } return enable_rtc; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_rtc_interface/package.xml b/planning/behavior_velocity_planner/autoware_behavior_velocity_rtc_interface/package.xml index 22935a111ffb2..53ef1ce14fc13 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_rtc_interface/package.xml +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_rtc_interface/package.xml @@ -24,7 +24,7 @@ autoware_planning_factor_interface autoware_planning_msgs autoware_rtc_interface - autoware_universe_utils + autoware_utils rclcpp rclcpp_components tier4_planning_msgs diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_rtc_interface/src/scene_module_interface_with_rtc.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_rtc_interface/src/scene_module_interface_with_rtc.cpp index e26e7f22ef625..fc980eb4ec800 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_rtc_interface/src/scene_module_interface_with_rtc.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_rtc_interface/src/scene_module_interface_with_rtc.cpp @@ -15,7 +15,7 @@ #include #include #include -#include +#include #include #include @@ -28,7 +28,7 @@ namespace autoware::behavior_velocity_planner SceneModuleInterfaceWithRTC::SceneModuleInterfaceWithRTC( const int64_t module_id, rclcpp::Logger logger, rclcpp::Clock::SharedPtr clock, - const std::shared_ptr time_keeper, + const std::shared_ptr time_keeper, const std::shared_ptr planning_factor_interface) : SceneModuleInterface(module_id, logger, clock, time_keeper, planning_factor_interface), @@ -85,9 +85,9 @@ UUID SceneModuleManagerInterfaceWithRTC::getUUID(const int64_t & module_id) cons return map_uuid_.at(module_id); } -void SceneModuleManagerInterfaceWithRTC::generateUUID(const int64_t & module_id) +void SceneModuleManagerInterfaceWithRTC::generate_uuid(const int64_t & module_id) { - map_uuid_.insert({module_id, autoware::universe_utils::generateUUID()}); + map_uuid_.insert({module_id, autoware_utils::generate_uuid()}); } void SceneModuleManagerInterfaceWithRTC::removeUUID(const int64_t & module_id) diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/package.xml b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/package.xml index 761ebeb94419a..9b73e1c295c4b 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/package.xml +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/package.xml @@ -28,7 +28,7 @@ autoware_perception_msgs autoware_planning_msgs autoware_route_handler - autoware_universe_utils + autoware_utils autoware_vehicle_info_utils eigen geometry_msgs diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/debug.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/debug.cpp index 3f4b9f330e2d0..a7b79896459b2 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/debug.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/debug.cpp @@ -17,19 +17,19 @@ #include "scene.hpp" #include -#include -#include +#include +#include #include #include -using autoware::universe_utils::appendMarkerArray; -using autoware::universe_utils::calcOffsetPose; -using autoware::universe_utils::createDefaultMarker; -using autoware::universe_utils::createMarkerColor; -using autoware::universe_utils::createMarkerOrientation; -using autoware::universe_utils::createMarkerScale; -using autoware::universe_utils::createPoint; +using autoware_utils::append_marker_array; +using autoware_utils::calc_offset_pose; +using autoware_utils::create_default_marker; +using autoware_utils::create_marker_color; +using autoware_utils::create_marker_orientation; +using autoware_utils::create_marker_scale; +using autoware_utils::create_point; namespace autoware::behavior_velocity_planner { @@ -38,7 +38,7 @@ namespace RunOutDebug::TextWithPosition createDebugText( const std::string & text, const geometry_msgs::msg::Pose pose, const float lateral_offset) { - const auto offset_pose = autoware::universe_utils::calcOffsetPose(pose, 0, lateral_offset, 0); + const auto offset_pose = autoware_utils::calc_offset_pose(pose, 0, lateral_offset, 0); RunOutDebug::TextWithPosition text_with_position; text_with_position.text = text; @@ -54,16 +54,16 @@ visualization_msgs::msg::MarkerArray createPolygonMarkerArray( { visualization_msgs::msg::MarkerArray marker_array; for (size_t i = 0; i < poly.size(); i++) { - auto marker = createDefaultMarker( + auto marker = create_default_marker( "map", time, ns, i, visualization_msgs::msg::Marker::LINE_STRIP, scale, color); for (const auto & p : poly.at(i)) { - const auto p_with_height = createPoint(p.x, p.y, height); + const auto p_with_height = create_point(p.x, p.y, height); marker.points.push_back(p_with_height); } // close the polygon const auto & p = poly.at(i).front(); - const auto p_with_height = createPoint(p.x, p.y, height); + const auto p_with_height = create_point(p.x, p.y, height); marker.points.push_back(p_with_height); marker_array.markers.push_back(marker); @@ -87,14 +87,14 @@ RunOutDebug::RunOutDebug(rclcpp::Node & node) : node_(node) void RunOutDebug::pushCollisionPoints(const geometry_msgs::msg::Point & point) { - const auto point_with_height = createPoint(point.x, point.y, height_); + const auto point_with_height = create_point(point.x, point.y, height_); collision_points_.push_back(point_with_height); } void RunOutDebug::pushEgoCutLine(const std::vector & line) { for (const auto & point : line) { - const auto point_with_height = createPoint(point.x, point.y, height_); + const auto point_with_height = create_point(point.x, point.y, height_); ego_cut_line_.push_back(point_with_height); } } @@ -102,14 +102,14 @@ void RunOutDebug::pushEgoCutLine(const std::vector & void RunOutDebug::pushCollisionPoints(const std::vector & points) { for (const auto & p : points) { - const auto point_with_height = createPoint(p.x, p.y, height_); + const auto point_with_height = create_point(p.x, p.y, height_); collision_points_.push_back(point_with_height); } } void RunOutDebug::pushNearestCollisionPoint(const geometry_msgs::msg::Point & point) { - const auto point_with_height = createPoint(point.x, point.y, height_); + const auto point_with_height = create_point(point.x, point.y, height_); nearest_collision_point_.push_back(point_with_height); } @@ -139,7 +139,7 @@ void RunOutDebug::pushDetectionAreaPolygons(const Polygon2d & debug_polygon) { std::vector ros_points; for (const auto & p : debug_polygon.outer()) { - ros_points.push_back(autoware::universe_utils::createPoint(p.x(), p.y(), 0.0)); + ros_points.push_back(autoware_utils::create_point(p.x(), p.y(), 0.0)); } detection_area_polygons_.push_back(ros_points); @@ -149,7 +149,7 @@ void RunOutDebug::pushMandatoryDetectionAreaPolygons(const Polygon2d & debug_pol { std::vector ros_points; for (const auto & p : debug_polygon.outer()) { - ros_points.push_back(autoware::universe_utils::createPoint(p.x(), p.y(), 0.0)); + ros_points.push_back(autoware_utils::create_point(p.x(), p.y(), 0.0)); } mandatory_detection_area_polygons_.push_back(ros_points); @@ -210,9 +210,9 @@ visualization_msgs::msg::MarkerArray RunOutDebug::createVisualizationMarkerArray visualization_msgs::msg::MarkerArray msg; if (!collision_points_.empty()) { - auto marker = createDefaultMarker( + auto marker = create_default_marker( "map", current_time, "collision_points", 0, visualization_msgs::msg::Marker::SPHERE_LIST, - createMarkerScale(0.3, 0.3, 0.3), createMarkerColor(0, 0.0, 1.0, 0.999)); + create_marker_scale(0.3, 0.3, 0.3), create_marker_color(0, 0.0, 1.0, 0.999)); for (const auto & p : collision_points_) { marker.points.push_back(p); } @@ -220,10 +220,10 @@ visualization_msgs::msg::MarkerArray RunOutDebug::createVisualizationMarkerArray } if (!nearest_collision_point_.empty()) { - auto marker = createDefaultMarker( + auto marker = create_default_marker( "map", current_time, "nearest_collision_point", 0, - visualization_msgs::msg::Marker::SPHERE_LIST, createMarkerScale(0.6, 0.6, 0.6), - createMarkerColor(1.0, 0, 0, 0.999)); + visualization_msgs::msg::Marker::SPHERE_LIST, create_marker_scale(0.6, 0.6, 0.6), + create_marker_color(1.0, 0, 0, 0.999)); for (const auto & p : nearest_collision_point_) { marker.points.push_back(p); } @@ -231,10 +231,10 @@ visualization_msgs::msg::MarkerArray RunOutDebug::createVisualizationMarkerArray } if (!predicted_vehicle_polygons_.empty()) { - auto marker = createDefaultMarker( + auto marker = create_default_marker( "map", current_time, "predicted_vehicle_polygons", 0, - visualization_msgs::msg::Marker::LINE_STRIP, createMarkerScale(0.05, 0.0, 0.0), - createMarkerColor(0.0, 1.0, 0.0, 0.999)); + visualization_msgs::msg::Marker::LINE_STRIP, create_marker_scale(0.05, 0.0, 0.0), + create_marker_color(0.0, 1.0, 0.0, 0.999)); for (const auto & poly : predicted_vehicle_polygons_) { for (const auto & p : poly) { @@ -248,41 +248,41 @@ visualization_msgs::msg::MarkerArray RunOutDebug::createVisualizationMarkerArray } if (!predicted_obstacle_polygons_.empty()) { - appendMarkerArray( + append_marker_array( createPolygonMarkerArray( predicted_obstacle_polygons_, current_time, "predicted_obstacle_polygons", - createMarkerScale(0.02, 0.0, 0.0), createMarkerColor(1.0, 1.0, 0.0, 0.999), height_), + create_marker_scale(0.02, 0.0, 0.0), create_marker_color(1.0, 1.0, 0.0, 0.999), height_), &msg); } if (!collision_obstacle_polygons_.empty()) { - appendMarkerArray( + append_marker_array( createPolygonMarkerArray( collision_obstacle_polygons_, current_time, "collision_obstacle_polygons", - createMarkerScale(0.04, 0.0, 0.0), createMarkerColor(1.0, 0.0, 0.0, 0.999), height_), + create_marker_scale(0.04, 0.0, 0.0), create_marker_color(1.0, 0.0, 0.0, 0.999), height_), &msg); } if (!detection_area_polygons_.empty()) { - appendMarkerArray( + append_marker_array( createPolygonMarkerArray( detection_area_polygons_, current_time, "detection_area_polygons", - createMarkerScale(0.04, 0.0, 0.0), createMarkerColor(0.0, 0.0, 1.0, 0.999), height_), + create_marker_scale(0.04, 0.0, 0.0), create_marker_color(0.0, 0.0, 1.0, 0.999), height_), &msg); } if (!mandatory_detection_area_polygons_.empty()) { - appendMarkerArray( + append_marker_array( createPolygonMarkerArray( mandatory_detection_area_polygons_, current_time, "mandatory_detection_area_polygons", - createMarkerScale(0.04, 0.0, 0.0), createMarkerColor(1.0, 1.0, 0.0, 0.999), height_), + create_marker_scale(0.04, 0.0, 0.0), create_marker_color(1.0, 1.0, 0.0, 0.999), height_), &msg); } if (!ego_cut_line_.empty()) { - auto marker = createDefaultMarker( + auto marker = create_default_marker( "map", current_time, "ego_cut_line", 0, visualization_msgs::msg::Marker::LINE_LIST, - createMarkerScale(0.2, 0.2, 0.2), createMarkerColor(0.7, 0.0, 0.7, 0.999)); + create_marker_scale(0.2, 0.2, 0.2), create_marker_color(0.7, 0.0, 0.7, 0.999)); for (const auto & p : ego_cut_line_) { marker.points.push_back(p); } @@ -290,10 +290,10 @@ visualization_msgs::msg::MarkerArray RunOutDebug::createVisualizationMarkerArray } if (!travel_time_texts_.empty()) { - auto marker = createDefaultMarker( + auto marker = create_default_marker( "map", current_time, "travel_time_texts", 0, - visualization_msgs::msg::Marker::TEXT_VIEW_FACING, createMarkerScale(0.0, 0.0, 0.8), - createMarkerColor(1.0, 1.0, 1.0, 0.999)); + visualization_msgs::msg::Marker::TEXT_VIEW_FACING, create_marker_scale(0.0, 0.0, 0.8), + create_marker_color(1.0, 1.0, 1.0, 0.999)); constexpr float height_offset = 2.0; for (const auto & text : travel_time_texts_) { diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/dynamic_obstacle.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/dynamic_obstacle.cpp index a15f232b75a37..201c060ad50aa 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/dynamic_obstacle.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/dynamic_obstacle.cpp @@ -15,11 +15,11 @@ #include "dynamic_obstacle.hpp" #include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include #include #include @@ -42,13 +42,12 @@ geometry_msgs::msg::Quaternion createQuaternionFacingToTrajectory( const auto nearest_idx = autoware::motion_utils::findNearestIndex(path_points, point); const auto & nearest_pose = path_points.at(nearest_idx).point.pose; - const auto longitudinal_offset = - autoware::universe_utils::calcLongitudinalDeviation(nearest_pose, point); + const auto longitudinal_offset = autoware_utils::calc_longitudinal_deviation(nearest_pose, point); const auto vertical_point = - autoware::universe_utils::calcOffsetPose(nearest_pose, longitudinal_offset, 0, 0).position; - const auto azimuth_angle = autoware::universe_utils::calcAzimuthAngle(point, vertical_point); + autoware_utils::calc_offset_pose(nearest_pose, longitudinal_offset, 0, 0).position; + const auto azimuth_angle = autoware_utils::calc_azimuth_angle(point, vertical_point); - return autoware::universe_utils::createQuaternionFromYaw(azimuth_angle); + return autoware_utils::create_quaternion_from_yaw(azimuth_angle); } // create predicted path assuming that obstacles move with constant velocity @@ -60,8 +59,7 @@ std::vector createPredictedPath( std::vector path_points; for (size_t i = 0; i < path_size; i++) { const float travel_dist = max_velocity_mps * time_step * i; - const auto predicted_pose = - autoware::universe_utils::calcOffsetPose(initial_pose, travel_dist, 0, 0); + const auto predicted_pose = autoware_utils::calc_offset_pose(initial_pose, travel_dist, 0, 0); path_points.emplace_back(predicted_pose); } @@ -115,7 +113,7 @@ pcl::PointCloud applyVoxelGridFilter( bool isAheadOf( const geometry_msgs::msg::Point & target_point, const geometry_msgs::msg::Pose & base_pose) { - return autoware::universe_utils::calcLongitudinalDeviation(base_pose, target_point) > 0; + return autoware_utils::calc_longitudinal_deviation(base_pose, target_point) > 0; } pcl::PointCloud extractObstaclePointsWithinPolygon( @@ -134,7 +132,7 @@ pcl::PointCloud extractObstaclePointsWithinPolygon( pcl::PointCloud output_points; output_points.header = input_points.header; for (const auto & poly : polys) { - const auto bounding_box = bg::return_envelope(poly); + const auto bounding_box = bg::return_envelope(poly); for (const auto & p : input_points) { Point2d point(p.x, p.y); @@ -163,7 +161,7 @@ std::vector> groupPointsWithNearestSegmentIndex( points_with_index.resize(path_points.size()); for (const auto & p : input_points.points) { - const auto ros_point = autoware::universe_utils::createPoint(p.x, p.y, p.z); + const auto ros_point = autoware_utils::create_point(p.x, p.y, p.z); const size_t nearest_seg_idx = autoware::motion_utils::findNearestSegmentIndex(path_points, ros_point); @@ -187,10 +185,10 @@ pcl::PointXYZ calculateLateralNearestPoint( { const auto lateral_nearest_point = std::min_element( input_points.points.begin(), input_points.points.end(), [&](const auto & p1, const auto & p2) { - const auto lateral_deviation_p1 = std::abs(autoware::universe_utils::calcLateralDeviation( - base_pose, autoware::universe_utils::createPoint(p1.x, p1.y, 0))); - const auto lateral_deviation_p2 = std::abs(autoware::universe_utils::calcLateralDeviation( - base_pose, autoware::universe_utils::createPoint(p2.x, p2.y, 0))); + const auto lateral_deviation_p1 = std::abs(autoware_utils::calc_lateral_deviation( + base_pose, autoware_utils::create_point(p1.x, p1.y, 0))); + const auto lateral_deviation_p2 = std::abs(autoware_utils::calc_lateral_deviation( + base_pose, autoware_utils::create_point(p2.x, p2.y, 0))); return lateral_deviation_p1 < lateral_deviation_p2; }); @@ -265,7 +263,7 @@ pcl::PointCloud transformPointCloud( pcl::PointCloud pointcloud_pcl; pcl::fromROSMsg(input_pointcloud, pointcloud_pcl); pcl::PointCloud pointcloud_pcl_transformed; - autoware::universe_utils::transformPointCloud( + autoware_utils::transform_pointcloud( pointcloud_pcl, pointcloud_pcl_transformed, transform_matrix); return pointcloud_pcl_transformed; @@ -362,8 +360,8 @@ std::vector DynamicObstacleCreatorForObject::createDynamicObsta dynamic_obstacle.pose = predicted_object.kinematics.initial_pose_with_covariance.pose; if (param_.assume_fixed_velocity) { - dynamic_obstacle.min_velocity_mps = autoware::universe_utils::kmph2mps(param_.min_vel_kmph); - dynamic_obstacle.max_velocity_mps = autoware::universe_utils::kmph2mps(param_.max_vel_kmph); + dynamic_obstacle.min_velocity_mps = autoware_utils::kmph2mps(param_.min_vel_kmph); + dynamic_obstacle.max_velocity_mps = autoware_utils::kmph2mps(param_.max_vel_kmph); } else { calculateMinAndMaxVelFromCovariance( predicted_object.kinematics.initial_twist_with_covariance, param_.std_dev_multiplier, @@ -405,8 +403,8 @@ std::vector DynamicObstacleCreatorForObjectWithoutPath::createD dynamic_obstacle.pose.orientation = createQuaternionFacingToTrajectory( dynamic_obstacle_data_.path.points, dynamic_obstacle.pose.position); - dynamic_obstacle.min_velocity_mps = autoware::universe_utils::kmph2mps(param_.min_vel_kmph); - dynamic_obstacle.max_velocity_mps = autoware::universe_utils::kmph2mps(param_.max_vel_kmph); + dynamic_obstacle.min_velocity_mps = autoware_utils::kmph2mps(param_.min_vel_kmph); + dynamic_obstacle.max_velocity_mps = autoware_utils::kmph2mps(param_.max_vel_kmph); dynamic_obstacle.classifications = predicted_object.classification; dynamic_obstacle.shape = predicted_object.shape; @@ -464,13 +462,12 @@ std::vector DynamicObstacleCreatorForPoints::createDynamicObsta DynamicObstacle dynamic_obstacle; // create pose facing the direction of the lane - dynamic_obstacle.pose.position = - autoware::universe_utils::createPoint(point.x, point.y, point.z); + dynamic_obstacle.pose.position = autoware_utils::create_point(point.x, point.y, point.z); dynamic_obstacle.pose.orientation = createQuaternionFacingToTrajectory( dynamic_obstacle_data_.path.points, dynamic_obstacle.pose.position); - dynamic_obstacle.min_velocity_mps = autoware::universe_utils::kmph2mps(param_.min_vel_kmph); - dynamic_obstacle.max_velocity_mps = autoware::universe_utils::kmph2mps(param_.max_vel_kmph); + dynamic_obstacle.min_velocity_mps = autoware_utils::kmph2mps(param_.min_vel_kmph); + dynamic_obstacle.max_velocity_mps = autoware_utils::kmph2mps(param_.max_vel_kmph); // create classification of points as pedestrian ObjectClassification classification; @@ -491,7 +488,7 @@ std::vector DynamicObstacleCreatorForPoints::createDynamicObsta param_.max_prediction_time); predicted_path.confidence = 1.0; dynamic_obstacle.predicted_paths.emplace_back(predicted_path); - dynamic_obstacle.uuid = autoware::universe_utils::generateUUID(); + dynamic_obstacle.uuid = autoware_utils::generate_uuid(); dynamic_obstacles.emplace_back(dynamic_obstacle); } diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/manager.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/manager.cpp index f9ddcf9b4c747..964ce93fab58c 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/manager.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/manager.cpp @@ -14,7 +14,7 @@ #include "manager.hpp" -#include +#include #include #include @@ -23,7 +23,7 @@ namespace autoware::behavior_velocity_planner { -using autoware::universe_utils::getOrDeclareParameter; +using autoware_utils::get_or_declare_parameter; RunOutModuleManager::RunOutModuleManager(rclcpp::Node & node) : SceneModuleManagerInterface(node, getModuleName()) { @@ -49,97 +49,97 @@ RunOutModuleManager::RunOutModuleManager(rclcpp::Node & node) { auto & p = planner_param_.common; - p.normal_min_jerk = getOrDeclareParameter(node, "normal.min_jerk"); - p.normal_min_acc = getOrDeclareParameter(node, "normal.min_acc"); - p.limit_min_jerk = getOrDeclareParameter(node, "limit.min_jerk"); - p.limit_min_acc = getOrDeclareParameter(node, "limit.min_acc"); + p.normal_min_jerk = get_or_declare_parameter(node, "normal.min_jerk"); + p.normal_min_acc = get_or_declare_parameter(node, "normal.min_acc"); + p.limit_min_jerk = get_or_declare_parameter(node, "limit.min_jerk"); + p.limit_min_acc = get_or_declare_parameter(node, "limit.min_acc"); } { auto & p = planner_param_.run_out; - p.detection_method = getOrDeclareParameter(node, ns + ".detection_method"); + p.detection_method = get_or_declare_parameter(node, ns + ".detection_method"); p.target_obstacle_types = - getOrDeclareParameter>(node, ns + ".target_obstacle_types"); - p.use_partition_lanelet = getOrDeclareParameter(node, ns + ".use_partition_lanelet"); - p.use_ego_cut_line = getOrDeclareParameter(node, ns + ".use_ego_cut_line"); + get_or_declare_parameter>(node, ns + ".target_obstacle_types"); + p.use_partition_lanelet = get_or_declare_parameter(node, ns + ".use_partition_lanelet"); + p.use_ego_cut_line = get_or_declare_parameter(node, ns + ".use_ego_cut_line"); p.exclude_obstacles_already_in_path = - getOrDeclareParameter(node, ns + ".exclude_obstacles_already_in_path"); - p.suppress_on_crosswalk = getOrDeclareParameter(node, ns + ".suppress_on_crosswalk"); - p.specify_decel_jerk = getOrDeclareParameter(node, ns + ".specify_decel_jerk"); - p.stop_margin = getOrDeclareParameter(node, ns + ".stop_margin"); - p.passing_margin = getOrDeclareParameter(node, ns + ".passing_margin"); - p.deceleration_jerk = getOrDeclareParameter(node, ns + ".deceleration_jerk"); - p.detection_distance = getOrDeclareParameter(node, ns + ".detection_distance"); - p.detection_span = getOrDeclareParameter(node, ns + ".detection_span"); - p.min_vel_ego_kmph = getOrDeclareParameter(node, ns + ".min_vel_ego_kmph"); - p.ego_cut_line_length = getOrDeclareParameter(node, ns + ".ego_cut_line_length"); + get_or_declare_parameter(node, ns + ".exclude_obstacles_already_in_path"); + p.suppress_on_crosswalk = get_or_declare_parameter(node, ns + ".suppress_on_crosswalk"); + p.specify_decel_jerk = get_or_declare_parameter(node, ns + ".specify_decel_jerk"); + p.stop_margin = get_or_declare_parameter(node, ns + ".stop_margin"); + p.passing_margin = get_or_declare_parameter(node, ns + ".passing_margin"); + p.deceleration_jerk = get_or_declare_parameter(node, ns + ".deceleration_jerk"); + p.detection_distance = get_or_declare_parameter(node, ns + ".detection_distance"); + p.detection_span = get_or_declare_parameter(node, ns + ".detection_span"); + p.min_vel_ego_kmph = get_or_declare_parameter(node, ns + ".min_vel_ego_kmph"); + p.ego_cut_line_length = get_or_declare_parameter(node, ns + ".ego_cut_line_length"); p.ego_footprint_extra_margin = - getOrDeclareParameter(node, ns + ".ego_footprint_extra_margin"); + get_or_declare_parameter(node, ns + ".ego_footprint_extra_margin"); p.keep_obstacle_on_path_time_threshold = - getOrDeclareParameter(node, ns + ".keep_obstacle_on_path_time_threshold"); - p.keep_stop_point_time = getOrDeclareParameter(node, ns + ".keep_stop_point_time"); + get_or_declare_parameter(node, ns + ".keep_obstacle_on_path_time_threshold"); + p.keep_stop_point_time = get_or_declare_parameter(node, ns + ".keep_stop_point_time"); } { auto & p = planner_param_.detection_area; const std::string ns_da = ns + ".detection_area"; - p.margin_ahead = getOrDeclareParameter(node, ns_da + ".margin_ahead"); - p.margin_behind = getOrDeclareParameter(node, ns_da + ".margin_behind"); + p.margin_ahead = get_or_declare_parameter(node, ns_da + ".margin_ahead"); + p.margin_behind = get_or_declare_parameter(node, ns_da + ".margin_behind"); } { auto & p = planner_param_.mandatory_area; const std::string ns_da = ns + ".mandatory_area"; - p.decel_jerk = getOrDeclareParameter(node, ns_da + ".decel_jerk"); + p.decel_jerk = get_or_declare_parameter(node, ns_da + ".decel_jerk"); } { auto & p = planner_param_.dynamic_obstacle; const std::string ns_do = ns + ".dynamic_obstacle"; - p.use_mandatory_area = getOrDeclareParameter(node, ns_do + ".use_mandatory_area"); + p.use_mandatory_area = get_or_declare_parameter(node, ns_do + ".use_mandatory_area"); p.assume_fixed_velocity = - getOrDeclareParameter(node, ns_do + ".assume_fixed_velocity.enable"); + get_or_declare_parameter(node, ns_do + ".assume_fixed_velocity.enable"); p.min_vel_kmph = - getOrDeclareParameter(node, ns_do + ".assume_fixed_velocity.min_vel_kmph"); + get_or_declare_parameter(node, ns_do + ".assume_fixed_velocity.min_vel_kmph"); p.max_vel_kmph = - getOrDeclareParameter(node, ns_do + ".assume_fixed_velocity.max_vel_kmph"); - p.std_dev_multiplier = getOrDeclareParameter(node, ns_do + ".std_dev_multiplier"); - p.diameter = getOrDeclareParameter(node, ns_do + ".diameter"); - p.height = getOrDeclareParameter(node, ns_do + ".height"); - p.max_prediction_time = getOrDeclareParameter(node, ns_do + ".max_prediction_time"); - p.time_step = getOrDeclareParameter(node, ns_do + ".time_step"); - p.points_interval = getOrDeclareParameter(node, ns_do + ".points_interval"); + get_or_declare_parameter(node, ns_do + ".assume_fixed_velocity.max_vel_kmph"); + p.std_dev_multiplier = get_or_declare_parameter(node, ns_do + ".std_dev_multiplier"); + p.diameter = get_or_declare_parameter(node, ns_do + ".diameter"); + p.height = get_or_declare_parameter(node, ns_do + ".height"); + p.max_prediction_time = get_or_declare_parameter(node, ns_do + ".max_prediction_time"); + p.time_step = get_or_declare_parameter(node, ns_do + ".time_step"); + p.points_interval = get_or_declare_parameter(node, ns_do + ".points_interval"); } { auto & p = planner_param_.approaching; const std::string ns_a = ns + ".approaching"; - p.enable = getOrDeclareParameter(node, ns_a + ".enable"); - p.margin = getOrDeclareParameter(node, ns_a + ".margin"); - p.limit_vel_kmph = getOrDeclareParameter(node, ns_a + ".limit_vel_kmph"); + p.enable = get_or_declare_parameter(node, ns_a + ".enable"); + p.margin = get_or_declare_parameter(node, ns_a + ".margin"); + p.limit_vel_kmph = get_or_declare_parameter(node, ns_a + ".limit_vel_kmph"); const std::string ns_as = ns_a + ".state"; - p.state.stop_thresh = getOrDeclareParameter(node, ns_as + ".stop_thresh"); - p.state.stop_time_thresh = getOrDeclareParameter(node, ns_as + ".stop_time_thresh"); + p.state.stop_thresh = get_or_declare_parameter(node, ns_as + ".stop_thresh"); + p.state.stop_time_thresh = get_or_declare_parameter(node, ns_as + ".stop_time_thresh"); p.state.disable_approach_dist = - getOrDeclareParameter(node, ns_as + ".disable_approach_dist"); + get_or_declare_parameter(node, ns_as + ".disable_approach_dist"); p.state.keep_approach_duration = - getOrDeclareParameter(node, ns_as + ".keep_approach_duration"); + get_or_declare_parameter(node, ns_as + ".keep_approach_duration"); } { auto & p = planner_param_.slow_down_limit; const std::string ns_m = ns + ".slow_down_limit"; - p.enable = getOrDeclareParameter(node, ns_m + ".enable"); - p.max_jerk = getOrDeclareParameter(node, ns_m + ".max_jerk"); - p.max_acc = getOrDeclareParameter(node, ns_m + ".max_acc"); + p.enable = get_or_declare_parameter(node, ns_m + ".enable"); + p.max_jerk = get_or_declare_parameter(node, ns_m + ".max_jerk"); + p.max_acc = get_or_declare_parameter(node, ns_m + ".max_acc"); } { auto & p = planner_param_.ignore_momentary_detection; const std::string ns_param = ns + ".ignore_momentary_detection"; - p.enable = getOrDeclareParameter(node, ns_param + ".enable"); - p.time_threshold = getOrDeclareParameter(node, ns_param + ".time_threshold"); + p.enable = get_or_declare_parameter(node, ns_param + ".enable"); + p.time_threshold = get_or_declare_parameter(node, ns_param + ".time_threshold"); } debug_ptr_ = std::make_shared(node); diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/scene.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/scene.cpp index 32cdddc55634b..e9165926f829d 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/scene.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/scene.cpp @@ -21,9 +21,9 @@ #include #include #include -#include -#include -#include +#include +#include +#include #include #include @@ -47,7 +47,7 @@ RunOutModule::RunOutModule( const PlannerParam & planner_param, const rclcpp::Logger logger, std::unique_ptr dynamic_obstacle_creator, const std::shared_ptr & debug_ptr, const rclcpp::Clock::SharedPtr clock, - const std::shared_ptr time_keeper, + const std::shared_ptr time_keeper, const std::shared_ptr planning_factor_interface) : SceneModuleInterface(module_id, logger, clock, time_keeper, planning_factor_interface), @@ -179,7 +179,7 @@ bool RunOutModule::modifyPathVelocity(PathWithLaneId * path) insertStopPoint(last_stop_point_, *path); // debug debug_ptr_->setAccelReason(RunOutDebug::AccelReason::STOP); - debug_ptr_->pushStopPose(autoware::universe_utils::calcOffsetPose( + debug_ptr_->pushStopPose(autoware_utils::calc_offset_pose( *last_stop_point_, planner_param_.vehicle_param.base_to_front, 0, 0)); } } @@ -219,7 +219,7 @@ std::optional RunOutModule::detectCollision( const auto & p2 = path.points.at(idx).point; const float prev_vel = std::max(p1.longitudinal_velocity_mps, planner_param_.run_out.min_vel_ego_kmph / 3.6f); - const float ds = autoware::universe_utils::calcDistance2d(p1, p2); + const float ds = autoware_utils::calc_distance2d(p1, p2); // calculate travel time from nearest point to p2 travel_time += ds / prev_vel; @@ -313,10 +313,10 @@ std::optional RunOutModule::findNearestCollisionObstacle( float RunOutModule::calcCollisionPositionOfVehicleSide( const geometry_msgs::msg::Point & point, const geometry_msgs::msg::Pose & base_pose) const { - const auto vehicle_front_pose = autoware::universe_utils::calcOffsetPose( - base_pose, planner_param_.vehicle_param.base_to_front, 0, 0); + const auto vehicle_front_pose = + autoware_utils::calc_offset_pose(base_pose, planner_param_.vehicle_param.base_to_front, 0, 0); const auto longitudinal_offset_from_front = - std::abs(autoware::universe_utils::calcLongitudinalDeviation(vehicle_front_pose, point)); + std::abs(autoware_utils::calc_longitudinal_deviation(vehicle_front_pose, point)); return longitudinal_offset_from_front; } @@ -338,11 +338,11 @@ std::vector RunOutModule::createVehiclePolygon( const double base_to_left = (planner_param_.vehicle_param.wheel_tread / 2.0) + planner_param_.vehicle_param.left_overhang; - using autoware::universe_utils::calcOffsetPose; - const auto p1 = calcOffsetPose(base_pose, base_to_front, base_to_left, 0.0); - const auto p2 = calcOffsetPose(base_pose, base_to_front, -base_to_right, 0.0); - const auto p3 = calcOffsetPose(base_pose, -base_to_rear, -base_to_right, 0.0); - const auto p4 = calcOffsetPose(base_pose, -base_to_rear, base_to_left, 0.0); + using autoware_utils::calc_offset_pose; + const auto p1 = calc_offset_pose(base_pose, base_to_front, base_to_left, 0.0); + const auto p2 = calc_offset_pose(base_pose, base_to_front, -base_to_right, 0.0); + const auto p3 = calc_offset_pose(base_pose, -base_to_rear, -base_to_right, 0.0); + const auto p4 = calc_offset_pose(base_pose, -base_to_rear, base_to_left, 0.0); std::vector vehicle_poly; vehicle_poly.push_back(p1.position); @@ -357,7 +357,7 @@ std::vector RunOutModule::excludeObstaclesOnEgoPath( const std::vector & dynamic_obstacles, const PathWithLaneId & path) { using autoware::motion_utils::findNearestIndex; - using autoware::universe_utils::calcOffsetPose; + using autoware_utils::calc_offset_pose; if (!planner_param_.run_out.exclude_obstacles_already_in_path || path.points.empty()) { return dynamic_obstacles; } @@ -376,18 +376,18 @@ std::vector RunOutModule::excludeObstaclesOnEgoPath( } const auto object_position = o.pose.position; const auto closest_ego_pose = path.points.at(*idx).point.pose; - const auto vehicle_left_pose = autoware::universe_utils::calcOffsetPose( - closest_ego_pose, 0, vehicle_with_with_margin_halved, 0); - const auto vehicle_right_pose = autoware::universe_utils::calcOffsetPose( - closest_ego_pose, 0, -vehicle_with_with_margin_halved, 0); + const auto vehicle_left_pose = + autoware_utils::calc_offset_pose(closest_ego_pose, 0, vehicle_with_with_margin_halved, 0); + const auto vehicle_right_pose = + autoware_utils::calc_offset_pose(closest_ego_pose, 0, -vehicle_with_with_margin_halved, 0); const double signed_distance_from_left = - autoware::universe_utils::calcLateralDeviation(vehicle_left_pose, object_position); + autoware_utils::calc_lateral_deviation(vehicle_left_pose, object_position); const double signed_distance_from_right = - autoware::universe_utils::calcLateralDeviation(vehicle_right_pose, object_position); + autoware_utils::calc_lateral_deviation(vehicle_right_pose, object_position); // If object is outside of the ego path, include it in list of possible target objects // It is also deleted from the path of objects inside ego path - const auto obstacle_uuid_hex = autoware::universe_utils::toHexString(o.uuid); + const auto obstacle_uuid_hex = autoware_utils::to_hex_string(o.uuid); if (signed_distance_from_left > 0.0 || signed_distance_from_right < 0.0) { obstacles_outside_of_path.push_back(o); obstacle_in_ego_path_times_.erase(obstacle_uuid_hex); @@ -479,7 +479,7 @@ std::optional RunOutModule::calcPredictedObstaclePose( const auto & p1 = predicted_path.at(i - 1); const auto & p2 = predicted_path.at(i); - const float ds = autoware::universe_utils::calcDistance2d(p1, p2); + const float ds = autoware_utils::calc_distance2d(p1, p2); const float dt = ds / velocity_mps; // apply linear interpolation between the predicted path points @@ -552,7 +552,7 @@ bool RunOutModule::checkCollisionWithCylinder( run_out_utils::createBoostPolyFromMsg(bounding_box_for_points); // check collision with 2d polygon - std::vector collision_points_bg; + std::vector collision_points_bg; bg::intersection(vehicle_polygon, bg_bounding_box_for_points, collision_points_bg); // no collision detected @@ -565,7 +565,7 @@ bool RunOutModule::checkCollisionWithCylinder( debug_ptr_->pushCollisionObstaclePolygons(bounding_box_for_points); for (const auto & p : collision_points_bg) { const auto p_msg = - autoware::universe_utils::createPoint(p.x(), p.y(), pose_with_range.pose_min.position.z); + autoware_utils::create_point(p.x(), p.y(), pose_with_range.pose_min.position.z); collision_points.emplace_back(p_msg); } @@ -578,30 +578,30 @@ std::vector RunOutModule::createBoundingBoxForRangedP const PoseWithRange & pose_with_range, const float x_offset, const float y_offset) const { const auto dist_p1_p2 = - autoware::universe_utils::calcDistance2d(pose_with_range.pose_min, pose_with_range.pose_max); + autoware_utils::calc_distance2d(pose_with_range.pose_min, pose_with_range.pose_max); geometry_msgs::msg::Pose p_min_to_p_max; if (dist_p1_p2 < std::numeric_limits::epsilon()) { // can't calculate the angle if two points are the same p_min_to_p_max = pose_with_range.pose_min; } else { - const auto azimuth_angle = autoware::universe_utils::calcAzimuthAngle( + const auto azimuth_angle = autoware_utils::calc_azimuth_angle( pose_with_range.pose_min.position, pose_with_range.pose_max.position); p_min_to_p_max.position = pose_with_range.pose_min.position; - p_min_to_p_max.orientation = autoware::universe_utils::createQuaternionFromYaw(azimuth_angle); + p_min_to_p_max.orientation = autoware_utils::create_quaternion_from_yaw(azimuth_angle); } std::vector poly; poly.emplace_back( - autoware::universe_utils::calcOffsetPose(p_min_to_p_max, dist_p1_p2 + x_offset, y_offset, 0.0) + autoware_utils::calc_offset_pose(p_min_to_p_max, dist_p1_p2 + x_offset, y_offset, 0.0) .position); poly.emplace_back( - autoware::universe_utils::calcOffsetPose(p_min_to_p_max, dist_p1_p2 + x_offset, -y_offset, 0.0) + autoware_utils::calc_offset_pose(p_min_to_p_max, dist_p1_p2 + x_offset, -y_offset, 0.0) .position); poly.emplace_back( - autoware::universe_utils::calcOffsetPose(p_min_to_p_max, -x_offset, -y_offset, 0.0).position); + autoware_utils::calc_offset_pose(p_min_to_p_max, -x_offset, -y_offset, 0.0).position); poly.emplace_back( - autoware::universe_utils::calcOffsetPose(p_min_to_p_max, -x_offset, y_offset, 0.0).position); + autoware_utils::calc_offset_pose(p_min_to_p_max, -x_offset, y_offset, 0.0).position); return poly; } @@ -617,7 +617,7 @@ bool RunOutModule::checkCollisionWithBoundingBox( const auto bg_bounding_box = run_out_utils::createBoostPolyFromMsg(bounding_box); // check collision with 2d polygon - std::vector collision_points_bg; + std::vector collision_points_bg; bg::intersection(vehicle_polygon, bg_bounding_box, collision_points_bg); // no collision detected @@ -630,7 +630,7 @@ bool RunOutModule::checkCollisionWithBoundingBox( debug_ptr_->pushCollisionObstaclePolygons(bounding_box); for (const auto & p : collision_points_bg) { const auto p_msg = - autoware::universe_utils::createPoint(p.x(), p.y(), pose_with_range.pose_min.position.z); + autoware_utils::create_point(p.x(), p.y(), pose_with_range.pose_min.position.z); collision_points.emplace_back(p_msg); } @@ -675,7 +675,7 @@ std::optional RunOutModule::calcStopPoint( // debug debug_ptr_->setAccelReason(RunOutDebug::AccelReason::STOP); - debug_ptr_->pushStopPose(autoware::universe_utils::calcOffsetPose( + debug_ptr_->pushStopPose(autoware_utils::calc_offset_pose( *stop_point, planner_param_.vehicle_param.base_to_front, 0, 0)); return stop_point; @@ -733,7 +733,7 @@ std::optional RunOutModule::calcStopPoint( // debug debug_ptr_->setAccelReason(RunOutDebug::AccelReason::STOP); - debug_ptr_->pushStopPose(autoware::universe_utils::calcOffsetPose( + debug_ptr_->pushStopPose(autoware_utils::calc_offset_pose( *stop_point, planner_param_.vehicle_param.base_to_front, 0, 0)); return stop_point; @@ -888,7 +888,7 @@ void RunOutModule::insertApproachingVelocity( 0.0 /*shift_distance*/, "run_out_approaching_velocity"); // debug - debug_ptr_->pushStopPose(autoware::universe_utils::calcOffsetPose( + debug_ptr_->pushStopPose(autoware_utils::calc_offset_pose( *stop_point, planner_param_.vehicle_param.base_to_front, 0, 0)); const auto nearest_seg_idx_stop = diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/scene.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/scene.hpp index dc7061b906542..b27ee3bebb3ce 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/scene.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/scene.hpp @@ -48,7 +48,7 @@ class RunOutModule : public SceneModuleInterface const PlannerParam & planner_param, const rclcpp::Logger logger, std::unique_ptr dynamic_obstacle_creator, const std::shared_ptr & debug_ptr, const rclcpp::Clock::SharedPtr clock, - const std::shared_ptr time_keeper, + const std::shared_ptr time_keeper, const std::shared_ptr planning_factor_interface); diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/utils.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/utils.cpp index 5c787234aafe1..bf85b651304f4 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/utils.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/utils.cpp @@ -16,7 +16,7 @@ #include #include -#include +#include #include #include @@ -35,7 +35,7 @@ #include #endif #include -#include +#include namespace autoware::behavior_velocity_planner { namespace run_out_utils @@ -104,15 +104,13 @@ std::vector findLateralSameSidePoints( const std::vector & points, const geometry_msgs::msg::Pose & base_pose, const geometry_msgs::msg::Point & target_point) { - const auto signed_deviation = - autoware::universe_utils::calcLateralDeviation(base_pose, target_point); + const auto signed_deviation = autoware_utils::calc_lateral_deviation(base_pose, target_point); RCLCPP_DEBUG_STREAM( rclcpp::get_logger("findLateralSameSidePoints"), "signed dev of target: " << signed_deviation); std::vector same_side_points; for (const auto & p : points) { - const auto signed_deviation_of_point = - autoware::universe_utils::calcLateralDeviation(base_pose, p); + const auto signed_deviation_of_point = autoware_utils::calc_lateral_deviation(base_pose, p); RCLCPP_DEBUG_STREAM( rclcpp::get_logger("findLateralSameSidePoints"), "signed dev of point: " << signed_deviation_of_point); @@ -130,7 +128,7 @@ std::vector findLateralSameSidePoints( bool isSamePoint(const geometry_msgs::msg::Point & p1, const geometry_msgs::msg::Point & p2) { - return (autoware::universe_utils::calcDistance2d(p1, p2) < std::numeric_limits::epsilon()); + return (autoware_utils::calc_distance2d(p1, p2) < std::numeric_limits::epsilon()); } // insert path velocity which doesn't exceed original velocity @@ -168,16 +166,14 @@ bool pathIntersectsEgoCutLine( const double half_line_length, std::vector & ego_cut_line) { if (path.size() < 2) return false; - const auto p1 = - autoware::universe_utils::calcOffsetPose(ego_pose, 0.0, half_line_length, 0.0).position; - const auto p2 = - autoware::universe_utils::calcOffsetPose(ego_pose, 0.0, -half_line_length, 0.0).position; + const auto p1 = autoware_utils::calc_offset_pose(ego_pose, 0.0, half_line_length, 0.0).position; + const auto p2 = autoware_utils::calc_offset_pose(ego_pose, 0.0, -half_line_length, 0.0).position; ego_cut_line = {p1, p2}; for (size_t i = 1; i < path.size(); ++i) { const auto & p3 = path.at(i).position; const auto & p4 = path.at(i - 1).position; - const auto intersection = autoware::universe_utils::intersect(p1, p2, p3, p4); + const auto intersection = autoware_utils::intersect(p1, p2, p3, p4); if (intersection.has_value()) { return true; } @@ -240,7 +236,7 @@ PathPointsWithLaneId decimatePathPoints( for (size_t i = 1; i < input_path_points.size(); i++) { const auto p1 = input_path_points.at(i - 1); const auto p2 = input_path_points.at(i); - const auto dist = autoware::universe_utils::calcDistance2d(p1, p2); + const auto dist = autoware_utils::calc_distance2d(p1, p2); dist_sum += dist; if (dist_sum > step) { @@ -269,8 +265,7 @@ PathWithLaneId trimPathFromSelfPose( output.points.push_back(input.points.at(i)); if (i != nearest_idx) { - dist_sum += - autoware::universe_utils::calcDistance2d(input.points.at(i - 1), input.points.at(i)); + dist_sum += autoware_utils::calc_distance2d(input.points.at(i - 1), input.points.at(i)); } if (dist_sum > trim_distance) { @@ -314,7 +309,7 @@ PathPointWithLaneId createExtendPathPoint( { PathPointWithLaneId extend_path_point = goal_point; extend_path_point.point.pose = - autoware::universe_utils::calcOffsetPose(goal_point.point.pose, extend_distance, 0.0, 0.0); + autoware_utils::calc_offset_pose(goal_point.point.pose, extend_distance, 0.0, 0.0); return extend_path_point; } diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/utils.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/utils.hpp index 755df2ed5ac82..295e7067b1b11 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/utils.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/utils.hpp @@ -15,7 +15,7 @@ #ifndef UTILS_HPP_ #define UTILS_HPP_ -#include "autoware/universe_utils/geometry/geometry.hpp" +#include "autoware_utils/geometry/geometry.hpp" #include #include @@ -34,10 +34,6 @@ namespace autoware::behavior_velocity_planner namespace run_out_utils { namespace bg = boost::geometry; -using autoware::universe_utils::Box2d; -using autoware::universe_utils::LineString2d; -using autoware::universe_utils::Point2d; -using autoware::universe_utils::Polygon2d; using autoware::vehicle_info_utils::VehicleInfo; using autoware_internal_debug_msgs::msg::Float32Stamped; using autoware_internal_planning_msgs::msg::PathWithLaneId; @@ -45,6 +41,10 @@ using autoware_perception_msgs::msg::ObjectClassification; using autoware_perception_msgs::msg::PredictedObjects; using autoware_perception_msgs::msg::Shape; using autoware_planning_msgs::msg::PathPoint; +using autoware_utils::Box2d; +using autoware_utils::LineString2d; +using autoware_utils::Point2d; +using autoware_utils::Polygon2d; using PathPointsWithLaneId = std::vector; struct CommonParam { diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/test/test_dynamic_obstacle.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/test/test_dynamic_obstacle.cpp index 948ee17421eac..271c08ac43615 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/test/test_dynamic_obstacle.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/test/test_dynamic_obstacle.cpp @@ -19,11 +19,11 @@ #include "utils.hpp" #include -#include -#include -#include -#include #include +#include +#include +#include +#include #include #include #include @@ -46,9 +46,9 @@ #include #include -using autoware::universe_utils::Point2d; -using autoware::universe_utils::Polygon2d; using autoware_perception_msgs::msg::ObjectClassification; +using autoware_utils::Point2d; +using autoware_utils::Polygon2d; using geometry_msgs::msg::Point; using Polygons2d = std::vector; @@ -160,8 +160,8 @@ TEST_F(TestDynamicObstacleMethods, testCreateQuaternionFacingToTrajectory) point.y = -2.0; const auto quaternion_facing_traj = createQuaternionFacingToTrajectory(path, point); - const auto rpy = autoware::universe_utils::getRPY(quaternion_facing_traj); - const auto yaw = autoware::universe_utils::normalizeRadian(rpy.z); + const auto rpy = autoware_utils::get_rpy(quaternion_facing_traj); + const auto yaw = autoware_utils::normalize_radian(rpy.z); EXPECT_DOUBLE_EQ(yaw, M_PI_2); geometry_msgs::msg::Pose geom_base_point; geom_base_point.position = base_point.point.pose.position; @@ -175,8 +175,8 @@ TEST_F(TestDynamicObstacleMethods, testCreateQuaternionFacingToTrajectory) point.y = -0.25; const auto quaternion_facing_traj = createQuaternionFacingToTrajectory(path, point); - const auto rpy = autoware::universe_utils::getRPY(quaternion_facing_traj); - const auto yaw = autoware::universe_utils::normalizeRadian(rpy.z); + const auto rpy = autoware_utils::get_rpy(quaternion_facing_traj); + const auto yaw = autoware_utils::normalize_radian(rpy.z); EXPECT_DOUBLE_EQ(std::abs(yaw), M_PI_2); geometry_msgs::msg::Pose geom_base_point; geom_base_point.position = base_point.point.pose.position; @@ -303,9 +303,8 @@ TEST_F(TestDynamicObstacleMethods, testCalculateLateralNearestPoint) for (size_t i = 0; i < lateral_nearest_points.size(); ++i) { const auto p = path.at(i); const auto curr_nearest_point = lateral_nearest_points.at(i); - auto deviation = std::abs(autoware::universe_utils::calcLateralDeviation( - p.point.pose, - autoware::universe_utils::createPoint(curr_nearest_point.x, curr_nearest_point.y, 0))); + auto deviation = std::abs(autoware_utils::calc_lateral_deviation( + p.point.pose, autoware_utils::create_point(curr_nearest_point.x, curr_nearest_point.y, 0))); EXPECT_DOUBLE_EQ(deviation, 0.0); } diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/test/test_path_utils.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/test/test_path_utils.cpp index 66f4a6c71f6fc..df90b4032dfa1 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/test/test_path_utils.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/test/test_path_utils.cpp @@ -15,8 +15,8 @@ #include "path_utils.hpp" #include -#include #include +#include #include #include #include @@ -48,11 +48,11 @@ TEST_F(TestPathUtils, testFindLongitudinalNearestPoint) const auto p_dst = path.points.back(); const auto p_med = path.points.at(path.points.size() / 2); - const auto geom_p_src = autoware::universe_utils::createPoint( + const auto geom_p_src = autoware_utils::create_point( p_src.point.pose.position.x, p_src.point.pose.position.y, p_src.point.pose.position.z); - const auto geom_p_dst = autoware::universe_utils::createPoint( + const auto geom_p_dst = autoware_utils::create_point( p_dst.point.pose.position.x, p_dst.point.pose.position.y, p_dst.point.pose.position.z); - const auto geom_p_med = autoware::universe_utils::createPoint( + const auto geom_p_med = autoware_utils::create_point( p_med.point.pose.position.x, p_med.point.pose.position.y, p_med.point.pose.position.z); std::vector dst_points{geom_p_src, geom_p_dst, geom_p_med}; const auto closest_point_src = findLongitudinalNearestPoint(path.points, geom_p_src, dst_points); @@ -60,12 +60,12 @@ TEST_F(TestPathUtils, testFindLongitudinalNearestPoint) const auto closest_point_med = findLongitudinalNearestPoint(path.points, geom_p_med, dst_points); EXPECT_DOUBLE_EQ( - autoware::universe_utils::calcDistance3d(closest_point_src, geom_p_src), - autoware::universe_utils::calcDistance3d(geom_p_src, geom_p_src)); + autoware_utils::calc_distance3d(closest_point_src, geom_p_src), + autoware_utils::calc_distance3d(geom_p_src, geom_p_src)); EXPECT_DOUBLE_EQ( - autoware::universe_utils::calcDistance3d(closest_point_dst, geom_p_dst), - autoware::universe_utils::calcDistance3d(geom_p_src, geom_p_dst)); + autoware_utils::calc_distance3d(closest_point_dst, geom_p_dst), + autoware_utils::calc_distance3d(geom_p_src, geom_p_dst)); EXPECT_DOUBLE_EQ( - autoware::universe_utils::calcDistance3d(closest_point_med, geom_p_med), - autoware::universe_utils::calcDistance3d(geom_p_src, geom_p_med)); + autoware_utils::calc_distance3d(closest_point_med, geom_p_med), + autoware_utils::calc_distance3d(geom_p_src, geom_p_med)); } diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/test/test_state_machine.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/test/test_state_machine.cpp index cbc94cb0e2486..d2e6ee7898b8c 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/test/test_state_machine.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/test/test_state_machine.cpp @@ -17,8 +17,8 @@ #include "utils.hpp" #include -#include #include +#include #include #include #include diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/test/test_utils.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/test/test_utils.cpp index 0eae81f186be1..0cf29843f386f 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/test/test_utils.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/test/test_utils.cpp @@ -18,8 +18,8 @@ #include #include -#include #include +#include #include #include #include @@ -68,10 +68,10 @@ class TestRunOutUtils : public ::testing::Test TEST_F(TestRunOutUtils, testFindLongitudinalNearestPoint) { std::vector poly; - poly.push_back(autoware::universe_utils::createPoint(0.0, 1.0, 0.0)); - poly.push_back(autoware::universe_utils::createPoint(0.0, -1.0, 0.0)); - poly.push_back(autoware::universe_utils::createPoint(1.0, 1.0, 0.0)); - poly.push_back(autoware::universe_utils::createPoint(1.0, 0.0, 0.0)); + poly.push_back(autoware_utils::create_point(0.0, 1.0, 0.0)); + poly.push_back(autoware_utils::create_point(0.0, -1.0, 0.0)); + poly.push_back(autoware_utils::create_point(1.0, 1.0, 0.0)); + poly.push_back(autoware_utils::create_point(1.0, 0.0, 0.0)); const auto boost_poly = createBoostPolyFromMsg(poly); EXPECT_FALSE(boost_poly.outer().empty()); diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_speed_bump_module/package.xml b/planning/behavior_velocity_planner/autoware_behavior_velocity_speed_bump_module/package.xml index 5519b54162062..aa35a3775fe32 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_speed_bump_module/package.xml +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_speed_bump_module/package.xml @@ -20,7 +20,7 @@ autoware_motion_utils autoware_planning_msgs autoware_route_handler - autoware_universe_utils + autoware_utils eigen geometry_msgs libboost-dev diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_speed_bump_module/src/debug.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_speed_bump_module/src/debug.cpp index aea47a198259c..e4001b79b22bc 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_speed_bump_module/src/debug.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_speed_bump_module/src/debug.cpp @@ -17,21 +17,21 @@ #include #include #include -#include -#include #include +#include +#include #include namespace autoware::behavior_velocity_planner { using autoware::motion_utils::createSlowDownVirtualWallMarker; -using autoware::universe_utils::appendMarkerArray; -using autoware::universe_utils::calcOffsetPose; -using autoware::universe_utils::createDefaultMarker; -using autoware::universe_utils::createMarkerColor; -using autoware::universe_utils::createMarkerScale; -using autoware::universe_utils::createPoint; +using autoware_utils::append_marker_array; +using autoware_utils::calc_offset_pose; +using autoware_utils::create_default_marker; +using autoware_utils::create_marker_color; +using autoware_utils::create_marker_scale; +using autoware_utils::create_point; using visualization_msgs::msg::Marker; namespace @@ -44,11 +44,11 @@ visualization_msgs::msg::MarkerArray createSpeedBumpMarkers( // Speed bump polygon if (!debug_data.speed_bump_polygon.empty()) { - auto marker = createDefaultMarker( - "map", now, "speed_bump polygon", uid, Marker::LINE_STRIP, createMarkerScale(0.1, 0.0, 0.0), - createMarkerColor(0.0, 0.0, 1.0, 0.999)); + auto marker = create_default_marker( + "map", now, "speed_bump polygon", uid, Marker::LINE_STRIP, create_marker_scale(0.1, 0.0, 0.0), + create_marker_color(0.0, 0.0, 1.0, 0.999)); for (const auto & p : debug_data.speed_bump_polygon) { - marker.points.push_back(createPoint(p.x, p.y, p.z)); + marker.points.push_back(create_point(p.x, p.y, p.z)); } marker.points.push_back(marker.points.front()); msg.markers.push_back(marker); @@ -56,38 +56,38 @@ visualization_msgs::msg::MarkerArray createSpeedBumpMarkers( // Slow start point if (!debug_data.slow_start_poses.empty()) { - auto marker = createDefaultMarker( - "map", now, "slow start point", uid, Marker::POINTS, createMarkerScale(0.25, 0.25, 0.0), - createMarkerColor(1.0, 1.0, 0.0, 0.999)); + auto marker = create_default_marker( + "map", now, "slow start point", uid, Marker::POINTS, create_marker_scale(0.25, 0.25, 0.0), + create_marker_color(1.0, 1.0, 0.0, 0.999)); for (const auto & p : debug_data.slow_start_poses) { - marker.points.push_back(createPoint(p.position.x, p.position.y, p.position.z)); + marker.points.push_back(create_point(p.position.x, p.position.y, p.position.z)); } msg.markers.push_back(marker); } // Slow end point if (!debug_data.slow_end_points.empty()) { - auto marker = createDefaultMarker( - "map", now, "slow end point", uid, Marker::POINTS, createMarkerScale(0.25, 0.25, 0.0), - createMarkerColor(0.2, 0.8, 1.0, 0.999)); + auto marker = create_default_marker( + "map", now, "slow end point", uid, Marker::POINTS, create_marker_scale(0.25, 0.25, 0.0), + create_marker_color(0.2, 0.8, 1.0, 0.999)); for (const auto & p : debug_data.slow_end_points) { - marker.points.push_back(createPoint(p.x, p.y, p.z)); + marker.points.push_back(create_point(p.x, p.y, p.z)); } msg.markers.push_back(marker); } // Path - polygon intersection points { - auto marker = createDefaultMarker( + auto marker = create_default_marker( "map", now, "path_polygon intersection points", uid, Marker::POINTS, - createMarkerScale(0.25, 0.25, 0.0), createMarkerColor(1.0, 0.0, 0.0, 0.999)); + create_marker_scale(0.25, 0.25, 0.0), create_marker_color(1.0, 0.0, 0.0, 0.999)); const auto & p_first = debug_data.path_polygon_intersection_status.first_intersection_point; if (p_first) { - marker.points.push_back(createPoint(p_first->x, p_first->y, p_first->z)); + marker.points.push_back(create_point(p_first->x, p_first->y, p_first->z)); } const auto & p_second = debug_data.path_polygon_intersection_status.second_intersection_point; if (p_second) { - marker.points.push_back(createPoint(p_second->x, p_second->y, p_second->z)); + marker.points.push_back(create_point(p_second->x, p_second->y, p_second->z)); } if (!marker.points.empty()) msg.markers.push_back(marker); } @@ -104,7 +104,7 @@ autoware::motion_utils::VirtualWalls SpeedBumpModule::createVirtualWalls() wall.ns = std::to_string(module_id_) + "_"; wall.style = autoware::motion_utils::VirtualWallType::slowdown; for (const auto & p : debug_data_.slow_start_poses) { - wall.pose = autoware::universe_utils::calcOffsetPose(p, debug_data_.base_link2front, 0.0, 0.0); + wall.pose = autoware_utils::calc_offset_pose(p, debug_data_.base_link2front, 0.0, 0.0); virtual_walls.push_back(wall); } return virtual_walls; @@ -114,7 +114,7 @@ visualization_msgs::msg::MarkerArray SpeedBumpModule::createDebugMarkerArray() { visualization_msgs::msg::MarkerArray debug_marker_array; - appendMarkerArray( + append_marker_array( createSpeedBumpMarkers(debug_data_, this->clock_->now(), module_id_), &debug_marker_array); return debug_marker_array; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_speed_bump_module/src/manager.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_speed_bump_module/src/manager.cpp index 7b54f34c35a07..ea5e68ee7b548 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_speed_bump_module/src/manager.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_speed_bump_module/src/manager.cpp @@ -15,9 +15,9 @@ #include "manager.hpp" #include -#include #include #include +#include #include @@ -30,27 +30,28 @@ namespace autoware::behavior_velocity_planner { -using autoware::universe_utils::getOrDeclareParameter; +using autoware_utils::get_or_declare_parameter; using lanelet::autoware::SpeedBump; SpeedBumpModuleManager::SpeedBumpModuleManager(rclcpp::Node & node) : SceneModuleManagerInterface(node, getModuleName()) { std::string ns(SpeedBumpModuleManager::getModuleName()); - planner_param_.slow_start_margin = getOrDeclareParameter(node, ns + ".slow_start_margin"); - planner_param_.slow_end_margin = getOrDeclareParameter(node, ns + ".slow_end_margin"); - planner_param_.print_debug_info = getOrDeclareParameter(node, ns + ".print_debug_info"); + planner_param_.slow_start_margin = + get_or_declare_parameter(node, ns + ".slow_start_margin"); + planner_param_.slow_end_margin = get_or_declare_parameter(node, ns + ".slow_end_margin"); + planner_param_.print_debug_info = get_or_declare_parameter(node, ns + ".print_debug_info"); // limits for speed bump height and slow down speed ns += ".speed_calculation"; planner_param_.speed_calculation_min_height = - static_cast(getOrDeclareParameter(node, ns + ".min_height")); + static_cast(get_or_declare_parameter(node, ns + ".min_height")); planner_param_.speed_calculation_max_height = - static_cast(getOrDeclareParameter(node, ns + ".max_height")); + static_cast(get_or_declare_parameter(node, ns + ".max_height")); planner_param_.speed_calculation_min_speed = - static_cast(getOrDeclareParameter(node, ns + ".min_speed")); + static_cast(get_or_declare_parameter(node, ns + ".min_speed")); planner_param_.speed_calculation_max_speed = - static_cast(getOrDeclareParameter(node, ns + ".max_speed")); + static_cast(get_or_declare_parameter(node, ns + ".max_speed")); } void SpeedBumpModuleManager::launchNewModules( diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_speed_bump_module/src/scene.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_speed_bump_module/src/scene.cpp index a5099eedce027..5e39a86286905 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_speed_bump_module/src/scene.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_speed_bump_module/src/scene.cpp @@ -15,7 +15,7 @@ #include "scene.hpp" #include "autoware/motion_utils/trajectory/trajectory.hpp" -#include "autoware/universe_utils/geometry/geometry.hpp" +#include "autoware_utils/geometry/geometry.hpp" #include "util.hpp" #include @@ -27,7 +27,7 @@ namespace autoware::behavior_velocity_planner { using autoware::motion_utils::calcSignedArcLength; -using autoware::universe_utils::createPoint; +using autoware_utils::create_point; using geometry_msgs::msg::Point32; @@ -35,7 +35,7 @@ SpeedBumpModule::SpeedBumpModule( const int64_t module_id, const int64_t lane_id, const lanelet::autoware::SpeedBump & speed_bump_reg_elem, const PlannerParam & planner_param, const rclcpp::Logger & logger, const rclcpp::Clock::SharedPtr clock, - const std::shared_ptr time_keeper, + const std::shared_ptr time_keeper, const std::shared_ptr planning_factor_interface) : SceneModuleInterface(module_id, logger, clock, time_keeper, planning_factor_interface), @@ -97,7 +97,7 @@ bool SpeedBumpModule::modifyPathVelocity(PathWithLaneId * path) debug_data_.path_polygon_intersection_status = path_polygon_intersection_status; for (const auto & p : speed_bump_reg_elem_.speedBump().basicPolygon()) { - debug_data_.speed_bump_polygon.push_back(createPoint(p.x(), p.y(), ego_pos.z)); + debug_data_.speed_bump_polygon.push_back(create_point(p.x(), p.y(), ego_pos.z)); } return applySlowDownSpeed(*path, speed_bump_slow_down_speed_, path_polygon_intersection_status); diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_speed_bump_module/src/scene.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_speed_bump_module/src/scene.hpp index 3ac4364dabb69..5b40d6802b1fb 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_speed_bump_module/src/scene.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_speed_bump_module/src/scene.hpp @@ -56,7 +56,7 @@ class SpeedBumpModule : public SceneModuleInterface const int64_t module_id, const int64_t lane_id, const lanelet::autoware::SpeedBump & speed_bump_reg_elem, const PlannerParam & planner_param, const rclcpp::Logger & logger, const rclcpp::Clock::SharedPtr clock, - const std::shared_ptr time_keeper, + const std::shared_ptr time_keeper, const std::shared_ptr planning_factor_interface); diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_speed_bump_module/src/util.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_speed_bump_module/src/util.cpp index 3cdcaccc8d245..247c8d3251f1e 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_speed_bump_module/src/util.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_speed_bump_module/src/util.cpp @@ -18,7 +18,7 @@ #include "autoware/motion_utils/trajectory/trajectory.hpp" #include -#include +#include #include #include @@ -44,7 +44,7 @@ using autoware::motion_utils::calcLongitudinalOffsetPoint; using autoware::motion_utils::calcSignedArcLength; using autoware::motion_utils::findNearestSegmentIndex; using autoware::motion_utils::insertTargetPoint; -using autoware::universe_utils::createPoint; +using autoware_utils::create_point; PathPolygonIntersectionStatus getPathPolygonIntersectionStatus( const PathWithLaneId & ego_path, const lanelet::BasicPolygon2d & polygon, @@ -77,10 +77,10 @@ PathPolygonIntersectionStatus getPathPolygonIntersectionStatus( const auto compare = [&](const Point & p1, const Point & p2) { const auto dist_l1 = - calcSignedArcLength(ego_path.points, size_t(0), createPoint(p1.x(), p1.y(), ego_pos.z)); + calcSignedArcLength(ego_path.points, size_t(0), create_point(p1.x(), p1.y(), ego_pos.z)); const auto dist_l2 = - calcSignedArcLength(ego_path.points, size_t(0), createPoint(p2.x(), p2.y(), ego_pos.z)); + calcSignedArcLength(ego_path.points, size_t(0), create_point(p2.x(), p2.y(), ego_pos.z)); return dist_l1 < dist_l2; }; @@ -105,17 +105,17 @@ PathPolygonIntersectionStatus getPathPolygonIntersectionStatus( } else if (intersects.size() == 1) { const auto & p = intersects.at(0); if (is_last_path_point_inside_polygon) { - polygon_intersection_status.first_intersection_point = createPoint(p.x(), p.y(), ego_pos.z); + polygon_intersection_status.first_intersection_point = create_point(p.x(), p.y(), ego_pos.z); } else if (is_first_path_point_inside_polygon) { - polygon_intersection_status.second_intersection_point = createPoint(p.x(), p.y(), ego_pos.z); + polygon_intersection_status.second_intersection_point = create_point(p.x(), p.y(), ego_pos.z); } else { // do nothing } } else if (intersects.size() == 2) { const auto & p0 = intersects.at(0); const auto & p1 = intersects.at(1); - polygon_intersection_status.first_intersection_point = createPoint(p0.x(), p0.y(), ego_pos.z); - polygon_intersection_status.second_intersection_point = createPoint(p1.x(), p1.y(), ego_pos.z); + polygon_intersection_status.first_intersection_point = create_point(p0.x(), p0.y(), ego_pos.z); + polygon_intersection_status.second_intersection_point = create_point(p1.x(), p1.y(), ego_pos.z); } else { // do nothing } diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/debug.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/debug.cpp index fa94bfbaa361b..cb29858bcea18 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/debug.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/debug.cpp @@ -13,7 +13,7 @@ // limitations under the License. #include "autoware/motion_utils/marker/virtual_wall_marker_creator.hpp" -#include "autoware/universe_utils/geometry/geometry.hpp" +#include "autoware_utils/geometry/geometry.hpp" #include "scene.hpp" namespace autoware::behavior_velocity_planner @@ -28,7 +28,7 @@ autoware::motion_utils::VirtualWalls StopLineModule::createVirtualWalls() wall.text = "stopline"; wall.style = autoware::motion_utils::VirtualWallType::stop; wall.ns = std::to_string(module_id_) + "_"; - wall.pose = autoware::universe_utils::calcOffsetPose( + wall.pose = autoware_utils::calc_offset_pose( *debug_data_.stop_pose, debug_data_.base_link2front, 0.0, 0.0); virtual_walls.push_back(wall); } diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/manager.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/manager.cpp index da059840d36e7..bd0c446b4e5f0 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/manager.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/manager.cpp @@ -14,7 +14,7 @@ #include "manager.hpp" -#include "autoware/universe_utils/ros/parameter.hpp" +#include "autoware_utils/ros/parameter.hpp" #include @@ -25,7 +25,7 @@ namespace autoware::behavior_velocity_planner { -using autoware::universe_utils::getOrDeclareParameter; +using autoware_utils::get_or_declare_parameter; using lanelet::TrafficSign; StopLineModuleManager::StopLineModuleManager(rclcpp::Node & node) @@ -33,10 +33,10 @@ StopLineModuleManager::StopLineModuleManager(rclcpp::Node & node) { const std::string ns(StopLineModuleManager::getModuleName()); auto & p = planner_param_; - p.stop_margin = getOrDeclareParameter(node, ns + ".stop_margin"); + p.stop_margin = get_or_declare_parameter(node, ns + ".stop_margin"); p.hold_stop_margin_distance = - getOrDeclareParameter(node, ns + ".hold_stop_margin_distance"); - p.stop_duration_sec = getOrDeclareParameter(node, ns + ".stop_duration_sec"); + get_or_declare_parameter(node, ns + ".hold_stop_margin_distance"); + p.stop_duration_sec = get_or_declare_parameter(node, ns + ".stop_duration_sec"); } std::vector StopLineModuleManager::getStopLinesWithLaneIdOnPath( diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/scene.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/scene.cpp index 5e132565a7a03..87b75b7126fa7 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/scene.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/scene.cpp @@ -32,7 +32,7 @@ namespace autoware::behavior_velocity_planner StopLineModule::StopLineModule( const int64_t module_id, lanelet::ConstLineString3d stop_line, const PlannerParam & planner_param, const rclcpp::Logger & logger, const rclcpp::Clock::SharedPtr clock, - const std::shared_ptr & time_keeper, + const std::shared_ptr & time_keeper, const std::shared_ptr & planning_factor_interface) : SceneModuleInterface(module_id, logger, clock, time_keeper, planning_factor_interface), diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/scene.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/scene.hpp index 0948e0540cd81..7973e46db9fc8 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/scene.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/scene.hpp @@ -71,7 +71,7 @@ class StopLineModule : public SceneModuleInterface const int64_t module_id, lanelet::ConstLineString3d stop_line, const PlannerParam & planner_param, const rclcpp::Logger & logger, const rclcpp::Clock::SharedPtr clock, - const std::shared_ptr & time_keeper, + const std::shared_ptr & time_keeper, const std::shared_ptr & planning_factor_interface); diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/test/test_scene.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/test/test_scene.cpp index 069122b6ed4c9..a98d66c6f7ed5 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/test/test_scene.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/test/test_scene.cpp @@ -75,7 +75,7 @@ class StopLineModuleTest : public ::testing::Test module_ = std::make_shared( 1, stop_line_, planner_param_, rclcpp::get_logger("test_logger"), clock_, - std::make_shared(), + std::make_shared(), std::make_shared( node_.get(), "test_stopline")); diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_template_module/package.xml b/planning/behavior_velocity_planner/autoware_behavior_velocity_template_module/package.xml index 8ece81460081f..dadac89e02628 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_template_module/package.xml +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_template_module/package.xml @@ -18,7 +18,7 @@ autoware_motion_utils autoware_planning_msgs autoware_route_handler - autoware_universe_utils + autoware_utils eigen geometry_msgs libboost-dev diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_template_module/src/manager.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_template_module/src/manager.cpp index 541bfa823d93d..c179be92ac33a 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_template_module/src/manager.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_template_module/src/manager.cpp @@ -14,8 +14,8 @@ #include "manager.hpp" -#include #include +#include #include @@ -28,13 +28,13 @@ namespace autoware::behavior_velocity_planner { -using autoware::universe_utils::getOrDeclareParameter; +using autoware_utils::get_or_declare_parameter; TemplateModuleManager::TemplateModuleManager(rclcpp::Node & node) : SceneModuleManagerInterface(node, getModuleName()) { std::string ns(TemplateModuleManager::getModuleName()); - dummy_parameter_ = getOrDeclareParameter(node, ns + ".dummy"); + dummy_parameter_ = get_or_declare_parameter(node, ns + ".dummy"); } void TemplateModuleManager::launchNewModules( diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_template_module/src/scene.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_template_module/src/scene.cpp index 7dc8ca9b2e43b..7183cc73b3784 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_template_module/src/scene.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_template_module/src/scene.cpp @@ -15,7 +15,6 @@ #include "scene.hpp" #include "autoware/motion_utils/trajectory/trajectory.hpp" -// #include "autoware/universe_utils/autoware_universe_utils.hpp" #include @@ -27,7 +26,7 @@ namespace autoware::behavior_velocity_planner TemplateModule::TemplateModule( const int64_t module_id, const rclcpp::Logger & logger, const rclcpp::Clock::SharedPtr clock, - const std::shared_ptr time_keeper, + const std::shared_ptr time_keeper, const std::shared_ptr planning_factor_interface) : SceneModuleInterface(module_id, logger, clock, time_keeper, planning_factor_interface) diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_template_module/src/scene.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_template_module/src/scene.hpp index 4f8442af4aeb2..87dd687689973 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_template_module/src/scene.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_template_module/src/scene.hpp @@ -30,7 +30,7 @@ class TemplateModule : public SceneModuleInterface public: TemplateModule( const int64_t module_id, const rclcpp::Logger & logger, const rclcpp::Clock::SharedPtr clock, - const std::shared_ptr time_keeper, + const std::shared_ptr time_keeper, const std::shared_ptr planning_factor_interface); diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/package.xml b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/package.xml index 518d3275a28d3..844a43d68d72b 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/package.xml +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/package.xml @@ -28,7 +28,7 @@ autoware_planning_msgs autoware_route_handler autoware_traffic_light_utils - autoware_universe_utils + autoware_utils eigen geometry_msgs pluginlib diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/debug.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/debug.cpp index d6d0505635079..d73908781c086 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/debug.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/debug.cpp @@ -16,8 +16,8 @@ #include #include -#include -#include +#include +#include #ifdef ROS_DISTRO_GALACTIC #include #else @@ -41,13 +41,13 @@ autoware::motion_utils::VirtualWalls TrafficLightModule::createVirtualWalls() wall.style = autoware::motion_utils::VirtualWallType::deadline; for (const auto & p : debug_data_.dead_line_poses) { - wall.pose = autoware::universe_utils::calcOffsetPose(p, debug_data_.base_link2front, 0.0, 0.0); + wall.pose = autoware_utils::calc_offset_pose(p, debug_data_.base_link2front, 0.0, 0.0); virtual_walls.push_back(wall); } wall.style = autoware::motion_utils::VirtualWallType::stop; for (const auto & p : debug_data_.stop_poses) { - wall.pose = autoware::universe_utils::calcOffsetPose(p, debug_data_.base_link2front, 0.0, 0.0); + wall.pose = autoware_utils::calc_offset_pose(p, debug_data_.base_link2front, 0.0, 0.0); virtual_walls.push_back(wall); } diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/manager.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/manager.cpp index 342ca57db4d27..7d86ac6028c82 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/manager.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/manager.cpp @@ -16,7 +16,7 @@ #include #include -#include +#include #include @@ -27,7 +27,7 @@ #include namespace autoware::behavior_velocity_planner { -using autoware::universe_utils::getOrDeclareParameter; +using autoware_utils::get_or_declare_parameter; using lanelet::TrafficLight; TrafficLightModuleManager::TrafficLightModuleManager(rclcpp::Node & node) @@ -35,15 +35,17 @@ TrafficLightModuleManager::TrafficLightModuleManager(rclcpp::Node & node) node, getModuleName(), getEnableRTC(node, std::string(getModuleName()) + ".enable_rtc")) { const std::string ns(TrafficLightModuleManager::getModuleName()); - planner_param_.stop_margin = getOrDeclareParameter(node, ns + ".stop_margin"); - planner_param_.tl_state_timeout = getOrDeclareParameter(node, ns + ".tl_state_timeout"); + planner_param_.stop_margin = get_or_declare_parameter(node, ns + ".stop_margin"); + planner_param_.tl_state_timeout = + get_or_declare_parameter(node, ns + ".tl_state_timeout"); planner_param_.stop_time_hysteresis = - getOrDeclareParameter(node, ns + ".stop_time_hysteresis"); - planner_param_.enable_pass_judge = getOrDeclareParameter(node, ns + ".enable_pass_judge"); + get_or_declare_parameter(node, ns + ".stop_time_hysteresis"); + planner_param_.enable_pass_judge = + get_or_declare_parameter(node, ns + ".enable_pass_judge"); planner_param_.yellow_lamp_period = - getOrDeclareParameter(node, ns + ".yellow_lamp_period"); + get_or_declare_parameter(node, ns + ".yellow_lamp_period"); planner_param_.yellow_light_stop_velocity = - getOrDeclareParameter(node, ns + ".yellow_light_stop_velocity"); + get_or_declare_parameter(node, ns + ".yellow_light_stop_velocity"); pub_tl_state_ = node.create_publisher( "~/output/traffic_signal", 1); } @@ -107,7 +109,7 @@ void TrafficLightModuleManager::launchNewModules( lane_id, *(traffic_light_reg_elem.first), traffic_light_reg_elem.second, planner_param_, logger_.get_child("traffic_light_module"), clock_, time_keeper_, planning_factor_interface_)); - generateUUID(lane_id); + generate_uuid(lane_id); updateRTCStatus( getUUID(lane_id), true, State::WAITING_FOR_EXECUTION, std::numeric_limits::lowest(), path.header.stamp); diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/scene.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/scene.cpp index 34980168a4912..2d3479b855787 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/scene.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/scene.cpp @@ -44,7 +44,7 @@ TrafficLightModule::TrafficLightModule( const int64_t lane_id, const lanelet::TrafficLight & traffic_light_reg_elem, lanelet::ConstLanelet lane, const PlannerParam & planner_param, const rclcpp::Logger logger, const rclcpp::Clock::SharedPtr clock, - const std::shared_ptr time_keeper, + const std::shared_ptr time_keeper, const std::shared_ptr planning_factor_interface) : SceneModuleInterfaceWithRTC(lane_id, logger, clock, time_keeper, planning_factor_interface), diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/scene.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/scene.hpp index 324e8a2c38cc4..cdf9266239edb 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/scene.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/scene.hpp @@ -71,7 +71,7 @@ class TrafficLightModule : public SceneModuleInterfaceWithRTC const int64_t lane_id, const lanelet::TrafficLight & traffic_light_reg_elem, lanelet::ConstLanelet lane, const PlannerParam & planner_param, const rclcpp::Logger logger, const rclcpp::Clock::SharedPtr clock, - const std::shared_ptr time_keeper, + const std::shared_ptr time_keeper, const std::shared_ptr planning_factor_interface); diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/test/test_utils.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/test/test_utils.cpp index da427b2c3be1b..2d95627a31bf7 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/test/test_utils.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/test/test_utils.cpp @@ -13,7 +13,7 @@ // limitations under the License. #include "../src/utils.hpp" -#include "autoware/universe_utils/geometry/geometry.hpp" +#include "autoware_utils/geometry/geometry.hpp" #include @@ -23,8 +23,8 @@ namespace autoware::behavior_velocity_planner using autoware_internal_planning_msgs::msg::PathPointWithLaneId; using autoware_internal_planning_msgs::msg::PathWithLaneId; -using autoware::universe_utils::createPoint; -using autoware::universe_utils::createQuaternion; +using autoware_utils::create_point; +using autoware_utils::create_quaternion; PathWithLaneId generatePath(const geometry_msgs::msg::Pose & pose) { @@ -81,8 +81,8 @@ TEST(BehaviorTrafficLightModuleUtilsTest, findNearestCollisionPoint) TEST(BehaviorTrafficLightModuleUtilsTest, createTargetPoint) { const auto pose = geometry_msgs::build() - .position(createPoint(0.0, 0.0, 0.0)) - .orientation(createQuaternion(0.0, 0.0, 0.0, 1.0)); + .position(create_point(0.0, 0.0, 0.0)) + .orientation(create_quaternion(0.0, 0.0, 0.0, 1.0)); const auto path = generatePath(pose); { @@ -127,8 +127,8 @@ TEST(BehaviorTrafficLightModuleUtilsTest, createTargetPoint) TEST(BehaviorTrafficLightModuleUtilsTest, calcStopPointAndInsertIndex) { const auto pose = geometry_msgs::build() - .position(createPoint(0.0, 0.0, 0.0)) - .orientation(createQuaternion(0.0, 0.0, 0.0, 1.0)); + .position(create_point(0.0, 0.0, 0.0)) + .orientation(create_quaternion(0.0, 0.0, 0.0, 1.0)); const auto path = generatePath(pose); constexpr double offset = 1.75; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/package.xml b/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/package.xml index 3a1e0d9580ded..9781061345504 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/package.xml +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/package.xml @@ -24,7 +24,7 @@ autoware_motion_utils autoware_planning_msgs autoware_route_handler - autoware_universe_utils + autoware_utils autoware_vehicle_info_utils geometry_msgs nlohmann-json-dev diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/debug.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/debug.cpp index 2b4acd7ff0ca9..a764ef90f7473 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/debug.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/debug.cpp @@ -16,18 +16,18 @@ #include #include -#include -#include +#include +#include #include using autoware::motion_utils::createStopVirtualWallMarker; -using autoware::universe_utils::appendMarkerArray; -using autoware::universe_utils::createDefaultMarker; -using autoware::universe_utils::createMarkerColor; -using autoware::universe_utils::createMarkerOrientation; -using autoware::universe_utils::createMarkerPosition; -using autoware::universe_utils::createMarkerScale; -using autoware::universe_utils::toMsg; +using autoware_utils::append_marker_array; +using autoware_utils::create_default_marker; +using autoware_utils::create_marker_color; +using autoware_utils::create_marker_orientation; +using autoware_utils::create_marker_position; +using autoware_utils::create_marker_scale; +using autoware_utils::to_msg; using namespace std::literals::string_literals; namespace autoware::behavior_velocity_planner @@ -64,11 +64,11 @@ visualization_msgs::msg::MarkerArray VirtualTrafficLightModule::createDebugMarke // instrument_id { - auto marker = createDefaultMarker( + auto marker = create_default_marker( "map", now, "instrument_id", module_id_, visualization_msgs::msg::Marker::TEXT_VIEW_FACING, - createMarkerScale(0.0, 0.0, 1.0), createMarkerColor(1.0, 1.0, 1.0, 0.999)); + create_marker_scale(0.0, 0.0, 1.0), create_marker_color(1.0, 1.0, 1.0, 0.999)); - marker.pose.position = toMsg(m.instrument_center); + marker.pose.position = to_msg(m.instrument_center); marker.text = m.instrument_id; debug_marker_array.markers.push_back(marker); @@ -76,23 +76,23 @@ visualization_msgs::msg::MarkerArray VirtualTrafficLightModule::createDebugMarke // instrument_center { - auto marker = createDefaultMarker( + auto marker = create_default_marker( "map", now, "instrument_center", module_id_, visualization_msgs::msg::Marker::SPHERE, - createMarkerScale(0.3, 0.3, 0.3), createMarkerColor(1.0, 0.0, 0.0, 0.999)); + create_marker_scale(0.3, 0.3, 0.3), create_marker_color(1.0, 0.0, 0.0, 0.999)); - marker.pose.position = toMsg(m.instrument_center); + marker.pose.position = to_msg(m.instrument_center); debug_marker_array.markers.push_back(marker); } // stop_line if (m.stop_line) { - auto marker = createDefaultMarker( + auto marker = create_default_marker( "map", now, "stop_line", module_id_, visualization_msgs::msg::Marker::LINE_STRIP, - createMarkerScale(0.3, 0.0, 0.0), createMarkerColor(1.0, 1.0, 1.0, 0.999)); + create_marker_scale(0.3, 0.0, 0.0), create_marker_color(1.0, 1.0, 1.0, 0.999)); for (const auto & p : *m.stop_line) { - marker.points.push_back(toMsg(p)); + marker.points.push_back(to_msg(p)); } debug_marker_array.markers.push_back(marker); @@ -100,12 +100,12 @@ visualization_msgs::msg::MarkerArray VirtualTrafficLightModule::createDebugMarke // start_line { - auto marker = createDefaultMarker( + auto marker = create_default_marker( "map", now, "start_line", module_id_, visualization_msgs::msg::Marker::LINE_STRIP, - createMarkerScale(0.3, 0.0, 0.0), createMarkerColor(0.0, 1.0, 0.0, 0.999)); + create_marker_scale(0.3, 0.0, 0.0), create_marker_color(0.0, 1.0, 0.0, 0.999)); for (const auto & p : m.start_line) { - marker.points.push_back(toMsg(p)); + marker.points.push_back(to_msg(p)); } debug_marker_array.markers.push_back(marker); @@ -113,14 +113,14 @@ visualization_msgs::msg::MarkerArray VirtualTrafficLightModule::createDebugMarke // end_lines { - auto marker = createDefaultMarker( + auto marker = create_default_marker( "map", now, "end_lines", module_id_, visualization_msgs::msg::Marker::LINE_LIST, - createMarkerScale(0.3, 0.0, 0.0), createMarkerColor(0.0, 1.0, 1.0, 0.999)); + create_marker_scale(0.3, 0.0, 0.0), create_marker_color(0.0, 1.0, 1.0, 0.999)); for (const auto & line : m.end_lines) { for (size_t i = 1; i < line.size(); ++i) { - marker.points.push_back(toMsg(line.at(i - 1))); - marker.points.push_back(toMsg(line.at(i))); + marker.points.push_back(to_msg(line.at(i - 1))); + marker.points.push_back(to_msg(line.at(i))); } } diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/manager.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/manager.cpp index 46fdb3236c75d..bd1b75a9615a5 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/manager.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/manager.cpp @@ -14,12 +14,12 @@ #include "manager.hpp" -#include "autoware/universe_utils/geometry/boost_geometry.hpp" +#include "autoware_utils/geometry/boost_geometry.hpp" #include -#include -#include #include +#include +#include #include @@ -35,7 +35,7 @@ namespace autoware::behavior_velocity_planner { -using autoware::universe_utils::getOrDeclareParameter; +using autoware_utils::get_or_declare_parameter; using lanelet::autoware::VirtualTrafficLight; VirtualTrafficLightModuleManager::VirtualTrafficLightModuleManager(rclcpp::Node & node) @@ -45,18 +45,18 @@ VirtualTrafficLightModuleManager::VirtualTrafficLightModuleManager(rclcpp::Node { auto & p = planner_param_; - p.max_delay_sec = getOrDeclareParameter(node, ns + ".max_delay_sec"); - p.near_line_distance = getOrDeclareParameter(node, ns + ".near_line_distance"); - p.dead_line_margin = getOrDeclareParameter(node, ns + ".dead_line_margin"); + p.max_delay_sec = get_or_declare_parameter(node, ns + ".max_delay_sec"); + p.near_line_distance = get_or_declare_parameter(node, ns + ".near_line_distance"); + p.dead_line_margin = get_or_declare_parameter(node, ns + ".dead_line_margin"); p.hold_stop_margin_distance = - getOrDeclareParameter(node, ns + ".hold_stop_margin_distance"); - p.max_yaw_deviation_rad = autoware::universe_utils::deg2rad( - getOrDeclareParameter(node, ns + ".max_yaw_deviation_deg")); + get_or_declare_parameter(node, ns + ".hold_stop_margin_distance"); + p.max_yaw_deviation_rad = autoware_utils::deg2rad( + get_or_declare_parameter(node, ns + ".max_yaw_deviation_deg")); p.check_timeout_after_stop_line = - getOrDeclareParameter(node, ns + ".check_timeout_after_stop_line"); + get_or_declare_parameter(node, ns + ".check_timeout_after_stop_line"); } - sub_virtual_traffic_light_states_ = autoware::universe_utils::InterProcessPollingSubscriber< + sub_virtual_traffic_light_states_ = autoware_utils::InterProcessPollingSubscriber< tier4_v2x_msgs::msg::VirtualTrafficLightStateArray>:: create_subscription(&node, "~/input/virtual_traffic_light_states"); @@ -68,10 +68,9 @@ VirtualTrafficLightModuleManager::VirtualTrafficLightModuleManager(rclcpp::Node void VirtualTrafficLightModuleManager::launchNewModules( const autoware_internal_planning_msgs::msg::PathWithLaneId & path) { - autoware::universe_utils::LineString2d ego_path_linestring; + autoware_utils::LineString2d ego_path_linestring; for (const auto & path_point : path.points) { - ego_path_linestring.push_back( - autoware::universe_utils::fromMsg(path_point.point.pose.position).to_2d()); + ego_path_linestring.push_back(autoware_utils::from_msg(path_point.point.pose.position).to_2d()); } for (const auto & m : planning_utils::getRegElemMapOnPath( @@ -118,7 +117,7 @@ void VirtualTrafficLightModuleManager::modifyPathVelocity( // NOTE: virtual traffic light specific implementation // Since the argument of modifyPathVelocity cannot be changed, the specific information // of virtual traffic light states is set here. - const auto virtual_traffic_light_states = sub_virtual_traffic_light_states_->takeData(); + const auto virtual_traffic_light_states = sub_virtual_traffic_light_states_->take_data(); for (const auto & scene_module : scene_modules_) { scene_module->setVirtualTrafficLightStates(virtual_traffic_light_states); } diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/manager.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/manager.hpp index 8e8fc1d08ecbc..2fd53f7106a8c 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/manager.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/manager.hpp @@ -20,7 +20,7 @@ #include #include #include -#include +#include #include #include @@ -50,7 +50,7 @@ class VirtualTrafficLightModuleManager std::function &)> getModuleExpiredFunction( const autoware_internal_planning_msgs::msg::PathWithLaneId & path) override; - autoware::universe_utils::InterProcessPollingSubscriber< + autoware_utils::InterProcessPollingSubscriber< tier4_v2x_msgs::msg::VirtualTrafficLightStateArray>::SharedPtr sub_virtual_traffic_light_states_; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.cpp index ce90ff4b1f1af..5db3983be12e2 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.cpp @@ -40,7 +40,7 @@ VirtualTrafficLightModule::VirtualTrafficLightModule( const lanelet::autoware::VirtualTrafficLight & reg_elem, lanelet::ConstLanelet lane, const PlannerParam & planner_param, const rclcpp::Logger logger, const rclcpp::Clock::SharedPtr clock, - const std::shared_ptr time_keeper, + const std::shared_ptr time_keeper, const std::shared_ptr planning_factor_interface) : SceneModuleInterface(module_id, logger, clock, time_keeper, planning_factor_interface), diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.hpp index 47f10f06ab940..6540c108143c1 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.hpp @@ -16,10 +16,10 @@ #define SCENE_HPP_ #include -#include -#include #include #include +#include +#include #include #include #include @@ -53,10 +53,10 @@ class VirtualTrafficLightModule : public SceneModuleInterface std::string instrument_type{}; std::string instrument_id{}; std::vector custom_tags{}; - autoware::universe_utils::Point3d instrument_center{}; - std::optional stop_line{}; - autoware::universe_utils::LineString3d start_line{}; - std::vector end_lines{}; + autoware_utils::Point3d instrument_center{}; + std::optional stop_line{}; + autoware_utils::LineString3d start_line{}; + std::vector end_lines{}; }; struct ModuleData @@ -83,7 +83,7 @@ class VirtualTrafficLightModule : public SceneModuleInterface const lanelet::autoware::VirtualTrafficLight & reg_elem, lanelet::ConstLanelet lane, const PlannerParam & planner_param, const rclcpp::Logger logger, const rclcpp::Clock::SharedPtr clock, - const std::shared_ptr time_keeper, + const std::shared_ptr time_keeper, const std::shared_ptr planning_factor_interface); diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/utils.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/utils.cpp index 234781aab0d61..5d918bb9fe1bf 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/utils.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/utils.cpp @@ -17,7 +17,7 @@ #include #include #include -#include +#include #include #include @@ -26,24 +26,23 @@ namespace autoware::behavior_velocity_planner::virtual_traffic_light { -using autoware::universe_utils::calcDistance2d; +using autoware_utils::calc_distance2d; tier4_v2x_msgs::msg::KeyValue createKeyValue(const std::string & key, const std::string & value) { return tier4_v2x_msgs::build().key(key).value(value); } -autoware::universe_utils::LineString3d toAutowarePoints( - const lanelet::ConstLineString3d & line_string) +autoware_utils::LineString3d toAutowarePoints(const lanelet::ConstLineString3d & line_string) { - autoware::universe_utils::LineString3d output; + autoware_utils::LineString3d output; for (const auto & p : line_string) { output.emplace_back(p.x(), p.y(), p.z()); } return output; } -std::optional toAutowarePoints( +std::optional toAutowarePoints( const lanelet::Optional & line_string) { if (!line_string) { @@ -52,18 +51,17 @@ std::optional toAutowarePoints( return toAutowarePoints(*line_string); } -std::vector toAutowarePoints( +std::vector toAutowarePoints( const lanelet::ConstLineStrings3d & line_strings) { - std::vector output; + std::vector output; for (const auto & line_string : line_strings) { output.emplace_back(toAutowarePoints(line_string)); } return output; } -autoware::universe_utils::Point3d calcCenter( - const autoware::universe_utils::LineString3d & line_string) +autoware_utils::Point3d calcCenter(const autoware_utils::LineString3d & line_string) { const auto p1 = line_string.front(); const auto p2 = line_string.back(); @@ -74,10 +72,10 @@ autoware::universe_utils::Point3d calcCenter( geometry_msgs::msg::Pose calcHeadPose( const geometry_msgs::msg::Pose & base_link_pose, const double base_link_to_front) { - return autoware::universe_utils::calcOffsetPose(base_link_pose, base_link_to_front, 0.0, 0.0); + return autoware_utils::calc_offset_pose(base_link_pose, base_link_to_front, 0.0, 0.0); } -geometry_msgs::msg::Point convertToGeomPoint(const autoware::universe_utils::Point3d & p) +geometry_msgs::msg::Point convertToGeomPoint(const autoware_utils::Point3d & p) { geometry_msgs::msg::Point geom_p; geom_p.x = p.x(); diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/utils.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/utils.hpp index 35b9c8d9ef46a..817d245cf5ab2 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/utils.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/utils.hpp @@ -41,22 +41,20 @@ struct SegmentIndexWithOffset tier4_v2x_msgs::msg::KeyValue createKeyValue(const std::string & key, const std::string & value); -autoware::universe_utils::LineString3d toAutowarePoints( - const lanelet::ConstLineString3d & line_string); +autoware_utils::LineString3d toAutowarePoints(const lanelet::ConstLineString3d & line_string); -std::optional toAutowarePoints( +std::optional toAutowarePoints( const lanelet::Optional & line_string); -std::vector toAutowarePoints( +std::vector toAutowarePoints( const lanelet::ConstLineStrings3d & line_strings); -autoware::universe_utils::Point3d calcCenter( - const autoware::universe_utils::LineString3d & line_string); +autoware_utils::Point3d calcCenter(const autoware_utils::LineString3d & line_string); geometry_msgs::msg::Pose calcHeadPose( const geometry_msgs::msg::Pose & base_link_pose, const double base_link_to_front); -geometry_msgs::msg::Point convertToGeomPoint(const autoware::universe_utils::Point3d & p); +geometry_msgs::msg::Point convertToGeomPoint(const autoware_utils::Point3d & p); void insertStopVelocityFromStart(autoware_internal_planning_msgs::msg::PathWithLaneId * path); @@ -66,16 +64,15 @@ std::optional insertStopVelocityAtCollision( template std::optional findLastCollisionBeforeEndLine( - const T & points, const autoware::universe_utils::LineString3d & target_line, - const size_t end_line_idx) + const T & points, const autoware_utils::LineString3d & target_line, const size_t end_line_idx) { const auto target_line_p1 = convertToGeomPoint(target_line.at(0)); const auto target_line_p2 = convertToGeomPoint(target_line.at(1)); for (size_t i = end_line_idx; 0 < i; --i) { // NOTE: size_t can be used since it will not be negative. - const auto & p1 = autoware::universe_utils::getPoint(points.at(i)); - const auto & p2 = autoware::universe_utils::getPoint(points.at(i - 1)); + const auto & p1 = autoware_utils::get_point(points.at(i)); + const auto & p2 = autoware_utils::get_point(points.at(i - 1)); const auto collision_point = arc_lane_utils::checkCollision(p1, p2, target_line_p1, target_line_p2); @@ -89,7 +86,7 @@ std::optional findLastCollisionBeforeEndLine( template std::optional findLastCollisionBeforeEndLine( - const T & points, const std::vector & lines, + const T & points, const std::vector & lines, const size_t end_line_idx) { for (const auto & line : lines) { diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/test/test_utils.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/test/test_utils.cpp index 7d89c57b89b41..97a7932c8fa4e 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/test/test_utils.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/test/test_utils.cpp @@ -131,7 +131,7 @@ TEST(VirtualTrafficLightTest, ToAutowarePoints) TEST(VirtualTrafficLightTest, CalcCenter) { - autoware::universe_utils::LineString3d line_string; + autoware_utils::LineString3d line_string; line_string.emplace_back(1.0, 2.0, 3.0); line_string.emplace_back(4.0, 5.0, 6.0); @@ -158,7 +158,7 @@ TEST(VirtualTrafficLightTest, CalcHeadPose) TEST(VirtualTrafficLightTest, ConvertToGeomPoint) { - autoware::universe_utils::Point3d point(1.0, 2.0, 3.0); + autoware_utils::Point3d point(1.0, 2.0, 3.0); auto geom_point = convertToGeomPoint(point); EXPECT_DOUBLE_EQ(geom_point.x, 1.0); @@ -385,7 +385,7 @@ TEST(VirtualTrafficLightTest, FindLastCollisionBeforeEndLine) // 1) find first collision point { std::cout << "----- find first collision point -----" << std::endl; - autoware::universe_utils::LineString3d target_line; + autoware_utils::LineString3d target_line; target_line.emplace_back(0.0, -1.0, 0.0); target_line.emplace_back(0.0, 1.0, 0.0); @@ -401,7 +401,7 @@ TEST(VirtualTrafficLightTest, FindLastCollisionBeforeEndLine) { std::cout << "----- find middle collision point -----" << std::endl; - autoware::universe_utils::LineString3d target_line; + autoware_utils::LineString3d target_line; target_line.emplace_back(5.0, -1.0, 0.0); target_line.emplace_back(5.0, 1.0, 0.0); @@ -418,7 +418,7 @@ TEST(VirtualTrafficLightTest, FindLastCollisionBeforeEndLine) { std::cout << "----- find middle collision point -----" << std::endl; - autoware::universe_utils::LineString3d target_line; + autoware_utils::LineString3d target_line; target_line.emplace_back(4.5, -1.0, 0.0); target_line.emplace_back(4.5, 1.0, 0.0); @@ -431,19 +431,19 @@ TEST(VirtualTrafficLightTest, FindLastCollisionBeforeEndLine) EXPECT_EQ(result.value().index, 5); } - // std::vector + // std::vector // 3) find middle collision point with multi target lines { std::cout << "----- find collision point with multi target lines -----" << std::endl; - std::vector target_lines; + std::vector target_lines; { - autoware::universe_utils::LineString3d target_line1; + autoware_utils::LineString3d target_line1; target_line1.emplace_back(3.5, -1.0, 0.0); target_line1.emplace_back(3.5, 1.0, 0.0); - autoware::universe_utils::LineString3d target_line2; + autoware_utils::LineString3d target_line2; target_line2.emplace_back(6.5, -1.0, 0.0); target_line2.emplace_back(6.5, 1.0, 0.0); diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/package.xml b/planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/package.xml index 4bda6bbe5826b..bf8d5e6b668d4 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/package.xml +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/package.xml @@ -24,7 +24,7 @@ autoware_motion_utils autoware_planning_msgs autoware_route_handler - autoware_universe_utils + autoware_utils autoware_vehicle_info_utils geometry_msgs libboost-dev diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/src/debug.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/src/debug.cpp index 59a4419087f18..3c78a9a651b40 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/src/debug.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/src/debug.cpp @@ -17,8 +17,8 @@ #include #include #include -#include -#include +#include +#include #include @@ -27,12 +27,12 @@ namespace autoware::behavior_velocity_planner using autoware::motion_utils::createSlowDownVirtualWallMarker; using autoware::motion_utils::createStopVirtualWallMarker; -using autoware::universe_utils::appendMarkerArray; -using autoware::universe_utils::calcOffsetPose; -using autoware::universe_utils::createDefaultMarker; -using autoware::universe_utils::createMarkerColor; -using autoware::universe_utils::createMarkerScale; -using autoware::universe_utils::createPoint; +using autoware_utils::append_marker_array; +using autoware_utils::calc_offset_pose; +using autoware_utils::create_default_marker; +using autoware_utils::create_marker_color; +using autoware_utils::create_marker_scale; +using autoware_utils::create_point; using visualization_msgs::msg::Marker; namespace @@ -45,11 +45,11 @@ visualization_msgs::msg::MarkerArray createWalkwayMarkers( // Stop point if (!debug_data.stop_poses.empty()) { - auto marker = createDefaultMarker( - "map", now, "stop point", uid, Marker::POINTS, createMarkerScale(0.25, 0.25, 0.0), - createMarkerColor(1.0, 0.0, 0.0, 0.999)); + auto marker = create_default_marker( + "map", now, "stop point", uid, Marker::POINTS, create_marker_scale(0.25, 0.25, 0.0), + create_marker_color(1.0, 0.0, 0.0, 0.999)); for (const auto & p : debug_data.stop_poses) { - marker.points.push_back(createPoint(p.position.x, p.position.y, p.position.z)); + marker.points.push_back(create_point(p.position.x, p.position.y, p.position.z)); } msg.markers.push_back(marker); } @@ -57,13 +57,13 @@ visualization_msgs::msg::MarkerArray createWalkwayMarkers( { size_t i = 0; for (const auto & p : debug_data.stop_poses) { - auto marker = createDefaultMarker( + auto marker = create_default_marker( "map", now, "walkway stop judge range", uid + i++, Marker::LINE_STRIP, - createMarkerScale(0.1, 0.1, 0.0), createMarkerColor(1.0, 0.0, 0.0, 0.5)); + create_marker_scale(0.1, 0.1, 0.0), create_marker_color(1.0, 0.0, 0.0, 0.5)); for (size_t j = 0; j < 50; ++j) { const auto x = p.position.x + debug_data.stop_judge_range * std::cos(M_PI * 2 / 50 * j); const auto y = p.position.y + debug_data.stop_judge_range * std::sin(M_PI * 2 / 50 * j); - marker.points.push_back(createPoint(x, y, p.position.z)); + marker.points.push_back(create_point(x, y, p.position.z)); } marker.points.push_back(marker.points.front()); msg.markers.push_back(marker); @@ -83,7 +83,7 @@ autoware::motion_utils::VirtualWalls WalkwayModule::createVirtualWalls() wall.style = autoware::motion_utils::VirtualWallType::stop; for (const auto & p : debug_data_.stop_poses) { - wall.pose = calcOffsetPose(p, debug_data_.base_link2front, 0.0, 0.0); + wall.pose = calc_offset_pose(p, debug_data_.base_link2front, 0.0, 0.0); virtual_walls.push_back(wall); } return virtual_walls; @@ -93,7 +93,7 @@ visualization_msgs::msg::MarkerArray WalkwayModule::createDebugMarkerArray() { visualization_msgs::msg::MarkerArray debug_marker_array; - appendMarkerArray( + append_marker_array( createWalkwayMarkers(debug_data_, this->clock_->now(), module_id_), &debug_marker_array); return debug_marker_array; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/src/manager.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/src/manager.cpp index bfa8a96531090..b3242ff0eaac7 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/src/manager.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/src/manager.cpp @@ -15,7 +15,7 @@ #include "manager.hpp" #include -#include +#include #include #include @@ -26,7 +26,7 @@ namespace autoware::behavior_velocity_planner { -using autoware::universe_utils::getOrDeclareParameter; +using autoware_utils::get_or_declare_parameter; using lanelet::autoware::Crosswalk; WalkwayModuleManager::WalkwayModuleManager(rclcpp::Node & node) @@ -37,8 +37,8 @@ WalkwayModuleManager::WalkwayModuleManager(rclcpp::Node & node) // for walkway parameters auto & wp = walkway_planner_param_; wp.stop_distance_from_crosswalk = - getOrDeclareParameter(node, ns + ".stop_distance_from_crosswalk"); - wp.stop_duration = getOrDeclareParameter(node, ns + ".stop_duration"); + get_or_declare_parameter(node, ns + ".stop_distance_from_crosswalk"); + wp.stop_duration = get_or_declare_parameter(node, ns + ".stop_duration"); } void WalkwayModuleManager::launchNewModules(const PathWithLaneId & path) diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/src/scene_walkway.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/src/scene_walkway.cpp index 1224125b3d846..f2e8ecff6894c 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/src/scene_walkway.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/src/scene_walkway.cpp @@ -16,7 +16,7 @@ #include #include -#include +#include #include #include @@ -28,14 +28,14 @@ namespace bg = boost::geometry; using autoware::motion_utils::calcLongitudinalOffsetPose; using autoware::motion_utils::calcSignedArcLength; using autoware::motion_utils::findNearestSegmentIndex; -using autoware::universe_utils::createPoint; -using autoware::universe_utils::getPose; +using autoware_utils::create_point; +using autoware_utils::get_pose; WalkwayModule::WalkwayModule( const int64_t module_id, const lanelet::LaneletMapPtr & lanelet_map_ptr, const PlannerParam & planner_param, const bool use_regulatory_element, const rclcpp::Logger & logger, const rclcpp::Clock::SharedPtr clock, - const std::shared_ptr time_keeper, + const std::shared_ptr time_keeper, const std::shared_ptr planning_factor_interface) : SceneModuleInterface(module_id, logger, clock, time_keeper, planning_factor_interface), diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/src/scene_walkway.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/src/scene_walkway.hpp index 3e06bf2d878c6..6b2c5a250cc50 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/src/scene_walkway.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/src/scene_walkway.hpp @@ -45,7 +45,7 @@ class WalkwayModule : public SceneModuleInterface const int64_t module_id, const lanelet::LaneletMapPtr & lanelet_map_ptr, const PlannerParam & planner_param, const bool use_regulatory_element, const rclcpp::Logger & logger, const rclcpp::Clock::SharedPtr clock, - const std::shared_ptr time_keeper, + const std::shared_ptr time_keeper, const std::shared_ptr planning_factor_interface); diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/package.xml b/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/package.xml index 2e94f1d32b8b3..695ba4645a838 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/package.xml +++ b/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/package.xml @@ -17,7 +17,7 @@ autoware_perception_msgs autoware_planning_msgs autoware_route_handler - autoware_universe_utils + autoware_utils autoware_vehicle_info_utils geometry_msgs libboost-dev diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/collision.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/collision.cpp index 73745c39ad221..8f8cacf052455 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/collision.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/collision.cpp @@ -15,9 +15,9 @@ #include "collision.hpp" #include -#include -#include -#include +#include +#include +#include #include @@ -30,7 +30,7 @@ namespace autoware::motion_velocity_planner::dynamic_obstacle_stop std::optional find_closest_collision_point( const EgoData & ego_data, const geometry_msgs::msg::Pose & object_pose, - const autoware::universe_utils::Polygon2d & object_footprint) + const autoware_utils::Polygon2d & object_footprint) { std::optional closest_collision_point; auto closest_dist = std::numeric_limits::max(); @@ -41,10 +41,10 @@ std::optional find_closest_collision_point( const auto traj_idx = rough_collision.second; const auto & ego_footprint = ego_data.trajectory_footprints[traj_idx]; const auto & ego_pose = ego_data.trajectory[traj_idx].pose; - const auto angle_diff = autoware::universe_utils::normalizeRadian( + const auto angle_diff = autoware_utils::normalize_radian( tf2::getYaw(ego_pose.orientation) - tf2::getYaw(object_pose.orientation)); if (std::abs(angle_diff) > (M_PI_2 + M_PI_4)) { // TODO(Maxime): make this angle a parameter - autoware::universe_utils::MultiPoint2d collision_points; + autoware_utils::MultiPoint2d collision_points; boost::geometry::intersection( object_footprint.outer(), ego_footprint.outer(), collision_points); for (const auto & coll_p : collision_points) { @@ -64,7 +64,7 @@ std::optional find_closest_collision_point( std::vector find_collisions( const EgoData & ego_data, const std::vector & objects, - const autoware::universe_utils::MultiPolygon2d & object_forward_footprints) + const autoware_utils::MultiPolygon2d & object_forward_footprints) { std::vector collisions; for (auto object_idx = 0UL; object_idx < objects.size(); ++object_idx) { @@ -73,7 +73,7 @@ std::vector find_collisions( const auto collision = find_closest_collision_point(ego_data, object_pose, object_footprint); if (collision) { Collision c; - c.object_uuid = autoware::universe_utils::toHexString(objects[object_idx].object_id); + c.object_uuid = autoware_utils::to_hex_string(objects[object_idx].object_id); c.point = *collision; collisions.push_back(c); } diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/collision.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/collision.hpp index 63ed073a0983d..7026c3f117253 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/collision.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/collision.hpp @@ -30,7 +30,7 @@ namespace autoware::motion_velocity_planner::dynamic_obstacle_stop /// @return the collision point closest to ego (if any) std::optional find_closest_collision_point( const EgoData & ego_data, const geometry_msgs::msg::Pose & object_pose, - const autoware::universe_utils::Polygon2d & object_footprint); + const autoware_utils::Polygon2d & object_footprint); /// @brief find the earliest collision along the ego trajectory /// @param [in] ego_data ego data including its trajectory and footprint @@ -40,7 +40,7 @@ std::optional find_closest_collision_point( std::vector find_collisions( const EgoData & ego_data, const std::vector & objects, - const autoware::universe_utils::MultiPolygon2d & object_forward_footprints); + const autoware_utils::MultiPolygon2d & object_forward_footprints); } // namespace autoware::motion_velocity_planner::dynamic_obstacle_stop diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/debug.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/debug.cpp index 3209a3be028ea..b1f27ddc2f286 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/debug.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/debug.cpp @@ -16,7 +16,7 @@ #include "types.hpp" -#include +#include #include @@ -59,8 +59,8 @@ std::vector make_collision_markers( marker.ns = ns; marker.id = 0; marker.action = visualization_msgs::msg::Marker::ADD; - marker.scale = autoware::universe_utils::createMarkerScale(0.2, 0.2, 0.5); - marker.color = autoware::universe_utils::createMarkerColor(0.6, 0.0, 0.6, 1.0); + marker.scale = autoware_utils::create_marker_scale(0.2, 0.2, 0.5); + marker.color = autoware_utils::create_marker_color(0.6, 0.0, 0.6, 1.0); for (const auto & [object_uuid, decision] : object_map) { marker.type = visualization_msgs::msg::Marker::TEXT_VIEW_FACING; marker.text = object_uuid.substr(0, 5) + "\n"; @@ -85,8 +85,7 @@ std::vector make_collision_markers( } std::vector make_polygon_markers( - const autoware::universe_utils::MultiPolygon2d & footprints, const std::string & ns, - const double z) + const autoware_utils::MultiPolygon2d & footprints, const std::string & ns, const double z) { std::vector markers; visualization_msgs::msg::Marker marker; @@ -96,8 +95,8 @@ std::vector make_polygon_markers( marker.id = 0; marker.type = visualization_msgs::msg::Marker::LINE_STRIP; marker.action = visualization_msgs::msg::Marker::ADD; - marker.scale = autoware::universe_utils::createMarkerScale(0.1, 0.1, 0.1); - marker.color = autoware::universe_utils::createMarkerColor(0.1, 1.0, 0.1, 0.8); + marker.scale = autoware_utils::create_marker_scale(0.1, 0.1, 0.1); + marker.color = autoware_utils::create_marker_color(0.1, 1.0, 0.1, 0.8); for (const auto & footprint : footprints) { marker.points.clear(); for (const auto & p : footprint.outer()) { diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/debug.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/debug.hpp index 2018e3e9c53a0..3e6b5214a69bf 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/debug.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/debug.hpp @@ -18,7 +18,7 @@ #include "object_stop_decision.hpp" #include "types.hpp" -#include +#include #include #include @@ -39,8 +39,7 @@ std::vector make_collision_markers( const ObjectStopDecisionMap & object_map, const std::string & ns, const double z, const rclcpp::Time & now); std::vector make_polygon_markers( - const autoware::universe_utils::MultiPolygon2d & footprints, const std::string & ns, - const double z); + const autoware_utils::MultiPolygon2d & footprints, const std::string & ns, const double z); } // namespace autoware::motion_velocity_planner::dynamic_obstacle_stop::debug #endif // DEBUG_HPP_ diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/dynamic_obstacle_stop_module.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/dynamic_obstacle_stop_module.cpp index 15fcc52f0fd24..12d2d3600695f 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/dynamic_obstacle_stop_module.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/dynamic_obstacle_stop_module.cpp @@ -23,10 +23,10 @@ #include #include -#include -#include -#include -#include +#include +#include +#include +#include #include #include @@ -52,26 +52,27 @@ void DynamicObstacleStopModule::init(rclcpp::Node & node, const std::string & mo node.create_publisher("~/" + ns_ + "/debug_markers", 1); virtual_wall_publisher_ = node.create_publisher("~/" + ns_ + "/virtual_walls", 1); - processing_diag_publisher_ = std::make_shared( + processing_diag_publisher_ = std::make_shared( &node, "~/debug/" + ns_ + "/processing_time_ms_diag"); processing_time_publisher_ = node.create_publisher( "~/debug/" + ns_ + "/processing_time_ms", 1); - using autoware::universe_utils::getOrDeclareParameter; + using autoware_utils::get_or_declare_parameter; auto & p = params_; - p.extra_object_width = getOrDeclareParameter(node, ns_ + ".extra_object_width"); - p.minimum_object_velocity = getOrDeclareParameter(node, ns_ + ".minimum_object_velocity"); - p.stop_distance_buffer = getOrDeclareParameter(node, ns_ + ".stop_distance_buffer"); - p.time_horizon = getOrDeclareParameter(node, ns_ + ".time_horizon"); - p.hysteresis = getOrDeclareParameter(node, ns_ + ".hysteresis"); - p.add_duration_buffer = getOrDeclareParameter(node, ns_ + ".add_stop_duration_buffer"); + p.extra_object_width = get_or_declare_parameter(node, ns_ + ".extra_object_width"); + p.minimum_object_velocity = + get_or_declare_parameter(node, ns_ + ".minimum_object_velocity"); + p.stop_distance_buffer = get_or_declare_parameter(node, ns_ + ".stop_distance_buffer"); + p.time_horizon = get_or_declare_parameter(node, ns_ + ".time_horizon"); + p.hysteresis = get_or_declare_parameter(node, ns_ + ".hysteresis"); + p.add_duration_buffer = get_or_declare_parameter(node, ns_ + ".add_stop_duration_buffer"); p.remove_duration_buffer = - getOrDeclareParameter(node, ns_ + ".remove_stop_duration_buffer"); + get_or_declare_parameter(node, ns_ + ".remove_stop_duration_buffer"); p.minimum_object_distance_from_ego_trajectory = - getOrDeclareParameter(node, ns_ + ".minimum_object_distance_from_ego_trajectory"); + get_or_declare_parameter(node, ns_ + ".minimum_object_distance_from_ego_trajectory"); p.ignore_unavoidable_collisions = - getOrDeclareParameter(node, ns_ + ".ignore_unavoidable_collisions"); + get_or_declare_parameter(node, ns_ + ".ignore_unavoidable_collisions"); const auto vehicle_info = autoware::vehicle_info_utils::VehicleInfoUtils(node).getVehicleInfo(); p.ego_lateral_offset = @@ -81,19 +82,19 @@ void DynamicObstacleStopModule::init(rclcpp::Node & node, const std::string & mo void DynamicObstacleStopModule::update_parameters(const std::vector & parameters) { - using autoware::universe_utils::updateParam; + using autoware_utils::update_param; auto & p = params_; - updateParam(parameters, ns_ + ".extra_object_width", p.extra_object_width); - updateParam(parameters, ns_ + ".minimum_object_velocity", p.minimum_object_velocity); - updateParam(parameters, ns_ + ".stop_distance_buffer", p.stop_distance_buffer); - updateParam(parameters, ns_ + ".time_horizon", p.time_horizon); - updateParam(parameters, ns_ + ".hysteresis", p.hysteresis); - updateParam(parameters, ns_ + ".add_stop_duration_buffer", p.add_duration_buffer); - updateParam(parameters, ns_ + ".remove_stop_duration_buffer", p.remove_duration_buffer); - updateParam( + update_param(parameters, ns_ + ".extra_object_width", p.extra_object_width); + update_param(parameters, ns_ + ".minimum_object_velocity", p.minimum_object_velocity); + update_param(parameters, ns_ + ".stop_distance_buffer", p.stop_distance_buffer); + update_param(parameters, ns_ + ".time_horizon", p.time_horizon); + update_param(parameters, ns_ + ".hysteresis", p.hysteresis); + update_param(parameters, ns_ + ".add_stop_duration_buffer", p.add_duration_buffer); + update_param(parameters, ns_ + ".remove_stop_duration_buffer", p.remove_duration_buffer); + update_param( parameters, ns_ + ".minimum_object_distance_from_ego_trajectory", p.minimum_object_distance_from_ego_trajectory); - updateParam(parameters, ns_ + ".ignore_unavoidable_collisions", p.ignore_unavoidable_collisions); + update_param(parameters, ns_ + ".ignore_unavoidable_collisions", p.ignore_unavoidable_collisions); } VelocityPlanningResult DynamicObstacleStopModule::plan( @@ -106,7 +107,7 @@ VelocityPlanningResult DynamicObstacleStopModule::plan( debug_data_.reset_data(); if (smoothed_trajectory_points.size() < 2) return result; - autoware::universe_utils::StopWatch stopwatch; + autoware_utils::StopWatch stopwatch; stopwatch.tic(); stopwatch.tic("preprocessing"); dynamic_obstacle_stop::EgoData ego_data; diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/footprint.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/footprint.cpp index 8f59160a56f2a..0b942ef674a3c 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/footprint.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/footprint.cpp @@ -14,7 +14,7 @@ #include "footprint.hpp" -#include +#include #include @@ -28,11 +28,11 @@ namespace autoware::motion_velocity_planner::dynamic_obstacle_stop { -autoware::universe_utils::MultiPolygon2d make_forward_footprints( +autoware_utils::MultiPolygon2d make_forward_footprints( const std::vector & obstacles, const PlannerParam & params, const double hysteresis) { - autoware::universe_utils::MultiPolygon2d forward_footprints; + autoware_utils::MultiPolygon2d forward_footprints; for (const auto & obstacle : obstacles) forward_footprints.push_back(project_to_pose( make_forward_footprint(obstacle, params, hysteresis), @@ -40,7 +40,7 @@ autoware::universe_utils::MultiPolygon2d make_forward_footprints( return forward_footprints; } -autoware::universe_utils::Polygon2d make_forward_footprint( +autoware_utils::Polygon2d make_forward_footprint( const autoware_perception_msgs::msg::PredictedObject & obstacle, const PlannerParam & params, const double hysteresis) { @@ -49,7 +49,7 @@ autoware::universe_utils::Polygon2d make_forward_footprint( shape.x / 2.0 + obstacle.kinematics.initial_twist_with_covariance.twist.linear.x * params.time_horizon; const auto lateral_offset = (shape.y + params.extra_object_width) / 2.0 + hysteresis; - return autoware::universe_utils::Polygon2d{ + return autoware_utils::Polygon2d{ {{longitudinal_offset, -lateral_offset}, {longitudinal_offset, lateral_offset}, {shape.x / 2.0, lateral_offset}, @@ -58,12 +58,12 @@ autoware::universe_utils::Polygon2d make_forward_footprint( {}}; } -autoware::universe_utils::Polygon2d project_to_pose( - const autoware::universe_utils::Polygon2d & base_footprint, const geometry_msgs::msg::Pose & pose) +autoware_utils::Polygon2d project_to_pose( + const autoware_utils::Polygon2d & base_footprint, const geometry_msgs::msg::Pose & pose) { const auto angle = tf2::getYaw(pose.orientation); - const auto rotated_footprint = autoware::universe_utils::rotatePolygon(base_footprint, angle); - autoware::universe_utils::Polygon2d footprint; + const auto rotated_footprint = autoware_utils::rotate_polygon(base_footprint, angle); + autoware_utils::Polygon2d footprint; for (const auto & p : rotated_footprint.outer()) footprint.outer().emplace_back(p.x() + pose.position.x, p.y() + pose.position.y); return footprint; @@ -72,13 +72,13 @@ autoware::universe_utils::Polygon2d project_to_pose( void make_ego_footprint_rtree(EgoData & ego_data, const PlannerParam & params) { for (const auto & p : ego_data.trajectory) - ego_data.trajectory_footprints.push_back(autoware::universe_utils::toFootprint( + ego_data.trajectory_footprints.push_back(autoware_utils::to_footprint( p.pose, params.ego_longitudinal_offset, 0.0, params.ego_lateral_offset * 2.0)); std::vector rtree_nodes; rtree_nodes.reserve(ego_data.trajectory_footprints.size()); for (auto i = 0UL; i < ego_data.trajectory_footprints.size(); ++i) { - const auto box = boost::geometry::return_envelope( - ego_data.trajectory_footprints[i]); + const auto box = + boost::geometry::return_envelope(ego_data.trajectory_footprints[i]); rtree_nodes.emplace_back(box, i); } ego_data.rtree = Rtree(rtree_nodes); diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/footprint.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/footprint.hpp index 01a112f85f4d9..a588288cd3a96 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/footprint.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/footprint.hpp @@ -17,7 +17,7 @@ #include "types.hpp" -#include +#include #include @@ -29,7 +29,7 @@ namespace autoware::motion_velocity_planner::dynamic_obstacle_stop /// @param [in] params parameters used to create the footprint /// @param [in] hysteresis [m] extra lateral distance to add to the footprints /// @return forward footprint of the obstacle -autoware::universe_utils::MultiPolygon2d make_forward_footprints( +autoware_utils::MultiPolygon2d make_forward_footprints( const std::vector & obstacles, const PlannerParam & params, const double hysteresis); /// @brief create the footprint of the given obstacle and its projection over a fixed time horizon @@ -37,16 +37,15 @@ autoware::universe_utils::MultiPolygon2d make_forward_footprints( /// @param [in] params parameters used to create the footprint /// @param [in] hysteresis [m] extra lateral distance to add to the footprint /// @return forward footprint of the obstacle -autoware::universe_utils::Polygon2d make_forward_footprint( +autoware_utils::Polygon2d make_forward_footprint( const autoware_perception_msgs::msg::PredictedObject & obstacle, const PlannerParam & params, const double hysteresis); /// @brief project a footprint to the given pose /// @param [in] base_footprint footprint to project /// @param [in] pose projection pose /// @return footprint projected to the given pose -autoware::universe_utils::Polygon2d project_to_pose( - const autoware::universe_utils::Polygon2d & base_footprint, - const geometry_msgs::msg::Pose & pose); +autoware_utils::Polygon2d project_to_pose( + const autoware_utils::Polygon2d & base_footprint, const geometry_msgs::msg::Pose & pose); /// @brief create the rtree indexing the ego footprint along the trajectory /// @param [inout] ego_data ego data with its trajectory and the rtree to populate /// @param [in] params parameters diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/object_filtering.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/object_filtering.cpp index 1e2f7f2466cf9..94d242f7403a2 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/object_filtering.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/object_filtering.cpp @@ -17,8 +17,8 @@ #include "types.hpp" #include -#include -#include +#include +#include #include @@ -74,21 +74,21 @@ bool is_unavoidable( const auto & o_pose = object.kinematics.initial_pose_with_covariance.pose; const auto o_yaw = tf2::getYaw(o_pose.orientation); const auto ego_yaw = tf2::getYaw(ego_pose.orientation); - const auto yaw_diff = std::abs(universe_utils::normalizeRadian(o_yaw - ego_yaw)); + const auto yaw_diff = std::abs(autoware_utils::normalize_radian(o_yaw - ego_yaw)); const auto opposite_heading = yaw_diff > same_direction_diff_threshold; const auto collision_distance_threshold = params.ego_lateral_offset + object.shape.dimensions.y / 2.0 + params.hysteresis; const auto lat_distance = - std::abs(universe_utils::calcLateralDeviation(o_pose, ego_pose.position)); + std::abs(autoware_utils::calc_lateral_deviation(o_pose, ego_pose.position)); auto has_collision = opposite_heading && lat_distance <= collision_distance_threshold; if (ego_earliest_stop_pose) { const auto direction_yaw = std::atan2( o_pose.position.y - ego_earliest_stop_pose->position.y, o_pose.position.x - ego_earliest_stop_pose->position.x); const auto yaw_diff_at_earliest_stop_pose = - std::abs(universe_utils::normalizeRadian(o_yaw - direction_yaw)); + std::abs(autoware_utils::normalize_radian(o_yaw - direction_yaw)); const auto lat_distance_at_earliest_stop_pose = - std::abs(universe_utils::calcLateralDeviation(o_pose, ego_earliest_stop_pose->position)); + std::abs(autoware_utils::calc_lateral_deviation(o_pose, ego_earliest_stop_pose->position)); const auto collision_at_earliest_stop_pose = yaw_diff_at_earliest_stop_pose > same_direction_diff_threshold && lat_distance_at_earliest_stop_pose <= collision_distance_threshold; diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/types.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/types.hpp index 2e3af577a83f4..efa7cfbb1b2e5 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/types.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/types.hpp @@ -15,7 +15,7 @@ #ifndef TYPES_HPP_ #define TYPES_HPP_ -#include +#include #include #include @@ -32,7 +32,7 @@ namespace autoware::motion_velocity_planner::dynamic_obstacle_stop { using TrajectoryPoints = std::vector; -using BoxIndexPair = std::pair; +using BoxIndexPair = std::pair; using Rtree = boost::geometry::index::rtree>; /// @brief parameters for the "out of lane" module @@ -57,7 +57,7 @@ struct EgoData size_t first_trajectory_idx{}; double longitudinal_offset_to_first_trajectory_idx{}; // [m] geometry_msgs::msg::Pose pose{}; - autoware::universe_utils::MultiPolygon2d trajectory_footprints{}; + autoware_utils::MultiPolygon2d trajectory_footprints{}; Rtree rtree{}; std::optional earliest_stop_pose{}; }; @@ -65,9 +65,9 @@ struct EgoData /// @brief debug data struct DebugData { - autoware::universe_utils::MultiPolygon2d obstacle_footprints{}; + autoware_utils::MultiPolygon2d obstacle_footprints{}; size_t prev_dynamic_obstacles_nb{}; - autoware::universe_utils::MultiPolygon2d ego_footprints{}; + autoware_utils::MultiPolygon2d ego_footprints{}; size_t prev_ego_footprints_nb{}; std::optional stop_pose{}; size_t prev_collisions_nb{}; diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/test/test_object_filtering.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/test/test_object_filtering.cpp index 3ff4cf0c9972f..c49899e867643 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/test/test_object_filtering.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/test/test_object_filtering.cpp @@ -15,7 +15,7 @@ #include "../src/object_filtering.hpp" #include -#include +#include #include #include @@ -160,18 +160,18 @@ TEST(TestObjectFiltering, isUnavoidable) ego_pose.position.y = 0.0; // ego and object heading in the same direction -> not unavoidable for (auto ego_yaw = -0.4; ego_yaw <= 0.4; ego_yaw += 0.1) { - ego_pose.orientation = autoware::universe_utils::createQuaternionFromYaw(ego_yaw); + ego_pose.orientation = autoware_utils::create_quaternion_from_yaw(ego_yaw); for (auto obj_yaw = -0.4; obj_yaw <= 0.4; obj_yaw += 0.1) { object.kinematics.initial_pose_with_covariance.pose.orientation = - autoware::universe_utils::createQuaternionFromYaw(obj_yaw); + autoware_utils::create_quaternion_from_yaw(obj_yaw); EXPECT_FALSE(is_unavoidable(object, ego_pose, ego_earliest_stop_pose, params)); } } // ego and object heading in opposite direction -> unavoidable for (auto ego_yaw = -0.4; ego_yaw <= 0.4; ego_yaw += 0.1) { - ego_pose.orientation = autoware::universe_utils::createQuaternionFromYaw(ego_yaw); + ego_pose.orientation = autoware_utils::create_quaternion_from_yaw(ego_yaw); object.kinematics.initial_pose_with_covariance.pose.orientation = - autoware::universe_utils::createQuaternionFromYaw(ego_yaw + M_PI); + autoware_utils::create_quaternion_from_yaw(ego_yaw + M_PI); EXPECT_TRUE(is_unavoidable(object, ego_pose, ego_earliest_stop_pose, params)); } @@ -179,16 +179,16 @@ TEST(TestObjectFiltering, isUnavoidable) object.kinematics.initial_pose_with_covariance.pose.position.x = 5.0; object.kinematics.initial_pose_with_covariance.pose.position.y = 5.0; for (auto ego_yaw = -0.4; ego_yaw <= 0.4; ego_yaw += 0.1) { - ego_pose.orientation = autoware::universe_utils::createQuaternionFromYaw(ego_yaw); + ego_pose.orientation = autoware_utils::create_quaternion_from_yaw(ego_yaw); object.kinematics.initial_pose_with_covariance.pose.orientation = - autoware::universe_utils::createQuaternionFromYaw(ego_yaw + M_PI); + autoware_utils::create_quaternion_from_yaw(ego_yaw + M_PI); EXPECT_FALSE(is_unavoidable(object, ego_pose, ego_earliest_stop_pose, params)); } // perpendicular case object.kinematics.initial_pose_with_covariance.pose.orientation = - autoware::universe_utils::createQuaternionFromYaw(-M_PI_2); - ego_pose.orientation = autoware::universe_utils::createQuaternionFromYaw(0.0); + autoware_utils::create_quaternion_from_yaw(-M_PI_2); + ego_pose.orientation = autoware_utils::create_quaternion_from_yaw(0.0); EXPECT_FALSE(is_unavoidable(object, ego_pose, ego_earliest_stop_pose, params)); // with earliest stop pose away from the object path -> no collision ego_earliest_stop_pose.emplace(); diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/package.xml b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/package.xml index e0cbf2c193b7a..e0f483b348224 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/package.xml +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/package.xml @@ -20,7 +20,7 @@ autoware_planning_msgs autoware_route_handler autoware_signal_processing - autoware_universe_utils + autoware_utils autoware_vehicle_info_utils geometry_msgs libboost-dev diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/src/obstacle_cruise_module.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/src/obstacle_cruise_module.cpp index d7b8d26761354..56199900ed1d8 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/src/obstacle_cruise_module.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/src/obstacle_cruise_module.cpp @@ -14,15 +14,15 @@ #include "obstacle_cruise_module.hpp" -#include "autoware/universe_utils/ros/uuid_helper.hpp" +#include "autoware_utils/ros/uuid_helper.hpp" #include #include #include -#include -#include -#include -#include +#include +#include +#include +#include #include #include @@ -44,7 +44,7 @@ double calc_diff_angle_against_trajectory( const double target_yaw = tf2::getYaw(target_pose.orientation); - const double diff_yaw = autoware::universe_utils::normalizeRadian(target_yaw - traj_yaw); + const double diff_yaw = autoware_utils::normalize_radian(target_yaw - traj_yaw); return diff_yaw; } @@ -110,7 +110,7 @@ void ObstacleCruiseModule::init(rclcpp::Node & node, const std::string & module_ // ros parameters planning_algorithm_ = - getOrDeclareParameter(node, "obstacle_cruise.option.planning_algorithm"); + get_or_declare_parameter(node, "obstacle_cruise.option.planning_algorithm"); common_param_ = CommonParam(node); cruise_planning_param_ = CruisePlanningParam(node); obstacle_filtering_param_ = ObstacleFilteringParam(node); @@ -126,7 +126,7 @@ void ObstacleCruiseModule::init(rclcpp::Node & node, const std::string & module_ metrics_pub_ = node.create_publisher("~/cruise/metrics", 10); debug_cruise_planning_info_pub_ = node.create_publisher("~/debug/cruise_planning_info", 1); - processing_time_detail_pub_ = node.create_publisher( + processing_time_detail_pub_ = node.create_publisher( "~/debug/processing_time_detail_ms/obstacle_cruise", 1); // interface @@ -138,7 +138,7 @@ void ObstacleCruiseModule::init(rclcpp::Node & node, const std::string & module_ &node, "obstacle_cruise"); // time keeper - time_keeper_ = std::make_shared(processing_time_detail_pub_); + time_keeper_ = std::make_shared(processing_time_detail_pub_); // cruise planner cruise_planner_ = create_cruise_planner(node); @@ -155,7 +155,7 @@ VelocityPlanningResult ObstacleCruiseModule::plan( smoothed_trajectory_points, const std::shared_ptr planner_data) { - autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + autoware_utils::ScopedTimeTrack st(__func__, *time_keeper_); // 1. init variables stop_watch_.tic(); @@ -217,7 +217,7 @@ std::vector ObstacleCruiseModule::filter_cruise_obstacle_for_pre const VehicleInfo & vehicle_info, const TrajectoryPolygonCollisionCheck & trajectory_polygon_collision_check) { - autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + autoware_utils::ScopedTimeTrack st(__func__, *time_keeper_); const auto & current_pose = odometry.pose.pose; @@ -284,7 +284,7 @@ std::vector ObstacleCruiseModule::filter_cruise_obstacle_for_pre void ObstacleCruiseModule::publish_debug_info() { - autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + autoware_utils::ScopedTimeTrack st(__func__, *time_keeper_); // 1. debug marker MarkerArray debug_marker; @@ -307,10 +307,10 @@ void ObstacleCruiseModule::publish_debug_info() // 1.2. collision points for (size_t i = 0; i < stop_collision_points.size(); ++i) { - auto collision_point_marker = autoware::universe_utils::createDefaultMarker( + auto collision_point_marker = autoware_utils::create_default_marker( "map", clock_->now(), "collision_points", i, Marker::SPHERE, - autoware::universe_utils::createMarkerScale(0.25, 0.25, 0.25), - autoware::universe_utils::createMarkerColor(1.0, 0.0, 0.0, 0.999)); + autoware_utils::create_marker_scale(0.25, 0.25, 0.25), + autoware_utils::create_marker_color(1.0, 0.0, 0.0, 0.999)); collision_point_marker.pose.position = stop_collision_points.at(i); debug_marker.markers.push_back(collision_point_marker); } @@ -325,10 +325,10 @@ void ObstacleCruiseModule::publish_debug_info() } // 1.4. detection area - auto decimated_traj_polys_marker = autoware::universe_utils::createDefaultMarker( + auto decimated_traj_polys_marker = autoware_utils::create_default_marker( "map", clock_->now(), "detection_area", 0, Marker::LINE_LIST, - autoware::universe_utils::createMarkerScale(0.01, 0.0, 0.0), - autoware::universe_utils::createMarkerColor(0.0, 1.0, 0.0, 0.999)); + autoware_utils::create_marker_scale(0.01, 0.0, 0.0), + autoware_utils::create_marker_color(0.0, 1.0, 0.0, 0.999)); for (const auto & decimated_traj_poly : debug_data_ptr_->decimated_traj_polys) { for (size_t dp_idx = 0; dp_idx < decimated_traj_poly.outer().size(); ++dp_idx) { const auto & current_point = decimated_traj_poly.outer().at(dp_idx); @@ -336,9 +336,9 @@ void ObstacleCruiseModule::publish_debug_info() decimated_traj_poly.outer().at((dp_idx + 1) % decimated_traj_poly.outer().size()); decimated_traj_polys_marker.points.push_back( - autoware::universe_utils::createPoint(current_point.x(), current_point.y(), 0.0)); + autoware_utils::create_point(current_point.x(), current_point.y(), 0.0)); decimated_traj_polys_marker.points.push_back( - autoware::universe_utils::createPoint(next_point.x(), next_point.y(), 0.0)); + autoware_utils::create_point(next_point.x(), next_point.y(), 0.0)); } } debug_marker.markers.push_back(decimated_traj_polys_marker); @@ -376,7 +376,7 @@ std::optional ObstacleCruiseModule::create_cruise_obstacle( const TrajectoryPolygonCollisionCheck & trajectory_polygon_collision_check) const { const auto & obj_uuid = object->predicted_object.object_id; - const auto & obj_uuid_str = autoware::universe_utils::toHexString(obj_uuid); + const auto & obj_uuid_str = autoware_utils::to_hex_string(obj_uuid); // NOTE: When driving backward, Stop will be planned instead of cruise. // When the obstacle is crossing the ego's trajectory, cruise can be ignored. @@ -543,7 +543,7 @@ ObstacleCruiseModule::create_collision_points_for_inside_cruise_obstacle( const TrajectoryPolygonCollisionCheck & trajectory_polygon_collision_check) const { const auto & obj_uuid = object->predicted_object.object_id; - const auto & obj_uuid_str = autoware::universe_utils::toHexString(obj_uuid); + const auto & obj_uuid_str = autoware_utils::to_hex_string(obj_uuid); // check label if (!is_inside_cruise_obstacle(object->predicted_object.classification.at(0).label)) { @@ -615,7 +615,7 @@ ObstacleCruiseModule::create_collision_points_for_outside_cruise_obstacle( const TrajectoryPolygonCollisionCheck & trajectory_polygon_collision_check) const { const auto & obj_uuid = object->predicted_object.object_id; - const auto & obj_uuid_str = autoware::universe_utils::toHexString(obj_uuid); + const auto & obj_uuid_str = autoware_utils::to_hex_string(obj_uuid); // check label if (!is_outside_cruise_obstacle(object->predicted_object.classification.at(0).label)) { @@ -710,7 +710,7 @@ std::optional ObstacleCruiseModule::create_yield_cruise_obstacle // check label const auto & obj_uuid = object->predicted_object.object_id; - const auto & obj_uuid_str = autoware::universe_utils::toHexString(obj_uuid); + const auto & obj_uuid_str = autoware_utils::to_hex_string(obj_uuid); if (!is_outside_cruise_obstacle(object->predicted_object.classification.at(0).label)) { RCLCPP_DEBUG( diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/src/obstacle_cruise_module.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/src/obstacle_cruise_module.hpp index d9e65c2a3b7db..cfb066c1d09df 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/src/obstacle_cruise_module.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/src/obstacle_cruise_module.hpp @@ -17,8 +17,8 @@ #include "autoware/motion_velocity_planner_common_universe/polygon_utils.hpp" #include "autoware/motion_velocity_planner_common_universe/utils.hpp" -#include "autoware/universe_utils/system/stop_watch.hpp" -#include "autoware/universe_utils/system/time_keeper.hpp" +#include "autoware_utils/system/stop_watch.hpp" +#include "autoware_utils/system/time_keeper.hpp" #include "cruise_planner_interface.hpp" #include "metrics_manager.hpp" #include "optimization_based_planner/optimization_based_planner.hpp" @@ -68,19 +68,18 @@ class ObstacleCruiseModule : public PluginModuleInterface rclcpp::Publisher::SharedPtr metrics_pub_; std::unique_ptr objects_of_interest_marker_interface_; - rclcpp::Publisher::SharedPtr - processing_time_detail_pub_; + rclcpp::Publisher::SharedPtr processing_time_detail_pub_; // cruise planner std::unique_ptr cruise_planner_; // internal variables - autoware::universe_utils::StopWatch stop_watch_; + autoware_utils::StopWatch stop_watch_; std::vector prev_cruise_object_obstacles_; mutable std::shared_ptr debug_data_ptr_; bool need_to_clear_velocity_limit_{false}; MetricsManager metrics_manager_; - mutable std::shared_ptr time_keeper_; + mutable std::shared_ptr time_keeper_; std::unique_ptr create_cruise_planner(rclcpp::Node & node) const; std::vector filter_cruise_obstacle_for_predicted_object( diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/src/optimization_based_planner/optimization_based_planner.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/src/optimization_based_planner/optimization_based_planner.cpp index c2489bbf3039f..9c053075a02d1 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/src/optimization_based_planner/optimization_based_planner.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/src/optimization_based_planner/optimization_based_planner.cpp @@ -23,8 +23,8 @@ #include "autoware/motion_utils/trajectory/interpolation.hpp" #include "autoware/motion_utils/trajectory/trajectory.hpp" #include "autoware/motion_velocity_planner_common_universe/utils.hpp" -#include "autoware/universe_utils/geometry/geometry.hpp" -#include "autoware/universe_utils/ros/marker_helper.hpp" +#include "autoware_utils/geometry/geometry.hpp" +#include "autoware_utils/ros/marker_helper.hpp" #include #include @@ -480,7 +480,7 @@ std::optional OptimizationBasedPlanner::getSBoundaries( const auto markers = autoware::motion_utils::createSlowDownVirtualWallMarker( marker_pose.value(), "obstacle to follow", current_time, 0); - autoware::universe_utils::appendMarkerArray(markers, &wall_msg); + autoware_utils::append_marker_array(markers, &wall_msg); // publish rviz marker debug_wall_marker_pub_->publish(wall_msg); diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/src/parameters.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/src/parameters.hpp index af50fb0d4325a..54538343d19d1 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/src/parameters.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/src/parameters.hpp @@ -20,9 +20,9 @@ #include "autoware/motion_utils/trajectory/trajectory.hpp" #include "autoware/motion_velocity_planner_common_universe/utils.hpp" #include "autoware/object_recognition_utils/predicted_path_utils.hpp" -#include "autoware/universe_utils/ros/parameter.hpp" -#include "autoware/universe_utils/ros/update_param.hpp" -#include "autoware/universe_utils/system/stop_watch.hpp" +#include "autoware_utils/ros/parameter.hpp" +#include "autoware_utils/ros/update_param.hpp" +#include "autoware_utils/system/stop_watch.hpp" #include "type_alias.hpp" #include "types.hpp" @@ -33,7 +33,7 @@ namespace autoware::motion_velocity_planner { -using autoware::universe_utils::getOrDeclareParameter; +using autoware_utils::get_or_declare_parameter; struct CommonParam { @@ -49,14 +49,14 @@ struct CommonParam CommonParam() = default; explicit CommonParam(rclcpp::Node & node) { - max_accel = getOrDeclareParameter(node, "normal.max_acc"); - min_accel = getOrDeclareParameter(node, "normal.min_acc"); - max_jerk = getOrDeclareParameter(node, "normal.max_jerk"); - min_jerk = getOrDeclareParameter(node, "normal.min_jerk"); - limit_max_accel = getOrDeclareParameter(node, "limit.max_acc"); - limit_min_accel = getOrDeclareParameter(node, "limit.min_acc"); - limit_max_jerk = getOrDeclareParameter(node, "limit.max_jerk"); - limit_min_jerk = getOrDeclareParameter(node, "limit.min_jerk"); + max_accel = get_or_declare_parameter(node, "normal.max_acc"); + min_accel = get_or_declare_parameter(node, "normal.min_acc"); + max_jerk = get_or_declare_parameter(node, "normal.max_jerk"); + min_jerk = get_or_declare_parameter(node, "normal.min_jerk"); + limit_max_accel = get_or_declare_parameter(node, "limit.max_acc"); + limit_min_accel = get_or_declare_parameter(node, "limit.min_acc"); + limit_max_jerk = get_or_declare_parameter(node, "limit.max_jerk"); + limit_min_jerk = get_or_declare_parameter(node, "limit.min_jerk"); } }; @@ -93,43 +93,43 @@ struct ObstacleFilteringParam utils::get_target_object_type(node, "obstacle_cruise.obstacle_filtering.object_type.inside."); outside_object_types = utils::get_target_object_type( node, "obstacle_cruise.obstacle_filtering.object_type.outside."); - // use_pointcloud = getOrDeclareParameter( + // use_pointcloud = get_or_declare_parameter( // node, "obstacle_cruise.obstacle_filtering.object_type.pointcloud"); max_lat_margin = - getOrDeclareParameter(node, "obstacle_cruise.obstacle_filtering.max_lat_margin"); + get_or_declare_parameter(node, "obstacle_cruise.obstacle_filtering.max_lat_margin"); - crossing_obstacle_velocity_threshold = getOrDeclareParameter( + crossing_obstacle_velocity_threshold = get_or_declare_parameter( node, "obstacle_cruise.obstacle_filtering.crossing_obstacle.obstacle_velocity_threshold"); - crossing_obstacle_traj_angle_threshold = getOrDeclareParameter( + crossing_obstacle_traj_angle_threshold = get_or_declare_parameter( node, "obstacle_cruise.obstacle_filtering.crossing_obstacle.obstacle_traj_angle_threshold"); - outside_obstacle_velocity_threshold = getOrDeclareParameter( + outside_obstacle_velocity_threshold = get_or_declare_parameter( node, "obstacle_cruise.obstacle_filtering.outside_obstacle.obstacle_velocity_threshold"); - ego_obstacle_overlap_time_threshold = getOrDeclareParameter( + ego_obstacle_overlap_time_threshold = get_or_declare_parameter( node, "obstacle_cruise.obstacle_filtering.outside_obstacle.ego_obstacle_overlap_time_threshold"); - max_prediction_time_for_collision_check = getOrDeclareParameter( + max_prediction_time_for_collision_check = get_or_declare_parameter( node, "obstacle_cruise.obstacle_filtering.outside_obstacle.max_prediction_time_for_collision_" "check"); - max_lateral_time_margin = getOrDeclareParameter( + max_lateral_time_margin = get_or_declare_parameter( node, "obstacle_cruise.obstacle_filtering.outside_obstacle.max_lateral_time_margin"); - num_of_predicted_paths_for_outside_cruise_obstacle = getOrDeclareParameter( + num_of_predicted_paths_for_outside_cruise_obstacle = get_or_declare_parameter( node, "obstacle_cruise.obstacle_filtering.outside_obstacle.num_of_predicted_paths"); enable_yield = - getOrDeclareParameter(node, "obstacle_cruise.obstacle_filtering.yield.enable_yield"); - yield_lat_distance_threshold = getOrDeclareParameter( + get_or_declare_parameter(node, "obstacle_cruise.obstacle_filtering.yield.enable_yield"); + yield_lat_distance_threshold = get_or_declare_parameter( node, "obstacle_cruise.obstacle_filtering.yield.lat_distance_threshold"); - max_lat_dist_between_obstacles = getOrDeclareParameter( + max_lat_dist_between_obstacles = get_or_declare_parameter( node, "obstacle_cruise.obstacle_filtering.yield.max_lat_dist_between_obstacles"); - max_obstacles_collision_time = getOrDeclareParameter( + max_obstacles_collision_time = get_or_declare_parameter( node, "obstacle_cruise.obstacle_filtering.yield.max_obstacles_collision_time"); - stopped_obstacle_velocity_threshold = getOrDeclareParameter( + stopped_obstacle_velocity_threshold = get_or_declare_parameter( node, "obstacle_cruise.obstacle_filtering.yield.stopped_obstacle_velocity_threshold"); - obstacle_velocity_threshold_from_cruise = getOrDeclareParameter( + obstacle_velocity_threshold_from_cruise = get_or_declare_parameter( node, "obstacle_cruise.obstacle_filtering.obstacle_velocity_threshold_from_cruise"); - obstacle_velocity_threshold_to_cruise = getOrDeclareParameter( + obstacle_velocity_threshold_to_cruise = get_or_declare_parameter( node, "obstacle_cruise.obstacle_filtering.obstacle_velocity_threshold_to_cruise"); } }; @@ -145,13 +145,13 @@ struct CruisePlanningParam explicit CruisePlanningParam(rclcpp::Node & node) { idling_time = - getOrDeclareParameter(node, "obstacle_cruise.cruise_planning.idling_time"); - min_ego_accel_for_rss = - getOrDeclareParameter(node, "obstacle_cruise.cruise_planning.min_ego_accel_for_rss"); - min_object_accel_for_rss = getOrDeclareParameter( + get_or_declare_parameter(node, "obstacle_cruise.cruise_planning.idling_time"); + min_ego_accel_for_rss = get_or_declare_parameter( + node, "obstacle_cruise.cruise_planning.min_ego_accel_for_rss"); + min_object_accel_for_rss = get_or_declare_parameter( node, "obstacle_cruise.cruise_planning.min_object_accel_for_rss"); - safe_distance_margin = - getOrDeclareParameter(node, "obstacle_cruise.cruise_planning.safe_distance_margin"); + safe_distance_margin = get_or_declare_parameter( + node, "obstacle_cruise.cruise_planning.safe_distance_margin"); } }; } // namespace autoware::motion_velocity_planner diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/src/pid_based_planner/pid_based_planner.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/src/pid_based_planner/pid_based_planner.cpp index 6bada506b977b..3984c3d935ccb 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/src/pid_based_planner/pid_based_planner.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/src/pid_based_planner/pid_based_planner.cpp @@ -20,9 +20,9 @@ #include "autoware/motion_utils/trajectory/interpolation.hpp" #include "autoware/motion_utils/trajectory/trajectory.hpp" #include "autoware/motion_velocity_planner_common_universe/utils.hpp" -#include "autoware/universe_utils/geometry/geometry.hpp" -#include "autoware/universe_utils/ros/marker_helper.hpp" -#include "autoware/universe_utils/ros/update_param.hpp" +#include "autoware_utils/geometry/geometry.hpp" +#include "autoware_utils/ros/marker_helper.hpp" +#include "autoware_utils/ros/update_param.hpp" #include "tier4_planning_msgs/msg/velocity_limit.hpp" @@ -350,9 +350,8 @@ std::vector PIDBasedPlanner::plan_cruise_trajectory( stop_traj_points.at(wall_idx).pose, wall_reason_string, clock_->now(), 0); // NOTE: use a different color from slow down one to visualize cruise and slow down // separately. - markers.markers.front().color = - autoware::universe_utils::createMarkerColor(1.0, 0.6, 0.1, 0.5); - autoware::universe_utils::appendMarkerArray(markers, &debug_data_ptr->cruise_wall_marker); + markers.markers.front().color = autoware_utils::create_marker_color(1.0, 0.6, 0.1, 0.5); + autoware_utils::append_marker_array(markers, &debug_data_ptr->cruise_wall_marker); // cruise obstacle debug_data_ptr->obstacles_to_cruise.push_back(cruise_obstacle_info->obstacle); @@ -383,7 +382,7 @@ std::vector PIDBasedPlanner::plan_cruise_trajectory( // delete marker const auto markers = autoware::motion_utils::createDeletedSlowDownVirtualWallMarker(clock_->now(), 0); - autoware::universe_utils::appendMarkerArray(markers, &debug_data_ptr->cruise_wall_marker); + autoware_utils::append_marker_array(markers, &debug_data_ptr->cruise_wall_marker); return stop_traj_points; } @@ -637,7 +636,7 @@ std::vector PIDBasedPlanner::get_acceleration_limited_trajector double sum_dist = 0.0; for (size_t i = ego_seg_idx; i < acc_limited_traj_points.size(); ++i) { if (i != ego_seg_idx) { - sum_dist += autoware::universe_utils::calcDistance2d( + sum_dist += autoware_utils::calc_distance2d( acc_limited_traj_points.at(i - 1), acc_limited_traj_points.at(i)); } @@ -660,7 +659,7 @@ std::vector PIDBasedPlanner::get_acceleration_limited_trajector void PIDBasedPlanner::update_parameters(const std::vector & parameters) { - autoware::universe_utils::updateParam( + autoware_utils::update_param( parameters, "obstacle_cruise.cruise_planning.pid_based_planner.min_accel_during_cruise", min_accel_during_cruise_); @@ -671,33 +670,33 @@ void PIDBasedPlanner::update_parameters(const std::vector & p double ki = p.pid_vel_controller->getKi(); double kd = p.pid_vel_controller->getKd(); - autoware::universe_utils::updateParam( + autoware_utils::update_param( parameters, "obstacle_cruise.cruise_planning.pid_based_planner.velocity_limit_based_planner.kp", kp); - autoware::universe_utils::updateParam( + autoware_utils::update_param( parameters, "obstacle_cruise.cruise_planning.pid_based_planner.velocity_limit_based_planner.ki", ki); - autoware::universe_utils::updateParam( + autoware_utils::update_param( parameters, "obstacle_cruise.cruise_planning.pid_based_planner.velocity_limit_based_planner.kd", kd); p.pid_vel_controller->updateParam(kp, ki, kd); - autoware::universe_utils::updateParam( + autoware_utils::update_param( parameters, "obstacle_cruise.cruise_planning.pid_based_planner.velocity_limit_based_planner.output_ratio_" "during_accel", p.output_ratio_during_accel); - autoware::universe_utils::updateParam( + autoware_utils::update_param( parameters, "obstacle_cruise.cruise_planning.pid_based_planner.vel_to_acc_weight", p.vel_to_acc_weight); - autoware::universe_utils::updateParam( + autoware_utils::update_param( parameters, "obstacle_cruise.cruise_planning.pid_based_planner.velocity_limit_based_planner.enable_jerk_" "limit_to_output_acc", p.enable_jerk_limit_to_output_acc); - autoware::universe_utils::updateParam( + autoware_utils::update_param( parameters, "obstacle_cruise.cruise_planning.pid_based_planner.velocity_limit_based_planner.disable_" "target_acceleration", @@ -712,15 +711,15 @@ void PIDBasedPlanner::update_parameters(const std::vector & p double ki_acc = p.pid_acc_controller->getKi(); double kd_acc = p.pid_acc_controller->getKd(); - autoware::universe_utils::updateParam( + autoware_utils::update_param( parameters, "obstacle_cruise.cruise_planning.pid_based_planner.velocity_insertion_based_planner.kp_acc", kp_acc); - autoware::universe_utils::updateParam( + autoware_utils::update_param( parameters, "obstacle_cruise.cruise_planning.pid_based_planner.velocity_insertion_based_planner.ki_acc", ki_acc); - autoware::universe_utils::updateParam( + autoware_utils::update_param( parameters, "obstacle_cruise.cruise_planning.pid_based_planner.velocity_insertion_based_planner.kd_acc", kd_acc); @@ -731,32 +730,32 @@ void PIDBasedPlanner::update_parameters(const std::vector & p double ki_jerk = p.pid_jerk_controller->getKi(); double kd_jerk = p.pid_jerk_controller->getKd(); - autoware::universe_utils::updateParam( + autoware_utils::update_param( parameters, "obstacle_cruise.cruise_planning.pid_based_planner.velocity_insertion_based_planner.kp_jerk", kp_jerk); - autoware::universe_utils::updateParam( + autoware_utils::update_param( parameters, "obstacle_cruise.cruise_planning.pid_based_planner.velocity_insertion_based_planner.ki_jerk", ki_jerk); - autoware::universe_utils::updateParam( + autoware_utils::update_param( parameters, "obstacle_cruise.cruise_planning.pid_based_planner.velocity_insertion_based_planner.kd_jerk", kd_jerk); p.pid_jerk_controller->updateParam(kp_jerk, ki_jerk, kd_jerk); - autoware::universe_utils::updateParam( + autoware_utils::update_param( parameters, "obstacle_cruise.cruise_planning.pid_based_planner.velocity_insertion_based_planner.output_" "acc_ratio_during_accel", p.output_acc_ratio_during_accel); - autoware::universe_utils::updateParam( + autoware_utils::update_param( parameters, "obstacle_cruise.cruise_planning.pid_based_planner.velocity_insertion_based_planner.output_" "jerk_ratio_during_accel", p.output_jerk_ratio_during_accel); - autoware::universe_utils::updateParam( + autoware_utils::update_param( parameters, "obstacle_cruise.cruise_planning.pid_based_planner.velocity_insertion_based_planner.enable_" "jerk_limit_to_output_acc", @@ -764,10 +763,10 @@ void PIDBasedPlanner::update_parameters(const std::vector & p } // min_cruise_target_vel - autoware::universe_utils::updateParam( + autoware_utils::update_param( parameters, "obstacle_cruise.cruise_planning.pid_based_planner.min_cruise_target_vel", min_cruise_target_vel_); - autoware::universe_utils::updateParam( + autoware_utils::update_param( parameters, "obstacle_cruise.cruise_planning.pid_based_planner.time_to_evaluate_rss", time_to_evaluate_rss_); } diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/src/type_alias.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/src/type_alias.hpp index 673574c24a914..24468376c5ae7 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/src/type_alias.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/src/type_alias.hpp @@ -58,8 +58,8 @@ using tier4_planning_msgs::msg::VelocityLimitClearCommand; using visualization_msgs::msg::Marker; using visualization_msgs::msg::MarkerArray; namespace bg = boost::geometry; -using autoware::universe_utils::Point2d; -using autoware::universe_utils::Polygon2d; +using autoware_utils::Point2d; +using autoware_utils::Polygon2d; using Metric = tier4_metric_msgs::msg::Metric; using MetricArray = tier4_metric_msgs::msg::MetricArray; using PointCloud = pcl::PointCloud; diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_slow_down_module/package.xml b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_slow_down_module/package.xml index aa1ec002ce5cc..7d445476618ad 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_slow_down_module/package.xml +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_slow_down_module/package.xml @@ -19,7 +19,7 @@ autoware_planning_msgs autoware_route_handler autoware_signal_processing - autoware_universe_utils + autoware_utils autoware_vehicle_info_utils geometry_msgs libboost-dev diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_slow_down_module/src/obstacle_slow_down_module.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_slow_down_module/src/obstacle_slow_down_module.cpp index 2b960af861df0..45bab49d3397b 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_slow_down_module/src/obstacle_slow_down_module.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_slow_down_module/src/obstacle_slow_down_module.cpp @@ -19,11 +19,11 @@ #include #include #include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include #include #include @@ -36,7 +36,7 @@ namespace autoware::motion_velocity_planner { -using autoware::universe_utils::getOrDeclareParameter; +using autoware_utils::get_or_declare_parameter; namespace { @@ -55,7 +55,7 @@ bool is_lower_considering_hysteresis( return false; } -geometry_msgs::msg::Point to_geom_point(const autoware::universe_utils::Point2d & point) +geometry_msgs::msg::Point to_geom_point(const autoware_utils::Point2d & point) { geometry_msgs::msg::Point geom_point; geom_point.x = point.x(); @@ -211,7 +211,7 @@ void ObstacleSlowDownModule::init(rclcpp::Node & node, const std::string & modul metrics_pub_ = node.create_publisher("~/slow_down/metrics", 10); debug_slow_down_planning_info_pub_ = node.create_publisher("~/debug/slow_down_planning_info", 1); - processing_time_detail_pub_ = node.create_publisher( + processing_time_detail_pub_ = node.create_publisher( "~/debug/processing_time_detail_ms/obstacle_slow_down", 1); // interface publisher @@ -223,7 +223,7 @@ void ObstacleSlowDownModule::init(rclcpp::Node & node, const std::string & modul &node, "obstacle_slow_down"); // time keeper - time_keeper_ = std::make_shared(processing_time_detail_pub_); + time_keeper_ = std::make_shared(processing_time_detail_pub_); } void ObstacleSlowDownModule::update_parameters( @@ -240,7 +240,7 @@ ObstacleSlowDownModule::convert_point_cloud_to_slow_down_points( return {}; } - autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + autoware_utils::ScopedTimeTrack st(__func__, *time_keeper_); const auto & p = obstacle_filtering_param_; @@ -325,7 +325,7 @@ VelocityPlanningResult ObstacleSlowDownModule::plan( smoothed_trajectory_points, const std::shared_ptr planner_data) { - autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + autoware_utils::ScopedTimeTrack st(__func__, *time_keeper_); stop_watch_.tic(); debug_data_ptr_ = std::make_shared(); @@ -390,7 +390,7 @@ ObstacleSlowDownModule::filter_slow_down_obstacle_for_predicted_object( const rclcpp::Time & predicted_objects_stamp, const VehicleInfo & vehicle_info, const TrajectoryPolygonCollisionCheck & trajectory_polygon_collision_check) { - autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + autoware_utils::ScopedTimeTrack st(__func__, *time_keeper_); const auto & current_pose = odometry.pose.pose; @@ -452,7 +452,7 @@ std::vector ObstacleSlowDownModule::filter_slow_down_obstacle_ const PlannerData::Pointcloud & point_cloud, const VehicleInfo & vehicle_info, const TrajectoryPolygonCollisionCheck & trajectory_polygon_collision_check, size_t ego_idx) { - autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + autoware_utils::ScopedTimeTrack st(__func__, *time_keeper_); // calculate collision points with trajectory with lateral stop margin // NOTE: For additional margin, hysteresis is not divided by two. @@ -500,12 +500,12 @@ ObstacleSlowDownModule::create_slow_down_obstacle_for_predicted_object( const std::shared_ptr object, const rclcpp::Time & predicted_objects_stamp, const double dist_from_obj_poly_to_traj_poly) { - autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + autoware_utils::ScopedTimeTrack st(__func__, *time_keeper_); const auto & p = obstacle_filtering_param_; const auto & obj_uuid = object->predicted_object.object_id; - const auto & obj_uuid_str = autoware::universe_utils::toHexString(obj_uuid); + const auto & obj_uuid_str = autoware_utils::to_hex_string(obj_uuid); const auto & obj_label = object->predicted_object.classification.at(0).label; slow_down_condition_counter_.add_current_uuid(obj_uuid_str); @@ -555,7 +555,7 @@ ObstacleSlowDownModule::create_slow_down_obstacle_for_predicted_object( return std::nullopt; } - const auto obstacle_poly = autoware::universe_utils::toPolygon2d( + const auto obstacle_poly = autoware_utils::to_polygon2d( object->predicted_object.kinematics.initial_pose_with_covariance.pose, object->predicted_object.shape); @@ -638,7 +638,7 @@ std::optional ObstacleSlowDownModule::create_slow_down_obstacl return std::nullopt; } const unique_identifier_msgs::msg::UUID obj_uuid; - const auto & obj_uuid_str = autoware::universe_utils::toHexString(obj_uuid); + const auto & obj_uuid_str = autoware_utils::to_hex_string(obj_uuid); ObjectClassification unknown_object_classification; unknown_object_classification.label = ObjectClassification::UNKNOWN; @@ -666,7 +666,7 @@ std::vector ObstacleSlowDownModule::plan_slow_down( const std::vector & traj_points, const std::vector & obstacles, [[maybe_unused]] std::optional & velocity_limit, const VehicleInfo & vehicle_info) { - autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + autoware_utils::ScopedTimeTrack st(__func__, *time_keeper_); auto slow_down_traj_points = traj_points; slow_down_debug_multi_array_ = Float32MultiArrayStamped(); @@ -776,7 +776,7 @@ std::vector ObstacleSlowDownModule::plan_slow_down( const auto markers = autoware::motion_utils::createSlowDownVirtualWallMarker( slow_down_traj_points.at(slow_down_wall_idx).pose, "obstacle slow down", clock_->now(), i, abs_ego_offset, "", planner_data->is_driving_forward); - autoware::universe_utils::appendMarkerArray(markers, &debug_data_ptr_->slow_down_wall_marker); + autoware_utils::append_marker_array(markers, &debug_data_ptr_->slow_down_wall_marker); // update planning factor planning_factor_interface_->add( @@ -791,15 +791,13 @@ std::vector ObstacleSlowDownModule::plan_slow_down( const auto markers = autoware::motion_utils::createSlowDownVirtualWallMarker( slow_down_traj_points.at(*slow_down_start_idx).pose, "obstacle slow down start", clock_->now(), i * 2, abs_ego_offset, "", planner_data->is_driving_forward); - autoware::universe_utils::appendMarkerArray( - markers, &debug_data_ptr_->slow_down_debug_wall_marker); + autoware_utils::append_marker_array(markers, &debug_data_ptr_->slow_down_debug_wall_marker); } if (slow_down_end_idx) { const auto markers = autoware::motion_utils::createSlowDownVirtualWallMarker( slow_down_traj_points.at(*slow_down_end_idx).pose, "obstacle slow down end", clock_->now(), i * 2 + 1, abs_ego_offset, "", planner_data->is_driving_forward); - autoware::universe_utils::appendMarkerArray( - markers, &debug_data_ptr_->slow_down_debug_wall_marker); + autoware_utils::append_marker_array(markers, &debug_data_ptr_->slow_down_debug_wall_marker); } // Add debug data @@ -827,7 +825,7 @@ Float32MultiArrayStamped ObstacleSlowDownModule::get_slow_down_planning_debug_me void ObstacleSlowDownModule::publish_debug_info() { - autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + autoware_utils::ScopedTimeTrack st(__func__, *time_keeper_); // 1. debug marker MarkerArray debug_marker; @@ -840,16 +838,16 @@ void ObstacleSlowDownModule::publish_debug_info() debug_marker.markers.push_back(obstacle_marker); // collision points - auto front_collision_point_marker = autoware::universe_utils::createDefaultMarker( + auto front_collision_point_marker = autoware_utils::create_default_marker( "map", clock_->now(), "collision_points", i * 2 + 0, Marker::SPHERE, - autoware::universe_utils::createMarkerScale(0.25, 0.25, 0.25), - autoware::universe_utils::createMarkerColor(1.0, 0.0, 0.0, 0.999)); + autoware_utils::create_marker_scale(0.25, 0.25, 0.25), + autoware_utils::create_marker_color(1.0, 0.0, 0.0, 0.999)); front_collision_point_marker.pose.position = debug_data_ptr_->obstacles_to_slow_down.at(i).front_collision_point; - auto back_collision_point_marker = autoware::universe_utils::createDefaultMarker( + auto back_collision_point_marker = autoware_utils::create_default_marker( "map", clock_->now(), "collision_points", i * 2 + 1, Marker::SPHERE, - autoware::universe_utils::createMarkerScale(0.25, 0.25, 0.25), - autoware::universe_utils::createMarkerColor(1.0, 0.0, 0.0, 0.999)); + autoware_utils::create_marker_scale(0.25, 0.25, 0.25), + autoware_utils::create_marker_color(1.0, 0.0, 0.0, 0.999)); back_collision_point_marker.pose.position = debug_data_ptr_->obstacles_to_slow_down.at(i).back_collision_point; @@ -858,14 +856,13 @@ void ObstacleSlowDownModule::publish_debug_info() } // 1.2. slow down debug wall marker - autoware::universe_utils::appendMarkerArray( - debug_data_ptr_->slow_down_debug_wall_marker, &debug_marker); + autoware_utils::append_marker_array(debug_data_ptr_->slow_down_debug_wall_marker, &debug_marker); // 1.3. detection area - auto decimated_traj_polys_marker = autoware::universe_utils::createDefaultMarker( + auto decimated_traj_polys_marker = autoware_utils::create_default_marker( "map", clock_->now(), "detection_area", 0, Marker::LINE_LIST, - autoware::universe_utils::createMarkerScale(0.01, 0.0, 0.0), - autoware::universe_utils::createMarkerColor(0.0, 1.0, 0.0, 0.999)); + autoware_utils::create_marker_scale(0.01, 0.0, 0.0), + autoware_utils::create_marker_color(0.0, 1.0, 0.0, 0.999)); for (const auto & decimated_traj_poly : debug_data_ptr_->decimated_traj_polys) { for (size_t dp_idx = 0; dp_idx < decimated_traj_poly.outer().size(); ++dp_idx) { const auto & current_point = decimated_traj_poly.outer().at(dp_idx); @@ -873,9 +870,9 @@ void ObstacleSlowDownModule::publish_debug_info() decimated_traj_poly.outer().at((dp_idx + 1) % decimated_traj_poly.outer().size()); decimated_traj_polys_marker.points.push_back( - autoware::universe_utils::createPoint(current_point.x(), current_point.y(), 0.0)); + autoware_utils::create_point(current_point.x(), current_point.y(), 0.0)); decimated_traj_polys_marker.points.push_back( - autoware::universe_utils::createPoint(next_point.x(), next_point.y(), 0.0)); + autoware_utils::create_point(next_point.x(), next_point.y(), 0.0)); } } debug_marker.markers.push_back(decimated_traj_polys_marker); @@ -916,7 +913,7 @@ ObstacleSlowDownModule::calculate_distance_to_slow_down_with_constraints( const std::optional & prev_output, const double dist_to_ego, const VehicleInfo & vehicle_info, const bool is_obstacle_moving) const { - autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + autoware_utils::ScopedTimeTrack st(__func__, *time_keeper_); const double abs_ego_offset = planner_data->is_driving_forward ? std::abs(vehicle_info.max_longitudinal_offset_m) diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_slow_down_module/src/obstacle_slow_down_module.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_slow_down_module/src/obstacle_slow_down_module.hpp index 143bd2b1768e3..4be6b0165869a 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_slow_down_module/src/obstacle_slow_down_module.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_slow_down_module/src/obstacle_slow_down_module.hpp @@ -17,8 +17,8 @@ #include "autoware/motion_velocity_planner_common_universe/polygon_utils.hpp" #include "autoware/motion_velocity_planner_common_universe/utils.hpp" -#include "autoware/universe_utils/system/stop_watch.hpp" -#include "autoware/universe_utils/system/time_keeper.hpp" +#include "autoware_utils/system/stop_watch.hpp" +#include "autoware_utils/system/time_keeper.hpp" #include "metrics_manager.hpp" #include "parameters.hpp" #include "type_alias.hpp" @@ -72,8 +72,7 @@ class ObstacleSlowDownModule : public PluginModuleInterface // module publisher rclcpp::Publisher::SharedPtr metrics_pub_; rclcpp::Publisher::SharedPtr debug_slow_down_planning_info_pub_; - rclcpp::Publisher::SharedPtr - processing_time_detail_pub_; + rclcpp::Publisher::SharedPtr processing_time_detail_pub_; // interface publisher std::unique_ptr @@ -84,11 +83,11 @@ class ObstacleSlowDownModule : public PluginModuleInterface std::vector prev_slow_down_output_; SlowDownConditionCounter slow_down_condition_counter_; Float32MultiArrayStamped slow_down_debug_multi_array_; - autoware::universe_utils::StopWatch stop_watch_; + autoware_utils::StopWatch stop_watch_; mutable std::shared_ptr debug_data_ptr_; MetricsManager metrics_manager_; bool need_to_clear_velocity_limit_{false}; - mutable std::shared_ptr time_keeper_; + mutable std::shared_ptr time_keeper_; mutable std::optional> decimated_traj_polys_{std::nullopt}; std::vector diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_slow_down_module/src/parameters.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_slow_down_module/src/parameters.hpp index 3bc11232b9ef6..a921b0d593ec9 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_slow_down_module/src/parameters.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_slow_down_module/src/parameters.hpp @@ -19,7 +19,7 @@ #include "type_alias.hpp" #include "types.hpp" -#include +#include #include #include @@ -27,7 +27,7 @@ namespace autoware::motion_velocity_planner { -using autoware::universe_utils::getOrDeclareParameter; +using autoware_utils::get_or_declare_parameter; struct CommonParam { @@ -43,14 +43,14 @@ struct CommonParam CommonParam() = default; explicit CommonParam(rclcpp::Node & node) { - max_accel = getOrDeclareParameter(node, "normal.max_acc"); - min_accel = getOrDeclareParameter(node, "normal.min_acc"); - max_jerk = getOrDeclareParameter(node, "normal.max_jerk"); - min_jerk = getOrDeclareParameter(node, "normal.min_jerk"); - limit_max_accel = getOrDeclareParameter(node, "limit.max_acc"); - limit_min_accel = getOrDeclareParameter(node, "limit.min_acc"); - limit_max_jerk = getOrDeclareParameter(node, "limit.max_jerk"); - limit_min_jerk = getOrDeclareParameter(node, "limit.min_jerk"); + max_accel = get_or_declare_parameter(node, "normal.max_acc"); + min_accel = get_or_declare_parameter(node, "normal.min_acc"); + max_jerk = get_or_declare_parameter(node, "normal.max_jerk"); + min_jerk = get_or_declare_parameter(node, "normal.min_jerk"); + limit_max_accel = get_or_declare_parameter(node, "limit.max_acc"); + limit_min_accel = get_or_declare_parameter(node, "limit.min_acc"); + limit_max_jerk = get_or_declare_parameter(node, "limit.max_jerk"); + limit_min_jerk = get_or_declare_parameter(node, "limit.min_jerk"); } }; @@ -82,34 +82,33 @@ struct ObstacleFilteringParam ObstacleFilteringParam() = default; explicit ObstacleFilteringParam(rclcpp::Node & node) { - use_pointcloud = getOrDeclareParameter( + use_pointcloud = get_or_declare_parameter( node, "obstacle_slow_down.obstacle_filtering.object_type.pointcloud"); object_types = utils::get_target_object_type(node, "obstacle_slow_down.obstacle_filtering.object_type."); - min_lat_margin = - getOrDeclareParameter(node, "obstacle_slow_down.obstacle_filtering.min_lat_margin"); - max_lat_margin = - getOrDeclareParameter(node, "obstacle_slow_down.obstacle_filtering.max_lat_margin"); - - lat_hysteresis_margin = getOrDeclareParameter( + min_lat_margin = get_or_declare_parameter( + node, "obstacle_slow_down.obstacle_filtering.min_lat_margin"); + max_lat_margin = get_or_declare_parameter( + node, "obstacle_slow_down.obstacle_filtering.max_lat_margin"); + lat_hysteresis_margin = get_or_declare_parameter( node, "obstacle_slow_down.obstacle_filtering.lat_hysteresis_margin"); - successive_num_to_entry_slow_down_condition = getOrDeclareParameter( + successive_num_to_entry_slow_down_condition = get_or_declare_parameter( node, "obstacle_slow_down.obstacle_filtering.successive_num_to_entry_slow_down_condition"); - successive_num_to_exit_slow_down_condition = getOrDeclareParameter( + successive_num_to_exit_slow_down_condition = get_or_declare_parameter( node, "obstacle_slow_down.obstacle_filtering.successive_num_to_exit_slow_down_condition"); - pointcloud_obstacle_filtering_param.pointcloud_voxel_grid_x = getOrDeclareParameter( + pointcloud_obstacle_filtering_param.pointcloud_voxel_grid_x = get_or_declare_parameter( node, "obstacle_slow_down.obstacle_filtering.pointcloud.pointcloud_voxel_grid_x"); - pointcloud_obstacle_filtering_param.pointcloud_voxel_grid_y = getOrDeclareParameter( + pointcloud_obstacle_filtering_param.pointcloud_voxel_grid_y = get_or_declare_parameter( node, "obstacle_slow_down.obstacle_filtering.pointcloud.pointcloud_voxel_grid_y"); - pointcloud_obstacle_filtering_param.pointcloud_voxel_grid_z = getOrDeclareParameter( + pointcloud_obstacle_filtering_param.pointcloud_voxel_grid_z = get_or_declare_parameter( node, "obstacle_slow_down.obstacle_filtering.pointcloud.pointcloud_voxel_grid_z"); pointcloud_obstacle_filtering_param.pointcloud_cluster_tolerance = - getOrDeclareParameter( + get_or_declare_parameter( node, "obstacle_slow_down.obstacle_filtering.pointcloud.pointcloud_cluster_tolerance"); - pointcloud_obstacle_filtering_param.pointcloud_min_cluster_size = getOrDeclareParameter( + pointcloud_obstacle_filtering_param.pointcloud_min_cluster_size = get_or_declare_parameter( node, "obstacle_slow_down.obstacle_filtering.pointcloud.pointcloud_min_cluster_size"); - pointcloud_obstacle_filtering_param.pointcloud_max_cluster_size = getOrDeclareParameter( + pointcloud_obstacle_filtering_param.pointcloud_max_cluster_size = get_or_declare_parameter( node, "obstacle_slow_down.obstacle_filtering.pointcloud.pointcloud_max_cluster_size"); } }; @@ -147,40 +146,40 @@ struct SlowDownPlanningParam SlowDownPlanningParam() = default; explicit SlowDownPlanningParam(rclcpp::Node & node) { - slow_down_min_acc = getOrDeclareParameter( + slow_down_min_acc = get_or_declare_parameter( node, "obstacle_slow_down.slow_down_planning.slow_down_min_acc"); - slow_down_min_jerk = getOrDeclareParameter( + slow_down_min_jerk = get_or_declare_parameter( node, "obstacle_slow_down.slow_down_planning.slow_down_min_jerk"); - lpf_gain_slow_down_vel = getOrDeclareParameter( + lpf_gain_slow_down_vel = get_or_declare_parameter( node, "obstacle_slow_down.slow_down_planning.lpf_gain_slow_down_vel"); - lpf_gain_lat_dist = getOrDeclareParameter( + lpf_gain_lat_dist = get_or_declare_parameter( node, "obstacle_slow_down.slow_down_planning.lpf_gain_lat_dist"); - lpf_gain_dist_to_slow_down = getOrDeclareParameter( + lpf_gain_dist_to_slow_down = get_or_declare_parameter( node, "obstacle_slow_down.slow_down_planning.lpf_gain_dist_to_slow_down"); - time_margin_on_target_velocity = getOrDeclareParameter( + time_margin_on_target_velocity = get_or_declare_parameter( node, "obstacle_slow_down.slow_down_planning.time_margin_on_target_velocity"); - moving_object_speed_threshold = getOrDeclareParameter( + moving_object_speed_threshold = get_or_declare_parameter( node, "obstacle_slow_down.slow_down_planning.moving_object_speed_threshold"); - moving_object_hysteresis_range = getOrDeclareParameter( + moving_object_hysteresis_range = get_or_declare_parameter( node, "obstacle_slow_down.slow_down_planning.moving_object_hysteresis_range"); const std::string param_prefix = "obstacle_slow_down.slow_down_planning.object_type_specified_params."; const auto object_types = - getOrDeclareParameter>(node, param_prefix + "types"); + get_or_declare_parameter>(node, param_prefix + "types"); for (const auto & type_str : object_types) { for (const auto & movement_type : std::vector{"moving", "static"}) { ObjectTypeSpecificParams param{ - getOrDeclareParameter( + get_or_declare_parameter( node, param_prefix + type_str + "." + movement_type + ".min_lat_margin"), - getOrDeclareParameter( + get_or_declare_parameter( node, param_prefix + type_str + "." + movement_type + ".max_lat_margin"), - getOrDeclareParameter( + get_or_declare_parameter( node, param_prefix + type_str + "." + movement_type + ".min_ego_velocity"), - getOrDeclareParameter( + get_or_declare_parameter( node, param_prefix + type_str + "." + movement_type + ".max_ego_velocity")}; object_type_specific_param_map.emplace(type_str + "." + movement_type, param); diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_slow_down_module/src/type_alias.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_slow_down_module/src/type_alias.hpp index 673574c24a914..24468376c5ae7 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_slow_down_module/src/type_alias.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_slow_down_module/src/type_alias.hpp @@ -58,8 +58,8 @@ using tier4_planning_msgs::msg::VelocityLimitClearCommand; using visualization_msgs::msg::Marker; using visualization_msgs::msg::MarkerArray; namespace bg = boost::geometry; -using autoware::universe_utils::Point2d; -using autoware::universe_utils::Polygon2d; +using autoware_utils::Point2d; +using autoware_utils::Polygon2d; using Metric = tier4_metric_msgs::msg::Metric; using MetricArray = tier4_metric_msgs::msg::MetricArray; using PointCloud = pcl::PointCloud; diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_stop_module/package.xml b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_stop_module/package.xml index 069e85f45dabd..8f84d1a865b9f 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_stop_module/package.xml +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_stop_module/package.xml @@ -18,7 +18,7 @@ autoware_motion_velocity_planner_common_universe autoware_perception_msgs autoware_planning_msgs - autoware_universe_utils + autoware_utils autoware_vehicle_info_utils geometry_msgs libboost-dev diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_stop_module/src/obstacle_stop_module.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_stop_module/src/obstacle_stop_module.cpp index fc48761fa636c..f739bed1f1d69 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_stop_module/src/obstacle_stop_module.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_stop_module/src/obstacle_stop_module.cpp @@ -18,11 +18,11 @@ #include #include #include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include #include @@ -36,7 +36,7 @@ namespace autoware::motion_velocity_planner { -using autoware::universe_utils::getOrDeclareParameter; +using autoware_utils::get_or_declare_parameter; namespace { @@ -56,9 +56,9 @@ double calc_minimum_distance_to_stop( return -std::pow(initial_vel, 2) / 2.0 / min_acc; } -autoware::universe_utils::Point2d convert_point(const geometry_msgs::msg::Point & p) +autoware_utils::Point2d convert_point(const geometry_msgs::msg::Point & p) { - return autoware::universe_utils::Point2d{p.x, p.y}; + return autoware_utils::Point2d{p.x, p.y}; } std::vector resample_trajectory_points( @@ -142,15 +142,15 @@ void ObstacleStopModule::init(rclcpp::Node & node, const std::string & module_na // ros parameters ignore_crossing_obstacle_ = - getOrDeclareParameter(node, "obstacle_stop.option.ignore_crossing_obstacle"); + get_or_declare_parameter(node, "obstacle_stop.option.ignore_crossing_obstacle"); suppress_sudden_stop_ = - getOrDeclareParameter(node, "obstacle_stop.option.suppress_sudden_stop"); + get_or_declare_parameter(node, "obstacle_stop.option.suppress_sudden_stop"); common_param_ = CommonParam(node); stop_planning_param_ = StopPlanningParam(node, common_param_); obstacle_filtering_param_ = ObstacleFilteringParam(node); use_pointcloud_ = - getOrDeclareParameter(node, "obstacle_stop.obstacle_filtering.object_type.pointcloud"); + get_or_declare_parameter(node, "obstacle_stop.obstacle_filtering.object_type.pointcloud"); // common publisher processing_time_publisher_ = @@ -164,7 +164,7 @@ void ObstacleStopModule::init(rclcpp::Node & node, const std::string & module_na debug_stop_planning_info_pub_ = node.create_publisher("~/debug/stop_planning_info", 1); metrics_pub_ = node.create_publisher("~/stop/metrics", 10); - processing_time_detail_pub_ = node.create_publisher( + processing_time_detail_pub_ = node.create_publisher( "~/debug/processing_time_detail_ms/obstacle_stop", 1); // interface publisher objects_of_interest_marker_interface_ = std::make_unique< @@ -175,16 +175,16 @@ void ObstacleStopModule::init(rclcpp::Node & node, const std::string & module_na &node, "obstacle_stop"); // time keeper - time_keeper_ = std::make_shared(processing_time_detail_pub_); + time_keeper_ = std::make_shared(processing_time_detail_pub_); } void ObstacleStopModule::update_parameters(const std::vector & parameters) { - using universe_utils::updateParam; + using autoware_utils::update_param; - updateParam( + update_param( parameters, "obstacle_stop.option.ignore_crossing_obstacle", ignore_crossing_obstacle_); - updateParam(parameters, "obstacle_stop.option.suppress_sudden_stop", suppress_sudden_stop_); + update_param(parameters, "obstacle_stop.option.suppress_sudden_stop", suppress_sudden_stop_); } VelocityPlanningResult ObstacleStopModule::plan( @@ -193,7 +193,7 @@ VelocityPlanningResult ObstacleStopModule::plan( smoothed_trajectory_points, const std::shared_ptr planner_data) { - autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + autoware_utils::ScopedTimeTrack st(__func__, *time_keeper_); // 1. init variables stop_watch_.tic(); @@ -258,7 +258,7 @@ std::vector ObstacleStopModule::convert_point_cloud_t const PlannerData::Pointcloud & pointcloud, const std::vector & traj_points, const VehicleInfo & vehicle_info, size_t ego_idx) { - autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + autoware_utils::ScopedTimeTrack st(__func__, *time_keeper_); if (pointcloud.pointcloud.empty()) { return {}; @@ -345,7 +345,7 @@ std::optional ObstacleStopModule::create_stop_obstacle_for_point_c autoware::motion_utils::calcSignedArcLength(traj_points, 0, stop_point); const unique_identifier_msgs::msg::UUID obj_uuid; - const auto & obj_uuid_str = autoware::universe_utils::toHexString(obj_uuid); + const auto & obj_uuid_str = autoware_utils::to_hex_string(obj_uuid); autoware_perception_msgs::msg::Shape bounding_box_shape; bounding_box_shape.type = autoware_perception_msgs::msg::Shape::BOUNDING_BOX; @@ -374,13 +374,13 @@ std::vector ObstacleStopModule::filter_stop_obstacle_for_predicted const VehicleInfo & vehicle_info, const double dist_to_bumper, const TrajectoryPolygonCollisionCheck & trajectory_polygon_collision_check) { - autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + autoware_utils::ScopedTimeTrack st(__func__, *time_keeper_); const auto & current_pose = odometry.pose.pose; std::vector stop_obstacles; for (const auto & object : objects) { - autoware::universe_utils::ScopedTimeTrack st_for_each_object("for_each_object", *time_keeper_); + autoware_utils::ScopedTimeTrack st_for_each_object("for_each_object", *time_keeper_); // 1. rough filtering // 1.1. Check if the obstacle is in front of the ego. @@ -401,14 +401,14 @@ std::vector ObstacleStopModule::filter_stop_obstacle_for_predicted // 2. precise filtering const auto & decimated_traj_polys = [&]() { - autoware::universe_utils::ScopedTimeTrack st_get_decimated_traj_polys( + autoware_utils::ScopedTimeTrack st_get_decimated_traj_polys( "get_decimated_traj_polys", *time_keeper_); return get_decimated_traj_polys( traj_points, current_pose, vehicle_info, ego_nearest_dist_threshold, ego_nearest_yaw_threshold, trajectory_polygon_collision_check); }(); const double dist_from_obj_to_traj_poly = [&]() { - autoware::universe_utils::ScopedTimeTrack st_get_dist_to_traj_poly( + autoware_utils::ScopedTimeTrack st_get_dist_to_traj_poly( "get_dist_to_traj_poly", *time_keeper_); return object->get_dist_to_traj_poly(decimated_traj_polys); }(); @@ -446,7 +446,7 @@ std::vector ObstacleStopModule::filter_stop_obstacle_for_point_clo const PlannerData::Pointcloud & point_cloud, const VehicleInfo & vehicle_info, const TrajectoryPolygonCollisionCheck & trajectory_polygon_collision_check, size_t ego_idx) { - autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + autoware_utils::ScopedTimeTrack st(__func__, *time_keeper_); const auto & tp = trajectory_polygon_collision_check; @@ -517,7 +517,7 @@ std::optional ObstacleStopModule::filter_inside_stop_obstacle_for_ const double dist_to_bumper, const TrajectoryPolygonCollisionCheck & trajectory_polygon_collision_check) const { - autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + autoware_utils::ScopedTimeTrack st(__func__, *time_keeper_); const auto & predicted_object = object->predicted_object; const auto & obj_pose = object->get_predicted_pose(clock_->now(), predicted_objects_stamp); @@ -565,7 +565,7 @@ std::optional ObstacleStopModule::filter_inside_stop_obstacle_for_ } return StopObstacle{ - autoware::universe_utils::toHexString(predicted_object.object_id), + autoware_utils::to_hex_string(predicted_object.object_id), predicted_objects_stamp, predicted_object.classification.at(0), obj_pose, @@ -581,8 +581,7 @@ bool ObstacleStopModule::is_inside_stop_obstacle_velocity( { const bool is_prev_object_stop = utils::get_obstacle_from_uuid( - prev_stop_obstacles_, - autoware::universe_utils::toHexString(object->predicted_object.object_id)) + prev_stop_obstacles_, autoware_utils::to_hex_string(object->predicted_object.object_id)) .has_value(); if (is_prev_object_stop) { @@ -662,10 +661,9 @@ std::optional ObstacleStopModule::filter_outside_stop_obstacle_for const VehicleInfo & vehicle_info, const double dist_to_bumper, const TrajectoryPolygonCollisionCheck & trajectory_polygon_collision_check) const { - autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + autoware_utils::ScopedTimeTrack st(__func__, *time_keeper_); - const auto & object_id = - autoware::universe_utils::toHexString(object->predicted_object.object_id); + const auto & object_id = autoware_utils::to_hex_string(object->predicted_object.object_id); // 1. filter by label const uint8_t obj_label = object->predicted_object.classification.at(0).label; @@ -770,8 +768,7 @@ ObstacleStopModule::create_collision_point_for_outside_stop_obstacle( const bool is_driving_forward, const VehicleInfo & vehicle_info, const double dist_to_bumper, const double decimate_trajectory_step_length) const { - const auto & object_id = - autoware::universe_utils::toHexString(object->predicted_object.object_id); + const auto & object_id = autoware_utils::to_hex_string(object->predicted_object.object_id); std::vector collision_index; const auto collision_points = polygon_utils::get_collision_points( @@ -811,12 +808,12 @@ std::optional ObstacleStopModule::plan_stop( const std::vector & traj_points, const std::vector & stop_obstacles, const double dist_to_bumper) { - autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + autoware_utils::ScopedTimeTrack st(__func__, *time_keeper_); if (stop_obstacles.empty()) { const auto markers = autoware::motion_utils::createDeletedStopVirtualWallMarker(clock_->now(), 0); - autoware::universe_utils::appendMarkerArray(markers, &debug_data_ptr_->stop_wall_marker); + autoware_utils::append_marker_array(markers, &debug_data_ptr_->stop_wall_marker); prev_stop_distance_info_ = std::nullopt; return std::nullopt; @@ -867,7 +864,7 @@ std::optional ObstacleStopModule::plan_stop( // delete marker const auto markers = autoware::motion_utils::createDeletedStopVirtualWallMarker(clock_->now(), 0); - autoware::universe_utils::appendMarkerArray(markers, &debug_data_ptr_->stop_wall_marker); + autoware_utils::append_marker_array(markers, &debug_data_ptr_->stop_wall_marker); prev_stop_distance_info_ = std::nullopt; return std::nullopt; @@ -1016,7 +1013,7 @@ std::optional ObstacleStopModule::calc_stop_point( const auto markers = autoware::motion_utils::createStopVirtualWallMarker( output_traj_points.at(*zero_vel_idx).pose, "obstacle stop", clock_->now(), 0, dist_to_bumper, "", planner_data->is_driving_forward); - autoware::universe_utils::appendMarkerArray(markers, &debug_data_ptr_->stop_wall_marker); + autoware_utils::append_marker_array(markers, &debug_data_ptr_->stop_wall_marker); debug_data_ptr_->obstacles_to_stop.push_back(*determined_stop_obstacle); // update planning factor @@ -1053,7 +1050,7 @@ void ObstacleStopModule::set_stop_planning_debug_info( void ObstacleStopModule::publish_debug_info() { - autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + autoware_utils::ScopedTimeTrack st(__func__, *time_keeper_); // 1. debug marker MarkerArray debug_marker; @@ -1066,10 +1063,10 @@ void ObstacleStopModule::publish_debug_info() debug_marker.markers.push_back(obstacle_marker); // collision point - auto collision_point_marker = autoware::universe_utils::createDefaultMarker( + auto collision_point_marker = autoware_utils::create_default_marker( "map", clock_->now(), "collision_points", 0, Marker::SPHERE, - autoware::universe_utils::createMarkerScale(0.25, 0.25, 0.25), - autoware::universe_utils::createMarkerColor(1.0, 0.0, 0.0, 0.999)); + autoware_utils::create_marker_scale(0.25, 0.25, 0.25), + autoware_utils::create_marker_color(1.0, 0.0, 0.0, 0.999)); collision_point_marker.pose.position = debug_data_ptr_->obstacles_to_stop.at(i).collision_point; debug_marker.markers.push_back(collision_point_marker); } @@ -1084,10 +1081,10 @@ void ObstacleStopModule::publish_debug_info() } // 1.3. detection area - auto decimated_traj_polys_marker = autoware::universe_utils::createDefaultMarker( + auto decimated_traj_polys_marker = autoware_utils::create_default_marker( "map", clock_->now(), "detection_area", 0, Marker::LINE_LIST, - autoware::universe_utils::createMarkerScale(0.01, 0.0, 0.0), - autoware::universe_utils::createMarkerColor(0.0, 1.0, 0.0, 0.999)); + autoware_utils::create_marker_scale(0.01, 0.0, 0.0), + autoware_utils::create_marker_color(0.0, 1.0, 0.0, 0.999)); for (const auto & decimated_traj_poly : debug_data_ptr_->decimated_traj_polys) { for (size_t dp_idx = 0; dp_idx < decimated_traj_poly.outer().size(); ++dp_idx) { const auto & current_point = decimated_traj_poly.outer().at(dp_idx); @@ -1095,9 +1092,9 @@ void ObstacleStopModule::publish_debug_info() decimated_traj_poly.outer().at((dp_idx + 1) % decimated_traj_poly.outer().size()); decimated_traj_polys_marker.points.push_back( - autoware::universe_utils::createPoint(current_point.x(), current_point.y(), 0.0)); + autoware_utils::create_point(current_point.x(), current_point.y(), 0.0)); decimated_traj_polys_marker.points.push_back( - autoware::universe_utils::createPoint(next_point.x(), next_point.y(), 0.0)); + autoware_utils::create_point(next_point.x(), next_point.y(), 0.0)); } } debug_marker.markers.push_back(decimated_traj_polys_marker); @@ -1188,13 +1185,13 @@ void ObstacleStopModule::check_consistency( const std::vector> & objects, std::vector & stop_obstacles) { - autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + autoware_utils::ScopedTimeTrack st(__func__, *time_keeper_); for (const auto & prev_closest_stop_obstacle : prev_closest_stop_obstacles_) { const auto object_itr = std::find_if( objects.begin(), objects.end(), [&prev_closest_stop_obstacle](const std::shared_ptr & o) { - return autoware::universe_utils::toHexString(o->predicted_object.object_id) == + return autoware_utils::to_hex_string(o->predicted_object.object_id) == prev_closest_stop_obstacle.uuid; }); // If previous closest obstacle disappear from the perception result, do nothing anymore. @@ -1247,7 +1244,7 @@ double ObstacleStopModule::calc_margin_from_obstacle_on_curve( break; } sum_short_traj_length += - autoware::universe_utils::calcDistance2d(traj_points.at(i), traj_points.at(i + 1)); + autoware_utils::calc_distance2d(traj_points.at(i), traj_points.at(i + 1)); } std::reverse(short_traj_points.begin(), short_traj_points.end()); if (short_traj_points.size() < 2) { @@ -1257,15 +1254,14 @@ double ObstacleStopModule::calc_margin_from_obstacle_on_curve( // calculate collision index between straight line from ego pose and object const auto calculate_distance_from_straight_ego_path = [&](const auto & ego_pose, const auto & object_polygon) { - const auto forward_ego_pose = autoware::universe_utils::calcOffsetPose( + const auto forward_ego_pose = autoware_utils::calc_offset_pose( ego_pose, stop_planning_param_.stop_margin + 3.0, 0.0, 0.0); - const auto ego_straight_segment = autoware::universe_utils::Segment2d{ + const auto ego_straight_segment = autoware_utils::Segment2d{ convert_point(ego_pose.position), convert_point(forward_ego_pose.position)}; return boost::geometry::distance(ego_straight_segment, object_polygon); }; const auto resampled_short_traj_points = resample_trajectory_points(short_traj_points, 0.5); - const auto object_polygon = - autoware::universe_utils::toPolygon2d(stop_obstacle.pose, stop_obstacle.shape); + const auto object_polygon = autoware_utils::to_polygon2d(stop_obstacle.pose, stop_obstacle.shape); const auto collision_idx = [&]() -> std::optional { for (size_t i = 0; i < resampled_short_traj_points.size(); ++i) { const double dist_to_obj = calculate_distance_from_straight_ego_path( @@ -1285,7 +1281,7 @@ double ObstacleStopModule::calc_margin_from_obstacle_on_curve( // calculate margin from obstacle const double partial_segment_length = [&]() { - const double collision_segment_length = autoware::universe_utils::calcDistance2d( + const double collision_segment_length = autoware_utils::calc_distance2d( resampled_short_traj_points.at(*collision_idx - 1), resampled_short_traj_points.at(*collision_idx)); const double prev_dist = calculate_distance_from_straight_ego_path( diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_stop_module/src/obstacle_stop_module.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_stop_module/src/obstacle_stop_module.hpp index 552c2152e0fca..10a29edd07990 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_stop_module/src/obstacle_stop_module.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_stop_module/src/obstacle_stop_module.hpp @@ -18,8 +18,8 @@ #include "autoware/motion_velocity_planner_common_universe/polygon_utils.hpp" #include "autoware/motion_velocity_planner_common_universe/utils.hpp" #include "autoware/object_recognition_utils/predicted_path_utils.hpp" -#include "autoware/universe_utils/system/stop_watch.hpp" -#include "autoware/universe_utils/system/time_keeper.hpp" +#include "autoware_utils/system/stop_watch.hpp" +#include "autoware_utils/system/time_keeper.hpp" #include "metrics_manager.hpp" #include "parameters.hpp" #include "stop_planning_debug_info.hpp" @@ -76,8 +76,7 @@ class ObstacleStopModule : public PluginModuleInterface // module publisher rclcpp::Publisher::SharedPtr debug_stop_planning_info_pub_{}; rclcpp::Publisher::SharedPtr metrics_pub_{}; - rclcpp::Publisher::SharedPtr - processing_time_detail_pub_{}; + rclcpp::Publisher::SharedPtr processing_time_detail_pub_{}; // interface publisher std::unique_ptr @@ -98,11 +97,11 @@ class ObstacleStopModule : public PluginModuleInterface // crossing lanes. std::optional, double>> prev_stop_distance_info_{ std::nullopt}; - autoware::universe_utils::StopWatch stop_watch_{}; + autoware_utils::StopWatch stop_watch_{}; mutable std::unordered_map> trajectory_polygon_for_inside_map_{}; mutable std::optional> trajectory_polygon_for_outside_{std::nullopt}; mutable std::optional> decimated_traj_polys_{std::nullopt}; - mutable std::shared_ptr time_keeper_{}; + mutable std::shared_ptr time_keeper_{}; std::vector convert_point_cloud_to_stop_points( const PlannerData::Pointcloud & pointcloud, const std::vector & traj_points, diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_stop_module/src/parameters.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_stop_module/src/parameters.hpp index bc97bf6e957b9..ce910287bb77a 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_stop_module/src/parameters.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_stop_module/src/parameters.hpp @@ -20,9 +20,9 @@ #include "autoware/motion_utils/trajectory/trajectory.hpp" #include "autoware/motion_velocity_planner_common_universe/utils.hpp" #include "autoware/object_recognition_utils/predicted_path_utils.hpp" -#include "autoware/universe_utils/ros/parameter.hpp" -#include "autoware/universe_utils/ros/update_param.hpp" -#include "autoware/universe_utils/system/stop_watch.hpp" +#include "autoware_utils/ros/parameter.hpp" +#include "autoware_utils/ros/update_param.hpp" +#include "autoware_utils/system/stop_watch.hpp" #include "type_alias.hpp" #include "types.hpp" @@ -37,7 +37,7 @@ namespace autoware::motion_velocity_planner { -using autoware::universe_utils::getOrDeclareParameter; +using autoware_utils::get_or_declare_parameter; struct CommonParam { @@ -53,14 +53,14 @@ struct CommonParam CommonParam() = default; explicit CommonParam(rclcpp::Node & node) { - max_accel = getOrDeclareParameter(node, "normal.max_acc"); - min_accel = getOrDeclareParameter(node, "normal.min_acc"); - max_jerk = getOrDeclareParameter(node, "normal.max_jerk"); - min_jerk = getOrDeclareParameter(node, "normal.min_jerk"); - limit_max_accel = getOrDeclareParameter(node, "limit.max_acc"); - limit_min_accel = getOrDeclareParameter(node, "limit.min_acc"); - limit_max_jerk = getOrDeclareParameter(node, "limit.max_jerk"); - limit_min_jerk = getOrDeclareParameter(node, "limit.min_jerk"); + max_accel = get_or_declare_parameter(node, "normal.max_acc"); + min_accel = get_or_declare_parameter(node, "normal.min_acc"); + max_jerk = get_or_declare_parameter(node, "normal.max_jerk"); + min_jerk = get_or_declare_parameter(node, "normal.min_jerk"); + limit_max_accel = get_or_declare_parameter(node, "limit.max_acc"); + limit_min_accel = get_or_declare_parameter(node, "limit.min_acc"); + limit_max_jerk = get_or_declare_parameter(node, "limit.max_jerk"); + limit_min_jerk = get_or_declare_parameter(node, "limit.min_jerk"); } }; @@ -101,51 +101,53 @@ struct ObstacleFilteringParam ObstacleFilteringParam() = default; explicit ObstacleFilteringParam(rclcpp::Node & node) { - pointcloud_obstacle_filtering_param.pointcloud_voxel_grid_x = getOrDeclareParameter( + pointcloud_obstacle_filtering_param.pointcloud_voxel_grid_x = get_or_declare_parameter( node, "obstacle_stop.obstacle_filtering.pointcloud.pointcloud_voxel_grid_x"); - pointcloud_obstacle_filtering_param.pointcloud_voxel_grid_y = getOrDeclareParameter( + pointcloud_obstacle_filtering_param.pointcloud_voxel_grid_y = get_or_declare_parameter( node, "obstacle_stop.obstacle_filtering.pointcloud.pointcloud_voxel_grid_y"); - pointcloud_obstacle_filtering_param.pointcloud_voxel_grid_z = getOrDeclareParameter( + pointcloud_obstacle_filtering_param.pointcloud_voxel_grid_z = get_or_declare_parameter( node, "obstacle_stop.obstacle_filtering.pointcloud.pointcloud_voxel_grid_z"); pointcloud_obstacle_filtering_param.pointcloud_cluster_tolerance = - getOrDeclareParameter( + get_or_declare_parameter( node, "obstacle_stop.obstacle_filtering.pointcloud.pointcloud_cluster_tolerance"); - pointcloud_obstacle_filtering_param.pointcloud_min_cluster_size = getOrDeclareParameter( + pointcloud_obstacle_filtering_param.pointcloud_min_cluster_size = get_or_declare_parameter( node, "obstacle_stop.obstacle_filtering.pointcloud.pointcloud_min_cluster_size"); - pointcloud_obstacle_filtering_param.pointcloud_max_cluster_size = getOrDeclareParameter( + pointcloud_obstacle_filtering_param.pointcloud_max_cluster_size = get_or_declare_parameter( node, "obstacle_stop.obstacle_filtering.pointcloud.pointcloud_max_cluster_size"); - use_pointcloud = - getOrDeclareParameter(node, "obstacle_stop.obstacle_filtering.object_type.pointcloud"); + use_pointcloud = get_or_declare_parameter( + node, "obstacle_stop.obstacle_filtering.object_type.pointcloud"); inside_stop_object_types = utils::get_target_object_type(node, "obstacle_stop.obstacle_filtering.object_type.inside."); outside_stop_object_types = utils::get_target_object_type(node, "obstacle_stop.obstacle_filtering.object_type.outside."); + use_pointcloud = get_or_declare_parameter( + node, "obstacle_stop.obstacle_filtering.object_type.pointcloud"); - obstacle_velocity_threshold_to_stop = getOrDeclareParameter( + obstacle_velocity_threshold_to_stop = get_or_declare_parameter( node, "obstacle_stop.obstacle_filtering.obstacle_velocity_threshold_to_stop"); - obstacle_velocity_threshold_from_stop = getOrDeclareParameter( + obstacle_velocity_threshold_from_stop = get_or_declare_parameter( node, "obstacle_stop.obstacle_filtering.obstacle_velocity_threshold_from_stop"); max_lat_margin = - getOrDeclareParameter(node, "obstacle_stop.obstacle_filtering.max_lat_margin"); - max_lat_margin_against_unknown = getOrDeclareParameter( + get_or_declare_parameter(node, "obstacle_stop.obstacle_filtering.max_lat_margin"); + max_lat_margin_against_unknown = get_or_declare_parameter( node, "obstacle_stop.obstacle_filtering.max_lat_margin_against_unknown"); - min_velocity_to_reach_collision_point = getOrDeclareParameter( + min_velocity_to_reach_collision_point = get_or_declare_parameter( node, "obstacle_stop.obstacle_filtering.min_velocity_to_reach_collision_point"); - stop_obstacle_hold_time_threshold = getOrDeclareParameter( + stop_obstacle_hold_time_threshold = get_or_declare_parameter( node, "obstacle_stop.obstacle_filtering.stop_obstacle_hold_time_threshold"); - outside_max_lat_time_margin = getOrDeclareParameter( + outside_max_lat_time_margin = get_or_declare_parameter( node, "obstacle_stop.obstacle_filtering.outside_obstacle.max_lateral_time_margin"); - outside_num_of_predicted_paths = getOrDeclareParameter( + outside_num_of_predicted_paths = get_or_declare_parameter( node, "obstacle_stop.obstacle_filtering.outside_obstacle.num_of_predicted_paths"); - outside_pedestrian_deceleration_rate = getOrDeclareParameter( + outside_pedestrian_deceleration_rate = get_or_declare_parameter( node, "obstacle_stop.obstacle_filtering.outside_obstacle.pedestrian_deceleration_rate"); - outside_bicycle_deceleration_rate = getOrDeclareParameter( + outside_bicycle_deceleration_rate = get_or_declare_parameter( node, "obstacle_stop.obstacle_filtering.outside_obstacle.bicycle_deceleration_rate"); - crossing_obstacle_collision_time_margin = getOrDeclareParameter( + crossing_obstacle_collision_time_margin = get_or_declare_parameter( node, "obstacle_stop.obstacle_filtering.crossing_obstacle.collision_time_margin"); } }; @@ -178,35 +180,35 @@ struct StopPlanningParam StopPlanningParam() = default; StopPlanningParam(rclcpp::Node & node, const CommonParam & common_param) { - stop_margin = getOrDeclareParameter(node, "obstacle_stop.stop_planning.stop_margin"); + stop_margin = get_or_declare_parameter(node, "obstacle_stop.stop_planning.stop_margin"); terminal_stop_margin = - getOrDeclareParameter(node, "obstacle_stop.stop_planning.terminal_stop_margin"); - min_behavior_stop_margin = - getOrDeclareParameter(node, "obstacle_stop.stop_planning.min_behavior_stop_margin"); - hold_stop_velocity_threshold = getOrDeclareParameter( + get_or_declare_parameter(node, "obstacle_stop.stop_planning.terminal_stop_margin"); + min_behavior_stop_margin = get_or_declare_parameter( + node, "obstacle_stop.stop_planning.min_behavior_stop_margin"); + hold_stop_velocity_threshold = get_or_declare_parameter( node, "obstacle_stop.stop_planning.hold_stop_velocity_threshold"); - hold_stop_distance_threshold = getOrDeclareParameter( + hold_stop_distance_threshold = get_or_declare_parameter( node, "obstacle_stop.stop_planning.hold_stop_distance_threshold"); - enable_approaching_on_curve = getOrDeclareParameter( + enable_approaching_on_curve = get_or_declare_parameter( node, "obstacle_stop.stop_planning.stop_on_curve.enable_approaching"); - additional_stop_margin_on_curve = getOrDeclareParameter( + additional_stop_margin_on_curve = get_or_declare_parameter( node, "obstacle_stop.stop_planning.stop_on_curve.additional_stop_margin"); - min_stop_margin_on_curve = getOrDeclareParameter( + min_stop_margin_on_curve = get_or_declare_parameter( node, "obstacle_stop.stop_planning.stop_on_curve.min_stop_margin"); const std::string param_prefix = "obstacle_stop.stop_planning.object_type_specified_params."; const auto object_types = - getOrDeclareParameter>(node, param_prefix + "types"); + get_or_declare_parameter>(node, param_prefix + "types"); for (const auto & type_str : object_types) { if (type_str != "default") { ObjectTypeSpecificParams param{ - getOrDeclareParameter(node, param_prefix + type_str + ".limit_min_acc"), - getOrDeclareParameter( + get_or_declare_parameter(node, param_prefix + type_str + ".limit_min_acc"), + get_or_declare_parameter( node, param_prefix + type_str + ".sudden_object_acc_threshold"), - getOrDeclareParameter( + get_or_declare_parameter( node, param_prefix + type_str + ".sudden_object_dist_threshold"), - getOrDeclareParameter(node, param_prefix + type_str + ".abandon_to_stop")}; + get_or_declare_parameter(node, param_prefix + type_str + ".abandon_to_stop")}; param.sudden_object_acc_threshold = std::min(param.sudden_object_acc_threshold, common_param.limit_min_accel); diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_stop_module/src/type_alias.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_stop_module/src/type_alias.hpp index b3c278fcafa50..11ebdf1139c63 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_stop_module/src/type_alias.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_stop_module/src/type_alias.hpp @@ -52,8 +52,8 @@ using sensor_msgs::msg::PointCloud2; using visualization_msgs::msg::Marker; using visualization_msgs::msg::MarkerArray; namespace bg = boost::geometry; -using autoware::universe_utils::Point2d; -using autoware::universe_utils::Polygon2d; +using autoware_utils::Point2d; +using autoware_utils::Polygon2d; using Metric = tier4_metric_msgs::msg::Metric; using MetricArray = tier4_metric_msgs::msg::MetricArray; using PointCloud = pcl::PointCloud; diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/package.xml b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/package.xml index 3b19c27807a81..ffabd5cf1f689 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/package.xml +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/package.xml @@ -19,7 +19,7 @@ autoware_perception_msgs autoware_planning_msgs autoware_planning_test_manager - autoware_universe_utils + autoware_utils autoware_vehicle_info_utils eigen grid_map_msgs diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacle_velocity_limiter.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacle_velocity_limiter.cpp index 1e63713a61a4d..f37118a9e1969 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacle_velocity_limiter.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacle_velocity_limiter.cpp @@ -17,7 +17,7 @@ #include "distance.hpp" #include "forward_projection.hpp" -#include +#include #include #include diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacle_velocity_limiter_module.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacle_velocity_limiter_module.cpp index baa98ea1f79b8..8fde9969e0e20 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacle_velocity_limiter_module.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacle_velocity_limiter_module.cpp @@ -22,9 +22,9 @@ #include #include -#include -#include -#include +#include +#include +#include #include #include #include @@ -53,7 +53,7 @@ void ObstacleVelocityLimiterModule::init(rclcpp::Node & node, const std::string node.create_publisher("~/" + ns_ + "/debug_markers", 1); virtual_wall_publisher_ = node.create_publisher("~/" + ns_ + "/virtual_walls", 1); - processing_diag_publisher_ = std::make_shared( + processing_diag_publisher_ = std::make_shared( &node, "~/debug/" + ns_ + "/processing_time_ms_diag"); processing_time_publisher_ = node.create_publisher( @@ -140,7 +140,7 @@ VelocityPlanningResult ObstacleVelocityLimiterModule::plan( const std::vector & smoothed_trajectory_points, const std::shared_ptr planner_data) { - autoware::universe_utils::StopWatch stopwatch; + autoware_utils::StopWatch stopwatch; stopwatch.tic(); VelocityPlanningResult result; stopwatch.tic("preprocessing"); diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacle_velocity_limiter_module.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacle_velocity_limiter_module.hpp index f89a8d43e9e93..ea0aabe78eb33 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacle_velocity_limiter_module.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacle_velocity_limiter_module.hpp @@ -19,9 +19,9 @@ #include #include -#include -#include -#include +#include +#include +#include #include #include diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacles.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacles.hpp index ddcb1161b6f59..3d113fa8d3856 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacles.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacles.hpp @@ -19,7 +19,7 @@ #include "types.hpp" #include -#include +#include #include #include diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/pointcloud_utils.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/pointcloud_utils.hpp index eda2e2624ca4a..215cd6f119951 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/pointcloud_utils.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/pointcloud_utils.hpp @@ -15,7 +15,7 @@ #ifndef POINTCLOUD_UTILS_HPP_ #define POINTCLOUD_UTILS_HPP_ -#include "autoware/universe_utils/ros/transform_listener.hpp" +#include "autoware_utils/ros/transform_listener.hpp" #include "obstacles.hpp" #include "types.hpp" diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/trajectory_preprocessing.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/trajectory_preprocessing.cpp index c064f2c717625..bf18f90587c37 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/trajectory_preprocessing.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/trajectory_preprocessing.cpp @@ -17,7 +17,7 @@ #include "types.hpp" #include -#include +#include #include #include @@ -35,7 +35,7 @@ size_t calculateStartIndex( auto dist = 0.0; auto idx = ego_idx; while (idx + 1 < trajectory.size() && dist < start_distance) { - dist += autoware::universe_utils::calcDistance2d(trajectory[idx], trajectory[idx + 1]); + dist += autoware_utils::calc_distance2d(trajectory[idx], trajectory[idx + 1]); ++idx; } return idx; @@ -49,8 +49,7 @@ size_t calculateEndIndex( auto duration = 0.0; auto idx = start_idx; while (idx + 1 < trajectory.size() && length < max_length && duration < max_duration) { - const auto length_d = - autoware::universe_utils::calcDistance2d(trajectory[idx], trajectory[idx + 1]); + const auto length_d = autoware_utils::calc_distance2d(trajectory[idx], trajectory[idx + 1]); length += length_d; if (trajectory[idx].longitudinal_velocity_mps > 0.0) duration += length_d / trajectory[idx].longitudinal_velocity_mps; @@ -77,8 +76,8 @@ void calculateSteeringAngles(TrajectoryPoints & trajectory, const double wheel_b for (auto i = 1ul; i < trajectory.size(); ++i) { prev_point = trajectory[i - 1]; auto & point = trajectory[i]; - const auto dt = autoware::universe_utils::calcDistance2d(prev_point, point) / - prev_point.longitudinal_velocity_mps; + const auto dt = + autoware_utils::calc_distance2d(prev_point, point) / prev_point.longitudinal_velocity_mps; const auto heading = tf2::getYaw(point.pose.orientation); const auto d_heading = heading - prev_heading; prev_heading = heading; diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/types.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/types.hpp index bd707402ce169..2793ac90b948b 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/types.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/types.hpp @@ -15,7 +15,7 @@ #ifndef TYPES_HPP_ #define TYPES_HPP_ -#include "autoware/universe_utils/geometry/boost_geometry.hpp" +#include "autoware_utils/geometry/boost_geometry.hpp" #include #include @@ -35,13 +35,13 @@ using nav_msgs::msg::OccupancyGrid; using PointCloud = pcl::PointCloud; using TrajectoryPoints = std::vector; -using point_t = autoware::universe_utils::Point2d; -using multipoint_t = autoware::universe_utils::MultiPoint2d; -using polygon_t = autoware::universe_utils::Polygon2d; -using multi_polygon_t = autoware::universe_utils::MultiPolygon2d; -using segment_t = autoware::universe_utils::Segment2d; -using linestring_t = autoware::universe_utils::LineString2d; -using multi_linestring_t = autoware::universe_utils::MultiLineString2d; +using point_t = autoware_utils::Point2d; +using multipoint_t = autoware_utils::MultiPoint2d; +using polygon_t = autoware_utils::Polygon2d; +using multi_polygon_t = autoware_utils::MultiPolygon2d; +using segment_t = autoware_utils::Segment2d; +using linestring_t = autoware_utils::LineString2d; +using multi_linestring_t = autoware_utils::MultiLineString2d; struct ObstacleMasks { diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/test/test_collision_distance.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/test/test_collision_distance.cpp index a538c7f6610a0..f37df3d8a998a 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/test/test_collision_distance.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/test/test_collision_distance.cpp @@ -16,7 +16,7 @@ #include "../src/obstacles.hpp" #include "../src/types.hpp" #include "autoware/motion_velocity_planner_common_universe/planner_data.hpp" -#include "autoware/universe_utils/geometry/geometry.hpp" +#include "autoware_utils/geometry/geometry.hpp" #include #include @@ -403,7 +403,7 @@ TEST(TestCollisionDistance, createObjPolygons) object1.kinematics.initial_pose_with_covariance.pose.position.x = 0.0; object1.kinematics.initial_pose_with_covariance.pose.position.y = 0.0; object1.kinematics.initial_pose_with_covariance.pose.orientation = - autoware::universe_utils::createQuaternionFromYaw(0.0); + autoware_utils::create_quaternion_from_yaw(0.0); object1.kinematics.initial_twist_with_covariance.twist.linear.x = 0.0; object1.shape.dimensions.x = 1.0; object1.shape.dimensions.y = 1.0; @@ -430,7 +430,7 @@ TEST(TestCollisionDistance, createObjPolygons) object2.kinematics.initial_pose_with_covariance.pose.position.x = 10.0; object2.kinematics.initial_pose_with_covariance.pose.position.y = 10.0; object2.kinematics.initial_pose_with_covariance.pose.orientation = - autoware::universe_utils::createQuaternionFromYaw(M_PI_2); + autoware_utils::create_quaternion_from_yaw(M_PI_2); object2.kinematics.initial_twist_with_covariance.twist.linear.x = 2.0; object2.shape.dimensions.x = 2.0; object2.shape.dimensions.y = 1.0; diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/test/test_forward_projection.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/test/test_forward_projection.cpp index d7b8e128b3cee..2ed4465b7ee16 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/test/test_forward_projection.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/test/test_forward_projection.cpp @@ -14,7 +14,7 @@ #include "../src/forward_projection.hpp" #include "../src/types.hpp" -#include "autoware/universe_utils/geometry/geometry.hpp" +#include "autoware_utils/geometry/geometry.hpp" #include diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/package.xml b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/package.xml index d9d2209edc6ef..133e273805ae6 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/package.xml +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/package.xml @@ -22,7 +22,7 @@ autoware_planning_msgs autoware_route_handler autoware_traffic_light_utils - autoware_universe_utils + autoware_utils autoware_vehicle_info_utils geometry_msgs libboost-dev diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/calculate_slowdown_points.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/calculate_slowdown_points.cpp index 352e5767d6baa..f437ccb6a1a98 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/calculate_slowdown_points.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/calculate_slowdown_points.cpp @@ -19,7 +19,7 @@ #include #include -#include +#include #include #include @@ -38,7 +38,7 @@ namespace autoware::motion_velocity_planner::out_of_lane std::optional calculate_last_avoiding_pose( const std::vector & trajectory, - const universe_utils::Polygon2d & footprint, const lanelet::BasicPolygons2d & polygons_to_avoid, + const autoware_utils::Polygon2d & footprint, const lanelet::BasicPolygons2d & polygons_to_avoid, const double min_arc_length, const double max_arc_length, const double precision) { geometry_msgs::msg::Pose interpolated_pose{}; @@ -68,7 +68,7 @@ std::optional calculate_last_avoiding_pose( std::optional calculate_pose_ahead_of_collision( const EgoData & ego_data, const OutOfLanePoint & point_to_avoid, - const universe_utils::Polygon2d & footprint, const double precision) + const autoware_utils::Polygon2d & footprint, const double precision) { const auto first_avoid_arc_length = motion_utils::calcSignedArcLength( ego_data.trajectory_points, 0UL, point_to_avoid.trajectory_index); diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/calculate_slowdown_points.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/calculate_slowdown_points.hpp index fccde78e1a9f2..d85ca8b202721 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/calculate_slowdown_points.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/calculate_slowdown_points.hpp @@ -18,7 +18,7 @@ #include "types.hpp" #include -#include +#include #include @@ -34,7 +34,7 @@ namespace autoware::motion_velocity_planner::out_of_lane /// @param [in] precision [m] search precision /// @return the last pose that is not out of lane (if found) std::optional calculate_last_in_lane_pose( - const EgoData & ego_data, const autoware::universe_utils::Polygon2d & footprint, + const EgoData & ego_data, const autoware_utils::Polygon2d & footprint, const double min_arc_length, const double max_arc_length, const double precision); /// @brief calculate the slowdown pose just ahead of a point to avoid @@ -45,7 +45,7 @@ std::optional calculate_last_in_lane_pose( /// @return optional slowdown point to insert in the trajectory std::optional calculate_pose_ahead_of_collision( const EgoData & ego_data, const OutOfLanePoint & point_to_avoid, - const universe_utils::Polygon2d & footprint, const double precision); + const autoware_utils::Polygon2d & footprint, const double precision); /// @brief calculate the slowdown point to insert in the trajectory /// @param ego_data ego data (trajectory, velocity, etc) diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/debug.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/debug.cpp index 7d845eb788ef8..568ed65d18dbd 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/debug.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/debug.cpp @@ -17,9 +17,9 @@ #include "types.hpp" #include -#include -#include -#include +#include +#include +#include #include #include @@ -52,10 +52,10 @@ visualization_msgs::msg::Marker get_base_marker() base_marker.id = 0; base_marker.type = visualization_msgs::msg::Marker::LINE_STRIP; base_marker.action = visualization_msgs::msg::Marker::ADD; - base_marker.pose.position = universe_utils::createMarkerPosition(0.0, 0.0, 0); - base_marker.pose.orientation = universe_utils::createMarkerOrientation(0, 0, 0, 1.0); - base_marker.scale = universe_utils::createMarkerScale(0.1, 0.1, 0.1); - base_marker.color = universe_utils::createMarkerColor(1.0, 0.1, 0.1, 0.5); + base_marker.pose.position = autoware_utils::create_marker_position(0.0, 0.0, 0); + base_marker.pose.orientation = autoware_utils::create_marker_orientation(0, 0, 0, 1.0); + base_marker.scale = autoware_utils::create_marker_scale(0.1, 0.1, 0.1); + base_marker.color = autoware_utils::create_marker_color(1.0, 0.1, 0.1, 0.5); return base_marker; } void add_polygons_markers( @@ -67,8 +67,8 @@ void add_polygons_markers( for (const auto & f : polygons) { boost::geometry::for_each_segment(f, [&](const auto & s) { const auto & [p1, p2] = s; - debug_marker.points.push_back(universe_utils::createMarkerPosition(p1.x(), p1.y(), 0.0)); - debug_marker.points.push_back(universe_utils::createMarkerPosition(p2.x(), p2.y(), 0.0)); + debug_marker.points.push_back(autoware_utils::create_marker_position(p1.x(), p1.y(), 0.0)); + debug_marker.points.push_back(autoware_utils::create_marker_position(p2.x(), p2.y(), 0.0)); }); } debug_marker_array.markers.push_back(debug_marker); @@ -82,9 +82,9 @@ void add_current_overlap_marker( debug_marker.ns = "current_overlap"; debug_marker.points.clear(); for (const auto & p : current_footprint) - debug_marker.points.push_back(universe_utils::createMarkerPosition(p.x(), p.y(), z)); + debug_marker.points.push_back(autoware_utils::create_marker_position(p.x(), p.y(), z)); if (!debug_marker.points.empty()) debug_marker.points.push_back(debug_marker.points.front()); - debug_marker.color = universe_utils::createMarkerColor(1.0, 0.1, 0.1, 0.5); + debug_marker.color = autoware_utils::create_marker_color(1.0, 0.1, 0.1, 0.5); debug_marker_array.markers.push_back(debug_marker); debug_marker.id++; } @@ -128,7 +128,8 @@ size_t add_stop_line_markers( for (const auto & ll : lanelets) { debug_marker.points.clear(); for (const auto & p : ll.polygon2d().basicPolygon()) { - debug_marker.points.push_back(universe_utils::createMarkerPosition(p.x(), p.y(), z + 0.5)); + debug_marker.points.push_back( + autoware_utils::create_marker_position(p.x(), p.y(), z + 0.5)); } debug_marker.points.push_back(debug_marker.points.front()); debug_marker_array.markers.push_back(debug_marker); @@ -139,7 +140,7 @@ size_t add_stop_line_markers( debug_marker.points.clear(); debug_marker.color.r = 1.0; for (const auto & p : stop_line.stop_line) { - debug_marker.points.push_back(universe_utils::createMarkerPosition(p.x(), p.y(), z + 0.5)); + debug_marker.points.push_back(autoware_utils::create_marker_position(p.x(), p.y(), z + 0.5)); } debug_marker_array.markers.push_back(debug_marker); ++debug_marker.id; @@ -163,7 +164,7 @@ void add_out_lanelets( drivable_lane_polygons.push_back(ll.polygon2d().basicPolygon()); } base_marker.ns = "out_lanelets"; - base_marker.color = universe_utils::createMarkerColor(0.0, 0.0, 1.0, 1.0); + base_marker.color = autoware_utils::create_marker_color(0.0, 0.0, 1.0, 1.0); add_polygons_markers(marker_array, base_marker, drivable_lane_polygons); } @@ -181,7 +182,7 @@ void add_out_of_lane_overlaps( } } base_marker.ns = "out_of_lane_areas"; - base_marker.color = universe_utils::createMarkerColor(1.0, 0.0, 0.0, 1.0); + base_marker.color = autoware_utils::create_marker_color(1.0, 0.0, 0.0, 1.0); add_polygons_markers(marker_array, base_marker, out_of_lane_overlaps); for (const auto & p : outside_points) { for (const auto & a : p.out_overlaps) { @@ -198,15 +199,15 @@ void add_predicted_paths( const geometry_msgs::msg::Pose & ego_pose) { base_marker.ns = "objects"; - base_marker.color = universe_utils::createMarkerColor(0.0, 1.0, 0.0, 1.0); + base_marker.color = autoware_utils::create_marker_color(0.0, 1.0, 0.0, 1.0); lanelet::BasicPolygons2d object_polygons; constexpr double max_draw_distance = 50.0; for (const auto & o : objects.objects) { for (const auto & path : o.kinematics.predicted_paths) { for (const auto & pose : path.path) { // limit the draw distance to improve performance - if (universe_utils::calcDistance2d(pose, ego_pose) < max_draw_distance) { - const auto poly = universe_utils::toPolygon2d(pose, o.shape).outer(); + if (autoware_utils::calc_distance2d(pose, ego_pose) < max_draw_distance) { + const auto poly = autoware_utils::to_polygon2d(pose, o.shape).outer(); lanelet::BasicPolygon2d ll_poly(poly.begin(), poly.end()); object_polygons.push_back(ll_poly); } @@ -227,7 +228,7 @@ visualization_msgs::msg::MarkerArray create_debug_marker_array( auto base_marker = get_base_marker(); base_marker.pose.position.z = z + 0.5; base_marker.ns = "footprints"; - base_marker.color = universe_utils::createMarkerColor(1.0, 1.0, 1.0, 1.0); + base_marker.color = autoware_utils::create_marker_color(1.0, 1.0, 1.0, 1.0); // TODO(Maxime): move the debug marker publishing AFTER the trajectory generation // disabled to prevent performance issues when publishing the debug markers // add_polygons_markers(debug_marker_array, base_marker, ego_data.trajectory_footprints); diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/filter_predicted_objects.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/filter_predicted_objects.cpp index 5d8f0a5e43610..9402c7b03f274 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/filter_predicted_objects.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/filter_predicted_objects.cpp @@ -16,8 +16,8 @@ #include #include -#include -#include +#include +#include #include #include @@ -33,7 +33,7 @@ namespace autoware::motion_velocity_planner::out_of_lane { void cut_predicted_path_beyond_line( autoware_perception_msgs::msg::PredictedPath & predicted_path, - const universe_utils::LineString2d & stop_line, const double object_front_overhang) + const autoware_utils::LineString2d & stop_line, const double object_front_overhang) { if (predicted_path.path.empty() || stop_line.size() < 2) return; @@ -55,7 +55,7 @@ void cut_predicted_path_beyond_line( auto cut_idx = stop_line_idx; double arc_length = 0; while (cut_idx > 0 && arc_length < object_front_overhang) { - arc_length += universe_utils::calcDistance2d( + arc_length += autoware_utils::calc_distance2d( predicted_path.path[cut_idx], predicted_path.path[cut_idx - 1]); --cut_idx; } @@ -63,17 +63,17 @@ void cut_predicted_path_beyond_line( } } -std::optional find_next_stop_line( +std::optional find_next_stop_line( const autoware_perception_msgs::msg::PredictedPath & path, const EgoData & ego_data) { - universe_utils::LineString2d query_path; + autoware_utils::LineString2d query_path; for (const auto & p : path.path) query_path.emplace_back(p.position.x, p.position.y); std::vector query_results; ego_data.stop_lines_rtree.query( boost::geometry::index::intersects(query_path), std::back_inserter(query_results)); auto earliest_intersecting_index = query_path.size(); - std::optional earliest_stop_line; - universe_utils::Segment2d path_segment; + std::optional earliest_stop_line; + autoware_utils::Segment2d path_segment; for (const auto & [_, stop_line] : query_results) { for (auto index = 0UL; index + 1 < query_path.size(); ++index) { path_segment.first = query_path[index]; diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/filter_predicted_objects.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/filter_predicted_objects.hpp index d59c8eae59427..b47e338d5d4ae 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/filter_predicted_objects.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/filter_predicted_objects.hpp @@ -29,13 +29,13 @@ namespace autoware::motion_velocity_planner::out_of_lane /// @param [in] object_front_overhang extra distance to cut ahead of the stop line void cut_predicted_path_beyond_line( autoware_perception_msgs::msg::PredictedPath & predicted_path, - const universe_utils::LineString2d & stop_line, const double object_front_overhang); + const autoware_utils::LineString2d & stop_line, const double object_front_overhang); /// @brief find the next red light stop line along the given path /// @param [in] path predicted path to check for a stop line /// @param [in] ego_data ego data with the stop lines information /// @return the first red light stop line found along the path (if any) -std::optional find_next_stop_line( +std::optional find_next_stop_line( const autoware_perception_msgs::msg::PredictedPath & path, const EgoData & ego_data); /// @brief cut predicted path beyond stop lines of red lights diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/footprint.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/footprint.cpp index cda114529bea3..14abf9e9c6f63 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/footprint.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/footprint.cpp @@ -14,7 +14,7 @@ #include "footprint.hpp" -#include +#include #include @@ -27,9 +27,9 @@ namespace autoware::motion_velocity_planner::out_of_lane { -universe_utils::Polygon2d make_base_footprint(const PlannerParam & p, const bool ignore_offset) +autoware_utils::Polygon2d make_base_footprint(const PlannerParam & p, const bool ignore_offset) { - universe_utils::Polygon2d base_footprint; + autoware_utils::Polygon2d base_footprint; const auto front_offset = ignore_offset ? 0.0 : p.extra_front_offset; const auto rear_offset = ignore_offset ? 0.0 : p.extra_rear_offset; const auto right_offset = ignore_offset ? 0.0 : p.extra_right_offset; @@ -44,10 +44,10 @@ universe_utils::Polygon2d make_base_footprint(const PlannerParam & p, const bool } lanelet::BasicPolygon2d project_to_pose( - const universe_utils::Polygon2d & base_footprint, const geometry_msgs::msg::Pose & pose) + const autoware_utils::Polygon2d & base_footprint, const geometry_msgs::msg::Pose & pose) { const auto angle = tf2::getYaw(pose.orientation); - const auto rotated_footprint = universe_utils::rotatePolygon(base_footprint, angle); + const auto rotated_footprint = autoware_utils::rotate_polygon(base_footprint, angle); lanelet::BasicPolygon2d footprint; for (const auto & p : rotated_footprint.outer()) footprint.emplace_back(p.x() + pose.position.x, p.y() + pose.position.y); @@ -63,7 +63,7 @@ std::vector calculate_trajectory_footprints( for (auto i = 0UL; i < ego_data.trajectory_points.size(); ++i) { const auto & trajectory_pose = ego_data.trajectory_points[i].pose; const auto angle = tf2::getYaw(trajectory_pose.orientation); - const auto rotated_footprint = universe_utils::rotatePolygon(base_footprint, angle); + const auto rotated_footprint = autoware_utils::rotate_polygon(base_footprint, angle); lanelet::BasicPolygon2d footprint; for (const auto & p : rotated_footprint.outer()) footprint.emplace_back( @@ -78,7 +78,7 @@ lanelet::BasicPolygon2d calculate_current_ego_footprint( { const auto base_footprint = make_base_footprint(params, ignore_offset); const auto angle = tf2::getYaw(ego_data.pose.orientation); - const auto rotated_footprint = universe_utils::rotatePolygon(base_footprint, angle); + const auto rotated_footprint = autoware_utils::rotate_polygon(base_footprint, angle); lanelet::BasicPolygon2d footprint; for (const auto & p : rotated_footprint.outer()) footprint.emplace_back(p.x() + ego_data.pose.position.x, p.y() + ego_data.pose.position.y); diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/footprint.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/footprint.hpp index 88baeefba6f40..c3b295c888601 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/footprint.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/footprint.hpp @@ -17,7 +17,7 @@ #include "types.hpp" -#include +#include #include @@ -28,14 +28,14 @@ namespace autoware::motion_velocity_planner::out_of_lane /// @param [in] ignore_offset optional parameter, if true, ignore the "extra offsets" to build the /// footprint /// @return base ego footprint -universe_utils::Polygon2d make_base_footprint( +autoware_utils::Polygon2d make_base_footprint( const PlannerParam & p, const bool ignore_offset = false); /// @brief project a footprint to the given pose /// @param [in] base_footprint footprint to project /// @param [in] pose projection pose /// @return footprint projected to the given pose lanelet::BasicPolygon2d project_to_pose( - const universe_utils::Polygon2d & base_footprint, const geometry_msgs::msg::Pose & pose); + const autoware_utils::Polygon2d & base_footprint, const geometry_msgs::msg::Pose & pose); /// @brief calculate the trajectory footprints /// @details the resulting polygon follows the format used by the lanelet library: clockwise order /// and implicit closing edge diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/lanelets_selection.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/lanelets_selection.cpp index e4840d724e43d..562df90f4a0b7 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/lanelets_selection.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/lanelets_selection.cpp @@ -17,8 +17,8 @@ #include "types.hpp" #include -#include -#include +#include +#include #include #include @@ -85,7 +85,7 @@ lanelet::ConstLanelets get_missing_lane_change_lanelets( } lanelet::ConstLanelets calculate_trajectory_lanelets( - const universe_utils::LineString2d & trajectory_ls, + const autoware_utils::LineString2d & trajectory_ls, const route_handler::RouteHandler & route_handler) { const auto lanelet_map_ptr = route_handler.getLaneletMapPtr(); @@ -123,7 +123,7 @@ lanelet::ConstLanelets calculate_ignored_lanelets( lanelet::ConstLanelets calculate_out_lanelets( const lanelet::LaneletLayer & lanelet_layer, - const universe_utils::MultiPolygon2d & trajectory_footprints, + const autoware_utils::MultiPolygon2d & trajectory_footprints, const lanelet::ConstLanelets & trajectory_lanelets, const lanelet::ConstLanelets & ignored_lanelets) { @@ -150,7 +150,7 @@ OutLaneletRtree calculate_out_lanelet_rtree(const lanelet::ConstLanelets & lanel nodes.reserve(lanelets.size()); for (auto i = 0UL; i < lanelets.size(); ++i) { nodes.emplace_back( - boost::geometry::return_envelope( + boost::geometry::return_envelope( lanelets[i].polygon2d().basicPolygon()), i); } @@ -161,12 +161,12 @@ void calculate_out_lanelet_rtree( EgoData & ego_data, const route_handler::RouteHandler & route_handler, const PlannerParam & params) { - universe_utils::LineString2d trajectory_ls; + autoware_utils::LineString2d trajectory_ls; for (const auto & p : ego_data.trajectory_points) { trajectory_ls.emplace_back(p.pose.position.x, p.pose.position.y); } // add a point beyond the last trajectory point to account for the ego front offset - const auto pose_beyond = universe_utils::calcOffsetPose( + const auto pose_beyond = autoware_utils::calc_offset_pose( ego_data.trajectory_points.back().pose, params.front_offset, 0.0, 0.0, 0.0); trajectory_ls.emplace_back(pose_beyond.position.x, pose_beyond.position.y); const auto trajectory_lanelets = calculate_trajectory_lanelets(trajectory_ls, route_handler); @@ -178,7 +178,7 @@ void calculate_out_lanelet_rtree( params.right_offset + params.extra_right_offset, params.rear_offset + params.extra_rear_offset, }); - universe_utils::MultiPolygon2d trajectory_footprints; + autoware_utils::MultiPolygon2d trajectory_footprints; const boost::geometry::strategy::buffer::distance_symmetric distance_strategy( max_ego_footprint_offset); const boost::geometry::strategy::buffer::join_miter join_strategy; diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/lanelets_selection.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/lanelets_selection.hpp index 0cb9e223c6456..ed967a32f52e6 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/lanelets_selection.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/lanelets_selection.hpp @@ -18,7 +18,7 @@ #include "types.hpp" #include -#include +#include #include @@ -44,7 +44,7 @@ inline bool contains_lanelet(const lanelet::ConstLanelets & lanelets, const lane /// @param [in] route_handler route handler /// @return lanelets crossed by the ego vehicle lanelet::ConstLanelets calculate_trajectory_lanelets( - const universe_utils::LineString2d & trajectory_ls, + const autoware_utils::LineString2d & trajectory_ls, const std::shared_ptr route_handler); /// @brief calculate lanelets that may not be crossed by the trajectory but may be overlapped during diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_collisions.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_collisions.cpp index 8b8a162846913..fe9ae5cd7f343 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_collisions.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_collisions.cpp @@ -16,8 +16,8 @@ #include "types.hpp" -#include -#include +#include +#include #include #include @@ -41,7 +41,7 @@ namespace autoware::motion_velocity_planner::out_of_lane void update_collision_times( OutOfLaneData & out_of_lane_data, const std::unordered_set & potential_collision_indexes, - const universe_utils::Polygon2d & object_footprint, const double time) + const autoware_utils::Polygon2d & object_footprint, const double time) { for (const auto index : potential_collision_indexes) { auto & out_of_lane_point = out_of_lane_data.outside_points[index]; @@ -64,7 +64,7 @@ void calculate_object_path_time_collisions( auto time = time_step; for (const auto & object_pose : object_path.path) { time += time_step; - const auto object_footprint = universe_utils::toPolygon2d(object_pose, object_shape); + const auto object_footprint = autoware_utils::to_polygon2d(object_pose, object_shape); std::vector query_results; out_of_lane_data.outside_areas_rtree.query( boost::geometry::index::intersects(object_footprint.outer()), @@ -142,7 +142,7 @@ OutOfLanePoint calculate_out_of_lane_point( lanelet::BasicPolygons2d intersections; boost::geometry::intersection(footprint, lanelet.polygon2d().basicPolygon(), intersections); for (const auto & intersection : intersections) { - universe_utils::Polygon2d poly; + autoware_utils::Polygon2d poly; boost::geometry::convert(intersection, poly); p.out_overlaps.push_back(poly); } @@ -173,7 +173,7 @@ void prepare_out_of_lane_areas_rtree(OutOfLaneData & out_of_lane_data) for (auto i = 0UL; i < out_of_lane_data.outside_points.size(); ++i) { for (const auto & out_overlap : out_of_lane_data.outside_points[i].out_overlaps) { OutAreaNode n; - n.first = boost::geometry::return_envelope(out_overlap); + n.first = boost::geometry::return_envelope(out_overlap); n.second = i; rtree_nodes.push_back(n); } diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.cpp index c215eacfa5c7e..ba02aaf301024 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.cpp @@ -27,10 +27,10 @@ #include #include #include -#include -#include -#include -#include +#include +#include +#include +#include #include #include @@ -67,7 +67,7 @@ void OutOfLaneModule::init(rclcpp::Node & node, const std::string & module_name) node.create_publisher("~/" + ns_ + "/debug_markers", 1); virtual_wall_publisher_ = node.create_publisher("~/" + ns_ + "/virtual_walls", 1); - processing_diag_publisher_ = std::make_shared( + processing_diag_publisher_ = std::make_shared( &node, "~/debug/" + ns_ + "/processing_time_ms_diag"); processing_time_publisher_ = node.create_publisher( @@ -75,38 +75,39 @@ void OutOfLaneModule::init(rclcpp::Node & node, const std::string & module_name) } void OutOfLaneModule::init_parameters(rclcpp::Node & node) { - using universe_utils::getOrDeclareParameter; + using autoware_utils::get_or_declare_parameter; auto & pp = params_; - pp.mode = getOrDeclareParameter(node, ns_ + ".mode"); + pp.mode = get_or_declare_parameter(node, ns_ + ".mode"); pp.skip_if_already_overlapping = - getOrDeclareParameter(node, ns_ + ".skip_if_already_overlapping"); - pp.max_arc_length = getOrDeclareParameter(node, ns_ + ".max_arc_length"); + get_or_declare_parameter(node, ns_ + ".skip_if_already_overlapping"); + pp.max_arc_length = get_or_declare_parameter(node, ns_ + ".max_arc_length"); - pp.time_threshold = getOrDeclareParameter(node, ns_ + ".threshold.time_threshold"); - pp.ttc_threshold = getOrDeclareParameter(node, ns_ + ".ttc.threshold"); + pp.time_threshold = get_or_declare_parameter(node, ns_ + ".threshold.time_threshold"); + pp.ttc_threshold = get_or_declare_parameter(node, ns_ + ".ttc.threshold"); - pp.objects_min_vel = getOrDeclareParameter(node, ns_ + ".objects.minimum_velocity"); + pp.objects_min_vel = get_or_declare_parameter(node, ns_ + ".objects.minimum_velocity"); pp.objects_min_confidence = - getOrDeclareParameter(node, ns_ + ".objects.predicted_path_min_confidence"); + get_or_declare_parameter(node, ns_ + ".objects.predicted_path_min_confidence"); pp.objects_cut_predicted_paths_beyond_red_lights = - getOrDeclareParameter(node, ns_ + ".objects.cut_predicted_paths_beyond_red_lights"); + get_or_declare_parameter(node, ns_ + ".objects.cut_predicted_paths_beyond_red_lights"); pp.objects_ignore_behind_ego = - getOrDeclareParameter(node, ns_ + ".objects.ignore_behind_ego"); + get_or_declare_parameter(node, ns_ + ".objects.ignore_behind_ego"); - pp.precision = getOrDeclareParameter(node, ns_ + ".action.precision"); - pp.min_decision_duration = getOrDeclareParameter(node, ns_ + ".action.min_duration"); + pp.precision = get_or_declare_parameter(node, ns_ + ".action.precision"); + pp.min_decision_duration = get_or_declare_parameter(node, ns_ + ".action.min_duration"); pp.lon_dist_buffer = - getOrDeclareParameter(node, ns_ + ".action.longitudinal_distance_buffer"); - pp.lat_dist_buffer = getOrDeclareParameter(node, ns_ + ".action.lateral_distance_buffer"); - pp.slow_velocity = getOrDeclareParameter(node, ns_ + ".action.slowdown.velocity"); + get_or_declare_parameter(node, ns_ + ".action.longitudinal_distance_buffer"); + pp.lat_dist_buffer = + get_or_declare_parameter(node, ns_ + ".action.lateral_distance_buffer"); + pp.slow_velocity = get_or_declare_parameter(node, ns_ + ".action.slowdown.velocity"); pp.stop_dist_threshold = - getOrDeclareParameter(node, ns_ + ".action.stop.distance_threshold"); + get_or_declare_parameter(node, ns_ + ".action.stop.distance_threshold"); - pp.extra_front_offset = getOrDeclareParameter(node, ns_ + ".ego.extra_front_offset"); - pp.extra_rear_offset = getOrDeclareParameter(node, ns_ + ".ego.extra_rear_offset"); - pp.extra_left_offset = getOrDeclareParameter(node, ns_ + ".ego.extra_left_offset"); - pp.extra_right_offset = getOrDeclareParameter(node, ns_ + ".ego.extra_right_offset"); + pp.extra_front_offset = get_or_declare_parameter(node, ns_ + ".ego.extra_front_offset"); + pp.extra_rear_offset = get_or_declare_parameter(node, ns_ + ".ego.extra_rear_offset"); + pp.extra_left_offset = get_or_declare_parameter(node, ns_ + ".ego.extra_left_offset"); + pp.extra_right_offset = get_or_declare_parameter(node, ns_ + ".ego.extra_right_offset"); const auto vehicle_info = vehicle_info_utils::VehicleInfoUtils(node).getVehicleInfo(); pp.front_offset = vehicle_info.max_longitudinal_offset_m; pp.rear_offset = vehicle_info.min_longitudinal_offset_m; @@ -116,34 +117,34 @@ void OutOfLaneModule::init_parameters(rclcpp::Node & node) void OutOfLaneModule::update_parameters(const std::vector & parameters) { - using universe_utils::updateParam; + using autoware_utils::update_param; auto & pp = params_; - updateParam(parameters, ns_ + ".mode", pp.mode); - updateParam(parameters, ns_ + ".skip_if_already_overlapping", pp.skip_if_already_overlapping); - updateParam(parameters, ns_ + ".max_arc_length", pp.max_arc_length); + update_param(parameters, ns_ + ".mode", pp.mode); + update_param(parameters, ns_ + ".skip_if_already_overlapping", pp.skip_if_already_overlapping); + update_param(parameters, ns_ + ".max_arc_length", pp.max_arc_length); - updateParam(parameters, ns_ + ".threshold.time_threshold", pp.time_threshold); - updateParam(parameters, ns_ + ".ttc.threshold", pp.ttc_threshold); + update_param(parameters, ns_ + ".threshold.time_threshold", pp.time_threshold); + update_param(parameters, ns_ + ".ttc.threshold", pp.ttc_threshold); - updateParam(parameters, ns_ + ".objects.minimum_velocity", pp.objects_min_vel); - updateParam( + update_param(parameters, ns_ + ".objects.minimum_velocity", pp.objects_min_vel); + update_param( parameters, ns_ + ".objects.predicted_path_min_confidence", pp.objects_min_confidence); - updateParam( + update_param( parameters, ns_ + ".objects.cut_predicted_paths_beyond_red_lights", pp.objects_cut_predicted_paths_beyond_red_lights); - updateParam(parameters, ns_ + ".objects.ignore_behind_ego", pp.objects_ignore_behind_ego); - - updateParam(parameters, ns_ + ".action.precision", pp.precision); - updateParam(parameters, ns_ + ".action.min_duration", pp.min_decision_duration); - updateParam(parameters, ns_ + ".action.longitudinal_distance_buffer", pp.lon_dist_buffer); - updateParam(parameters, ns_ + ".action.lateral_distance_buffer", pp.lat_dist_buffer); - updateParam(parameters, ns_ + ".action.slowdown.velocity", pp.slow_velocity); - updateParam(parameters, ns_ + ".action.stop.distance_threshold", pp.stop_dist_threshold); - - updateParam(parameters, ns_ + ".ego.extra_front_offset", pp.extra_front_offset); - updateParam(parameters, ns_ + ".ego.extra_rear_offset", pp.extra_rear_offset); - updateParam(parameters, ns_ + ".ego.extra_left_offset", pp.extra_left_offset); - updateParam(parameters, ns_ + ".ego.extra_right_offset", pp.extra_right_offset); + update_param(parameters, ns_ + ".objects.ignore_behind_ego", pp.objects_ignore_behind_ego); + + update_param(parameters, ns_ + ".action.precision", pp.precision); + update_param(parameters, ns_ + ".action.min_duration", pp.min_decision_duration); + update_param(parameters, ns_ + ".action.longitudinal_distance_buffer", pp.lon_dist_buffer); + update_param(parameters, ns_ + ".action.lateral_distance_buffer", pp.lat_dist_buffer); + update_param(parameters, ns_ + ".action.slowdown.velocity", pp.slow_velocity); + update_param(parameters, ns_ + ".action.stop.distance_threshold", pp.stop_dist_threshold); + + update_param(parameters, ns_ + ".ego.extra_front_offset", pp.extra_front_offset); + update_param(parameters, ns_ + ".ego.extra_rear_offset", pp.extra_rear_offset); + update_param(parameters, ns_ + ".ego.extra_left_offset", pp.extra_left_offset); + update_param(parameters, ns_ + ".ego.extra_right_offset", pp.extra_right_offset); } void OutOfLaneModule::limit_trajectory_size( @@ -159,7 +160,7 @@ void OutOfLaneModule::limit_trajectory_size( auto l = -ego_data.longitudinal_offset_to_first_trajectory_index; ego_data.trajectory_points.push_back(smoothed_trajectory_points[ego_data.first_trajectory_idx]); for (auto i = ego_data.first_trajectory_idx + 1; i < smoothed_trajectory_points.size(); ++i) { - l += universe_utils::calcDistance2d( + l += autoware_utils::calc_distance2d( smoothed_trajectory_points[i - 1], smoothed_trajectory_points[i]); if (l >= max_arc_length) { break; @@ -197,7 +198,7 @@ void prepare_stop_lines_rtree( stop_line_node.second.stop_line.back() += diff * 0.5; stop_line_node.second.lanelets = planner_data.route_handler->getPreviousLanelets(ll); stop_line_node.first = - boost::geometry::return_envelope(stop_line_node.second.stop_line); + boost::geometry::return_envelope(stop_line_node.second.stop_line); rtree_nodes.push_back(stop_line_node); } } @@ -220,7 +221,7 @@ VelocityPlanningResult OutOfLaneModule::plan( const std::shared_ptr planner_data) { VelocityPlanningResult result; - universe_utils::StopWatch stopwatch; + autoware_utils::StopWatch stopwatch; stopwatch.tic(); stopwatch.tic("preprocessing"); diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/types.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/types.hpp index a632a02b4012f..6539ed7f2259c 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/types.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/types.hpp @@ -16,7 +16,7 @@ #define TYPES_HPP_ #include -#include +#include #include #include @@ -81,14 +81,14 @@ struct PlannerParam namespace bgi = boost::geometry::index; struct StopLine { - universe_utils::LineString2d stop_line; + autoware_utils::LineString2d stop_line; lanelet::ConstLanelets lanelets; }; -using StopLineNode = std::pair; +using StopLineNode = std::pair; using StopLinesRtree = bgi::rtree>; -using OutAreaNode = std::pair; +using OutAreaNode = std::pair; using OutAreaRtree = bgi::rtree>; -using LaneletNode = std::pair; +using LaneletNode = std::pair; using OutLaneletRtree = bgi::rtree>; /// @brief data related to the ego vehicle @@ -119,7 +119,7 @@ struct EgoData struct OutOfLanePoint { size_t trajectory_index; - universe_utils::MultiPolygon2d out_overlaps; + autoware_utils::MultiPolygon2d out_overlaps; std::set collision_times; std::optional min_object_arrival_time; std::optional max_object_arrival_time; diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/test/test_filter_predicted_objects.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/test/test_filter_predicted_objects.cpp index 2287a7cbf308a..8c934f77aae2c 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/test/test_filter_predicted_objects.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/test/test_filter_predicted_objects.cpp @@ -25,7 +25,7 @@ TEST(TestCollisionDistance, CutPredictedPathBeyondLine) { using autoware::motion_velocity_planner::out_of_lane::cut_predicted_path_beyond_line; autoware_perception_msgs::msg::PredictedPath predicted_path; - autoware::universe_utils::LineString2d stop_line; + autoware_utils::LineString2d stop_line; double object_front_overhang = 0.0; const auto eps = 1e-9; diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_common_universe/include/autoware/motion_velocity_planner_common_universe/collision_checker.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common_universe/include/autoware/motion_velocity_planner_common_universe/collision_checker.hpp index 3b135225c01cf..f2b2ca6adccbd 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_common_universe/include/autoware/motion_velocity_planner_common_universe/collision_checker.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common_universe/include/autoware/motion_velocity_planner_common_universe/collision_checker.hpp @@ -15,7 +15,7 @@ #ifndef AUTOWARE__MOTION_VELOCITY_PLANNER_COMMON_UNIVERSE__COLLISION_CHECKER_HPP_ #define AUTOWARE__MOTION_VELOCITY_PLANNER_COMMON_UNIVERSE__COLLISION_CHECKER_HPP_ -#include +#include #include #include @@ -28,27 +28,27 @@ namespace autoware::motion_velocity_planner { namespace bgi = boost::geometry::index; -using RtreeNode = std::pair; +using RtreeNode = std::pair; using Rtree = bgi::rtree>; /// @brief collision as a trajectory index and corresponding collision points struct Collision { size_t trajectory_index{}; - autoware::universe_utils::MultiPoint2d collision_points; + autoware_utils::MultiPoint2d collision_points; }; /// @brief collision checker for a trajectory as a sequence of 2D footprints class CollisionChecker { - const autoware::universe_utils::MultiPolygon2d trajectory_footprints_; + const autoware_utils::MultiPolygon2d trajectory_footprints_; std::shared_ptr rtree_; public: /// @brief construct the collisions checker /// @param trajectory_footprints footprints of the trajectory to be used for collision checking /// @details internally, the rtree is built with the packing algorithm - explicit CollisionChecker(autoware::universe_utils::MultiPolygon2d trajectory_footprints); + explicit CollisionChecker(autoware_utils::MultiPolygon2d trajectory_footprints); /// @brief efficiently calculate collisions with a geometry /// @tparam Geometry boost geometry type diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_common_universe/include/autoware/motion_velocity_planner_common_universe/planner_data.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common_universe/include/autoware/motion_velocity_planner_common_universe/planner_data.hpp index 06916e61be8d2..cd65a26a5a4fc 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_common_universe/include/autoware/motion_velocity_planner_common_universe/planner_data.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common_universe/include/autoware/motion_velocity_planner_common_universe/planner_data.hpp @@ -19,8 +19,8 @@ #include #include #include -#include #include +#include #include #include @@ -80,7 +80,7 @@ struct PlannerData autoware_perception_msgs::msg::PredictedObject predicted_object; double get_dist_to_traj_poly( - const std::vector & decimated_traj_polys) const; + const std::vector & decimated_traj_polys) const; double get_dist_to_traj_lateral(const std::vector & traj_points) const; double get_dist_from_ego_longitudinal( const std::vector & traj_points, diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_common_universe/include/autoware/motion_velocity_planner_common_universe/plugin_module_interface.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common_universe/include/autoware/motion_velocity_planner_common_universe/plugin_module_interface.hpp index d8c32f93d7c15..8ede2e3686bb6 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_common_universe/include/autoware/motion_velocity_planner_common_universe/plugin_module_interface.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common_universe/include/autoware/motion_velocity_planner_common_universe/plugin_module_interface.hpp @@ -19,7 +19,7 @@ #include "velocity_planning_result.hpp" #include -#include +#include #include #include @@ -50,7 +50,7 @@ class PluginModuleInterface rclcpp::Logger logger_ = rclcpp::get_logger(""); rclcpp::Publisher::SharedPtr debug_publisher_; rclcpp::Publisher::SharedPtr virtual_wall_publisher_; - std::shared_ptr processing_diag_publisher_; + std::shared_ptr processing_diag_publisher_; rclcpp::Publisher::SharedPtr processing_time_publisher_; autoware::motion_utils::VirtualWallMarkerCreator virtual_wall_marker_creator{}; diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_common_universe/include/autoware/motion_velocity_planner_common_universe/polygon_utils.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common_universe/include/autoware/motion_velocity_planner_common_universe/polygon_utils.hpp index 8f7b0920a4372..71e43e6f66979 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_common_universe/include/autoware/motion_velocity_planner_common_universe/polygon_utils.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common_universe/include/autoware/motion_velocity_planner_common_universe/polygon_utils.hpp @@ -15,7 +15,7 @@ #ifndef AUTOWARE__MOTION_VELOCITY_PLANNER_COMMON_UNIVERSE__POLYGON_UTILS_HPP_ #define AUTOWARE__MOTION_VELOCITY_PLANNER_COMMON_UNIVERSE__POLYGON_UTILS_HPP_ -#include "autoware/universe_utils/geometry/boost_geometry.hpp" +#include "autoware_utils/geometry/boost_geometry.hpp" #include "autoware_vehicle_info_utils/vehicle_info_utils.hpp" #include @@ -39,8 +39,8 @@ namespace autoware::motion_velocity_planner namespace polygon_utils { namespace bg = boost::geometry; -using autoware::universe_utils::Point2d; -using autoware::universe_utils::Polygon2d; +using autoware_utils::Point2d; +using autoware_utils::Polygon2d; using autoware_perception_msgs::msg::PredictedPath; using autoware_perception_msgs::msg::Shape; diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_common_universe/include/autoware/motion_velocity_planner_common_universe/utils.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common_universe/include/autoware/motion_velocity_planner_common_universe/utils.hpp index 352dc56091981..a45d0ed7cad30 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_common_universe/include/autoware/motion_velocity_planner_common_universe/utils.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common_universe/include/autoware/motion_velocity_planner_common_universe/utils.hpp @@ -15,7 +15,7 @@ #ifndef AUTOWARE__MOTION_VELOCITY_PLANNER_COMMON_UNIVERSE__UTILS_HPP_ #define AUTOWARE__MOTION_VELOCITY_PLANNER_COMMON_UNIVERSE__UTILS_HPP_ -#include "autoware/universe_utils/geometry/geometry.hpp" +#include "autoware_utils/geometry/geometry.hpp" #include "autoware_vehicle_info_utils/vehicle_info_utils.hpp" #include "planner_data.hpp" @@ -33,18 +33,18 @@ namespace autoware::motion_velocity_planner::utils { -using autoware::universe_utils::Polygon2d; using autoware::vehicle_info_utils::VehicleInfo; using autoware_perception_msgs::msg::ObjectClassification; using autoware_perception_msgs::msg::Shape; using autoware_planning_msgs::msg::Trajectory; using autoware_planning_msgs::msg::TrajectoryPoint; +using autoware_utils::Polygon2d; using nav_msgs::msg::Odometry; using visualization_msgs::msg::Marker; using visualization_msgs::msg::MarkerArray; geometry_msgs::msg::Point to_geometry_point(const pcl::PointXYZ & point); -geometry_msgs::msg::Point to_geometry_point(const autoware::universe_utils::Point2d & point); +geometry_msgs::msg::Point to_geometry_point(const autoware_utils::Point2d & point); std::optional calc_distance_to_front_object( const std::vector & traj_points, const size_t ego_idx, @@ -109,8 +109,7 @@ size_t get_index_with_longitudinal_offset( double sum_length = 0.0; if (longitudinal_offset > 0) { for (size_t i = *start_idx; i < points.size() - 1; ++i) { - const double segment_length = - autoware::universe_utils::calcDistance2d(points.at(i), points.at(i + 1)); + const double segment_length = autoware_utils::calc_distance2d(points.at(i), points.at(i + 1)); sum_length += segment_length; if (sum_length >= longitudinal_offset) { const double back_length = sum_length - longitudinal_offset; @@ -126,8 +125,7 @@ size_t get_index_with_longitudinal_offset( } for (size_t i = *start_idx; 0 < i; --i) { - const double segment_length = - autoware::universe_utils::calcDistance2d(points.at(i - 1), points.at(i)); + const double segment_length = autoware_utils::calc_distance2d(points.at(i - 1), points.at(i)); sum_length += segment_length; if (sum_length >= -longitudinal_offset) { const double back_length = sum_length + longitudinal_offset; diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_common_universe/package.xml b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common_universe/package.xml index 214e69f4122a2..63c43f31e626a 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_common_universe/package.xml +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common_universe/package.xml @@ -25,7 +25,7 @@ autoware_perception_msgs autoware_planning_msgs autoware_route_handler - autoware_universe_utils + autoware_utils autoware_velocity_smoother eigen geometry_msgs diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_common_universe/src/collision_checker.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common_universe/src/collision_checker.cpp index f2f930a9adf73..2da7e4a6bf0af 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_common_universe/src/collision_checker.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common_universe/src/collision_checker.cpp @@ -20,15 +20,14 @@ namespace autoware::motion_velocity_planner { -CollisionChecker::CollisionChecker(autoware::universe_utils::MultiPolygon2d trajectory_footprints) +CollisionChecker::CollisionChecker(autoware_utils::MultiPolygon2d trajectory_footprints) : trajectory_footprints_(std::move(trajectory_footprints)) { std::vector nodes; nodes.reserve(trajectory_footprints_.size()); for (auto i = 0UL; i < trajectory_footprints_.size(); ++i) { nodes.emplace_back( - boost::geometry::return_envelope(trajectory_footprints_[i]), - i); + boost::geometry::return_envelope(trajectory_footprints_[i]), i); } rtree_ = std::make_shared(nodes.begin(), nodes.end()); } @@ -38,7 +37,7 @@ std::vector CollisionChecker::get_collisions(const Geometry & geometr { std::vector collisions; std::vector approximate_results; - autoware::universe_utils::MultiPoint2d intersections; + autoware_utils::MultiPoint2d intersections; ; rtree_->query(bgi::intersects(geometry), std::back_inserter(approximate_results)); for (const auto & result : approximate_results) { @@ -54,21 +53,18 @@ std::vector CollisionChecker::get_collisions(const Geometry & geometr return collisions; } -template std::vector CollisionChecker::get_collisions( - const autoware::universe_utils::Point2d & geometry) const; -template std::vector CollisionChecker::get_collisions( - const autoware::universe_utils::Line2d & geometry) const; -template std::vector -CollisionChecker::get_collisions( - const autoware::universe_utils::MultiPolygon2d & geometry) const; +template std::vector CollisionChecker::get_collisions( + const autoware_utils::Point2d & geometry) const; +template std::vector CollisionChecker::get_collisions( + const autoware_utils::Line2d & geometry) const; +template std::vector CollisionChecker::get_collisions( + const autoware_utils::MultiPolygon2d & geometry) const; // @warn Multi geometries usually lead to very inefficient queries -template std::vector -CollisionChecker::get_collisions( - const autoware::universe_utils::MultiPoint2d & geometry) const; -template std::vector -CollisionChecker::get_collisions( - const autoware::universe_utils::MultiLineString2d & geometry) const; -template std::vector CollisionChecker::get_collisions< - autoware::universe_utils::Polygon2d>(const autoware::universe_utils::Polygon2d & geometry) const; +template std::vector CollisionChecker::get_collisions( + const autoware_utils::MultiPoint2d & geometry) const; +template std::vector CollisionChecker::get_collisions( + const autoware_utils::MultiLineString2d & geometry) const; +template std::vector CollisionChecker::get_collisions( + const autoware_utils::Polygon2d & geometry) const; } // namespace autoware::motion_velocity_planner diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_common_universe/src/planner_data.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common_universe/src/planner_data.cpp index a83c9d51aab96..23e7b35b63a98 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_common_universe/src/planner_data.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common_universe/src/planner_data.cpp @@ -17,7 +17,7 @@ #include "autoware/motion_velocity_planner_common_universe/polygon_utils.hpp" #include "autoware/motion_velocity_planner_common_universe/utils.hpp" #include "autoware/object_recognition_utils/predicted_path_utils.hpp" -#include "autoware/universe_utils/geometry/boost_polygon_utils.hpp" +#include "autoware_utils/geometry/boost_polygon_utils.hpp" #include "autoware_perception_msgs/msg/predicted_path.hpp" @@ -69,11 +69,11 @@ std::optional get_predicted_object_pose_from_predicted } // namespace double PlannerData::Object::get_dist_to_traj_poly( - const std::vector & decimated_traj_polys) const + const std::vector & decimated_traj_polys) const { if (!dist_to_traj_poly) { const auto & obj_pose = predicted_object.kinematics.initial_pose_with_covariance.pose; - const auto obj_poly = autoware::universe_utils::toPolygon2d(obj_pose, predicted_object.shape); + const auto obj_poly = autoware_utils::to_polygon2d(obj_pose, predicted_object.shape); dist_to_traj_poly = std::numeric_limits::max(); for (const auto & traj_poly : decimated_traj_polys) { const double current_dist_to_traj_poly = bg::distance(traj_poly, obj_poly); @@ -134,8 +134,7 @@ void PlannerData::Object::calc_vel_relative_to_traj( const double traj_yaw = tf2::getYaw(nearest_traj_point.pose.orientation); const double obj_yaw = tf2::getYaw(obj_pose.orientation); - const Eigen::Rotation2Dd R_ego_to_obstacle( - autoware::universe_utils::normalizeRadian(obj_yaw - traj_yaw)); + const Eigen::Rotation2Dd R_ego_to_obstacle(autoware_utils::normalize_radian(obj_yaw - traj_yaw)); // Calculate the trajectory direction and the vector from the trajectory to the obstacle const Eigen::Vector2d traj_direction(std::cos(traj_yaw), std::sin(traj_yaw)); diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_common_universe/src/polygon_utils.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common_universe/src/polygon_utils.cpp index cd99d9d8469e9..91e416e73f50f 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_common_universe/src/polygon_utils.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common_universe/src/polygon_utils.cpp @@ -15,8 +15,8 @@ #include "autoware/motion_velocity_planner_common_universe/polygon_utils.hpp" #include "autoware/motion_utils/trajectory/trajectory.hpp" -#include "autoware/universe_utils/geometry/boost_polygon_utils.hpp" -#include "autoware/universe_utils/geometry/geometry.hpp" +#include "autoware_utils/geometry/boost_polygon_utils.hpp" +#include "autoware_utils/geometry/geometry.hpp" #include #include @@ -58,10 +58,10 @@ std::optional>> get_collision_inde const geometry_msgs::msg::Pose & object_pose, const rclcpp::Time & object_time, const Shape & object_shape, const double max_dist = std::numeric_limits::max()) { - const auto obj_polygon = autoware::universe_utils::toPolygon2d(object_pose, object_shape); + const auto obj_polygon = autoware_utils::to_polygon2d(object_pose, object_shape); for (size_t i = 0; i < traj_polygons.size(); ++i) { const double approximated_dist = - autoware::universe_utils::calcDistance2d(traj_points.at(i).pose, object_pose); + autoware_utils::calc_distance2d(traj_points.at(i).pose, object_pose); if (approximated_dist > max_dist) { continue; } @@ -108,14 +108,14 @@ std::optional> get_collision_point( return std::nullopt; } - const auto bumper_pose = autoware::universe_utils::calcOffsetPose( + const auto bumper_pose = autoware_utils::calc_offset_pose( traj_points.at(collision_info->first).pose, dist_to_bumper, 0.0, 0.0); std::optional max_collision_length = std::nullopt; std::optional max_collision_point = std::nullopt; for (const auto & poly_vertex : collision_info->second) { const double dist_from_bumper = - std::abs(autoware::universe_utils::inverseTransformPoint(poly_vertex.point, bumper_pose).x); + std::abs(autoware_utils::inverse_transform_point(poly_vertex.point, bumper_pose).x); if (!max_collision_length.has_value() || dist_from_bumper > *max_collision_length) { max_collision_length = dist_from_bumper; @@ -136,14 +136,14 @@ std::optional> get_collision_point( collision_info.first = collision_idx; collision_info.second = collision_points; - const auto bumper_pose = autoware::universe_utils::calcOffsetPose( + const auto bumper_pose = autoware_utils::calc_offset_pose( traj_points.at(collision_info.first).pose, dist_to_bumper, 0.0, 0.0); std::optional max_collision_length = std::nullopt; std::optional max_collision_point = std::nullopt; for (const auto & poly_vertex : collision_info.second) { const double dist_from_bumper = - std::abs(autoware::universe_utils::inverseTransformPoint(poly_vertex.point, bumper_pose).x); + std::abs(autoware_utils::inverse_transform_point(poly_vertex.point, bumper_pose).x); if (!max_collision_length.has_value() || dist_from_bumper > *max_collision_length) { max_collision_length = dist_from_bumper; @@ -208,7 +208,7 @@ std::vector create_one_step_polygons( autoware::motion_utils::findNearestSegmentIndex(traj_points, current_ego_pose.position); const auto nearest_pose = traj_points.at(nearest_idx).pose; const auto current_ego_pose_error = - autoware::universe_utils::inverseTransformPose(current_ego_pose, nearest_pose); + autoware_utils::inverse_transform_pose(current_ego_pose, nearest_pose); const double current_ego_lat_error = current_ego_pose_error.position.y; const double current_ego_yaw_error = tf2::getYaw(current_ego_pose_error.orientation); double time_elapsed{0.0}; @@ -224,11 +224,11 @@ std::vector create_one_step_polygons( const double rem_ratio = (time_to_convergence - time_elapsed) / time_to_convergence; geometry_msgs::msg::Pose indexed_pose_err; indexed_pose_err.set__orientation( - autoware::universe_utils::createQuaternionFromYaw(current_ego_yaw_error * rem_ratio)); + autoware_utils::create_quaternion_from_yaw(current_ego_yaw_error * rem_ratio)); indexed_pose_err.set__position( - autoware::universe_utils::createPoint(0.0, current_ego_lat_error * rem_ratio, 0.0)); + autoware_utils::create_point(0.0, current_ego_lat_error * rem_ratio, 0.0)); current_poses.push_back( - autoware::universe_utils::transformPose(indexed_pose_err, traj_points.at(i).pose)); + autoware_utils::transform_pose(indexed_pose_err, traj_points.at(i).pose)); if (traj_points.at(i).longitudinal_velocity_mps != 0.0) { time_elapsed += decimate_trajectory_step_length / std::abs(traj_points.at(i).longitudinal_velocity_mps); @@ -242,23 +242,22 @@ std::vector create_one_step_polygons( if (i == 0 && traj_points.at(i).longitudinal_velocity_mps > 1e-3) { boost::geometry::append( idx_poly, - autoware::universe_utils::toFootprint(pose, front_length, rear_length, vehicle_width) - .outer()); + autoware_utils::to_footprint(pose, front_length, rear_length, vehicle_width).outer()); boost::geometry::append( - idx_poly, autoware::universe_utils::fromMsg( - autoware::universe_utils::calcOffsetPose( - pose, front_length, vehicle_width * 0.5 + lat_margin, 0.0) - .position) - .to_2d()); + idx_poly, + autoware_utils::from_msg(autoware_utils::calc_offset_pose( + pose, front_length, vehicle_width * 0.5 + lat_margin, 0.0) + .position) + .to_2d()); boost::geometry::append( - idx_poly, autoware::universe_utils::fromMsg( - autoware::universe_utils::calcOffsetPose( - pose, front_length, -vehicle_width * 0.5 - lat_margin, 0.0) - .position) - .to_2d()); + idx_poly, + autoware_utils::from_msg(autoware_utils::calc_offset_pose( + pose, front_length, -vehicle_width * 0.5 - lat_margin, 0.0) + .position) + .to_2d()); } else { boost::geometry::append( - idx_poly, autoware::universe_utils::toFootprint( + idx_poly, autoware_utils::to_footprint( pose, front_length, rear_length, vehicle_width + lat_margin * 2.0) .outer()); } diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_common_universe/src/utils.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common_universe/src/utils.cpp index 930af57668dd3..5221e442dfaf7 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_common_universe/src/utils.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common_universe/src/utils.cpp @@ -18,7 +18,7 @@ #include "autoware/motion_utils/trajectory/conversion.hpp" #include "autoware/motion_utils/trajectory/trajectory.hpp" #include "autoware/motion_velocity_planner_common_universe/planner_data.hpp" -#include "autoware/universe_utils/ros/marker_helper.hpp" +#include "autoware_utils/ros/marker_helper.hpp" #include @@ -37,7 +37,7 @@ TrajectoryPoint extend_trajectory_point( const double extend_distance, const TrajectoryPoint & goal_point, const bool is_driving_forward) { TrajectoryPoint extended_trajectory_point; - extended_trajectory_point.pose = autoware::universe_utils::calcOffsetPose( + extended_trajectory_point.pose = autoware_utils::calc_offset_pose( goal_point.pose, extend_distance * (is_driving_forward ? 1.0 : -1.0), 0.0, 0.0); extended_trajectory_point.longitudinal_velocity_mps = goal_point.longitudinal_velocity_mps; extended_trajectory_point.lateral_velocity_mps = goal_point.lateral_velocity_mps; @@ -118,7 +118,7 @@ geometry_msgs::msg::Point to_geometry_point(const pcl::PointXYZ & point) return geom_point; } -geometry_msgs::msg::Point to_geometry_point(const autoware::universe_utils::Point2d & point) +geometry_msgs::msg::Point to_geometry_point(const autoware_utils::Point2d & point) { geometry_msgs::msg::Point geom_point; geom_point.x = point.x(); @@ -179,10 +179,10 @@ visualization_msgs::msg::Marker get_object_marker( { const auto current_time = rclcpp::Clock().now(); - auto marker = autoware::universe_utils::createDefaultMarker( + auto marker = autoware_utils::create_default_marker( "map", current_time, ns, idx, visualization_msgs::msg::Marker::SPHERE, - autoware::universe_utils::createMarkerScale(2.0, 2.0, 2.0), - autoware::universe_utils::createMarkerColor(r, g, b, 0.8)); + autoware_utils::create_marker_scale(2.0, 2.0, 2.0), + autoware_utils::create_marker_color(r, g, b, 0.8)); marker.pose = obj_pose; diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_common_universe/test/test_collision_checker.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common_universe/test/test_collision_checker.cpp index 79d8519189b21..178a1ae5f972e 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_common_universe/test/test_collision_checker.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common_universe/test/test_collision_checker.cpp @@ -24,12 +24,12 @@ #include using autoware::motion_velocity_planner::CollisionChecker; -using autoware::universe_utils::Line2d; -using autoware::universe_utils::MultiLineString2d; -using autoware::universe_utils::MultiPoint2d; -using autoware::universe_utils::MultiPolygon2d; -using autoware::universe_utils::Point2d; -using autoware::universe_utils::Polygon2d; +using autoware_utils::Line2d; +using autoware_utils::MultiLineString2d; +using autoware_utils::MultiPoint2d; +using autoware_utils::MultiPolygon2d; +using autoware_utils::Point2d; +using autoware_utils::Polygon2d; Point2d random_point() { diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node_universe/package.xml b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node_universe/package.xml index 570e6822ce2cf..70d9782951fa2 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node_universe/package.xml +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node_universe/package.xml @@ -25,7 +25,7 @@ autoware_perception_msgs autoware_planning_factor_interface autoware_planning_msgs - autoware_universe_utils + autoware_utils autoware_velocity_smoother eigen geometry_msgs diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node_universe/src/node.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node_universe/src/node.cpp index 32c7877b1dccd..931633341631d 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node_universe/src/node.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node_universe/src/node.cpp @@ -16,13 +16,13 @@ #include #include -#include -#include -#include -#include -#include #include #include +#include +#include +#include +#include +#include #include #include @@ -125,7 +125,7 @@ MotionVelocityPlannerNode::MotionVelocityPlannerNode(const rclcpp::NodeOptions & set_param_callback_ = this->add_on_set_parameters_callback( std::bind(&MotionVelocityPlannerNode::on_set_param, this, std::placeholders::_1)); - logger_configure_ = std::make_unique(this); + logger_configure_ = std::make_unique(this); } void MotionVelocityPlannerNode::on_load_plugin( @@ -161,23 +161,23 @@ bool MotionVelocityPlannerNode::update_planner_data( return true; }; - universe_utils::StopWatch sw; - const auto ego_state_ptr = sub_vehicle_odometry_.takeData(); + autoware_utils::StopWatch sw; + const auto ego_state_ptr = sub_vehicle_odometry_.take_data(); if (check_with_log(ego_state_ptr, "Waiting for current odometry")) planner_data_.current_odometry = *ego_state_ptr; processing_times["update_planner_data.odom"] = sw.toc(true); - const auto ego_accel_ptr = sub_acceleration_.takeData(); + const auto ego_accel_ptr = sub_acceleration_.take_data(); if (check_with_log(ego_accel_ptr, "Waiting for current acceleration")) planner_data_.current_acceleration = *ego_accel_ptr; processing_times["update_planner_data.accel"] = sw.toc(true); - const auto predicted_objects_ptr = sub_predicted_objects_.takeData(); + const auto predicted_objects_ptr = sub_predicted_objects_.take_data(); if (check_with_log(predicted_objects_ptr, "Waiting for predicted objects")) planner_data_.process_predicted_objects(*predicted_objects_ptr); processing_times["update_planner_data.pred_obj"] = sw.toc(true); - const auto no_ground_pointcloud_ptr = sub_no_ground_pointcloud_.takeData(); + const auto no_ground_pointcloud_ptr = sub_no_ground_pointcloud_.take_data(); if (check_with_log(no_ground_pointcloud_ptr, "Waiting for pointcloud")) { const auto no_ground_pointcloud = process_no_ground_pointcloud(no_ground_pointcloud_ptr); if (no_ground_pointcloud) @@ -185,7 +185,7 @@ bool MotionVelocityPlannerNode::update_planner_data( } processing_times["update_planner_data.pcd"] = sw.toc(true); - const auto occupancy_grid_ptr = sub_occupancy_grid_.takeData(); + const auto occupancy_grid_ptr = sub_occupancy_grid_.take_data(); if (check_with_log(occupancy_grid_ptr, "Waiting for the occupancy grid")) planner_data_.occupancy_grid = *occupancy_grid_ptr; processing_times["update_planner_data.occ_grid"] = sw.toc(true); @@ -206,7 +206,7 @@ bool MotionVelocityPlannerNode::update_planner_data( processing_times["update_planner_data.is_driving_forward"] = sw.toc(true); // optional data - const auto traffic_signals_ptr = sub_traffic_signals_.takeData(); + const auto traffic_signals_ptr = sub_traffic_signals_.take_data(); if (traffic_signals_ptr) process_traffic_signals(traffic_signals_ptr); processing_times["update_planner_data.traffic_lights"] = sw.toc(true); @@ -230,7 +230,7 @@ MotionVelocityPlannerNode::process_no_ground_pointcloud( Eigen::Affine3f affine = tf2::transformToEigen(transform.transform).cast(); pcl::PointCloud::Ptr pc_transformed(new pcl::PointCloud); - if (!pc.empty()) autoware::universe_utils::transformPointCloud(pc, *pc_transformed, affine); + if (!pc.empty()) autoware_utils::transform_pointcloud(pc, *pc_transformed, affine); return *pc_transformed; } @@ -289,7 +289,7 @@ void MotionVelocityPlannerNode::on_trajectory( { std::unique_lock lk(mutex_); - autoware::universe_utils::StopWatch stop_watch; + autoware_utils::StopWatch stop_watch; std::map processing_times; stop_watch.tic("Total"); @@ -410,7 +410,7 @@ autoware_planning_msgs::msg::Trajectory MotionVelocityPlannerNode::generate_traj const autoware::motion_velocity_planner::TrajectoryPoints & input_trajectory_points, std::map & processing_times) { - universe_utils::StopWatch stop_watch; + autoware_utils::StopWatch stop_watch; autoware_planning_msgs::msg::Trajectory output_trajectory_msg; output_trajectory_msg.points = {input_trajectory_points.begin(), input_trajectory_points.end()}; @@ -432,7 +432,7 @@ autoware_planning_msgs::msg::Trajectory MotionVelocityPlannerNode::generate_traj for (auto i = 1UL; i < smoothed_trajectory_points.size(); ++i) { const auto & p = smoothed_trajectory_points[i]; const auto dist_to_prev_point = - universe_utils::calcSquaredDistance2d(resampled_smoothed_trajectory_points.back(), p); + autoware_utils::calc_squared_distance2d(resampled_smoothed_trajectory_points.back(), p); if (dist_to_prev_point > min_interval_squared) { resampled_smoothed_trajectory_points.push_back(p); } @@ -468,27 +468,27 @@ autoware_planning_msgs::msg::Trajectory MotionVelocityPlannerNode::generate_traj rcl_interfaces::msg::SetParametersResult MotionVelocityPlannerNode::on_set_param( const std::vector & parameters) { - using autoware::universe_utils::updateParam; + using autoware_utils::update_param; { std::unique_lock lk(mutex_); // for planner_manager_ planner_manager_.update_module_parameters(parameters); } - updateParam(parameters, "smooth_velocity_before_planning", smooth_velocity_before_planning_); - updateParam(parameters, "ego_nearest_dist_threshold", planner_data_.ego_nearest_dist_threshold); - updateParam(parameters, "ego_nearest_yaw_threshold", planner_data_.ego_nearest_yaw_threshold); - updateParam( + update_param(parameters, "smooth_velocity_before_planning", smooth_velocity_before_planning_); + update_param(parameters, "ego_nearest_dist_threshold", planner_data_.ego_nearest_dist_threshold); + update_param(parameters, "ego_nearest_yaw_threshold", planner_data_.ego_nearest_yaw_threshold); + update_param( parameters, "trajectory_polygon_collision_check.decimate_trajectory_step_length", planner_data_.trajectory_polygon_collision_check.decimate_trajectory_step_length); - updateParam( + update_param( parameters, "trajectory_polygon_collision_check.goal_extended_trajectory_length", planner_data_.trajectory_polygon_collision_check.goal_extended_trajectory_length); - updateParam( + update_param( parameters, "trajectory_polygon_collision_check.consider_current_pose.enable_to_consider_current_pose", planner_data_.trajectory_polygon_collision_check.enable_to_consider_current_pose); - updateParam( + update_param( parameters, "trajectory_polygon_collision_check.consider_current_pose.time_to_convergence", planner_data_.trajectory_polygon_collision_check.time_to_convergence); diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node_universe/src/node.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node_universe/src/node.hpp index dee10e94ee64f..58d9687ebfa8c 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node_universe/src/node.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node_universe/src/node.hpp @@ -18,11 +18,11 @@ #include "planner_manager.hpp" #include -#include -#include -#include #include #include +#include +#include +#include #include #include @@ -68,20 +68,18 @@ class MotionVelocityPlannerNode : public rclcpp::Node // subscriber rclcpp::Subscription::SharedPtr sub_trajectory_; - autoware::universe_utils::InterProcessPollingSubscriber< - autoware_perception_msgs::msg::PredictedObjects> + autoware_utils::InterProcessPollingSubscriber sub_predicted_objects_{this, "~/input/dynamic_objects"}; - autoware::universe_utils::InterProcessPollingSubscriber + autoware_utils::InterProcessPollingSubscriber sub_no_ground_pointcloud_{ - this, "~/input/no_ground_pointcloud", autoware::universe_utils::SingleDepthSensorQoS()}; - autoware::universe_utils::InterProcessPollingSubscriber - sub_vehicle_odometry_{this, "~/input/vehicle_odometry"}; - autoware::universe_utils::InterProcessPollingSubscriber< - geometry_msgs::msg::AccelWithCovarianceStamped> + this, "~/input/no_ground_pointcloud", autoware_utils::single_depth_sensor_qos()}; + autoware_utils::InterProcessPollingSubscriber sub_vehicle_odometry_{ + this, "~/input/vehicle_odometry"}; + autoware_utils::InterProcessPollingSubscriber sub_acceleration_{this, "~/input/accel"}; - autoware::universe_utils::InterProcessPollingSubscriber - sub_occupancy_grid_{this, "~/input/occupancy_grid"}; - autoware::universe_utils::InterProcessPollingSubscriber< + autoware_utils::InterProcessPollingSubscriber sub_occupancy_grid_{ + this, "~/input/occupancy_grid"}; + autoware_utils::InterProcessPollingSubscriber< autoware_perception_msgs::msg::TrafficLightGroupArray> sub_traffic_signals_{this, "~/input/traffic_signals"}; rclcpp::Subscription::SharedPtr sub_lanelet_map_; @@ -99,11 +97,11 @@ class MotionVelocityPlannerNode : public rclcpp::Node rclcpp::Publisher::SharedPtr velocity_limit_pub_; rclcpp::Publisher::SharedPtr clear_velocity_limit_pub_; rclcpp::Publisher::SharedPtr debug_viz_pub_; - autoware::universe_utils::ProcessingTimePublisher processing_diag_publisher_{ + autoware_utils::ProcessingTimePublisher processing_diag_publisher_{ this, "~/debug/processing_time_ms_diag"}; rclcpp::Publisher::SharedPtr processing_time_publisher_; - autoware::universe_utils::PublishedTimePublisher published_time_publisher_{this}; + autoware_utils::PublishedTimePublisher published_time_publisher_{this}; rclcpp::Publisher::SharedPtr metrics_pub_; // parameters @@ -150,7 +148,7 @@ class MotionVelocityPlannerNode : public rclcpp::Node const autoware::motion_velocity_planner::TrajectoryPoints & input_trajectory_points, std::map & processing_times); - std::unique_ptr logger_configure_; + std::unique_ptr logger_configure_; }; } // namespace autoware::motion_velocity_planner diff --git a/planning/sampling_based_planner/autoware_frenet_planner/include/autoware_frenet_planner/frenet_planner.hpp b/planning/sampling_based_planner/autoware_frenet_planner/include/autoware_frenet_planner/frenet_planner.hpp index e9625beacc158..c8e921baaea50 100644 --- a/planning/sampling_based_planner/autoware_frenet_planner/include/autoware_frenet_planner/frenet_planner.hpp +++ b/planning/sampling_based_planner/autoware_frenet_planner/include/autoware_frenet_planner/frenet_planner.hpp @@ -15,10 +15,10 @@ #ifndef AUTOWARE_FRENET_PLANNER__FRENET_PLANNER_HPP_ #define AUTOWARE_FRENET_PLANNER__FRENET_PLANNER_HPP_ -#include "autoware/universe_utils/geometry/geometry.hpp" #include "autoware_frenet_planner/structures.hpp" #include "autoware_sampler_common/structures.hpp" #include "autoware_sampler_common/transform/spline_transform.hpp" +#include "autoware_utils/geometry/geometry.hpp" #include #include diff --git a/planning/sampling_based_planner/autoware_frenet_planner/src/frenet_planner/frenet_planner.cpp b/planning/sampling_based_planner/autoware_frenet_planner/src/frenet_planner/frenet_planner.cpp index 54d4bc2e90714..091676aaf5af9 100644 --- a/planning/sampling_based_planner/autoware_frenet_planner/src/frenet_planner/frenet_planner.cpp +++ b/planning/sampling_based_planner/autoware_frenet_planner/src/frenet_planner/frenet_planner.cpp @@ -135,7 +135,7 @@ void calculateCartesian( pose.position.x = it->x(); pose.position.y = it->y(); pose.position.z = 0.0; - pose.orientation = autoware::universe_utils::createQuaternionFromRPY(0.0, 0.0, yaw); + pose.orientation = autoware_utils::create_quaternion_from_rpy(0.0, 0.0, yaw); path.poses.push_back(pose); } path.yaws.push_back(path.yaws.back()); @@ -202,8 +202,7 @@ void calculateCartesian( pose.position.x = trajectory.points[i].x(); pose.position.y = trajectory.points[i].y(); pose.position.z = 0.0; - pose.orientation = - autoware::universe_utils::createQuaternionFromRPY(0.0, 0.0, trajectory.yaws[i]); + pose.orientation = autoware_utils::create_quaternion_from_rpy(0.0, 0.0, trajectory.yaws[i]); trajectory.poses.push_back(pose); } } diff --git a/planning/sampling_based_planner/autoware_path_sampler/include/autoware_path_sampler/common_structs.hpp b/planning/sampling_based_planner/autoware_path_sampler/include/autoware_path_sampler/common_structs.hpp index 101d135dcf8bf..883e943a1d39a 100644 --- a/planning/sampling_based_planner/autoware_path_sampler/include/autoware_path_sampler/common_structs.hpp +++ b/planning/sampling_based_planner/autoware_path_sampler/include/autoware_path_sampler/common_structs.hpp @@ -15,13 +15,13 @@ #ifndef AUTOWARE_PATH_SAMPLER__COMMON_STRUCTS_HPP_ #define AUTOWARE_PATH_SAMPLER__COMMON_STRUCTS_HPP_ -#include "autoware/universe_utils/ros/update_param.hpp" -#include "autoware/universe_utils/system/stop_watch.hpp" #include "autoware_path_sampler/type_alias.hpp" #include "autoware_sampler_common/structures.hpp" +#include "autoware_utils/ros/update_param.hpp" +#include "autoware_utils/system/stop_watch.hpp" #include "rclcpp/rclcpp.hpp" -#include +#include #include #include @@ -80,7 +80,7 @@ struct TimeKeeper std::string accumulated_msg = "\n"; std::stringstream latest_stream; - autoware::universe_utils::StopWatch< + autoware_utils::StopWatch< std::chrono::milliseconds, std::chrono::microseconds, std::chrono::steady_clock> stop_watch_; }; @@ -89,8 +89,8 @@ struct DebugData { std::vector sampled_candidates{}; size_t previous_sampled_candidates_nb = 0UL; - std::vector obstacles{}; - std::vector footprints{}; + std::vector obstacles{}; + std::vector footprints{}; }; struct TrajectoryParam @@ -103,10 +103,10 @@ struct TrajectoryParam void onParam(const std::vector & parameters) { - using autoware::universe_utils::updateParam; + using autoware_utils::update_param; // common - updateParam(parameters, "common.output_delta_arc_length", output_delta_arc_length); + update_param(parameters, "common.output_delta_arc_length", output_delta_arc_length); } double output_delta_arc_length; @@ -123,9 +123,9 @@ struct EgoNearestParam void onParam(const std::vector & parameters) { - using autoware::universe_utils::updateParam; - updateParam(parameters, "ego_nearest_dist_threshold", dist_threshold); - updateParam(parameters, "ego_nearest_yaw_threshold", yaw_threshold); + using autoware_utils::update_param; + update_param(parameters, "ego_nearest_dist_threshold", dist_threshold); + update_param(parameters, "ego_nearest_yaw_threshold", yaw_threshold); } double dist_threshold{0.0}; diff --git a/planning/sampling_based_planner/autoware_path_sampler/include/autoware_path_sampler/node.hpp b/planning/sampling_based_planner/autoware_path_sampler/include/autoware_path_sampler/node.hpp index ac0686d177dfb..c2f0d534147b8 100644 --- a/planning/sampling_based_planner/autoware_path_sampler/include/autoware_path_sampler/node.hpp +++ b/planning/sampling_based_planner/autoware_path_sampler/include/autoware_path_sampler/node.hpp @@ -22,8 +22,8 @@ #include "autoware_vehicle_info_utils/vehicle_info_utils.hpp" #include "rclcpp/rclcpp.hpp" -#include #include +#include #include #include @@ -62,9 +62,8 @@ class PathSampler : public rclcpp::Node // interface subscriber rclcpp::Subscription::SharedPtr path_sub_; - autoware::universe_utils::InterProcessPollingSubscriber odom_sub_{ - this, "~/input/odometry"}; - autoware::universe_utils::InterProcessPollingSubscriber objects_sub_{ + autoware_utils::InterProcessPollingSubscriber odom_sub_{this, "~/input/odometry"}; + autoware_utils::InterProcessPollingSubscriber objects_sub_{ this, "~/input/objects"}; // debug publisher diff --git a/planning/sampling_based_planner/autoware_path_sampler/include/autoware_path_sampler/utils/geometry_utils.hpp b/planning/sampling_based_planner/autoware_path_sampler/include/autoware_path_sampler/utils/geometry_utils.hpp index 60ac81f8b6654..eba36e29ebe22 100644 --- a/planning/sampling_based_planner/autoware_path_sampler/include/autoware_path_sampler/utils/geometry_utils.hpp +++ b/planning/sampling_based_planner/autoware_path_sampler/include/autoware_path_sampler/utils/geometry_utils.hpp @@ -19,9 +19,9 @@ #include "autoware/interpolation/spline_interpolation.hpp" #include "autoware/interpolation/spline_interpolation_points_2d.hpp" #include "autoware/motion_utils/trajectory/trajectory.hpp" -#include "autoware/universe_utils/geometry/geometry.hpp" #include "autoware_path_sampler/common_structs.hpp" #include "autoware_path_sampler/type_alias.hpp" +#include "autoware_utils/geometry/geometry.hpp" #include "autoware_vehicle_info_utils/vehicle_info_utils.hpp" #include "eigen3/Eigen/Core" @@ -43,8 +43,8 @@ namespace geometry_utils template bool isSamePoint(const T1 & t1, const T2 & t2) { - const auto p1 = autoware::universe_utils::getPoint(t1); - const auto p2 = autoware::universe_utils::getPoint(t2); + const auto p1 = autoware_utils::get_point(t1); + const auto p2 = autoware_utils::get_point(t2); constexpr double epsilon = 1e-6; if (epsilon < std::abs(p1.x - p2.x) || epsilon < std::abs(p1.y - p2.y)) { diff --git a/planning/sampling_based_planner/autoware_path_sampler/include/autoware_path_sampler/utils/trajectory_utils.hpp b/planning/sampling_based_planner/autoware_path_sampler/include/autoware_path_sampler/utils/trajectory_utils.hpp index 5bab37287ae52..2c07a61ac158d 100644 --- a/planning/sampling_based_planner/autoware_path_sampler/include/autoware_path_sampler/utils/trajectory_utils.hpp +++ b/planning/sampling_based_planner/autoware_path_sampler/include/autoware_path_sampler/utils/trajectory_utils.hpp @@ -19,10 +19,10 @@ #include "autoware/interpolation/spline_interpolation.hpp" #include "autoware/interpolation/spline_interpolation_points_2d.hpp" #include "autoware/motion_utils/trajectory/trajectory.hpp" -#include "autoware/universe_utils/geometry/geometry.hpp" #include "autoware_path_sampler/common_structs.hpp" #include "autoware_path_sampler/type_alias.hpp" #include "autoware_sampler_common/structures.hpp" +#include "autoware_utils/geometry/geometry.hpp" #include "eigen3/Eigen/Core" #include "autoware_planning_msgs/msg/path_point.hpp" @@ -53,7 +53,7 @@ std::optional getPointIndexAfter( // search forward if (sum_length < offset) { for (size_t i = target_seg_idx + 1; i < points.size(); ++i) { - sum_length += autoware::universe_utils::calcDistance2d(points.at(i), points.at(i - 1)); + sum_length += autoware_utils::calc_distance2d(points.at(i), points.at(i - 1)); if (offset < sum_length) { return i - 1; } @@ -65,7 +65,7 @@ std::optional getPointIndexAfter( // search backward for (size_t i = target_seg_idx; 0 < i; --i) { // NOTE: use size_t since i is always positive value - sum_length -= autoware::universe_utils::calcDistance2d(points.at(i), points.at(i - 1)); + sum_length -= autoware_utils::calc_distance2d(points.at(i), points.at(i - 1)); if (sum_length < offset) { return i - 1; } @@ -78,7 +78,7 @@ template TrajectoryPoint convertToTrajectoryPoint(const T & point) { TrajectoryPoint traj_point; - traj_point.pose = autoware::universe_utils::getPose(point); + traj_point.pose = autoware_utils::get_pose(point); traj_point.longitudinal_velocity_mps = point.longitudinal_velocity_mps; traj_point.lateral_velocity_mps = point.lateral_velocity_mps; traj_point.heading_rate_rps = point.heading_rate_rps; @@ -149,7 +149,7 @@ std::optional updateFrontPointForFix( { // calculate front point to insert in points as a fixed point const size_t front_seg_idx_for_fix = trajectory_utils::findEgoSegmentIndex( - points_for_fix, autoware::universe_utils::getPose(points.front()), ego_nearest_param); + points_for_fix, autoware_utils::get_pose(points.front()), ego_nearest_param); const size_t front_point_idx_for_fix = front_seg_idx_for_fix; const auto & front_fix_point = points_for_fix.at(front_point_idx_for_fix); @@ -163,7 +163,7 @@ std::optional updateFrontPointForFix( return std::nullopt; } - const double dist = autoware::universe_utils::calcDistance2d(points.front(), front_fix_point); + const double dist = autoware_utils::calc_distance2d(points.front(), front_fix_point); // check if deviation is not too large constexpr double max_lat_error = 3.0; diff --git a/planning/sampling_based_planner/autoware_path_sampler/package.xml b/planning/sampling_based_planner/autoware_path_sampler/package.xml index 9efd0907c2727..78086030ddb03 100644 --- a/planning/sampling_based_planner/autoware_path_sampler/package.xml +++ b/planning/sampling_based_planner/autoware_path_sampler/package.xml @@ -20,7 +20,7 @@ autoware_motion_utils autoware_perception_msgs autoware_planning_msgs - autoware_universe_utils + autoware_utils autoware_vehicle_info_utils geometry_msgs nav_msgs diff --git a/planning/sampling_based_planner/autoware_path_sampler/src/node.cpp b/planning/sampling_based_planner/autoware_path_sampler/src/node.cpp index fd07516d61bbf..9c28ca5ea3b82 100644 --- a/planning/sampling_based_planner/autoware_path_sampler/src/node.cpp +++ b/planning/sampling_based_planner/autoware_path_sampler/src/node.cpp @@ -147,59 +147,62 @@ PathSampler::PathSampler(const rclcpp::NodeOptions & node_options) rcl_interfaces::msg::SetParametersResult PathSampler::onParam( const std::vector & parameters) { - using autoware::universe_utils::updateParam; + using autoware_utils::update_param; - updateParam(parameters, "constraints.hard.max_curvature", params_.constraints.hard.max_curvature); - updateParam(parameters, "constraints.hard.min_curvature", params_.constraints.hard.min_curvature); - updateParam( + update_param( + parameters, "constraints.hard.max_curvature", params_.constraints.hard.max_curvature); + update_param( + parameters, "constraints.hard.min_curvature", params_.constraints.hard.min_curvature); + update_param( parameters, "constraints.hard.min_distance_from_obstacles", params_.constraints.hard.min_dist_from_obstacles); - updateParam( + update_param( parameters, "constraints.hard.limit_footprint_inside_drivable_area", params_.constraints.hard.limit_footprint_inside_drivable_area); - updateParam( + update_param( parameters, "constraints.soft.lateral_deviation_weight", params_.constraints.soft.lateral_deviation_weight); - updateParam(parameters, "constraints.soft.length_weight", params_.constraints.soft.length_weight); - updateParam( + update_param( + parameters, "constraints.soft.length_weight", params_.constraints.soft.length_weight); + update_param( parameters, "constraints.soft.curvature_weight", params_.constraints.soft.curvature_weight); - updateParam(parameters, "sampling.enable_frenet", params_.sampling.enable_frenet); - updateParam(parameters, "sampling.enable_bezier", params_.sampling.enable_bezier); - updateParam(parameters, "sampling.resolution", params_.sampling.resolution); - updateParam( + update_param(parameters, "sampling.enable_frenet", params_.sampling.enable_frenet); + update_param(parameters, "sampling.enable_bezier", params_.sampling.enable_bezier); + update_param(parameters, "sampling.resolution", params_.sampling.resolution); + update_param( parameters, "sampling.previous_path_reuse_points_nb", params_.sampling.previous_path_reuse_points_nb); - updateParam(parameters, "sampling.target_lengths", params_.sampling.target_lengths); - updateParam( + update_param(parameters, "sampling.target_lengths", params_.sampling.target_lengths); + update_param( parameters, "sampling.target_lateral_positions", params_.sampling.target_lateral_positions); - updateParam( + update_param( parameters, "sampling.nb_target_lateral_positions", params_.sampling.nb_target_lateral_positions); - updateParam( + update_param( parameters, "sampling.frenet.target_lateral_velocities", params_.sampling.frenet.target_lateral_velocities); - updateParam( + update_param( parameters, "sampling.frenet.target_lateral_accelerations", params_.sampling.frenet.target_lateral_accelerations); - updateParam(parameters, "sampling.bezier.nb_k", params_.sampling.bezier.nb_k); - updateParam(parameters, "sampling.bezier.mk_min", params_.sampling.bezier.mk_min); - updateParam(parameters, "sampling.bezier.mk_max", params_.sampling.bezier.mk_max); - updateParam(parameters, "sampling.bezier.nb_t", params_.sampling.bezier.nb_t); - updateParam(parameters, "sampling.bezier.mt_min", params_.sampling.bezier.mt_min); - updateParam(parameters, "sampling.bezier.mt_max", params_.sampling.bezier.mt_max); - updateParam( + update_param(parameters, "sampling.bezier.nb_k", params_.sampling.bezier.nb_k); + update_param(parameters, "sampling.bezier.mk_min", params_.sampling.bezier.mk_min); + update_param(parameters, "sampling.bezier.mk_max", params_.sampling.bezier.mk_max); + update_param(parameters, "sampling.bezier.nb_t", params_.sampling.bezier.nb_t); + update_param(parameters, "sampling.bezier.mt_min", params_.sampling.bezier.mt_min); + update_param(parameters, "sampling.bezier.mt_max", params_.sampling.bezier.mt_max); + update_param( parameters, "preprocessing.force_zero_initial_deviation", params_.preprocessing.force_zero_deviation); - updateParam( + update_param( parameters, "preprocessing.force_zero_initial_heading", params_.preprocessing.force_zero_heading); - updateParam( + update_param( parameters, "preprocessing.smooth_reference_trajectory", params_.preprocessing.smooth_reference); - updateParam( + update_param( parameters, "debug.enable_calculation_time_info", time_keeper_ptr_->enable_calculation_time_info); - updateParam(parameters, "debug.id", debug_id_); + update_param(parameters, "debug.id", debug_id_); // parameters for ego nearest search ego_nearest_param_.onParam(parameters); @@ -235,7 +238,7 @@ void PathSampler::onPath(const Path::SharedPtr path_ptr) time_keeper_ptr_->tic(__func__); // check if data is ready and valid - const auto ego_state_ptr = odom_sub_.takeData(); + const auto ego_state_ptr = odom_sub_.take_data(); if (!isDataReady(*path_ptr, ego_state_ptr, *get_clock())) { return; } @@ -293,7 +296,7 @@ bool PathSampler::isDataReady( return false; } - if (!objects_sub_.takeData()) { + if (!objects_sub_.take_data()) { RCLCPP_INFO_SKIPFIRST_THROTTLE(get_logger(), clock, 5000, "Waiting for detected objects."); return false; } @@ -321,14 +324,14 @@ void PathSampler::copyZ( to_traj.front().pose.position.z = from_traj.front().pose.position.z; if (from_traj.size() < 2 || to_traj.size() < 2) return; auto from = from_traj.begin() + 1; - auto s_from = autoware::universe_utils::calcDistance2d(from->pose, std::next(from)->pose); + auto s_from = autoware_utils::calc_distance2d(from->pose, std::next(from)->pose); auto s_to = 0.0; auto s_from_prev = 0.0; for (auto to = to_traj.begin() + 1; to + 1 != to_traj.end(); ++to) { - s_to += autoware::universe_utils::calcDistance2d(std::prev(to)->pose, to->pose); + s_to += autoware_utils::calc_distance2d(std::prev(to)->pose, to->pose); for (; s_from < s_to && from + 1 != from_traj.end(); ++from) { s_from_prev = s_from; - s_from += autoware::universe_utils::calcDistance2d(from->pose, std::next(from)->pose); + s_from += autoware_utils::calc_distance2d(from->pose, std::next(from)->pose); } const auto ratio = (s_to - s_from_prev) / (s_from - s_from_prev); to->pose.position.z = std::prev(from)->pose.position.z + @@ -344,8 +347,8 @@ void PathSampler::copyVelocity( if (to_traj.empty() || from_traj.empty()) return; const auto closest_fn = [&](const auto & p1, const auto & p2) { - return autoware::universe_utils::calcDistance2d(p1.pose, ego_pose) <= - autoware::universe_utils::calcDistance2d(p2.pose, ego_pose); + return autoware_utils::calc_distance2d(p1.pose, ego_pose) <= + autoware_utils::calc_distance2d(p2.pose, ego_pose); }; const auto first_from = std::min_element(from_traj.begin(), from_traj.end() - 1, closest_fn); const auto first_to = std::min_element(to_traj.begin(), to_traj.end() - 1, closest_fn); @@ -355,14 +358,14 @@ void PathSampler::copyVelocity( to->longitudinal_velocity_mps = first_from->longitudinal_velocity_mps; auto from = first_from; - auto s_from = autoware::universe_utils::calcDistance2d(from->pose, std::next(from)->pose); + auto s_from = autoware_utils::calc_distance2d(from->pose, std::next(from)->pose); auto s_to = 0.0; auto s_from_prev = 0.0; for (; to + 1 != to_traj.end(); ++to) { - s_to += autoware::universe_utils::calcDistance2d(to->pose, std::next(to)->pose); + s_to += autoware_utils::calc_distance2d(to->pose, std::next(to)->pose); for (; s_from < s_to && from + 1 != from_traj.end(); ++from) { s_from_prev = s_from; - s_from += autoware::universe_utils::calcDistance2d(from->pose, std::next(from)->pose); + s_from += autoware_utils::calc_distance2d(from->pose, std::next(from)->pose); } if ( from->longitudinal_velocity_mps == 0.0 || std::prev(from)->longitudinal_velocity_mps == 0.0) { @@ -469,7 +472,7 @@ autoware::sampler_common::Path PathSampler::generatePath(const PlannerData & pla current_state.heading = tf2::getYaw(planner_data.ego_pose.orientation); const auto planning_state = getPlanningState(current_state, path_spline); - const auto objects = objects_sub_.takeData(); + const auto objects = objects_sub_.take_data(); prepareConstraints( params_.constraints, *objects, planner_data.left_bound, planner_data.right_bound); diff --git a/planning/sampling_based_planner/autoware_path_sampler/src/prepare_inputs.cpp b/planning/sampling_based_planner/autoware_path_sampler/src/prepare_inputs.cpp index 7f313318af236..abcf07094dca7 100644 --- a/planning/sampling_based_planner/autoware_path_sampler/src/prepare_inputs.cpp +++ b/planning/sampling_based_planner/autoware_path_sampler/src/prepare_inputs.cpp @@ -14,11 +14,11 @@ #include "autoware_path_sampler/prepare_inputs.hpp" -#include "autoware/universe_utils/geometry/boost_polygon_utils.hpp" #include "autoware_frenet_planner/structures.hpp" #include "autoware_path_sampler/utils/geometry_utils.hpp" #include "autoware_sampler_common/structures.hpp" #include "autoware_sampler_common/transform/spline_transform.hpp" +#include "autoware_utils/geometry/boost_polygon_utils.hpp" #include @@ -41,7 +41,7 @@ void prepareConstraints( constraints.obstacle_polygons = autoware::sampler_common::MultiPolygon2d(); for (const auto & o : predicted_objects.objects) if (o.kinematics.initial_twist_with_covariance.twist.linear.x < 0.5) // TODO(Maxime): param - constraints.obstacle_polygons.push_back(autoware::universe_utils::toPolygon2d(o)); + constraints.obstacle_polygons.push_back(autoware_utils::to_polygon2d(o)); constraints.dynamic_obstacles = {}; // TODO(Maxime): not implemented // TODO(Maxime): directly use lines instead of polygon diff --git a/planning/sampling_based_planner/autoware_sampler_common/include/autoware_sampler_common/structures.hpp b/planning/sampling_based_planner/autoware_sampler_common/include/autoware_sampler_common/structures.hpp index c2cf432229b1b..6e24851e2999c 100644 --- a/planning/sampling_based_planner/autoware_sampler_common/include/autoware_sampler_common/structures.hpp +++ b/planning/sampling_based_planner/autoware_sampler_common/include/autoware_sampler_common/structures.hpp @@ -15,7 +15,7 @@ #ifndef AUTOWARE_SAMPLER_COMMON__STRUCTURES_HPP_ #define AUTOWARE_SAMPLER_COMMON__STRUCTURES_HPP_ -#include "autoware/universe_utils/geometry/boost_geometry.hpp" +#include "autoware_utils/geometry/boost_geometry.hpp" #include #include @@ -38,13 +38,13 @@ namespace autoware::sampler_common { -using autoware::universe_utils::LinearRing2d; -using autoware::universe_utils::MultiPoint2d; -using autoware::universe_utils::MultiPolygon2d; -using autoware::universe_utils::Point2d; -using autoware::universe_utils::Polygon2d; +using autoware_utils::LinearRing2d; +using autoware_utils::MultiPoint2d; +using autoware_utils::MultiPolygon2d; +using autoware_utils::Point2d; +using autoware_utils::Polygon2d; -typedef std::pair BoxIndexPair; +typedef std::pair BoxIndexPair; typedef boost::geometry::index::rtree> Rtree; /// @brief data about constraint check results of a given path diff --git a/planning/sampling_based_planner/autoware_sampler_common/package.xml b/planning/sampling_based_planner/autoware_sampler_common/package.xml index 385f0da88d846..8f632ed6f5090 100644 --- a/planning/sampling_based_planner/autoware_sampler_common/package.xml +++ b/planning/sampling_based_planner/autoware_sampler_common/package.xml @@ -12,7 +12,7 @@ autoware_cmake autoware_interpolation - autoware_universe_utils + autoware_utils ament_cmake_ros ament_lint_auto diff --git a/sensing/autoware_image_diagnostics/package.xml b/sensing/autoware_image_diagnostics/package.xml index 91edff5213f2a..9895e055e0472 100644 --- a/sensing/autoware_image_diagnostics/package.xml +++ b/sensing/autoware_image_diagnostics/package.xml @@ -13,7 +13,7 @@ ament_cmake_auto autoware_internal_debug_msgs - autoware_universe_utils + autoware_utils cv_bridge diagnostic_updater image_transport diff --git a/sensing/autoware_imu_corrector/package.xml b/sensing/autoware_imu_corrector/package.xml index c4938585b9275..902728eecdc38 100644 --- a/sensing/autoware_imu_corrector/package.xml +++ b/sensing/autoware_imu_corrector/package.xml @@ -12,7 +12,7 @@ ament_cmake_auto autoware_cmake - autoware_universe_utils + autoware_utils diagnostic_updater rclcpp rclcpp_components diff --git a/sensing/autoware_imu_corrector/src/gyro_bias_estimation_module.cpp b/sensing/autoware_imu_corrector/src/gyro_bias_estimation_module.cpp index 2fbd5d4a6a39e..3e39f465c7656 100644 --- a/sensing/autoware_imu_corrector/src/gyro_bias_estimation_module.cpp +++ b/sensing/autoware_imu_corrector/src/gyro_bias_estimation_module.cpp @@ -14,7 +14,7 @@ #include "gyro_bias_estimation_module.hpp" -#include +#include #include #include @@ -53,15 +53,15 @@ geometry_msgs::msg::Vector3 calculate_error_rpy( const geometry_msgs::msg::Vector3 & gyro_bias) { const geometry_msgs::msg::Vector3 rpy_0 = - autoware::universe_utils::getRPY(pose_list.front().pose.orientation); + autoware_utils::get_rpy(pose_list.front().pose.orientation); const geometry_msgs::msg::Vector3 rpy_1 = - autoware::universe_utils::getRPY(pose_list.back().pose.orientation); + autoware_utils::get_rpy(pose_list.back().pose.orientation); const geometry_msgs::msg::Vector3 d_rpy = integrate_orientation(gyro_list, gyro_bias); geometry_msgs::msg::Vector3 error_rpy; - error_rpy.x = autoware::universe_utils::normalizeRadian(-rpy_1.x + rpy_0.x + d_rpy.x); - error_rpy.y = autoware::universe_utils::normalizeRadian(-rpy_1.y + rpy_0.y + d_rpy.y); - error_rpy.z = autoware::universe_utils::normalizeRadian(-rpy_1.z + rpy_0.z + d_rpy.z); + error_rpy.x = autoware_utils::normalize_radian(-rpy_1.x + rpy_0.x + d_rpy.x); + error_rpy.y = autoware_utils::normalize_radian(-rpy_1.y + rpy_0.y + d_rpy.y); + error_rpy.z = autoware_utils::normalize_radian(-rpy_1.z + rpy_0.z + d_rpy.z); return error_rpy; } diff --git a/sensing/autoware_imu_corrector/src/gyro_bias_estimator.cpp b/sensing/autoware_imu_corrector/src/gyro_bias_estimator.cpp index 17194ac1b0459..02c329bb54808 100644 --- a/sensing/autoware_imu_corrector/src/gyro_bias_estimator.cpp +++ b/sensing/autoware_imu_corrector/src/gyro_bias_estimator.cpp @@ -14,7 +14,7 @@ #include "gyro_bias_estimator.hpp" -#include +#include #include #include @@ -60,7 +60,7 @@ GyroBiasEstimator::GyroBiasEstimator(const rclcpp::NodeOptions & options) this->get_node_base_interface()->get_context()); this->get_node_timers_interface()->add_timer(timer_, nullptr); - transform_listener_ = std::make_shared(this); + transform_listener_ = std::make_shared(this); // initialize diagnostics_info_ { @@ -79,7 +79,7 @@ void GyroBiasEstimator::callback_imu(const Imu::ConstSharedPtr imu_msg_ptr) { imu_frame_ = imu_msg_ptr->header.frame_id; geometry_msgs::msg::TransformStamped::ConstSharedPtr tf_imu2base_ptr = - transform_listener_->getLatestTransform(imu_frame_, output_frame_); + transform_listener_->get_latest_transform(imu_frame_, output_frame_); if (!tf_imu2base_ptr) { RCLCPP_ERROR( this->get_logger(), "Please publish TF %s to %s", output_frame_.c_str(), @@ -151,10 +151,10 @@ void GyroBiasEstimator::timer_callback() // Check if the vehicle is moving straight const geometry_msgs::msg::Vector3 rpy_0 = - autoware::universe_utils::getRPY(pose_buf.front().pose.orientation); + autoware_utils::get_rpy(pose_buf.front().pose.orientation); const geometry_msgs::msg::Vector3 rpy_1 = - autoware::universe_utils::getRPY(pose_buf.back().pose.orientation); - const double yaw_diff = std::abs(autoware::universe_utils::normalizeRadian(rpy_1.z - rpy_0.z)); + autoware_utils::get_rpy(pose_buf.back().pose.orientation); + const double yaw_diff = std::abs(autoware_utils::normalize_radian(rpy_1.z - rpy_0.z)); const double time_diff = (t1_rclcpp_time - t0_rclcpp_time).seconds(); const double yaw_vel = yaw_diff / time_diff; const bool is_straight = (yaw_vel < straight_motion_ang_vel_upper_limit_); @@ -168,7 +168,7 @@ void GyroBiasEstimator::timer_callback() gyro_bias_estimation_module_->update_bias(pose_buf, gyro_filtered); geometry_msgs::msg::TransformStamped::ConstSharedPtr tf_base2imu_ptr = - transform_listener_->getLatestTransform(output_frame_, imu_frame_); + transform_listener_->get_latest_transform(output_frame_, imu_frame_); if (!tf_base2imu_ptr) { RCLCPP_ERROR( this->get_logger(), "Please publish TF %s to %s", imu_frame_.c_str(), output_frame_.c_str()); diff --git a/sensing/autoware_imu_corrector/src/gyro_bias_estimator.hpp b/sensing/autoware_imu_corrector/src/gyro_bias_estimator.hpp index 68539a2181fd3..56958ab9e0f28 100644 --- a/sensing/autoware_imu_corrector/src/gyro_bias_estimator.hpp +++ b/sensing/autoware_imu_corrector/src/gyro_bias_estimator.hpp @@ -16,7 +16,7 @@ #include "gyro_bias_estimation_module.hpp" -#include +#include #include #include @@ -76,7 +76,7 @@ class GyroBiasEstimator : public rclcpp::Node std::optional gyro_bias_; - std::shared_ptr transform_listener_; + std::shared_ptr transform_listener_; std::string imu_frame_; diff --git a/sensing/autoware_imu_corrector/src/imu_corrector_core.cpp b/sensing/autoware_imu_corrector/src/imu_corrector_core.cpp index d615a721ca5eb..e2932c3e03a61 100644 --- a/sensing/autoware_imu_corrector/src/imu_corrector_core.cpp +++ b/sensing/autoware_imu_corrector/src/imu_corrector_core.cpp @@ -28,7 +28,7 @@ std::array transform_covariance(const std::array & cov) { - using COV_IDX = autoware::universe_utils::xyz_covariance_index::XYZ_COV_IDX; + using COV_IDX = autoware_utils::xyz_covariance_index::XYZ_COV_IDX; double max_cov = 0.0; max_cov = std::max(max_cov, cov[COV_IDX::X_X]); @@ -60,7 +60,7 @@ ImuCorrector::ImuCorrector(const rclcpp::NodeOptions & options) : rclcpp::Node("imu_corrector", options), output_frame_(declare_parameter("base_link", "base_link")) { - transform_listener_ = std::make_shared(this); + transform_listener_ = std::make_shared(this); angular_velocity_offset_x_imu_link_ = declare_parameter("angular_velocity_offset_x", 0.0); angular_velocity_offset_y_imu_link_ = declare_parameter("angular_velocity_offset_y", 0.0); @@ -104,7 +104,7 @@ void ImuCorrector::callback_imu(const sensor_msgs::msg::Imu::ConstSharedPtr imu_ accel_stddev_imu_link_ * accel_stddev_imu_link_; geometry_msgs::msg::TransformStamped::ConstSharedPtr tf_imu2base_ptr = - transform_listener_->getLatestTransform(imu_msg.header.frame_id, output_frame_); + transform_listener_->get_latest_transform(imu_msg.header.frame_id, output_frame_); if (!tf_imu2base_ptr) { RCLCPP_ERROR( this->get_logger(), "Please publish TF %s to %s", output_frame_.c_str(), diff --git a/sensing/autoware_imu_corrector/src/imu_corrector_core.hpp b/sensing/autoware_imu_corrector/src/imu_corrector_core.hpp index 0c4839f6b5290..3df256a7c933c 100644 --- a/sensing/autoware_imu_corrector/src/imu_corrector_core.hpp +++ b/sensing/autoware_imu_corrector/src/imu_corrector_core.hpp @@ -14,8 +14,8 @@ #ifndef IMU_CORRECTOR_CORE_HPP_ #define IMU_CORRECTOR_CORE_HPP_ -#include -#include +#include +#include #include #include @@ -30,7 +30,7 @@ namespace autoware::imu_corrector { class ImuCorrector : public rclcpp::Node { - using COV_IDX = autoware::universe_utils::xyz_covariance_index::XYZ_COV_IDX; + using COV_IDX = autoware_utils::xyz_covariance_index::XYZ_COV_IDX; public: explicit ImuCorrector(const rclcpp::NodeOptions & options); @@ -52,7 +52,7 @@ class ImuCorrector : public rclcpp::Node double accel_stddev_imu_link_; - std::shared_ptr transform_listener_; + std::shared_ptr transform_listener_; std::string output_frame_; }; diff --git a/sensing/autoware_pointcloud_preprocessor/CMakeLists.txt b/sensing/autoware_pointcloud_preprocessor/CMakeLists.txt index 19c698b11bc5f..98d66bf63addf 100644 --- a/sensing/autoware_pointcloud_preprocessor/CMakeLists.txt +++ b/sensing/autoware_pointcloud_preprocessor/CMakeLists.txt @@ -42,7 +42,7 @@ ament_target_dependencies(pointcloud_preprocessor_filter_base rclcpp sensor_msgs tf2_ros - autoware_universe_utils + autoware_utils pcl_ros autoware_point_types ) diff --git a/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/concatenate_data/combine_cloud_handler.hpp b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/concatenate_data/combine_cloud_handler.hpp index fa8c58aa74c11..0dcf3856e73aa 100644 --- a/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/concatenate_data/combine_cloud_handler.hpp +++ b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/concatenate_data/combine_cloud_handler.hpp @@ -24,7 +24,7 @@ // ROS includes #include "autoware/point_types/types.hpp" -#include +#include #include #include @@ -34,8 +34,6 @@ #include #include #include -#include -#include #include #include @@ -65,7 +63,7 @@ class CombineCloudHandler bool is_motion_compensated_; bool publish_synchronized_pointcloud_; bool keep_input_frame_in_synchronized_pointcloud_; - std::unique_ptr managed_tf_buffer_{nullptr}; + std::unique_ptr managed_tf_buffer_{nullptr}; std::deque twist_queue_; diff --git a/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/concatenate_data/concatenate_and_time_sync_node.hpp b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/concatenate_data/concatenate_and_time_sync_node.hpp index fc422c7d2e54e..3055cf977ce02 100644 --- a/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/concatenate_data/concatenate_and_time_sync_node.hpp +++ b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/concatenate_data/concatenate_and_time_sync_node.hpp @@ -26,8 +26,8 @@ #include "collector_matching_strategy.hpp" #include "combine_cloud_handler.hpp" -#include -#include +#include +#include #include #include @@ -107,9 +107,9 @@ class PointCloudConcatenateDataSynchronizerComponent : public rclcpp::Node rclcpp::Publisher::SharedPtr concatenated_cloud_publisher_; std::unordered_map::SharedPtr> topic_to_transformed_cloud_publisher_map_; - std::unique_ptr debug_publisher_; + std::unique_ptr debug_publisher_; - std::unique_ptr> stop_watch_ptr_; + std::unique_ptr> stop_watch_ptr_; diagnostic_updater::Updater diagnostic_updater_{this}; void cloud_callback( diff --git a/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/concatenate_data/concatenate_pointclouds.hpp b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/concatenate_data/concatenate_pointclouds.hpp index d57e44431a8d2..07fa0a3165b18 100644 --- a/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/concatenate_data/concatenate_pointclouds.hpp +++ b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/concatenate_data/concatenate_pointclouds.hpp @@ -62,9 +62,9 @@ // ROS includes #include -#include -#include -#include +#include +#include +#include #include #include @@ -142,7 +142,7 @@ class PointCloudConcatenationComponent : public rclcpp::Node // XmlRpc::XmlRpcValue input_topics_; std::vector input_topics_; - std::unique_ptr managed_tf_buffer_{nullptr}; + std::unique_ptr managed_tf_buffer_{nullptr}; std::deque twist_ptr_queue_; @@ -169,8 +169,8 @@ class PointCloudConcatenationComponent : public rclcpp::Node void checkConcatStatus(diagnostic_updater::DiagnosticStatusWrapper & stat); /** \brief processing time publisher. **/ - std::unique_ptr> stop_watch_ptr_; - std::unique_ptr debug_publisher_; + std::unique_ptr> stop_watch_ptr_; + std::unique_ptr debug_publisher_; }; } // namespace autoware::pointcloud_preprocessor diff --git a/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp index 1cc8fd2ff407f..a4e0370186826 100644 --- a/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp +++ b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp @@ -16,7 +16,7 @@ #define AUTOWARE__POINTCLOUD_PREPROCESSOR__DISTORTION_CORRECTOR__DISTORTION_CORRECTOR_HPP_ #include -#include +#include #include #include @@ -66,7 +66,7 @@ class DistortionCorrectorBase bool pointcloud_transform_needed_{false}; bool pointcloud_transform_exists_{false}; bool imu_transform_exists_{false}; - std::unique_ptr managed_tf_buffer_{nullptr}; + std::unique_ptr managed_tf_buffer_{nullptr}; std::deque twist_queue_; std::deque angular_velocity_queue_; @@ -88,7 +88,7 @@ class DistortionCorrectorBase : node_(node) { managed_tf_buffer_ = - std::make_unique(&node, has_static_tf_only); + std::make_unique(&node, has_static_tf_only); } virtual ~DistortionCorrectorBase() = default; diff --git a/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/distortion_corrector/distortion_corrector_node.hpp b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/distortion_corrector/distortion_corrector_node.hpp index 0188c1af97ab7..7c26c45ca0490 100644 --- a/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/distortion_corrector/distortion_corrector_node.hpp +++ b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/distortion_corrector/distortion_corrector_node.hpp @@ -17,9 +17,9 @@ #include "autoware/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp" -#include -#include -#include +#include +#include +#include #include #include @@ -41,17 +41,17 @@ class DistortionCorrectorComponent : public rclcpp::Node explicit DistortionCorrectorComponent(const rclcpp::NodeOptions & options); private: - autoware::universe_utils::InterProcessPollingSubscriber< - geometry_msgs::msg::TwistWithCovarianceStamped, - autoware::universe_utils::polling_policy::All>::SharedPtr twist_sub_; - autoware::universe_utils::InterProcessPollingSubscriber< - sensor_msgs::msg::Imu, autoware::universe_utils::polling_policy::All>::SharedPtr imu_sub_; + autoware_utils::InterProcessPollingSubscriber< + geometry_msgs::msg::TwistWithCovarianceStamped, autoware_utils::polling_policy::All>::SharedPtr + twist_sub_; + autoware_utils::InterProcessPollingSubscriber< + sensor_msgs::msg::Imu, autoware_utils::polling_policy::All>::SharedPtr imu_sub_; rclcpp::Subscription::SharedPtr pointcloud_sub_; rclcpp::Publisher::SharedPtr undistorted_pointcloud_pub_; - std::unique_ptr> stop_watch_ptr_; - std::unique_ptr debug_publisher_; + std::unique_ptr> stop_watch_ptr_; + std::unique_ptr debug_publisher_; std::string base_frame_; bool use_imu_; diff --git a/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/filter.hpp b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/filter.hpp index b39be3cfaf9e5..f43db051f3d67 100644 --- a/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/filter.hpp +++ b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/filter.hpp @@ -75,10 +75,10 @@ #include // Include tier4 autoware utils -#include -#include -#include -#include +#include +#include +#include +#include namespace autoware::pointcloud_preprocessor { @@ -176,9 +176,9 @@ class Filter : public rclcpp::Node std::mutex mutex_; /** \brief processing time publisher. **/ - std::unique_ptr> stop_watch_ptr_; - std::unique_ptr debug_publisher_; - std::unique_ptr published_time_publisher_; + std::unique_ptr> stop_watch_ptr_; + std::unique_ptr debug_publisher_; + std::unique_ptr published_time_publisher_; /** \brief Virtual abstract filter method. To be implemented by every child. * \param input the input point cloud dataset. @@ -238,7 +238,7 @@ class Filter : public rclcpp::Node * versus an exact one (false by default). */ bool approximate_sync_ = false; - std::unique_ptr managed_tf_buffer_{nullptr}; + std::unique_ptr managed_tf_buffer_{nullptr}; inline bool isValid( const PointCloud2ConstPtr & cloud, const std::string & /*topic_name*/ = "input") diff --git a/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/time_synchronizer/time_synchronizer_node.hpp b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/time_synchronizer/time_synchronizer_node.hpp index f4921be66a7e2..62f8e9f49806d 100644 --- a/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/time_synchronizer/time_synchronizer_node.hpp +++ b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/time_synchronizer/time_synchronizer_node.hpp @@ -62,9 +62,9 @@ // ROS includes #include -#include -#include -#include +#include +#include +#include #include #include @@ -147,7 +147,7 @@ class PointCloudDataSynchronizerComponent : public rclcpp::Node // XmlRpc::XmlRpcValue input_topics_; std::vector input_topics_; - std::unique_ptr managed_tf_buffer_{nullptr}; + std::unique_ptr managed_tf_buffer_{nullptr}; std::deque twist_ptr_queue_; @@ -179,8 +179,8 @@ class PointCloudDataSynchronizerComponent : public rclcpp::Node const std::string & original_topic_name, const std::string & postfix); /** \brief processing time publisher. **/ - std::unique_ptr> stop_watch_ptr_; - std::unique_ptr debug_publisher_; + std::unique_ptr> stop_watch_ptr_; + std::unique_ptr debug_publisher_; }; } // namespace autoware::pointcloud_preprocessor diff --git a/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/vector_map_filter/lanelet2_map_filter_node.hpp b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/vector_map_filter/lanelet2_map_filter_node.hpp index 5572a9a6f4588..172ed8c985f4a 100644 --- a/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/vector_map_filter/lanelet2_map_filter_node.hpp +++ b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/vector_map_filter/lanelet2_map_filter_node.hpp @@ -15,9 +15,9 @@ #ifndef AUTOWARE__POINTCLOUD_PREPROCESSOR__VECTOR_MAP_FILTER__LANELET2_MAP_FILTER_NODE_HPP_ #define AUTOWARE__POINTCLOUD_PREPROCESSOR__VECTOR_MAP_FILTER__LANELET2_MAP_FILTER_NODE_HPP_ -#include #include #include +#include #include #include @@ -40,9 +40,9 @@ #include #include -using autoware::universe_utils::LinearRing2d; -using autoware::universe_utils::MultiPoint2d; -using autoware::universe_utils::Point2d; +using autoware_utils::LinearRing2d; +using autoware_utils::MultiPoint2d; +using autoware_utils::Point2d; namespace autoware::pointcloud_preprocessor { diff --git a/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/vector_map_filter/vector_map_inside_area_filter_node.hpp b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/vector_map_filter/vector_map_inside_area_filter_node.hpp index 690be042dcd37..10502277aad59 100644 --- a/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/vector_map_filter/vector_map_inside_area_filter_node.hpp +++ b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/vector_map_filter/vector_map_inside_area_filter_node.hpp @@ -18,16 +18,16 @@ #include "autoware/pointcloud_preprocessor/filter.hpp" #include "autoware/pointcloud_preprocessor/utility/geometry.hpp" -#include #include #include +#include #include #include #include -using autoware::universe_utils::MultiPoint2d; +using autoware_utils::MultiPoint2d; namespace autoware::pointcloud_preprocessor { diff --git a/sensing/autoware_pointcloud_preprocessor/package.xml b/sensing/autoware_pointcloud_preprocessor/package.xml index 8fb95f0d37c34..d9df00e08d257 100644 --- a/sensing/autoware_pointcloud_preprocessor/package.xml +++ b/sensing/autoware_pointcloud_preprocessor/package.xml @@ -30,7 +30,7 @@ autoware_lanelet2_extension autoware_pcl_extensions autoware_point_types - autoware_universe_utils + autoware_utils cgal cv_bridge diagnostic_updater diff --git a/sensing/autoware_pointcloud_preprocessor/src/concatenate_data/combine_cloud_handler.cpp b/sensing/autoware_pointcloud_preprocessor/src/concatenate_data/combine_cloud_handler.cpp index 8574e3e8a35ae..1d2eac5044d06 100644 --- a/sensing/autoware_pointcloud_preprocessor/src/concatenate_data/combine_cloud_handler.cpp +++ b/sensing/autoware_pointcloud_preprocessor/src/concatenate_data/combine_cloud_handler.cpp @@ -40,7 +40,7 @@ CombineCloudHandler::CombineCloudHandler( publish_synchronized_pointcloud_(publish_synchronized_pointcloud), keep_input_frame_in_synchronized_pointcloud_(keep_input_frame_in_synchronized_pointcloud), managed_tf_buffer_( - std::make_unique(&node_, has_static_tf_only)) + std::make_unique(&node_, has_static_tf_only)) { } @@ -219,7 +219,7 @@ ConcatenatedCloudResult CombineCloudHandler::combine_pointclouds( convert_to_xyzirc_cloud(cloud, xyzirc_cloud); auto transformed_cloud_ptr = std::make_shared(); - managed_tf_buffer_->transformPointcloud(output_frame_, *xyzirc_cloud, *transformed_cloud_ptr); + managed_tf_buffer_->transform_pointcloud(output_frame_, *xyzirc_cloud, *transformed_cloud_ptr); concatenate_cloud_result.topic_to_original_stamp_map[topic] = rclcpp::Time(cloud->header.stamp).seconds(); @@ -249,7 +249,7 @@ ConcatenatedCloudResult CombineCloudHandler::combine_pointclouds( if (keep_input_frame_in_synchronized_pointcloud_ && need_transform_to_sensor_frame) { auto transformed_cloud_ptr_in_sensor_frame = std::make_shared(); - managed_tf_buffer_->transformPointcloud( + managed_tf_buffer_->transform_pointcloud( cloud->header.frame_id, *transformed_delay_compensated_cloud_ptr, *transformed_cloud_ptr_in_sensor_frame); transformed_cloud_ptr_in_sensor_frame->header.stamp = oldest_stamp; diff --git a/sensing/autoware_pointcloud_preprocessor/src/concatenate_data/concatenate_and_time_sync_node.cpp b/sensing/autoware_pointcloud_preprocessor/src/concatenate_data/concatenate_and_time_sync_node.cpp index 693da939ed196..26ada764d48c5 100644 --- a/sensing/autoware_pointcloud_preprocessor/src/concatenate_data/concatenate_and_time_sync_node.cpp +++ b/sensing/autoware_pointcloud_preprocessor/src/concatenate_data/concatenate_and_time_sync_node.cpp @@ -39,8 +39,8 @@ PointCloudConcatenateDataSynchronizerComponent::PointCloudConcatenateDataSynchro : Node("point_cloud_concatenator_component", node_options) { // initialize debug tool - using autoware::universe_utils::DebugPublisher; - using autoware::universe_utils::StopWatch; + using autoware_utils::DebugPublisher; + using autoware_utils::StopWatch; stop_watch_ptr_ = std::make_unique>(); debug_publisher_ = std::make_unique(this, "concatenate_data_synchronizer"); stop_watch_ptr_->tic("cyclic_time"); diff --git a/sensing/autoware_pointcloud_preprocessor/src/concatenate_data/concatenate_pointclouds.cpp b/sensing/autoware_pointcloud_preprocessor/src/concatenate_data/concatenate_pointclouds.cpp index ba5e2eb51997a..3634d593a32bf 100644 --- a/sensing/autoware_pointcloud_preprocessor/src/concatenate_data/concatenate_pointclouds.cpp +++ b/sensing/autoware_pointcloud_preprocessor/src/concatenate_data/concatenate_pointclouds.cpp @@ -39,8 +39,8 @@ PointCloudConcatenationComponent::PointCloudConcatenationComponent( { // initialize debug tool { - using autoware::universe_utils::DebugPublisher; - using autoware::universe_utils::StopWatch; + using autoware_utils::DebugPublisher; + using autoware_utils::StopWatch; stop_watch_ptr_ = std::make_unique>(); debug_publisher_ = std::make_unique(this, "concatenate_pointclouds_debug"); stop_watch_ptr_->tic("cyclic_time"); @@ -96,7 +96,7 @@ PointCloudConcatenationComponent::PointCloudConcatenationComponent( // tf2 listener { managed_tf_buffer_ = - std::make_unique(this, has_static_tf_only_); + std::make_unique(this, has_static_tf_only_); } // Output Publishers @@ -240,7 +240,7 @@ void PointCloudConcatenationComponent::combineClouds( // transform to output frame sensor_msgs::msg::PointCloud2::SharedPtr transformed_cloud_ptr( new sensor_msgs::msg::PointCloud2()); - managed_tf_buffer_->transformPointcloud(output_frame_, *e.second, *transformed_cloud_ptr); + managed_tf_buffer_->transform_pointcloud(output_frame_, *e.second, *transformed_cloud_ptr); // concatenate if (concat_cloud_ptr == nullptr) { diff --git a/sensing/autoware_pointcloud_preprocessor/src/crop_box_filter/crop_box_filter_node.cpp b/sensing/autoware_pointcloud_preprocessor/src/crop_box_filter/crop_box_filter_node.cpp index 54c940d414db8..8dc60eff7a74a 100644 --- a/sensing/autoware_pointcloud_preprocessor/src/crop_box_filter/crop_box_filter_node.cpp +++ b/sensing/autoware_pointcloud_preprocessor/src/crop_box_filter/crop_box_filter_node.cpp @@ -63,8 +63,8 @@ CropBoxFilterComponent::CropBoxFilterComponent(const rclcpp::NodeOptions & optio { // initialize debug tool { - using autoware::universe_utils::DebugPublisher; - using autoware::universe_utils::StopWatch; + using autoware_utils::DebugPublisher; + using autoware_utils::StopWatch; stop_watch_ptr_ = std::make_unique>(); debug_publisher_ = std::make_unique(this, this->get_name()); stop_watch_ptr_->tic("cyclic_time"); diff --git a/sensing/autoware_pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp b/sensing/autoware_pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp index 8391894a4ada6..a010810784300 100644 --- a/sensing/autoware_pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp +++ b/sensing/autoware_pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp @@ -15,9 +15,9 @@ #include "autoware/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp" #include "autoware/pointcloud_preprocessor/utility/memory.hpp" -#include "autoware/universe_utils/math/constants.hpp" +#include "autoware_utils/math/constants.hpp" -#include +#include #include #include @@ -90,7 +90,7 @@ void DistortionCorrectorBase::get_imu_transformation( Eigen::Matrix4f eigen_imu_to_base_link; imu_transform_exists_ = - managed_tf_buffer_->getTransform(base_frame, imu_frame, eigen_imu_to_base_link); + managed_tf_buffer_->get_transform(base_frame, imu_frame, eigen_imu_to_base_link); tf2::Transform tf2_imu_to_base_link = convert_matrix_to_transform(eigen_imu_to_base_link); geometry_imu_to_base_link_ptr_ = std::make_shared(); @@ -218,8 +218,8 @@ std::optional DistortionCorrectorBase::try_compute_angle_conver for (; next_it_x != it_x.end(); ++it_x, ++it_y, ++it_azimuth, ++next_it_x, ++next_it_y, ++next_it_azimuth) { - auto current_cartesian_rad = autoware::universe_utils::opencv_fast_atan2(*it_y, *it_x); - auto next_cartesian_rad = autoware::universe_utils::opencv_fast_atan2(*next_it_y, *next_it_x); + auto current_cartesian_rad = autoware_utils::opencv_fast_atan2(*it_y, *it_x); + auto next_cartesian_rad = autoware_utils::opencv_fast_atan2(*next_it_y, *next_it_x); // If the angle exceeds 180 degrees, it may cross the 0-degree axis, // which could disrupt the calculation of the formula. @@ -233,13 +233,12 @@ std::optional DistortionCorrectorBase::try_compute_angle_conver } // restrict the angle difference between [-180, 180] (degrees) - float azimuth_diff = - std::abs(*next_it_azimuth - *it_azimuth) > autoware::universe_utils::pi - ? std::abs(*next_it_azimuth - *it_azimuth) - 2 * autoware::universe_utils::pi - : *next_it_azimuth - *it_azimuth; + float azimuth_diff = std::abs(*next_it_azimuth - *it_azimuth) > autoware_utils::pi + ? std::abs(*next_it_azimuth - *it_azimuth) - 2 * autoware_utils::pi + : *next_it_azimuth - *it_azimuth; float cartesian_rad_diff = - std::abs(next_cartesian_rad - current_cartesian_rad) > autoware::universe_utils::pi - ? std::abs(next_cartesian_rad - current_cartesian_rad) - 2 * autoware::universe_utils::pi + std::abs(next_cartesian_rad - current_cartesian_rad) > autoware_utils::pi + ? std::abs(next_cartesian_rad - current_cartesian_rad) - 2 * autoware_utils::pi : next_cartesian_rad - current_cartesian_rad; float sign = azimuth_diff / cartesian_rad_diff; @@ -258,9 +257,9 @@ std::optional DistortionCorrectorBase::try_compute_angle_conver float offset_rad = *it_azimuth - sign * current_cartesian_rad; // Check if 'offset_rad' can be adjusted to offset_rad multiple of π/2 - int multiple_of_90_degrees = std::round(offset_rad / (autoware::universe_utils::pi / 2)); + int multiple_of_90_degrees = std::round(offset_rad / (autoware_utils::pi / 2)); if ( - std::abs(offset_rad - multiple_of_90_degrees * (autoware::universe_utils::pi / 2)) > + std::abs(offset_rad - multiple_of_90_degrees * (autoware_utils::pi / 2)) > angle_conversion.offset_rad_threshold) { RCLCPP_DEBUG_STREAM_THROTTLE( node_.get_logger(), *node_.get_clock(), 10000 /* ms */, @@ -272,7 +271,7 @@ std::optional DistortionCorrectorBase::try_compute_angle_conver // Limit the range of offset_rad in [0, 360) multiple_of_90_degrees = (multiple_of_90_degrees % 4 + 4) % 4; - angle_conversion.offset_rad = multiple_of_90_degrees * (autoware::universe_utils::pi / 2); + angle_conversion.offset_rad = multiple_of_90_degrees * (autoware_utils::pi / 2); return angle_conversion; } @@ -388,14 +387,13 @@ void DistortionCorrector::undistort_pointcloud( "updated. " "Please change the input pointcloud or set update_azimuth_and_distance to false."); } - float cartesian_coordinate_azimuth = - autoware::universe_utils::opencv_fast_atan2(*it_y, *it_x); + float cartesian_coordinate_azimuth = autoware_utils::opencv_fast_atan2(*it_y, *it_x); float updated_azimuth = angle_conversion_opt->offset_rad + angle_conversion_opt->sign * cartesian_coordinate_azimuth; if (updated_azimuth < 0) { - updated_azimuth += autoware::universe_utils::pi * 2; - } else if (updated_azimuth > 2 * autoware::universe_utils::pi) { - updated_azimuth -= autoware::universe_utils::pi * 2; + updated_azimuth += autoware_utils::pi * 2; + } else if (updated_azimuth > 2 * autoware_utils::pi) { + updated_azimuth -= autoware_utils::pi * 2; } *it_azimuth = updated_azimuth; @@ -434,7 +432,7 @@ void DistortionCorrector2D::set_pointcloud_transform( Eigen::Matrix4f eigen_lidar_to_base_link; pointcloud_transform_exists_ = - managed_tf_buffer_->getTransform(base_frame, lidar_frame, eigen_lidar_to_base_link); + managed_tf_buffer_->get_transform(base_frame, lidar_frame, eigen_lidar_to_base_link); tf2_lidar_to_base_link_ = convert_matrix_to_transform(eigen_lidar_to_base_link); tf2_base_link_to_lidar_ = tf2_lidar_to_base_link_.inverse(); pointcloud_transform_needed_ = base_frame != lidar_frame && pointcloud_transform_exists_; @@ -448,7 +446,7 @@ void DistortionCorrector3D::set_pointcloud_transform( } pointcloud_transform_exists_ = - managed_tf_buffer_->getTransform(base_frame, lidar_frame, eigen_lidar_to_base_link_); + managed_tf_buffer_->get_transform(base_frame, lidar_frame, eigen_lidar_to_base_link_); eigen_base_link_to_lidar_ = eigen_lidar_to_base_link_.inverse(); pointcloud_transform_needed_ = base_frame != lidar_frame && pointcloud_transform_exists_; } @@ -478,8 +476,8 @@ inline void DistortionCorrector2D::undistort_point_implementation( point_tf_ = tf2_lidar_to_base_link_ * point_tf_; } theta_ += w * time_offset; - auto [sin_half_theta, cos_half_theta] = autoware::universe_utils::sin_and_cos(theta_ * 0.5f); - auto [sin_theta, cos_theta] = autoware::universe_utils::sin_and_cos(theta_); + auto [sin_half_theta, cos_half_theta] = autoware_utils::sin_and_cos(theta_ * 0.5f); + auto [sin_theta, cos_theta] = autoware_utils::sin_and_cos(theta_); baselink_quat_.setValue( 0, 0, sin_half_theta, cos_half_theta); // baselink_quat.setRPY(0.0, 0.0, theta); (Note that the diff --git a/sensing/autoware_pointcloud_preprocessor/src/distortion_corrector/distortion_corrector_node.cpp b/sensing/autoware_pointcloud_preprocessor/src/distortion_corrector/distortion_corrector_node.cpp index 4c0227de7c701..2867d0de9e856 100644 --- a/sensing/autoware_pointcloud_preprocessor/src/distortion_corrector/distortion_corrector_node.cpp +++ b/sensing/autoware_pointcloud_preprocessor/src/distortion_corrector/distortion_corrector_node.cpp @@ -29,8 +29,8 @@ DistortionCorrectorComponent::DistortionCorrectorComponent(const rclcpp::NodeOpt { // initialize debug tool - using universe_utils::DebugPublisher; - using universe_utils::StopWatch; + using autoware_utils::DebugPublisher; + using autoware_utils::StopWatch; stop_watch_ptr_ = std::make_unique>(); debug_publisher_ = std::make_unique(this, "distortion_corrector"); stop_watch_ptr_->tic("cyclic_time"); @@ -59,11 +59,11 @@ DistortionCorrectorComponent::DistortionCorrectorComponent(const rclcpp::NodeOpt const uint16_t TWIST_QUEUE_SIZE = 100; // Subscriber - twist_sub_ = universe_utils::InterProcessPollingSubscriber< - geometry_msgs::msg::TwistWithCovarianceStamped, universe_utils::polling_policy::All>:: + twist_sub_ = autoware_utils::InterProcessPollingSubscriber< + geometry_msgs::msg::TwistWithCovarianceStamped, autoware_utils::polling_policy::All>:: create_subscription(this, "~/input/twist", rclcpp::QoS(TWIST_QUEUE_SIZE)); - imu_sub_ = universe_utils::InterProcessPollingSubscriber< - sensor_msgs::msg::Imu, universe_utils::polling_policy::All>:: + imu_sub_ = autoware_utils::InterProcessPollingSubscriber< + sensor_msgs::msg::Imu, autoware_utils::polling_policy::All>:: create_subscription(this, "~/input/imu", rclcpp::QoS(TWIST_QUEUE_SIZE)); pointcloud_sub_ = this->create_subscription( "~/input/pointcloud", rclcpp::SensorDataQoS(), @@ -89,13 +89,13 @@ void DistortionCorrectorComponent::pointcloud_callback(PointCloud2::UniquePtr po } std::vector twist_msgs = - twist_sub_->takeData(); + twist_sub_->take_data(); for (const auto & msg : twist_msgs) { distortion_corrector_->process_twist_message(msg); } if (use_imu_) { - std::vector imu_msgs = imu_sub_->takeData(); + std::vector imu_msgs = imu_sub_->take_data(); for (const auto & msg : imu_msgs) { distortion_corrector_->process_imu_message(base_frame_, msg); } diff --git a/sensing/autoware_pointcloud_preprocessor/src/downsample_filter/pickup_based_voxel_grid_downsample_filter_node.cpp b/sensing/autoware_pointcloud_preprocessor/src/downsample_filter/pickup_based_voxel_grid_downsample_filter_node.cpp index 996a7f8206728..eb6814870d8dc 100644 --- a/sensing/autoware_pointcloud_preprocessor/src/downsample_filter/pickup_based_voxel_grid_downsample_filter_node.cpp +++ b/sensing/autoware_pointcloud_preprocessor/src/downsample_filter/pickup_based_voxel_grid_downsample_filter_node.cpp @@ -60,8 +60,8 @@ PickupBasedVoxelGridDownsampleFilterComponent::PickupBasedVoxelGridDownsampleFil { // initialize debug tool { - using autoware::universe_utils::DebugPublisher; - using autoware::universe_utils::StopWatch; + using autoware_utils::DebugPublisher; + using autoware_utils::StopWatch; stop_watch_ptr_ = std::make_unique>(); debug_publisher_ = std::make_unique(this, this->get_name()); stop_watch_ptr_->tic("cyclic_time"); diff --git a/sensing/autoware_pointcloud_preprocessor/src/filter.cpp b/sensing/autoware_pointcloud_preprocessor/src/filter.cpp index 62495fc8b70cb..03ce774df6c8c 100644 --- a/sensing/autoware_pointcloud_preprocessor/src/filter.cpp +++ b/sensing/autoware_pointcloud_preprocessor/src/filter.cpp @@ -107,8 +107,7 @@ autoware::pointcloud_preprocessor::Filter::Filter( set_param_res_filter_ = this->add_on_set_parameters_callback( std::bind(&Filter::filterParamCallback, this, std::placeholders::_1)); - published_time_publisher_ = - std::make_unique(this); + published_time_publisher_ = std::make_unique(this); RCLCPP_DEBUG(this->get_logger(), "[Filter Constructor] successfully created."); } @@ -125,7 +124,7 @@ void autoware::pointcloud_preprocessor::Filter::setupTF() has_static_tf_only_ = true; } managed_tf_buffer_ = - std::make_unique(this, has_static_tf_only_); + std::make_unique(this, has_static_tf_only_); } ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// @@ -280,7 +279,7 @@ void autoware::pointcloud_preprocessor::Filter::input_indices_callback( // Convert the cloud into the different frame PointCloud2 cloud_transformed; - if (!managed_tf_buffer_->transformPointcloud(tf_input_frame_, *cloud, cloud_transformed)) { + if (!managed_tf_buffer_->transform_pointcloud(tf_input_frame_, *cloud, cloud_transformed)) { return; } cloud_tf = std::make_shared(cloud_transformed); @@ -309,7 +308,7 @@ bool autoware::pointcloud_preprocessor::Filter::calculate_transform_matrix( this->get_logger(), "[get_transform_matrix] Transforming input dataset from %s to %s.", from.header.frame_id.c_str(), target_frame.c_str()); - if (!managed_tf_buffer_->getTransform( + if (!managed_tf_buffer_->get_transform( target_frame, from.header.frame_id, transform_info.eigen_transform)) { return false; } @@ -332,7 +331,7 @@ bool autoware::pointcloud_preprocessor::Filter::convert_output_costly( // Convert the cloud into the different frame auto cloud_transformed = std::make_unique(); - if (!managed_tf_buffer_->transformPointcloud(tf_output_frame_, *output, *cloud_transformed)) { + if (!managed_tf_buffer_->transform_pointcloud(tf_output_frame_, *output, *cloud_transformed)) { RCLCPP_ERROR( this->get_logger(), "[convert_output_costly] Error converting output dataset from %s to %s.", @@ -352,7 +351,7 @@ bool autoware::pointcloud_preprocessor::Filter::convert_output_costly( auto cloud_transformed = std::make_unique(); - if (!managed_tf_buffer_->transformPointcloud( + if (!managed_tf_buffer_->transform_pointcloud( tf_input_orig_frame_, *output, *cloud_transformed)) { return false; } diff --git a/sensing/autoware_pointcloud_preprocessor/src/outlier_filter/ring_outlier_filter_node.cpp b/sensing/autoware_pointcloud_preprocessor/src/outlier_filter/ring_outlier_filter_node.cpp index fcea299ba5aff..7756ba3b6bd91 100644 --- a/sensing/autoware_pointcloud_preprocessor/src/outlier_filter/ring_outlier_filter_node.cpp +++ b/sensing/autoware_pointcloud_preprocessor/src/outlier_filter/ring_outlier_filter_node.cpp @@ -31,8 +31,8 @@ RingOutlierFilterComponent::RingOutlierFilterComponent(const rclcpp::NodeOptions { // initialize debug tool { - using autoware::universe_utils::DebugPublisher; - using autoware::universe_utils::StopWatch; + using autoware_utils::DebugPublisher; + using autoware_utils::StopWatch; stop_watch_ptr_ = std::make_unique>(); debug_publisher_ = std::make_unique(this, "ring_outlier_filter"); { diff --git a/sensing/autoware_pointcloud_preprocessor/src/time_synchronizer/time_synchronizer_node.cpp b/sensing/autoware_pointcloud_preprocessor/src/time_synchronizer/time_synchronizer_node.cpp index e5bd4a955590f..9bb2ed7176d8d 100644 --- a/sensing/autoware_pointcloud_preprocessor/src/time_synchronizer/time_synchronizer_node.cpp +++ b/sensing/autoware_pointcloud_preprocessor/src/time_synchronizer/time_synchronizer_node.cpp @@ -47,8 +47,8 @@ PointCloudDataSynchronizerComponent::PointCloudDataSynchronizerComponent( { // initialize debug tool { - using autoware::universe_utils::DebugPublisher; - using autoware::universe_utils::StopWatch; + using autoware_utils::DebugPublisher; + using autoware_utils::StopWatch; stop_watch_ptr_ = std::make_unique>(); debug_publisher_ = std::make_unique(this, "time_synchronizer"); stop_watch_ptr_->tic("cyclic_time"); @@ -113,7 +113,7 @@ PointCloudDataSynchronizerComponent::PointCloudDataSynchronizerComponent( // tf2 listener { managed_tf_buffer_ = - std::make_unique(this, has_static_tf_only_); + std::make_unique(this, has_static_tf_only_); } // Subscribers @@ -324,7 +324,7 @@ PointCloudDataSynchronizerComponent::synchronizeClouds() continue; } // transform pointcloud to output frame - managed_tf_buffer_->transformPointcloud(output_frame_, *e.second, *transformed_cloud_ptr); + managed_tf_buffer_->transform_pointcloud(output_frame_, *e.second, *transformed_cloud_ptr); // calculate transforms to oldest stamp and transform pointcloud to oldest stamp Eigen::Matrix4f adjust_to_old_data_transform = Eigen::Matrix4f::Identity(); @@ -344,7 +344,7 @@ PointCloudDataSynchronizerComponent::synchronizeClouds() sensor_msgs::msg::PointCloud2::SharedPtr transformed_delay_compensated_cloud_ptr_in_input_frame( new sensor_msgs::msg::PointCloud2()); - managed_tf_buffer_->transformPointcloud( + managed_tf_buffer_->transform_pointcloud( e.second->header.frame_id, *transformed_delay_compensated_cloud_ptr, *transformed_delay_compensated_cloud_ptr_in_input_frame); transformed_delay_compensated_cloud_ptr = diff --git a/sensing/autoware_pointcloud_preprocessor/src/vector_map_filter/vector_map_inside_area_filter_node.cpp b/sensing/autoware_pointcloud_preprocessor/src/vector_map_filter/vector_map_inside_area_filter_node.cpp index 310b86526193c..68b53bd553353 100644 --- a/sensing/autoware_pointcloud_preprocessor/src/vector_map_filter/vector_map_inside_area_filter_node.cpp +++ b/sensing/autoware_pointcloud_preprocessor/src/vector_map_filter/vector_map_inside_area_filter_node.cpp @@ -20,19 +20,18 @@ namespace { -autoware::universe_utils::Box2d calcBoundingBox( - const pcl::PointCloud::ConstPtr & input_cloud) +autoware_utils::Box2d calcBoundingBox(const pcl::PointCloud::ConstPtr & input_cloud) { MultiPoint2d candidate_points; for (const auto & p : input_cloud->points) { candidate_points.emplace_back(p.x, p.y); } - return boost::geometry::return_envelope(candidate_points); + return boost::geometry::return_envelope(candidate_points); } lanelet::ConstPolygons3d calcIntersectedPolygons( - const autoware::universe_utils::Box2d & bounding_box, const lanelet::ConstPolygons3d & polygons) + const autoware_utils::Box2d & bounding_box, const lanelet::ConstPolygons3d & polygons) { lanelet::ConstPolygons3d intersected_polygons; for (const auto & polygon : polygons) { diff --git a/sensing/autoware_pointcloud_preprocessor/test/test_distortion_corrector_node.cpp b/sensing/autoware_pointcloud_preprocessor/test/test_distortion_corrector_node.cpp index aba3c354473eb..3ce80843d64ac 100644 --- a/sensing/autoware_pointcloud_preprocessor/test/test_distortion_corrector_node.cpp +++ b/sensing/autoware_pointcloud_preprocessor/test/test_distortion_corrector_node.cpp @@ -26,8 +26,8 @@ // 10.09, 10.117, 10.144, 10.171, 10.198, 10.225 #include "autoware/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp" -#include "autoware/universe_utils/math/constants.hpp" -#include "autoware/universe_utils/math/trigonometry.hpp" +#include "autoware_utils/math/constants.hpp" +#include "autoware_utils/math/trigonometry.hpp" #include @@ -197,19 +197,19 @@ class DistortionCorrectorTest : public ::testing::Test if (coordinate_system == AngleCoordinateSystem::VELODYNE) { // velodyne coordinates: x-axis is 0 degrees, y-axis is 270 degrees, angle increase in // clockwise direction - float cartesian_deg = std::atan2(point.y(), point.x()) * 180 / autoware::universe_utils::pi; + float cartesian_deg = std::atan2(point.y(), point.x()) * 180 / autoware_utils::pi; if (cartesian_deg < 0) cartesian_deg += 360; float velodyne_deg = 360 - cartesian_deg; if (velodyne_deg == 360) velodyne_deg = 0; - default_azimuths.push_back(velodyne_deg * autoware::universe_utils::pi / 180); + default_azimuths.push_back(velodyne_deg * autoware_utils::pi / 180); } else if (coordinate_system == AngleCoordinateSystem::HESAI) { // hesai coordinates: y-axis is 0 degrees, x-axis is 90 degrees, angle increase in clockwise // direction - float cartesian_deg = std::atan2(point.y(), point.x()) * 180 / autoware::universe_utils::pi; + float cartesian_deg = std::atan2(point.y(), point.x()) * 180 / autoware_utils::pi; if (cartesian_deg < 0) cartesian_deg += 360; float hesai_deg = 90 - cartesian_deg < 0 ? 90 - cartesian_deg + 360 : 90 - cartesian_deg; if (hesai_deg == 360) hesai_deg = 0; - default_azimuths.push_back(hesai_deg * autoware::universe_utils::pi / 180); + default_azimuths.push_back(hesai_deg * autoware_utils::pi / 180); } else if (coordinate_system == AngleCoordinateSystem::CARTESIAN) { // Cartesian coordinates: x-axis is 0 degrees, y-axis is 90 degrees, angle increase in // counterclockwise direction @@ -884,9 +884,7 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointcloudWithPureRotationalMotion) // Set the quaternion for the current angle tf2::Quaternion quaternion; - quaternion.setValue( - 0, 0, autoware::universe_utils::sin(angle * 0.5f), - autoware::universe_utils::cos(angle * 0.5f)); + quaternion.setValue(0, 0, autoware_utils::sin(angle * 0.5f), autoware_utils::cos(angle * 0.5f)); tf2::Vector3 point(*iter_x, *iter_y, *iter_z); tf2::Vector3 rotated_point = tf2::quatRotate(quaternion, point); @@ -1173,8 +1171,7 @@ TEST_F(DistortionCorrectorTest, TestTryComputeAngleConversionOnVelodynePointclou Eigen::Vector3f(1.0f, -1.0f, 1.0f), Eigen::Vector3f(0.0f, -2.0f, 1.0f), }; - std::vector velodyne_azimuths = { - 0.0f, autoware::universe_utils::pi / 4, autoware::universe_utils::pi / 2}; + std::vector velodyne_azimuths = {0.0f, autoware_utils::pi / 4, autoware_utils::pi / 2}; auto velodyne_pointcloud = generate_pointcloud_msg(true, timestamp, velodyne_points, velodyne_azimuths); @@ -1196,8 +1193,7 @@ TEST_F(DistortionCorrectorTest, TestTryComputeAngleConversionOnHesaiPointcloud) Eigen::Vector3f(0.0f, -2.0f, 1.0f), }; std::vector hesai_azimuths = { - autoware::universe_utils::pi / 2, autoware::universe_utils::pi * 3 / 4, - autoware::universe_utils::pi}; + autoware_utils::pi / 2, autoware_utils::pi * 3 / 4, autoware_utils::pi}; auto hesai_pointcloud = generate_pointcloud_msg(true, timestamp, hesai_points, hesai_azimuths); auto angle_conversion_opt = @@ -1205,8 +1201,7 @@ TEST_F(DistortionCorrectorTest, TestTryComputeAngleConversionOnHesaiPointcloud) EXPECT_TRUE(angle_conversion_opt.has_value()); EXPECT_EQ(angle_conversion_opt->sign, -1); - EXPECT_NEAR( - angle_conversion_opt->offset_rad, autoware::universe_utils::pi / 2, standard_tolerance); + EXPECT_NEAR(angle_conversion_opt->offset_rad, autoware_utils::pi / 2, standard_tolerance); } TEST_F(DistortionCorrectorTest, TestTryComputeAngleConversionCartesianPointcloud) @@ -1221,8 +1216,7 @@ TEST_F(DistortionCorrectorTest, TestTryComputeAngleConversionCartesianPointcloud Eigen::Vector3f(1.0f, 1.0f, 1.0f), Eigen::Vector3f(0.0f, 2.0f, 1.0f), }; - std::vector cartesian_azimuths = { - 0, autoware::universe_utils::pi / 4, autoware::universe_utils::pi / 2}; + std::vector cartesian_azimuths = {0, autoware_utils::pi / 4, autoware_utils::pi / 2}; auto cartesian_pointcloud = generate_pointcloud_msg(true, timestamp, cartesian_points, cartesian_azimuths); @@ -1246,16 +1240,14 @@ TEST_F(DistortionCorrectorTest, TestTryComputeAngleConversionOnRandomPointcloud) Eigen::Vector3f(2.0f, 0.0f, 1.0f), Eigen::Vector3f(1.0f, 1.0f, 1.0f), }; - std::vector azimuths = { - 0, autoware::universe_utils::pi * 3 / 2, autoware::universe_utils::pi * 7 / 4}; + std::vector azimuths = {0, autoware_utils::pi * 3 / 2, autoware_utils::pi * 7 / 4}; auto pointcloud = generate_pointcloud_msg(true, timestamp, points, azimuths); auto angle_conversion_opt = distortion_corrector_2d_->try_compute_angle_conversion(pointcloud); EXPECT_TRUE(angle_conversion_opt.has_value()); EXPECT_EQ(angle_conversion_opt->sign, 1); - EXPECT_NEAR( - angle_conversion_opt->offset_rad, autoware::universe_utils::pi * 3 / 2, standard_tolerance); + EXPECT_NEAR(angle_conversion_opt->offset_rad, autoware_utils::pi * 3 / 2, standard_tolerance); } TEST_F(DistortionCorrectorTest, TestTryComputeAngleConversionOnBadAzimuthPointcloud) @@ -1274,7 +1266,7 @@ TEST_F(DistortionCorrectorTest, TestTryComputeAngleConversionOnBadAzimuthPointcl }; // generate random bad azimuths - std::vector azimuths = {0, 0, autoware::universe_utils::pi}; + std::vector azimuths = {0, 0, autoware_utils::pi}; auto pointcloud = generate_pointcloud_msg(true, timestamp, points, azimuths); auto angle_conversion_opt = distortion_corrector_2d_->try_compute_angle_conversion(pointcloud); diff --git a/sensing/autoware_radar_static_pointcloud_filter/package.xml b/sensing/autoware_radar_static_pointcloud_filter/package.xml index 10ab1e8d663bc..87c6314d80958 100644 --- a/sensing/autoware_radar_static_pointcloud_filter/package.xml +++ b/sensing/autoware_radar_static_pointcloud_filter/package.xml @@ -13,7 +13,7 @@ ament_cmake_auto autoware_cmake - autoware_universe_utils + autoware_utils message_filters nav_msgs radar_msgs diff --git a/sensing/autoware_radar_static_pointcloud_filter/src/radar_static_pointcloud_filter_node.cpp b/sensing/autoware_radar_static_pointcloud_filter/src/radar_static_pointcloud_filter_node.cpp index 74c099b9bf32b..435cf901feab3 100644 --- a/sensing/autoware_radar_static_pointcloud_filter/src/radar_static_pointcloud_filter_node.cpp +++ b/sensing/autoware_radar_static_pointcloud_filter/src/radar_static_pointcloud_filter_node.cpp @@ -96,7 +96,7 @@ RadarStaticPointcloudFilterNode::RadarStaticPointcloudFilterNode( node_param_.doppler_velocity_sd = declare_parameter("doppler_velocity_sd"); // Subscriber - transform_listener_ = std::make_shared(this); + transform_listener_ = std::make_shared(this); sub_radar_.subscribe(this, "~/input/radar", rclcpp::QoS{1}.get_rmw_qos_profile()); sub_odometry_.subscribe(this, "~/input/odometry", rclcpp::QoS{1}.get_rmw_qos_profile()); @@ -132,7 +132,7 @@ void RadarStaticPointcloudFilterNode::onData( geometry_msgs::msg::TransformStamped::ConstSharedPtr transform; try { - transform = transform_listener_->getTransform( + transform = transform_listener_->get_transform( odom_msg->header.frame_id, radar_msg->header.frame_id, odom_msg->header.stamp, rclcpp::Duration::from_seconds(0.2)); } catch (tf2::TransformException & ex) { diff --git a/sensing/autoware_radar_static_pointcloud_filter/src/radar_static_pointcloud_filter_node.hpp b/sensing/autoware_radar_static_pointcloud_filter/src/radar_static_pointcloud_filter_node.hpp index eff8437eb51d2..0cfa7303bdfaf 100644 --- a/sensing/autoware_radar_static_pointcloud_filter/src/radar_static_pointcloud_filter_node.hpp +++ b/sensing/autoware_radar_static_pointcloud_filter/src/radar_static_pointcloud_filter_node.hpp @@ -15,7 +15,7 @@ #ifndef RADAR_STATIC_POINTCLOUD_FILTER_NODE_HPP_ #define RADAR_STATIC_POINTCLOUD_FILTER_NODE_HPP_ -#include "autoware/universe_utils/ros/transform_listener.hpp" +#include "autoware_utils/ros/transform_listener.hpp" #include @@ -51,7 +51,7 @@ class RadarStaticPointcloudFilterNode : public rclcpp::Node // Subscriber message_filters::Subscriber sub_radar_{}; message_filters::Subscriber sub_odometry_{}; - std::shared_ptr transform_listener_; + std::shared_ptr transform_listener_; using SyncPolicy = message_filters::sync_policies::ApproximateTime; using Sync = message_filters::Synchronizer; diff --git a/simulator/autoware_dummy_perception_publisher/package.xml b/simulator/autoware_dummy_perception_publisher/package.xml index e9dd3cb1614c9..a885602a70fc6 100644 --- a/simulator/autoware_dummy_perception_publisher/package.xml +++ b/simulator/autoware_dummy_perception_publisher/package.xml @@ -20,7 +20,7 @@ autoware_lint_common autoware_perception_msgs - autoware_universe_utils + autoware_utils libpcl-all-dev pcl_conversions rclcpp diff --git a/simulator/autoware_dummy_perception_publisher/src/node.cpp b/simulator/autoware_dummy_perception_publisher/src/node.cpp index 22797e252813c..46650e9a1f32d 100644 --- a/simulator/autoware_dummy_perception_publisher/src/node.cpp +++ b/simulator/autoware_dummy_perception_publisher/src/node.cpp @@ -14,7 +14,7 @@ #include "autoware/dummy_perception_publisher/node.hpp" -#include "autoware/universe_utils/geometry/geometry.hpp" +#include "autoware_utils/geometry/geometry.hpp" #include #include @@ -97,8 +97,7 @@ ObjectInfo::ObjectInfo( } } - const auto current_pose = - autoware::universe_utils::calcOffsetPose(initial_pose, move_distance, 0.0, 0.0); + const auto current_pose = autoware_utils::calc_offset_pose(initial_pose, move_distance, 0.0, 0.0); // calculate tf from map to moved_object geometry_msgs::msg::Transform ros_map2moved_object; diff --git a/simulator/autoware_fault_injection/package.xml b/simulator/autoware_fault_injection/package.xml index a0479ff49ee50..5593c844e75ca 100644 --- a/simulator/autoware_fault_injection/package.xml +++ b/simulator/autoware_fault_injection/package.xml @@ -11,7 +11,7 @@ ament_cmake_auto autoware_cmake - autoware_universe_utils + autoware_utils diagnostic_aggregator diagnostic_msgs diagnostic_updater diff --git a/simulator/autoware_fault_injection/src/fault_injection_node/fault_injection_node.cpp b/simulator/autoware_fault_injection/src/fault_injection_node/fault_injection_node.cpp index 5e65b4bdcc327..45000d9870d3b 100644 --- a/simulator/autoware_fault_injection/src/fault_injection_node/fault_injection_node.cpp +++ b/simulator/autoware_fault_injection/src/fault_injection_node/fault_injection_node.cpp @@ -14,7 +14,7 @@ #include "autoware/fault_injection/fault_injection_node.hpp" -#include +#include #include #include diff --git a/simulator/autoware_simple_planning_simulator/package.xml b/simulator/autoware_simple_planning_simulator/package.xml index 47ca399a9ad95..7749fc36b3ff0 100644 --- a/simulator/autoware_simple_planning_simulator/package.xml +++ b/simulator/autoware_simple_planning_simulator/package.xml @@ -23,7 +23,7 @@ autoware_map_msgs autoware_motion_utils autoware_planning_msgs - autoware_universe_utils + autoware_utils autoware_vehicle_info_utils autoware_vehicle_msgs geometry_msgs diff --git a/simulator/autoware_simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp b/simulator/autoware_simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp index b03de1906db78..f98d5f2281b72 100644 --- a/simulator/autoware_simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp +++ b/simulator/autoware_simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp @@ -17,9 +17,9 @@ #include "autoware/motion_utils/trajectory/trajectory.hpp" #include "autoware/simple_planning_simulator/vehicle_model/sim_model.hpp" #include "autoware/simple_planning_simulator/vehicle_model/sim_model_actuation_cmd.hpp" -#include "autoware/universe_utils/geometry/geometry.hpp" -#include "autoware/universe_utils/ros/msg_covariance.hpp" -#include "autoware/universe_utils/ros/update_param.hpp" +#include "autoware_utils/geometry/geometry.hpp" +#include "autoware_utils/ros/msg_covariance.hpp" +#include "autoware_utils/ros/update_param.hpp" #include "autoware_vehicle_info_utils/vehicle_info_utils.hpp" #include "rclcpp_components/register_node_macro.hpp" @@ -62,8 +62,8 @@ nav_msgs::msg::Odometry to_odometry( nav_msgs::msg::Odometry odometry; odometry.pose.pose.position.x = vehicle_model_ptr->getX(); odometry.pose.pose.position.y = vehicle_model_ptr->getY(); - odometry.pose.pose.orientation = autoware::universe_utils::createQuaternionFromRPY( - 0.0, ego_pitch_angle, vehicle_model_ptr->getYaw()); + odometry.pose.pose.orientation = + autoware_utils::create_quaternion_from_rpy(0.0, ego_pitch_angle, vehicle_model_ptr->getYaw()); odometry.twist.twist.linear.x = vehicle_model_ptr->getVx(); odometry.twist.twist.angular.z = vehicle_model_ptr->getWz(); @@ -403,8 +403,8 @@ rcl_interfaces::msg::SetParametersResult SimplePlanningSimulator::on_parameter( result.reason = "success"; try { - autoware::universe_utils::updateParam(parameters, "x_stddev", x_stddev_); - autoware::universe_utils::updateParam(parameters, "y_stddev", y_stddev_); + autoware_utils::update_param(parameters, "x_stddev", x_stddev_); + autoware_utils::update_param(parameters, "y_stddev", y_stddev_); } catch (const rclcpp::exceptions::InvalidParameterTypeException & e) { result.successful = false; result.reason = e.what(); @@ -422,7 +422,7 @@ double SimplePlanningSimulator::calculate_ego_pitch() const geometry_msgs::msg::Pose ego_pose; ego_pose.position.x = ego_x; ego_pose.position.y = ego_y; - ego_pose.orientation = autoware::universe_utils::createQuaternionFromYaw(ego_yaw); + ego_pose.orientation = autoware_utils::create_quaternion_from_yaw(ego_yaw); // calculate prev/next point of lanelet centerline nearest to ego pose. lanelet::Lanelet ego_lanelet; @@ -497,7 +497,7 @@ void SimplePlanningSimulator::on_timer() // add estimate covariance { - using COV_IDX = autoware::universe_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; + using COV_IDX = autoware_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; current_odometry_.pose.covariance[COV_IDX::X_X] = x_stddev_; current_odometry_.pose.covariance[COV_IDX::Y_Y] = y_stddev_; } @@ -695,7 +695,7 @@ void SimplePlanningSimulator::add_measurement_noise( odom.twist.twist.linear.x += velocity_noise; double yaw = tf2::getYaw(odom.pose.pose.orientation); yaw += static_cast((*n.rpy_dist_)(*n.rand_engine_)); - odom.pose.pose.orientation = autoware::universe_utils::createQuaternionFromYaw(yaw); + odom.pose.pose.orientation = autoware_utils::create_quaternion_from_yaw(yaw); vel.longitudinal_velocity += static_cast(velocity_noise); @@ -823,7 +823,7 @@ void SimplePlanningSimulator::publish_pose(const Odometry & odometry) geometry_msgs::msg::PoseWithCovarianceStamped msg; msg.pose = odometry.pose; - using COV_IDX = autoware::universe_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; + using COV_IDX = autoware_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; constexpr auto COV_POS = 0.0225; // same value as current ndt output constexpr auto COV_ANGLE = 0.000625; // same value as current ndt output msg.pose.covariance.at(COV_IDX::X_X) = COV_POS; @@ -853,7 +853,7 @@ void SimplePlanningSimulator::publish_acceleration() msg.accel.accel.linear.x = vehicle_model_ptr_->getAx(); msg.accel.accel.linear.y = vehicle_model_ptr_->getWz() * vehicle_model_ptr_->getVx(); - using COV_IDX = autoware::universe_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; + using COV_IDX = autoware_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; constexpr auto COV = 0.001; msg.accel.covariance.at(COV_IDX::X_X) = COV; // linear x msg.accel.covariance.at(COV_IDX::Y_Y) = COV; // linear y @@ -866,7 +866,7 @@ void SimplePlanningSimulator::publish_acceleration() void SimplePlanningSimulator::publish_imu() { - using COV_IDX = autoware::universe_utils::xyz_covariance_index::XYZ_COV_IDX; + using COV_IDX = autoware_utils::xyz_covariance_index::XYZ_COV_IDX; sensor_msgs::msg::Imu imu; imu.header.frame_id = "base_link"; diff --git a/system/autoware_hazard_status_converter/package.xml b/system/autoware_hazard_status_converter/package.xml index ba501e70003d5..6289e469dc6d1 100644 --- a/system/autoware_hazard_status_converter/package.xml +++ b/system/autoware_hazard_status_converter/package.xml @@ -14,7 +14,7 @@ autoware_adapi_v1_msgs autoware_diagnostic_graph_utils autoware_system_msgs - autoware_universe_utils + autoware_utils diagnostic_msgs rclcpp rclcpp_components diff --git a/system/autoware_hazard_status_converter/src/converter.cpp b/system/autoware_hazard_status_converter/src/converter.cpp index 9faf4e186e06c..5ee1578d8b684 100644 --- a/system/autoware_hazard_status_converter/src/converter.cpp +++ b/system/autoware_hazard_status_converter/src/converter.cpp @@ -119,7 +119,7 @@ void Converter::on_update(DiagGraph::ConstSharedPtr graph) hazard.status.level = get_system_level(hazard.status); hazard.status.emergency = hazard.status.level == HazardStatus::SINGLE_POINT_FAULT; - const auto is_emergency_holding = sub_emergency_holding_.takeData(); + const auto is_emergency_holding = sub_emergency_holding_.take_data(); hazard.status.emergency_holding = is_emergency_holding == nullptr ? false : is_emergency_holding->is_holding; pub_hazard_->publish(hazard); diff --git a/system/autoware_hazard_status_converter/src/converter.hpp b/system/autoware_hazard_status_converter/src/converter.hpp index 01b28a54c4413..3ca06fe5f3d3c 100644 --- a/system/autoware_hazard_status_converter/src/converter.hpp +++ b/system/autoware_hazard_status_converter/src/converter.hpp @@ -16,7 +16,7 @@ #define CONVERTER_HPP_ #include -#include +#include #include #include @@ -40,8 +40,7 @@ class Converter : public rclcpp::Node void on_update(DiagGraph::ConstSharedPtr graph); autoware::diagnostic_graph_utils::DiagGraphSubscription sub_graph_; rclcpp::Publisher::SharedPtr pub_hazard_; - autoware::universe_utils::InterProcessPollingSubscriber< - tier4_system_msgs::msg::EmergencyHoldingState> + autoware_utils::InterProcessPollingSubscriber sub_emergency_holding_{this, "~/input/emergency_holding"}; DiagUnit * auto_mode_root_; diff --git a/system/autoware_mrm_comfortable_stop_operator/package.xml b/system/autoware_mrm_comfortable_stop_operator/package.xml index 33746a7d2dda0..7e1147955bc75 100644 --- a/system/autoware_mrm_comfortable_stop_operator/package.xml +++ b/system/autoware_mrm_comfortable_stop_operator/package.xml @@ -13,7 +13,7 @@ autoware_cmake autoware_adapi_v1_msgs - autoware_universe_utils + autoware_utils rclcpp rclcpp_components std_msgs diff --git a/system/autoware_mrm_comfortable_stop_operator/src/mrm_comfortable_stop_operator/mrm_comfortable_stop_operator_core.cpp b/system/autoware_mrm_comfortable_stop_operator/src/mrm_comfortable_stop_operator/mrm_comfortable_stop_operator_core.cpp index 397367e568a25..5f9df2789a9b5 100644 --- a/system/autoware_mrm_comfortable_stop_operator/src/mrm_comfortable_stop_operator/mrm_comfortable_stop_operator_core.cpp +++ b/system/autoware_mrm_comfortable_stop_operator/src/mrm_comfortable_stop_operator/mrm_comfortable_stop_operator_core.cpp @@ -14,7 +14,7 @@ #include "autoware/mrm_comfortable_stop_operator/mrm_comfortable_stop_operator_core.hpp" -#include +#include #include @@ -76,10 +76,10 @@ void MrmComfortableStopOperator::operateComfortableStop( rcl_interfaces::msg::SetParametersResult MrmComfortableStopOperator::onParameter( const std::vector & parameters) { - using autoware::universe_utils::updateParam; - updateParam(parameters, "min_acceleration", params_.min_acceleration); - updateParam(parameters, "max_jerk", params_.max_jerk); - updateParam(parameters, "min_jerk", params_.min_jerk); + using autoware_utils::update_param; + update_param(parameters, "min_acceleration", params_.min_acceleration); + update_param(parameters, "max_jerk", params_.max_jerk); + update_param(parameters, "min_jerk", params_.min_jerk); rcl_interfaces::msg::SetParametersResult result; result.successful = true; diff --git a/system/autoware_mrm_emergency_stop_operator/package.xml b/system/autoware_mrm_emergency_stop_operator/package.xml index 169c3569b5da2..12d94851883e6 100644 --- a/system/autoware_mrm_emergency_stop_operator/package.xml +++ b/system/autoware_mrm_emergency_stop_operator/package.xml @@ -14,7 +14,7 @@ autoware_adapi_v1_msgs autoware_control_msgs - autoware_universe_utils + autoware_utils rclcpp rclcpp_components std_msgs diff --git a/system/autoware_mrm_emergency_stop_operator/src/mrm_emergency_stop_operator/mrm_emergency_stop_operator_core.cpp b/system/autoware_mrm_emergency_stop_operator/src/mrm_emergency_stop_operator/mrm_emergency_stop_operator_core.cpp index 7db4ddaef9a9c..7918bbb971c59 100644 --- a/system/autoware_mrm_emergency_stop_operator/src/mrm_emergency_stop_operator/mrm_emergency_stop_operator_core.cpp +++ b/system/autoware_mrm_emergency_stop_operator/src/mrm_emergency_stop_operator/mrm_emergency_stop_operator_core.cpp @@ -14,7 +14,7 @@ #include "autoware/mrm_emergency_stop_operator/mrm_emergency_stop_operator_core.hpp" -#include +#include #include @@ -61,9 +61,9 @@ MrmEmergencyStopOperator::MrmEmergencyStopOperator(const rclcpp::NodeOptions & n rcl_interfaces::msg::SetParametersResult MrmEmergencyStopOperator::onParameter( const std::vector & parameters) { - using autoware::universe_utils::updateParam; - updateParam(parameters, "target_acceleration", params_.target_acceleration); - updateParam(parameters, "target_jerk", params_.target_jerk); + using autoware_utils::update_param; + update_param(parameters, "target_acceleration", params_.target_acceleration); + update_param(parameters, "target_jerk", params_.target_jerk); rcl_interfaces::msg::SetParametersResult result; result.successful = true; diff --git a/system/autoware_mrm_handler/include/autoware/mrm_handler/mrm_handler_core.hpp b/system/autoware_mrm_handler/include/autoware/mrm_handler/mrm_handler_core.hpp index 8d74db4e5c843..ac5939c08dcb2 100644 --- a/system/autoware_mrm_handler/include/autoware/mrm_handler/mrm_handler_core.hpp +++ b/system/autoware_mrm_handler/include/autoware/mrm_handler/mrm_handler_core.hpp @@ -22,7 +22,7 @@ #include // Autoware -#include +#include #include #include @@ -76,21 +76,19 @@ class MrmHandler : public rclcpp::Node rclcpp::Subscription::SharedPtr sub_operation_mode_availability_; // Subscribers without callback - autoware::universe_utils::InterProcessPollingSubscriber sub_odom_{ + autoware_utils::InterProcessPollingSubscriber sub_odom_{ this, "~/input/odometry"}; - autoware::universe_utils::InterProcessPollingSubscriber< - autoware_vehicle_msgs::msg::ControlModeReport> + autoware_utils::InterProcessPollingSubscriber sub_control_mode_{this, "~/input/control_mode"}; - autoware::universe_utils::InterProcessPollingSubscriber + autoware_utils::InterProcessPollingSubscriber sub_mrm_pull_over_status_{this, "~/input/mrm/pull_over/status"}; - autoware::universe_utils::InterProcessPollingSubscriber + autoware_utils::InterProcessPollingSubscriber sub_mrm_comfortable_stop_status_{this, "~/input/mrm/comfortable_stop/status"}; - autoware::universe_utils::InterProcessPollingSubscriber + autoware_utils::InterProcessPollingSubscriber sub_mrm_emergency_stop_status_{this, "~/input/mrm/emergency_stop/status"}; - autoware::universe_utils::InterProcessPollingSubscriber< - autoware_adapi_v1_msgs::msg::OperationModeState> + autoware_utils::InterProcessPollingSubscriber sub_operation_mode_state_{this, "~/input/api/operation_mode/state"}; - autoware::universe_utils::InterProcessPollingSubscriber + autoware_utils::InterProcessPollingSubscriber sub_gear_cmd_{this, "~/input/gear"}; tier4_system_msgs::msg::OperationModeAvailability::ConstSharedPtr operation_mode_availability_; diff --git a/system/autoware_mrm_handler/package.xml b/system/autoware_mrm_handler/package.xml index 92cfef1634bc1..d10f6c5a9fe73 100644 --- a/system/autoware_mrm_handler/package.xml +++ b/system/autoware_mrm_handler/package.xml @@ -14,7 +14,7 @@ autoware_cmake autoware_adapi_v1_msgs - autoware_universe_utils + autoware_utils autoware_vehicle_msgs nav_msgs rclcpp diff --git a/system/autoware_mrm_handler/src/mrm_handler/mrm_handler_core.cpp b/system/autoware_mrm_handler/src/mrm_handler/mrm_handler_core.cpp index e9429300de573..088b9432421fc 100644 --- a/system/autoware_mrm_handler/src/mrm_handler/mrm_handler_core.cpp +++ b/system/autoware_mrm_handler/src/mrm_handler/mrm_handler_core.cpp @@ -139,7 +139,7 @@ void MrmHandler::publishGearCmd() (param_.use_parking_after_stopped && isStopped()) ? GearCommand::PARK : last_gear_command_; } else { // use the same gear as the input gear - auto gear = sub_gear_cmd_.takeData(); + auto gear = sub_gear_cmd_.take_data(); msg.command = (gear == nullptr) ? last_gear_command_ : gear->command; last_gear_command_ = msg.command; } @@ -529,7 +529,7 @@ autoware_adapi_v1_msgs::msg::MrmState::_behavior_type MrmHandler::getCurrentMrmB bool MrmHandler::isStopped() { - auto odom = sub_odom_.takeData(); + auto odom = sub_odom_.take_data(); if (odom == nullptr) return false; constexpr auto th_stopped_velocity = 0.001; return (std::abs(odom->twist.twist.linear.x) < th_stopped_velocity); @@ -544,7 +544,7 @@ bool MrmHandler::isEmergency() const bool MrmHandler::isControlModeAutonomous() { using autoware_vehicle_msgs::msg::ControlModeReport; - auto mode = sub_control_mode_.takeData(); + auto mode = sub_control_mode_.take_data(); if (mode == nullptr) return false; return mode->mode == ControlModeReport::AUTONOMOUS; } @@ -552,28 +552,28 @@ bool MrmHandler::isControlModeAutonomous() bool MrmHandler::isOperationModeAutonomous() { using autoware_adapi_v1_msgs::msg::OperationModeState; - auto state = sub_operation_mode_state_.takeData(); + auto state = sub_operation_mode_state_.take_data(); if (state == nullptr) return false; return state->mode == OperationModeState::AUTONOMOUS; } bool MrmHandler::isPullOverStatusAvailable() { - auto status = sub_mrm_pull_over_status_.takeData(); + auto status = sub_mrm_pull_over_status_.take_data(); if (status == nullptr) return false; return status->state != tier4_system_msgs::msg::MrmBehaviorStatus::NOT_AVAILABLE; } bool MrmHandler::isComfortableStopStatusAvailable() { - auto status = sub_mrm_comfortable_stop_status_.takeData(); + auto status = sub_mrm_comfortable_stop_status_.take_data(); if (status == nullptr) return false; return status->state != tier4_system_msgs::msg::MrmBehaviorStatus::NOT_AVAILABLE; } bool MrmHandler::isEmergencyStopStatusAvailable() { - auto status = sub_mrm_emergency_stop_status_.takeData(); + auto status = sub_mrm_emergency_stop_status_.take_data(); if (status == nullptr) return false; return status->state != tier4_system_msgs::msg::MrmBehaviorStatus::NOT_AVAILABLE; } @@ -581,7 +581,7 @@ bool MrmHandler::isEmergencyStopStatusAvailable() bool MrmHandler::isArrivedAtGoal() { using autoware_adapi_v1_msgs::msg::OperationModeState; - auto state = sub_operation_mode_state_.takeData(); + auto state = sub_operation_mode_state_.take_data(); if (state == nullptr) return false; return state->mode == OperationModeState::STOP; } diff --git a/system/autoware_processing_time_checker/package.xml b/system/autoware_processing_time_checker/package.xml index ffbdfc85c3cbe..b67e03afdceee 100644 --- a/system/autoware_processing_time_checker/package.xml +++ b/system/autoware_processing_time_checker/package.xml @@ -13,7 +13,7 @@ autoware_cmake autoware_internal_debug_msgs - autoware_universe_utils + autoware_utils nlohmann-json-dev rclcpp rclcpp_components diff --git a/system/autoware_processing_time_checker/src/processing_time_checker.hpp b/system/autoware_processing_time_checker/src/processing_time_checker.hpp index f6664b901f890..b177fdf47e2e8 100644 --- a/system/autoware_processing_time_checker/src/processing_time_checker.hpp +++ b/system/autoware_processing_time_checker/src/processing_time_checker.hpp @@ -15,7 +15,7 @@ #ifndef PROCESSING_TIME_CHECKER_HPP_ #define PROCESSING_TIME_CHECKER_HPP_ -#include "autoware/universe_utils/math/accumulator.hpp" +#include "autoware_utils/math/accumulator.hpp" #include @@ -29,7 +29,7 @@ namespace autoware::processing_time_checker { -using autoware::universe_utils::Accumulator; +using autoware_utils::Accumulator; using MetricMsg = tier4_metric_msgs::msg::Metric; using MetricArrayMsg = tier4_metric_msgs::msg::MetricArray; using autoware_internal_debug_msgs::msg::Float64Stamped; diff --git a/system/autoware_system_monitor/package.xml b/system/autoware_system_monitor/package.xml index 0212b4ec61751..28e2300c0c8ba 100644 --- a/system/autoware_system_monitor/package.xml +++ b/system/autoware_system_monitor/package.xml @@ -12,7 +12,7 @@ ament_cmake_auto autoware_cmake - autoware_universe_utils + autoware_utils diagnostic_msgs diagnostic_updater fmt diff --git a/system/autoware_system_monitor/src/ntp_monitor/ntp_monitor.cpp b/system/autoware_system_monitor/src/ntp_monitor/ntp_monitor.cpp index 590793cf1f7fe..02516b9e9d902 100644 --- a/system/autoware_system_monitor/src/ntp_monitor/ntp_monitor.cpp +++ b/system/autoware_system_monitor/src/ntp_monitor/ntp_monitor.cpp @@ -19,7 +19,7 @@ #include "system_monitor/ntp_monitor/ntp_monitor.hpp" -#include +#include #include #include @@ -127,7 +127,7 @@ void NTPMonitor::checkOffset(diagnostic_updater::DiagnosticStatusWrapper & stat) void NTPMonitor::onTimer() { // Start to measure elapsed time - autoware::universe_utils::StopWatch stop_watch; + autoware_utils::StopWatch stop_watch; stop_watch.tic("execution_time"); std::string error_str; diff --git a/system/autoware_system_monitor/src/process_monitor/process_monitor.cpp b/system/autoware_system_monitor/src/process_monitor/process_monitor.cpp index faaf1abf1c9d3..9922c417c3fff 100644 --- a/system/autoware_system_monitor/src/process_monitor/process_monitor.cpp +++ b/system/autoware_system_monitor/src/process_monitor/process_monitor.cpp @@ -21,7 +21,7 @@ #include "system_monitor/system_monitor_utility.hpp" -#include +#include #include @@ -517,7 +517,7 @@ void ProcessMonitor::onTimer() bool is_top_error = false; // Start to measure elapsed time - autoware::universe_utils::StopWatch stop_watch; + autoware_utils::StopWatch stop_watch; stop_watch.tic("execution_time"); int out_fd[2]; diff --git a/tools/reaction_analyzer/include/utils.hpp b/tools/reaction_analyzer/include/utils.hpp index 17e3786eff7fe..0faddc6a4ab18 100644 --- a/tools/reaction_analyzer/include/utils.hpp +++ b/tools/reaction_analyzer/include/utils.hpp @@ -15,8 +15,8 @@ #ifndef UTILS_HPP_ #define UTILS_HPP_ -#include -#include +#include +#include #include #include #include diff --git a/tools/reaction_analyzer/package.xml b/tools/reaction_analyzer/package.xml index 9d1928813f07e..0df035291d575 100644 --- a/tools/reaction_analyzer/package.xml +++ b/tools/reaction_analyzer/package.xml @@ -21,7 +21,7 @@ autoware_perception_msgs autoware_planning_msgs autoware_system_msgs - autoware_universe_utils + autoware_utils autoware_vehicle_msgs eigen libpcl-all-dev diff --git a/tools/reaction_analyzer/src/reaction_analyzer_node.cpp b/tools/reaction_analyzer/src/reaction_analyzer_node.cpp index 1a4b2e7367705..e9a0df738bd1c 100644 --- a/tools/reaction_analyzer/src/reaction_analyzer_node.cpp +++ b/tools/reaction_analyzer/src/reaction_analyzer_node.cpp @@ -14,8 +14,8 @@ #include "reaction_analyzer_node.hpp" -#include -#include +#include +#include #include #include @@ -210,7 +210,7 @@ void ReactionAnalyzerNode::spawn_obstacle(const geometry_msgs::msg::Point & ego_ } } else { if ( - autoware::universe_utils::calcDistance3d(ego_pose, entity_pose_.position) < + autoware_utils::calc_distance3d(ego_pose, entity_pose_.position) < node_params_.spawn_distance_threshold) { if (!spawn_object_cmd_) { spawn_object_cmd_ = true; @@ -399,8 +399,8 @@ bool ReactionAnalyzerNode::check_ego_init_correctly( } constexpr double deviation_threshold = 0.1; - const auto deviation = autoware::universe_utils::calcPoseDeviation( - ground_truth_pose_ptr->pose, odometry_ptr->pose.pose); + const auto deviation = + autoware_utils::calc_pose_deviation(ground_truth_pose_ptr->pose, odometry_ptr->pose.pose); const bool is_position_initialized_correctly = deviation.longitudinal < deviation_threshold && deviation.lateral < deviation_threshold && deviation.yaw < deviation_threshold; diff --git a/tools/reaction_analyzer/src/utils.cpp b/tools/reaction_analyzer/src/utils.cpp index 9075cdc4a77dd..ed9730caa1bf2 100644 --- a/tools/reaction_analyzer/src/utils.cpp +++ b/tools/reaction_analyzer/src/utils.cpp @@ -142,8 +142,8 @@ visualization_msgs::msg::Marker create_polyhedron_marker(const EntityParams & pa tf2::Quaternion quaternion; quaternion.setRPY( - autoware::universe_utils::deg2rad(params.roll), autoware::universe_utils::deg2rad(params.pitch), - autoware::universe_utils::deg2rad(params.yaw)); + autoware_utils::deg2rad(params.roll), autoware_utils::deg2rad(params.pitch), + autoware_utils::deg2rad(params.yaw)); marker.pose.orientation = tf2::toMsg(quaternion); marker.scale.x = 0.1; // Line width @@ -230,7 +230,7 @@ size_t get_index_after_distance( size_t target_id = curr_id; for (size_t traj_id = curr_id + 1; traj_id < traj.points.size(); ++traj_id) { const double current_distance = - autoware::universe_utils::calcDistance3d(traj.points.at(traj_id), curr_p); + autoware_utils::calc_distance3d(traj.points.at(traj_id), curr_p); if (current_distance >= distance) { break; } @@ -292,9 +292,8 @@ geometry_msgs::msg::Pose create_entity_pose(const EntityParams & entity_params) tf2::Quaternion entity_q_orientation; entity_q_orientation.setRPY( - autoware::universe_utils::deg2rad(entity_params.roll), - autoware::universe_utils::deg2rad(entity_params.pitch), - autoware::universe_utils::deg2rad(entity_params.yaw)); + autoware_utils::deg2rad(entity_params.roll), autoware_utils::deg2rad(entity_params.pitch), + autoware_utils::deg2rad(entity_params.yaw)); entity_pose.orientation = tf2::toMsg(entity_q_orientation); return entity_pose; } @@ -308,9 +307,8 @@ geometry_msgs::msg::Pose pose_params_to_pose(const PoseParams & pose_params) tf2::Quaternion pose_q_orientation; pose_q_orientation.setRPY( - autoware::universe_utils::deg2rad(pose_params.roll), - autoware::universe_utils::deg2rad(pose_params.pitch), - autoware::universe_utils::deg2rad(pose_params.yaw)); + autoware_utils::deg2rad(pose_params.roll), autoware_utils::deg2rad(pose_params.pitch), + autoware_utils::deg2rad(pose_params.yaw)); pose.orientation = tf2::toMsg(pose_q_orientation); return pose; } @@ -322,9 +320,8 @@ PointCloud2::SharedPtr create_entity_pointcloud_ptr( tf2::Quaternion entity_q_orientation; entity_q_orientation.setRPY( - autoware::universe_utils::deg2rad(entity_params.roll), - autoware::universe_utils::deg2rad(entity_params.pitch), - autoware::universe_utils::deg2rad(entity_params.yaw)); + autoware_utils::deg2rad(entity_params.roll), autoware_utils::deg2rad(entity_params.pitch), + autoware_utils::deg2rad(entity_params.yaw)); tf2::Transform tf(entity_q_orientation); const auto origin = tf2::Vector3(entity_params.x, entity_params.y, entity_params.z); tf.setOrigin(origin); @@ -401,7 +398,7 @@ bool search_pointcloud_near_pose( return std::any_of( pcl_pointcloud.points.begin(), pcl_pointcloud.points.end(), [pose, search_radius](const auto & point) { - return autoware::universe_utils::calcDistance3d(pose.position, point) <= search_radius; + return autoware_utils::calc_distance3d(pose.position, point) <= search_radius; }); } @@ -412,7 +409,7 @@ bool search_predicted_objects_near_pose( return std::any_of( predicted_objects.objects.begin(), predicted_objects.objects.end(), [pose, search_radius](const PredictedObject & object) { - return autoware::universe_utils::calcDistance3d( + return autoware_utils::calc_distance3d( pose.position, object.kinematics.initial_pose_with_covariance.pose.position) <= search_radius; }); @@ -426,7 +423,7 @@ bool search_detected_objects_near_pose( return std::any_of( detected_objects.objects.begin(), detected_objects.objects.end(), [pose, search_radius](const DetectedObject & object) { - return autoware::universe_utils::calcDistance3d( + return autoware_utils::calc_distance3d( pose.position, object.kinematics.pose_with_covariance.pose.position) <= search_radius; }); @@ -439,7 +436,7 @@ bool search_tracked_objects_near_pose( return std::any_of( tracked_objects.objects.begin(), tracked_objects.objects.end(), [pose, search_radius](const TrackedObject & object) { - return autoware::universe_utils::calcDistance3d( + return autoware_utils::calc_distance3d( pose.position, object.kinematics.pose_with_covariance.pose.position) <= search_radius; }); diff --git a/vehicle/autoware_accel_brake_map_calibrator/include/autoware_accel_brake_map_calibrator/accel_brake_map_calibrator_node.hpp b/vehicle/autoware_accel_brake_map_calibrator/include/autoware_accel_brake_map_calibrator/accel_brake_map_calibrator_node.hpp index cd05bed5ce476..d8c02214284e1 100644 --- a/vehicle/autoware_accel_brake_map_calibrator/include/autoware_accel_brake_map_calibrator/accel_brake_map_calibrator_node.hpp +++ b/vehicle/autoware_accel_brake_map_calibrator/include/autoware_accel_brake_map_calibrator/accel_brake_map_calibrator_node.hpp @@ -17,11 +17,11 @@ #ifndef AUTOWARE_ACCEL_BRAKE_MAP_CALIBRATOR__ACCEL_BRAKE_MAP_CALIBRATOR_NODE_HPP_ #define AUTOWARE_ACCEL_BRAKE_MAP_CALIBRATOR__ACCEL_BRAKE_MAP_CALIBRATOR_NODE_HPP_ -#include "autoware/universe_utils/ros/logger_level_configure.hpp" -#include "autoware/universe_utils/ros/polling_subscriber.hpp" -#include "autoware/universe_utils/ros/transform_listener.hpp" #include "autoware_raw_vehicle_cmd_converter/accel_map.hpp" #include "autoware_raw_vehicle_cmd_converter/brake_map.hpp" +#include "autoware_utils/ros/logger_level_configure.hpp" +#include "autoware_utils/ros/polling_subscriber.hpp" +#include "autoware_utils/ros/transform_listener.hpp" #include "diagnostic_updater/diagnostic_updater.hpp" #include "rclcpp/rclcpp.hpp" #include "tf2/utils.h" @@ -89,7 +89,7 @@ using DataStampedPtr = std::shared_ptr; class AccelBrakeMapCalibrator : public rclcpp::Node { private: - std::shared_ptr transform_listener_; + std::shared_ptr transform_listener_; std::string csv_default_map_dir_; rclcpp::Publisher::SharedPtr original_map_occ_pub_; rclcpp::Publisher::SharedPtr update_map_occ_pub_; @@ -108,13 +108,12 @@ class AccelBrakeMapCalibrator : public rclcpp::Node rclcpp::Publisher::SharedPtr map_error_ratio_pub_; rclcpp::Publisher::SharedPtr calibration_status_pub_; - autoware::universe_utils::InterProcessPollingSubscriber steer_sub_{ - this, "~/input/steer"}; - autoware::universe_utils::InterProcessPollingSubscriber - actuation_status_sub_{this, "~/input/actuation_status"}; - autoware::universe_utils::InterProcessPollingSubscriber - actuation_cmd_sub_{this, "~/input/actuation_cmd"}; - autoware::universe_utils::InterProcessPollingSubscriber velocity_sub_{ + autoware_utils::InterProcessPollingSubscriber steer_sub_{this, "~/input/steer"}; + autoware_utils::InterProcessPollingSubscriber actuation_status_sub_{ + this, "~/input/actuation_status"}; + autoware_utils::InterProcessPollingSubscriber actuation_cmd_sub_{ + this, "~/input/actuation_cmd"}; + autoware_utils::InterProcessPollingSubscriber velocity_sub_{ this, "~/input/velocity"}; // Service @@ -375,7 +374,7 @@ class AccelBrakeMapCalibrator : public rclcpp::Node COMMAND = 1, }; - std::unique_ptr logger_configure_; + std::unique_ptr logger_configure_; public: explicit AccelBrakeMapCalibrator(const rclcpp::NodeOptions & node_options); diff --git a/vehicle/autoware_accel_brake_map_calibrator/package.xml b/vehicle/autoware_accel_brake_map_calibrator/package.xml index 78c3b518057de..71c0256682bdd 100644 --- a/vehicle/autoware_accel_brake_map_calibrator/package.xml +++ b/vehicle/autoware_accel_brake_map_calibrator/package.xml @@ -17,7 +17,7 @@ autoware_internal_debug_msgs autoware_motion_utils autoware_raw_vehicle_cmd_converter - autoware_universe_utils + autoware_utils autoware_vehicle_msgs diagnostic_updater geometry_msgs diff --git a/vehicle/autoware_accel_brake_map_calibrator/src/accel_brake_map_calibrator_node.cpp b/vehicle/autoware_accel_brake_map_calibrator/src/accel_brake_map_calibrator_node.cpp index 9d7de55aff54d..d4bdf9e16f759 100644 --- a/vehicle/autoware_accel_brake_map_calibrator/src/accel_brake_map_calibrator_node.cpp +++ b/vehicle/autoware_accel_brake_map_calibrator/src/accel_brake_map_calibrator_node.cpp @@ -33,7 +33,7 @@ namespace autoware::accel_brake_map_calibrator AccelBrakeMapCalibrator::AccelBrakeMapCalibrator(const rclcpp::NodeOptions & node_options) : Node("accel_brake_map_calibrator", node_options) { - transform_listener_ = std::make_shared(this); + transform_listener_ = std::make_shared(this); // get parameter update_hz_ = declare_parameter("update_hz", 10.0); covariance_ = declare_parameter("initial_covariance", 0.05); @@ -218,7 +218,7 @@ AccelBrakeMapCalibrator::AccelBrakeMapCalibrator(const rclcpp::NodeOptions & nod init_timer(1.0 / update_hz_); init_output_csv_timer(30.0); - logger_configure_ = std::make_unique(this); + logger_configure_ = std::make_unique(this); } void AccelBrakeMapCalibrator::init_output_csv_timer(double period_s) @@ -247,7 +247,7 @@ bool AccelBrakeMapCalibrator::get_current_pitch_from_tf(double * pitch) } // get tf - const auto transform = transform_listener_->getTransform( + const auto transform = transform_listener_->get_transform( "map", "base_link", rclcpp::Time(0), rclcpp::Duration::from_seconds(0.5)); if (!transform) { RCLCPP_WARN_STREAM_THROTTLE( @@ -268,24 +268,24 @@ bool AccelBrakeMapCalibrator::take_data() { // take data from subscribers if (accel_brake_value_source_ == ACCEL_BRAKE_SOURCE::STATUS) { - ActuationStatusStamped::ConstSharedPtr actuation_status_ptr = actuation_status_sub_.takeData(); + ActuationStatusStamped::ConstSharedPtr actuation_status_ptr = actuation_status_sub_.take_data(); if (!actuation_status_ptr) return false; take_actuation_status(actuation_status_ptr); } // take actuation data if (accel_brake_value_source_ == ACCEL_BRAKE_SOURCE::COMMAND) { - ActuationCommandStamped::ConstSharedPtr actuation_cmd_ptr = actuation_cmd_sub_.takeData(); + ActuationCommandStamped::ConstSharedPtr actuation_cmd_ptr = actuation_cmd_sub_.take_data(); if (!actuation_cmd_ptr) return false; take_actuation_command(actuation_cmd_ptr); } // take velocity data - VelocityReport::ConstSharedPtr velocity_ptr = velocity_sub_.takeData(); + VelocityReport::ConstSharedPtr velocity_ptr = velocity_sub_.take_data(); if (!velocity_ptr) return false; take_velocity(velocity_ptr); // take steer data - steer_ptr_ = steer_sub_.takeData(); + steer_ptr_ = steer_sub_.take_data(); /* valid check */ // data check diff --git a/vehicle/autoware_external_cmd_converter/include/autoware_external_cmd_converter/node.hpp b/vehicle/autoware_external_cmd_converter/include/autoware_external_cmd_converter/node.hpp index 8d045279fc281..dc9ffa1b60d78 100644 --- a/vehicle/autoware_external_cmd_converter/include/autoware_external_cmd_converter/node.hpp +++ b/vehicle/autoware_external_cmd_converter/include/autoware_external_cmd_converter/node.hpp @@ -15,7 +15,7 @@ #ifndef AUTOWARE_EXTERNAL_CMD_CONVERTER__NODE_HPP_ #define AUTOWARE_EXTERNAL_CMD_CONVERTER__NODE_HPP_ -#include "autoware/universe_utils/ros/polling_subscriber.hpp" +#include "autoware_utils/ros/polling_subscriber.hpp" #include #include @@ -62,11 +62,9 @@ class ExternalCmdConverterNode : public rclcpp::Node emergency_stop_heartbeat_sub_; // Polling Subscriber - autoware::universe_utils::InterProcessPollingSubscriber velocity_sub_{ - this, "in/odometry"}; - autoware::universe_utils::InterProcessPollingSubscriber shift_cmd_sub_{ - this, "in/shift_cmd"}; - autoware::universe_utils::InterProcessPollingSubscriber gate_mode_sub_{ + autoware_utils::InterProcessPollingSubscriber velocity_sub_{this, "in/odometry"}; + autoware_utils::InterProcessPollingSubscriber shift_cmd_sub_{this, "in/shift_cmd"}; + autoware_utils::InterProcessPollingSubscriber gate_mode_sub_{ this, "in/current_gate_mode"}; void on_external_cmd(const ExternalControlCommand::ConstSharedPtr cmd_ptr); diff --git a/vehicle/autoware_external_cmd_converter/src/node.cpp b/vehicle/autoware_external_cmd_converter/src/node.cpp index 267836b91f945..209db32f79ba0 100644 --- a/vehicle/autoware_external_cmd_converter/src/node.cpp +++ b/vehicle/autoware_external_cmd_converter/src/node.cpp @@ -100,8 +100,8 @@ void ExternalCmdConverterNode::on_external_cmd(const ExternalControlCommand::Con latest_cmd_received_time_ = std::make_shared(this->now()); // take data from subscribers - current_velocity_ptr_ = velocity_sub_.takeData(); - current_shift_cmd_ = shift_cmd_sub_.takeData(); + current_velocity_ptr_ = velocity_sub_.take_data(); + current_shift_cmd_ = shift_cmd_sub_.take_data(); // Wait for input data if (!current_velocity_ptr_ || !acc_map_initialized_ || !current_shift_cmd_) { @@ -190,7 +190,7 @@ void ExternalCmdConverterNode::check_topic_status( using diagnostic_msgs::msg::DiagnosticStatus; DiagnosticStatus status; - current_gate_mode_ = gate_mode_sub_.takeData(); + current_gate_mode_ = gate_mode_sub_.take_data(); if (!check_emergency_stop_topic_timeout()) { status.level = DiagnosticStatus::ERROR; diff --git a/vehicle/autoware_raw_vehicle_cmd_converter/include/autoware_raw_vehicle_cmd_converter/node.hpp b/vehicle/autoware_raw_vehicle_cmd_converter/include/autoware_raw_vehicle_cmd_converter/node.hpp index eaf39cf729971..3be093b201e3c 100644 --- a/vehicle/autoware_raw_vehicle_cmd_converter/include/autoware_raw_vehicle_cmd_converter/node.hpp +++ b/vehicle/autoware_raw_vehicle_cmd_converter/include/autoware_raw_vehicle_cmd_converter/node.hpp @@ -15,14 +15,14 @@ #ifndef AUTOWARE_RAW_VEHICLE_CMD_CONVERTER__NODE_HPP_ #define AUTOWARE_RAW_VEHICLE_CMD_CONVERTER__NODE_HPP_ -#include "autoware/universe_utils/ros/logger_level_configure.hpp" -#include "autoware/universe_utils/ros/polling_subscriber.hpp" #include "autoware_raw_vehicle_cmd_converter/accel_map.hpp" #include "autoware_raw_vehicle_cmd_converter/brake_map.hpp" #include "autoware_raw_vehicle_cmd_converter/pid.hpp" #include "autoware_raw_vehicle_cmd_converter/steer_map.hpp" #include "autoware_raw_vehicle_cmd_converter/vehicle_adaptor/vehicle_adaptor.hpp" #include "autoware_raw_vehicle_cmd_converter/vgr.hpp" +#include "autoware_utils/ros/logger_level_configure.hpp" +#include "autoware_utils/ros/polling_subscriber.hpp" #include @@ -88,15 +88,14 @@ class RawVehicleCommandConverterNode : public rclcpp::Node rclcpp::Subscription::SharedPtr sub_actuation_status_; rclcpp::Subscription::SharedPtr sub_steering_; // polling subscribers - autoware::universe_utils::InterProcessPollingSubscriber sub_odometry_{ - this, "~/input/odometry"}; + autoware_utils::InterProcessPollingSubscriber sub_odometry_{this, "~/input/odometry"}; // polling subscribers for vehicle_adaptor - autoware::universe_utils::InterProcessPollingSubscriber sub_accel_{ + autoware_utils::InterProcessPollingSubscriber sub_accel_{ this, "~/input/accel"}; - autoware::universe_utils::InterProcessPollingSubscriber sub_operation_mode_{ + autoware_utils::InterProcessPollingSubscriber sub_operation_mode_{ this, "~/input/operation_mode_state"}; // control_horizon is an experimental topic, but vehicle_adaptor uses it to improve performance, - autoware::universe_utils::InterProcessPollingSubscriber sub_control_horizon_{ + autoware_utils::InterProcessPollingSubscriber sub_control_horizon_{ this, "~/input/control_horizon"}; rclcpp::TimerBase::SharedPtr timer_; @@ -146,7 +145,7 @@ class RawVehicleCommandConverterNode : public rclcpp::Node rclcpp::Publisher::SharedPtr debug_pub_steer_pid_; DebugValues debug_steer_; - std::unique_ptr logger_configure_; + std::unique_ptr logger_configure_; }; } // namespace autoware::raw_vehicle_cmd_converter diff --git a/vehicle/autoware_raw_vehicle_cmd_converter/package.xml b/vehicle/autoware_raw_vehicle_cmd_converter/package.xml index 815ebf33159e2..81acd9a4575a2 100644 --- a/vehicle/autoware_raw_vehicle_cmd_converter/package.xml +++ b/vehicle/autoware_raw_vehicle_cmd_converter/package.xml @@ -25,7 +25,7 @@ autoware_control_msgs autoware_internal_debug_msgs autoware_interpolation - autoware_universe_utils + autoware_utils autoware_vehicle_msgs geometry_msgs nav_msgs diff --git a/vehicle/autoware_raw_vehicle_cmd_converter/src/node.cpp b/vehicle/autoware_raw_vehicle_cmd_converter/src/node.cpp index 8d05e4faa69ad..c9cd23c94dddd 100644 --- a/vehicle/autoware_raw_vehicle_cmd_converter/src/node.cpp +++ b/vehicle/autoware_raw_vehicle_cmd_converter/src/node.cpp @@ -123,7 +123,7 @@ RawVehicleCommandConverterNode::RawVehicleCommandConverterNode( "/vehicle/raw_vehicle_cmd_converter/debug/compensated_control_cmd", 1); } - logger_configure_ = std::make_unique(this); + logger_configure_ = std::make_unique(this); } void RawVehicleCommandConverterNode::publishActuationCmd() @@ -147,9 +147,9 @@ void RawVehicleCommandConverterNode::publishActuationCmd() /* compensate control command if vehicle adaptor is enabled */ Control control_cmd = *control_cmd_ptr_; if (use_vehicle_adaptor_) { - const auto current_accel = sub_accel_.takeData(); - const auto current_operation_mode = sub_operation_mode_.takeData(); - const auto control_horizon = sub_control_horizon_.takeData(); + const auto current_accel = sub_accel_.take_data(); + const auto current_operation_mode = sub_operation_mode_.take_data(); + const auto control_horizon = sub_control_horizon_.take_data(); if (!current_accel || !current_operation_mode || !control_horizon) { RCLCPP_WARN_EXPRESSION( get_logger(), is_debugging_, "some pointers are null: %s, %s %s", @@ -280,7 +280,7 @@ double RawVehicleCommandConverterNode::calculateBrakeMap( void RawVehicleCommandConverterNode::onControlCmd(const Control::ConstSharedPtr msg) { - current_odometry_ = sub_odometry_.takeData(); + current_odometry_ = sub_odometry_.take_data(); control_cmd_ptr_ = msg; publishActuationCmd(); } @@ -300,7 +300,7 @@ void RawVehicleCommandConverterNode::onActuationStatus( } // calculate steering status from actuation status - current_odometry_ = sub_odometry_.takeData(); + current_odometry_ = sub_odometry_.take_data(); if (current_odometry_) { if (convert_steer_cmd_method_.value() == "vgr") { current_steer_ptr_ = std::make_unique(vgr_.calculateSteeringTireState( diff --git a/vehicle/autoware_steer_offset_estimator/package.xml b/vehicle/autoware_steer_offset_estimator/package.xml index 5673025dbb54a..1a1ca222c5b76 100644 --- a/vehicle/autoware_steer_offset_estimator/package.xml +++ b/vehicle/autoware_steer_offset_estimator/package.xml @@ -10,7 +10,7 @@ autoware_cmake autoware_internal_debug_msgs - autoware_universe_utils + autoware_utils autoware_vehicle_info_utils autoware_vehicle_msgs diagnostic_updater diff --git a/visualization/tier4_planning_factor_rviz_plugin/package.xml b/visualization/tier4_planning_factor_rviz_plugin/package.xml index 6e703458782c4..d1dfff287886f 100644 --- a/visualization/tier4_planning_factor_rviz_plugin/package.xml +++ b/visualization/tier4_planning_factor_rviz_plugin/package.xml @@ -15,7 +15,7 @@ qtbase5-dev autoware_motion_utils - autoware_universe_utils + autoware_utils autoware_vehicle_info_utils rviz_common rviz_default_plugins diff --git a/visualization/tier4_planning_factor_rviz_plugin/src/planning_factor_rviz_plugin.cpp b/visualization/tier4_planning_factor_rviz_plugin/src/planning_factor_rviz_plugin.cpp index 9530368421596..52bba6f9d2736 100644 --- a/visualization/tier4_planning_factor_rviz_plugin/src/planning_factor_rviz_plugin.cpp +++ b/visualization/tier4_planning_factor_rviz_plugin/src/planning_factor_rviz_plugin.cpp @@ -15,9 +15,9 @@ #include "planning_factor_rviz_plugin.hpp" #include -#include -#include -#include +#include +#include +#include #include @@ -27,8 +27,8 @@ namespace autoware::rviz_plugins using autoware::motion_utils::createDeadLineVirtualWallMarker; using autoware::motion_utils::createSlowDownVirtualWallMarker; using autoware::motion_utils::createStopVirtualWallMarker; -using autoware::universe_utils::createDefaultMarker; -using autoware::universe_utils::createMarkerScale; +using autoware_utils::create_default_marker; +using autoware_utils::create_marker_scale; namespace { @@ -39,7 +39,7 @@ std_msgs::msg::ColorRGBA convertFromColorCode(const uint64_t code, const float a const float g = static_cast((code << 48) >> 56) / 255.0; const float b = static_cast((code << 56) >> 56) / 255.0; - return autoware::universe_utils::createMarkerColor(r, g, b, alpha); + return autoware_utils::create_marker_color(r, g, b, alpha); } std_msgs::msg::ColorRGBA getGreen(const float alpha) @@ -69,10 +69,10 @@ visualization_msgs::msg::Marker createArrowMarker( const double head_width = 0.5 * arrow_length; const double head_height = 0.5 * arrow_length; - auto marker = createDefaultMarker( + auto marker = create_default_marker( "map", rclcpp::Clock{RCL_ROS_TIME}.now(), name + "_arrow", id, - visualization_msgs::msg::Marker::ARROW, createMarkerScale(line_width, head_width, head_height), - color); + visualization_msgs::msg::Marker::ARROW, + create_marker_scale(line_width, head_width, head_height), color); geometry_msgs::msg::Point src, dst; src = position; @@ -91,17 +91,17 @@ visualization_msgs::msg::Marker createCircleMarker( const std_msgs::msg::ColorRGBA & color, const std::string & name, const double radius, const double height_offset, const double line_width = 0.1) { - auto marker = createDefaultMarker( + auto marker = create_default_marker( "map", rclcpp::Clock{RCL_ROS_TIME}.now(), name, id, visualization_msgs::msg::Marker::LINE_STRIP, - createMarkerScale(line_width, 0.0, 0.0), color); + create_marker_scale(line_width, 0.0, 0.0), color); constexpr size_t num_points = 20; for (size_t i = 0; i < num_points; ++i) { geometry_msgs::msg::Point point; const double ratio = static_cast(i) / static_cast(num_points); - const double theta = 2 * autoware::universe_utils::pi * ratio; - point.x = position.x + radius * autoware::universe_utils::cos(theta); - point.y = position.y + radius * autoware::universe_utils::sin(theta); + const double theta = 2 * autoware_utils::pi * ratio; + point.x = position.x + radius * autoware_utils::cos(theta); + point.y = position.y + radius * autoware_utils::sin(theta); point.z = position.z + height_offset; marker.points.push_back(point); } @@ -114,9 +114,9 @@ visualization_msgs::msg::Marker createNameTextMarker( const size_t id, const geometry_msgs::msg::Point & position, const std::string & name, const double height_offset, const double text_size = 0.5) { - auto marker = createDefaultMarker( + auto marker = create_default_marker( "map", rclcpp::Clock{RCL_ROS_TIME}.now(), name + "_name_text", id, - visualization_msgs::msg::Marker::TEXT_VIEW_FACING, createMarkerScale(0.0, 0.0, text_size), + visualization_msgs::msg::Marker::TEXT_VIEW_FACING, create_marker_scale(0.0, 0.0, text_size), getGray(0.999)); marker.text = name; diff --git a/visualization/tier4_planning_rviz_plugin/include/tier4_planning_rviz_plugin/path/display.hpp b/visualization/tier4_planning_rviz_plugin/include/tier4_planning_rviz_plugin/path/display.hpp index 92127a2affcf1..e600e5957c3b4 100644 --- a/visualization/tier4_planning_rviz_plugin/include/tier4_planning_rviz_plugin/path/display.hpp +++ b/visualization/tier4_planning_rviz_plugin/include/tier4_planning_rviz_plugin/path/display.hpp @@ -61,22 +61,22 @@ void visualizeBound( const auto [normal_vector_angle, adaptive_width] = [&]() -> std::pair { if (i == 0) { return std::make_pair( - autoware::universe_utils::calcAzimuthAngle(bound.at(i), bound.at(i + 1)) + M_PI_2, width); + autoware_utils::calc_azimuth_angle(bound.at(i), bound.at(i + 1)) + M_PI_2, width); } if (i == bound.size() - 1) { return std::make_pair( - autoware::universe_utils::calcAzimuthAngle(bound.at(i - 1), bound.at(i)) + M_PI_2, width); + autoware_utils::calc_azimuth_angle(bound.at(i - 1), bound.at(i)) + M_PI_2, width); } const auto & prev_p = bound.at(i - 1); const auto & curr_p = bound.at(i); const auto & next_p = bound.at(i + 1); - const float curr_to_prev_angle = autoware::universe_utils::calcAzimuthAngle(curr_p, prev_p); - const float curr_to_next_angle = autoware::universe_utils::calcAzimuthAngle(curr_p, next_p); + const float curr_to_prev_angle = autoware_utils::calc_azimuth_angle(curr_p, prev_p); + const float curr_to_next_angle = autoware_utils::calc_azimuth_angle(curr_p, next_p); const float normal_vector_angle = (curr_to_prev_angle + curr_to_next_angle) / 2.0; const float diff_angle = - autoware::universe_utils::normalizeRadian(normal_vector_angle - curr_to_next_angle); + autoware_utils::normalize_radian(normal_vector_angle - curr_to_next_angle); if (diff_angle <= 1e-7 && diff_angle >= -1e-7) { return std::make_pair(normal_vector_angle, width); } diff --git a/visualization/tier4_planning_rviz_plugin/include/tier4_planning_rviz_plugin/path/display_base.hpp b/visualization/tier4_planning_rviz_plugin/include/tier4_planning_rviz_plugin/path/display_base.hpp index 1fcdacef17697..34370e6ee6946 100644 --- a/visualization/tier4_planning_rviz_plugin/include/tier4_planning_rviz_plugin/path/display_base.hpp +++ b/visualization/tier4_planning_rviz_plugin/include/tier4_planning_rviz_plugin/path/display_base.hpp @@ -91,8 +91,8 @@ bool validateFloats(const typename T::ConstSharedPtr & msg_ptr) { for (auto && path_point : msg_ptr->points) { if ( - !rviz_common::validateFloats(autoware::universe_utils::getPose(path_point)) && - !rviz_common::validateFloats(autoware::universe_utils::getLongitudinalVelocity(path_point))) { + !rviz_common::validateFloats(autoware_utils::get_pose(path_point)) && + !rviz_common::validateFloats(autoware_utils::get_longitudinal_velocity(path_point))) { return false; } } @@ -371,11 +371,10 @@ class AutowarePathBaseDisplay : public rviz_common::MessageFilterDisplay alphas.back() = 0.0f; float cumulative_distance = 0.0f; for (size_t point_idx = msg_ptr->points.size() - 1; point_idx > 0; point_idx--) { - const auto & curr_point = autoware::universe_utils::getPose(msg_ptr->points.at(point_idx)); - const auto & prev_point = - autoware::universe_utils::getPose(msg_ptr->points.at(point_idx - 1)); - float distance = std::sqrt(autoware::universe_utils::calcSquaredDistance2d( - prev_point.position, curr_point.position)); + const auto & curr_point = autoware_utils::get_pose(msg_ptr->points.at(point_idx)); + const auto & prev_point = autoware_utils::get_pose(msg_ptr->points.at(point_idx - 1)); + float distance = std::sqrt( + autoware_utils::calc_squared_distance2d(prev_point.position, curr_point.position)); cumulative_distance += distance; if (cumulative_distance <= property_fade_out_distance_.getFloat()) { @@ -393,8 +392,8 @@ class AutowarePathBaseDisplay : public rviz_common::MessageFilterDisplay // Forward iteration to visualize path for (size_t point_idx = 0; point_idx < msg_ptr->points.size(); point_idx++) { const auto & path_point = msg_ptr->points.at(point_idx); - const auto & pose = autoware::universe_utils::getPose(path_point); - const auto & velocity = autoware::universe_utils::getLongitudinalVelocity(path_point); + const auto & pose = autoware_utils::get_pose(path_point); + const auto & velocity = autoware_utils::get_longitudinal_velocity(path_point); // path if (property_path_view_.getBool()) { @@ -489,9 +488,9 @@ class AutowarePathBaseDisplay : public rviz_common::MessageFilterDisplay (point_idx != msg_ptr->points.size() - 1) ? point_idx + 1 : point_idx; const auto & prev_path_pos = - autoware::universe_utils::getPose(msg_ptr->points.at(prev_idx)).position; + autoware_utils::get_pose(msg_ptr->points.at(prev_idx)).position; const auto & next_path_pos = - autoware::universe_utils::getPose(msg_ptr->points.at(next_idx)).position; + autoware_utils::get_pose(msg_ptr->points.at(next_idx)).position; Ogre::Vector3 position; position.x = pose.position.x; @@ -501,8 +500,7 @@ class AutowarePathBaseDisplay : public rviz_common::MessageFilterDisplay node->setPosition(position); rviz_rendering::MovableText * text = slope_texts_.at(point_idx); - const double slope = - autoware::universe_utils::calcElevationAngle(prev_path_pos, next_path_pos); + const double slope = autoware_utils::calc_elevation_angle(prev_path_pos, next_path_pos); std::stringstream ss; ss << std::fixed << std::setprecision(2) << slope; @@ -546,7 +544,7 @@ class AutowarePathBaseDisplay : public rviz_common::MessageFilterDisplay for (size_t p_idx = 0; p_idx < msg_ptr->points.size(); p_idx++) { const auto & point = msg_ptr->points.at(p_idx); - const auto & pose = autoware::universe_utils::getPose(point); + const auto & pose = autoware_utils::get_pose(point); // footprint if (property_footprint_view_.getBool()) { Ogre::ColourValue color; diff --git a/visualization/tier4_planning_rviz_plugin/include/tier4_planning_rviz_plugin/utils.hpp b/visualization/tier4_planning_rviz_plugin/include/tier4_planning_rviz_plugin/utils.hpp index 7e99af8b9efb6..c0a53d9d2805b 100644 --- a/visualization/tier4_planning_rviz_plugin/include/tier4_planning_rviz_plugin/utils.hpp +++ b/visualization/tier4_planning_rviz_plugin/include/tier4_planning_rviz_plugin/utils.hpp @@ -15,8 +15,8 @@ #ifndef TIER4_PLANNING_RVIZ_PLUGIN__UTILS_HPP_ #define TIER4_PLANNING_RVIZ_PLUGIN__UTILS_HPP_ -#include -#include +#include +#include #include @@ -29,7 +29,7 @@ bool isDrivingForward(const T points_with_twist, size_t target_idx) // 1. check velocity const double target_velocity = - autoware::universe_utils::getLongitudinalVelocity(points_with_twist.at(target_idx)); + autoware_utils::get_longitudinal_velocity(points_with_twist.at(target_idx)); if (epsilon < target_velocity) { return true; } else if (target_velocity < -epsilon) { @@ -46,14 +46,12 @@ bool isDrivingForward(const T points_with_twist, size_t target_idx) const size_t first_idx = is_last_point ? target_idx - 1 : target_idx; const size_t second_idx = is_last_point ? target_idx : target_idx + 1; - const auto first_pose = autoware::universe_utils::getPose(points_with_twist.at(first_idx)); - const auto second_pose = autoware::universe_utils::getPose(points_with_twist.at(second_idx)); + const auto first_pose = autoware_utils::get_pose(points_with_twist.at(first_idx)); + const auto second_pose = autoware_utils::get_pose(points_with_twist.at(second_idx)); const double first_traj_yaw = tf2::getYaw(first_pose.orientation); const double driving_direction_yaw = - autoware::universe_utils::calcAzimuthAngle(first_pose.position, second_pose.position); - if ( - std::abs(autoware::universe_utils::normalizeRadian(first_traj_yaw - driving_direction_yaw)) < - M_PI_2) { + autoware_utils::calc_azimuth_angle(first_pose.position, second_pose.position); + if (std::abs(autoware_utils::normalize_radian(first_traj_yaw - driving_direction_yaw)) < M_PI_2) { return true; } diff --git a/visualization/tier4_planning_rviz_plugin/package.xml b/visualization/tier4_planning_rviz_plugin/package.xml index 352e3bfbf5840..a05d56276c76a 100644 --- a/visualization/tier4_planning_rviz_plugin/package.xml +++ b/visualization/tier4_planning_rviz_plugin/package.xml @@ -13,7 +13,7 @@ autoware_motion_utils autoware_planning_msgs - autoware_universe_utils + autoware_utils autoware_vehicle_info_utils libqt5-core libqt5-gui diff --git a/visualization/tier4_system_rviz_plugin/package.xml b/visualization/tier4_system_rviz_plugin/package.xml index 9493e5b1c45a3..2eab477277d22 100644 --- a/visualization/tier4_system_rviz_plugin/package.xml +++ b/visualization/tier4_system_rviz_plugin/package.xml @@ -11,7 +11,7 @@ autoware_cmake autoware_system_msgs - autoware_universe_utils + autoware_utils diagnostic_msgs libqt5-core libqt5-gui diff --git a/visualization/tier4_vehicle_rviz_plugin/package.xml b/visualization/tier4_vehicle_rviz_plugin/package.xml index 91c34ec8ca5b4..2758eb28ba87a 100644 --- a/visualization/tier4_vehicle_rviz_plugin/package.xml +++ b/visualization/tier4_vehicle_rviz_plugin/package.xml @@ -11,7 +11,7 @@ autoware_cmake ament_index_cpp - autoware_universe_utils + autoware_utils autoware_vehicle_msgs libqt5-core libqt5-gui diff --git a/visualization/tier4_vehicle_rviz_plugin/src/tools/acceleration_meter.cpp b/visualization/tier4_vehicle_rviz_plugin/src/tools/acceleration_meter.cpp index 1d884d01065fa..02e0da53b63e7 100644 --- a/visualization/tier4_vehicle_rviz_plugin/src/tools/acceleration_meter.cpp +++ b/visualization/tier4_vehicle_rviz_plugin/src/tools/acceleration_meter.cpp @@ -231,14 +231,14 @@ void AccelerationMeterDisplay::updateVisualization() inner_arc_.y0 = line_width_ / 2.0; inner_arc_.x1 = w; inner_arc_.y1 = h; - inner_arc_.start_angle = autoware::universe_utils::rad2deg(min_range_theta - M_PI); - inner_arc_.end_angle = autoware::universe_utils::rad2deg(max_range_theta - min_range_theta); + inner_arc_.start_angle = autoware_utils::rad2deg(min_range_theta - M_PI); + inner_arc_.end_angle = autoware_utils::rad2deg(max_range_theta - min_range_theta); outer_arc_.x0 = w * 3 / 8; outer_arc_.y0 = h * 3 / 8; outer_arc_.x1 = w / 4; outer_arc_.y1 = h / 4; - outer_arc_.start_angle = autoware::universe_utils::rad2deg(min_range_theta - M_PI); - outer_arc_.end_angle = autoware::universe_utils::rad2deg(max_range_theta - min_range_theta); + outer_arc_.start_angle = autoware_utils::rad2deg(min_range_theta - M_PI); + outer_arc_.end_angle = autoware_utils::rad2deg(max_range_theta - min_range_theta); } } // namespace rviz_plugins diff --git a/visualization/tier4_vehicle_rviz_plugin/src/tools/acceleration_meter.hpp b/visualization/tier4_vehicle_rviz_plugin/src/tools/acceleration_meter.hpp index 03fef97f536a5..68aab170c8eaa 100644 --- a/visualization/tier4_vehicle_rviz_plugin/src/tools/acceleration_meter.hpp +++ b/visualization/tier4_vehicle_rviz_plugin/src/tools/acceleration_meter.hpp @@ -21,7 +21,7 @@ #ifndef Q_MOC_RUN #include "jsk_overlay_utils.hpp" -#include +#include #include #include #include @@ -67,8 +67,8 @@ private Q_SLOTS: private: static constexpr float meter_min_acceleration_ = -10.0f; static constexpr float meter_max_acceleration_ = 10.0f; - static constexpr float meter_min_angle_ = autoware::universe_utils::deg2rad(40.f); - static constexpr float meter_max_angle_ = autoware::universe_utils::deg2rad(320.f); + static constexpr float meter_min_angle_ = autoware_utils::deg2rad(40.f); + static constexpr float meter_max_angle_ = autoware_utils::deg2rad(320.f); static constexpr int line_width_ = 2; static constexpr int hand_width_ = 4; struct Line // for drawLine diff --git a/visualization/tier4_vehicle_rviz_plugin/src/tools/console_meter.cpp b/visualization/tier4_vehicle_rviz_plugin/src/tools/console_meter.cpp index 8f7348c87be3c..5029625c9dc95 100644 --- a/visualization/tier4_vehicle_rviz_plugin/src/tools/console_meter.cpp +++ b/visualization/tier4_vehicle_rviz_plugin/src/tools/console_meter.cpp @@ -212,14 +212,14 @@ void ConsoleMeterDisplay::updateVisualization() inner_arc_.y0 = line_width_ / 2.0; inner_arc_.x1 = w; inner_arc_.y1 = h; - inner_arc_.start_angle = autoware::universe_utils::rad2deg(min_range_theta - M_PI); - inner_arc_.end_angle = autoware::universe_utils::rad2deg(max_range_theta - min_range_theta); + inner_arc_.start_angle = autoware_utils::rad2deg(min_range_theta - M_PI); + inner_arc_.end_angle = autoware_utils::rad2deg(max_range_theta - min_range_theta); outer_arc_.x0 = w * 3 / 8; outer_arc_.y0 = h * 3 / 8; outer_arc_.x1 = w / 4; outer_arc_.y1 = h / 4; - outer_arc_.start_angle = autoware::universe_utils::rad2deg(min_range_theta - M_PI); - outer_arc_.end_angle = autoware::universe_utils::rad2deg(max_range_theta - min_range_theta); + outer_arc_.start_angle = autoware_utils::rad2deg(min_range_theta - M_PI); + outer_arc_.end_angle = autoware_utils::rad2deg(max_range_theta - min_range_theta); } } // namespace rviz_plugins diff --git a/visualization/tier4_vehicle_rviz_plugin/src/tools/console_meter.hpp b/visualization/tier4_vehicle_rviz_plugin/src/tools/console_meter.hpp index 98cf8bbae4228..aeeea66f39eb9 100644 --- a/visualization/tier4_vehicle_rviz_plugin/src/tools/console_meter.hpp +++ b/visualization/tier4_vehicle_rviz_plugin/src/tools/console_meter.hpp @@ -21,8 +21,8 @@ #ifndef Q_MOC_RUN #include "jsk_overlay_utils.hpp" -#include -#include +#include +#include #include #include #include @@ -63,10 +63,10 @@ private Q_SLOTS: // QImage hud_; private: - static constexpr float meter_min_velocity_ = autoware::universe_utils::kmph2mps(0.f); - static constexpr float meter_max_velocity_ = autoware::universe_utils::kmph2mps(60.f); - static constexpr float meter_min_angle_ = autoware::universe_utils::deg2rad(40.f); - static constexpr float meter_max_angle_ = autoware::universe_utils::deg2rad(320.f); + static constexpr float meter_min_velocity_ = autoware_utils::kmph2mps(0.f); + static constexpr float meter_max_velocity_ = autoware_utils::kmph2mps(60.f); + static constexpr float meter_min_angle_ = autoware_utils::deg2rad(40.f); + static constexpr float meter_max_angle_ = autoware_utils::deg2rad(320.f); static constexpr int line_width_ = 2; static constexpr int hand_width_ = 4; struct Line // for drawLine