From ae8126bed8244c9b71dbe35097c7aa858a6380d4 Mon Sep 17 00:00:00 2001 From: Tomoya Kimura Date: Tue, 14 Dec 2021 18:36:15 +0900 Subject: [PATCH] feat: rename existing packages name starting with autoware to different names (#180) * autoware_api_utils -> tier4_api_utils Signed-off-by: tomoya.kimura * autoware_debug_tools -> tier4_debug_tools Signed-off-by: tomoya.kimura * autoware_error_monitor -> system_error_monitor Signed-off-by: tomoya.kimura * autoware_utils -> tier4_autoware_utils Signed-off-by: tomoya.kimura * autoware_global_parameter_loader -> global_parameter_loader Signed-off-by: tomoya.kimura * autoware_iv_auto_msgs_converter -> tier4_auto_msgs_converter Signed-off-by: tomoya.kimura * autoware_joy_controller -> joy_controller Signed-off-by: tomoya.kimura * autoware_error_monitor -> system_error_monitor(launch) Signed-off-by: tomoya.kimura * autoware_state_monitor -> ad_service_state_monitor Signed-off-by: tomoya.kimura * autoware_web_controller -> web_controller Signed-off-by: tomoya.kimura * remove autoware_version Signed-off-by: tomoya.kimura * remove autoware_rosbag_recorder Signed-off-by: tomoya.kimura * autoware_*_rviz_plugin -> tier4_*_rviz_plugin Signed-off-by: tomoya.kimura * fix ad_service_state_monitor Signed-off-by: tomoya.kimura * ci(pre-commit): autofix Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- awapi/awapi_awiv_adapter/package.xml | 2 +- .../src/awapi_autoware_state_publisher.cpp | 8 +- .../src/awapi_lane_change_state_publisher.cpp | 4 +- ...api_obstacle_avoidance_state_publisher.cpp | 4 +- .../src/awapi_vehicle_state_publisher.cpp | 8 +- .../scripts/stop_reason2tf | 15 -- .../autoware_rosbag_recorder/CMakeLists.txt | 17 --- common/autoware_rosbag_recorder/Readme.md | 21 --- common/autoware_rosbag_recorder/package.xml | 18 --- .../scripts/record.sh | 21 --- .../include/autoware_utils/autoware_utils.hpp | 37 ----- .../CMakeLists.txt | 2 +- .../Readme.md | 2 +- .../launch/global_params.launch.py | 0 .../package.xml | 4 +- .../goal_distance_calculator.hpp | 4 +- .../goal_distance_calculator_node.hpp | 8 +- common/goal_distance_calculator/package.xml | 2 +- .../src/goal_distance_calculator.cpp | 2 +- .../src/goal_distance_calculator_node.cpp | 4 +- common/path_distance_calculator/package.xml | 2 +- .../src/path_distance_calculator.cpp | 4 +- .../src/path_distance_calculator.hpp | 4 +- .../CMakeLists.txt | 2 +- .../tier4_api_utils}/rclcpp/client.hpp | 12 +- .../include/tier4_api_utils}/rclcpp/proxy.hpp | 14 +- .../tier4_api_utils}/rclcpp/service.hpp | 10 +- .../tier4_api_utils/tier4_api_utils.hpp} | 10 +- .../tier4_api_utils}/types/response.hpp | 10 +- .../package.xml | 4 +- .../test/test.cpp | 8 +- .../CMakeLists.txt | 12 +- .../README.md | 2 +- .../geometry/boost_geometry.hpp | 18 +-- .../geometry/geometry.hpp | 12 +- .../geometry/pose_deviation.hpp | 14 +- .../tier4_autoware_utils}/math/constants.hpp | 10 +- .../math/normalization.hpp | 12 +- .../tier4_autoware_utils}/math/range.hpp | 10 +- .../math/unit_conversion.hpp | 12 +- .../planning/planning_marker_helper.hpp | 12 +- .../ros/debug_publisher.hpp | 16 +-- .../ros/debug_traits.hpp | 10 +- .../ros/marker_helper.hpp | 10 +- .../ros/processing_time_publisher.hpp | 10 +- .../ros/self_pose_listener.hpp | 14 +- .../ros/transform_listener.hpp | 10 +- .../ros/update_param.hpp | 10 +- .../ros/wait_for_param.hpp | 10 +- .../system/heartbeat_checker.hpp | 6 +- .../system/stop_watch.hpp | 10 +- .../tier4_autoware_utils.hpp | 37 +++++ .../trajectory/tmp_conversion.hpp | 14 +- .../trajectory/trajectory.hpp | 14 +- .../package.xml | 4 +- .../src/planning/planning_marker_helper.cpp | 12 +- .../src/tier4_autoware_utils.cpp} | 2 +- .../test/src/geometry/test_boost_geometry.cpp | 10 +- .../test/src/geometry/test_geometry.cpp | 128 +++++++++--------- .../test/src/geometry/test_pose_deviation.cpp | 10 +- .../test/src/math/test_constants.cpp | 6 +- .../test/src/math/test_normalization.cpp | 6 +- .../test/src/math/test_range.cpp | 14 +- .../test/src/math/test_unit_conversion.cpp | 12 +- .../test/src/system/test_stop_watch.cpp | 6 +- .../test/src/test_autoware_utils.cpp | 2 +- .../test/src/trajectory/test_trajectory.cpp | 52 +++---- .../CMakeLists.txt | 2 +- .../README.md | 4 +- .../icons/classes/AutowareDateTimePanel.png | Bin .../images/select_datetime_plugin.png | Bin .../images/select_panels.png | Bin .../package.xml | 4 +- .../plugins/plugin_description.xml | 2 +- .../src/autoware_datetime_panel.cpp | 0 .../src/autoware_datetime_panel.hpp | 0 .../CMakeLists.txt | 2 +- .../README.md | 20 +-- .../config/lateral_error_publisher.param.yaml | 0 .../lateral_error_publisher.hpp | 8 +- .../launch/lateral_error_publisher.launch.xml | 4 +- .../media/lateral_error_publisher.svg | 0 .../package.xml | 6 +- .../scripts/case_converter.py | 0 .../scripts/pose2tf.py | 0 .../scripts/self_pose_listener.py | 0 .../scripts/stop_reason2pose.py | 4 +- .../tier4_debug_tools/scripts/stop_reason2tf | 15 ++ .../scripts/tf2pose.py | 2 +- .../src/lateral_error_publisher.cpp | 4 +- .../CMakeLists.txt | 2 +- .../README.md | 4 +- .../icons/classes/PoseHistory.png | Bin .../images/select_add.png | Bin .../images/select_localization_plugin.png | Bin .../images/select_topic_name.png | Bin .../package.xml | 4 +- .../plugins/plugin_description.xml | 2 +- .../src/pose_history/pose_history_display.cpp | 0 .../src/pose_history/pose_history_display.hpp | 0 .../CMakeLists.txt | 6 +- .../README.md | 4 +- .../icons/classes/CarInitialPoseTool.png | Bin .../icons/classes/DeleteAllObjectsTool.png | Bin .../classes/PedestrianInitialPoseTool.png | Bin .../icons/classes/UnknownInitialPoseTool.png | Bin .../images/select_add.png | Bin .../images/select_dummy_car.png | Bin .../images/select_plugin.png | Bin .../package.xml | 4 +- .../plugins/plugin_description.xml | 2 +- .../src/tools/car_pose.cpp | 0 .../src/tools/car_pose.hpp | 0 .../src/tools/delete_all_objects.cpp | 0 .../src/tools/delete_all_objects.hpp | 0 .../src/tools/pedestrian_pose.cpp | 0 .../src/tools/pedestrian_pose.hpp | 0 .../src/tools/unknown_pose.cpp | 0 .../src/tools/unknown_pose.hpp | 0 .../CMakeLists.txt | 6 +- .../README.md | 4 +- .../icons/classes/MaxVelocity.png | Bin .../icons/classes/MissionCheckpointTool.png | Bin .../icons/classes/Path.png | Bin .../icons/classes/PathFootprint.png | Bin .../icons/classes/Trajectory.png | Bin .../icons/classes/TrajectoryFootprint.png | Bin .../images/select_add.png | Bin .../images/select_planning_plugin.png | Bin .../images/select_topic_name.png | Bin .../mission_checkpoint/mission_checkpoint.hpp | 0 .../include/path/display.hpp | 0 .../include/path_footprint/display.hpp | 0 .../include/trajectory/display.hpp | 0 .../include/trajectory_footprint/display.hpp | 0 .../package.xml | 4 +- .../plugins/plugin_description.xml | 2 +- .../mission_checkpoint/mission_checkpoint.cpp | 0 .../src/path/display.cpp | 0 .../src/path_footprint/display.cpp | 0 .../src/tools/jsk_overlay_utils.cpp | 0 .../src/tools/jsk_overlay_utils.hpp | 0 .../src/tools/max_velocity.cpp | 0 .../src/tools/max_velocity.hpp | 0 .../src/trajectory/display.cpp | 0 .../src/trajectory_footprint/display.cpp | 0 .../CMakeLists.txt | 2 +- .../README.md | 4 +- .../icons/classes/AutowareStatePanel.png | Bin .../images/select_engage.png | Bin .../images/select_panels.png | Bin .../images/select_state_plugin.png | Bin .../package.xml | 2 +- .../plugins/plugin_description.xml | 2 +- .../src/autoware_state_panel.cpp | 0 .../src/autoware_state_panel.hpp | 0 .../CMakeLists.txt | 6 +- .../README.md | 4 +- .../icons/classes/ConsoleMeter.png | Bin .../icons/classes/SteeringAngle.png | Bin .../icons/classes/TurnSignal.png | Bin .../icons/classes/VelocityHistory.png | Bin .../images/handle.png | Bin .../images/select_add.png | Bin .../images/select_topic_name.png | Bin .../images/select_vehicle_plugin.png | Bin .../package.xml | 6 +- .../plugins/plugin_description.xml | 2 +- .../src/tools/console_meter.cpp | 8 +- .../src/tools/console_meter.hpp | 10 +- .../src/tools/jsk_overlay_utils.cpp | 0 .../src/tools/jsk_overlay_utils.hpp | 0 .../src/tools/steering_angle.cpp | 2 +- .../src/tools/steering_angle.hpp | 0 .../src/tools/turn_signal.cpp | 0 .../src/tools/turn_signal.hpp | 0 .../src/tools/velocity_history.cpp | 0 .../src/tools/velocity_history.hpp | 0 .../CMakeLists.txt | 2 +- .../README.md | 4 +- .../images/web_controller.png | Bin .../launch/web_controller.launch.xml} | 2 +- .../package.xml | 4 +- .../css/web_controller_style.css} | 0 .../www/web_controller}/icon/autoware.ico | Bin .../www/web_controller}/index.html | 2 +- .../www/web_controller}/js/autoware_engage.js | 0 .../www/web_controller}/js/autoware_state.js | 0 .../js/lane_change_approval.js | 0 .../www/web_controller}/js/menu.js | 0 .../js/path_change_approval.js | 0 .../www/web_controller}/js/vehicle_engage.js | 0 .../www/web_controller}/js/velocity_limit.js | 0 .../control_performance_analysis_node.hpp | 4 +- .../control_performance_analysis/package.xml | 4 +- control/external_cmd_selector/package.xml | 2 +- .../external_cmd_selector_node.cpp | 8 +- .../CMakeLists.txt | 12 +- .../README.md | 4 +- .../config/joy_controller.param.yaml} | 0 .../joy_controller/joy_controller.hpp} | 12 +- .../joy_converter/ds4_joy_converter.hpp | 12 +- .../joy_converter/g29_joy_converter.hpp | 12 +- .../joy_converter/joy_converter_base.hpp | 10 +- .../launch/joy_controller.launch.xml} | 4 +- .../package.xml | 6 +- .../joy_controller/joy_controller_node.cpp} | 24 ++-- .../lane_departure_checker.hpp | 8 +- .../lane_departure_checker_node.hpp | 12 +- .../util/create_vehicle_footprint.hpp | 8 +- control/lane_departure_checker/package.xml | 2 +- .../lane_departure_checker.cpp | 32 ++--- .../lane_departure_checker_node.cpp | 16 +-- .../obstacle_collision_checker.hpp | 4 +- .../obstacle_collision_checker_node.hpp | 18 +-- .../util/create_vehicle_footprint.hpp | 8 +- .../obstacle_collision_checker/package.xml | 2 +- .../obstacle_collision_checker.cpp | 28 ++-- .../obstacle_collision_checker_node.cpp | 20 +-- .../src/longitudinal_controller_node.cpp | 2 +- control/vehicle_cmd_gate/package.xml | 2 +- .../vehicle_cmd_gate/src/vehicle_cmd_gate.cpp | 8 +- launch/system_launch/README.md | 2 +- launch/system_launch/launch/system.launch.xml | 12 +- launch/system_launch/package.xml | 4 +- launch/system_launch/system_launch.drawio.svg | 32 ++--- .../include/ekf_localizer/ekf_localizer.hpp | 6 +- localization/ekf_localizer/package.xml | 2 +- .../ekf_localizer/src/ekf_localizer.cpp | 13 +- .../plugins/plugin_description.xml | 4 +- .../src/initial_pose_button_panel.cpp | 7 +- .../src/initial_pose_button_panel.hpp | 4 +- localization/ndt_scan_matcher/package.xml | 2 +- .../src/ndt_scan_matcher_core.cpp | 14 +- .../pose_initializer_core.hpp | 2 +- localization/pose_initializer/package.xml | 2 +- .../src/pose_initializer_core.cpp | 2 +- map/map_loader/package.xml | 2 +- .../CMakeLists.txt | 2 +- .../tier4_auto_msgs_converter.hpp} | 10 +- .../package.xml | 4 +- .../detection_by_tracker_core.hpp | 2 +- .../include/detection_by_tracker/utils.hpp | 2 +- perception/detection_by_tracker/package.xml | 2 +- .../src/detection_by_tracker_core.cpp | 8 +- perception/detection_by_tracker/src/utils.cpp | 42 +++--- .../elevation_map_loader_node.hpp | 6 +- perception/elevation_map_loader/package.xml | 2 +- .../src/elevation_map_loader_node.cpp | 11 +- .../src/scan_ground_filter_nodelet.cpp | 12 +- .../lib/include/centerpoint_trt.hpp | 2 +- .../lib/src/centerpoint_trt.cpp | 4 +- perception/lidar_centerpoint/package.xml | 2 +- perception/lidar_centerpoint/src/node.cpp | 8 +- perception/map_based_prediction/package.xml | 2 +- .../src/map_based_prediction.cpp | 16 +-- .../src/map_based_prediction_ros.cpp | 32 +++-- .../multi_object_tracker/utils/utils.hpp | 2 +- perception/multi_object_tracker/package.xml | 2 +- .../src/data_association/data_association.cpp | 5 +- .../src/tracker/model/bicycle_tracker.cpp | 38 +++--- .../src/tracker/model/big_vehicle_tracker.cpp | 36 ++--- .../model/multiple_vehicle_tracker.cpp | 2 +- .../tracker/model/normal_vehicle_tracker.cpp | 36 ++--- .../model/pedestrian_and_bicycle_tracker.cpp | 2 +- .../src/tracker/model/pedestrian_tracker.cpp | 38 +++--- .../src/tracker/model/unknown_tracker.cpp | 26 ++-- .../multi_object_tracker/src/utils/utils.cpp | 36 ++--- .../package.xml | 2 +- ...upancy_grid_map_outlier_filter_nodelet.cpp | 4 +- .../shape_estimation/corrector/utils.hpp | 2 +- .../model/model_interface.hpp | 2 +- .../shape_estimation/lib/corrector/utils.cpp | 4 +- .../lib/model/bounding_box.cpp | 4 +- perception/shape_estimation/package.xml | 2 +- .../package.xml | 2 +- .../src/node.cpp | 10 +- .../behavior_path_planner_node.hpp | 4 +- .../path_shifter/path_shifter.hpp | 2 +- .../scene_module/avoidance/debug.hpp | 2 +- .../scene_module/pull_out/pull_out_module.hpp | 2 +- .../scene_module/pull_out/util.hpp | 4 +- .../pull_over/pull_over_module.hpp | 2 +- .../util/create_vehicle_footprint.hpp | 8 +- .../behavior_path_planner/utilities.hpp | 14 +- planning/behavior_path_planner/package.xml | 2 +- .../src/behavior_path_planner_node.cpp | 6 +- .../src/path_shifter/path_shifter.cpp | 10 +- .../src/path_utilities.cpp | 18 +-- .../avoidance/avoidance_module.cpp | 18 +-- .../avoidance/avoidance_utils.cpp | 17 +-- .../src/scene_module/avoidance/debug.cpp | 74 +++++----- .../lane_change/lane_change_module.cpp | 4 +- .../src/scene_module/lane_change/util.cpp | 8 +- .../scene_module/pull_out/pull_out_module.cpp | 16 ++- .../src/scene_module/pull_out/util.cpp | 14 +- .../pull_over/pull_over_module.cpp | 4 +- .../src/scene_module/pull_over/util.cpp | 16 +-- .../side_shift/side_shift_module.cpp | 8 +- .../src/turn_signal_decider.cpp | 5 +- .../behavior_path_planner/src/utilities.cpp | 48 +++---- .../behavior_velocity_planner/CMakeLists.txt | 2 +- .../intersection/scene_intersection.hpp | 2 +- .../scene_merge_from_private_road.hpp | 2 +- .../virtual_traffic_light/scene.hpp | 10 +- .../include/utilization/arc_lane_util.hpp | 10 +- .../include/utilization/path_utilization.hpp | 2 +- .../behavior_velocity_planner/package.xml | 2 +- .../intersection/scene_intersection.cpp | 8 +- .../scene_merge_from_private_road.cpp | 2 +- .../scene_module/no_stopping_area/debug.cpp | 4 +- .../scene_no_stopping_area.cpp | 7 +- .../src/scene_module/occlusion_spot/debug.cpp | 38 +++--- .../occlusion_spot/occlusion_spot_utils.cpp | 10 +- .../virtual_traffic_light/debug.cpp | 28 ++-- .../virtual_traffic_light/scene.cpp | 53 ++++---- .../src/utilization/path_utilization.cpp | 2 +- .../test/src/test_occlusion_spot_utils.cpp | 4 +- planning/freespace_planner/package.xml | 2 +- .../freespace_planner_node.cpp | 12 +- .../freespace_planning_algorithms/package.xml | 2 +- .../src/abstract_algorithm.cpp | 6 +- .../src/astar_search.cpp | 16 ++- .../motion_velocity_smoother_node.hpp | 16 +-- .../motion_velocity_smoother/resample.hpp | 2 +- .../analytical_jerk_constrained_smoother.hpp | 2 +- .../velocity_planning_utils.hpp | 2 +- .../smoother/jerk_filtered_smoother.hpp | 4 +- .../smoother/l2_pseudo_jerk_smoother.hpp | 4 +- .../smoother/linf_pseudo_jerk_smoother.hpp | 4 +- .../smoother/smoother_base.hpp | 4 +- .../trajectory_utils.hpp | 4 +- planning/motion_velocity_smoother/package.xml | 2 +- .../src/motion_velocity_smoother_node.cpp | 50 +++---- .../motion_velocity_smoother/src/resample.cpp | 12 +- .../analytical_jerk_constrained_smoother.cpp | 11 +- .../velocity_planning_utils.cpp | 3 +- .../src/smoother/jerk_filtered_smoother.cpp | 6 +- .../src/trajectory_utils.cpp | 20 +-- .../obstacle_avoidance_planner/util.hpp | 2 +- .../obstacle_avoidance_planner/package.xml | 2 +- .../obstacle_avoidance_planner/src/node.cpp | 9 +- .../obstacle_avoidance_planner/src/util.cpp | 8 +- .../include/obstacle_stop_planner/node.hpp | 8 +- planning/obstacle_stop_planner/package.xml | 2 +- .../src/debug_marker.cpp | 20 +-- planning/obstacle_stop_planner/src/node.cpp | 18 +-- planning/planning_error_monitor/package.xml | 2 +- .../src/debug_marker.cpp | 10 +- .../src/planning_error_monitor_node.cpp | 6 +- .../include/route_handler/route_handler.hpp | 2 +- planning/route_handler/package.xml | 2 +- planning/route_handler/src/route_handler.cpp | 18 +-- .../surround_obstacle_checker/node.hpp | 2 +- .../surround_obstacle_checker/package.xml | 2 +- .../surround_obstacle_checker/src/node.cpp | 4 +- .../package.xml | 2 +- .../laserscan_to_occupancy_grid_map_node.cpp | 4 +- .../lanelet2_map_filter_nodelet.hpp | 8 +- sensing/pointcloud_preprocessor/package.xml | 2 +- simulator/fault_injection/package.xml | 2 +- .../fault_injection_node.cpp | 4 +- .../simple_planning_simulator_core.hpp | 4 +- .../simple_planning_simulator/package.xml | 4 +- .../simple_planning_simulator_core.cpp | 10 +- .../CMakeLists.txt | 16 +-- .../README.md | 2 +- .../ad_service_state_monitor.param.yaml} | 0 ...te_monitor.planning_simulation.param.yaml} | 0 .../ad_service_state.hpp} | 6 +- .../ad_service_state_monitor_node.hpp} | 12 +- .../ad_service_state_monitor}/config.hpp | 6 +- .../ad_service_state_monitor}/module_name.hpp | 6 +- .../state_machine.hpp | 16 +-- .../ad_service_state_monitor.launch.xml} | 4 +- .../package.xml | 6 +- .../ad_service_state_monitor_node.cpp} | 6 +- .../diagnostics.cpp | 4 +- .../ad_service_state_monitor_node}/main.cpp | 2 +- .../state_machine.cpp | 4 +- system/autoware_version/CMakeLists.txt | 17 --- system/autoware_version/README.md | 9 -- system/autoware_version/package.xml | 20 --- system/autoware_version/script/print | 14 -- system/dummy_diag_publisher/package.xml | 2 +- .../dummy_diag_publisher_node.cpp | 6 +- .../emergency_handler_core.hpp | 2 +- system/emergency_handler/package.xml | 2 +- .../CMakeLists.txt | 12 +- .../README.md | 28 ++-- .../diagnostic_aggregator/_empty.param.yaml | 0 .../diagnostic_aggregator/control.param.yaml | 0 .../localization.param.yaml | 0 .../diagnostic_aggregator/map.param.yaml | 0 .../perception.param.yaml | 0 .../diagnostic_aggregator/planning.param.yaml | 0 .../diagnostic_aggregator/sensing.param.yaml | 0 .../diagnostic_aggregator/system.param.yaml | 0 .../diagnostic_aggregator/vehicle.param.yaml | 0 .../config/system_error_monitor.param.yaml} | 0 ...or_monitor.planning_simulation.param.yaml} | 0 .../diagnostics_filter.hpp | 6 +- .../system_error_monitor_core.hpp} | 6 +- .../launch/system_error_monitor.launch.xml} | 24 ++-- .../system_error_monitor_node.launch.xml} | 4 +- .../package.xml | 6 +- .../src/system_error_monitor_core.cpp} | 12 +- .../src/system_error_monitor_node.cpp} | 2 +- .../accel_brake_map_calibrator_node.hpp | 6 +- .../accel_brake_map_calibrator/package.xml | 2 +- .../src/accel_brake_map_calibrator_node.cpp | 2 +- 411 files changed, 1404 insertions(+), 1514 deletions(-) delete mode 100755 common/autoware_debug_tools/scripts/stop_reason2tf delete mode 100644 common/autoware_rosbag_recorder/CMakeLists.txt delete mode 100644 common/autoware_rosbag_recorder/Readme.md delete mode 100644 common/autoware_rosbag_recorder/package.xml delete mode 100755 common/autoware_rosbag_recorder/scripts/record.sh delete mode 100644 common/autoware_utils/include/autoware_utils/autoware_utils.hpp rename common/{autoware_global_parameter_loader => global_parameter_loader}/CMakeLists.txt (86%) rename common/{autoware_global_parameter_loader => global_parameter_loader}/Readme.md (83%) rename common/{autoware_global_parameter_loader => global_parameter_loader}/launch/global_params.launch.py (100%) rename common/{autoware_global_parameter_loader => global_parameter_loader}/package.xml (78%) rename common/{autoware_api_utils => tier4_api_utils}/CMakeLists.txt (96%) rename common/{autoware_api_utils/include/autoware_api_utils => tier4_api_utils/include/tier4_api_utils}/rclcpp/client.hpp (88%) rename common/{autoware_api_utils/include/autoware_api_utils => tier4_api_utils/include/tier4_api_utils}/rclcpp/proxy.hpp (86%) rename common/{autoware_api_utils/include/autoware_api_utils => tier4_api_utils/include/tier4_api_utils}/rclcpp/service.hpp (86%) rename common/{autoware_api_utils/include/autoware_api_utils/autoware_api_utils.hpp => tier4_api_utils/include/tier4_api_utils/tier4_api_utils.hpp} (69%) rename common/{autoware_api_utils/include/autoware_api_utils => tier4_api_utils/include/tier4_api_utils}/types/response.hpp (91%) rename common/{autoware_api_utils => tier4_api_utils}/package.xml (89%) rename common/{autoware_api_utils => tier4_api_utils}/test/test.cpp (80%) rename common/{autoware_utils => tier4_autoware_utils}/CMakeLists.txt (72%) rename common/{autoware_utils => tier4_autoware_utils}/README.md (83%) rename common/{autoware_utils/include/autoware_utils => tier4_autoware_utils/include/tier4_autoware_utils}/geometry/boost_geometry.hpp (86%) rename common/{autoware_utils/include/autoware_utils => tier4_autoware_utils/include/tier4_autoware_utils}/geometry/geometry.hpp (97%) rename common/{autoware_utils/include/autoware_utils => tier4_autoware_utils/include/tier4_autoware_utils}/geometry/pose_deviation.hpp (87%) rename common/{autoware_utils/include/autoware_utils => tier4_autoware_utils/include/tier4_autoware_utils}/math/constants.hpp (76%) rename common/{autoware_utils/include/autoware_utils => tier4_autoware_utils/include/tier4_autoware_utils}/math/normalization.hpp (80%) rename common/{autoware_utils/include/autoware_utils => tier4_autoware_utils/include/tier4_autoware_utils}/math/range.hpp (90%) rename common/{autoware_utils/include/autoware_utils => tier4_autoware_utils/include/tier4_autoware_utils}/math/unit_conversion.hpp (75%) rename common/{autoware_utils/include/autoware_utils => tier4_autoware_utils/include/tier4_autoware_utils}/planning/planning_marker_helper.hpp (78%) rename common/{autoware_utils/include/autoware_utils => tier4_autoware_utils/include/tier4_autoware_utils}/ros/debug_publisher.hpp (83%) rename common/{autoware_utils/include/autoware_utils => tier4_autoware_utils/include/tier4_autoware_utils}/ros/debug_traits.hpp (89%) rename common/{autoware_utils/include/autoware_utils => tier4_autoware_utils/include/tier4_autoware_utils}/ros/marker_helper.hpp (91%) rename common/{autoware_utils/include/autoware_utils => tier4_autoware_utils/include/tier4_autoware_utils}/ros/processing_time_publisher.hpp (86%) rename common/{autoware_utils/include/autoware_utils => tier4_autoware_utils/include/tier4_autoware_utils}/ros/self_pose_listener.hpp (78%) rename common/{autoware_utils/include/autoware_utils => tier4_autoware_utils/include/tier4_autoware_utils}/ros/transform_listener.hpp (91%) rename common/{autoware_utils/include/autoware_utils => tier4_autoware_utils/include/tier4_autoware_utils}/ros/update_param.hpp (82%) rename common/{autoware_utils/include/autoware_utils => tier4_autoware_utils/include/tier4_autoware_utils}/ros/wait_for_param.hpp (85%) rename common/{autoware_utils/include/autoware_utils => tier4_autoware_utils/include/tier4_autoware_utils}/system/heartbeat_checker.hpp (89%) rename common/{autoware_utils/include/autoware_utils => tier4_autoware_utils/include/tier4_autoware_utils}/system/stop_watch.hpp (88%) create mode 100644 common/tier4_autoware_utils/include/tier4_autoware_utils/tier4_autoware_utils.hpp rename common/{autoware_utils/include/autoware_utils => tier4_autoware_utils/include/tier4_autoware_utils}/trajectory/tmp_conversion.hpp (87%) rename common/{autoware_utils/include/autoware_utils => tier4_autoware_utils/include/tier4_autoware_utils}/trajectory/trajectory.hpp (95%) rename common/{autoware_utils => tier4_autoware_utils}/package.xml (89%) rename common/{autoware_utils => tier4_autoware_utils}/src/planning/planning_marker_helper.cpp (90%) rename common/{autoware_utils/src/autoware_utils.cpp => tier4_autoware_utils/src/tier4_autoware_utils.cpp} (91%) rename common/{autoware_utils => tier4_autoware_utils}/test/src/geometry/test_boost_geometry.cpp (89%) rename common/{autoware_utils => tier4_autoware_utils}/test/src/geometry/test_geometry.cpp (86%) rename common/{autoware_utils => tier4_autoware_utils}/test/src/geometry/test_pose_deviation.cpp (82%) rename common/{autoware_utils => tier4_autoware_utils}/test/src/math/test_constants.cpp (85%) rename common/{autoware_utils => tier4_autoware_utils}/test/src/math/test_normalization.cpp (94%) rename common/{autoware_utils => tier4_autoware_utils}/test/src/math/test_range.cpp (96%) rename common/{autoware_utils => tier4_autoware_utils}/test/src/math/test_unit_conversion.cpp (86%) rename common/{autoware_utils => tier4_autoware_utils}/test/src/system/test_stop_watch.cpp (92%) rename common/{autoware_utils => tier4_autoware_utils}/test/src/test_autoware_utils.cpp (92%) rename common/{autoware_utils => tier4_autoware_utils}/test/src/trajectory/test_trajectory.cpp (93%) rename common/{autoware_datetime_rviz_plugin => tier4_datetime_rviz_plugin}/CMakeLists.txt (96%) rename common/{autoware_datetime_rviz_plugin => tier4_datetime_rviz_plugin}/README.md (72%) rename common/{autoware_datetime_rviz_plugin => tier4_datetime_rviz_plugin}/icons/classes/AutowareDateTimePanel.png (100%) rename common/{autoware_datetime_rviz_plugin => tier4_datetime_rviz_plugin}/images/select_datetime_plugin.png (100%) rename common/{autoware_datetime_rviz_plugin => tier4_datetime_rviz_plugin}/images/select_panels.png (100%) rename common/{autoware_datetime_rviz_plugin => tier4_datetime_rviz_plugin}/package.xml (84%) rename common/{autoware_datetime_rviz_plugin => tier4_datetime_rviz_plugin}/plugins/plugin_description.xml (77%) rename common/{autoware_datetime_rviz_plugin => tier4_datetime_rviz_plugin}/src/autoware_datetime_panel.cpp (100%) rename common/{autoware_datetime_rviz_plugin => tier4_datetime_rviz_plugin}/src/autoware_datetime_panel.hpp (100%) rename common/{autoware_debug_tools => tier4_debug_tools}/CMakeLists.txt (97%) rename common/{autoware_debug_tools => tier4_debug_tools}/README.md (79%) rename common/{autoware_debug_tools => tier4_debug_tools}/config/lateral_error_publisher.param.yaml (100%) rename common/{autoware_debug_tools/include/autoware_debug_tools => tier4_debug_tools/include/tier4_debug_tools}/lateral_error_publisher.hpp (92%) rename common/{autoware_debug_tools => tier4_debug_tools}/launch/lateral_error_publisher.launch.xml (71%) rename common/{autoware_debug_tools => tier4_debug_tools}/media/lateral_error_publisher.svg (100%) rename common/{autoware_debug_tools => tier4_debug_tools}/package.xml (85%) rename common/{autoware_debug_tools => tier4_debug_tools}/scripts/case_converter.py (100%) rename common/{autoware_debug_tools => tier4_debug_tools}/scripts/pose2tf.py (100%) rename common/{autoware_debug_tools => tier4_debug_tools}/scripts/self_pose_listener.py (100%) rename common/{autoware_debug_tools => tier4_debug_tools}/scripts/stop_reason2pose.py (97%) create mode 100755 common/tier4_debug_tools/scripts/stop_reason2tf rename common/{autoware_debug_tools => tier4_debug_tools}/scripts/tf2pose.py (96%) rename common/{autoware_debug_tools => tier4_debug_tools}/src/lateral_error_publisher.cpp (98%) rename common/{autoware_localization_rviz_plugin => tier4_localization_rviz_plugin}/CMakeLists.txt (96%) rename common/{autoware_localization_rviz_plugin => tier4_localization_rviz_plugin}/README.md (93%) rename common/{autoware_localization_rviz_plugin => tier4_localization_rviz_plugin}/icons/classes/PoseHistory.png (100%) rename common/{autoware_localization_rviz_plugin => tier4_localization_rviz_plugin}/images/select_add.png (100%) rename common/{autoware_localization_rviz_plugin => tier4_localization_rviz_plugin}/images/select_localization_plugin.png (100%) rename common/{autoware_localization_rviz_plugin => tier4_localization_rviz_plugin}/images/select_topic_name.png (100%) rename common/{autoware_localization_rviz_plugin => tier4_localization_rviz_plugin}/package.xml (85%) rename common/{autoware_localization_rviz_plugin => tier4_localization_rviz_plugin}/plugins/plugin_description.xml (76%) rename common/{autoware_localization_rviz_plugin => tier4_localization_rviz_plugin}/src/pose_history/pose_history_display.cpp (100%) rename common/{autoware_localization_rviz_plugin => tier4_localization_rviz_plugin}/src/pose_history/pose_history_display.hpp (100%) rename common/{autoware_perception_rviz_plugin => tier4_perception_rviz_plugin}/CMakeLists.txt (86%) rename common/{autoware_perception_rviz_plugin => tier4_perception_rviz_plugin}/README.md (98%) rename common/{autoware_perception_rviz_plugin => tier4_perception_rviz_plugin}/icons/classes/CarInitialPoseTool.png (100%) rename common/{autoware_perception_rviz_plugin => tier4_perception_rviz_plugin}/icons/classes/DeleteAllObjectsTool.png (100%) rename common/{autoware_perception_rviz_plugin => tier4_perception_rviz_plugin}/icons/classes/PedestrianInitialPoseTool.png (100%) rename common/{autoware_perception_rviz_plugin => tier4_perception_rviz_plugin}/icons/classes/UnknownInitialPoseTool.png (100%) rename common/{autoware_perception_rviz_plugin => tier4_perception_rviz_plugin}/images/select_add.png (100%) rename common/{autoware_perception_rviz_plugin => tier4_perception_rviz_plugin}/images/select_dummy_car.png (100%) rename common/{autoware_perception_rviz_plugin => tier4_perception_rviz_plugin}/images/select_plugin.png (100%) rename common/{autoware_perception_rviz_plugin => tier4_perception_rviz_plugin}/package.xml (86%) rename common/{autoware_perception_rviz_plugin => tier4_perception_rviz_plugin}/plugins/plugin_description.xml (93%) rename common/{autoware_perception_rviz_plugin => tier4_perception_rviz_plugin}/src/tools/car_pose.cpp (100%) rename common/{autoware_perception_rviz_plugin => tier4_perception_rviz_plugin}/src/tools/car_pose.hpp (100%) rename common/{autoware_perception_rviz_plugin => tier4_perception_rviz_plugin}/src/tools/delete_all_objects.cpp (100%) rename common/{autoware_perception_rviz_plugin => tier4_perception_rviz_plugin}/src/tools/delete_all_objects.hpp (100%) rename common/{autoware_perception_rviz_plugin => tier4_perception_rviz_plugin}/src/tools/pedestrian_pose.cpp (100%) rename common/{autoware_perception_rviz_plugin => tier4_perception_rviz_plugin}/src/tools/pedestrian_pose.hpp (100%) rename common/{autoware_perception_rviz_plugin => tier4_perception_rviz_plugin}/src/tools/unknown_pose.cpp (100%) rename common/{autoware_perception_rviz_plugin => tier4_perception_rviz_plugin}/src/tools/unknown_pose.hpp (100%) rename common/{autoware_planning_rviz_plugin => tier4_planning_rviz_plugin}/CMakeLists.txt (89%) rename common/{autoware_planning_rviz_plugin => tier4_planning_rviz_plugin}/README.md (98%) rename common/{autoware_planning_rviz_plugin => tier4_planning_rviz_plugin}/icons/classes/MaxVelocity.png (100%) rename common/{autoware_planning_rviz_plugin => tier4_planning_rviz_plugin}/icons/classes/MissionCheckpointTool.png (100%) rename common/{autoware_planning_rviz_plugin => tier4_planning_rviz_plugin}/icons/classes/Path.png (100%) rename common/{autoware_planning_rviz_plugin => tier4_planning_rviz_plugin}/icons/classes/PathFootprint.png (100%) rename common/{autoware_planning_rviz_plugin => tier4_planning_rviz_plugin}/icons/classes/Trajectory.png (100%) rename common/{autoware_planning_rviz_plugin => tier4_planning_rviz_plugin}/icons/classes/TrajectoryFootprint.png (100%) rename common/{autoware_planning_rviz_plugin => tier4_planning_rviz_plugin}/images/select_add.png (100%) rename common/{autoware_planning_rviz_plugin => tier4_planning_rviz_plugin}/images/select_planning_plugin.png (100%) rename common/{autoware_planning_rviz_plugin => tier4_planning_rviz_plugin}/images/select_topic_name.png (100%) rename common/{autoware_planning_rviz_plugin => tier4_planning_rviz_plugin}/include/mission_checkpoint/mission_checkpoint.hpp (100%) rename common/{autoware_planning_rviz_plugin => tier4_planning_rviz_plugin}/include/path/display.hpp (100%) rename common/{autoware_planning_rviz_plugin => tier4_planning_rviz_plugin}/include/path_footprint/display.hpp (100%) rename common/{autoware_planning_rviz_plugin => tier4_planning_rviz_plugin}/include/trajectory/display.hpp (100%) rename common/{autoware_planning_rviz_plugin => tier4_planning_rviz_plugin}/include/trajectory_footprint/display.hpp (100%) rename common/{autoware_planning_rviz_plugin => tier4_planning_rviz_plugin}/package.xml (88%) rename common/{autoware_planning_rviz_plugin => tier4_planning_rviz_plugin}/plugins/plugin_description.xml (96%) rename common/{autoware_planning_rviz_plugin => tier4_planning_rviz_plugin}/src/mission_checkpoint/mission_checkpoint.cpp (100%) rename common/{autoware_planning_rviz_plugin => tier4_planning_rviz_plugin}/src/path/display.cpp (100%) rename common/{autoware_planning_rviz_plugin => tier4_planning_rviz_plugin}/src/path_footprint/display.cpp (100%) rename common/{autoware_planning_rviz_plugin => tier4_planning_rviz_plugin}/src/tools/jsk_overlay_utils.cpp (100%) rename common/{autoware_planning_rviz_plugin => tier4_planning_rviz_plugin}/src/tools/jsk_overlay_utils.hpp (100%) rename common/{autoware_planning_rviz_plugin => tier4_planning_rviz_plugin}/src/tools/max_velocity.cpp (100%) rename common/{autoware_planning_rviz_plugin => tier4_planning_rviz_plugin}/src/tools/max_velocity.hpp (100%) rename common/{autoware_planning_rviz_plugin => tier4_planning_rviz_plugin}/src/trajectory/display.cpp (100%) rename common/{autoware_planning_rviz_plugin => tier4_planning_rviz_plugin}/src/trajectory_footprint/display.cpp (100%) rename common/{autoware_state_rviz_plugin => tier4_state_rviz_plugin}/CMakeLists.txt (96%) rename common/{autoware_state_rviz_plugin => tier4_state_rviz_plugin}/README.md (94%) rename common/{autoware_state_rviz_plugin => tier4_state_rviz_plugin}/icons/classes/AutowareStatePanel.png (100%) rename common/{autoware_state_rviz_plugin => tier4_state_rviz_plugin}/images/select_engage.png (100%) rename common/{autoware_state_rviz_plugin => tier4_state_rviz_plugin}/images/select_panels.png (100%) rename common/{autoware_state_rviz_plugin => tier4_state_rviz_plugin}/images/select_state_plugin.png (100%) rename common/{autoware_state_rviz_plugin => tier4_state_rviz_plugin}/package.xml (95%) rename common/{autoware_state_rviz_plugin => tier4_state_rviz_plugin}/plugins/plugin_description.xml (79%) rename common/{autoware_state_rviz_plugin => tier4_state_rviz_plugin}/src/autoware_state_panel.cpp (100%) rename common/{autoware_state_rviz_plugin => tier4_state_rviz_plugin}/src/autoware_state_panel.hpp (100%) rename common/{autoware_vehicle_rviz_plugin => tier4_vehicle_rviz_plugin}/CMakeLists.txt (88%) rename common/{autoware_vehicle_rviz_plugin => tier4_vehicle_rviz_plugin}/README.md (97%) rename common/{autoware_vehicle_rviz_plugin => tier4_vehicle_rviz_plugin}/icons/classes/ConsoleMeter.png (100%) rename common/{autoware_vehicle_rviz_plugin => tier4_vehicle_rviz_plugin}/icons/classes/SteeringAngle.png (100%) rename common/{autoware_vehicle_rviz_plugin => tier4_vehicle_rviz_plugin}/icons/classes/TurnSignal.png (100%) rename common/{autoware_vehicle_rviz_plugin => tier4_vehicle_rviz_plugin}/icons/classes/VelocityHistory.png (100%) rename common/{autoware_vehicle_rviz_plugin => tier4_vehicle_rviz_plugin}/images/handle.png (100%) rename common/{autoware_vehicle_rviz_plugin => tier4_vehicle_rviz_plugin}/images/select_add.png (100%) rename common/{autoware_vehicle_rviz_plugin => tier4_vehicle_rviz_plugin}/images/select_topic_name.png (100%) rename common/{autoware_vehicle_rviz_plugin => tier4_vehicle_rviz_plugin}/images/select_vehicle_plugin.png (100%) rename common/{autoware_vehicle_rviz_plugin => tier4_vehicle_rviz_plugin}/package.xml (82%) rename common/{autoware_vehicle_rviz_plugin => tier4_vehicle_rviz_plugin}/plugins/plugin_description.xml (93%) rename common/{autoware_vehicle_rviz_plugin => tier4_vehicle_rviz_plugin}/src/tools/console_meter.cpp (95%) rename common/{autoware_vehicle_rviz_plugin => tier4_vehicle_rviz_plugin}/src/tools/console_meter.hpp (86%) rename common/{autoware_vehicle_rviz_plugin => tier4_vehicle_rviz_plugin}/src/tools/jsk_overlay_utils.cpp (100%) rename common/{autoware_vehicle_rviz_plugin => tier4_vehicle_rviz_plugin}/src/tools/jsk_overlay_utils.hpp (100%) rename common/{autoware_vehicle_rviz_plugin => tier4_vehicle_rviz_plugin}/src/tools/steering_angle.cpp (99%) rename common/{autoware_vehicle_rviz_plugin => tier4_vehicle_rviz_plugin}/src/tools/steering_angle.hpp (100%) rename common/{autoware_vehicle_rviz_plugin => tier4_vehicle_rviz_plugin}/src/tools/turn_signal.cpp (100%) rename common/{autoware_vehicle_rviz_plugin => tier4_vehicle_rviz_plugin}/src/tools/turn_signal.hpp (100%) rename common/{autoware_vehicle_rviz_plugin => tier4_vehicle_rviz_plugin}/src/tools/velocity_history.cpp (100%) rename common/{autoware_vehicle_rviz_plugin => tier4_vehicle_rviz_plugin}/src/tools/velocity_history.hpp (100%) rename common/{autoware_web_controller => web_controller}/CMakeLists.txt (83%) rename common/{autoware_web_controller => web_controller}/README.md (96%) rename common/{autoware_web_controller => web_controller}/images/web_controller.png (100%) rename common/{autoware_web_controller/launch/autoware_web_controller.launch.xml => web_controller/launch/web_controller.launch.xml} (81%) rename common/{autoware_web_controller => web_controller}/package.xml (86%) rename common/{autoware_web_controller/www/autoware_web_controller/css/autoware_web_controller_style.css => web_controller/www/web_controller/css/web_controller_style.css} (100%) rename common/{autoware_web_controller/www/autoware_web_controller => web_controller/www/web_controller}/icon/autoware.ico (100%) rename common/{autoware_web_controller/www/autoware_web_controller => web_controller/www/web_controller}/index.html (97%) rename common/{autoware_web_controller/www/autoware_web_controller => web_controller/www/web_controller}/js/autoware_engage.js (100%) rename common/{autoware_web_controller/www/autoware_web_controller => web_controller/www/web_controller}/js/autoware_state.js (100%) rename common/{autoware_web_controller/www/autoware_web_controller => web_controller/www/web_controller}/js/lane_change_approval.js (100%) rename common/{autoware_web_controller/www/autoware_web_controller => web_controller/www/web_controller}/js/menu.js (100%) rename common/{autoware_web_controller/www/autoware_web_controller => web_controller/www/web_controller}/js/path_change_approval.js (100%) rename common/{autoware_web_controller/www/autoware_web_controller => web_controller/www/web_controller}/js/vehicle_engage.js (100%) rename common/{autoware_web_controller/www/autoware_web_controller => web_controller/www/web_controller}/js/velocity_limit.js (100%) rename control/{autoware_joy_controller => joy_controller}/CMakeLists.txt (60%) rename control/{autoware_joy_controller => joy_controller}/README.md (95%) rename control/{autoware_joy_controller/config/autoware_joy_controller.param.yaml => joy_controller/config/joy_controller.param.yaml} (100%) rename control/{autoware_joy_controller/include/autoware_joy_controller/autoware_joy_controller.hpp => joy_controller/include/joy_controller/joy_controller.hpp} (92%) rename control/{autoware_joy_controller/include/autoware_joy_controller => joy_controller/include/joy_controller}/joy_converter/ds4_joy_converter.hpp (89%) rename control/{autoware_joy_controller/include/autoware_joy_controller => joy_controller/include/joy_controller}/joy_converter/g29_joy_converter.hpp (88%) rename control/{autoware_joy_controller/include/autoware_joy_controller => joy_controller/include/joy_controller}/joy_converter/joy_converter_base.hpp (82%) rename control/{autoware_joy_controller/launch/autoware_joy_controller.launch.xml => joy_controller/launch/joy_controller.launch.xml} (89%) rename control/{autoware_joy_controller => joy_controller}/package.xml (84%) rename control/{autoware_joy_controller/src/autoware_joy_controller/autoware_joy_controller_node.cpp => joy_controller/src/joy_controller/joy_controller_node.cpp} (95%) rename messages/{autoware_iv_auto_msgs_converter => tier4_auto_msgs_converter}/CMakeLists.txt (92%) rename messages/{autoware_iv_auto_msgs_converter/include/autoware_iv_auto_msgs_converter/autoware_iv_auto_msgs_converter.hpp => tier4_auto_msgs_converter/include/tier4_auto_msgs_converter/tier4_auto_msgs_converter.hpp} (97%) rename messages/{autoware_iv_auto_msgs_converter => tier4_auto_msgs_converter}/package.xml (85%) rename system/{autoware_state_monitor => ad_service_state_monitor}/CMakeLists.txt (63%) rename system/{autoware_state_monitor => ad_service_state_monitor}/README.md (98%) rename system/{autoware_state_monitor/config/autoware_state_monitor.param.yaml => ad_service_state_monitor/config/ad_service_state_monitor.param.yaml} (100%) rename system/{autoware_state_monitor/config/autoware_state_monitor.planning_simulation.param.yaml => ad_service_state_monitor/config/ad_service_state_monitor.planning_simulation.param.yaml} (100%) rename system/{autoware_state_monitor/include/autoware_state_monitor/autoware_state.hpp => ad_service_state_monitor/include/ad_service_state_monitor/ad_service_state.hpp} (93%) rename system/{autoware_state_monitor/include/autoware_state_monitor/autoware_state_monitor_node.hpp => ad_service_state_monitor/include/ad_service_state_monitor/ad_service_state_monitor_node.hpp} (92%) rename system/{autoware_state_monitor/include/autoware_state_monitor => ad_service_state_monitor/include/ad_service_state_monitor}/config.hpp (96%) rename system/{autoware_state_monitor/include/autoware_state_monitor => ad_service_state_monitor/include/ad_service_state_monitor}/module_name.hpp (87%) rename system/{autoware_state_monitor/include/autoware_state_monitor => ad_service_state_monitor/include/ad_service_state_monitor}/state_machine.hpp (88%) rename system/{autoware_state_monitor/launch/autoware_state_monitor.launch.xml => ad_service_state_monitor/launch/ad_service_state_monitor.launch.xml} (89%) rename system/{autoware_state_monitor => ad_service_state_monitor}/package.xml (87%) rename system/{autoware_state_monitor/src/autoware_state_monitor_node/autoware_state_monitor_node.cpp => ad_service_state_monitor/src/ad_service_state_monitor_node/ad_service_state_monitor_node.cpp} (98%) rename system/{autoware_state_monitor/src/autoware_state_monitor_node => ad_service_state_monitor/src/ad_service_state_monitor_node}/diagnostics.cpp (97%) rename system/{autoware_state_monitor/src/autoware_state_monitor_node => ad_service_state_monitor/src/ad_service_state_monitor_node}/main.cpp (93%) rename system/{autoware_state_monitor/src/autoware_state_monitor_node => ad_service_state_monitor/src/ad_service_state_monitor_node}/state_machine.cpp (98%) delete mode 100644 system/autoware_version/CMakeLists.txt delete mode 100644 system/autoware_version/README.md delete mode 100644 system/autoware_version/package.xml delete mode 100755 system/autoware_version/script/print rename system/{autoware_error_monitor => system_error_monitor}/CMakeLists.txt (76%) rename system/{autoware_error_monitor => system_error_monitor}/README.md (95%) rename system/{autoware_error_monitor => system_error_monitor}/config/diagnostic_aggregator/_empty.param.yaml (100%) rename system/{autoware_error_monitor => system_error_monitor}/config/diagnostic_aggregator/control.param.yaml (100%) rename system/{autoware_error_monitor => system_error_monitor}/config/diagnostic_aggregator/localization.param.yaml (100%) rename system/{autoware_error_monitor => system_error_monitor}/config/diagnostic_aggregator/map.param.yaml (100%) rename system/{autoware_error_monitor => system_error_monitor}/config/diagnostic_aggregator/perception.param.yaml (100%) rename system/{autoware_error_monitor => system_error_monitor}/config/diagnostic_aggregator/planning.param.yaml (100%) rename system/{autoware_error_monitor => system_error_monitor}/config/diagnostic_aggregator/sensing.param.yaml (100%) rename system/{autoware_error_monitor => system_error_monitor}/config/diagnostic_aggregator/system.param.yaml (100%) rename system/{autoware_error_monitor => system_error_monitor}/config/diagnostic_aggregator/vehicle.param.yaml (100%) rename system/{autoware_error_monitor/config/autoware_error_monitor.param.yaml => system_error_monitor/config/system_error_monitor.param.yaml} (100%) rename system/{autoware_error_monitor/config/autoware_error_monitor.planning_simulation.param.yaml => system_error_monitor/config/system_error_monitor.planning_simulation.param.yaml} (100%) rename system/{autoware_error_monitor/include/autoware_error_monitor => system_error_monitor/include/system_error_monitor}/diagnostics_filter.hpp (94%) rename system/{autoware_error_monitor/include/autoware_error_monitor/autoware_error_monitor_core.hpp => system_error_monitor/include/system_error_monitor/system_error_monitor_core.hpp} (96%) rename system/{autoware_error_monitor/launch/autoware_error_monitor.launch.xml => system_error_monitor/launch/system_error_monitor.launch.xml} (68%) rename system/{autoware_error_monitor/launch/autoware_error_monitor_node.launch.xml => system_error_monitor/launch/system_error_monitor_node.launch.xml} (87%) rename system/{autoware_error_monitor => system_error_monitor}/package.xml (85%) rename system/{autoware_error_monitor/src/autoware_error_monitor_core.cpp => system_error_monitor/src/system_error_monitor_core.cpp} (98%) rename system/{autoware_error_monitor/src/autoware_error_monitor_node.cpp => system_error_monitor/src/system_error_monitor_node.cpp} (92%) diff --git a/awapi/awapi_awiv_adapter/package.xml b/awapi/awapi_awiv_adapter/package.xml index 8a0bed8882495..0edb0a9072933 100644 --- a/awapi/awapi_awiv_adapter/package.xml +++ b/awapi/awapi_awiv_adapter/package.xml @@ -13,7 +13,6 @@ autoware_auto_planning_msgs autoware_auto_system_msgs autoware_auto_vehicle_msgs - autoware_iv_auto_msgs_converter diagnostic_msgs geometry_msgs nav_msgs @@ -24,6 +23,7 @@ tf2 tf2_geometry_msgs tier4_api_msgs + tier4_auto_msgs_converter tier4_control_msgs tier4_external_api_msgs tier4_planning_msgs diff --git a/awapi/awapi_awiv_adapter/src/awapi_autoware_state_publisher.cpp b/awapi/awapi_awiv_adapter/src/awapi_autoware_state_publisher.cpp index aaab949738162..12e7af3651ad9 100644 --- a/awapi/awapi_awiv_adapter/src/awapi_autoware_state_publisher.cpp +++ b/awapi/awapi_awiv_adapter/src/awapi_autoware_state_publisher.cpp @@ -14,8 +14,8 @@ #include "awapi_awiv_adapter/awapi_autoware_state_publisher.hpp" -#include "autoware_iv_auto_msgs_converter/autoware_iv_auto_msgs_converter.hpp" #include "awapi_awiv_adapter/diagnostics_filter.hpp" +#include "tier4_auto_msgs_converter/tier4_auto_msgs_converter.hpp" #include #include @@ -67,7 +67,7 @@ void AutowareIvAutowareStatePublisher::getAutowareStateInfo( } // get autoware_state - using autoware_iv_auto_msgs_converter::convert; + using tier4_auto_msgs_converter::convert; status->autoware_state = convert(*autoware_state_ptr).state; status->arrived_goal = isGoal(autoware_state_ptr); } @@ -82,7 +82,7 @@ void AutowareIvAutowareStatePublisher::getControlModeInfo( } // get control mode - using autoware_iv_auto_msgs_converter::convert; + using tier4_auto_msgs_converter::convert; status->control_mode = convert(*control_mode_ptr).data; } @@ -155,7 +155,7 @@ void AutowareIvAutowareStatePublisher::getHazardStatusInfo( } // get emergency - using autoware_iv_auto_msgs_converter::convert; + using tier4_auto_msgs_converter::convert; status->hazard_status = convert(*aw_info.hazard_status_ptr); // filter leaf diagnostics diff --git a/awapi/awapi_awiv_adapter/src/awapi_lane_change_state_publisher.cpp b/awapi/awapi_awiv_adapter/src/awapi_lane_change_state_publisher.cpp index a2d63fe492c33..cb74bf222417f 100644 --- a/awapi/awapi_awiv_adapter/src/awapi_lane_change_state_publisher.cpp +++ b/awapi/awapi_awiv_adapter/src/awapi_lane_change_state_publisher.cpp @@ -14,7 +14,7 @@ #include "awapi_awiv_adapter/awapi_lane_change_state_publisher.hpp" -#include "autoware_iv_auto_msgs_converter/autoware_iv_auto_msgs_converter.hpp" +#include "tier4_auto_msgs_converter/tier4_auto_msgs_converter.hpp" namespace autoware_api { @@ -81,7 +81,7 @@ void AutowareIvLaneChangeStatePublisher::getCandidatePathInfo( return; } - using autoware_iv_auto_msgs_converter::convert; + using tier4_auto_msgs_converter::convert; status->candidate_path = convert(*path_ptr); } diff --git a/awapi/awapi_awiv_adapter/src/awapi_obstacle_avoidance_state_publisher.cpp b/awapi/awapi_awiv_adapter/src/awapi_obstacle_avoidance_state_publisher.cpp index d2a2ce3e2194e..e877a47f18a4e 100644 --- a/awapi/awapi_awiv_adapter/src/awapi_obstacle_avoidance_state_publisher.cpp +++ b/awapi/awapi_awiv_adapter/src/awapi_obstacle_avoidance_state_publisher.cpp @@ -14,7 +14,7 @@ #include "awapi_awiv_adapter/awapi_obstacle_avoidance_state_publisher.hpp" -#include "autoware_iv_auto_msgs_converter/autoware_iv_auto_msgs_converter.hpp" +#include "tier4_auto_msgs_converter/tier4_auto_msgs_converter.hpp" namespace autoware_api { @@ -67,7 +67,7 @@ void AutowareIvObstacleAvoidanceStatePublisher::getCandidatePathInfo( return; } - using autoware_iv_auto_msgs_converter::convert; + using tier4_auto_msgs_converter::convert; status->candidate_path = convert(*path_ptr); } diff --git a/awapi/awapi_awiv_adapter/src/awapi_vehicle_state_publisher.cpp b/awapi/awapi_awiv_adapter/src/awapi_vehicle_state_publisher.cpp index ef5329f43e746..0de8626df6b38 100644 --- a/awapi/awapi_awiv_adapter/src/awapi_vehicle_state_publisher.cpp +++ b/awapi/awapi_awiv_adapter/src/awapi_vehicle_state_publisher.cpp @@ -14,7 +14,7 @@ #include "awapi_awiv_adapter/awapi_vehicle_state_publisher.hpp" -#include "autoware_iv_auto_msgs_converter/autoware_iv_auto_msgs_converter.hpp" +#include "tier4_auto_msgs_converter/tier4_auto_msgs_converter.hpp" #include #include @@ -93,7 +93,7 @@ void AutowareIvVehicleStatePublisher::getSteerInfo( } // get steer - using autoware_iv_auto_msgs_converter::convert; + using tier4_auto_msgs_converter::convert; status->steering = convert(*steer_ptr).data; // get steer vel @@ -144,7 +144,7 @@ void AutowareIvVehicleStatePublisher::getTurnSignalInfo( } // get turn signal - using autoware_iv_auto_msgs_converter::convert; + using tier4_auto_msgs_converter::convert; status->turn_signal = convert(*turn_indicators_ptr, *hazard_lights_ptr).data; } @@ -191,7 +191,7 @@ void AutowareIvVehicleStatePublisher::getGearInfo( } // get gear (shift) - using autoware_iv_auto_msgs_converter::convert; + using tier4_auto_msgs_converter::convert; status->gear = convert(*gear_ptr).shift.data; } diff --git a/common/autoware_debug_tools/scripts/stop_reason2tf b/common/autoware_debug_tools/scripts/stop_reason2tf deleted file mode 100755 index 23d58183bdd97..0000000000000 --- a/common/autoware_debug_tools/scripts/stop_reason2tf +++ /dev/null @@ -1,15 +0,0 @@ -#!/usr/bin/env bash - -stop_reason_name="$1" - -if [ -z "${stop_reason_name}" ]; then - echo "Please input stop_reason_name as the 1st argument." - exit 1 -fi - -ros2 run autoware_debug_tools stop_reason2pose.py /planning/scenario_planning/status/stop_reasons >/dev/null 2>&1 & -ros2 run autoware_debug_tools pose2tf.py /autoware_debug_tools/stop_reason2pose/"${stop_reason_name}" "${stop_reason_name}" >/dev/null 2>&1 & -ros2 run autoware_debug_tools tf2pose.py "${stop_reason_name}" base_link 100 >/dev/null 2>&1 & -ros2 run tf2_ros tf2_echo "${stop_reason_name}" base_link 100 2>/dev/null - -wait diff --git a/common/autoware_rosbag_recorder/CMakeLists.txt b/common/autoware_rosbag_recorder/CMakeLists.txt deleted file mode 100644 index 03e7ca43e5296..0000000000000 --- a/common/autoware_rosbag_recorder/CMakeLists.txt +++ /dev/null @@ -1,17 +0,0 @@ -cmake_minimum_required(VERSION 3.5) -project(autoware_rosbag_recorder) - -find_package(ament_cmake_auto REQUIRED) -ament_auto_find_build_dependencies() - -if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - ament_lint_auto_find_test_dependencies() -endif() - -ament_auto_package() - -install(PROGRAMS - scripts/record.sh - DESTINATION lib/${PROJECT_NAME} -) diff --git a/common/autoware_rosbag_recorder/Readme.md b/common/autoware_rosbag_recorder/Readme.md deleted file mode 100644 index 087e2cffbff2f..0000000000000 --- a/common/autoware_rosbag_recorder/Readme.md +++ /dev/null @@ -1,21 +0,0 @@ -# Autoware Rosbag Recorder - -This package is to record ros2 bag with desired topic. - -## Usage - -Here is a usage below. - -```sh -ros2 run autoware_rosbag_recorder record.sh [-o filename] -``` - -If you want some other topics to record, you need to modify below in record.sh as an example. - -```sh - ros2 bag record -e "(.*)/velodyne_packets|/pacmod_interface/(.*)|/pacmod/(.*)|/vehicle/(.*)|/sensing/imu/(.*)|/sensing/gnss/(.*)|/sensing/camera/(.*)/camera_info|/sensing/camera/(.*)/compressed|/perception/object_recognition/detection/rois(.)|/perception/object_recognition/objects" -o "$OPTARG"; -``` - -## Assumptions / Known limits - -Recording all topics in autoware is very heavy so usually you need to avoid points cloud topics to record. diff --git a/common/autoware_rosbag_recorder/package.xml b/common/autoware_rosbag_recorder/package.xml deleted file mode 100644 index e0d2cedb04044..0000000000000 --- a/common/autoware_rosbag_recorder/package.xml +++ /dev/null @@ -1,18 +0,0 @@ - - - autoware_rosbag_recorder - 0.1.0 - The autoware_rosbag_recorder package - - Yukihiro Saito - Apache License 2.0 - - ament_cmake_auto - - ament_lint_auto - autoware_lint_common - - - ament_cmake - - diff --git a/common/autoware_rosbag_recorder/scripts/record.sh b/common/autoware_rosbag_recorder/scripts/record.sh deleted file mode 100755 index 64cf200a26da6..0000000000000 --- a/common/autoware_rosbag_recorder/scripts/record.sh +++ /dev/null @@ -1,21 +0,0 @@ -#!/bin/sh - -usage_exit() { - echo "Usage: ros2 run autoware_rosbag_recorder record.sh [-o filename]" 1>&2 - exit 1 -} - -while getopts o:h OPT; do - case $OPT in - "o") echo "record as $OPTARG" ;; - "h") usage_exit ;; - "*") usage_exit ;; - \?) usage_exit ;; - esac -done - -if [ -n "$OPTARG" ]; then - ros2 bag record -e "(.*)/velodyne_packets|/pacmod_interface/(.*)|/pacmod/(.*)|/vehicle/(.*)|/sensing/imu/(.*)|/sensing/gnss/(.*)|/sensing/camera/(.*)/camera_info|/sensing/camera/(.*)/compressed|/perception/object_recognition/detection/rois(.)|/perception/object_recognition/objects" -o "$OPTARG" -else - usage_exit -fi diff --git a/common/autoware_utils/include/autoware_utils/autoware_utils.hpp b/common/autoware_utils/include/autoware_utils/autoware_utils.hpp deleted file mode 100644 index 19542dd07a8fd..0000000000000 --- a/common/autoware_utils/include/autoware_utils/autoware_utils.hpp +++ /dev/null @@ -1,37 +0,0 @@ -// Copyright 2020 Tier IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef AUTOWARE_UTILS__AUTOWARE_UTILS_HPP_ -#define AUTOWARE_UTILS__AUTOWARE_UTILS_HPP_ - -#include "autoware_utils/geometry/boost_geometry.hpp" -#include "autoware_utils/geometry/geometry.hpp" -#include "autoware_utils/geometry/pose_deviation.hpp" -#include "autoware_utils/math/constants.hpp" -#include "autoware_utils/math/normalization.hpp" -#include "autoware_utils/math/range.hpp" -#include "autoware_utils/math/unit_conversion.hpp" -#include "autoware_utils/planning/planning_marker_helper.hpp" -#include "autoware_utils/ros/debug_publisher.hpp" -#include "autoware_utils/ros/debug_traits.hpp" -#include "autoware_utils/ros/marker_helper.hpp" -#include "autoware_utils/ros/processing_time_publisher.hpp" -#include "autoware_utils/ros/self_pose_listener.hpp" -#include "autoware_utils/ros/transform_listener.hpp" -#include "autoware_utils/ros/update_param.hpp" -#include "autoware_utils/ros/wait_for_param.hpp" -#include "autoware_utils/system/stop_watch.hpp" -#include "autoware_utils/trajectory/trajectory.hpp" - -#endif // AUTOWARE_UTILS__AUTOWARE_UTILS_HPP_ diff --git a/common/autoware_global_parameter_loader/CMakeLists.txt b/common/global_parameter_loader/CMakeLists.txt similarity index 86% rename from common/autoware_global_parameter_loader/CMakeLists.txt rename to common/global_parameter_loader/CMakeLists.txt index 28d71ba28ffad..06ab9ac22158f 100644 --- a/common/autoware_global_parameter_loader/CMakeLists.txt +++ b/common/global_parameter_loader/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.5) -project(autoware_global_parameter_loader) +project(global_parameter_loader) find_package(ament_cmake_auto REQUIRED) ament_auto_find_build_dependencies() diff --git a/common/autoware_global_parameter_loader/Readme.md b/common/global_parameter_loader/Readme.md similarity index 83% rename from common/autoware_global_parameter_loader/Readme.md rename to common/global_parameter_loader/Readme.md index 7b818c26a87f2..53cb9e11e096f 100644 --- a/common/autoware_global_parameter_loader/Readme.md +++ b/common/global_parameter_loader/Readme.md @@ -8,7 +8,7 @@ Add the following lines to the launch file of the node in which you want to get ```xml - + ``` diff --git a/common/autoware_global_parameter_loader/launch/global_params.launch.py b/common/global_parameter_loader/launch/global_params.launch.py similarity index 100% rename from common/autoware_global_parameter_loader/launch/global_params.launch.py rename to common/global_parameter_loader/launch/global_params.launch.py diff --git a/common/autoware_global_parameter_loader/package.xml b/common/global_parameter_loader/package.xml similarity index 78% rename from common/autoware_global_parameter_loader/package.xml rename to common/global_parameter_loader/package.xml index 144fcf02eaf21..71842dfc807be 100644 --- a/common/autoware_global_parameter_loader/package.xml +++ b/common/global_parameter_loader/package.xml @@ -1,8 +1,8 @@ - autoware_global_parameter_loader + global_parameter_loader 0.1.0 - The autoware_global_parameter_loader package + The global_parameter_loader package Kenji Miyake Apache License 2.0 diff --git a/common/goal_distance_calculator/include/goal_distance_calculator/goal_distance_calculator.hpp b/common/goal_distance_calculator/include/goal_distance_calculator/goal_distance_calculator.hpp index 618b8db7db37b..2f59543e2442c 100644 --- a/common/goal_distance_calculator/include/goal_distance_calculator/goal_distance_calculator.hpp +++ b/common/goal_distance_calculator/include/goal_distance_calculator/goal_distance_calculator.hpp @@ -15,8 +15,8 @@ #ifndef GOAL_DISTANCE_CALCULATOR__GOAL_DISTANCE_CALCULATOR_HPP_ #define GOAL_DISTANCE_CALCULATOR__GOAL_DISTANCE_CALCULATOR_HPP_ -#include #include +#include #include #include @@ -25,7 +25,7 @@ namespace goal_distance_calculator { -using autoware_utils::PoseDeviation; +using tier4_autoware_utils::PoseDeviation; struct Param { diff --git a/common/goal_distance_calculator/include/goal_distance_calculator/goal_distance_calculator_node.hpp b/common/goal_distance_calculator/include/goal_distance_calculator/goal_distance_calculator_node.hpp index 3ce2afcbe3f52..10aba5872b66a 100644 --- a/common/goal_distance_calculator/include/goal_distance_calculator/goal_distance_calculator_node.hpp +++ b/common/goal_distance_calculator/include/goal_distance_calculator/goal_distance_calculator_node.hpp @@ -17,9 +17,9 @@ #include "goal_distance_calculator/goal_distance_calculator.hpp" -#include -#include #include +#include +#include #include #include @@ -45,7 +45,7 @@ class GoalDistanceCalculatorNode : public rclcpp::Node private: // Subscriber rclcpp::Subscription::SharedPtr sub_initial_pose_; - autoware_utils::SelfPoseListener self_pose_listener_; + tier4_autoware_utils::SelfPoseListener self_pose_listener_; rclcpp::Subscription::SharedPtr sub_route_; // Data Buffer @@ -56,7 +56,7 @@ class GoalDistanceCalculatorNode : public rclcpp::Node void onRoute(const autoware_auto_planning_msgs::msg::Route::ConstSharedPtr & msg); // Publisher - autoware_utils::DebugPublisher debug_publisher_; + tier4_autoware_utils::DebugPublisher debug_publisher_; // Timer rclcpp::TimerBase::SharedPtr timer_; diff --git a/common/goal_distance_calculator/package.xml b/common/goal_distance_calculator/package.xml index 4171f61f41d64..d191ffd3945c9 100644 --- a/common/goal_distance_calculator/package.xml +++ b/common/goal_distance_calculator/package.xml @@ -10,10 +10,10 @@ ament_cmake autoware_auto_planning_msgs - autoware_utils geometry_msgs rclcpp rclcpp_components + tier4_autoware_utils tier4_debug_msgs ament_lint_auto diff --git a/common/goal_distance_calculator/src/goal_distance_calculator.cpp b/common/goal_distance_calculator/src/goal_distance_calculator.cpp index c0e31b24ba92b..1405d5bd62f7d 100755 --- a/common/goal_distance_calculator/src/goal_distance_calculator.cpp +++ b/common/goal_distance_calculator/src/goal_distance_calculator.cpp @@ -21,7 +21,7 @@ Output GoalDistanceCalculator::update(const Input & input) Output output{}; output.goal_deviation = - autoware_utils::calcPoseDeviation(input.route->goal_point.pose, input.current_pose->pose); + tier4_autoware_utils::calcPoseDeviation(input.route->goal_point.pose, input.current_pose->pose); return output; } diff --git a/common/goal_distance_calculator/src/goal_distance_calculator_node.cpp b/common/goal_distance_calculator/src/goal_distance_calculator_node.cpp index 57e38ed9d7ead..cac75e3bba195 100644 --- a/common/goal_distance_calculator/src/goal_distance_calculator_node.cpp +++ b/common/goal_distance_calculator/src/goal_distance_calculator_node.cpp @@ -14,9 +14,9 @@ #include "goal_distance_calculator/goal_distance_calculator_node.hpp" -#include #include #include +#include #include @@ -112,7 +112,7 @@ void GoalDistanceCalculatorNode::onTimer() output_ = goal_distance_calculator_->update(input_); { - using autoware_utils::rad2deg; + using tier4_autoware_utils::rad2deg; const auto & deviation = output_.goal_deviation; debug_publisher_.publish( diff --git a/common/path_distance_calculator/package.xml b/common/path_distance_calculator/package.xml index d7a2a3ce857ab..c69f961f271ef 100644 --- a/common/path_distance_calculator/package.xml +++ b/common/path_distance_calculator/package.xml @@ -10,9 +10,9 @@ ament_cmake autoware_auto_planning_msgs - autoware_utils rclcpp rclcpp_components + tier4_autoware_utils tier4_debug_msgs ament_lint_auto autoware_lint_common diff --git a/common/path_distance_calculator/src/path_distance_calculator.cpp b/common/path_distance_calculator/src/path_distance_calculator.cpp index de328f497b1e0..bfc3b55c1d66a 100644 --- a/common/path_distance_calculator/src/path_distance_calculator.cpp +++ b/common/path_distance_calculator/src/path_distance_calculator.cpp @@ -14,7 +14,7 @@ #include "path_distance_calculator.hpp" -#include +#include #include #include @@ -46,7 +46,7 @@ PathDistanceCalculator::PathDistanceCalculator(const rclcpp::NodeOptions & optio RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 5000, "path empty"); } - const double distance = autoware_utils::calcSignedArcLength( + const double distance = tier4_autoware_utils::calcSignedArcLength( path->points, pose->pose.position, path->points.size() - 1); tier4_debug_msgs::msg::Float64Stamped msg; diff --git a/common/path_distance_calculator/src/path_distance_calculator.hpp b/common/path_distance_calculator/src/path_distance_calculator.hpp index 9d6c3a6e29543..f709701024f51 100644 --- a/common/path_distance_calculator/src/path_distance_calculator.hpp +++ b/common/path_distance_calculator/src/path_distance_calculator.hpp @@ -15,8 +15,8 @@ #ifndef PATH_DISTANCE_CALCULATOR_HPP_ #define PATH_DISTANCE_CALCULATOR_HPP_ -#include #include +#include #include #include @@ -30,7 +30,7 @@ class PathDistanceCalculator : public rclcpp::Node rclcpp::Subscription::SharedPtr sub_path_; rclcpp::Publisher::SharedPtr pub_dist_; rclcpp::TimerBase::SharedPtr timer_; - autoware_utils::SelfPoseListener self_pose_listener_; + tier4_autoware_utils::SelfPoseListener self_pose_listener_; autoware_auto_planning_msgs::msg::Path::SharedPtr path_; }; diff --git a/common/autoware_api_utils/CMakeLists.txt b/common/tier4_api_utils/CMakeLists.txt similarity index 96% rename from common/autoware_api_utils/CMakeLists.txt rename to common/tier4_api_utils/CMakeLists.txt index fe5462b744ac4..1beed4368d313 100644 --- a/common/autoware_api_utils/CMakeLists.txt +++ b/common/tier4_api_utils/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.5) -project(autoware_api_utils) +project(tier4_api_utils) if(NOT CMAKE_CXX_STANDARD) set(CMAKE_CXX_STANDARD 17) diff --git a/common/autoware_api_utils/include/autoware_api_utils/rclcpp/client.hpp b/common/tier4_api_utils/include/tier4_api_utils/rclcpp/client.hpp similarity index 88% rename from common/autoware_api_utils/include/autoware_api_utils/rclcpp/client.hpp rename to common/tier4_api_utils/include/tier4_api_utils/rclcpp/client.hpp index e60b6b5c4a28e..7cc190eb8eaa9 100644 --- a/common/autoware_api_utils/include/autoware_api_utils/rclcpp/client.hpp +++ b/common/tier4_api_utils/include/tier4_api_utils/rclcpp/client.hpp @@ -12,16 +12,16 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE_API_UTILS__RCLCPP__CLIENT_HPP_ -#define AUTOWARE_API_UTILS__RCLCPP__CLIENT_HPP_ +#ifndef TIER4_API_UTILS__RCLCPP__CLIENT_HPP_ +#define TIER4_API_UTILS__RCLCPP__CLIENT_HPP_ -#include "autoware_api_utils/types/response.hpp" #include "rclcpp/client.hpp" +#include "tier4_api_utils/types/response.hpp" #include #include -namespace autoware_api_utils +namespace tier4_api_utils { template class Client @@ -65,6 +65,6 @@ class Client rclcpp::Logger logger_; }; -} // namespace autoware_api_utils +} // namespace tier4_api_utils -#endif // AUTOWARE_API_UTILS__RCLCPP__CLIENT_HPP_ +#endif // TIER4_API_UTILS__RCLCPP__CLIENT_HPP_ diff --git a/common/autoware_api_utils/include/autoware_api_utils/rclcpp/proxy.hpp b/common/tier4_api_utils/include/tier4_api_utils/rclcpp/proxy.hpp similarity index 86% rename from common/autoware_api_utils/include/autoware_api_utils/rclcpp/proxy.hpp rename to common/tier4_api_utils/include/tier4_api_utils/rclcpp/proxy.hpp index f2259b19d5c35..6b6cadd1f82fe 100644 --- a/common/autoware_api_utils/include/autoware_api_utils/rclcpp/proxy.hpp +++ b/common/tier4_api_utils/include/tier4_api_utils/rclcpp/proxy.hpp @@ -12,17 +12,17 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE_API_UTILS__RCLCPP__PROXY_HPP_ -#define AUTOWARE_API_UTILS__RCLCPP__PROXY_HPP_ +#ifndef TIER4_API_UTILS__RCLCPP__PROXY_HPP_ +#define TIER4_API_UTILS__RCLCPP__PROXY_HPP_ -#include "autoware_api_utils/rclcpp/client.hpp" -#include "autoware_api_utils/rclcpp/service.hpp" #include "rclcpp/rclcpp.hpp" +#include "tier4_api_utils/rclcpp/client.hpp" +#include "tier4_api_utils/rclcpp/service.hpp" #include #include -namespace autoware_api_utils +namespace tier4_api_utils { template class ServiceProxyNodeInterface @@ -58,6 +58,6 @@ class ServiceProxyNodeInterface NodeT * node_; }; -} // namespace autoware_api_utils +} // namespace tier4_api_utils -#endif // AUTOWARE_API_UTILS__RCLCPP__PROXY_HPP_ +#endif // TIER4_API_UTILS__RCLCPP__PROXY_HPP_ diff --git a/common/autoware_api_utils/include/autoware_api_utils/rclcpp/service.hpp b/common/tier4_api_utils/include/tier4_api_utils/rclcpp/service.hpp similarity index 86% rename from common/autoware_api_utils/include/autoware_api_utils/rclcpp/service.hpp rename to common/tier4_api_utils/include/tier4_api_utils/rclcpp/service.hpp index 39d11b19fda68..f99cb9632f607 100644 --- a/common/autoware_api_utils/include/autoware_api_utils/rclcpp/service.hpp +++ b/common/tier4_api_utils/include/tier4_api_utils/rclcpp/service.hpp @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE_API_UTILS__RCLCPP__SERVICE_HPP_ -#define AUTOWARE_API_UTILS__RCLCPP__SERVICE_HPP_ +#ifndef TIER4_API_UTILS__RCLCPP__SERVICE_HPP_ +#define TIER4_API_UTILS__RCLCPP__SERVICE_HPP_ #include "rclcpp/service.hpp" -namespace autoware_api_utils +namespace tier4_api_utils { template class Service @@ -46,6 +46,6 @@ class Service typename rclcpp::Service::SharedPtr service_; }; -} // namespace autoware_api_utils +} // namespace tier4_api_utils -#endif // AUTOWARE_API_UTILS__RCLCPP__SERVICE_HPP_ +#endif // TIER4_API_UTILS__RCLCPP__SERVICE_HPP_ diff --git a/common/autoware_api_utils/include/autoware_api_utils/autoware_api_utils.hpp b/common/tier4_api_utils/include/tier4_api_utils/tier4_api_utils.hpp similarity index 69% rename from common/autoware_api_utils/include/autoware_api_utils/autoware_api_utils.hpp rename to common/tier4_api_utils/include/tier4_api_utils/tier4_api_utils.hpp index d9aa11fa353a2..efd6477be7e0b 100644 --- a/common/autoware_api_utils/include/autoware_api_utils/autoware_api_utils.hpp +++ b/common/tier4_api_utils/include/tier4_api_utils/tier4_api_utils.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE_API_UTILS__AUTOWARE_API_UTILS_HPP_ -#define AUTOWARE_API_UTILS__AUTOWARE_API_UTILS_HPP_ +#ifndef TIER4_API_UTILS__TIER4_API_UTILS_HPP_ +#define TIER4_API_UTILS__TIER4_API_UTILS_HPP_ -#include "autoware_api_utils/rclcpp/proxy.hpp" -#include "autoware_api_utils/types/response.hpp" +#include "tier4_api_utils/rclcpp/proxy.hpp" +#include "tier4_api_utils/types/response.hpp" -#endif // AUTOWARE_API_UTILS__AUTOWARE_API_UTILS_HPP_ +#endif // TIER4_API_UTILS__TIER4_API_UTILS_HPP_ diff --git a/common/autoware_api_utils/include/autoware_api_utils/types/response.hpp b/common/tier4_api_utils/include/tier4_api_utils/types/response.hpp similarity index 91% rename from common/autoware_api_utils/include/autoware_api_utils/types/response.hpp rename to common/tier4_api_utils/include/tier4_api_utils/types/response.hpp index 69c6889b38589..5ed69a67617cf 100644 --- a/common/autoware_api_utils/include/autoware_api_utils/types/response.hpp +++ b/common/tier4_api_utils/include/tier4_api_utils/types/response.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE_API_UTILS__TYPES__RESPONSE_HPP_ -#define AUTOWARE_API_UTILS__TYPES__RESPONSE_HPP_ +#ifndef TIER4_API_UTILS__TYPES__RESPONSE_HPP_ +#define TIER4_API_UTILS__TYPES__RESPONSE_HPP_ #include "rclcpp/rclcpp.hpp" @@ -21,7 +21,7 @@ #include -namespace autoware_api_utils +namespace tier4_api_utils { using ResponseStatus = tier4_external_api_msgs::msg::ResponseStatus; @@ -73,6 +73,6 @@ inline ResponseStatus response_error(const std::string & message = "") .message(message); } -} // namespace autoware_api_utils +} // namespace tier4_api_utils -#endif // AUTOWARE_API_UTILS__TYPES__RESPONSE_HPP_ +#endif // TIER4_API_UTILS__TYPES__RESPONSE_HPP_ diff --git a/common/autoware_api_utils/package.xml b/common/tier4_api_utils/package.xml similarity index 89% rename from common/autoware_api_utils/package.xml rename to common/tier4_api_utils/package.xml index b1e1d64c9c54f..4141904db9945 100644 --- a/common/autoware_api_utils/package.xml +++ b/common/tier4_api_utils/package.xml @@ -2,9 +2,9 @@ - autoware_api_utils + tier4_api_utils 0.0.0 - The autoware_api_utils package + The tier4_api_utils package Takagi, Isamu Apache License 2.0 diff --git a/common/autoware_api_utils/test/test.cpp b/common/tier4_api_utils/test/test.cpp similarity index 80% rename from common/autoware_api_utils/test/test.cpp rename to common/tier4_api_utils/test/test.cpp index 8054b5bbe629a..1075c6e3ed01b 100644 --- a/common/autoware_api_utils/test/test.cpp +++ b/common/tier4_api_utils/test/test.cpp @@ -12,14 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware_api_utils/autoware_api_utils.hpp" #include "gtest/gtest.h" #include "rclcpp/rclcpp.hpp" +#include "tier4_api_utils/tier4_api_utils.hpp" -TEST(autoware_api_utils, instantiate) +TEST(tier4_api_utils, instantiate) { - rclcpp::Node node("autoware_api_utils_test"); - autoware_api_utils::ServiceProxyNodeInterface proxy(&node); + rclcpp::Node node("tier4_api_utils_test"); + tier4_api_utils::ServiceProxyNodeInterface proxy(&node); } int main(int argc, char ** argv) diff --git a/common/autoware_utils/CMakeLists.txt b/common/tier4_autoware_utils/CMakeLists.txt similarity index 72% rename from common/autoware_utils/CMakeLists.txt rename to common/tier4_autoware_utils/CMakeLists.txt index d6c7c1308de2d..e8676d9d4a4fb 100644 --- a/common/autoware_utils/CMakeLists.txt +++ b/common/tier4_autoware_utils/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.5) -project(autoware_utils) +project(tier4_autoware_utils) ### Compile options if(NOT CMAKE_CXX_STANDARD) @@ -14,8 +14,8 @@ ament_auto_find_build_dependencies() find_package(Boost REQUIRED) -ament_auto_add_library(autoware_utils SHARED - src/autoware_utils.cpp +ament_auto_add_library(tier4_autoware_utils SHARED + src/tier4_autoware_utils.cpp src/planning/planning_marker_helper.cpp ) @@ -28,10 +28,10 @@ if(BUILD_TESTING) file(GLOB_RECURSE test_files test/**/*.cpp) - ament_add_gtest(test_autoware_utils ${test_files}) + ament_add_gtest(test_tier4_autoware_utils ${test_files}) - target_link_libraries(test_autoware_utils - autoware_utils + target_link_libraries(test_tier4_autoware_utils + tier4_autoware_utils ) endif() diff --git a/common/autoware_utils/README.md b/common/tier4_autoware_utils/README.md similarity index 83% rename from common/autoware_utils/README.md rename to common/tier4_autoware_utils/README.md index 42ca7e498f87a..7a352e971a58d 100644 --- a/common/autoware_utils/README.md +++ b/common/tier4_autoware_utils/README.md @@ -1,4 +1,4 @@ -# autoware_utils +# tier4_autoware_utils ## Purpose diff --git a/common/autoware_utils/include/autoware_utils/geometry/boost_geometry.hpp b/common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/boost_geometry.hpp similarity index 86% rename from common/autoware_utils/include/autoware_utils/geometry/boost_geometry.hpp rename to common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/boost_geometry.hpp index 743bafceb0ace..a565fe764d1ce 100644 --- a/common/autoware_utils/include/autoware_utils/geometry/boost_geometry.hpp +++ b/common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/boost_geometry.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE_UTILS__GEOMETRY__BOOST_GEOMETRY_HPP_ -#define AUTOWARE_UTILS__GEOMETRY__BOOST_GEOMETRY_HPP_ +#ifndef TIER4_AUTOWARE_UTILS__GEOMETRY__BOOST_GEOMETRY_HPP_ +#define TIER4_AUTOWARE_UTILS__GEOMETRY__BOOST_GEOMETRY_HPP_ #include #include @@ -25,7 +25,7 @@ #include -namespace autoware_utils +namespace tier4_autoware_utils { // 2D struct Point2d; @@ -86,11 +86,11 @@ inline Point3d fromMsg(const geometry_msgs::msg::Point & msg) point.z() = msg.z; return point; } -} // namespace autoware_utils +} // namespace tier4_autoware_utils -BOOST_GEOMETRY_REGISTER_POINT_2D( // NOLINT - autoware_utils::Point2d, double, cs::cartesian, x(), y()) // NOLINT -BOOST_GEOMETRY_REGISTER_POINT_3D( // NOLINT - autoware_utils::Point3d, double, cs::cartesian, x(), y(), z()) // NOLINT +BOOST_GEOMETRY_REGISTER_POINT_2D( // NOLINT + tier4_autoware_utils::Point2d, double, cs::cartesian, x(), y()) // NOLINT +BOOST_GEOMETRY_REGISTER_POINT_3D( // NOLINT + tier4_autoware_utils::Point3d, double, cs::cartesian, x(), y(), z()) // NOLINT -#endif // AUTOWARE_UTILS__GEOMETRY__BOOST_GEOMETRY_HPP_ +#endif // TIER4_AUTOWARE_UTILS__GEOMETRY__BOOST_GEOMETRY_HPP_ diff --git a/common/autoware_utils/include/autoware_utils/geometry/geometry.hpp b/common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/geometry.hpp similarity index 97% rename from common/autoware_utils/include/autoware_utils/geometry/geometry.hpp rename to common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/geometry.hpp index aa03ffabef3fd..c4615db56590b 100644 --- a/common/autoware_utils/include/autoware_utils/geometry/geometry.hpp +++ b/common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/geometry.hpp @@ -12,15 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE_UTILS__GEOMETRY__GEOMETRY_HPP_ -#define AUTOWARE_UTILS__GEOMETRY__GEOMETRY_HPP_ +#ifndef TIER4_AUTOWARE_UTILS__GEOMETRY__GEOMETRY_HPP_ +#define TIER4_AUTOWARE_UTILS__GEOMETRY__GEOMETRY_HPP_ #include #include #include #define EIGEN_MPL2_ONLY -#include "autoware_utils/geometry/boost_geometry.hpp" +#include "tier4_autoware_utils/geometry/boost_geometry.hpp" #include #include @@ -35,7 +35,7 @@ #include -namespace autoware_utils +namespace tier4_autoware_utils { template geometry_msgs::msg::Point getPoint(const T & p) @@ -352,6 +352,6 @@ inline geometry_msgs::msg::Pose calcOffsetPose( tf2::toMsg(tf_pose * tf_offset, pose); return pose; } -} // namespace autoware_utils +} // namespace tier4_autoware_utils -#endif // AUTOWARE_UTILS__GEOMETRY__GEOMETRY_HPP_ +#endif // TIER4_AUTOWARE_UTILS__GEOMETRY__GEOMETRY_HPP_ diff --git a/common/autoware_utils/include/autoware_utils/geometry/pose_deviation.hpp b/common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/pose_deviation.hpp similarity index 87% rename from common/autoware_utils/include/autoware_utils/geometry/pose_deviation.hpp rename to common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/pose_deviation.hpp index 7f5d02076c77b..cd9e109c5c9ed 100644 --- a/common/autoware_utils/include/autoware_utils/geometry/pose_deviation.hpp +++ b/common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/pose_deviation.hpp @@ -12,15 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE_UTILS__GEOMETRY__POSE_DEVIATION_HPP_ -#define AUTOWARE_UTILS__GEOMETRY__POSE_DEVIATION_HPP_ +#ifndef TIER4_AUTOWARE_UTILS__GEOMETRY__POSE_DEVIATION_HPP_ +#define TIER4_AUTOWARE_UTILS__GEOMETRY__POSE_DEVIATION_HPP_ -#include "autoware_utils/geometry/geometry.hpp" -#include "autoware_utils/math/normalization.hpp" +#include "tier4_autoware_utils/geometry/geometry.hpp" +#include "tier4_autoware_utils/math/normalization.hpp" #include -namespace autoware_utils +namespace tier4_autoware_utils { struct PoseDeviation { @@ -80,6 +80,6 @@ inline PoseDeviation calcPoseDeviation( return deviation; } -} // namespace autoware_utils +} // namespace tier4_autoware_utils -#endif // AUTOWARE_UTILS__GEOMETRY__POSE_DEVIATION_HPP_ +#endif // TIER4_AUTOWARE_UTILS__GEOMETRY__POSE_DEVIATION_HPP_ diff --git a/common/autoware_utils/include/autoware_utils/math/constants.hpp b/common/tier4_autoware_utils/include/tier4_autoware_utils/math/constants.hpp similarity index 76% rename from common/autoware_utils/include/autoware_utils/math/constants.hpp rename to common/tier4_autoware_utils/include/tier4_autoware_utils/math/constants.hpp index 6b350a9d27e32..984fa04016b57 100644 --- a/common/autoware_utils/include/autoware_utils/math/constants.hpp +++ b/common/tier4_autoware_utils/include/tier4_autoware_utils/math/constants.hpp @@ -12,13 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE_UTILS__MATH__CONSTANTS_HPP_ -#define AUTOWARE_UTILS__MATH__CONSTANTS_HPP_ +#ifndef TIER4_AUTOWARE_UTILS__MATH__CONSTANTS_HPP_ +#define TIER4_AUTOWARE_UTILS__MATH__CONSTANTS_HPP_ -namespace autoware_utils +namespace tier4_autoware_utils { constexpr double pi = 3.14159265358979323846; // To be replaced by std::numbers::pi in C++20 constexpr double gravity = 9.80665; -} // namespace autoware_utils +} // namespace tier4_autoware_utils -#endif // AUTOWARE_UTILS__MATH__CONSTANTS_HPP_ +#endif // TIER4_AUTOWARE_UTILS__MATH__CONSTANTS_HPP_ diff --git a/common/autoware_utils/include/autoware_utils/math/normalization.hpp b/common/tier4_autoware_utils/include/tier4_autoware_utils/math/normalization.hpp similarity index 80% rename from common/autoware_utils/include/autoware_utils/math/normalization.hpp rename to common/tier4_autoware_utils/include/tier4_autoware_utils/math/normalization.hpp index ec992468cf087..1eb32f33886cf 100644 --- a/common/autoware_utils/include/autoware_utils/math/normalization.hpp +++ b/common/tier4_autoware_utils/include/tier4_autoware_utils/math/normalization.hpp @@ -12,14 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE_UTILS__MATH__NORMALIZATION_HPP_ -#define AUTOWARE_UTILS__MATH__NORMALIZATION_HPP_ +#ifndef TIER4_AUTOWARE_UTILS__MATH__NORMALIZATION_HPP_ +#define TIER4_AUTOWARE_UTILS__MATH__NORMALIZATION_HPP_ -#include "autoware_utils/math/constants.hpp" +#include "tier4_autoware_utils/math/constants.hpp" #include -namespace autoware_utils +namespace tier4_autoware_utils { inline double normalizeDegree(const double deg, const double min_deg = -180) { @@ -45,6 +45,6 @@ inline double normalizeRadian(const double rad, const double min_rad = -pi) return value - std::copysign(2 * pi, value); } -} // namespace autoware_utils +} // namespace tier4_autoware_utils -#endif // AUTOWARE_UTILS__MATH__NORMALIZATION_HPP_ +#endif // TIER4_AUTOWARE_UTILS__MATH__NORMALIZATION_HPP_ diff --git a/common/autoware_utils/include/autoware_utils/math/range.hpp b/common/tier4_autoware_utils/include/tier4_autoware_utils/math/range.hpp similarity index 90% rename from common/autoware_utils/include/autoware_utils/math/range.hpp rename to common/tier4_autoware_utils/include/tier4_autoware_utils/math/range.hpp index 50ea769f9e8ee..957a6f4e729e5 100644 --- a/common/autoware_utils/include/autoware_utils/math/range.hpp +++ b/common/tier4_autoware_utils/include/tier4_autoware_utils/math/range.hpp @@ -12,15 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE_UTILS__MATH__RANGE_HPP_ -#define AUTOWARE_UTILS__MATH__RANGE_HPP_ +#ifndef TIER4_AUTOWARE_UTILS__MATH__RANGE_HPP_ +#define TIER4_AUTOWARE_UTILS__MATH__RANGE_HPP_ #include #include #include #include -namespace autoware_utils +namespace tier4_autoware_utils { template std::vector arange(const T start, const T stop, const T step = 1) @@ -73,6 +73,6 @@ std::vector linspace(const T start, const T stop, const size_t num) return out; } -} // namespace autoware_utils +} // namespace tier4_autoware_utils -#endif // AUTOWARE_UTILS__MATH__RANGE_HPP_ +#endif // TIER4_AUTOWARE_UTILS__MATH__RANGE_HPP_ diff --git a/common/autoware_utils/include/autoware_utils/math/unit_conversion.hpp b/common/tier4_autoware_utils/include/tier4_autoware_utils/math/unit_conversion.hpp similarity index 75% rename from common/autoware_utils/include/autoware_utils/math/unit_conversion.hpp rename to common/tier4_autoware_utils/include/tier4_autoware_utils/math/unit_conversion.hpp index e4bfc992a136b..44eb5a3ff0666 100644 --- a/common/autoware_utils/include/autoware_utils/math/unit_conversion.hpp +++ b/common/tier4_autoware_utils/include/tier4_autoware_utils/math/unit_conversion.hpp @@ -12,18 +12,18 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE_UTILS__MATH__UNIT_CONVERSION_HPP_ -#define AUTOWARE_UTILS__MATH__UNIT_CONVERSION_HPP_ +#ifndef TIER4_AUTOWARE_UTILS__MATH__UNIT_CONVERSION_HPP_ +#define TIER4_AUTOWARE_UTILS__MATH__UNIT_CONVERSION_HPP_ -#include "autoware_utils/math/constants.hpp" +#include "tier4_autoware_utils/math/constants.hpp" -namespace autoware_utils +namespace tier4_autoware_utils { constexpr double deg2rad(const double deg) { return deg * pi / 180.0; } constexpr double rad2deg(const double rad) { return rad * 180.0 / pi; } constexpr double kmph2mps(const double kmph) { return kmph * 1000.0 / 3600.0; } constexpr double mps2kmph(const double mps) { return mps * 3600.0 / 1000.0; } -} // namespace autoware_utils +} // namespace tier4_autoware_utils -#endif // AUTOWARE_UTILS__MATH__UNIT_CONVERSION_HPP_ +#endif // TIER4_AUTOWARE_UTILS__MATH__UNIT_CONVERSION_HPP_ diff --git a/common/autoware_utils/include/autoware_utils/planning/planning_marker_helper.hpp b/common/tier4_autoware_utils/include/tier4_autoware_utils/planning/planning_marker_helper.hpp similarity index 78% rename from common/autoware_utils/include/autoware_utils/planning/planning_marker_helper.hpp rename to common/tier4_autoware_utils/include/tier4_autoware_utils/planning/planning_marker_helper.hpp index cb0448ee08e35..e4c7035fd7d87 100644 --- a/common/autoware_utils/include/autoware_utils/planning/planning_marker_helper.hpp +++ b/common/tier4_autoware_utils/include/tier4_autoware_utils/planning/planning_marker_helper.hpp @@ -12,14 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE_UTILS__PLANNING__PLANNING_MARKER_HELPER_HPP_ -#define AUTOWARE_UTILS__PLANNING__PLANNING_MARKER_HELPER_HPP_ +#ifndef TIER4_AUTOWARE_UTILS__PLANNING__PLANNING_MARKER_HELPER_HPP_ +#define TIER4_AUTOWARE_UTILS__PLANNING__PLANNING_MARKER_HELPER_HPP_ -#include "autoware_utils/ros/marker_helper.hpp" +#include "tier4_autoware_utils/ros/marker_helper.hpp" #include -namespace autoware_utils +namespace tier4_autoware_utils { visualization_msgs::msg::MarkerArray createStopVirtualWallMarker( const geometry_msgs::msg::Pose & pose, const std::string & module_name, const rclcpp::Time & now, @@ -32,6 +32,6 @@ visualization_msgs::msg::MarkerArray createSlowDownVirtualWallMarker( visualization_msgs::msg::MarkerArray createDeadLineVirtualWallMarker( const geometry_msgs::msg::Pose & pose, const std::string & module_name, const rclcpp::Time & now, const int32_t id); -} // namespace autoware_utils +} // namespace tier4_autoware_utils -#endif // AUTOWARE_UTILS__PLANNING__PLANNING_MARKER_HELPER_HPP_ +#endif // TIER4_AUTOWARE_UTILS__PLANNING__PLANNING_MARKER_HELPER_HPP_ diff --git a/common/autoware_utils/include/autoware_utils/ros/debug_publisher.hpp b/common/tier4_autoware_utils/include/tier4_autoware_utils/ros/debug_publisher.hpp similarity index 83% rename from common/autoware_utils/include/autoware_utils/ros/debug_publisher.hpp rename to common/tier4_autoware_utils/include/tier4_autoware_utils/ros/debug_publisher.hpp index bd2fd5641c6d5..0750b6894debe 100644 --- a/common/autoware_utils/include/autoware_utils/ros/debug_publisher.hpp +++ b/common/tier4_autoware_utils/include/tier4_autoware_utils/ros/debug_publisher.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE_UTILS__ROS__DEBUG_PUBLISHER_HPP_ -#define AUTOWARE_UTILS__ROS__DEBUG_PUBLISHER_HPP_ +#ifndef TIER4_AUTOWARE_UTILS__ROS__DEBUG_PUBLISHER_HPP_ +#define TIER4_AUTOWARE_UTILS__ROS__DEBUG_PUBLISHER_HPP_ -#include "autoware_utils/ros/debug_traits.hpp" +#include "tier4_autoware_utils/ros/debug_traits.hpp" #include #include @@ -25,14 +25,14 @@ #include #include -namespace autoware_utils +namespace tier4_autoware_utils { namespace debug_publisher { template < class T_msg, class T, - std::enable_if_t::value, std::nullptr_t> = - nullptr> + std::enable_if_t< + tier4_autoware_utils::debug_traits::is_debug_message::value, std::nullptr_t> = nullptr> T_msg toDebugMsg(const T & data, const rclcpp::Time & stamp) { T_msg msg; @@ -72,6 +72,6 @@ class DebugPublisher const char * ns_; std::unordered_map> pub_map_; }; -} // namespace autoware_utils +} // namespace tier4_autoware_utils -#endif // AUTOWARE_UTILS__ROS__DEBUG_PUBLISHER_HPP_ +#endif // TIER4_AUTOWARE_UTILS__ROS__DEBUG_PUBLISHER_HPP_ diff --git a/common/autoware_utils/include/autoware_utils/ros/debug_traits.hpp b/common/tier4_autoware_utils/include/tier4_autoware_utils/ros/debug_traits.hpp similarity index 89% rename from common/autoware_utils/include/autoware_utils/ros/debug_traits.hpp rename to common/tier4_autoware_utils/include/tier4_autoware_utils/ros/debug_traits.hpp index bf9a049b0a7cf..345e49079eede 100644 --- a/common/autoware_utils/include/autoware_utils/ros/debug_traits.hpp +++ b/common/tier4_autoware_utils/include/tier4_autoware_utils/ros/debug_traits.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE_UTILS__ROS__DEBUG_TRAITS_HPP_ -#define AUTOWARE_UTILS__ROS__DEBUG_TRAITS_HPP_ +#ifndef TIER4_AUTOWARE_UTILS__ROS__DEBUG_TRAITS_HPP_ +#define TIER4_AUTOWARE_UTILS__ROS__DEBUG_TRAITS_HPP_ #include #include @@ -28,7 +28,7 @@ #include -namespace autoware_utils::debug_traits +namespace tier4_autoware_utils::debug_traits { template struct is_debug_message : std::false_type @@ -84,6 +84,6 @@ template <> struct is_debug_message : std::true_type { }; -} // namespace autoware_utils::debug_traits +} // namespace tier4_autoware_utils::debug_traits -#endif // AUTOWARE_UTILS__ROS__DEBUG_TRAITS_HPP_ +#endif // TIER4_AUTOWARE_UTILS__ROS__DEBUG_TRAITS_HPP_ diff --git a/common/autoware_utils/include/autoware_utils/ros/marker_helper.hpp b/common/tier4_autoware_utils/include/tier4_autoware_utils/ros/marker_helper.hpp similarity index 91% rename from common/autoware_utils/include/autoware_utils/ros/marker_helper.hpp rename to common/tier4_autoware_utils/include/tier4_autoware_utils/ros/marker_helper.hpp index cffb05e9239e2..d674b6c1caaf4 100644 --- a/common/autoware_utils/include/autoware_utils/ros/marker_helper.hpp +++ b/common/tier4_autoware_utils/include/tier4_autoware_utils/ros/marker_helper.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE_UTILS__ROS__MARKER_HELPER_HPP_ -#define AUTOWARE_UTILS__ROS__MARKER_HELPER_HPP_ +#ifndef TIER4_AUTOWARE_UTILS__ROS__MARKER_HELPER_HPP_ +#define TIER4_AUTOWARE_UTILS__ROS__MARKER_HELPER_HPP_ #include @@ -21,7 +21,7 @@ #include -namespace autoware_utils +namespace tier4_autoware_utils { inline geometry_msgs::msg::Point createMarkerPosition(double x, double y, double z) { @@ -94,6 +94,6 @@ inline void appendMarkerArray( marker_array->markers.push_back(marker); } } -} // namespace autoware_utils +} // namespace tier4_autoware_utils -#endif // AUTOWARE_UTILS__ROS__MARKER_HELPER_HPP_ +#endif // TIER4_AUTOWARE_UTILS__ROS__MARKER_HELPER_HPP_ diff --git a/common/autoware_utils/include/autoware_utils/ros/processing_time_publisher.hpp b/common/tier4_autoware_utils/include/tier4_autoware_utils/ros/processing_time_publisher.hpp similarity index 86% rename from common/autoware_utils/include/autoware_utils/ros/processing_time_publisher.hpp rename to common/tier4_autoware_utils/include/tier4_autoware_utils/ros/processing_time_publisher.hpp index 2da2fa54defab..80d394a1c200e 100644 --- a/common/autoware_utils/include/autoware_utils/ros/processing_time_publisher.hpp +++ b/common/tier4_autoware_utils/include/tier4_autoware_utils/ros/processing_time_publisher.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE_UTILS__ROS__PROCESSING_TIME_PUBLISHER_HPP_ -#define AUTOWARE_UTILS__ROS__PROCESSING_TIME_PUBLISHER_HPP_ +#ifndef TIER4_AUTOWARE_UTILS__ROS__PROCESSING_TIME_PUBLISHER_HPP_ +#define TIER4_AUTOWARE_UTILS__ROS__PROCESSING_TIME_PUBLISHER_HPP_ #include @@ -23,7 +23,7 @@ #include #include -namespace autoware_utils +namespace tier4_autoware_utils { class ProcessingTimePublisher { @@ -62,6 +62,6 @@ class ProcessingTimePublisher return oss.str(); } }; -} // namespace autoware_utils +} // namespace tier4_autoware_utils -#endif // AUTOWARE_UTILS__ROS__PROCESSING_TIME_PUBLISHER_HPP_ +#endif // TIER4_AUTOWARE_UTILS__ROS__PROCESSING_TIME_PUBLISHER_HPP_ diff --git a/common/autoware_utils/include/autoware_utils/ros/self_pose_listener.hpp b/common/tier4_autoware_utils/include/tier4_autoware_utils/ros/self_pose_listener.hpp similarity index 78% rename from common/autoware_utils/include/autoware_utils/ros/self_pose_listener.hpp rename to common/tier4_autoware_utils/include/tier4_autoware_utils/ros/self_pose_listener.hpp index 27f1e5a86b466..8148392ecda3b 100644 --- a/common/autoware_utils/include/autoware_utils/ros/self_pose_listener.hpp +++ b/common/tier4_autoware_utils/include/tier4_autoware_utils/ros/self_pose_listener.hpp @@ -12,17 +12,17 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE_UTILS__ROS__SELF_POSE_LISTENER_HPP_ -#define AUTOWARE_UTILS__ROS__SELF_POSE_LISTENER_HPP_ +#ifndef TIER4_AUTOWARE_UTILS__ROS__SELF_POSE_LISTENER_HPP_ +#define TIER4_AUTOWARE_UTILS__ROS__SELF_POSE_LISTENER_HPP_ -#include "autoware_utils/geometry/geometry.hpp" -#include "autoware_utils/ros/transform_listener.hpp" +#include "tier4_autoware_utils/geometry/geometry.hpp" +#include "tier4_autoware_utils/ros/transform_listener.hpp" #include #include -namespace autoware_utils +namespace tier4_autoware_utils { class SelfPoseListener { @@ -53,6 +53,6 @@ class SelfPoseListener private: TransformListener transform_listener_; }; -} // namespace autoware_utils +} // namespace tier4_autoware_utils -#endif // AUTOWARE_UTILS__ROS__SELF_POSE_LISTENER_HPP_ +#endif // TIER4_AUTOWARE_UTILS__ROS__SELF_POSE_LISTENER_HPP_ diff --git a/common/autoware_utils/include/autoware_utils/ros/transform_listener.hpp b/common/tier4_autoware_utils/include/tier4_autoware_utils/ros/transform_listener.hpp similarity index 91% rename from common/autoware_utils/include/autoware_utils/ros/transform_listener.hpp rename to common/tier4_autoware_utils/include/tier4_autoware_utils/ros/transform_listener.hpp index a18ce2a207d50..ffc845575a76f 100644 --- a/common/autoware_utils/include/autoware_utils/ros/transform_listener.hpp +++ b/common/tier4_autoware_utils/include/tier4_autoware_utils/ros/transform_listener.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE_UTILS__ROS__TRANSFORM_LISTENER_HPP_ -#define AUTOWARE_UTILS__ROS__TRANSFORM_LISTENER_HPP_ +#ifndef TIER4_AUTOWARE_UTILS__ROS__TRANSFORM_LISTENER_HPP_ +#define TIER4_AUTOWARE_UTILS__ROS__TRANSFORM_LISTENER_HPP_ #include @@ -26,7 +26,7 @@ #include #include -namespace autoware_utils +namespace tier4_autoware_utils { class TransformListener { @@ -80,6 +80,6 @@ class TransformListener std::shared_ptr tf_buffer_; std::shared_ptr tf_listener_; }; -} // namespace autoware_utils +} // namespace tier4_autoware_utils -#endif // AUTOWARE_UTILS__ROS__TRANSFORM_LISTENER_HPP_ +#endif // TIER4_AUTOWARE_UTILS__ROS__TRANSFORM_LISTENER_HPP_ diff --git a/common/autoware_utils/include/autoware_utils/ros/update_param.hpp b/common/tier4_autoware_utils/include/tier4_autoware_utils/ros/update_param.hpp similarity index 82% rename from common/autoware_utils/include/autoware_utils/ros/update_param.hpp rename to common/tier4_autoware_utils/include/tier4_autoware_utils/ros/update_param.hpp index 4fce3e06530bc..36abcc11175e1 100644 --- a/common/autoware_utils/include/autoware_utils/ros/update_param.hpp +++ b/common/tier4_autoware_utils/include/tier4_autoware_utils/ros/update_param.hpp @@ -12,15 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE_UTILS__ROS__UPDATE_PARAM_HPP_ -#define AUTOWARE_UTILS__ROS__UPDATE_PARAM_HPP_ +#ifndef TIER4_AUTOWARE_UTILS__ROS__UPDATE_PARAM_HPP_ +#define TIER4_AUTOWARE_UTILS__ROS__UPDATE_PARAM_HPP_ #include #include #include -namespace autoware_utils +namespace tier4_autoware_utils { template bool updateParam(const std::vector & params, const std::string & name, T & value) @@ -37,6 +37,6 @@ bool updateParam(const std::vector & params, const std::strin value = itr->template get_value(); return true; } -} // namespace autoware_utils +} // namespace tier4_autoware_utils -#endif // AUTOWARE_UTILS__ROS__UPDATE_PARAM_HPP_ +#endif // TIER4_AUTOWARE_UTILS__ROS__UPDATE_PARAM_HPP_ diff --git a/common/autoware_utils/include/autoware_utils/ros/wait_for_param.hpp b/common/tier4_autoware_utils/include/tier4_autoware_utils/ros/wait_for_param.hpp similarity index 85% rename from common/autoware_utils/include/autoware_utils/ros/wait_for_param.hpp rename to common/tier4_autoware_utils/include/tier4_autoware_utils/ros/wait_for_param.hpp index 6f8616b3a502d..67a1249c5b042 100644 --- a/common/autoware_utils/include/autoware_utils/ros/wait_for_param.hpp +++ b/common/tier4_autoware_utils/include/tier4_autoware_utils/ros/wait_for_param.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE_UTILS__ROS__WAIT_FOR_PARAM_HPP_ -#define AUTOWARE_UTILS__ROS__WAIT_FOR_PARAM_HPP_ +#ifndef TIER4_AUTOWARE_UTILS__ROS__WAIT_FOR_PARAM_HPP_ +#define TIER4_AUTOWARE_UTILS__ROS__WAIT_FOR_PARAM_HPP_ #include @@ -21,7 +21,7 @@ #include #include -namespace autoware_utils +namespace tier4_autoware_utils { template T waitForParam( @@ -47,6 +47,6 @@ T waitForParam( return {}; } -} // namespace autoware_utils +} // namespace tier4_autoware_utils -#endif // AUTOWARE_UTILS__ROS__WAIT_FOR_PARAM_HPP_ +#endif // TIER4_AUTOWARE_UTILS__ROS__WAIT_FOR_PARAM_HPP_ diff --git a/common/autoware_utils/include/autoware_utils/system/heartbeat_checker.hpp b/common/tier4_autoware_utils/include/tier4_autoware_utils/system/heartbeat_checker.hpp similarity index 89% rename from common/autoware_utils/include/autoware_utils/system/heartbeat_checker.hpp rename to common/tier4_autoware_utils/include/tier4_autoware_utils/system/heartbeat_checker.hpp index d197c18d7833d..1c2f9d5dfb5da 100644 --- a/common/autoware_utils/include/autoware_utils/system/heartbeat_checker.hpp +++ b/common/tier4_autoware_utils/include/tier4_autoware_utils/system/heartbeat_checker.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE_UTILS__SYSTEM__HEARTBEAT_CHECKER_HPP_ -#define AUTOWARE_UTILS__SYSTEM__HEARTBEAT_CHECKER_HPP_ +#ifndef TIER4_AUTOWARE_UTILS__SYSTEM__HEARTBEAT_CHECKER_HPP_ +#define TIER4_AUTOWARE_UTILS__SYSTEM__HEARTBEAT_CHECKER_HPP_ #include @@ -55,4 +55,4 @@ class HeaderlessHeartbeatChecker } }; -#endif // AUTOWARE_UTILS__SYSTEM__HEARTBEAT_CHECKER_HPP_ +#endif // TIER4_AUTOWARE_UTILS__SYSTEM__HEARTBEAT_CHECKER_HPP_ diff --git a/common/autoware_utils/include/autoware_utils/system/stop_watch.hpp b/common/tier4_autoware_utils/include/tier4_autoware_utils/system/stop_watch.hpp similarity index 88% rename from common/autoware_utils/include/autoware_utils/system/stop_watch.hpp rename to common/tier4_autoware_utils/include/tier4_autoware_utils/system/stop_watch.hpp index bde2bdc727bd8..0d804e797936c 100644 --- a/common/autoware_utils/include/autoware_utils/system/stop_watch.hpp +++ b/common/tier4_autoware_utils/include/tier4_autoware_utils/system/stop_watch.hpp @@ -12,14 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE_UTILS__SYSTEM__STOP_WATCH_HPP_ -#define AUTOWARE_UTILS__SYSTEM__STOP_WATCH_HPP_ +#ifndef TIER4_AUTOWARE_UTILS__SYSTEM__STOP_WATCH_HPP_ +#define TIER4_AUTOWARE_UTILS__SYSTEM__STOP_WATCH_HPP_ #include #include #include -namespace autoware_utils +namespace tier4_autoware_utils { template < class OutputUnit = std::chrono::seconds, class InternalUnit = std::chrono::microseconds, @@ -58,6 +58,6 @@ class StopWatch std::unordered_map t_start_; }; -} // namespace autoware_utils +} // namespace tier4_autoware_utils -#endif // AUTOWARE_UTILS__SYSTEM__STOP_WATCH_HPP_ +#endif // TIER4_AUTOWARE_UTILS__SYSTEM__STOP_WATCH_HPP_ diff --git a/common/tier4_autoware_utils/include/tier4_autoware_utils/tier4_autoware_utils.hpp b/common/tier4_autoware_utils/include/tier4_autoware_utils/tier4_autoware_utils.hpp new file mode 100644 index 0000000000000..e86af3c17b4e0 --- /dev/null +++ b/common/tier4_autoware_utils/include/tier4_autoware_utils/tier4_autoware_utils.hpp @@ -0,0 +1,37 @@ +// Copyright 2020 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef TIER4_AUTOWARE_UTILS__TIER4_AUTOWARE_UTILS_HPP_ +#define TIER4_AUTOWARE_UTILS__TIER4_AUTOWARE_UTILS_HPP_ + +#include "tier4_autoware_utils/geometry/boost_geometry.hpp" +#include "tier4_autoware_utils/geometry/geometry.hpp" +#include "tier4_autoware_utils/geometry/pose_deviation.hpp" +#include "tier4_autoware_utils/math/constants.hpp" +#include "tier4_autoware_utils/math/normalization.hpp" +#include "tier4_autoware_utils/math/range.hpp" +#include "tier4_autoware_utils/math/unit_conversion.hpp" +#include "tier4_autoware_utils/planning/planning_marker_helper.hpp" +#include "tier4_autoware_utils/ros/debug_publisher.hpp" +#include "tier4_autoware_utils/ros/debug_traits.hpp" +#include "tier4_autoware_utils/ros/marker_helper.hpp" +#include "tier4_autoware_utils/ros/processing_time_publisher.hpp" +#include "tier4_autoware_utils/ros/self_pose_listener.hpp" +#include "tier4_autoware_utils/ros/transform_listener.hpp" +#include "tier4_autoware_utils/ros/update_param.hpp" +#include "tier4_autoware_utils/ros/wait_for_param.hpp" +#include "tier4_autoware_utils/system/stop_watch.hpp" +#include "tier4_autoware_utils/trajectory/trajectory.hpp" + +#endif // TIER4_AUTOWARE_UTILS__TIER4_AUTOWARE_UTILS_HPP_ diff --git a/common/autoware_utils/include/autoware_utils/trajectory/tmp_conversion.hpp b/common/tier4_autoware_utils/include/tier4_autoware_utils/trajectory/tmp_conversion.hpp similarity index 87% rename from common/autoware_utils/include/autoware_utils/trajectory/tmp_conversion.hpp rename to common/tier4_autoware_utils/include/tier4_autoware_utils/trajectory/tmp_conversion.hpp index ba745d41d8104..51f865a6f8867 100644 --- a/common/autoware_utils/include/autoware_utils/trajectory/tmp_conversion.hpp +++ b/common/tier4_autoware_utils/include/tier4_autoware_utils/trajectory/tmp_conversion.hpp @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE_UTILS__TRAJECTORY__CONVERT_HPP_ -#define AUTOWARE_UTILS__TRAJECTORY__CONVERT_HPP_ +#ifndef TIER4_AUTOWARE_UTILS__TRAJECTORY__CONVERT_HPP_ +#define TIER4_AUTOWARE_UTILS__TRAJECTORY__CONVERT_HPP_ -#include "autoware_utils/geometry/geometry.hpp" -#include "autoware_utils/geometry/pose_deviation.hpp" +#include "tier4_autoware_utils/geometry/geometry.hpp" +#include "tier4_autoware_utils/geometry/pose_deviation.hpp" #include "autoware_auto_planning_msgs/msg/trajectory.hpp" #include "autoware_auto_planning_msgs/msg/trajectory_point.hpp" @@ -26,7 +26,7 @@ #include #include -namespace autoware_utils +namespace tier4_autoware_utils { /** * @brief Convert std::vector to @@ -64,6 +64,6 @@ std::vector convertToTrajecto return output; } -} // namespace autoware_utils +} // namespace tier4_autoware_utils -#endif // AUTOWARE_UTILS__TRAJECTORY__CONVERT_HPP_ +#endif // TIER4_AUTOWARE_UTILS__TRAJECTORY__CONVERT_HPP_ diff --git a/common/autoware_utils/include/autoware_utils/trajectory/trajectory.hpp b/common/tier4_autoware_utils/include/tier4_autoware_utils/trajectory/trajectory.hpp similarity index 95% rename from common/autoware_utils/include/autoware_utils/trajectory/trajectory.hpp rename to common/tier4_autoware_utils/include/tier4_autoware_utils/trajectory/trajectory.hpp index ae2c572eee2a7..7e8d50dbbff94 100644 --- a/common/autoware_utils/include/autoware_utils/trajectory/trajectory.hpp +++ b/common/tier4_autoware_utils/include/tier4_autoware_utils/trajectory/trajectory.hpp @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE_UTILS__TRAJECTORY__TRAJECTORY_HPP_ -#define AUTOWARE_UTILS__TRAJECTORY__TRAJECTORY_HPP_ +#ifndef TIER4_AUTOWARE_UTILS__TRAJECTORY__TRAJECTORY_HPP_ +#define TIER4_AUTOWARE_UTILS__TRAJECTORY__TRAJECTORY_HPP_ -#include "autoware_utils/geometry/geometry.hpp" -#include "autoware_utils/geometry/pose_deviation.hpp" +#include "tier4_autoware_utils/geometry/geometry.hpp" +#include "tier4_autoware_utils/geometry/pose_deviation.hpp" #include @@ -24,7 +24,7 @@ #include #include -namespace autoware_utils +namespace tier4_autoware_utils { template void validateNonEmpty(const T & points) @@ -275,6 +275,6 @@ double calcArcLength(const T & points) return calcSignedArcLength(points, 0, points.size() - 1); } -} // namespace autoware_utils +} // namespace tier4_autoware_utils -#endif // AUTOWARE_UTILS__TRAJECTORY__TRAJECTORY_HPP_ +#endif // TIER4_AUTOWARE_UTILS__TRAJECTORY__TRAJECTORY_HPP_ diff --git a/common/autoware_utils/package.xml b/common/tier4_autoware_utils/package.xml similarity index 89% rename from common/autoware_utils/package.xml rename to common/tier4_autoware_utils/package.xml index 6bb8d68feac94..85a476331d529 100644 --- a/common/autoware_utils/package.xml +++ b/common/tier4_autoware_utils/package.xml @@ -1,8 +1,8 @@ - autoware_utils + tier4_autoware_utils 0.1.0 - The autoware_utils package + The tier4_autoware_utils package Takamasa Horibe Kenji Miyake Apache License 2.0 diff --git a/common/autoware_utils/src/planning/planning_marker_helper.cpp b/common/tier4_autoware_utils/src/planning/planning_marker_helper.cpp similarity index 90% rename from common/autoware_utils/src/planning/planning_marker_helper.cpp rename to common/tier4_autoware_utils/src/planning/planning_marker_helper.cpp index d98337a636080..6ffc26e5bb277 100644 --- a/common/autoware_utils/src/planning/planning_marker_helper.cpp +++ b/common/tier4_autoware_utils/src/planning/planning_marker_helper.cpp @@ -12,13 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware_utils/planning/planning_marker_helper.hpp" +#include "tier4_autoware_utils/planning/planning_marker_helper.hpp" #include -using autoware_utils::createDefaultMarker; -using autoware_utils::createMarkerColor; -using autoware_utils::createMarkerScale; +using tier4_autoware_utils::createDefaultMarker; +using tier4_autoware_utils::createMarkerColor; +using tier4_autoware_utils::createMarkerScale; namespace { @@ -59,7 +59,7 @@ inline visualization_msgs::msg::MarkerArray createVirtualWallMarkerArray( } // namespace -namespace autoware_utils +namespace tier4_autoware_utils { visualization_msgs::msg::MarkerArray createStopVirtualWallMarker( const geometry_msgs::msg::Pose & pose, const std::string & module_name, const rclcpp::Time & now, @@ -84,4 +84,4 @@ visualization_msgs::msg::MarkerArray createDeadLineVirtualWallMarker( return createVirtualWallMarkerArray( pose, module_name, "dead_line_", now, id, createMarkerColor(0.0, 1.0, 0.0, 0.5)); } -} // namespace autoware_utils +} // namespace tier4_autoware_utils diff --git a/common/autoware_utils/src/autoware_utils.cpp b/common/tier4_autoware_utils/src/tier4_autoware_utils.cpp similarity index 91% rename from common/autoware_utils/src/autoware_utils.cpp rename to common/tier4_autoware_utils/src/tier4_autoware_utils.cpp index 2458e2ce146ea..a541d314aa365 100644 --- a/common/autoware_utils/src/autoware_utils.cpp +++ b/common/tier4_autoware_utils/src/tier4_autoware_utils.cpp @@ -12,4 +12,4 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware_utils/autoware_utils.hpp" +#include "tier4_autoware_utils/tier4_autoware_utils.hpp" diff --git a/common/autoware_utils/test/src/geometry/test_boost_geometry.cpp b/common/tier4_autoware_utils/test/src/geometry/test_boost_geometry.cpp similarity index 89% rename from common/autoware_utils/test/src/geometry/test_boost_geometry.cpp rename to common/tier4_autoware_utils/test/src/geometry/test_boost_geometry.cpp index 6e96855e4289d..4e5483935da00 100644 --- a/common/autoware_utils/test/src/geometry/test_boost_geometry.cpp +++ b/common/tier4_autoware_utils/test/src/geometry/test_boost_geometry.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware_utils/geometry/boost_geometry.hpp" +#include "tier4_autoware_utils/geometry/boost_geometry.hpp" #include @@ -20,8 +20,8 @@ namespace bg = boost::geometry; -using autoware_utils::Point2d; -using autoware_utils::Point3d; +using tier4_autoware_utils::Point2d; +using tier4_autoware_utils::Point3d; TEST(boost_geometry, boost_geometry_distance) { @@ -54,7 +54,7 @@ TEST(boost_geometry, to_2d) TEST(boost_geometry, toMsg) { - using autoware_utils::toMsg; + using tier4_autoware_utils::toMsg; { const Point3d p(1.0, 2.0, 3.0); @@ -68,7 +68,7 @@ TEST(boost_geometry, toMsg) TEST(boost_geometry, fromMsg) { - using autoware_utils::fromMsg; + using tier4_autoware_utils::fromMsg; geometry_msgs::msg::Point p_msg; p_msg.x = 1.0; diff --git a/common/autoware_utils/test/src/geometry/test_geometry.cpp b/common/tier4_autoware_utils/test/src/geometry/test_geometry.cpp similarity index 86% rename from common/autoware_utils/test/src/geometry/test_geometry.cpp rename to common/tier4_autoware_utils/test/src/geometry/test_geometry.cpp index 99ae3ac32dfa8..eee28d89ee36d 100644 --- a/common/autoware_utils/test/src/geometry/test_geometry.cpp +++ b/common/tier4_autoware_utils/test/src/geometry/test_geometry.cpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware_utils/geometry/geometry.hpp" -#include "autoware_utils/math/unit_conversion.hpp" +#include "tier4_autoware_utils/geometry/geometry.hpp" +#include "tier4_autoware_utils/math/unit_conversion.hpp" #include @@ -23,7 +23,7 @@ constexpr double epsilon = 1e-6; TEST(geometry, getPoint) { - using autoware_utils::getPoint; + using tier4_autoware_utils::getPoint; const double x_ans = 1.0; const double y_ans = 2.0; @@ -112,7 +112,7 @@ TEST(geometry, getPoint) TEST(geometry, getPose) { - using autoware_utils::getPose; + using tier4_autoware_utils::getPose; const double x_ans = 1.0; const double y_ans = 2.0; @@ -201,7 +201,7 @@ TEST(geometry, getPose) TEST(geometry, createPoint) { - using autoware_utils::createPoint; + using tier4_autoware_utils::createPoint; const geometry_msgs::msg::Point p_out = createPoint(1.0, 2.0, 3.0); EXPECT_DOUBLE_EQ(p_out.x, 1.0); @@ -211,7 +211,7 @@ TEST(geometry, createPoint) TEST(geometry, createQuaternion) { - using autoware_utils::createQuaternion; + using tier4_autoware_utils::createQuaternion; // (0.18257419, 0.36514837, 0.54772256, 0.73029674) is normalized quaternion of (1, 2, 3, 4) const geometry_msgs::msg::Quaternion q_out = @@ -224,7 +224,7 @@ TEST(geometry, createQuaternion) TEST(geometry, createTranslation) { - using autoware_utils::createTranslation; + using tier4_autoware_utils::createTranslation; const geometry_msgs::msg::Vector3 v_out = createTranslation(1.0, 2.0, 3.0); EXPECT_DOUBLE_EQ(v_out.x, 1.0); @@ -234,8 +234,8 @@ TEST(geometry, createTranslation) TEST(geometry, createQuaternionFromRPY) { - using autoware_utils::createQuaternionFromRPY; - using autoware_utils::deg2rad; + using tier4_autoware_utils::createQuaternionFromRPY; + using tier4_autoware_utils::deg2rad; { const auto q_out = createQuaternionFromRPY(0, 0, 0); @@ -264,8 +264,8 @@ TEST(geometry, createQuaternionFromRPY) TEST(geometry, createQuaternionFromYaw) { - using autoware_utils::createQuaternionFromYaw; - using autoware_utils::deg2rad; + using tier4_autoware_utils::createQuaternionFromYaw; + using tier4_autoware_utils::deg2rad; { const auto q_out = createQuaternionFromYaw(0); @@ -294,9 +294,9 @@ TEST(geometry, createQuaternionFromYaw) TEST(geometry, calcElevationAngle) { - using autoware_utils::calcElevationAngle; - using autoware_utils::createPoint; - using autoware_utils::deg2rad; + using tier4_autoware_utils::calcElevationAngle; + using tier4_autoware_utils::createPoint; + using tier4_autoware_utils::deg2rad; { const auto p1 = createPoint(1.0, 1.0, 1.0); @@ -342,9 +342,9 @@ TEST(geometry, calcElevationAngle) TEST(geometry, calcAzimuthAngle) { - using autoware_utils::calcAzimuthAngle; - using autoware_utils::createPoint; - using autoware_utils::deg2rad; + using tier4_autoware_utils::calcAzimuthAngle; + using tier4_autoware_utils::createPoint; + using tier4_autoware_utils::deg2rad; { const auto p1 = createPoint(0.0, 0.0, 9.0); @@ -395,7 +395,7 @@ TEST(geometry, calcAzimuthAngle) TEST(geometry, calcDistance2d) { - using autoware_utils::calcDistance2d; + using tier4_autoware_utils::calcDistance2d; geometry_msgs::msg::Point point; point.x = 1.0; @@ -412,7 +412,7 @@ TEST(geometry, calcDistance2d) TEST(geometry, calcSquaredDistance2d) { - using autoware_utils::calcSquaredDistance2d; + using tier4_autoware_utils::calcSquaredDistance2d; geometry_msgs::msg::Point point; point.x = 1.0; @@ -429,7 +429,7 @@ TEST(geometry, calcSquaredDistance2d) TEST(geometry, calcDistance3d) { - using autoware_utils::calcDistance3d; + using tier4_autoware_utils::calcDistance3d; geometry_msgs::msg::Point point; point.x = 1.0; @@ -446,9 +446,9 @@ TEST(geometry, calcDistance3d) TEST(geometry, getRPY) { - using autoware_utils::createQuaternionFromRPY; - using autoware_utils::deg2rad; - using autoware_utils::getRPY; + using tier4_autoware_utils::createQuaternionFromRPY; + using tier4_autoware_utils::deg2rad; + using tier4_autoware_utils::getRPY; { const double ans_roll = deg2rad(5); @@ -484,9 +484,9 @@ TEST(geometry, getRPY) TEST(geometry, getRPY_wrapper) { - using autoware_utils::createQuaternionFromRPY; - using autoware_utils::deg2rad; - using autoware_utils::getRPY; + using tier4_autoware_utils::createQuaternionFromRPY; + using tier4_autoware_utils::deg2rad; + using tier4_autoware_utils::getRPY; { const double ans_roll = deg2rad(45); @@ -526,9 +526,9 @@ TEST(geometry, getRPY_wrapper) TEST(geometry, transform2pose) { - using autoware_utils::createQuaternionFromRPY; - using autoware_utils::deg2rad; - using autoware_utils::transform2pose; + using tier4_autoware_utils::createQuaternionFromRPY; + using tier4_autoware_utils::deg2rad; + using tier4_autoware_utils::transform2pose; { geometry_msgs::msg::Transform transform; @@ -577,9 +577,9 @@ TEST(geometry, transform2pose) TEST(geometry, pose2transform) { - using autoware_utils::createQuaternionFromRPY; - using autoware_utils::deg2rad; - using autoware_utils::pose2transform; + using tier4_autoware_utils::createQuaternionFromRPY; + using tier4_autoware_utils::deg2rad; + using tier4_autoware_utils::pose2transform; { geometry_msgs::msg::Pose pose; @@ -630,11 +630,11 @@ TEST(geometry, pose2transform) TEST(geometry, transformPoint) { - using autoware_utils::createQuaternionFromRPY; - using autoware_utils::deg2rad; - using autoware_utils::Point2d; - using autoware_utils::Point3d; - using autoware_utils::transformPoint; + using tier4_autoware_utils::createQuaternionFromRPY; + using tier4_autoware_utils::deg2rad; + using tier4_autoware_utils::Point2d; + using tier4_autoware_utils::Point3d; + using tier4_autoware_utils::transformPoint; { const Point2d p(1.0, 2.0); @@ -669,10 +669,10 @@ TEST(geometry, transformPoint) TEST(geometry, transformVector) { - using autoware_utils::createQuaternionFromRPY; - using autoware_utils::deg2rad; - using autoware_utils::MultiPoint3d; - using autoware_utils::transformVector; + using tier4_autoware_utils::createQuaternionFromRPY; + using tier4_autoware_utils::deg2rad; + using tier4_autoware_utils::MultiPoint3d; + using tier4_autoware_utils::transformVector; { const MultiPoint3d ps{{1.0, 2.0, 3.0}, {2.0, 3.0, 4.0}}; @@ -697,51 +697,51 @@ TEST(geometry, transformVector) TEST(geometry, calcCurvature) { - using autoware_utils::calcCurvature; + using tier4_autoware_utils::calcCurvature; // Straight Line { - geometry_msgs::msg::Point p1 = autoware_utils::createPoint(0.0, 0.0, 0.0); - geometry_msgs::msg::Point p2 = autoware_utils::createPoint(1.0, 0.0, 0.0); - geometry_msgs::msg::Point p3 = autoware_utils::createPoint(2.0, 0.0, 0.0); + geometry_msgs::msg::Point p1 = tier4_autoware_utils::createPoint(0.0, 0.0, 0.0); + geometry_msgs::msg::Point p2 = tier4_autoware_utils::createPoint(1.0, 0.0, 0.0); + geometry_msgs::msg::Point p3 = tier4_autoware_utils::createPoint(2.0, 0.0, 0.0); EXPECT_DOUBLE_EQ(calcCurvature(p1, p2, p3), 0.0); } // Clockwise Curved Road with a 1[m] radius { - geometry_msgs::msg::Point p1 = autoware_utils::createPoint(0.0, 0.0, 0.0); - geometry_msgs::msg::Point p2 = autoware_utils::createPoint(1.0, 1.0, 0.0); - geometry_msgs::msg::Point p3 = autoware_utils::createPoint(2.0, 0.0, 0.0); + geometry_msgs::msg::Point p1 = tier4_autoware_utils::createPoint(0.0, 0.0, 0.0); + geometry_msgs::msg::Point p2 = tier4_autoware_utils::createPoint(1.0, 1.0, 0.0); + geometry_msgs::msg::Point p3 = tier4_autoware_utils::createPoint(2.0, 0.0, 0.0); EXPECT_DOUBLE_EQ(calcCurvature(p1, p2, p3), -1.0); } // Clockwise Curved Road with a 5[m] radius { - geometry_msgs::msg::Point p1 = autoware_utils::createPoint(0.0, 0.0, 0.0); - geometry_msgs::msg::Point p2 = autoware_utils::createPoint(5.0, 5.0, 0.0); - geometry_msgs::msg::Point p3 = autoware_utils::createPoint(10.0, 0.0, 0.0); + geometry_msgs::msg::Point p1 = tier4_autoware_utils::createPoint(0.0, 0.0, 0.0); + geometry_msgs::msg::Point p2 = tier4_autoware_utils::createPoint(5.0, 5.0, 0.0); + geometry_msgs::msg::Point p3 = tier4_autoware_utils::createPoint(10.0, 0.0, 0.0); EXPECT_DOUBLE_EQ(calcCurvature(p1, p2, p3), -0.2); } // Counter-Clockwise Curved Road with a 1[m] radius { - geometry_msgs::msg::Point p1 = autoware_utils::createPoint(0.0, 0.0, 0.0); - geometry_msgs::msg::Point p2 = autoware_utils::createPoint(-1.0, 1.0, 0.0); - geometry_msgs::msg::Point p3 = autoware_utils::createPoint(-2.0, 0.0, 0.0); + geometry_msgs::msg::Point p1 = tier4_autoware_utils::createPoint(0.0, 0.0, 0.0); + geometry_msgs::msg::Point p2 = tier4_autoware_utils::createPoint(-1.0, 1.0, 0.0); + geometry_msgs::msg::Point p3 = tier4_autoware_utils::createPoint(-2.0, 0.0, 0.0); EXPECT_DOUBLE_EQ(calcCurvature(p1, p2, p3), 1.0); } // Counter-Clockwise Curved Road with a 5[m] radius { - geometry_msgs::msg::Point p1 = autoware_utils::createPoint(0.0, 0.0, 0.0); - geometry_msgs::msg::Point p2 = autoware_utils::createPoint(-5.0, 5.0, 0.0); - geometry_msgs::msg::Point p3 = autoware_utils::createPoint(-10.0, 0.0, 0.0); + geometry_msgs::msg::Point p1 = tier4_autoware_utils::createPoint(0.0, 0.0, 0.0); + geometry_msgs::msg::Point p2 = tier4_autoware_utils::createPoint(-5.0, 5.0, 0.0); + geometry_msgs::msg::Point p3 = tier4_autoware_utils::createPoint(-10.0, 0.0, 0.0); EXPECT_DOUBLE_EQ(calcCurvature(p1, p2, p3), 0.2); } // Give same points { - geometry_msgs::msg::Point p1 = autoware_utils::createPoint(0.0, 0.0, 0.0); - geometry_msgs::msg::Point p2 = autoware_utils::createPoint(1.0, 0.0, 0.0); + geometry_msgs::msg::Point p1 = tier4_autoware_utils::createPoint(0.0, 0.0, 0.0); + geometry_msgs::msg::Point p2 = tier4_autoware_utils::createPoint(1.0, 0.0, 0.0); ASSERT_ANY_THROW(calcCurvature(p1, p1, p1)); ASSERT_ANY_THROW(calcCurvature(p1, p1, p2)); ASSERT_ANY_THROW(calcCurvature(p1, p2, p1)); @@ -751,11 +751,11 @@ TEST(geometry, calcCurvature) TEST(geometry, calcOffsetPose) { - using autoware_utils::calcOffsetPose; - using autoware_utils::createPoint; - using autoware_utils::createQuaternion; - using autoware_utils::createQuaternionFromRPY; - using autoware_utils::deg2rad; + using tier4_autoware_utils::calcOffsetPose; + using tier4_autoware_utils::createPoint; + using tier4_autoware_utils::createQuaternion; + using tier4_autoware_utils::createQuaternionFromRPY; + using tier4_autoware_utils::deg2rad; { geometry_msgs::msg::Pose p_in; diff --git a/common/autoware_utils/test/src/geometry/test_pose_deviation.cpp b/common/tier4_autoware_utils/test/src/geometry/test_pose_deviation.cpp similarity index 82% rename from common/autoware_utils/test/src/geometry/test_pose_deviation.cpp rename to common/tier4_autoware_utils/test/src/geometry/test_pose_deviation.cpp index 8ce2e9448a7d1..383c35921c994 100644 --- a/common/autoware_utils/test/src/geometry/test_pose_deviation.cpp +++ b/common/tier4_autoware_utils/test/src/geometry/test_pose_deviation.cpp @@ -12,16 +12,16 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware_utils/geometry/pose_deviation.hpp" -#include "autoware_utils/math/unit_conversion.hpp" +#include "tier4_autoware_utils/geometry/pose_deviation.hpp" +#include "tier4_autoware_utils/math/unit_conversion.hpp" #include TEST(geometry, pose_deviation) { - using autoware_utils::calcPoseDeviation; - using autoware_utils::createQuaternionFromRPY; - using autoware_utils::deg2rad; + using tier4_autoware_utils::calcPoseDeviation; + using tier4_autoware_utils::createQuaternionFromRPY; + using tier4_autoware_utils::deg2rad; geometry_msgs::msg::Pose pose1; pose1.position.x = 1.0; diff --git a/common/autoware_utils/test/src/math/test_constants.cpp b/common/tier4_autoware_utils/test/src/math/test_constants.cpp similarity index 85% rename from common/autoware_utils/test/src/math/test_constants.cpp rename to common/tier4_autoware_utils/test/src/math/test_constants.cpp index 9aeb0f6909194..80db5414a30f4 100644 --- a/common/autoware_utils/test/src/math/test_constants.cpp +++ b/common/tier4_autoware_utils/test/src/math/test_constants.cpp @@ -12,20 +12,20 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware_utils/math/constants.hpp" +#include "tier4_autoware_utils/math/constants.hpp" #include TEST(constants, pi) { - using autoware_utils::pi; + using tier4_autoware_utils::pi; EXPECT_DOUBLE_EQ(pi, 3.14159265358979323846); } TEST(constants, gravity) { - using autoware_utils::gravity; + using tier4_autoware_utils::gravity; EXPECT_DOUBLE_EQ(gravity, 9.80665); } diff --git a/common/autoware_utils/test/src/math/test_normalization.cpp b/common/tier4_autoware_utils/test/src/math/test_normalization.cpp similarity index 94% rename from common/autoware_utils/test/src/math/test_normalization.cpp rename to common/tier4_autoware_utils/test/src/math/test_normalization.cpp index 76ac7ec17d02a..0ae15ea320850 100644 --- a/common/autoware_utils/test/src/math/test_normalization.cpp +++ b/common/tier4_autoware_utils/test/src/math/test_normalization.cpp @@ -12,13 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware_utils/math/normalization.hpp" +#include "tier4_autoware_utils/math/normalization.hpp" #include TEST(normalization, normalizeDegree) { - using autoware_utils::normalizeDegree; + using tier4_autoware_utils::normalizeDegree; // -180 <= deg < 180 { @@ -51,7 +51,7 @@ TEST(normalization, normalizeDegree) TEST(normalization, normalizeRadian) { - using autoware_utils::normalizeRadian; + using tier4_autoware_utils::normalizeRadian; // -M_PI <= deg < M_PI { diff --git a/common/autoware_utils/test/src/math/test_range.cpp b/common/tier4_autoware_utils/test/src/math/test_range.cpp similarity index 96% rename from common/autoware_utils/test/src/math/test_range.cpp rename to common/tier4_autoware_utils/test/src/math/test_range.cpp index 2d166dbfa9044..429c08938949f 100644 --- a/common/autoware_utils/test/src/math/test_range.cpp +++ b/common/tier4_autoware_utils/test/src/math/test_range.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware_utils/math/range.hpp" +#include "tier4_autoware_utils/math/range.hpp" #include @@ -44,7 +44,7 @@ void expect_eq_vector(const std::vector & input, const std::vector & e TEST(arange_Test, arange_double) { - using autoware_utils::arange; + using tier4_autoware_utils::arange; // general cases { @@ -82,7 +82,7 @@ TEST(arange_Test, arange_double) TEST(arange_Test, arange_float) { - using autoware_utils::arange; + using tier4_autoware_utils::arange; // general cases { @@ -121,7 +121,7 @@ TEST(arange_Test, arange_float) TEST(arange_Test, arange_int) { - using autoware_utils::arange; + using tier4_autoware_utils::arange; // general cases { @@ -154,7 +154,7 @@ TEST(arange_Test, arange_int) TEST(test_linspace, linspace_double) { - using autoware_utils::linspace; + using tier4_autoware_utils::linspace; // general cases { @@ -182,7 +182,7 @@ TEST(test_linspace, linspace_double) TEST(test_linspace, linspace_float) { - using autoware_utils::linspace; + using tier4_autoware_utils::linspace; // general cases { @@ -211,7 +211,7 @@ TEST(test_linspace, linspace_float) TEST(test_linspace, linspace_int) { - using autoware_utils::linspace; + using tier4_autoware_utils::linspace; // general cases { diff --git a/common/autoware_utils/test/src/math/test_unit_conversion.cpp b/common/tier4_autoware_utils/test/src/math/test_unit_conversion.cpp similarity index 86% rename from common/autoware_utils/test/src/math/test_unit_conversion.cpp rename to common/tier4_autoware_utils/test/src/math/test_unit_conversion.cpp index 0b776b7dd8d24..9b689d1257932 100644 --- a/common/autoware_utils/test/src/math/test_unit_conversion.cpp +++ b/common/tier4_autoware_utils/test/src/math/test_unit_conversion.cpp @@ -12,15 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware_utils/math/unit_conversion.hpp" +#include "tier4_autoware_utils/math/unit_conversion.hpp" #include -using autoware_utils::pi; +using tier4_autoware_utils::pi; TEST(unit_conversion, deg2rad) { - using autoware_utils::deg2rad; + using tier4_autoware_utils::deg2rad; EXPECT_DOUBLE_EQ(deg2rad(-720), -4 * pi); EXPECT_DOUBLE_EQ(deg2rad(0), 0); @@ -33,7 +33,7 @@ TEST(unit_conversion, deg2rad) TEST(unit_conversion, rad2deg) { - using autoware_utils::rad2deg; + using tier4_autoware_utils::rad2deg; EXPECT_DOUBLE_EQ(rad2deg(-4 * pi), -720); EXPECT_DOUBLE_EQ(rad2deg(0), 0); @@ -46,7 +46,7 @@ TEST(unit_conversion, rad2deg) TEST(unit_conversion, kmph2mps) { - using autoware_utils::kmph2mps; + using tier4_autoware_utils::kmph2mps; EXPECT_DOUBLE_EQ(kmph2mps(0), 0); EXPECT_DOUBLE_EQ(kmph2mps(36), 10); @@ -56,7 +56,7 @@ TEST(unit_conversion, kmph2mps) TEST(unit_conversion, mps2kmph) { - using autoware_utils::mps2kmph; + using tier4_autoware_utils::mps2kmph; EXPECT_DOUBLE_EQ(mps2kmph(0), 0); EXPECT_DOUBLE_EQ(mps2kmph(10), 36); diff --git a/common/autoware_utils/test/src/system/test_stop_watch.cpp b/common/tier4_autoware_utils/test/src/system/test_stop_watch.cpp similarity index 92% rename from common/autoware_utils/test/src/system/test_stop_watch.cpp rename to common/tier4_autoware_utils/test/src/system/test_stop_watch.cpp index 45a1c357a5b09..cb201ab0dd5ec 100644 --- a/common/autoware_utils/test/src/system/test_stop_watch.cpp +++ b/common/tier4_autoware_utils/test/src/system/test_stop_watch.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware_utils/system/stop_watch.hpp" +#include "tier4_autoware_utils/system/stop_watch.hpp" #include @@ -21,7 +21,7 @@ TEST(system, StopWatch_sec) { - using autoware_utils::StopWatch; + using tier4_autoware_utils::StopWatch; StopWatch stop_watch; @@ -50,7 +50,7 @@ TEST(system, StopWatch_sec) TEST(system, StopWatch_msec) { - using autoware_utils::StopWatch; + using tier4_autoware_utils::StopWatch; StopWatch stop_watch; diff --git a/common/autoware_utils/test/src/test_autoware_utils.cpp b/common/tier4_autoware_utils/test/src/test_autoware_utils.cpp similarity index 92% rename from common/autoware_utils/test/src/test_autoware_utils.cpp rename to common/tier4_autoware_utils/test/src/test_autoware_utils.cpp index 764089a855439..c2c4bc4e5fbbd 100644 --- a/common/autoware_utils/test/src/test_autoware_utils.cpp +++ b/common/tier4_autoware_utils/test/src/test_autoware_utils.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware_utils/autoware_utils.hpp" +#include "tier4_autoware_utils/tier4_autoware_utils.hpp" #include diff --git a/common/autoware_utils/test/src/trajectory/test_trajectory.cpp b/common/tier4_autoware_utils/test/src/trajectory/test_trajectory.cpp similarity index 93% rename from common/autoware_utils/test/src/trajectory/test_trajectory.cpp rename to common/tier4_autoware_utils/test/src/trajectory/test_trajectory.cpp index 6aa016b26e276..941f0eba8da97 100644 --- a/common/autoware_utils/test/src/trajectory/test_trajectory.cpp +++ b/common/tier4_autoware_utils/test/src/trajectory/test_trajectory.cpp @@ -12,9 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware_utils/geometry/boost_geometry.hpp" -#include "autoware_utils/trajectory/tmp_conversion.hpp" -#include "autoware_utils/trajectory/trajectory.hpp" +#include "tier4_autoware_utils/geometry/boost_geometry.hpp" +#include "tier4_autoware_utils/trajectory/tmp_conversion.hpp" +#include "tier4_autoware_utils/trajectory/trajectory.hpp" #include #include @@ -26,9 +26,9 @@ namespace { using autoware_auto_planning_msgs::msg::Trajectory; using TrajectoryPointArray = std::vector; -using autoware_utils::createPoint; -using autoware_utils::createQuaternionFromRPY; -using autoware_utils::transformPoint; +using tier4_autoware_utils::createPoint; +using tier4_autoware_utils::createQuaternionFromRPY; +using tier4_autoware_utils::transformPoint; constexpr double epsilon = 1e-6; @@ -92,7 +92,7 @@ void updateTrajectoryVelocityAt(T & points, const size_t idx, const double vel) TEST(trajectory, validateNonEmpty) { - using autoware_utils::validateNonEmpty; + using tier4_autoware_utils::validateNonEmpty; // Empty EXPECT_THROW(validateNonEmpty(Trajectory{}.points), std::invalid_argument); @@ -104,7 +104,7 @@ TEST(trajectory, validateNonEmpty) TEST(trajectory, searchZeroVelocityIndex) { - using autoware_utils::searchZeroVelocityIndex; + using tier4_autoware_utils::searchZeroVelocityIndex; // Empty EXPECT_THROW(searchZeroVelocityIndex(Trajectory{}.points), std::invalid_argument); @@ -183,7 +183,7 @@ TEST(trajectory, searchZeroVelocityIndex) TEST(trajectory, findNearestIndex_Pos_StraightTrajectory) { - using autoware_utils::findNearestIndex; + using tier4_autoware_utils::findNearestIndex; const auto traj = generateTestTrajectory(10, 1.0); @@ -214,7 +214,7 @@ TEST(trajectory, findNearestIndex_Pos_StraightTrajectory) TEST(trajectory, findNearestIndex_Pos_CurvedTrajectory) { - using autoware_utils::findNearestIndex; + using tier4_autoware_utils::findNearestIndex; const auto traj = generateTestTrajectory(10, 1.0, 0.0, 0.0, 0.1); @@ -224,7 +224,7 @@ TEST(trajectory, findNearestIndex_Pos_CurvedTrajectory) TEST(trajectory, findNearestIndex_Pose_NoThreshold) { - using autoware_utils::findNearestIndex; + using tier4_autoware_utils::findNearestIndex; const auto traj = generateTestTrajectory(10, 1.0); @@ -251,7 +251,7 @@ TEST(trajectory, findNearestIndex_Pose_NoThreshold) TEST(trajectory, findNearestIndex_Pose_DistThreshold) { - using autoware_utils::findNearestIndex; + using tier4_autoware_utils::findNearestIndex; const auto traj = generateTestTrajectory(10, 1.0); @@ -267,7 +267,7 @@ TEST(trajectory, findNearestIndex_Pose_DistThreshold) TEST(trajectory, findNearestIndex_Pose_YawThreshold) { - using autoware_utils::findNearestIndex; + using tier4_autoware_utils::findNearestIndex; const auto traj = generateTestTrajectory(10, 1.0); const auto max_d = std::numeric_limits::max(); @@ -286,7 +286,7 @@ TEST(trajectory, findNearestIndex_Pose_YawThreshold) TEST(trajectory, findNearestIndex_Pose_DistAndYawThreshold) { - using autoware_utils::findNearestIndex; + using tier4_autoware_utils::findNearestIndex; const auto traj = generateTestTrajectory(10, 1.0); @@ -300,7 +300,7 @@ TEST(trajectory, findNearestIndex_Pose_DistAndYawThreshold) TEST(trajectory, findNearestSegmentIndex) { - using autoware_utils::findNearestSegmentIndex; + using tier4_autoware_utils::findNearestSegmentIndex; const auto traj = generateTestTrajectory(10, 1.0); @@ -340,7 +340,7 @@ TEST(trajectory, findNearestSegmentIndex) TEST(trajectory, calcLongitudinalOffsetToSegment_StraightTrajectory) { - using autoware_utils::calcLongitudinalOffsetToSegment; + using tier4_autoware_utils::calcLongitudinalOffsetToSegment; const auto traj = generateTestTrajectory(10, 1.0); @@ -386,7 +386,7 @@ TEST(trajectory, calcLongitudinalOffsetToSegment_StraightTrajectory) TEST(trajectory, calcLongitudinalOffsetToSegment_CurveTrajectory) { - using autoware_utils::calcLongitudinalOffsetToSegment; + using tier4_autoware_utils::calcLongitudinalOffsetToSegment; const auto traj = generateTestTrajectory(10, 1.0, 0.0, 0.0, 0.1); @@ -398,7 +398,7 @@ TEST(trajectory, calcLongitudinalOffsetToSegment_CurveTrajectory) TEST(trajectory, calcLateralOffset) { - using autoware_utils::calcLateralOffset; + using tier4_autoware_utils::calcLateralOffset; const auto traj = generateTestTrajectory(10, 1.0); @@ -436,7 +436,7 @@ TEST(trajectory, calcLateralOffset) TEST(trajectory, calcLateralOffset_CurveTrajectory) { - using autoware_utils::calcLateralOffset; + using tier4_autoware_utils::calcLateralOffset; const auto traj = generateTestTrajectory(10, 1.0, 0.0, 0.0, 0.1); @@ -447,7 +447,7 @@ TEST(trajectory, calcLateralOffset_CurveTrajectory) TEST(trajectory, calcSignedArcLengthFromIndexToIndex) { - using autoware_utils::calcSignedArcLength; + using tier4_autoware_utils::calcSignedArcLength; const auto traj = generateTestTrajectory(10, 1.0); @@ -470,7 +470,7 @@ TEST(trajectory, calcSignedArcLengthFromIndexToIndex) TEST(trajectory, calcSignedArcLengthFromPointToIndex) { - using autoware_utils::calcSignedArcLength; + using tier4_autoware_utils::calcSignedArcLength; const auto traj = generateTestTrajectory(10, 1.0); @@ -499,7 +499,7 @@ TEST(trajectory, calcSignedArcLengthFromPointToIndex) TEST(trajectory, calcSignedArcLengthFromIndexToPoint) { - using autoware_utils::calcSignedArcLength; + using tier4_autoware_utils::calcSignedArcLength; const auto traj = generateTestTrajectory(10, 1.0); @@ -528,7 +528,7 @@ TEST(trajectory, calcSignedArcLengthFromIndexToPoint) TEST(trajectory, calcSignedArcLengthFromPointToPoint) { - using autoware_utils::calcSignedArcLength; + using tier4_autoware_utils::calcSignedArcLength; const auto traj = generateTestTrajectory(10, 1.0); @@ -591,7 +591,7 @@ TEST(trajectory, calcSignedArcLengthFromPointToPoint) TEST(trajectory, calcArcLength) { - using autoware_utils::calcArcLength; + using tier4_autoware_utils::calcArcLength; const auto traj = generateTestTrajectory(10, 1.0); @@ -604,7 +604,7 @@ TEST(trajectory, calcArcLength) TEST(trajectory, convertToTrajectory) { - using autoware_utils::convertToTrajectory; + using tier4_autoware_utils::convertToTrajectory; // Size check { @@ -627,7 +627,7 @@ TEST(trajectory, convertToTrajectory) TEST(trajectory, convertToTrajectoryPointArray) { - using autoware_utils::convertToTrajectoryPointArray; + using tier4_autoware_utils::convertToTrajectoryPointArray; const auto traj_input = generateTestTrajectory(100, 1.0); const auto traj = convertToTrajectoryPointArray(traj_input); diff --git a/common/autoware_datetime_rviz_plugin/CMakeLists.txt b/common/tier4_datetime_rviz_plugin/CMakeLists.txt similarity index 96% rename from common/autoware_datetime_rviz_plugin/CMakeLists.txt rename to common/tier4_datetime_rviz_plugin/CMakeLists.txt index 2d9168b0f1843..b35caa4d5a2cb 100644 --- a/common/autoware_datetime_rviz_plugin/CMakeLists.txt +++ b/common/tier4_datetime_rviz_plugin/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.5) -project(autoware_datetime_rviz_plugin) +project(tier4_datetime_rviz_plugin) if(NOT CMAKE_CXX_STANDARD) set(CMAKE_CXX_STANDARD 17) diff --git a/common/autoware_datetime_rviz_plugin/README.md b/common/tier4_datetime_rviz_plugin/README.md similarity index 72% rename from common/autoware_datetime_rviz_plugin/README.md rename to common/tier4_datetime_rviz_plugin/README.md index b8e6bf77a75b4..b483155b342eb 100644 --- a/common/autoware_datetime_rviz_plugin/README.md +++ b/common/tier4_datetime_rviz_plugin/README.md @@ -1,4 +1,4 @@ -# autoware_datetime_rviz_plugin +# tier4_datetime_rviz_plugin ## Purpose @@ -12,5 +12,5 @@ TBD. 1. Start rviz and select panels/Add new panel. ![select_panel](./images/select_panels.png) -2. Select autoware_datetime_rviz_plugin/AutowareDateTimePanel and press OK. +2. Select tier4_datetime_rviz_plugin/AutowareDateTimePanel and press OK. ![select_datetime_plugin](./images/select_datetime_plugin.png) diff --git a/common/autoware_datetime_rviz_plugin/icons/classes/AutowareDateTimePanel.png b/common/tier4_datetime_rviz_plugin/icons/classes/AutowareDateTimePanel.png similarity index 100% rename from common/autoware_datetime_rviz_plugin/icons/classes/AutowareDateTimePanel.png rename to common/tier4_datetime_rviz_plugin/icons/classes/AutowareDateTimePanel.png diff --git a/common/autoware_datetime_rviz_plugin/images/select_datetime_plugin.png b/common/tier4_datetime_rviz_plugin/images/select_datetime_plugin.png similarity index 100% rename from common/autoware_datetime_rviz_plugin/images/select_datetime_plugin.png rename to common/tier4_datetime_rviz_plugin/images/select_datetime_plugin.png diff --git a/common/autoware_datetime_rviz_plugin/images/select_panels.png b/common/tier4_datetime_rviz_plugin/images/select_panels.png similarity index 100% rename from common/autoware_datetime_rviz_plugin/images/select_panels.png rename to common/tier4_datetime_rviz_plugin/images/select_panels.png diff --git a/common/autoware_datetime_rviz_plugin/package.xml b/common/tier4_datetime_rviz_plugin/package.xml similarity index 84% rename from common/autoware_datetime_rviz_plugin/package.xml rename to common/tier4_datetime_rviz_plugin/package.xml index 747e7a0204553..853b22b49be10 100644 --- a/common/autoware_datetime_rviz_plugin/package.xml +++ b/common/tier4_datetime_rviz_plugin/package.xml @@ -1,9 +1,9 @@ - autoware_datetime_rviz_plugin + tier4_datetime_rviz_plugin 0.0.0 - The autoware_datetime_rviz_plugin package + The tier4_datetime_rviz_plugin package Takagi, Isamu Apache License 2.0 diff --git a/common/autoware_datetime_rviz_plugin/plugins/plugin_description.xml b/common/tier4_datetime_rviz_plugin/plugins/plugin_description.xml similarity index 77% rename from common/autoware_datetime_rviz_plugin/plugins/plugin_description.xml rename to common/tier4_datetime_rviz_plugin/plugins/plugin_description.xml index 98ed9bb7b0242..ab796eaa90bf3 100644 --- a/common/autoware_datetime_rviz_plugin/plugins/plugin_description.xml +++ b/common/tier4_datetime_rviz_plugin/plugins/plugin_description.xml @@ -1,4 +1,4 @@ - + #include #include #include +#include #include #include @@ -71,4 +71,4 @@ class LateralErrorPublisher : public rclcpp::Node void onGroundTruthPose(const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg); }; -#endif // AUTOWARE_DEBUG_TOOLS__LATERAL_ERROR_PUBLISHER_HPP_ +#endif // TIER4_DEBUG_TOOLS__LATERAL_ERROR_PUBLISHER_HPP_ diff --git a/common/autoware_debug_tools/launch/lateral_error_publisher.launch.xml b/common/tier4_debug_tools/launch/lateral_error_publisher.launch.xml similarity index 71% rename from common/autoware_debug_tools/launch/lateral_error_publisher.launch.xml rename to common/tier4_debug_tools/launch/lateral_error_publisher.launch.xml index c4732e239c9ab..f2f0f6e33d633 100644 --- a/common/autoware_debug_tools/launch/lateral_error_publisher.launch.xml +++ b/common/tier4_debug_tools/launch/lateral_error_publisher.launch.xml @@ -1,9 +1,9 @@ - + - + diff --git a/common/autoware_debug_tools/media/lateral_error_publisher.svg b/common/tier4_debug_tools/media/lateral_error_publisher.svg similarity index 100% rename from common/autoware_debug_tools/media/lateral_error_publisher.svg rename to common/tier4_debug_tools/media/lateral_error_publisher.svg diff --git a/common/autoware_debug_tools/package.xml b/common/tier4_debug_tools/package.xml similarity index 85% rename from common/autoware_debug_tools/package.xml rename to common/tier4_debug_tools/package.xml index 0ceb00a24145b..fa447301d36c3 100644 --- a/common/autoware_debug_tools/package.xml +++ b/common/tier4_debug_tools/package.xml @@ -1,19 +1,19 @@ - autoware_debug_tools + tier4_debug_tools 0.1.0 - The autoware_debug_tools package + The tier4_debug_tools package Kenji Miyake Apache License 2.0 ament_cmake_auto autoware_auto_planning_msgs - autoware_utils geometry_msgs rclcpp rclcpp_components tf2_ros + tier4_autoware_utils tier4_debug_msgs launch_ros diff --git a/common/autoware_debug_tools/scripts/case_converter.py b/common/tier4_debug_tools/scripts/case_converter.py similarity index 100% rename from common/autoware_debug_tools/scripts/case_converter.py rename to common/tier4_debug_tools/scripts/case_converter.py diff --git a/common/autoware_debug_tools/scripts/pose2tf.py b/common/tier4_debug_tools/scripts/pose2tf.py similarity index 100% rename from common/autoware_debug_tools/scripts/pose2tf.py rename to common/tier4_debug_tools/scripts/pose2tf.py diff --git a/common/autoware_debug_tools/scripts/self_pose_listener.py b/common/tier4_debug_tools/scripts/self_pose_listener.py similarity index 100% rename from common/autoware_debug_tools/scripts/self_pose_listener.py rename to common/tier4_debug_tools/scripts/self_pose_listener.py diff --git a/common/autoware_debug_tools/scripts/stop_reason2pose.py b/common/tier4_debug_tools/scripts/stop_reason2pose.py similarity index 97% rename from common/autoware_debug_tools/scripts/stop_reason2pose.py rename to common/tier4_debug_tools/scripts/stop_reason2pose.py index ab0bb3c9e8866..67a6809dc21a8 100755 --- a/common/autoware_debug_tools/scripts/stop_reason2pose.py +++ b/common/tier4_debug_tools/scripts/stop_reason2pose.py @@ -66,7 +66,7 @@ def _on_stop_reasons(self, msg): pose_id = self._register_pose(snake_case_stop_reason, pose.pose) pose_topic_name = "{snake_case_stop_reason}_{pose_id}".format(**locals()) - topic_ns = "/autoware_debug_tools/stop_reason2pose/" + topic_ns = "/tier4_debug_tools/stop_reason2pose/" if pose_topic_name not in self._pub_pose_map: self._pub_pose_map[pose_topic_name] = self.create_publisher( PoseStamped, topic_ns + pose_topic_name, 1 @@ -82,7 +82,7 @@ def _on_stop_reasons(self, msg): if nearest_pose.pose: if snake_case_stop_reason not in self._pub_pose_map: - topic_ns = "/autoware_debug_tools/stop_reason2pose/" + topic_ns = "/tier4_debug_tools/stop_reason2pose/" self._pub_pose_map[snake_case_stop_reason] = self.create_publisher( PoseStamped, topic_ns + snake_case_stop_reason, 1 ) diff --git a/common/tier4_debug_tools/scripts/stop_reason2tf b/common/tier4_debug_tools/scripts/stop_reason2tf new file mode 100755 index 0000000000000..60d5956130869 --- /dev/null +++ b/common/tier4_debug_tools/scripts/stop_reason2tf @@ -0,0 +1,15 @@ +#!/usr/bin/env bash + +stop_reason_name="$1" + +if [ -z "${stop_reason_name}" ]; then + echo "Please input stop_reason_name as the 1st argument." + exit 1 +fi + +ros2 run tier4_debug_tools stop_reason2pose.py /planning/scenario_planning/status/stop_reasons >/dev/null 2>&1 & +ros2 run tier4_debug_tools pose2tf.py /tier4_debug_tools/stop_reason2pose/"${stop_reason_name}" "${stop_reason_name}" >/dev/null 2>&1 & +ros2 run tier4_debug_tools tf2pose.py "${stop_reason_name}" base_link 100 >/dev/null 2>&1 & +ros2 run tf2_ros tf2_echo "${stop_reason_name}" base_link 100 2>/dev/null + +wait diff --git a/common/autoware_debug_tools/scripts/tf2pose.py b/common/tier4_debug_tools/scripts/tf2pose.py similarity index 96% rename from common/autoware_debug_tools/scripts/tf2pose.py rename to common/tier4_debug_tools/scripts/tf2pose.py index 6bee0348ca811..a4422fdd8015b 100755 --- a/common/autoware_debug_tools/scripts/tf2pose.py +++ b/common/tier4_debug_tools/scripts/tf2pose.py @@ -32,7 +32,7 @@ def __init__(self, options): self._options = options self.tf_buffer = Buffer() self.tf_listener = TransformListener(self.tf_buffer, self) - self._pub_pose = self.create_publisher(PoseStamped, "/autoware_debug_tools/tf2pose/pose", 1) + self._pub_pose = self.create_publisher(PoseStamped, "/tier4_debug_tools/tf2pose/pose", 1) self.timer = self.create_timer((1.0 / self._options.hz), self._on_timer) def _on_timer(self): diff --git a/common/autoware_debug_tools/src/lateral_error_publisher.cpp b/common/tier4_debug_tools/src/lateral_error_publisher.cpp similarity index 98% rename from common/autoware_debug_tools/src/lateral_error_publisher.cpp rename to common/tier4_debug_tools/src/lateral_error_publisher.cpp index 06f9de1bb8b66..3d7a0de7e6946 100644 --- a/common/autoware_debug_tools/src/lateral_error_publisher.cpp +++ b/common/tier4_debug_tools/src/lateral_error_publisher.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware_debug_tools/lateral_error_publisher.hpp" +#include "tier4_debug_tools/lateral_error_publisher.hpp" #include @@ -74,7 +74,7 @@ void LateralErrorPublisher::onGroundTruthPose( } // Search closest trajectory point with vehicle pose - const auto closest_index = autoware_utils::findNearestIndex( + const auto closest_index = tier4_autoware_utils::findNearestIndex( current_trajectory_ptr_->points, current_vehicle_pose_ptr_->pose.pose, std::numeric_limits::max(), yaw_threshold_to_search_closest_); if (!closest_index) { diff --git a/common/autoware_localization_rviz_plugin/CMakeLists.txt b/common/tier4_localization_rviz_plugin/CMakeLists.txt similarity index 96% rename from common/autoware_localization_rviz_plugin/CMakeLists.txt rename to common/tier4_localization_rviz_plugin/CMakeLists.txt index 0b1854c34d7d3..eb3b1605c7ca6 100644 --- a/common/autoware_localization_rviz_plugin/CMakeLists.txt +++ b/common/tier4_localization_rviz_plugin/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.5) -project(autoware_localization_rviz_plugin) +project(tier4_localization_rviz_plugin) if(NOT CMAKE_CXX_STANDARD) set(CMAKE_CXX_STANDARD 14) diff --git a/common/autoware_localization_rviz_plugin/README.md b/common/tier4_localization_rviz_plugin/README.md similarity index 93% rename from common/autoware_localization_rviz_plugin/README.md rename to common/tier4_localization_rviz_plugin/README.md index d26c0e71a5b82..60a923f15b563 100644 --- a/common/autoware_localization_rviz_plugin/README.md +++ b/common/tier4_localization_rviz_plugin/README.md @@ -1,4 +1,4 @@ -# autoware_localization_rviz_plugin +# tier4_localization_rviz_plugin ## Purpose @@ -32,7 +32,7 @@ TBD. 1. Start rviz and select Add under the Displays panel. ![select_add](./images/select_add.png) -2. Select autoware_localization_rviz_plugin/PoseHistory and press OK. +2. Select tier4_localization_rviz_plugin/PoseHistory and press OK. ![select_localization_plugin](./images/select_localization_plugin.png) 3. Enter the name of the topic where you want to view the trajectory. ![select_topic_name](./images/select_topic_name.png) diff --git a/common/autoware_localization_rviz_plugin/icons/classes/PoseHistory.png b/common/tier4_localization_rviz_plugin/icons/classes/PoseHistory.png similarity index 100% rename from common/autoware_localization_rviz_plugin/icons/classes/PoseHistory.png rename to common/tier4_localization_rviz_plugin/icons/classes/PoseHistory.png diff --git a/common/autoware_localization_rviz_plugin/images/select_add.png b/common/tier4_localization_rviz_plugin/images/select_add.png similarity index 100% rename from common/autoware_localization_rviz_plugin/images/select_add.png rename to common/tier4_localization_rviz_plugin/images/select_add.png diff --git a/common/autoware_localization_rviz_plugin/images/select_localization_plugin.png b/common/tier4_localization_rviz_plugin/images/select_localization_plugin.png similarity index 100% rename from common/autoware_localization_rviz_plugin/images/select_localization_plugin.png rename to common/tier4_localization_rviz_plugin/images/select_localization_plugin.png diff --git a/common/autoware_localization_rviz_plugin/images/select_topic_name.png b/common/tier4_localization_rviz_plugin/images/select_topic_name.png similarity index 100% rename from common/autoware_localization_rviz_plugin/images/select_topic_name.png rename to common/tier4_localization_rviz_plugin/images/select_topic_name.png diff --git a/common/autoware_localization_rviz_plugin/package.xml b/common/tier4_localization_rviz_plugin/package.xml similarity index 85% rename from common/autoware_localization_rviz_plugin/package.xml rename to common/tier4_localization_rviz_plugin/package.xml index e3283062da819..7d054c46c5083 100644 --- a/common/autoware_localization_rviz_plugin/package.xml +++ b/common/tier4_localization_rviz_plugin/package.xml @@ -1,9 +1,9 @@ - autoware_localization_rviz_plugin + tier4_localization_rviz_plugin 0.1.0 - The autoware_localization_rviz_plugin package + The tier4_localization_rviz_plugin package Apache License 2.0 Isamu Takagi diff --git a/common/autoware_localization_rviz_plugin/plugins/plugin_description.xml b/common/tier4_localization_rviz_plugin/plugins/plugin_description.xml similarity index 76% rename from common/autoware_localization_rviz_plugin/plugins/plugin_description.xml rename to common/tier4_localization_rviz_plugin/plugins/plugin_description.xml index c06af3364eb2e..80c5abfdab753 100644 --- a/common/autoware_localization_rviz_plugin/plugins/plugin_description.xml +++ b/common/tier4_localization_rviz_plugin/plugins/plugin_description.xml @@ -1,4 +1,4 @@ - + - autoware_perception_rviz_plugin + tier4_perception_rviz_plugin 0.1.0 - The autoware_perception_rviz_plugin package + The tier4_perception_rviz_plugin package Yukihiro Saito Apache License 2.0 diff --git a/common/autoware_perception_rviz_plugin/plugins/plugin_description.xml b/common/tier4_perception_rviz_plugin/plugins/plugin_description.xml similarity index 93% rename from common/autoware_perception_rviz_plugin/plugins/plugin_description.xml rename to common/tier4_perception_rviz_plugin/plugins/plugin_description.xml index fcbbd27f65eb6..5ab5eaa461eba 100644 --- a/common/autoware_perception_rviz_plugin/plugins/plugin_description.xml +++ b/common/tier4_perception_rviz_plugin/plugins/plugin_description.xml @@ -1,4 +1,4 @@ - + diff --git a/common/autoware_perception_rviz_plugin/src/tools/car_pose.cpp b/common/tier4_perception_rviz_plugin/src/tools/car_pose.cpp similarity index 100% rename from common/autoware_perception_rviz_plugin/src/tools/car_pose.cpp rename to common/tier4_perception_rviz_plugin/src/tools/car_pose.cpp diff --git a/common/autoware_perception_rviz_plugin/src/tools/car_pose.hpp b/common/tier4_perception_rviz_plugin/src/tools/car_pose.hpp similarity index 100% rename from common/autoware_perception_rviz_plugin/src/tools/car_pose.hpp rename to common/tier4_perception_rviz_plugin/src/tools/car_pose.hpp diff --git a/common/autoware_perception_rviz_plugin/src/tools/delete_all_objects.cpp b/common/tier4_perception_rviz_plugin/src/tools/delete_all_objects.cpp similarity index 100% rename from common/autoware_perception_rviz_plugin/src/tools/delete_all_objects.cpp rename to common/tier4_perception_rviz_plugin/src/tools/delete_all_objects.cpp diff --git a/common/autoware_perception_rviz_plugin/src/tools/delete_all_objects.hpp b/common/tier4_perception_rviz_plugin/src/tools/delete_all_objects.hpp similarity index 100% rename from common/autoware_perception_rviz_plugin/src/tools/delete_all_objects.hpp rename to common/tier4_perception_rviz_plugin/src/tools/delete_all_objects.hpp diff --git a/common/autoware_perception_rviz_plugin/src/tools/pedestrian_pose.cpp b/common/tier4_perception_rviz_plugin/src/tools/pedestrian_pose.cpp similarity index 100% rename from common/autoware_perception_rviz_plugin/src/tools/pedestrian_pose.cpp rename to common/tier4_perception_rviz_plugin/src/tools/pedestrian_pose.cpp diff --git a/common/autoware_perception_rviz_plugin/src/tools/pedestrian_pose.hpp b/common/tier4_perception_rviz_plugin/src/tools/pedestrian_pose.hpp similarity index 100% rename from common/autoware_perception_rviz_plugin/src/tools/pedestrian_pose.hpp rename to common/tier4_perception_rviz_plugin/src/tools/pedestrian_pose.hpp diff --git a/common/autoware_perception_rviz_plugin/src/tools/unknown_pose.cpp b/common/tier4_perception_rviz_plugin/src/tools/unknown_pose.cpp similarity index 100% rename from common/autoware_perception_rviz_plugin/src/tools/unknown_pose.cpp rename to common/tier4_perception_rviz_plugin/src/tools/unknown_pose.cpp diff --git a/common/autoware_perception_rviz_plugin/src/tools/unknown_pose.hpp b/common/tier4_perception_rviz_plugin/src/tools/unknown_pose.hpp similarity index 100% rename from common/autoware_perception_rviz_plugin/src/tools/unknown_pose.hpp rename to common/tier4_perception_rviz_plugin/src/tools/unknown_pose.hpp diff --git a/common/autoware_planning_rviz_plugin/CMakeLists.txt b/common/tier4_planning_rviz_plugin/CMakeLists.txt similarity index 89% rename from common/autoware_planning_rviz_plugin/CMakeLists.txt rename to common/tier4_planning_rviz_plugin/CMakeLists.txt index 73d932ae94336..b0b1ff34f48d1 100644 --- a/common/autoware_planning_rviz_plugin/CMakeLists.txt +++ b/common/tier4_planning_rviz_plugin/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.5) -project(autoware_planning_rviz_plugin) +project(tier4_plannnig_rviz_plugin) if(NOT CMAKE_CXX_STANDARD) set(CMAKE_CXX_STANDARD 14) @@ -20,7 +20,7 @@ set(CMAKE_INCLUDE_CURRENT_DIR ON) add_definitions(-DQT_NO_KEYWORDS) find_package(Eigen3 REQUIRED) -ament_auto_add_library(autoware_planning_rviz_plugin SHARED +ament_auto_add_library(tier4_plannnig_rviz_plugin SHARED include/path/display.hpp src/path/display.cpp include/path_footprint/display.hpp @@ -35,7 +35,7 @@ ament_auto_add_library(autoware_planning_rviz_plugin SHARED src/tools/max_velocity.cpp ) -target_link_libraries(autoware_planning_rviz_plugin +target_link_libraries(tier4_plannnig_rviz_plugin ${QT_LIBRARIES} ) diff --git a/common/autoware_planning_rviz_plugin/README.md b/common/tier4_planning_rviz_plugin/README.md similarity index 98% rename from common/autoware_planning_rviz_plugin/README.md rename to common/tier4_planning_rviz_plugin/README.md index 10087d435f864..96d2c27438183 100644 --- a/common/autoware_planning_rviz_plugin/README.md +++ b/common/tier4_planning_rviz_plugin/README.md @@ -1,4 +1,4 @@ -# autoware_planning_rviz_plugin +# tier4_plannnig_rviz_plugin This package is including jsk code. Note that jsk_overlay_utils.cpp and jsk_overlay_utils.hpp are BSD license. @@ -112,7 +112,7 @@ This plugin displays the path, trajectory, and maximum speed. 1. Start rviz and select Add under the Displays panel. ![select_add](./images/select_add.png) -2. Select any one of the autoware_planning_rviz_plugin and press OK. +2. Select any one of the tier4_plannnig_rviz_plugin and press OK. ![select_planning_plugin](./images/select_planning_plugin.png) 3. Enter the name of the topic where you want to view the path or trajectory. ![select_topic_name](./images/select_topic_name.png) diff --git a/common/autoware_planning_rviz_plugin/icons/classes/MaxVelocity.png b/common/tier4_planning_rviz_plugin/icons/classes/MaxVelocity.png similarity index 100% rename from common/autoware_planning_rviz_plugin/icons/classes/MaxVelocity.png rename to common/tier4_planning_rviz_plugin/icons/classes/MaxVelocity.png diff --git a/common/autoware_planning_rviz_plugin/icons/classes/MissionCheckpointTool.png b/common/tier4_planning_rviz_plugin/icons/classes/MissionCheckpointTool.png similarity index 100% rename from common/autoware_planning_rviz_plugin/icons/classes/MissionCheckpointTool.png rename to common/tier4_planning_rviz_plugin/icons/classes/MissionCheckpointTool.png diff --git a/common/autoware_planning_rviz_plugin/icons/classes/Path.png b/common/tier4_planning_rviz_plugin/icons/classes/Path.png similarity index 100% rename from common/autoware_planning_rviz_plugin/icons/classes/Path.png rename to common/tier4_planning_rviz_plugin/icons/classes/Path.png diff --git a/common/autoware_planning_rviz_plugin/icons/classes/PathFootprint.png b/common/tier4_planning_rviz_plugin/icons/classes/PathFootprint.png similarity index 100% rename from common/autoware_planning_rviz_plugin/icons/classes/PathFootprint.png rename to common/tier4_planning_rviz_plugin/icons/classes/PathFootprint.png diff --git a/common/autoware_planning_rviz_plugin/icons/classes/Trajectory.png b/common/tier4_planning_rviz_plugin/icons/classes/Trajectory.png similarity index 100% rename from common/autoware_planning_rviz_plugin/icons/classes/Trajectory.png rename to common/tier4_planning_rviz_plugin/icons/classes/Trajectory.png diff --git a/common/autoware_planning_rviz_plugin/icons/classes/TrajectoryFootprint.png b/common/tier4_planning_rviz_plugin/icons/classes/TrajectoryFootprint.png similarity index 100% rename from common/autoware_planning_rviz_plugin/icons/classes/TrajectoryFootprint.png rename to common/tier4_planning_rviz_plugin/icons/classes/TrajectoryFootprint.png diff --git a/common/autoware_planning_rviz_plugin/images/select_add.png b/common/tier4_planning_rviz_plugin/images/select_add.png similarity index 100% rename from common/autoware_planning_rviz_plugin/images/select_add.png rename to common/tier4_planning_rviz_plugin/images/select_add.png diff --git a/common/autoware_planning_rviz_plugin/images/select_planning_plugin.png b/common/tier4_planning_rviz_plugin/images/select_planning_plugin.png similarity index 100% rename from common/autoware_planning_rviz_plugin/images/select_planning_plugin.png rename to common/tier4_planning_rviz_plugin/images/select_planning_plugin.png diff --git a/common/autoware_planning_rviz_plugin/images/select_topic_name.png b/common/tier4_planning_rviz_plugin/images/select_topic_name.png similarity index 100% rename from common/autoware_planning_rviz_plugin/images/select_topic_name.png rename to common/tier4_planning_rviz_plugin/images/select_topic_name.png diff --git a/common/autoware_planning_rviz_plugin/include/mission_checkpoint/mission_checkpoint.hpp b/common/tier4_planning_rviz_plugin/include/mission_checkpoint/mission_checkpoint.hpp similarity index 100% rename from common/autoware_planning_rviz_plugin/include/mission_checkpoint/mission_checkpoint.hpp rename to common/tier4_planning_rviz_plugin/include/mission_checkpoint/mission_checkpoint.hpp diff --git a/common/autoware_planning_rviz_plugin/include/path/display.hpp b/common/tier4_planning_rviz_plugin/include/path/display.hpp similarity index 100% rename from common/autoware_planning_rviz_plugin/include/path/display.hpp rename to common/tier4_planning_rviz_plugin/include/path/display.hpp diff --git a/common/autoware_planning_rviz_plugin/include/path_footprint/display.hpp b/common/tier4_planning_rviz_plugin/include/path_footprint/display.hpp similarity index 100% rename from common/autoware_planning_rviz_plugin/include/path_footprint/display.hpp rename to common/tier4_planning_rviz_plugin/include/path_footprint/display.hpp diff --git a/common/autoware_planning_rviz_plugin/include/trajectory/display.hpp b/common/tier4_planning_rviz_plugin/include/trajectory/display.hpp similarity index 100% rename from common/autoware_planning_rviz_plugin/include/trajectory/display.hpp rename to common/tier4_planning_rviz_plugin/include/trajectory/display.hpp diff --git a/common/autoware_planning_rviz_plugin/include/trajectory_footprint/display.hpp b/common/tier4_planning_rviz_plugin/include/trajectory_footprint/display.hpp similarity index 100% rename from common/autoware_planning_rviz_plugin/include/trajectory_footprint/display.hpp rename to common/tier4_planning_rviz_plugin/include/trajectory_footprint/display.hpp diff --git a/common/autoware_planning_rviz_plugin/package.xml b/common/tier4_planning_rviz_plugin/package.xml similarity index 88% rename from common/autoware_planning_rviz_plugin/package.xml rename to common/tier4_planning_rviz_plugin/package.xml index 30556658555dd..abc15751f35ca 100644 --- a/common/autoware_planning_rviz_plugin/package.xml +++ b/common/tier4_planning_rviz_plugin/package.xml @@ -1,9 +1,9 @@ - autoware_planning_rviz_plugin + tier4_plannnig_rviz_plugin 0.1.0 - The autoware_planning_rviz_plugin package + The tier4_plannnig_rviz_plugin package Yukihiro Saito Apache License 2.0 diff --git a/common/autoware_planning_rviz_plugin/plugins/plugin_description.xml b/common/tier4_planning_rviz_plugin/plugins/plugin_description.xml similarity index 96% rename from common/autoware_planning_rviz_plugin/plugins/plugin_description.xml rename to common/tier4_planning_rviz_plugin/plugins/plugin_description.xml index 2e18834824c76..0250012e7a25c 100644 --- a/common/autoware_planning_rviz_plugin/plugins/plugin_description.xml +++ b/common/tier4_planning_rviz_plugin/plugins/plugin_description.xml @@ -1,4 +1,4 @@ - + diff --git a/common/autoware_planning_rviz_plugin/src/mission_checkpoint/mission_checkpoint.cpp b/common/tier4_planning_rviz_plugin/src/mission_checkpoint/mission_checkpoint.cpp similarity index 100% rename from common/autoware_planning_rviz_plugin/src/mission_checkpoint/mission_checkpoint.cpp rename to common/tier4_planning_rviz_plugin/src/mission_checkpoint/mission_checkpoint.cpp diff --git a/common/autoware_planning_rviz_plugin/src/path/display.cpp b/common/tier4_planning_rviz_plugin/src/path/display.cpp similarity index 100% rename from common/autoware_planning_rviz_plugin/src/path/display.cpp rename to common/tier4_planning_rviz_plugin/src/path/display.cpp diff --git a/common/autoware_planning_rviz_plugin/src/path_footprint/display.cpp b/common/tier4_planning_rviz_plugin/src/path_footprint/display.cpp similarity index 100% rename from common/autoware_planning_rviz_plugin/src/path_footprint/display.cpp rename to common/tier4_planning_rviz_plugin/src/path_footprint/display.cpp diff --git a/common/autoware_planning_rviz_plugin/src/tools/jsk_overlay_utils.cpp b/common/tier4_planning_rviz_plugin/src/tools/jsk_overlay_utils.cpp similarity index 100% rename from common/autoware_planning_rviz_plugin/src/tools/jsk_overlay_utils.cpp rename to common/tier4_planning_rviz_plugin/src/tools/jsk_overlay_utils.cpp diff --git a/common/autoware_planning_rviz_plugin/src/tools/jsk_overlay_utils.hpp b/common/tier4_planning_rviz_plugin/src/tools/jsk_overlay_utils.hpp similarity index 100% rename from common/autoware_planning_rviz_plugin/src/tools/jsk_overlay_utils.hpp rename to common/tier4_planning_rviz_plugin/src/tools/jsk_overlay_utils.hpp diff --git a/common/autoware_planning_rviz_plugin/src/tools/max_velocity.cpp b/common/tier4_planning_rviz_plugin/src/tools/max_velocity.cpp similarity index 100% rename from common/autoware_planning_rviz_plugin/src/tools/max_velocity.cpp rename to common/tier4_planning_rviz_plugin/src/tools/max_velocity.cpp diff --git a/common/autoware_planning_rviz_plugin/src/tools/max_velocity.hpp b/common/tier4_planning_rviz_plugin/src/tools/max_velocity.hpp similarity index 100% rename from common/autoware_planning_rviz_plugin/src/tools/max_velocity.hpp rename to common/tier4_planning_rviz_plugin/src/tools/max_velocity.hpp diff --git a/common/autoware_planning_rviz_plugin/src/trajectory/display.cpp b/common/tier4_planning_rviz_plugin/src/trajectory/display.cpp similarity index 100% rename from common/autoware_planning_rviz_plugin/src/trajectory/display.cpp rename to common/tier4_planning_rviz_plugin/src/trajectory/display.cpp diff --git a/common/autoware_planning_rviz_plugin/src/trajectory_footprint/display.cpp b/common/tier4_planning_rviz_plugin/src/trajectory_footprint/display.cpp similarity index 100% rename from common/autoware_planning_rviz_plugin/src/trajectory_footprint/display.cpp rename to common/tier4_planning_rviz_plugin/src/trajectory_footprint/display.cpp diff --git a/common/autoware_state_rviz_plugin/CMakeLists.txt b/common/tier4_state_rviz_plugin/CMakeLists.txt similarity index 96% rename from common/autoware_state_rviz_plugin/CMakeLists.txt rename to common/tier4_state_rviz_plugin/CMakeLists.txt index 9098124f01131..a2cf6b29e5cd4 100644 --- a/common/autoware_state_rviz_plugin/CMakeLists.txt +++ b/common/tier4_state_rviz_plugin/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.5) -project(autoware_state_rviz_plugin) +project(tier4_state_rviz_plugin) if(NOT CMAKE_CXX_STANDARD) set(CMAKE_CXX_STANDARD 17) diff --git a/common/autoware_state_rviz_plugin/README.md b/common/tier4_state_rviz_plugin/README.md similarity index 94% rename from common/autoware_state_rviz_plugin/README.md rename to common/tier4_state_rviz_plugin/README.md index 592a88a56cc35..ed96226f6bb75 100644 --- a/common/autoware_state_rviz_plugin/README.md +++ b/common/tier4_state_rviz_plugin/README.md @@ -1,4 +1,4 @@ -# autoware_state_rviz_plugin +# tier4_state_rviz_plugin ## Purpose @@ -26,7 +26,7 @@ This plugin also can engage from the panel. 1. Start rviz and select panels/Add new panel. ![select_panel](./images/select_panels.png) -2. Select autoware_state_rviz_plugin/AutowareStatePanel and press OK. +2. Select tier4_state_rviz_plugin/AutowareStatePanel and press OK. ![select_state_plugin](./images/select_state_plugin.png) 3. If the AutowareState is WaitingForEngage, can engage by clicking the Engage button. ![select_engage](./images/select_engage.png) diff --git a/common/autoware_state_rviz_plugin/icons/classes/AutowareStatePanel.png b/common/tier4_state_rviz_plugin/icons/classes/AutowareStatePanel.png similarity index 100% rename from common/autoware_state_rviz_plugin/icons/classes/AutowareStatePanel.png rename to common/tier4_state_rviz_plugin/icons/classes/AutowareStatePanel.png diff --git a/common/autoware_state_rviz_plugin/images/select_engage.png b/common/tier4_state_rviz_plugin/images/select_engage.png similarity index 100% rename from common/autoware_state_rviz_plugin/images/select_engage.png rename to common/tier4_state_rviz_plugin/images/select_engage.png diff --git a/common/autoware_state_rviz_plugin/images/select_panels.png b/common/tier4_state_rviz_plugin/images/select_panels.png similarity index 100% rename from common/autoware_state_rviz_plugin/images/select_panels.png rename to common/tier4_state_rviz_plugin/images/select_panels.png diff --git a/common/autoware_state_rviz_plugin/images/select_state_plugin.png b/common/tier4_state_rviz_plugin/images/select_state_plugin.png similarity index 100% rename from common/autoware_state_rviz_plugin/images/select_state_plugin.png rename to common/tier4_state_rviz_plugin/images/select_state_plugin.png diff --git a/common/autoware_state_rviz_plugin/package.xml b/common/tier4_state_rviz_plugin/package.xml similarity index 95% rename from common/autoware_state_rviz_plugin/package.xml rename to common/tier4_state_rviz_plugin/package.xml index 237c68b1513a7..e949c0c6a6148 100644 --- a/common/autoware_state_rviz_plugin/package.xml +++ b/common/tier4_state_rviz_plugin/package.xml @@ -1,7 +1,7 @@ - autoware_state_rviz_plugin + tier4_state_rviz_plugin 0.0.0 The autoware state rviz plugin package Hiroki OTA diff --git a/common/autoware_state_rviz_plugin/plugins/plugin_description.xml b/common/tier4_state_rviz_plugin/plugins/plugin_description.xml similarity index 79% rename from common/autoware_state_rviz_plugin/plugins/plugin_description.xml rename to common/tier4_state_rviz_plugin/plugins/plugin_description.xml index df5b5500b30fa..54567c05e96de 100644 --- a/common/autoware_state_rviz_plugin/plugins/plugin_description.xml +++ b/common/tier4_state_rviz_plugin/plugins/plugin_description.xml @@ -1,4 +1,4 @@ - + - autoware_vehicle_rviz_plugin + tier4_vehicle_rviz_plugin 0.1.0 - The autoware_vehicle_rviz_plugin package + The tier4_vehicle_rviz_plugin package Yukihiro Saito Apache License 2.0 ament_cmake_auto autoware_auto_vehicle_msgs - autoware_utils libqt5-core libqt5-gui libqt5-widgets @@ -16,6 +15,7 @@ rviz_common rviz_default_plugins rviz_ogre_vendor + tier4_autoware_utils ament_lint_auto autoware_lint_common diff --git a/common/autoware_vehicle_rviz_plugin/plugins/plugin_description.xml b/common/tier4_vehicle_rviz_plugin/plugins/plugin_description.xml similarity index 93% rename from common/autoware_vehicle_rviz_plugin/plugins/plugin_description.xml rename to common/tier4_vehicle_rviz_plugin/plugins/plugin_description.xml index 1f809c748035b..35431a17d5937 100644 --- a/common/autoware_vehicle_rviz_plugin/plugins/plugin_description.xml +++ b/common/tier4_vehicle_rviz_plugin/plugins/plugin_description.xml @@ -1,4 +1,4 @@ - + diff --git a/common/autoware_vehicle_rviz_plugin/src/tools/console_meter.cpp b/common/tier4_vehicle_rviz_plugin/src/tools/console_meter.cpp similarity index 95% rename from common/autoware_vehicle_rviz_plugin/src/tools/console_meter.cpp rename to common/tier4_vehicle_rviz_plugin/src/tools/console_meter.cpp index 632a74170ee54..2c449c032b98b 100644 --- a/common/autoware_vehicle_rviz_plugin/src/tools/console_meter.cpp +++ b/common/tier4_vehicle_rviz_plugin/src/tools/console_meter.cpp @@ -200,14 +200,14 @@ void ConsoleMeterDisplay::updateVisualization() inner_arc_.y0 = line_width_ / 2.0; inner_arc_.x1 = w; inner_arc_.y1 = h; - 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); + inner_arc_.start_angle = tier4_autoware_utils::rad2deg(min_range_theta - M_PI); + inner_arc_.end_angle = tier4_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_utils::rad2deg(min_range_theta - M_PI); - outer_arc_.end_angle = autoware_utils::rad2deg(max_range_theta - min_range_theta); + outer_arc_.start_angle = tier4_autoware_utils::rad2deg(min_range_theta - M_PI); + outer_arc_.end_angle = tier4_autoware_utils::rad2deg(max_range_theta - min_range_theta); } } // namespace rviz_plugins diff --git a/common/autoware_vehicle_rviz_plugin/src/tools/console_meter.hpp b/common/tier4_vehicle_rviz_plugin/src/tools/console_meter.hpp similarity index 86% rename from common/autoware_vehicle_rviz_plugin/src/tools/console_meter.hpp rename to common/tier4_vehicle_rviz_plugin/src/tools/console_meter.hpp index 7847a5725274f..e71003ea7f61c 100644 --- a/common/autoware_vehicle_rviz_plugin/src/tools/console_meter.hpp +++ b/common/tier4_vehicle_rviz_plugin/src/tools/console_meter.hpp @@ -21,11 +21,11 @@ #ifndef Q_MOC_RUN #include "jsk_overlay_utils.hpp" -#include #include #include #include #include +#include #include #endif @@ -62,10 +62,10 @@ private Q_SLOTS: // QImage hud_; private: - 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 float meter_min_velocity_ = tier4_autoware_utils::kmph2mps(0.f); + static constexpr float meter_max_velocity_ = tier4_autoware_utils::kmph2mps(60.f); + static constexpr float meter_min_angle_ = tier4_autoware_utils::deg2rad(40.f); + static constexpr float meter_max_angle_ = tier4_autoware_utils::deg2rad(320.f); static constexpr int line_width_ = 2; static constexpr int hand_width_ = 4; struct Line // for drawLine diff --git a/common/autoware_vehicle_rviz_plugin/src/tools/jsk_overlay_utils.cpp b/common/tier4_vehicle_rviz_plugin/src/tools/jsk_overlay_utils.cpp similarity index 100% rename from common/autoware_vehicle_rviz_plugin/src/tools/jsk_overlay_utils.cpp rename to common/tier4_vehicle_rviz_plugin/src/tools/jsk_overlay_utils.cpp diff --git a/common/autoware_vehicle_rviz_plugin/src/tools/jsk_overlay_utils.hpp b/common/tier4_vehicle_rviz_plugin/src/tools/jsk_overlay_utils.hpp similarity index 100% rename from common/autoware_vehicle_rviz_plugin/src/tools/jsk_overlay_utils.hpp rename to common/tier4_vehicle_rviz_plugin/src/tools/jsk_overlay_utils.hpp diff --git a/common/autoware_vehicle_rviz_plugin/src/tools/steering_angle.cpp b/common/tier4_vehicle_rviz_plugin/src/tools/steering_angle.cpp similarity index 99% rename from common/autoware_vehicle_rviz_plugin/src/tools/steering_angle.cpp rename to common/tier4_vehicle_rviz_plugin/src/tools/steering_angle.cpp index f09c2c2f14f79..ee922e017511b 100644 --- a/common/autoware_vehicle_rviz_plugin/src/tools/steering_angle.cpp +++ b/common/tier4_vehicle_rviz_plugin/src/tools/steering_angle.cpp @@ -27,7 +27,7 @@ namespace rviz_plugins { SteeringAngleDisplay::SteeringAngleDisplay() : handle_image_(std::string( - ament_index_cpp::get_package_share_directory("autoware_vehicle_rviz_plugin") + + ament_index_cpp::get_package_share_directory("tier4_vehicle_rviz_plugin") + "/images/handle.png") .c_str()) { diff --git a/common/autoware_vehicle_rviz_plugin/src/tools/steering_angle.hpp b/common/tier4_vehicle_rviz_plugin/src/tools/steering_angle.hpp similarity index 100% rename from common/autoware_vehicle_rviz_plugin/src/tools/steering_angle.hpp rename to common/tier4_vehicle_rviz_plugin/src/tools/steering_angle.hpp diff --git a/common/autoware_vehicle_rviz_plugin/src/tools/turn_signal.cpp b/common/tier4_vehicle_rviz_plugin/src/tools/turn_signal.cpp similarity index 100% rename from common/autoware_vehicle_rviz_plugin/src/tools/turn_signal.cpp rename to common/tier4_vehicle_rviz_plugin/src/tools/turn_signal.cpp diff --git a/common/autoware_vehicle_rviz_plugin/src/tools/turn_signal.hpp b/common/tier4_vehicle_rviz_plugin/src/tools/turn_signal.hpp similarity index 100% rename from common/autoware_vehicle_rviz_plugin/src/tools/turn_signal.hpp rename to common/tier4_vehicle_rviz_plugin/src/tools/turn_signal.hpp diff --git a/common/autoware_vehicle_rviz_plugin/src/tools/velocity_history.cpp b/common/tier4_vehicle_rviz_plugin/src/tools/velocity_history.cpp similarity index 100% rename from common/autoware_vehicle_rviz_plugin/src/tools/velocity_history.cpp rename to common/tier4_vehicle_rviz_plugin/src/tools/velocity_history.cpp diff --git a/common/autoware_vehicle_rviz_plugin/src/tools/velocity_history.hpp b/common/tier4_vehicle_rviz_plugin/src/tools/velocity_history.hpp similarity index 100% rename from common/autoware_vehicle_rviz_plugin/src/tools/velocity_history.hpp rename to common/tier4_vehicle_rviz_plugin/src/tools/velocity_history.hpp diff --git a/common/autoware_web_controller/CMakeLists.txt b/common/web_controller/CMakeLists.txt similarity index 83% rename from common/autoware_web_controller/CMakeLists.txt rename to common/web_controller/CMakeLists.txt index 8df93922fdb67..7ffdcb57798e6 100644 --- a/common/autoware_web_controller/CMakeLists.txt +++ b/common/web_controller/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.5) -project(autoware_web_controller) +project(web_controller) find_package(ament_cmake REQUIRED) diff --git a/common/autoware_web_controller/README.md b/common/web_controller/README.md similarity index 96% rename from common/autoware_web_controller/README.md rename to common/web_controller/README.md index bfe193134e9e3..32b9352a4b53c 100644 --- a/common/autoware_web_controller/README.md +++ b/common/web_controller/README.md @@ -1,4 +1,4 @@ -# autoware_web_controller +# web_controller ## Purpose @@ -37,6 +37,6 @@ TBD. ## Usage -1. Access to after launching Autoware. +1. Access to after launching Autoware. ![select_panel](./images/web_controller.png) 2. Check the status of Autoware or send topics by the buttons. diff --git a/common/autoware_web_controller/images/web_controller.png b/common/web_controller/images/web_controller.png similarity index 100% rename from common/autoware_web_controller/images/web_controller.png rename to common/web_controller/images/web_controller.png diff --git a/common/autoware_web_controller/launch/autoware_web_controller.launch.xml b/common/web_controller/launch/web_controller.launch.xml similarity index 81% rename from common/autoware_web_controller/launch/autoware_web_controller.launch.xml rename to common/web_controller/launch/web_controller.launch.xml index 9c22240408f8f..b6a17ac01ec50 100644 --- a/common/autoware_web_controller/launch/autoware_web_controller.launch.xml +++ b/common/web_controller/launch/web_controller.launch.xml @@ -2,7 +2,7 @@ - + diff --git a/common/autoware_web_controller/package.xml b/common/web_controller/package.xml similarity index 86% rename from common/autoware_web_controller/package.xml rename to common/web_controller/package.xml index 9aae3e225c32a..5943b8efce171 100644 --- a/common/autoware_web_controller/package.xml +++ b/common/web_controller/package.xml @@ -1,9 +1,9 @@ - autoware_web_controller + web_controller 0.1.0 - The autoware_web_controller package + The web_controller package Yukihiro Saito Apache License 2.0 diff --git a/common/autoware_web_controller/www/autoware_web_controller/css/autoware_web_controller_style.css b/common/web_controller/www/web_controller/css/web_controller_style.css similarity index 100% rename from common/autoware_web_controller/www/autoware_web_controller/css/autoware_web_controller_style.css rename to common/web_controller/www/web_controller/css/web_controller_style.css diff --git a/common/autoware_web_controller/www/autoware_web_controller/icon/autoware.ico b/common/web_controller/www/web_controller/icon/autoware.ico similarity index 100% rename from common/autoware_web_controller/www/autoware_web_controller/icon/autoware.ico rename to common/web_controller/www/web_controller/icon/autoware.ico diff --git a/common/autoware_web_controller/www/autoware_web_controller/index.html b/common/web_controller/www/web_controller/index.html similarity index 97% rename from common/autoware_web_controller/www/autoware_web_controller/index.html rename to common/web_controller/www/web_controller/index.html index 4442d64c9548a..110481ae298fe 100644 --- a/common/autoware_web_controller/www/autoware_web_controller/index.html +++ b/common/web_controller/www/web_controller/index.html @@ -4,7 +4,7 @@ Autoware WEB Controller - + diff --git a/common/autoware_web_controller/www/autoware_web_controller/js/autoware_engage.js b/common/web_controller/www/web_controller/js/autoware_engage.js similarity index 100% rename from common/autoware_web_controller/www/autoware_web_controller/js/autoware_engage.js rename to common/web_controller/www/web_controller/js/autoware_engage.js diff --git a/common/autoware_web_controller/www/autoware_web_controller/js/autoware_state.js b/common/web_controller/www/web_controller/js/autoware_state.js similarity index 100% rename from common/autoware_web_controller/www/autoware_web_controller/js/autoware_state.js rename to common/web_controller/www/web_controller/js/autoware_state.js diff --git a/common/autoware_web_controller/www/autoware_web_controller/js/lane_change_approval.js b/common/web_controller/www/web_controller/js/lane_change_approval.js similarity index 100% rename from common/autoware_web_controller/www/autoware_web_controller/js/lane_change_approval.js rename to common/web_controller/www/web_controller/js/lane_change_approval.js diff --git a/common/autoware_web_controller/www/autoware_web_controller/js/menu.js b/common/web_controller/www/web_controller/js/menu.js similarity index 100% rename from common/autoware_web_controller/www/autoware_web_controller/js/menu.js rename to common/web_controller/www/web_controller/js/menu.js diff --git a/common/autoware_web_controller/www/autoware_web_controller/js/path_change_approval.js b/common/web_controller/www/web_controller/js/path_change_approval.js similarity index 100% rename from common/autoware_web_controller/www/autoware_web_controller/js/path_change_approval.js rename to common/web_controller/www/web_controller/js/path_change_approval.js diff --git a/common/autoware_web_controller/www/autoware_web_controller/js/vehicle_engage.js b/common/web_controller/www/web_controller/js/vehicle_engage.js similarity index 100% rename from common/autoware_web_controller/www/autoware_web_controller/js/vehicle_engage.js rename to common/web_controller/www/web_controller/js/vehicle_engage.js diff --git a/common/autoware_web_controller/www/autoware_web_controller/js/velocity_limit.js b/common/web_controller/www/web_controller/js/velocity_limit.js similarity index 100% rename from common/autoware_web_controller/www/autoware_web_controller/js/velocity_limit.js rename to common/web_controller/www/web_controller/js/velocity_limit.js diff --git a/control/control_performance_analysis/include/control_performance_analysis/control_performance_analysis_node.hpp b/control/control_performance_analysis/include/control_performance_analysis/control_performance_analysis_node.hpp index 201d7b2c89b4b..36b8311c70efd 100644 --- a/control/control_performance_analysis/include/control_performance_analysis/control_performance_analysis_node.hpp +++ b/control/control_performance_analysis/include/control_performance_analysis/control_performance_analysis_node.hpp @@ -18,8 +18,8 @@ #include "control_performance_analysis/control_performance_analysis_core.hpp" #include "control_performance_analysis/msg/error_stamped.hpp" -#include #include +#include #include #include @@ -65,7 +65,7 @@ class ControlPerformanceAnalysisNode : public rclcpp::Node rclcpp::Subscription::SharedPtr sub_vehicle_steering_; // Self Pose listener. - autoware_utils::SelfPoseListener self_pose_listener_{this}; // subscribe to pose listener. + tier4_autoware_utils::SelfPoseListener self_pose_listener_{this}; // subscribe to pose listener. // Publishers rclcpp::Publisher::SharedPtr pub_error_msg_; // publish error message diff --git a/control/control_performance_analysis/package.xml b/control/control_performance_analysis/package.xml index bda3b7cce680b..1d261adce1ecd 100644 --- a/control/control_performance_analysis/package.xml +++ b/control/control_performance_analysis/package.xml @@ -13,7 +13,6 @@ autoware_auto_control_msgs autoware_auto_planning_msgs autoware_auto_vehicle_msgs - autoware_utils geometry_msgs libboost-dev nav_msgs @@ -23,9 +22,10 @@ tf2 tf2_eigen tf2_ros + tier4_autoware_utils vehicle_info_util - autoware_global_parameter_loader + global_parameter_loader rosidl_default_runtime ament_lint_auto diff --git a/control/external_cmd_selector/package.xml b/control/external_cmd_selector/package.xml index fbc294618998e..6e0d07dff6368 100644 --- a/control/external_cmd_selector/package.xml +++ b/control/external_cmd_selector/package.xml @@ -10,12 +10,12 @@ ament_cmake_auto autoware_auto_vehicle_msgs - autoware_iv_auto_msgs_converter diagnostic_updater geometry_msgs rclcpp rclcpp_components std_msgs + tier4_auto_msgs_converter tier4_control_msgs tier4_external_api_msgs diff --git a/control/external_cmd_selector/src/external_cmd_selector/external_cmd_selector_node.cpp b/control/external_cmd_selector/src/external_cmd_selector/external_cmd_selector_node.cpp index 0c4168ff245c2..cd2175f714204 100644 --- a/control/external_cmd_selector/src/external_cmd_selector/external_cmd_selector_node.cpp +++ b/control/external_cmd_selector/src/external_cmd_selector/external_cmd_selector_node.cpp @@ -14,7 +14,7 @@ #include "external_cmd_selector/external_cmd_selector_node.hpp" -#include +#include #include #include @@ -130,7 +130,7 @@ void ExternalCmdSelector::onLocalTurnSignalCmd(const ExternalTurnSignal::ConstSh if (current_selector_mode_.data != CommandSourceMode::LOCAL) { return; } - auto light_signal = autoware_iv_auto_msgs_converter::convert(*msg); + auto light_signal = tier4_auto_msgs_converter::convert(*msg); pub_turn_signal_cmd_->publish(light_signal.turn_signal); pub_hazard_signal_cmd_->publish(light_signal.hazard_signal); } @@ -164,7 +164,7 @@ void ExternalCmdSelector::onRemoteTurnSignalCmd(const ExternalTurnSignal::ConstS if (current_selector_mode_.data != CommandSourceMode::REMOTE) { return; } - auto light_signal = autoware_iv_auto_msgs_converter::convert(*msg); + auto light_signal = tier4_auto_msgs_converter::convert(*msg); pub_turn_signal_cmd_->publish(light_signal.turn_signal); pub_hazard_signal_cmd_->publish(light_signal.hazard_signal); } @@ -192,7 +192,7 @@ void ExternalCmdSelector::onTimer() { pub_current_selector_mode_->publish(curren ExternalCmdSelector::InternalGearShift ExternalCmdSelector::convert( const ExternalGearShift & command) { - return autoware_iv_auto_msgs_converter::convert(command); + return tier4_auto_msgs_converter::convert(command); } ExternalCmdSelector::InternalHeartbeat ExternalCmdSelector::convert( diff --git a/control/autoware_joy_controller/CMakeLists.txt b/control/joy_controller/CMakeLists.txt similarity index 60% rename from control/autoware_joy_controller/CMakeLists.txt rename to control/joy_controller/CMakeLists.txt index 55bd83ac16158..841c18d11c804 100644 --- a/control/autoware_joy_controller/CMakeLists.txt +++ b/control/joy_controller/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.5) -project(autoware_joy_controller) +project(joy_controller) if(NOT CMAKE_CXX_STANDARD) set(CMAKE_CXX_STANDARD 14) @@ -11,13 +11,13 @@ endif() find_package(ament_cmake_auto REQUIRED) ament_auto_find_build_dependencies() -ament_auto_add_library(autoware_joy_controller_node SHARED - src/autoware_joy_controller/autoware_joy_controller_node.cpp +ament_auto_add_library(joy_controller_node SHARED + src/joy_controller/joy_controller_node.cpp ) -rclcpp_components_register_node(autoware_joy_controller_node - PLUGIN "autoware_joy_controller::AutowareJoyControllerNode" - EXECUTABLE autoware_joy_controller +rclcpp_components_register_node(joy_controller_node + PLUGIN "joy_controller::AutowareJoyControllerNode" + EXECUTABLE joy_controller ) if(BUILD_TESTING) diff --git a/control/autoware_joy_controller/README.md b/control/joy_controller/README.md similarity index 95% rename from control/autoware_joy_controller/README.md rename to control/joy_controller/README.md index c52179dd6892a..a494a9e6fb330 100644 --- a/control/autoware_joy_controller/README.md +++ b/control/joy_controller/README.md @@ -1,8 +1,8 @@ -# autoware_joy_controller +# joy_controller ## Role -`autoware_joy_controller` is the package to convert a joy msg to autoware commands (e.g. steering wheel, shift, turn signal, engage) for a vehicle. +`joy_controller` is the package to convert a joy msg to autoware commands (e.g. steering wheel, shift, turn signal, engage) for a vehicle. ## Input / Output diff --git a/control/autoware_joy_controller/config/autoware_joy_controller.param.yaml b/control/joy_controller/config/joy_controller.param.yaml similarity index 100% rename from control/autoware_joy_controller/config/autoware_joy_controller.param.yaml rename to control/joy_controller/config/joy_controller.param.yaml diff --git a/control/autoware_joy_controller/include/autoware_joy_controller/autoware_joy_controller.hpp b/control/joy_controller/include/joy_controller/joy_controller.hpp similarity index 92% rename from control/autoware_joy_controller/include/autoware_joy_controller/autoware_joy_controller.hpp rename to control/joy_controller/include/joy_controller/joy_controller.hpp index a79700691d6fd..c6de42afc38d9 100644 --- a/control/autoware_joy_controller/include/autoware_joy_controller/autoware_joy_controller.hpp +++ b/control/joy_controller/include/joy_controller/joy_controller.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE_JOY_CONTROLLER__AUTOWARE_JOY_CONTROLLER_HPP_ -#define AUTOWARE_JOY_CONTROLLER__AUTOWARE_JOY_CONTROLLER_HPP_ +#ifndef JOY_CONTROLLER__JOY_CONTROLLER_HPP_ +#define JOY_CONTROLLER__JOY_CONTROLLER_HPP_ -#include "autoware_joy_controller/joy_converter/joy_converter_base.hpp" +#include "joy_controller/joy_converter/joy_converter_base.hpp" #include @@ -36,7 +36,7 @@ #include #include -namespace autoware_joy_controller +namespace joy_controller { using GearShiftType = tier4_external_api_msgs::msg::GearShift::_data_type; using TurnSignalType = tier4_external_api_msgs::msg::TurnSignal::_data_type; @@ -118,6 +118,6 @@ class AutowareJoyControllerNode : public rclcpp::Node bool isDataReady(); void onTimer(); }; -} // namespace autoware_joy_controller +} // namespace joy_controller -#endif // AUTOWARE_JOY_CONTROLLER__AUTOWARE_JOY_CONTROLLER_HPP_ +#endif // JOY_CONTROLLER__JOY_CONTROLLER_HPP_ diff --git a/control/autoware_joy_controller/include/autoware_joy_controller/joy_converter/ds4_joy_converter.hpp b/control/joy_controller/include/joy_controller/joy_converter/ds4_joy_converter.hpp similarity index 89% rename from control/autoware_joy_controller/include/autoware_joy_controller/joy_converter/ds4_joy_converter.hpp rename to control/joy_controller/include/joy_controller/joy_converter/ds4_joy_converter.hpp index 0a35696808d94..faa920d493478 100644 --- a/control/autoware_joy_controller/include/autoware_joy_controller/joy_converter/ds4_joy_converter.hpp +++ b/control/joy_controller/include/joy_controller/joy_converter/ds4_joy_converter.hpp @@ -12,14 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE_JOY_CONTROLLER__JOY_CONVERTER__DS4_JOY_CONVERTER_HPP_ -#define AUTOWARE_JOY_CONTROLLER__JOY_CONVERTER__DS4_JOY_CONVERTER_HPP_ +#ifndef JOY_CONTROLLER__JOY_CONVERTER__DS4_JOY_CONVERTER_HPP_ +#define JOY_CONTROLLER__JOY_CONVERTER__DS4_JOY_CONVERTER_HPP_ -#include "autoware_joy_controller/joy_converter/joy_converter_base.hpp" +#include "joy_controller/joy_converter/joy_converter_base.hpp" #include -namespace autoware_joy_controller +namespace joy_controller { class DS4JoyConverter : public JoyConverterBase { @@ -90,6 +90,6 @@ class DS4JoyConverter : public JoyConverterBase bool reverse() const { return Share(); } }; -} // namespace autoware_joy_controller +} // namespace joy_controller -#endif // AUTOWARE_JOY_CONTROLLER__JOY_CONVERTER__DS4_JOY_CONVERTER_HPP_ +#endif // JOY_CONTROLLER__JOY_CONVERTER__DS4_JOY_CONVERTER_HPP_ diff --git a/control/autoware_joy_controller/include/autoware_joy_controller/joy_converter/g29_joy_converter.hpp b/control/joy_controller/include/joy_controller/joy_converter/g29_joy_converter.hpp similarity index 88% rename from control/autoware_joy_controller/include/autoware_joy_controller/joy_converter/g29_joy_converter.hpp rename to control/joy_controller/include/joy_controller/joy_converter/g29_joy_converter.hpp index 22039cdf6f95f..7ba062fe10d19 100644 --- a/control/autoware_joy_controller/include/autoware_joy_controller/joy_converter/g29_joy_converter.hpp +++ b/control/joy_controller/include/joy_controller/joy_converter/g29_joy_converter.hpp @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE_JOY_CONTROLLER__JOY_CONVERTER__G29_JOY_CONVERTER_HPP_ -#define AUTOWARE_JOY_CONTROLLER__JOY_CONVERTER__G29_JOY_CONVERTER_HPP_ +#ifndef JOY_CONTROLLER__JOY_CONVERTER__G29_JOY_CONVERTER_HPP_ +#define JOY_CONTROLLER__JOY_CONVERTER__G29_JOY_CONVERTER_HPP_ -#include "autoware_joy_controller/joy_converter/joy_converter_base.hpp" +#include "joy_controller/joy_converter/joy_converter_base.hpp" -namespace autoware_joy_controller +namespace joy_controller { class G29JoyConverter : public JoyConverterBase { @@ -88,6 +88,6 @@ class G29JoyConverter : public JoyConverterBase bool reverse() const { return Share(); } }; -} // namespace autoware_joy_controller +} // namespace joy_controller -#endif // AUTOWARE_JOY_CONTROLLER__JOY_CONVERTER__G29_JOY_CONVERTER_HPP_ +#endif // JOY_CONTROLLER__JOY_CONVERTER__G29_JOY_CONVERTER_HPP_ diff --git a/control/autoware_joy_controller/include/autoware_joy_controller/joy_converter/joy_converter_base.hpp b/control/joy_controller/include/joy_controller/joy_converter/joy_converter_base.hpp similarity index 82% rename from control/autoware_joy_controller/include/autoware_joy_controller/joy_converter/joy_converter_base.hpp rename to control/joy_controller/include/joy_controller/joy_converter/joy_converter_base.hpp index 3da69c915e21a..94587b88e22f5 100644 --- a/control/autoware_joy_controller/include/autoware_joy_controller/joy_converter/joy_converter_base.hpp +++ b/control/joy_controller/include/joy_controller/joy_converter/joy_converter_base.hpp @@ -12,14 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE_JOY_CONTROLLER__JOY_CONVERTER__JOY_CONVERTER_BASE_HPP_ -#define AUTOWARE_JOY_CONTROLLER__JOY_CONVERTER__JOY_CONVERTER_BASE_HPP_ +#ifndef JOY_CONTROLLER__JOY_CONVERTER__JOY_CONVERTER_BASE_HPP_ +#define JOY_CONTROLLER__JOY_CONVERTER__JOY_CONVERTER_BASE_HPP_ #include #include -namespace autoware_joy_controller +namespace joy_controller { class JoyConverterBase { @@ -50,6 +50,6 @@ class JoyConverterBase virtual bool vehicle_engage() const = 0; virtual bool vehicle_disengage() const = 0; }; -} // namespace autoware_joy_controller +} // namespace joy_controller -#endif // AUTOWARE_JOY_CONTROLLER__JOY_CONVERTER__JOY_CONVERTER_BASE_HPP_ +#endif // JOY_CONTROLLER__JOY_CONVERTER__JOY_CONVERTER_BASE_HPP_ diff --git a/control/autoware_joy_controller/launch/autoware_joy_controller.launch.xml b/control/joy_controller/launch/joy_controller.launch.xml similarity index 89% rename from control/autoware_joy_controller/launch/autoware_joy_controller.launch.xml rename to control/joy_controller/launch/joy_controller.launch.xml index 62dc933a3dfbf..c08db53cfe1cf 100644 --- a/control/autoware_joy_controller/launch/autoware_joy_controller.launch.xml +++ b/control/joy_controller/launch/joy_controller.launch.xml @@ -13,9 +13,9 @@ - + - + diff --git a/control/autoware_joy_controller/package.xml b/control/joy_controller/package.xml similarity index 84% rename from control/autoware_joy_controller/package.xml rename to control/joy_controller/package.xml index e1c5d3ef53783..59534317842a7 100644 --- a/control/autoware_joy_controller/package.xml +++ b/control/joy_controller/package.xml @@ -1,14 +1,13 @@ - autoware_joy_controller + joy_controller 0.1.0 - The autoware_joy_controller package + The joy_controller package Kenji Miyake Apache License 2.0 ament_cmake_auto - autoware_api_utils autoware_auto_control_msgs autoware_auto_vehicle_msgs geometry_msgs @@ -18,6 +17,7 @@ rclcpp_components sensor_msgs std_srvs + tier4_api_utils tier4_control_msgs tier4_external_api_msgs diff --git a/control/autoware_joy_controller/src/autoware_joy_controller/autoware_joy_controller_node.cpp b/control/joy_controller/src/joy_controller/joy_controller_node.cpp similarity index 95% rename from control/autoware_joy_controller/src/autoware_joy_controller/autoware_joy_controller_node.cpp rename to control/joy_controller/src/joy_controller/joy_controller_node.cpp index 6ead3635a83be..f7e3f656e903e 100644 --- a/control/autoware_joy_controller/src/autoware_joy_controller/autoware_joy_controller_node.cpp +++ b/control/joy_controller/src/joy_controller/joy_controller_node.cpp @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware_joy_controller/autoware_joy_controller.hpp" -#include "autoware_joy_controller/joy_converter/ds4_joy_converter.hpp" -#include "autoware_joy_controller/joy_converter/g29_joy_converter.hpp" +#include "joy_controller/joy_controller.hpp" +#include "joy_controller/joy_converter/ds4_joy_converter.hpp" +#include "joy_controller/joy_converter/g29_joy_converter.hpp" -#include +#include #include #include @@ -25,9 +25,9 @@ namespace { -using autoware_joy_controller::GateModeType; -using autoware_joy_controller::GearShiftType; -using autoware_joy_controller::TurnSignalType; +using joy_controller::GateModeType; +using joy_controller::GearShiftType; +using joy_controller::TurnSignalType; using GearShift = tier4_external_api_msgs::msg::GearShift; using TurnSignal = tier4_external_api_msgs::msg::TurnSignal; using GateMode = tier4_control_msgs::msg::GateMode; @@ -144,7 +144,7 @@ double calcMapping(const double input, const double sensitivity) } // namespace -namespace autoware_joy_controller +namespace joy_controller { void AutowareJoyControllerNode::onJoy(const sensor_msgs::msg::Joy::ConstSharedPtr msg) { @@ -384,7 +384,7 @@ void AutowareJoyControllerNode::sendEmergencyRequest(bool emergency) request, [this, emergency]( rclcpp::Client::SharedFuture result) { auto response = result.get(); - if (autoware_api_utils::is_success(response->status)) { + if (tier4_api_utils::is_success(response->status)) { RCLCPP_INFO(get_logger(), "service succeeded"); } else { RCLCPP_WARN(get_logger(), "service failed: %s", response->status.message.c_str()); @@ -447,7 +447,7 @@ void AutowareJoyControllerNode::initTimer(double period_s) } AutowareJoyControllerNode::AutowareJoyControllerNode(const rclcpp::NodeOptions & node_options) -: Node("autoware_joy_controller", node_options) +: Node("joy_controller", node_options) { // Parameter joy_type_ = declare_parameter("joy_type", std::string("DS4")); @@ -517,7 +517,7 @@ AutowareJoyControllerNode::AutowareJoyControllerNode(const rclcpp::NodeOptions & // Timer initTimer(1.0 / update_rate_); } -} // namespace autoware_joy_controller +} // namespace joy_controller #include -RCLCPP_COMPONENTS_REGISTER_NODE(autoware_joy_controller::AutowareJoyControllerNode) +RCLCPP_COMPONENTS_REGISTER_NODE(joy_controller::AutowareJoyControllerNode) diff --git a/control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker.hpp b/control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker.hpp index 1dd6fe3558d82..0ee843bce22b7 100644 --- a/control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker.hpp +++ b/control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker.hpp @@ -15,9 +15,9 @@ #ifndef LANE_DEPARTURE_CHECKER__LANE_DEPARTURE_CHECKER_HPP_ #define LANE_DEPARTURE_CHECKER__LANE_DEPARTURE_CHECKER_HPP_ -#include -#include #include +#include +#include #include #include @@ -42,8 +42,8 @@ namespace lane_departure_checker using autoware_auto_planning_msgs::msg::HADMapRoute; using autoware_auto_planning_msgs::msg::Trajectory; using autoware_auto_planning_msgs::msg::TrajectoryPoint; -using autoware_utils::LinearRing2d; -using autoware_utils::PoseDeviation; +using tier4_autoware_utils::LinearRing2d; +using tier4_autoware_utils::PoseDeviation; using TrajectoryPoints = std::vector; struct Param diff --git a/control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker_node.hpp b/control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker_node.hpp index 015218277534f..3359d13e19fb1 100644 --- a/control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker_node.hpp +++ b/control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker_node.hpp @@ -17,12 +17,12 @@ #include "lane_departure_checker/lane_departure_checker.hpp" -#include -#include -#include #include #include #include +#include +#include +#include #include #include @@ -56,7 +56,7 @@ class LaneDepartureCheckerNode : public rclcpp::Node private: // Subscriber - autoware_utils::SelfPoseListener self_pose_listener_{this}; + tier4_autoware_utils::SelfPoseListener self_pose_listener_{this}; rclcpp::Subscription::SharedPtr sub_odom_; rclcpp::Subscription::SharedPtr sub_lanelet_map_bin_; rclcpp::Subscription::SharedPtr sub_route_; @@ -84,8 +84,8 @@ class LaneDepartureCheckerNode : public rclcpp::Node void onPredictedTrajectory(const Trajectory::ConstSharedPtr msg); // Publisher - autoware_utils::DebugPublisher debug_publisher_{this, "~/debug"}; - autoware_utils::ProcessingTimePublisher processing_time_publisher_{this}; + tier4_autoware_utils::DebugPublisher debug_publisher_{this, "~/debug"}; + tier4_autoware_utils::ProcessingTimePublisher processing_time_publisher_{this}; // Timer rclcpp::TimerBase::SharedPtr timer_; diff --git a/control/lane_departure_checker/include/lane_departure_checker/util/create_vehicle_footprint.hpp b/control/lane_departure_checker/include/lane_departure_checker/util/create_vehicle_footprint.hpp index 2bcaebd5341bc..1e0701eb403c8 100644 --- a/control/lane_departure_checker/include/lane_departure_checker/util/create_vehicle_footprint.hpp +++ b/control/lane_departure_checker/include/lane_departure_checker/util/create_vehicle_footprint.hpp @@ -29,7 +29,7 @@ #define LANE_DEPARTURE_CHECKER__UTIL__CREATE_VEHICLE_FOOTPRINT_HPP_ #include -#include +#include #include #include @@ -40,11 +40,11 @@ struct FootprintMargin double lat; }; -inline autoware_utils::LinearRing2d createVehicleFootprint( +inline tier4_autoware_utils::LinearRing2d createVehicleFootprint( const vehicle_info_util::VehicleInfo & vehicle_info, const FootprintMargin & margin = {0.0, 0.0}) { - using autoware_utils::LinearRing2d; - using autoware_utils::Point2d; + using tier4_autoware_utils::LinearRing2d; + using tier4_autoware_utils::Point2d; const auto & i = vehicle_info; diff --git a/control/lane_departure_checker/package.xml b/control/lane_departure_checker/package.xml index 036dfe278e88e..baa78c263a5eb 100644 --- a/control/lane_departure_checker/package.xml +++ b/control/lane_departure_checker/package.xml @@ -10,7 +10,6 @@ autoware_auto_mapping_msgs autoware_auto_planning_msgs - autoware_utils boost diagnostic_updater eigen @@ -23,6 +22,7 @@ tf2_eigen tf2_geometry_msgs tf2_ros + tier4_autoware_utils tier4_debug_msgs vehicle_info_util diff --git a/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker.cpp b/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker.cpp index 6031296c79a25..867de951accab 100644 --- a/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker.cpp +++ b/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker.cpp @@ -16,10 +16,10 @@ #include "lane_departure_checker/util/create_vehicle_footprint.hpp" -#include -#include -#include -#include +#include +#include +#include +#include #include @@ -29,9 +29,9 @@ #include #include -using autoware_utils::LinearRing2d; -using autoware_utils::MultiPoint2d; -using autoware_utils::Point2d; +using tier4_autoware_utils::LinearRing2d; +using tier4_autoware_utils::MultiPoint2d; +using tier4_autoware_utils::Point2d; namespace { @@ -63,8 +63,8 @@ size_t findNearestIndex(const Trajectory & trajectory, const geometry_msgs::msg: std::transform( trajectory.points.cbegin(), trajectory.points.cend(), std::back_inserter(distances), [&](const TrajectoryPoint & p) { - const auto p1 = autoware_utils::fromMsg(p.pose.position).to_2d(); - const auto p2 = autoware_utils::fromMsg(pose.position).to_2d(); + const auto p1 = tier4_autoware_utils::fromMsg(p.pose.position).to_2d(); + const auto p2 = tier4_autoware_utils::fromMsg(pose.position).to_2d(); return boost::geometry::distance(p1, p2); }); @@ -114,7 +114,7 @@ Output LaneDepartureChecker::update(const Input & input) { Output output{}; - autoware_utils::StopWatch stop_watch; + tier4_autoware_utils::StopWatch stop_watch; output.trajectory_deviation = calcTrajectoryDeviation(*input.reference_trajectory, input.current_pose->pose); @@ -155,7 +155,7 @@ PoseDeviation LaneDepartureChecker::calcTrajectoryDeviation( const Trajectory & trajectory, const geometry_msgs::msg::Pose & pose) { const auto nearest_idx = findNearestIndex(trajectory, pose); - return autoware_utils::calcPoseDeviation(trajectory.points.at(nearest_idx).pose, pose); + return tier4_autoware_utils::calcPoseDeviation(trajectory.points.at(nearest_idx).pose, pose); } TrajectoryPoints LaneDepartureChecker::resampleTrajectory( @@ -167,8 +167,8 @@ TrajectoryPoints LaneDepartureChecker::resampleTrajectory( for (size_t i = 1; i < trajectory.points.size() - 1; ++i) { const auto & point = trajectory.points.at(i); - const auto p1 = autoware_utils::fromMsg(resampled.back().pose.position); - const auto p2 = autoware_utils::fromMsg(point.pose.position); + const auto p1 = tier4_autoware_utils::fromMsg(resampled.back().pose.position); + const auto p2 = tier4_autoware_utils::fromMsg(point.pose.position); if (boost::geometry::distance(p1.to_2d(), p2.to_2d()) > interval) { resampled.push_back(point); @@ -189,8 +189,8 @@ TrajectoryPoints LaneDepartureChecker::cutTrajectory( for (size_t i = 1; i < trajectory.size(); ++i) { const auto & point = trajectory.at(i); - const auto p1 = autoware_utils::fromMsg(cut.back().pose.position); - const auto p2 = autoware_utils::fromMsg(point.pose.position); + const auto p1 = tier4_autoware_utils::fromMsg(cut.back().pose.position); + const auto p2 = tier4_autoware_utils::fromMsg(point.pose.position); const auto points_distance = boost::geometry::distance(p1.to_2d(), p2.to_2d()); const auto remain_distance = length - total_length; @@ -235,7 +235,7 @@ std::vector LaneDepartureChecker::createVehicleFootprints( std::vector vehicle_footprints; for (const auto & p : trajectory) { vehicle_footprints.push_back( - transformVector(local_vehicle_footprint, autoware_utils::pose2transform(p.pose))); + transformVector(local_vehicle_footprint, tier4_autoware_utils::pose2transform(p.pose))); } return vehicle_footprints; diff --git a/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker_node.cpp b/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker_node.cpp index 36e2ae5afaf4d..dc14871da47fa 100644 --- a/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker_node.cpp +++ b/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker_node.cpp @@ -14,11 +14,11 @@ #include "lane_departure_checker/lane_departure_checker_node.hpp" -#include -#include -#include #include #include +#include +#include +#include #include @@ -28,7 +28,7 @@ #include #include -using autoware_utils::rad2deg; +using tier4_autoware_utils::rad2deg; namespace { @@ -277,7 +277,7 @@ bool LaneDepartureCheckerNode::isDataValid() void LaneDepartureCheckerNode::onTimer() { std::map processing_time_map; - autoware_utils::StopWatch stop_watch; + tier4_autoware_utils::StopWatch stop_watch; stop_watch.tic("Total"); current_pose_ = self_pose_listener_.getCurrentPose(); @@ -427,9 +427,9 @@ void LaneDepartureCheckerNode::checkTrajectoryDeviation( visualization_msgs::msg::MarkerArray LaneDepartureCheckerNode::createMarkerArray() const { - using autoware_utils::createDefaultMarker; - using autoware_utils::createMarkerColor; - using autoware_utils::createMarkerScale; + using tier4_autoware_utils::createDefaultMarker; + using tier4_autoware_utils::createMarkerColor; + using tier4_autoware_utils::createMarkerScale; visualization_msgs::msg::MarkerArray marker_array; diff --git a/control/obstacle_collision_checker/include/obstacle_collision_checker/obstacle_collision_checker.hpp b/control/obstacle_collision_checker/include/obstacle_collision_checker/obstacle_collision_checker.hpp index ba609407386d4..4ab26c31f9910 100644 --- a/control/obstacle_collision_checker/include/obstacle_collision_checker/obstacle_collision_checker.hpp +++ b/control/obstacle_collision_checker/include/obstacle_collision_checker/obstacle_collision_checker.hpp @@ -15,7 +15,7 @@ #ifndef OBSTACLE_COLLISION_CHECKER__OBSTACLE_COLLISION_CHECKER_HPP_ #define OBSTACLE_COLLISION_CHECKER__OBSTACLE_COLLISION_CHECKER_HPP_ -#include +#include #include #include @@ -35,7 +35,7 @@ namespace obstacle_collision_checker { -using autoware_utils::LinearRing2d; +using tier4_autoware_utils::LinearRing2d; struct Param { diff --git a/control/obstacle_collision_checker/include/obstacle_collision_checker/obstacle_collision_checker_node.hpp b/control/obstacle_collision_checker/include/obstacle_collision_checker/obstacle_collision_checker_node.hpp index 591e1f1a0ea69..12b1e51a1cf3c 100644 --- a/control/obstacle_collision_checker/include/obstacle_collision_checker/obstacle_collision_checker_node.hpp +++ b/control/obstacle_collision_checker/include/obstacle_collision_checker/obstacle_collision_checker_node.hpp @@ -17,13 +17,13 @@ #include "obstacle_collision_checker/obstacle_collision_checker.hpp" -#include -#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_; @@ -72,8 +72,8 @@ class ObstacleCollisionCheckerNode : public rclcpp::Node void onOdom(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/obstacle_collision_checker/include/obstacle_collision_checker/util/create_vehicle_footprint.hpp b/control/obstacle_collision_checker/include/obstacle_collision_checker/util/create_vehicle_footprint.hpp index 5d80924603974..41d746693a8e1 100644 --- a/control/obstacle_collision_checker/include/obstacle_collision_checker/util/create_vehicle_footprint.hpp +++ b/control/obstacle_collision_checker/include/obstacle_collision_checker/util/create_vehicle_footprint.hpp @@ -15,14 +15,14 @@ #ifndef OBSTACLE_COLLISION_CHECKER__UTIL__CREATE_VEHICLE_FOOTPRINT_HPP_ #define OBSTACLE_COLLISION_CHECKER__UTIL__CREATE_VEHICLE_FOOTPRINT_HPP_ -#include +#include #include -inline autoware_utils::LinearRing2d createVehicleFootprint( +inline tier4_autoware_utils::LinearRing2d createVehicleFootprint( const vehicle_info_util::VehicleInfo & vehicle_info, const double margin = 0.0) { - using autoware_utils::LinearRing2d; - using autoware_utils::Point2d; + using tier4_autoware_utils::LinearRing2d; + using tier4_autoware_utils::Point2d; const auto & i = vehicle_info; diff --git a/control/obstacle_collision_checker/package.xml b/control/obstacle_collision_checker/package.xml index 71ff20b67d013..eea21a305a81a 100644 --- a/control/obstacle_collision_checker/package.xml +++ b/control/obstacle_collision_checker/package.xml @@ -10,7 +10,6 @@ ament_cmake autoware_auto_planning_msgs - autoware_utils boost diagnostic_updater eigen @@ -24,6 +23,7 @@ tf2_eigen tf2_geometry_msgs tf2_ros + tier4_autoware_utils vehicle_info_util ament_lint_auto diff --git a/control/obstacle_collision_checker/src/obstacle_collision_checker_node/obstacle_collision_checker.cpp b/control/obstacle_collision_checker/src/obstacle_collision_checker_node/obstacle_collision_checker.cpp index 221bc5d6349cb..40898c39394f4 100644 --- a/control/obstacle_collision_checker/src/obstacle_collision_checker_node/obstacle_collision_checker.cpp +++ b/control/obstacle_collision_checker/src/obstacle_collision_checker_node/obstacle_collision_checker.cpp @@ -16,12 +16,12 @@ #include "obstacle_collision_checker/util/create_vehicle_footprint.hpp" -#include -#include -#include -#include #include #include +#include +#include +#include +#include #include @@ -86,7 +86,7 @@ ObstacleCollisionChecker::ObstacleCollisionChecker(rclcpp::Node & node) Output ObstacleCollisionChecker::update(const Input & input) { Output output; - autoware_utils::StopWatch stop_watch; + tier4_autoware_utils::StopWatch stop_watch; // resample trajectory by braking distance constexpr double min_velocity = 0.01; @@ -127,8 +127,8 @@ autoware_auto_planning_msgs::msg::Trajectory ObstacleCollisionChecker::resampleT for (size_t i = 1; i < trajectory.points.size() - 1; ++i) { const auto & point = trajectory.points.at(i); - const auto p1 = autoware_utils::fromMsg(resampled.points.back().pose.position).to_2d(); - const auto p2 = autoware_utils::fromMsg(point.pose.position).to_2d(); + const auto p1 = tier4_autoware_utils::fromMsg(resampled.points.back().pose.position).to_2d(); + const auto p2 = tier4_autoware_utils::fromMsg(point.pose.position).to_2d(); if (boost::geometry::distance(p1, p2) > interval) { resampled.points.push_back(point); @@ -150,8 +150,8 @@ autoware_auto_planning_msgs::msg::Trajectory ObstacleCollisionChecker::cutTrajec for (size_t i = 1; i < trajectory.points.size(); ++i) { const auto & point = trajectory.points.at(i); - const auto p1 = autoware_utils::fromMsg(cut.points.back().pose.position); - const auto p2 = autoware_utils::fromMsg(point.pose.position); + const auto p1 = tier4_autoware_utils::fromMsg(cut.points.back().pose.position); + const auto p2 = tier4_autoware_utils::fromMsg(point.pose.position); const auto points_distance = boost::geometry::distance(p1.to_2d(), p2.to_2d()); const auto remain_distance = length - total_length; @@ -192,8 +192,9 @@ std::vector ObstacleCollisionChecker::createVehicleFootprints( // Create vehicle footprint on each TrajectoryPoint std::vector vehicle_footprints; for (const auto & p : trajectory.points) { - vehicle_footprints.push_back(autoware_utils::transformVector( - local_vehicle_footprint, autoware_utils::pose2transform(p.pose))); + vehicle_footprints.push_back( + tier4_autoware_utils::transformVector( + local_vehicle_footprint, tier4_autoware_utils::pose2transform(p.pose))); } return vehicle_footprints; @@ -216,7 +217,7 @@ std::vector ObstacleCollisionChecker::createVehiclePassingAreas( LinearRing2d ObstacleCollisionChecker::createHullFromFootprints( const LinearRing2d & area1, const LinearRing2d & area2) { - autoware_utils::MultiPoint2d combined; + tier4_autoware_utils::MultiPoint2d combined; for (const auto & p : area1) { combined.push_back(p); } @@ -248,7 +249,8 @@ bool ObstacleCollisionChecker::hasCollision( const LinearRing2d & vehicle_footprint) { for (const auto & point : obstacle_pointcloud) { - if (boost::geometry::within(autoware_utils::Point2d{point.x, point.y}, vehicle_footprint)) { + if (boost::geometry::within( + tier4_autoware_utils::Point2d{point.x, point.y}, vehicle_footprint)) { RCLCPP_WARN( rclcpp::get_logger("obstacle_collision_checker"), "[ObstacleCollisionChecker] Collide to Point x: %f y: %f", point.x, point.y); diff --git a/control/obstacle_collision_checker/src/obstacle_collision_checker_node/obstacle_collision_checker_node.cpp b/control/obstacle_collision_checker/src/obstacle_collision_checker_node/obstacle_collision_checker_node.cpp index 9817721d9c37a..2afb81453db26 100644 --- a/control/obstacle_collision_checker/src/obstacle_collision_checker_node/obstacle_collision_checker_node.cpp +++ b/control/obstacle_collision_checker/src/obstacle_collision_checker_node/obstacle_collision_checker_node.cpp @@ -16,9 +16,9 @@ #include "obstacle_collision_checker/util/create_vehicle_footprint.hpp" -#include -#include -#include +#include +#include +#include #include #include @@ -67,8 +67,8 @@ ObstacleCollisionCheckerNode::ObstacleCollisionCheckerNode(const rclcpp::NodeOpt obstacle_collision_checker_->setParam(param_); // 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", 1, @@ -83,8 +83,8 @@ ObstacleCollisionCheckerNode::ObstacleCollisionCheckerNode(const rclcpp::NodeOpt "input/odometry", 1, std::bind(&ObstacleCollisionCheckerNode::onOdom, 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"); @@ -266,9 +266,9 @@ void ObstacleCollisionCheckerNode::checkLaneDeparture( visualization_msgs::msg::MarkerArray ObstacleCollisionCheckerNode::createMarkerArray() const { - using autoware_utils::createDefaultMarker; - using autoware_utils::createMarkerColor; - using autoware_utils::createMarkerScale; + using tier4_autoware_utils::createDefaultMarker; + using tier4_autoware_utils::createMarkerColor; + using tier4_autoware_utils::createMarkerScale; visualization_msgs::msg::MarkerArray marker_array; diff --git a/control/trajectory_follower_nodes/src/longitudinal_controller_node.cpp b/control/trajectory_follower_nodes/src/longitudinal_controller_node.cpp index dc97d14f34c9e..edb52345bc07d 100644 --- a/control/trajectory_follower_nodes/src/longitudinal_controller_node.cpp +++ b/control/trajectory_follower_nodes/src/longitudinal_controller_node.cpp @@ -796,7 +796,7 @@ LongitudinalController::Motion LongitudinalController::keepBrakeBeforeStop( if (m_enable_brake_keeping_before_stop == false) { return output_motion; } - // const auto stop_idx = autoware_utils::searchZeroVelocityIndex(traj.points); + // const auto stop_idx = tier4_autoware_utils::searchZeroVelocityIndex(traj.points); const auto stop_idx = motion_common::searchZeroVelocityIndex(traj.points); if (!stop_idx) { return output_motion; diff --git a/control/vehicle_cmd_gate/package.xml b/control/vehicle_cmd_gate/package.xml index a6f0682694628..a03d5a9947baf 100644 --- a/control/vehicle_cmd_gate/package.xml +++ b/control/vehicle_cmd_gate/package.xml @@ -8,7 +8,6 @@ ament_cmake - autoware_api_utils autoware_auto_control_msgs autoware_auto_system_msgs autoware_auto_vehicle_msgs @@ -17,6 +16,7 @@ rclcpp rclcpp_components std_srvs + tier4_api_utils tier4_control_msgs tier4_debug_msgs tier4_external_api_msgs diff --git a/control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp b/control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp index 96972d239f4b0..88e7088d13a0c 100644 --- a/control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp +++ b/control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp @@ -14,8 +14,8 @@ #include "vehicle_cmd_gate/vehicle_cmd_gate.hpp" -#include #include +#include #include #include @@ -577,7 +577,7 @@ void VehicleCmdGate::onEngageService( const tier4_external_api_msgs::srv::Engage::Response::SharedPtr response) { is_engaged_ = request->engage; - response->status = autoware_api_utils::response_success(); + response->status = tier4_api_utils::response_success(); } void VehicleCmdGate::onSteering(autoware_auto_vehicle_msgs::msg::SteeringReport::ConstSharedPtr msg) @@ -613,9 +613,9 @@ void VehicleCmdGate::onExternalEmergencyStopService( } if (res->success) { - response->status = autoware_api_utils::response_success(res->message); + response->status = tier4_api_utils::response_success(res->message); } else { - response->status = autoware_api_utils::response_error(res->message); + response->status = tier4_api_utils::response_error(res->message); } } diff --git a/launch/system_launch/README.md b/launch/system_launch/README.md index ae95bb8a937f4..ba36a77a5e90f 100644 --- a/launch/system_launch/README.md +++ b/launch/system_launch/README.md @@ -17,4 +17,4 @@ Please see `` in `package.xml`. ``` -The sensing configuration parameters used in autoware_error_monitor are loaded from "config/diagnostic_aggregator/sensor_kit.param.yaml" in the "`SENSOR_MODEL`\_description" package. +The sensing configuration parameters used in system_error_monitor are loaded from "config/diagnostic_aggregator/sensor_kit.param.yaml" in the "`SENSOR_MODEL`\_description" package. diff --git a/launch/system_launch/launch/system.launch.xml b/launch/system_launch/launch/system.launch.xml index 8ae917f3082fb..1eb54a4a5ca24 100644 --- a/launch/system_launch/launch/system.launch.xml +++ b/launch/system_launch/launch/system.launch.xml @@ -23,16 +23,16 @@ - - - + + + - - - + + + diff --git a/launch/system_launch/package.xml b/launch/system_launch/package.xml index b2150348efb06..7f8169ed5e672 100644 --- a/launch/system_launch/package.xml +++ b/launch/system_launch/package.xml @@ -10,9 +10,9 @@ ament_cmake_auto - autoware_error_monitor - autoware_state_monitor + ad_service_state_monitor emergency_handler + system_error_monitor system_monitor ament_lint_auto diff --git a/launch/system_launch/system_launch.drawio.svg b/launch/system_launch/system_launch.drawio.svg index b22f005948a02..539356860e529 100644 --- a/launch/system_launch/system_launch.drawio.svg +++ b/launch/system_launch/system_launch.drawio.svg @@ -200,11 +200,11 @@
- autoware_state_monitor.launch.xml + ad_service_state_monitor.launch.xml

- package: autoware_state_monitor + package: ad_service_state_monitor

@@ -212,7 +212,7 @@
args: config_file = - autoware_state_monitor.param.yaml + ad_service_state_monitor.param.yaml
@@ -220,7 +220,7 @@
- autoware_state_monitor.launch.xml... + ad_service_state_monitor.launch.xml... @@ -231,11 +231,11 @@
- autoware_state_monitor.launch.xml + ad_service_state_monitor.launch.xml

- package: autoware_state_monitor + package: ad_service_state_monitor

@@ -243,7 +243,7 @@
args: config_file = - autoware_state_monitor.planning_simulation.param.yaml + ad_service_state_monitor.planning_simulation.param.yaml
@@ -251,7 +251,7 @@
- autoware_state_monitor.launch.xml... + ad_service_state_monitor.launch.xml... @@ -317,11 +317,11 @@
- autoware_error_monitor.launch.xml + system_error_monitor.launch.xml

- package: autoware_error_monitor + package: system_error_monitor

@@ -329,7 +329,7 @@
args: config_file = - autoware_error_monitor.param.yaml + system_error_monitor.param.yaml
@@ -337,7 +337,7 @@
- autoware_error_monitor.launch.xml... + system_error_monitor.launch.xml... @@ -348,11 +348,11 @@
- autoware_error_monitor.launch.xml + system_error_monitor.launch.xml

- package: autoware_error_monitor + package: system_error_monitor

@@ -360,7 +360,7 @@
args: config_file = - autoware_error_monitor.planning_simulation.param.yaml + system_error_monitor.planning_simulation.param.yaml
@@ -368,7 +368,7 @@
- autoware_error_monitor.launch.xml... + system_error_monitor.launch.xml... diff --git a/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp b/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp index 253162171fe29..e500e5b91a49a 100644 --- a/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp +++ b/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp @@ -15,11 +15,11 @@ #ifndef EKF_LOCALIZER__EKF_LOCALIZER_HPP_ #define EKF_LOCALIZER__EKF_LOCALIZER_HPP_ -#include -#include #include #include #include +#include +#include #include #include @@ -227,7 +227,7 @@ class EKFLocalizer : public rclcpp::Node */ void showCurrentX(); - autoware_utils::StopWatch stop_watch_; + tier4_autoware_utils::StopWatch stop_watch_; friend class EKFLocalizerTestSuite; // for test code }; diff --git a/localization/ekf_localizer/package.xml b/localization/ekf_localizer/package.xml index 7efacd4783a68..e4a380039ff02 100644 --- a/localization/ekf_localizer/package.xml +++ b/localization/ekf_localizer/package.xml @@ -11,7 +11,6 @@ eigen3_cmake_module eigen - autoware_utils geometry_msgs kalman_filter nav_msgs @@ -20,6 +19,7 @@ std_msgs tf2 tf2_ros + tier4_autoware_utils tier4_debug_msgs ament_cmake_gtest diff --git a/localization/ekf_localizer/src/ekf_localizer.cpp b/localization/ekf_localizer/src/ekf_localizer.cpp index 806e8bf3d304b..183a1fa35f019 100644 --- a/localization/ekf_localizer/src/ekf_localizer.cpp +++ b/localization/ekf_localizer/src/ekf_localizer.cpp @@ -14,8 +14,8 @@ #include "ekf_localizer/ekf_localizer.hpp" -#include #include +#include #include #include @@ -185,11 +185,12 @@ void EKFLocalizer::setCurrentResult() tf2::Matrix3x3(q_tf).getRPY(roll, pitch, yaw_tmp); } double yaw = ekf_.getXelement(IDX::YAW) + ekf_.getXelement(IDX::YAWB); - current_ekf_pose_.pose.orientation = autoware_utils::createQuaternionFromRPY(roll, pitch, yaw); + current_ekf_pose_.pose.orientation = + tier4_autoware_utils::createQuaternionFromRPY(roll, pitch, yaw); current_ekf_pose_no_yawbias_ = current_ekf_pose_; current_ekf_pose_no_yawbias_.pose.orientation = - autoware_utils::createQuaternionFromRPY(roll, pitch, ekf_.getXelement(IDX::YAW)); + tier4_autoware_utils::createQuaternionFromRPY(roll, pitch, ekf_.getXelement(IDX::YAW)); current_ekf_twist_.header.frame_id = "base_link"; current_ekf_twist_.header.stamp = this->now(); @@ -705,9 +706,9 @@ void EKFLocalizer::publishEstimateResult() tier4_debug_msgs::msg::Float64MultiArrayStamped msg; msg.stamp = current_time; - msg.data.push_back(autoware_utils::rad2deg(X(IDX::YAW))); // [0] ekf yaw angle - msg.data.push_back(autoware_utils::rad2deg(pose_yaw)); // [1] measurement yaw angle - msg.data.push_back(autoware_utils::rad2deg(X(IDX::YAWB))); // [2] yaw bias + msg.data.push_back(tier4_autoware_utils::rad2deg(X(IDX::YAW))); // [0] ekf yaw angle + msg.data.push_back(tier4_autoware_utils::rad2deg(pose_yaw)); // [1] measurement yaw angle + msg.data.push_back(tier4_autoware_utils::rad2deg(X(IDX::YAWB))); // [2] yaw bias pub_debug_->publish(msg); } diff --git a/localization/initial_pose_button_panel/plugins/plugin_description.xml b/localization/initial_pose_button_panel/plugins/plugin_description.xml index f872e68d4f50b..eea08b60d03e1 100644 --- a/localization/initial_pose_button_panel/plugins/plugin_description.xml +++ b/localization/initial_pose_button_panel/plugins/plugin_description.xml @@ -1,6 +1,6 @@ - initial button. diff --git a/localization/initial_pose_button_panel/src/initial_pose_button_panel.cpp b/localization/initial_pose_button_panel/src/initial_pose_button_panel.cpp index 680699c9f876d..1de861edc2a58 100644 --- a/localization/initial_pose_button_panel/src/initial_pose_button_panel.cpp +++ b/localization/initial_pose_button_panel/src/initial_pose_button_panel.cpp @@ -27,7 +27,7 @@ #include #include -namespace autoware_localization_rviz_plugin +namespace tier4_localization_rviz_plugin { InitialPoseButtonPanel::InitialPoseButtonPanel(QWidget * parent) : rviz_common::Panel(parent) { @@ -119,7 +119,6 @@ void InitialPoseButtonPanel::pushInitializeButton() thread.detach(); } -} // end namespace autoware_localization_rviz_plugin +} // end namespace tier4_localization_rviz_plugin -PLUGINLIB_EXPORT_CLASS( - autoware_localization_rviz_plugin::InitialPoseButtonPanel, rviz_common::Panel) +PLUGINLIB_EXPORT_CLASS(tier4_localization_rviz_plugin::InitialPoseButtonPanel, rviz_common::Panel) diff --git a/localization/initial_pose_button_panel/src/initial_pose_button_panel.hpp b/localization/initial_pose_button_panel/src/initial_pose_button_panel.hpp index 18f36c5bf9e5c..44defe637b7df 100644 --- a/localization/initial_pose_button_panel/src/initial_pose_button_panel.hpp +++ b/localization/initial_pose_button_panel/src/initial_pose_button_panel.hpp @@ -32,7 +32,7 @@ #include #include -namespace autoware_localization_rviz_plugin +namespace tier4_localization_rviz_plugin { class InitialPoseButtonPanel : public rviz_common::Panel { @@ -60,6 +60,6 @@ public Q_SLOTS: geometry_msgs::msg::PoseWithCovarianceStamped pose_cov_msg_; }; -} // end namespace autoware_localization_rviz_plugin +} // end namespace tier4_localization_rviz_plugin #endif // INITIAL_POSE_BUTTON_PANEL_HPP_ diff --git a/localization/ndt_scan_matcher/package.xml b/localization/ndt_scan_matcher/package.xml index 5460ecd0cacd0..c83659f337958 100644 --- a/localization/ndt_scan_matcher/package.xml +++ b/localization/ndt_scan_matcher/package.xml @@ -7,7 +7,6 @@ Apache License 2.0 ament_cmake_auto - autoware_utils diagnostic_msgs geometry_msgs nav_msgs @@ -22,6 +21,7 @@ tf2_geometry_msgs tf2_ros tf2_sensor_msgs + tier4_autoware_utils tier4_debug_msgs tier4_localization_msgs visualization_msgs diff --git a/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp b/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp index 6d8e8e6d8549f..87330e81578d7 100644 --- a/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp +++ b/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp @@ -19,8 +19,8 @@ #include "ndt_scan_matcher/particle.hpp" #include "ndt_scan_matcher/util_func.hpp" -#include -#include +#include +#include #include @@ -55,8 +55,8 @@ geometry_msgs::msg::TransformStamped identityTransformStamped( transform.header.stamp = timestamp; transform.header.frame_id = header_frame_id; transform.child_frame_id = child_frame_id; - transform.transform.rotation = autoware_utils::createQuaternion(0.0, 0.0, 0.0, 1.0); - transform.transform.translation = autoware_utils::createTranslation(0.0, 0.0, 0.0); + transform.transform.rotation = tier4_autoware_utils::createQuaternion(0.0, 0.0, 0.0, 1.0); + transform.transform.translation = tier4_autoware_utils::createTranslation(0.0, 0.0, 0.0); return transform; } @@ -543,7 +543,7 @@ void NDTScanMatcher::callbackSensorPoints( marker.header.frame_id = map_frame_; marker.type = visualization_msgs::msg::Marker::ARROW; marker.action = visualization_msgs::msg::Marker::ADD; - marker.scale = autoware_utils::createMarkerScale(0.3, 0.1, 0.1); + marker.scale = tier4_autoware_utils::createMarkerScale(0.3, 0.1, 0.1); int i = 0; marker.ns = "result_pose_matrix_array"; marker.action = visualization_msgs::msg::Marker::ADD; @@ -627,7 +627,7 @@ geometry_msgs::msg::PoseWithCovarianceStamped NDTScanMatcher::alignUsingMonteCar Particle particle(initial_pose, result_pose, transform_probability, num_iteration); particle_array.push_back(particle); const auto marker_array = makeDebugMarkers( - this->now(), map_frame_, autoware_utils::createMarkerScale(0.3, 0.1, 0.1), particle, i); + this->now(), map_frame_, tier4_autoware_utils::createMarkerScale(0.3, 0.1, 0.1), particle, i); ndt_monte_carlo_initial_pose_marker_pub_->publish(marker_array); auto sensor_points_mapTF_ptr = std::make_shared>(); @@ -656,7 +656,7 @@ geometry_msgs::msg::PoseWithCovarianceStamped NDTScanMatcher::alignUsingMonteCar void NDTScanMatcher::publishTF( const std::string & child_frame_id, const geometry_msgs::msg::PoseStamped & pose_msg) { - tf2_broadcaster_.sendTransform(autoware_utils::pose2transform(pose_msg, child_frame_id)); + tf2_broadcaster_.sendTransform(tier4_autoware_utils::pose2transform(pose_msg, child_frame_id)); } bool NDTScanMatcher::getTransform( diff --git a/localization/pose_initializer/include/pose_initializer/pose_initializer_core.hpp b/localization/pose_initializer/include/pose_initializer/pose_initializer_core.hpp index 22a570dc0f72e..4a25963bc911e 100644 --- a/localization/pose_initializer/include/pose_initializer/pose_initializer_core.hpp +++ b/localization/pose_initializer/include/pose_initializer/pose_initializer_core.hpp @@ -15,8 +15,8 @@ #ifndef POSE_INITIALIZER__POSE_INITIALIZER_CORE_HPP_ #define POSE_INITIALIZER__POSE_INITIALIZER_CORE_HPP_ -#include #include +#include #include #include diff --git a/localization/pose_initializer/package.xml b/localization/pose_initializer/package.xml index a534e91c263a1..1b3ab3d1276e6 100644 --- a/localization/pose_initializer/package.xml +++ b/localization/pose_initializer/package.xml @@ -8,7 +8,6 @@ Apache License 2.0 ament_cmake - autoware_api_utils geometry_msgs libpcl-all-dev pcl_conversions @@ -18,6 +17,7 @@ tf2 tf2_geometry_msgs tf2_ros + tier4_api_utils tier4_external_api_msgs tier4_localization_msgs diff --git a/localization/pose_initializer/src/pose_initializer_core.cpp b/localization/pose_initializer/src/pose_initializer_core.cpp index 4d12f09bb661f..21258dea6ead0 100644 --- a/localization/pose_initializer/src/pose_initializer_core.cpp +++ b/localization/pose_initializer/src/pose_initializer_core.cpp @@ -162,7 +162,7 @@ void PoseInitializer::serviceInitializePoseAuto( { RCLCPP_INFO(this->get_logger(), "Called Pose Initialize Service"); enable_gnss_callback_ = true; - res->status = autoware_api_utils::response_success(); + res->status = tier4_api_utils::response_success(); } void PoseInitializer::callbackPoseInitializationRequest( diff --git a/map/map_loader/package.xml b/map/map_loader/package.xml index c91e16847dff7..d15a2ab1d6b07 100644 --- a/map/map_loader/package.xml +++ b/map/map_loader/package.xml @@ -11,7 +11,6 @@ ament_cmake_auto autoware_auto_mapping_msgs - autoware_utils geometry_msgs lanelet2_extension libpcl-all-dev @@ -21,6 +20,7 @@ std_msgs tf2_geometry_msgs tf2_ros + tier4_autoware_utils visualization_msgs ament_lint_auto diff --git a/messages/autoware_iv_auto_msgs_converter/CMakeLists.txt b/messages/tier4_auto_msgs_converter/CMakeLists.txt similarity index 92% rename from messages/autoware_iv_auto_msgs_converter/CMakeLists.txt rename to messages/tier4_auto_msgs_converter/CMakeLists.txt index e689dc13de47f..6c4c8bbd768c8 100644 --- a/messages/autoware_iv_auto_msgs_converter/CMakeLists.txt +++ b/messages/tier4_auto_msgs_converter/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.5) -project(autoware_iv_auto_msgs_converter) +project(tier4_auto_msgs_converter) if(NOT CMAKE_CXX_STANDARD) set(CMAKE_CXX_STANDARD 17) diff --git a/messages/autoware_iv_auto_msgs_converter/include/autoware_iv_auto_msgs_converter/autoware_iv_auto_msgs_converter.hpp b/messages/tier4_auto_msgs_converter/include/tier4_auto_msgs_converter/tier4_auto_msgs_converter.hpp similarity index 97% rename from messages/autoware_iv_auto_msgs_converter/include/autoware_iv_auto_msgs_converter/autoware_iv_auto_msgs_converter.hpp rename to messages/tier4_auto_msgs_converter/include/tier4_auto_msgs_converter/tier4_auto_msgs_converter.hpp index 3f0f482a51762..422623d368535 100644 --- a/messages/autoware_iv_auto_msgs_converter/include/autoware_iv_auto_msgs_converter/autoware_iv_auto_msgs_converter.hpp +++ b/messages/tier4_auto_msgs_converter/include/tier4_auto_msgs_converter/tier4_auto_msgs_converter.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE_IV_AUTO_MSGS_CONVERTER__AUTOWARE_IV_AUTO_MSGS_CONVERTER_HPP_ -#define AUTOWARE_IV_AUTO_MSGS_CONVERTER__AUTOWARE_IV_AUTO_MSGS_CONVERTER_HPP_ +#ifndef TIER4_AUTO_MSGS_CONVERTER__TIER4_AUTO_MSGS_CONVERTER_HPP_ +#define TIER4_AUTO_MSGS_CONVERTER__TIER4_AUTO_MSGS_CONVERTER_HPP_ #include "autoware_auto_planning_msgs/msg/path.hpp" #include "autoware_auto_planning_msgs/msg/trajectory.hpp" @@ -38,7 +38,7 @@ #include "tier4_vehicle_msgs/msg/steering.hpp" #include "tier4_vehicle_msgs/msg/turn_signal.hpp" -namespace autoware_iv_auto_msgs_converter +namespace tier4_auto_msgs_converter { struct LightSignal { @@ -276,6 +276,6 @@ inline auto convert(const autoware_auto_vehicle_msgs::msg::SteeringReport & stee return iv_steering; } -} // namespace autoware_iv_auto_msgs_converter +} // namespace tier4_auto_msgs_converter -#endif // AUTOWARE_IV_AUTO_MSGS_CONVERTER__AUTOWARE_IV_AUTO_MSGS_CONVERTER_HPP_ +#endif // TIER4_AUTO_MSGS_CONVERTER__TIER4_AUTO_MSGS_CONVERTER_HPP_ diff --git a/messages/autoware_iv_auto_msgs_converter/package.xml b/messages/tier4_auto_msgs_converter/package.xml similarity index 85% rename from messages/autoware_iv_auto_msgs_converter/package.xml rename to messages/tier4_auto_msgs_converter/package.xml index ccd9b6102565a..5058c429189e2 100644 --- a/messages/autoware_iv_auto_msgs_converter/package.xml +++ b/messages/tier4_auto_msgs_converter/package.xml @@ -2,9 +2,9 @@ - autoware_iv_auto_msgs_converter + tier4_auto_msgs_converter 0.0.0 - The autoware_iv_auto_msgs_converter package + The tier4_auto_msgs_converter package Takagi, Isamu Apache License 2.0 diff --git a/perception/detection_by_tracker/include/detection_by_tracker/detection_by_tracker_core.hpp b/perception/detection_by_tracker/include/detection_by_tracker/detection_by_tracker_core.hpp index 9c14d8725fb11..02500e2f3652a 100644 --- a/perception/detection_by_tracker/include/detection_by_tracker/detection_by_tracker_core.hpp +++ b/perception/detection_by_tracker/include/detection_by_tracker/detection_by_tracker_core.hpp @@ -15,12 +15,12 @@ #ifndef DETECTION_BY_TRACKER__DETECTION_BY_TRACKER_CORE_HPP_ #define DETECTION_BY_TRACKER__DETECTION_BY_TRACKER_CORE_HPP_ -#include #include #include #include #include #include +#include #include #include diff --git a/perception/detection_by_tracker/include/detection_by_tracker/utils.hpp b/perception/detection_by_tracker/include/detection_by_tracker/utils.hpp index 1205c050f8921..63c669c3dcbd2 100644 --- a/perception/detection_by_tracker/include/detection_by_tracker/utils.hpp +++ b/perception/detection_by_tracker/include/detection_by_tracker/utils.hpp @@ -15,7 +15,7 @@ #ifndef DETECTION_BY_TRACKER__UTILS_HPP_ #define DETECTION_BY_TRACKER__UTILS_HPP_ -#include +#include #include #include diff --git a/perception/detection_by_tracker/package.xml b/perception/detection_by_tracker/package.xml index ccf214344c7ff..0c969ba0077af 100644 --- a/perception/detection_by_tracker/package.xml +++ b/perception/detection_by_tracker/package.xml @@ -9,7 +9,6 @@ ament_cmake_auto eigen3_cmake_module - autoware_utils eigen euclidean_cluster libpcl-all-dev @@ -19,6 +18,7 @@ shape_estimation tf2 tf2_ros + tier4_autoware_utils tier4_perception_msgs ament_lint_auto diff --git a/perception/detection_by_tracker/src/detection_by_tracker_core.cpp b/perception/detection_by_tracker/src/detection_by_tracker_core.cpp index af35883a759d0..390734e93cedd 100644 --- a/perception/detection_by_tracker/src/detection_by_tracker_core.cpp +++ b/perception/detection_by_tracker/src/detection_by_tracker_core.cpp @@ -177,9 +177,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_utils::normalizeRadian(yaw + wz * dt.seconds()); + const float yaw_hat = tier4_autoware_utils::normalizeRadian(yaw + wz * dt.seconds()); estimated_object.kinematics.pose_with_covariance.pose.orientation = - autoware_utils::createQuaternionFromYaw(yaw_hat); + tier4_autoware_utils::createQuaternionFromYaw(yaw_hat); output.objects.push_back(estimated_object); } return true; @@ -274,7 +274,7 @@ void DetectionByTracker::divideUnderSegmentedObjects( for (const auto & initial_object : in_cluster_objects.feature_objects) { // search near object - const float distance = autoware_utils::calcDistance2d( + const float distance = tier4_autoware_utils::calcDistance2d( tracked_object.kinematics.pose_with_covariance.pose, initial_object.object.kinematics.pose_with_covariance.pose); if (max_search_range < distance) { @@ -397,7 +397,7 @@ void DetectionByTracker::mergeOverSegmentedObjects( pcl::PointCloud pcl_merged_cluster; for (const auto & initial_object : in_cluster_objects.feature_objects) { - const float distance = autoware_utils::calcDistance2d( + const float distance = tier4_autoware_utils::calcDistance2d( tracked_object.kinematics.pose_with_covariance.pose, initial_object.object.kinematics.pose_with_covariance.pose); diff --git a/perception/detection_by_tracker/src/utils.cpp b/perception/detection_by_tracker/src/utils.cpp index a4a015ad846ac..6e8ab272d67f5 100644 --- a/perception/detection_by_tracker/src/utils.cpp +++ b/perception/detection_by_tracker/src/utils.cpp @@ -26,9 +26,9 @@ namespace utils { void toPolygon2d( const autoware_auto_perception_msgs::msg::DetectedObject & object, - autoware_utils::Polygon2d & output); -bool isClockWise(const autoware_utils::Polygon2d & polygon); -autoware_utils::Polygon2d inverseClockWise(const autoware_utils::Polygon2d & polygon); + tier4_autoware_utils::Polygon2d & output); +bool isClockWise(const tier4_autoware_utils::Polygon2d & polygon); +tier4_autoware_utils::Polygon2d inverseClockWise(const tier4_autoware_utils::Polygon2d & polygon); double getArea(const autoware_auto_perception_msgs::msg::Shape & shape) { @@ -70,12 +70,12 @@ double get2dIoU( const autoware_auto_perception_msgs::msg::DetectedObject & object1, const autoware_auto_perception_msgs::msg::DetectedObject & object2) { - autoware_utils::Polygon2d polygon1, polygon2; + tier4_autoware_utils::Polygon2d polygon1, polygon2; toPolygon2d(object1, polygon1); toPolygon2d(object2, polygon2); - std::vector union_polygons; - std::vector intersection_polygons; + std::vector union_polygons; + std::vector intersection_polygons; boost::geometry::union_(polygon1, polygon2, union_polygons); boost::geometry::intersection(polygon1, polygon2, intersection_polygons); @@ -95,11 +95,11 @@ double get2dPrecision( const autoware_auto_perception_msgs::msg::DetectedObject & source_object, const autoware_auto_perception_msgs::msg::DetectedObject & target_object) { - autoware_utils::Polygon2d source_polygon, target_polygon; + tier4_autoware_utils::Polygon2d source_polygon, target_polygon; toPolygon2d(source_object, source_polygon); toPolygon2d(target_object, target_polygon); - std::vector intersection_polygons; + std::vector intersection_polygons; boost::geometry::intersection(source_polygon, target_polygon, intersection_polygons); double intersection_area = 0.0; @@ -116,11 +116,11 @@ double get2dRecall( const autoware_auto_perception_msgs::msg::DetectedObject & source_object, const autoware_auto_perception_msgs::msg::DetectedObject & target_object) { - autoware_utils::Polygon2d source_polygon, target_polygon; + tier4_autoware_utils::Polygon2d source_polygon, target_polygon; toPolygon2d(source_object, source_polygon); toPolygon2d(target_object, target_polygon); - std::vector intersection_polygons; + std::vector intersection_polygons; boost::geometry::union_(source_polygon, target_polygon, intersection_polygons); double intersection_area = 0.0; @@ -133,16 +133,16 @@ double get2dRecall( return recall; } -autoware_utils::Polygon2d inverseClockWise(const autoware_utils::Polygon2d & polygon) +tier4_autoware_utils::Polygon2d inverseClockWise(const tier4_autoware_utils::Polygon2d & polygon) { - autoware_utils::Polygon2d inverted_polygon; + tier4_autoware_utils::Polygon2d inverted_polygon; for (int i = polygon.outer().size() - 1; 0 <= i; --i) { inverted_polygon.outer().push_back(polygon.outer().at(i)); } return inverted_polygon; } -bool isClockWise(const autoware_utils::Polygon2d & polygon) +bool isClockWise(const tier4_autoware_utils::Polygon2d & polygon) { const int n = polygon.outer().size(); const double x_offset = polygon.outer().at(0).x(); @@ -159,11 +159,11 @@ bool isClockWise(const autoware_utils::Polygon2d & polygon) void toPolygon2d( const autoware_auto_perception_msgs::msg::DetectedObject & object, - autoware_utils::Polygon2d & output) + tier4_autoware_utils::Polygon2d & output) { if (object.shape.type == autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { const auto & pose = object.kinematics.pose_with_covariance.pose; - const double yaw = autoware_utils::normalizeRadian(tf2::getYaw(pose.orientation)); + const double yaw = tier4_autoware_utils::normalizeRadian(tf2::getYaw(pose.orientation)); Eigen::Matrix2d rotation; rotation << std::cos(yaw), -std::sin(yaw), std::sin(yaw), std::cos(yaw); Eigen::Vector2d offset0, offset1, offset2, offset3; @@ -175,13 +175,13 @@ void toPolygon2d( Eigen::Vector2d(-object.shape.dimensions.x * 0.5f, -object.shape.dimensions.y * 0.5f); offset3 = rotation * Eigen::Vector2d(-object.shape.dimensions.x * 0.5f, object.shape.dimensions.y * 0.5f); - output.outer().push_back(boost::geometry::make( + output.outer().push_back(boost::geometry::make( pose.position.x + offset0.x(), pose.position.y + offset0.y())); - output.outer().push_back(boost::geometry::make( + output.outer().push_back(boost::geometry::make( pose.position.x + offset1.x(), pose.position.y + offset1.y())); - output.outer().push_back(boost::geometry::make( + output.outer().push_back(boost::geometry::make( pose.position.x + offset2.x(), pose.position.y + offset2.y())); - output.outer().push_back(boost::geometry::make( + output.outer().push_back(boost::geometry::make( pose.position.x + offset3.x(), pose.position.y + offset3.y())); output.outer().push_back(output.outer().front()); } else if (object.shape.type == autoware_auto_perception_msgs::msg::Shape::CYLINDER) { @@ -201,13 +201,13 @@ void toPolygon2d( radius + center.y; output.outer().push_back( - boost::geometry::make(point.x(), point.y())); + boost::geometry::make(point.x(), point.y())); } output.outer().push_back(output.outer().front()); } else if (object.shape.type == autoware_auto_perception_msgs::msg::Shape::POLYGON) { const auto & pose = object.kinematics.pose_with_covariance.pose; for (const auto & point : object.shape.footprint.points) { - output.outer().push_back(boost::geometry::make( + output.outer().push_back(boost::geometry::make( pose.position.x + point.x, pose.position.y + point.y)); } output.outer().push_back(output.outer().front()); diff --git a/perception/elevation_map_loader/include/elevation_map_loader/elevation_map_loader_node.hpp b/perception/elevation_map_loader/include/elevation_map_loader/elevation_map_loader_node.hpp index 2ec23ddba66f5..81bfca84441ec 100644 --- a/perception/elevation_map_loader/include/elevation_map_loader/elevation_map_loader_node.hpp +++ b/perception/elevation_map_loader/include/elevation_map_loader/elevation_map_loader_node.hpp @@ -15,7 +15,6 @@ #ifndef MAP_LOADER__ELEVATION_MAP_LOADER_NODE_HPP_ #define MAP_LOADER__ELEVATION_MAP_LOADER_NODE_HPP_ -#include #include #include #include @@ -23,6 +22,7 @@ #include #include #include +#include #include "tier4_external_api_msgs/msg/map_hash.hpp" #include @@ -74,10 +74,10 @@ class ElevationMapLoaderNode : public rclcpp::Node void createElevationMap(); void setVerbosityLevelToDebugIfFlagSet(); void createElevationMapFromPointcloud(); - autoware_utils::LinearRing2d getConvexHull( + tier4_autoware_utils::LinearRing2d getConvexHull( const pcl::PointCloud::Ptr & input_cloud); lanelet::ConstLanelets getIntersectedLanelets( - const autoware_utils::LinearRing2d & convex_hull, + const tier4_autoware_utils::LinearRing2d & convex_hull, const lanelet::ConstLanelets & road_lanelets_); pcl::PointCloud::Ptr getLaneFilteredPointCloud( const lanelet::ConstLanelets & joint_lanelets, diff --git a/perception/elevation_map_loader/package.xml b/perception/elevation_map_loader/package.xml index d2dc9108e50c8..b60113c41c26f 100644 --- a/perception/elevation_map_loader/package.xml +++ b/perception/elevation_map_loader/package.xml @@ -11,7 +11,6 @@ ament_cmake_auto autoware_auto_mapping_msgs - autoware_utils grid_map_cv grid_map_pcl grid_map_ros @@ -22,6 +21,7 @@ rclcpp_components tf2_geometry_msgs tf2_ros + tier4_autoware_utils tier4_external_api_msgs ament_lint_auto diff --git a/perception/elevation_map_loader/src/elevation_map_loader_node.cpp b/perception/elevation_map_loader/src/elevation_map_loader_node.cpp index 5f7d855cac6c7..cac07dab64fa7 100644 --- a/perception/elevation_map_loader/src/elevation_map_loader_node.cpp +++ b/perception/elevation_map_loader/src/elevation_map_loader_node.cpp @@ -218,7 +218,7 @@ void ElevationMapLoaderNode::inpaintElevationMap(const float radius) elevation_map_.erase("inpaint_mask"); } -autoware_utils::LinearRing2d ElevationMapLoaderNode::getConvexHull( +tier4_autoware_utils::LinearRing2d ElevationMapLoaderNode::getConvexHull( const pcl::PointCloud::Ptr & input_cloud) { // downsample pointcloud to reduce convex hull calculation cost @@ -229,19 +229,20 @@ autoware_utils::LinearRing2d ElevationMapLoaderNode::getConvexHull( filter.setLeafSize(0.5, 0.5, 100.0); filter.filter(*downsampled_cloud); - autoware_utils::MultiPoint2d candidate_points; + tier4_autoware_utils::MultiPoint2d candidate_points; for (const auto & p : downsampled_cloud->points) { candidate_points.emplace_back(p.x, p.y); } - autoware_utils::LinearRing2d convex_hull; + tier4_autoware_utils::LinearRing2d convex_hull; boost::geometry::convex_hull(candidate_points, convex_hull); return convex_hull; } lanelet::ConstLanelets ElevationMapLoaderNode::getIntersectedLanelets( - const autoware_utils::LinearRing2d & convex_hull, const lanelet::ConstLanelets & road_lanelets) + const tier4_autoware_utils::LinearRing2d & convex_hull, + const lanelet::ConstLanelets & road_lanelets) { lanelet::ConstLanelets intersected_lanelets; for (const auto & road_lanelet : road_lanelets) { @@ -255,7 +256,7 @@ lanelet::ConstLanelets ElevationMapLoaderNode::getIntersectedLanelets( bool ElevationMapLoaderNode::checkPointWithinLanelets( const pcl::PointXYZ & point, const lanelet::ConstLanelets & intersected_lanelets) { - autoware_utils::Point2d point2d(point.x, point.y); + tier4_autoware_utils::Point2d point2d(point.x, point.y); for (const auto & lanelet : intersected_lanelets) { if (lane_filter_.lane_margin_ > 0) { if ( diff --git a/perception/ground_segmentation/src/scan_ground_filter_nodelet.cpp b/perception/ground_segmentation/src/scan_ground_filter_nodelet.cpp index 49e9bf0d8e509..20deb7a785c0f 100644 --- a/perception/ground_segmentation/src/scan_ground_filter_nodelet.cpp +++ b/perception/ground_segmentation/src/scan_ground_filter_nodelet.cpp @@ -14,10 +14,10 @@ #include "ground_segmentation/scan_ground_filter_nodelet.hpp" -#include -#include -#include #include +#include +#include +#include #include #include @@ -26,10 +26,10 @@ namespace ground_segmentation { -using autoware_utils::calcDistance3d; -using autoware_utils::deg2rad; -using autoware_utils::normalizeRadian; using pointcloud_preprocessor::get_param; +using tier4_autoware_utils::calcDistance3d; +using tier4_autoware_utils::deg2rad; +using tier4_autoware_utils::normalizeRadian; using vehicle_info_util::VehicleInfoUtil; ScanGroundFilterComponent::ScanGroundFilterComponent(const rclcpp::NodeOptions & options) diff --git a/perception/lidar_centerpoint/lib/include/centerpoint_trt.hpp b/perception/lidar_centerpoint/lib/include/centerpoint_trt.hpp index fab250e8059f1..13e1a42bec1ce 100644 --- a/perception/lidar_centerpoint/lib/include/centerpoint_trt.hpp +++ b/perception/lidar_centerpoint/lib/include/centerpoint_trt.hpp @@ -15,10 +15,10 @@ #ifndef CENTERPOINT_TRT_HPP_ #define CENTERPOINT_TRT_HPP_ -#include #include #include #include +#include #include #include diff --git a/perception/lidar_centerpoint/lib/src/centerpoint_trt.cpp b/perception/lidar_centerpoint/lib/src/centerpoint_trt.cpp index 374e890d5c3f1..768fd6b4ed041 100644 --- a/perception/lidar_centerpoint/lib/src/centerpoint_trt.cpp +++ b/perception/lidar_centerpoint/lib/src/centerpoint_trt.cpp @@ -12,9 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include #include #include +#include #include #include @@ -275,7 +275,7 @@ at::Tensor CenterPointTRT::generatePredictedBoxes() Config::pointcloud_range_ymin; dim_poi = torch::exp(dim_poi); at::Tensor rot = torch::atan2(rot_poi.slice(2, 0, 1), rot_poi.slice(2, 1, 2)); - rot = -rot - autoware_utils::pi / 2; + rot = -rot - tier4_autoware_utils::pi / 2; at::Tensor boxes3d = torch::cat( diff --git a/perception/lidar_centerpoint/package.xml b/perception/lidar_centerpoint/package.xml index 23009ea141acf..3f1293183af9c 100644 --- a/perception/lidar_centerpoint/package.xml +++ b/perception/lidar_centerpoint/package.xml @@ -11,11 +11,11 @@ ament_cmake_auto autoware_auto_perception_msgs - autoware_utils pcl_ros rclcpp rclcpp_components tf2_geometry_msgs + tier4_autoware_utils ament_lint_auto autoware_lint_common diff --git a/perception/lidar_centerpoint/src/node.cpp b/perception/lidar_centerpoint/src/node.cpp index d3b2328dbb6d9..d1a7deb198d43 100644 --- a/perception/lidar_centerpoint/src/node.cpp +++ b/perception/lidar_centerpoint/src/node.cpp @@ -14,9 +14,9 @@ #include "lidar_centerpoint/node.hpp" -#include #include #include +#include #include @@ -117,11 +117,11 @@ void LidarCenterPointNode::pointCloudCallback( obj.classification.emplace_back(classification); - obj.kinematics.pose_with_covariance.pose.position = autoware_utils::createPoint(x, y, z); + obj.kinematics.pose_with_covariance.pose.position = tier4_autoware_utils::createPoint(x, y, z); obj.kinematics.pose_with_covariance.pose.orientation = - autoware_utils::createQuaternionFromYaw(yaw); + tier4_autoware_utils::createQuaternionFromYaw(yaw); obj.shape.type = autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX; - obj.shape.dimensions = autoware_utils::createTranslation(l, w, h); + obj.shape.dimensions = tier4_autoware_utils::createTranslation(l, w, h); geometry_msgs::msg::Twist twist; twist.linear.x = std::sqrt(std::pow(vel_x, 2) + std::pow(vel_y, 2)); diff --git a/perception/map_based_prediction/package.xml b/perception/map_based_prediction/package.xml index b5e7f9ad941cf..2a2dd0690ee76 100644 --- a/perception/map_based_prediction/package.xml +++ b/perception/map_based_prediction/package.xml @@ -11,13 +11,13 @@ ament_cmake autoware_auto_perception_msgs - autoware_utils lanelet2_extension rclcpp rclcpp_components tf2 tf2_geometry_msgs tf2_ros + tier4_autoware_utils unique_identifier_msgs ament_lint_auto diff --git a/perception/map_based_prediction/src/map_based_prediction.cpp b/perception/map_based_prediction/src/map_based_prediction.cpp index 34e7b748faa3f..a816f780c5214 100644 --- a/perception/map_based_prediction/src/map_based_prediction.cpp +++ b/perception/map_based_prediction/src/map_based_prediction.cpp @@ -12,9 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include #include #include +#include #include @@ -43,7 +43,7 @@ bool MapBasedPrediction::doPrediction( std::vector path = object_with_lanes.lanes.at(path_id); for (size_t i = 0; i < path.size(); i++) { if (i > 0) { - double dist = autoware_utils::calcDistance2d(path.at(i), path.at(i - 1)); + double dist = tier4_autoware_utils::calcDistance2d(path.at(i), path.at(i - 1)); if (dist < interpolating_resolution_) { continue; } @@ -73,18 +73,18 @@ bool MapBasedPrediction::doPrediction( object_with_lanes.object.kinematics.pose_with_covariance.pose.position; const size_t nearest_segment_idx = - autoware_utils::findNearestSegmentIndex(interpolated_points, object_point); - const double l = autoware_utils::calcLongitudinalOffsetToSegment( + tier4_autoware_utils::findNearestSegmentIndex(interpolated_points, object_point); + const double l = tier4_autoware_utils::calcLongitudinalOffsetToSegment( interpolated_points, nearest_segment_idx, object_point); const double current_s_position = - autoware_utils::calcSignedArcLength(interpolated_points, 0, nearest_segment_idx) + l; + tier4_autoware_utils::calcSignedArcLength(interpolated_points, 0, nearest_segment_idx) + l; if (current_s_position > spline2d.s.back()) { continue; } std::array s_point = spline2d.calc_position(current_s_position); geometry_msgs::msg::Point nearest_point = - autoware_utils::createPoint(s_point[0], s_point[1], object_point.z); - double current_d_position = autoware_utils::calcDistance2d(nearest_point, object_point); + tier4_autoware_utils::createPoint(s_point[0], s_point[1], object_point.z); + double current_d_position = tier4_autoware_utils::calcDistance2d(nearest_point, object_point); const double lane_yaw = spline2d.calc_yaw(current_s_position); const std::vector origin_v = {std::cos(lane_yaw), std::sin(lane_yaw)}; const std::vector object_v = { @@ -114,7 +114,7 @@ bool MapBasedPrediction::doPrediction( for (const auto & prev_path : tmp_object.kinematics.predicted_paths) { const auto prev_path_end = prev_path.path.back().position; const auto current_path_end = predicted_path.path.back().position; - const double dist = autoware_utils::calcDistance2d(prev_path_end, current_path_end); + const double dist = tier4_autoware_utils::calcDistance2d(prev_path_end, current_path_end); if (dist < CLOSE_PATH_THRESHOLD) { duplicate_flag = true; break; diff --git a/perception/map_based_prediction/src/map_based_prediction_ros.cpp b/perception/map_based_prediction/src/map_based_prediction_ros.cpp index d77603e70f2a0..f4cfdc32dae3a 100644 --- a/perception/map_based_prediction/src/map_based_prediction_ros.cpp +++ b/perception/map_based_prediction/src/map_based_prediction_ros.cpp @@ -16,8 +16,8 @@ #include "map_based_prediction.hpp" -#include #include +#include #include @@ -122,27 +122,27 @@ double MapBasedPredictionROS::calculateLikelihood( { // We compute the confidence value based on the object current position and angle // Calculate path length - const double path_len = autoware_utils::calcArcLength(path); - const size_t nearest_segment_idx = autoware_utils::findNearestSegmentIndex( + const double path_len = tier4_autoware_utils::calcArcLength(path); + const size_t nearest_segment_idx = tier4_autoware_utils::findNearestSegmentIndex( path, object.kinematics.pose_with_covariance.pose.position); - const double l = autoware_utils::calcLongitudinalOffsetToSegment( + const double l = tier4_autoware_utils::calcLongitudinalOffsetToSegment( path, nearest_segment_idx, object.kinematics.pose_with_covariance.pose.position); const double current_s_position = - autoware_utils::calcSignedArcLength(path, 0, nearest_segment_idx) + l; + tier4_autoware_utils::calcSignedArcLength(path, 0, nearest_segment_idx) + l; // If the obstacle is ahead of this path, we assume the confidence for this path is 0 if (current_s_position > path_len) { return 0.0; } // Euclid Lateral Distance - const double abs_d = std::fabs( - autoware_utils::calcLateralOffset(path, object.kinematics.pose_with_covariance.pose.position)); + const double abs_d = std::fabs(tier4_autoware_utils::calcLateralOffset( + path, object.kinematics.pose_with_covariance.pose.position)); // Yaw Difference between obstacle and lane angle const double lane_yaw = tf2::getYaw(path.at(nearest_segment_idx).orientation); const double object_yaw = getObjectYaw(object); const double delta_yaw = object_yaw - lane_yaw; - const double abs_norm_delta_yaw = std::fabs(autoware_utils::normalizeRadian(delta_yaw)); + const double abs_norm_delta_yaw = std::fabs(tier4_autoware_utils::normalizeRadian(delta_yaw)); // Compute Chi-squared distributed (Equation (8) in the paper) const double sigma_d = sigma_lateral_offset_; // Standard Deviation for lateral position @@ -339,7 +339,8 @@ bool MapBasedPredictionROS::updateObjectBuffer( single_object_data.future_possible_lanelets = current_lanelets; single_object_data.pose = current_object_pose; const double object_yaw = getObjectYaw(object); - single_object_data.pose.pose.orientation = autoware_utils::createQuaternionFromYaw(object_yaw); + single_object_data.pose.pose.orientation = + tier4_autoware_utils::createQuaternionFromYaw(object_yaw); std::deque object_data; object_data.push_back(single_object_data); @@ -355,7 +356,8 @@ bool MapBasedPredictionROS::updateObjectBuffer( single_object_data.future_possible_lanelets = current_lanelets; single_object_data.pose = current_object_pose; const double object_yaw = getObjectYaw(object); - single_object_data.pose.pose.orientation = autoware_utils::createQuaternionFromYaw(object_yaw); + single_object_data.pose.pose.orientation = + tier4_autoware_utils::createQuaternionFromYaw(object_yaw); // push new object data object_data.push_back(single_object_data); @@ -417,10 +419,10 @@ double MapBasedPredictionROS::calcRightLateralOffset( for (size_t i = 0; i < bound_path.size(); ++i) { const double x = bound_line[i].x(); const double y = bound_line[i].y(); - bound_path[i] = autoware_utils::createPoint(x, y, 0.0); + bound_path[i] = tier4_autoware_utils::createPoint(x, y, 0.0); } - return std::fabs(autoware_utils::calcLateralOffset(bound_path, search_pose.position)); + return std::fabs(tier4_autoware_utils::calcLateralOffset(bound_path, search_pose.position)); } double MapBasedPredictionROS::calcLeftLateralOffset( @@ -468,7 +470,7 @@ Maneuver MapBasedPredictionROS::detectLaneChange( for (const auto & lanelet : prev_lanelets) { const double lane_yaw = lanelet::utils::getLaneletAngle(lanelet, prev_pose.pose.position); const double delta_yaw = tf2::getYaw(prev_pose.pose.orientation) - lane_yaw; - const double normalized_delta_yaw = autoware_utils::normalizeRadian(delta_yaw); + const double normalized_delta_yaw = tier4_autoware_utils::normalizeRadian(delta_yaw); if (normalized_delta_yaw < closest_prev_yaw) { closest_prev_yaw = normalized_delta_yaw; prev_lanelet = lanelet; @@ -477,7 +479,7 @@ Maneuver MapBasedPredictionROS::detectLaneChange( // Step4. Check if the vehicle has changed lane const auto current_pose = object.kinematics.pose_with_covariance.pose; - const double dist = autoware_utils::calcDistance2d(prev_pose, current_pose); + const double dist = tier4_autoware_utils::calcDistance2d(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; @@ -686,7 +688,7 @@ void MapBasedPredictionROS::objectsCallback( // Prevent from inserting same points if (!tmp_path.empty()) { const auto prev_pose = tmp_path.back(); - const double tmp_dist = autoware_utils::calcDistance2d(prev_pose, tmp_pose); + const double tmp_dist = tier4_autoware_utils::calcDistance2d(prev_pose, tmp_pose); if (tmp_dist < 1e-6) { continue; } diff --git a/perception/multi_object_tracker/include/multi_object_tracker/utils/utils.hpp b/perception/multi_object_tracker/include/multi_object_tracker/utils/utils.hpp index 95f81275e3e97..00ba413700901 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/utils/utils.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/utils/utils.hpp @@ -19,7 +19,7 @@ #ifndef MULTI_OBJECT_TRACKER__UTILS__UTILS_HPP_ #define MULTI_OBJECT_TRACKER__UTILS__UTILS_HPP_ -#include +#include #include #include diff --git a/perception/multi_object_tracker/package.xml b/perception/multi_object_tracker/package.xml index bff742a43c3aa..4ea5a10166b23 100644 --- a/perception/multi_object_tracker/package.xml +++ b/perception/multi_object_tracker/package.xml @@ -12,7 +12,6 @@ eigen3_cmake_module autoware_auto_perception_msgs - autoware_utils eigen kalman_filter mussp @@ -20,6 +19,7 @@ rclcpp_components tf2 tf2_ros + tier4_autoware_utils tier4_perception_msgs unique_identifier_msgs diff --git a/perception/multi_object_tracker/src/data_association/data_association.cpp b/perception/multi_object_tracker/src/data_association/data_association.cpp index 3f3c523c73afa..42923a59c7f0b 100644 --- a/perception/multi_object_tracker/src/data_association/data_association.cpp +++ b/perception/multi_object_tracker/src/data_association/data_association.cpp @@ -59,8 +59,9 @@ 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_utils::normalizeRadian(tf2::getYaw(measurement_quat)); - const double tracker_yaw = autoware_utils::normalizeRadian(tf2::getYaw(tracker_quat)); + const double measurement_yaw = + tier4_autoware_utils::normalizeRadian(tf2::getYaw(measurement_quat)); + const double tracker_yaw = tier4_autoware_utils::normalizeRadian(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) diff --git a/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp index 5d34e995f2f27..a48a2ef0b7e15 100644 --- a/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp @@ -20,7 +20,7 @@ #include "multi_object_tracker/utils/utils.hpp" -#include +#include #include #include @@ -43,19 +43,19 @@ BicycleTracker::BicycleTracker( // initialize params ekf_params_.use_measurement_covariance = false; - float q_stddev_x = 0.0; // [m/s] - float q_stddev_y = 0.0; // [m/s] - float q_stddev_yaw = autoware_utils::deg2rad(10); // [rad/s] - float q_stddev_vx = autoware_utils::kmph2mps(10); // [m/(s*s)] - float q_stddev_wz = autoware_utils::deg2rad(25); // [rad/(s*s)] - float r_stddev_x = 0.6; // [m] - float r_stddev_y = 0.4; // [m] - float r_stddev_yaw = autoware_utils::deg2rad(30); // [rad] - float p0_stddev_x = 0.8; // [m/s] - float p0_stddev_y = 0.5; // [m/s] - float p0_stddev_yaw = autoware_utils::deg2rad(1000); // [rad/s] - float p0_stddev_vx = autoware_utils::kmph2mps(1000); // [m/(s*s)] - float p0_stddev_wz = autoware_utils::deg2rad(10); // [rad/(s*s)] + float q_stddev_x = 0.0; // [m/s] + float q_stddev_y = 0.0; // [m/s] + float q_stddev_yaw = tier4_autoware_utils::deg2rad(10); // [rad/s] + float q_stddev_vx = tier4_autoware_utils::kmph2mps(10); // [m/(s*s)] + float q_stddev_wz = tier4_autoware_utils::deg2rad(25); // [rad/(s*s)] + float r_stddev_x = 0.6; // [m] + float r_stddev_y = 0.4; // [m] + float r_stddev_yaw = tier4_autoware_utils::deg2rad(30); // [rad] + float p0_stddev_x = 0.8; // [m/s] + float p0_stddev_y = 0.5; // [m/s] + float p0_stddev_yaw = tier4_autoware_utils::deg2rad(1000); // [rad/s] + float p0_stddev_vx = tier4_autoware_utils::kmph2mps(1000); // [m/(s*s)] + float p0_stddev_wz = tier4_autoware_utils::deg2rad(10); // [rad/(s*s)] ekf_params_.q_cov_x = std::pow(q_stddev_x, 2.0); ekf_params_.q_cov_y = std::pow(q_stddev_y, 2.0); ekf_params_.q_cov_yaw = std::pow(q_stddev_yaw, 2.0); @@ -69,8 +69,8 @@ BicycleTracker::BicycleTracker( ekf_params_.p0_cov_yaw = std::pow(p0_stddev_yaw, 2.0); ekf_params_.p0_cov_vx = std::pow(p0_stddev_vx, 2.0); ekf_params_.p0_cov_wz = std::pow(p0_stddev_wz, 2.0); - max_vx_ = autoware_utils::kmph2mps(100); // [m/s] - max_wz_ = autoware_utils::deg2rad(30); // [rad/s] + max_vx_ = tier4_autoware_utils::kmph2mps(100); // [m/s] + max_wz_ = tier4_autoware_utils::deg2rad(30); // [rad/s] // initialize X matrix Eigen::MatrixXd X(ekf_params_.dim_x, 1); @@ -215,7 +215,7 @@ bool BicycleTracker::measureWithPose( { constexpr int dim_y = 2; // pos x, pos y, depending on Pose output // double measurement_yaw = - // autoware_utils::normalizeRadian(tf2::getYaw(object.state.pose_covariance.pose.orientation)); + // tier4_autoware_utils::normalizeRadian(tf2::getYaw(object.state.pose_covariance.pose.orientation)); // { // Eigen::MatrixXd X_t(ekf_params_.dim_x, 1); // ekf_.getX(X_t); @@ -228,7 +228,7 @@ bool BicycleTracker::measureWithPose( // float theta = std::acos( // std::cos(X_t(IDX::YAW)) * std::cos(measurement_yaw) + // std::sin(X_t(IDX::YAW)) * std::sin(measurement_yaw)); - // if (autoware_utils::deg2rad(60) < std::fabs(theta)) return false; + // if (tier4_autoware_utils::deg2rad(60) < std::fabs(theta)) return false; // } /* Set measurement matrix */ @@ -274,7 +274,7 @@ bool BicycleTracker::measureWithPose( Eigen::MatrixXd P_t(ekf_params_.dim_x, ekf_params_.dim_x); ekf_.getX(X_t); ekf_.getP(P_t); - X_t(IDX::YAW) = autoware_utils::normalizeRadian(X_t(IDX::YAW)); + X_t(IDX::YAW) = tier4_autoware_utils::normalizeRadian(X_t(IDX::YAW)); if (!(-max_vx_ <= X_t(IDX::VX) && X_t(IDX::VX) <= max_vx_)) { X_t(IDX::VX) = X_t(IDX::VX) < 0 ? -max_vx_ : max_vx_; } diff --git a/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp index 19cee1613a2c5..0c58c83ae722d 100644 --- a/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp @@ -28,7 +28,7 @@ #include #include -#include +#include BigVehicleTracker::BigVehicleTracker( const rclcpp::Time & time, const autoware_auto_perception_msgs::msg::DetectedObject & object) @@ -41,19 +41,19 @@ BigVehicleTracker::BigVehicleTracker( // initialize params ekf_params_.use_measurement_covariance = false; - float q_stddev_x = 0.0; // [m/s] - float q_stddev_y = 0.0; // [m/s] - float q_stddev_yaw = autoware_utils::deg2rad(20); // [rad/s] - float q_stddev_vx = autoware_utils::kmph2mps(10); // [m/(s*s)] - float q_stddev_wz = autoware_utils::deg2rad(20); // [rad/(s*s)] - float r_stddev_x = 1.5; // [m] - float r_stddev_y = 0.5; // [m] - float r_stddev_yaw = autoware_utils::deg2rad(30); // [rad] - float p0_stddev_x = 1.5; // [m] - float p0_stddev_y = 0.5; // [m] - float p0_stddev_yaw = autoware_utils::deg2rad(30); // [rad] - float p0_stddev_vx = autoware_utils::kmph2mps(1000); // [m/s] - float p0_stddev_wz = autoware_utils::deg2rad(10); // [rad/s] + float q_stddev_x = 0.0; // [m/s] + float q_stddev_y = 0.0; // [m/s] + float q_stddev_yaw = tier4_autoware_utils::deg2rad(20); // [rad/s] + float q_stddev_vx = tier4_autoware_utils::kmph2mps(10); // [m/(s*s)] + float q_stddev_wz = tier4_autoware_utils::deg2rad(20); // [rad/(s*s)] + float r_stddev_x = 1.5; // [m] + float r_stddev_y = 0.5; // [m] + float r_stddev_yaw = tier4_autoware_utils::deg2rad(30); // [rad] + float p0_stddev_x = 1.5; // [m] + float p0_stddev_y = 0.5; // [m] + float p0_stddev_yaw = tier4_autoware_utils::deg2rad(30); // [rad] + float p0_stddev_vx = tier4_autoware_utils::kmph2mps(1000); // [m/s] + float p0_stddev_wz = tier4_autoware_utils::deg2rad(10); // [rad/s] ekf_params_.q_cov_x = std::pow(q_stddev_x, 2.0); ekf_params_.q_cov_y = std::pow(q_stddev_y, 2.0); ekf_params_.q_cov_yaw = std::pow(q_stddev_yaw, 2.0); @@ -67,8 +67,8 @@ BigVehicleTracker::BigVehicleTracker( ekf_params_.p0_cov_yaw = std::pow(p0_stddev_yaw, 2.0); ekf_params_.p0_cov_vx = std::pow(p0_stddev_vx, 2.0); ekf_params_.p0_cov_wz = std::pow(p0_stddev_wz, 2.0); - max_vx_ = autoware_utils::kmph2mps(100); // [m/s] - max_wz_ = autoware_utils::deg2rad(30); // [rad/s] + max_vx_ = tier4_autoware_utils::kmph2mps(100); // [m/s] + max_wz_ = tier4_autoware_utils::deg2rad(30); // [rad/s] // initialize X matrix Eigen::MatrixXd X(ekf_params_.dim_x, 1); @@ -231,7 +231,7 @@ bool BigVehicleTracker::measureWithPose( } constexpr int dim_y = 3; // pos x, pos y, yaw, depending on Pose output - double measurement_yaw = autoware_utils::normalizeRadian( + double measurement_yaw = tier4_autoware_utils::normalizeRadian( tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation)); { Eigen::MatrixXd X_t(ekf_params_.dim_x, 1); @@ -292,7 +292,7 @@ bool BigVehicleTracker::measureWithPose( Eigen::MatrixXd P_t(ekf_params_.dim_x, ekf_params_.dim_x); ekf_.getX(X_t); ekf_.getP(P_t); - X_t(IDX::YAW) = autoware_utils::normalizeRadian(X_t(IDX::YAW)); + X_t(IDX::YAW) = tier4_autoware_utils::normalizeRadian(X_t(IDX::YAW)); if (!(-max_vx_ <= X_t(IDX::VX) && X_t(IDX::VX) <= max_vx_)) { X_t(IDX::VX) = X_t(IDX::VX) < 0 ? -max_vx_ : max_vx_; } diff --git a/perception/multi_object_tracker/src/tracker/model/multiple_vehicle_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/multiple_vehicle_tracker.cpp index 1444ede87bf38..eac68ce185b34 100644 --- a/perception/multi_object_tracker/src/tracker/model/multiple_vehicle_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/multiple_vehicle_tracker.cpp @@ -18,7 +18,7 @@ #include "multi_object_tracker/tracker/model/multiple_vehicle_tracker.hpp" -#include +#include MultipleVehicleTracker::MultipleVehicleTracker( const rclcpp::Time & time, const autoware_auto_perception_msgs::msg::DetectedObject & object) diff --git a/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp index 907a28c9b5582..818912aa7b6b8 100644 --- a/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp @@ -28,7 +28,7 @@ #include #include -#include +#include NormalVehicleTracker::NormalVehicleTracker( const rclcpp::Time & time, const autoware_auto_perception_msgs::msg::DetectedObject & object) @@ -41,19 +41,19 @@ NormalVehicleTracker::NormalVehicleTracker( // initialize params ekf_params_.use_measurement_covariance = false; - float q_stddev_x = 0.0; // object coordinate [m/s] - float q_stddev_y = 0.0; // object coordinate [m/s] - float q_stddev_yaw = autoware_utils::deg2rad(20); // map coordinate[rad/s] - float q_stddev_vx = autoware_utils::kmph2mps(10); // object coordinate [m/(s*s)] - float q_stddev_wz = autoware_utils::deg2rad(20); // object coordinate [rad/(s*s)] - float r_stddev_x = 1.0; // object coordinate [m] - float r_stddev_y = 0.3; // object coordinate [m] - float r_stddev_yaw = autoware_utils::deg2rad(30); // map coordinate [rad] - float p0_stddev_x = 1.0; // object coordinate [m/s] - float p0_stddev_y = 0.3; // object coordinate [m/s] - float p0_stddev_yaw = autoware_utils::deg2rad(30); // map coordinate [rad] - float p0_stddev_vx = autoware_utils::kmph2mps(1000); // object coordinate [m/s] - float p0_stddev_wz = autoware_utils::deg2rad(10); // object coordinate [rad/s] + float q_stddev_x = 0.0; // object coordinate [m/s] + float q_stddev_y = 0.0; // object coordinate [m/s] + float q_stddev_yaw = tier4_autoware_utils::deg2rad(20); // map coordinate[rad/s] + float q_stddev_vx = tier4_autoware_utils::kmph2mps(10); // object coordinate [m/(s*s)] + float q_stddev_wz = tier4_autoware_utils::deg2rad(20); // object coordinate [rad/(s*s)] + float r_stddev_x = 1.0; // object coordinate [m] + float r_stddev_y = 0.3; // object coordinate [m] + float r_stddev_yaw = tier4_autoware_utils::deg2rad(30); // map coordinate [rad] + float p0_stddev_x = 1.0; // object coordinate [m/s] + float p0_stddev_y = 0.3; // object coordinate [m/s] + float p0_stddev_yaw = tier4_autoware_utils::deg2rad(30); // map coordinate [rad] + float p0_stddev_vx = tier4_autoware_utils::kmph2mps(1000); // object coordinate [m/s] + float p0_stddev_wz = tier4_autoware_utils::deg2rad(10); // object coordinate [rad/s] ekf_params_.q_cov_x = std::pow(q_stddev_x, 2.0); ekf_params_.q_cov_y = std::pow(q_stddev_y, 2.0); ekf_params_.q_cov_yaw = std::pow(q_stddev_yaw, 2.0); @@ -67,8 +67,8 @@ NormalVehicleTracker::NormalVehicleTracker( ekf_params_.p0_cov_yaw = std::pow(p0_stddev_yaw, 2.0); ekf_params_.p0_cov_vx = std::pow(p0_stddev_vx, 2.0); ekf_params_.p0_cov_wz = std::pow(p0_stddev_wz, 2.0); - max_vx_ = autoware_utils::kmph2mps(100); // [m/s] - max_wz_ = autoware_utils::deg2rad(30); // [rad/s] + max_vx_ = tier4_autoware_utils::kmph2mps(100); // [m/s] + max_wz_ = tier4_autoware_utils::deg2rad(30); // [rad/s] // initialize X matrix Eigen::MatrixXd X(ekf_params_.dim_x, 1); @@ -231,7 +231,7 @@ bool NormalVehicleTracker::measureWithPose( } constexpr int dim_y = 3; // pos x, pos y, yaw, depending on Pose output - double measurement_yaw = autoware_utils::normalizeRadian( + double measurement_yaw = tier4_autoware_utils::normalizeRadian( tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation)); { Eigen::MatrixXd X_t(ekf_params_.dim_x, 1); @@ -292,7 +292,7 @@ bool NormalVehicleTracker::measureWithPose( Eigen::MatrixXd P_t(ekf_params_.dim_x, ekf_params_.dim_x); ekf_.getX(X_t); ekf_.getP(P_t); - X_t(IDX::YAW) = autoware_utils::normalizeRadian(X_t(IDX::YAW)); + X_t(IDX::YAW) = tier4_autoware_utils::normalizeRadian(X_t(IDX::YAW)); if (!(-max_vx_ <= X_t(IDX::VX) && X_t(IDX::VX) <= max_vx_)) { X_t(IDX::VX) = X_t(IDX::VX) < 0 ? -max_vx_ : max_vx_; } diff --git a/perception/multi_object_tracker/src/tracker/model/pedestrian_and_bicycle_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/pedestrian_and_bicycle_tracker.cpp index 74d1d254bdbb8..496408aea2993 100644 --- a/perception/multi_object_tracker/src/tracker/model/pedestrian_and_bicycle_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/pedestrian_and_bicycle_tracker.cpp @@ -18,7 +18,7 @@ #include "multi_object_tracker/tracker/model/pedestrian_and_bicycle_tracker.hpp" -#include +#include PedestrianAndBicycleTracker::PedestrianAndBicycleTracker( const rclcpp::Time & time, const autoware_auto_perception_msgs::msg::DetectedObject & object) diff --git a/perception/multi_object_tracker/src/tracker/model/pedestrian_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/pedestrian_tracker.cpp index f69ac9426e974..5b198dfb1235e 100644 --- a/perception/multi_object_tracker/src/tracker/model/pedestrian_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/pedestrian_tracker.cpp @@ -20,7 +20,7 @@ #include "multi_object_tracker/utils/utils.hpp" -#include +#include #include #include @@ -43,19 +43,19 @@ PedestrianTracker::PedestrianTracker( // initialize params ekf_params_.use_measurement_covariance = false; - float q_stddev_x = 0.0; // [m/s] - float q_stddev_y = 0.0; // [m/s] - float q_stddev_yaw = autoware_utils::deg2rad(20); // [rad/s] - float q_stddev_vx = autoware_utils::kmph2mps(5); // [m/(s*s)] - float q_stddev_wz = autoware_utils::deg2rad(20); // [rad/(s*s)] - float r_stddev_x = 0.4; // [m] - float r_stddev_y = 0.4; // [m] - float r_stddev_yaw = autoware_utils::deg2rad(30); // [rad] - float p0_stddev_x = 1.0; // [m/s] - float p0_stddev_y = 1.0; // [m/s] - float p0_stddev_yaw = autoware_utils::deg2rad(1000); // [rad/s] - float p0_stddev_vx = autoware_utils::kmph2mps(5); // [m/(s*s)] - float p0_stddev_wz = autoware_utils::deg2rad(10); // [rad/(s*s)] + float q_stddev_x = 0.0; // [m/s] + float q_stddev_y = 0.0; // [m/s] + float q_stddev_yaw = tier4_autoware_utils::deg2rad(20); // [rad/s] + float q_stddev_vx = tier4_autoware_utils::kmph2mps(5); // [m/(s*s)] + float q_stddev_wz = tier4_autoware_utils::deg2rad(20); // [rad/(s*s)] + float r_stddev_x = 0.4; // [m] + float r_stddev_y = 0.4; // [m] + float r_stddev_yaw = tier4_autoware_utils::deg2rad(30); // [rad] + float p0_stddev_x = 1.0; // [m/s] + float p0_stddev_y = 1.0; // [m/s] + float p0_stddev_yaw = tier4_autoware_utils::deg2rad(1000); // [rad/s] + float p0_stddev_vx = tier4_autoware_utils::kmph2mps(5); // [m/(s*s)] + float p0_stddev_wz = tier4_autoware_utils::deg2rad(10); // [rad/(s*s)] ekf_params_.q_cov_x = std::pow(q_stddev_x, 2.0); ekf_params_.q_cov_y = std::pow(q_stddev_y, 2.0); ekf_params_.q_cov_yaw = std::pow(q_stddev_yaw, 2.0); @@ -69,8 +69,8 @@ PedestrianTracker::PedestrianTracker( ekf_params_.p0_cov_yaw = std::pow(p0_stddev_yaw, 2.0); ekf_params_.p0_cov_vx = std::pow(p0_stddev_vx, 2.0); ekf_params_.p0_cov_wz = std::pow(p0_stddev_wz, 2.0); - max_vx_ = autoware_utils::kmph2mps(10); // [m/s] - max_wz_ = autoware_utils::deg2rad(30); // [rad/s] + max_vx_ = tier4_autoware_utils::kmph2mps(10); // [m/s] + max_wz_ = tier4_autoware_utils::deg2rad(30); // [rad/s] // initialize X matrix Eigen::MatrixXd X(ekf_params_.dim_x, 1); @@ -218,7 +218,7 @@ bool PedestrianTracker::measureWithPose( { constexpr int dim_y = 2; // pos x, pos y depending on Pose output // double measurement_yaw = - // autoware_utils::normalizeRadian(tf2::getYaw(object.state.pose_covariance.pose.orientation)); + // tier4_autoware_utils::normalizeRadian(tf2::getYaw(object.state.pose_covariance.pose.orientation)); // { // Eigen::MatrixXd X_t(ekf_params_.dim_x, 1); // ekf_.getX(X_t); @@ -231,7 +231,7 @@ bool PedestrianTracker::measureWithPose( // float theta = std::acos( // std::cos(X_t(IDX::YAW)) * std::cos(measurement_yaw) + // std::sin(X_t(IDX::YAW)) * std::sin(measurement_yaw)); - // if (autoware_utils::deg2rad(60) < std::fabs(theta)) return false; + // if (tier4_autoware_utils::deg2rad(60) < std::fabs(theta)) return false; // } /* Set measurement matrix */ @@ -277,7 +277,7 @@ bool PedestrianTracker::measureWithPose( Eigen::MatrixXd P_t(ekf_params_.dim_x, ekf_params_.dim_x); ekf_.getX(X_t); ekf_.getP(P_t); - X_t(IDX::YAW) = autoware_utils::normalizeRadian(X_t(IDX::YAW)); + X_t(IDX::YAW) = tier4_autoware_utils::normalizeRadian(X_t(IDX::YAW)); if (!(-max_vx_ <= X_t(IDX::VX) && X_t(IDX::VX) <= max_vx_)) { X_t(IDX::VX) = X_t(IDX::VX) < 0 ? -max_vx_ : max_vx_; } diff --git a/perception/multi_object_tracker/src/tracker/model/unknown_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/unknown_tracker.cpp index ebc484b84ef76..a279856052489 100644 --- a/perception/multi_object_tracker/src/tracker/model/unknown_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/unknown_tracker.cpp @@ -23,7 +23,7 @@ #include #include -#include +#include UnknownTracker::UnknownTracker( const rclcpp::Time & time, const autoware_auto_perception_msgs::msg::DetectedObject & object) @@ -36,16 +36,16 @@ UnknownTracker::UnknownTracker( // initialize params ekf_params_.use_measurement_covariance = false; - float q_stddev_x = 0.0; // [m/s] - float q_stddev_y = 0.0; // [m/s] - float q_stddev_vx = autoware_utils::kmph2mps(0.1); // [m/(s*s)] - float q_stddev_vy = autoware_utils::kmph2mps(0.1); // [m/(s*s)] - float r_stddev_x = 0.4; // [m] - float r_stddev_y = 0.4; // [m] - float p0_stddev_x = 1.0; // [m/s] - float p0_stddev_y = 1.0; // [m/s] - float p0_stddev_vx = autoware_utils::kmph2mps(0.1); // [m/(s*s)] - float p0_stddev_vy = autoware_utils::kmph2mps(0.1); // [m/(s*s)] + float q_stddev_x = 0.0; // [m/s] + float q_stddev_y = 0.0; // [m/s] + float q_stddev_vx = tier4_autoware_utils::kmph2mps(0.1); // [m/(s*s)] + float q_stddev_vy = tier4_autoware_utils::kmph2mps(0.1); // [m/(s*s)] + float r_stddev_x = 0.4; // [m] + float r_stddev_y = 0.4; // [m] + float p0_stddev_x = 1.0; // [m/s] + float p0_stddev_y = 1.0; // [m/s] + float p0_stddev_vx = tier4_autoware_utils::kmph2mps(0.1); // [m/(s*s)] + float p0_stddev_vy = tier4_autoware_utils::kmph2mps(0.1); // [m/(s*s)] ekf_params_.q_cov_x = std::pow(q_stddev_x, 2.0); ekf_params_.q_cov_y = std::pow(q_stddev_y, 2.0); ekf_params_.q_cov_vx = std::pow(q_stddev_vx, 2.0); @@ -56,8 +56,8 @@ UnknownTracker::UnknownTracker( ekf_params_.p0_cov_y = std::pow(p0_stddev_y, 2.0); ekf_params_.p0_cov_vx = std::pow(p0_stddev_vx, 2.0); ekf_params_.p0_cov_vy = std::pow(p0_stddev_vy, 2.0); - max_vx_ = autoware_utils::kmph2mps(5); // [m/s] - max_vy_ = autoware_utils::kmph2mps(5); // [m/s] + max_vx_ = tier4_autoware_utils::kmph2mps(5); // [m/s] + max_vy_ = tier4_autoware_utils::kmph2mps(5); // [m/s] // initialize X matrix Eigen::MatrixXd X(ekf_params_.dim_x, 1); diff --git a/perception/multi_object_tracker/src/utils/utils.cpp b/perception/multi_object_tracker/src/utils/utils.cpp index 723262f551136..79dfeb92359ea 100644 --- a/perception/multi_object_tracker/src/utils/utils.cpp +++ b/perception/multi_object_tracker/src/utils/utils.cpp @@ -30,9 +30,9 @@ namespace utils { void toPolygon2d( const autoware_auto_perception_msgs::msg::TrackedObject & object, - autoware_utils::Polygon2d & output); -bool isClockWise(const autoware_utils::Polygon2d & polygon); -autoware_utils::Polygon2d inverseClockWise(const autoware_utils::Polygon2d & polygon); + tier4_autoware_utils::Polygon2d & output); +bool isClockWise(const tier4_autoware_utils::Polygon2d & polygon); +tier4_autoware_utils::Polygon2d inverseClockWise(const tier4_autoware_utils::Polygon2d & polygon); double getArea(const autoware_auto_perception_msgs::msg::Shape & shape) { @@ -74,12 +74,12 @@ double get2dIoU( const autoware_auto_perception_msgs::msg::TrackedObject & object1, const autoware_auto_perception_msgs::msg::TrackedObject & object2) { - autoware_utils::Polygon2d polygon1, polygon2; + tier4_autoware_utils::Polygon2d polygon1, polygon2; toPolygon2d(object1, polygon1); toPolygon2d(object2, polygon2); - std::vector union_polygons; - std::vector intersection_polygons; + std::vector union_polygons; + std::vector intersection_polygons; boost::geometry::union_(polygon1, polygon2, union_polygons); boost::geometry::intersection(polygon1, polygon2, intersection_polygons); @@ -99,16 +99,16 @@ double get2dIoU( return iou; } -autoware_utils::Polygon2d inverseClockWise(const autoware_utils::Polygon2d & polygon) +tier4_autoware_utils::Polygon2d inverseClockWise(const tier4_autoware_utils::Polygon2d & polygon) { - autoware_utils::Polygon2d inverted_polygon; + tier4_autoware_utils::Polygon2d inverted_polygon; for (int i = polygon.outer().size() - 1; 0 <= i; --i) { inverted_polygon.outer().push_back(polygon.outer().at(i)); } return inverted_polygon; } -bool isClockWise(const autoware_utils::Polygon2d & polygon) +bool isClockWise(const tier4_autoware_utils::Polygon2d & polygon) { const int n = polygon.outer().size(); const double x_offset = polygon.outer().at(0).x(); @@ -125,11 +125,11 @@ bool isClockWise(const autoware_utils::Polygon2d & polygon) void toPolygon2d( const autoware_auto_perception_msgs::msg::TrackedObject & object, - autoware_utils::Polygon2d & output) + tier4_autoware_utils::Polygon2d & output) { if (object.shape.type == autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { const auto & pose = object.kinematics.pose_with_covariance.pose; - double yaw = autoware_utils::normalizeRadian(tf2::getYaw(pose.orientation)); + double yaw = tier4_autoware_utils::normalizeRadian(tf2::getYaw(pose.orientation)); Eigen::Matrix2d rotation; rotation << std::cos(yaw), -std::sin(yaw), std::sin(yaw), std::cos(yaw); Eigen::Vector2d offset0, offset1, offset2, offset3; @@ -141,13 +141,13 @@ void toPolygon2d( Eigen::Vector2d(-object.shape.dimensions.x * 0.5f, -object.shape.dimensions.y * 0.5f); offset3 = rotation * Eigen::Vector2d(-object.shape.dimensions.x * 0.5f, object.shape.dimensions.y * 0.5f); - output.outer().push_back(boost::geometry::make( + output.outer().push_back(boost::geometry::make( pose.position.x + offset0.x(), pose.position.y + offset0.y())); - output.outer().push_back(boost::geometry::make( + output.outer().push_back(boost::geometry::make( pose.position.x + offset1.x(), pose.position.y + offset1.y())); - output.outer().push_back(boost::geometry::make( + output.outer().push_back(boost::geometry::make( pose.position.x + offset2.x(), pose.position.y + offset2.y())); - output.outer().push_back(boost::geometry::make( + output.outer().push_back(boost::geometry::make( pose.position.x + offset3.x(), pose.position.y + offset3.y())); output.outer().push_back(output.outer().front()); } else if (object.shape.type == autoware_auto_perception_msgs::msg::Shape::CYLINDER) { @@ -167,17 +167,17 @@ void toPolygon2d( radius + center.y; output.outer().push_back( - boost::geometry::make(point.x(), point.y())); + boost::geometry::make(point.x(), point.y())); } output.outer().push_back(output.outer().front()); } else if (object.shape.type == autoware_auto_perception_msgs::msg::Shape::POLYGON) { const auto & pose = object.kinematics.pose_with_covariance.pose; // don't use yaw - // double yaw = autoware_utils::normalizeRadian(tf2::getYaw(pose.orientation)); + // double yaw = tier4_autoware_utils::normalizeRadian(tf2::getYaw(pose.orientation)); // Eigen::Matrix2d rotation; // rotation << std::cos(yaw), -std::sin(yaw), std::sin(yaw), std::cos(yaw); for (const auto & point : object.shape.footprint.points) { - output.outer().push_back(boost::geometry::make( + output.outer().push_back(boost::geometry::make( pose.position.x + point.x, pose.position.y + point.y)); } output.outer().push_back(output.outer().front()); diff --git a/perception/occupancy_grid_map_outlier_filter/package.xml b/perception/occupancy_grid_map_outlier_filter/package.xml index b420de794de4a..dec4aa9bf5a7e 100644 --- a/perception/occupancy_grid_map_outlier_filter/package.xml +++ b/perception/occupancy_grid_map_outlier_filter/package.xml @@ -18,7 +18,6 @@ ament_cmake_auto autoware_auto_vehicle_msgs - autoware_utils diagnostic_updater image_transport lanelet2_extension @@ -38,6 +37,7 @@ tf2_eigen tf2_geometry_msgs tf2_ros + tier4_autoware_utils tier4_debug_msgs tier4_pcl_extensions diff --git a/perception/occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_nodelet.cpp b/perception/occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_nodelet.cpp index 680ea1614d2c0..8b47cee943138 100644 --- a/perception/occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_nodelet.cpp +++ b/perception/occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_nodelet.cpp @@ -14,8 +14,8 @@ #include "occupancy_grid_map_outlier_filter/occupancy_grid_map_outlier_filter_nodelet.hpp" -#include #include +#include #include @@ -66,7 +66,7 @@ geometry_msgs::msg::PoseStamped getPoseStamped( RCLCPP_WARN_THROTTLE( rclcpp::get_logger("occupancy_grid_map_outlier_filter"), clock, 5000, "%s", ex.what()); } - return autoware_utils::transform2pose(tf_stamped); + return tier4_autoware_utils::transform2pose(tf_stamped); } boost::optional getCost( diff --git a/perception/shape_estimation/include/shape_estimation/corrector/utils.hpp b/perception/shape_estimation/include/shape_estimation/corrector/utils.hpp index dc5184a511f10..637370ece1c41 100644 --- a/perception/shape_estimation/include/shape_estimation/corrector/utils.hpp +++ b/perception/shape_estimation/include/shape_estimation/corrector/utils.hpp @@ -15,7 +15,7 @@ #ifndef SHAPE_ESTIMATION__CORRECTOR__UTILS_HPP_ #define SHAPE_ESTIMATION__CORRECTOR__UTILS_HPP_ -#include +#include #include #include diff --git a/perception/shape_estimation/include/shape_estimation/model/model_interface.hpp b/perception/shape_estimation/include/shape_estimation/model/model_interface.hpp index 142ae64a6a270..7f9baa9a0f5e0 100644 --- a/perception/shape_estimation/include/shape_estimation/model/model_interface.hpp +++ b/perception/shape_estimation/include/shape_estimation/model/model_interface.hpp @@ -15,7 +15,7 @@ #ifndef SHAPE_ESTIMATION__MODEL__MODEL_INTERFACE_HPP_ #define SHAPE_ESTIMATION__MODEL__MODEL_INTERFACE_HPP_ -#include +#include #include #include diff --git a/perception/shape_estimation/lib/corrector/utils.cpp b/perception/shape_estimation/lib/corrector/utils.cpp index d498b15ac1fd6..8829ccb9cf9a8 100644 --- a/perception/shape_estimation/lib/corrector/utils.cpp +++ b/perception/shape_estimation/lib/corrector/utils.cpp @@ -230,9 +230,9 @@ bool correctVehicleBoundingBox( // correct to set long length is x, short length is y if (shape_output.dimensions.x < shape_output.dimensions.y) { - geometry_msgs::msg::Vector3 rpy = autoware_utils::getRPY(pose_output.orientation); + geometry_msgs::msg::Vector3 rpy = tier4_autoware_utils::getRPY(pose_output.orientation); rpy.z = rpy.z + M_PI_2; - pose_output.orientation = autoware_utils::createQuaternionFromRPY(rpy.x, rpy.y, rpy.z); + pose_output.orientation = tier4_autoware_utils::createQuaternionFromRPY(rpy.x, rpy.y, rpy.z); double temp = shape_output.dimensions.x; shape_output.dimensions.x = shape_output.dimensions.y; shape_output.dimensions.y = temp; diff --git a/perception/shape_estimation/lib/model/bounding_box.cpp b/perception/shape_estimation/lib/model/bounding_box.cpp index 5f814f11f8977..c8f6344748bc9 100644 --- a/perception/shape_estimation/lib/model/bounding_box.cpp +++ b/perception/shape_estimation/lib/model/bounding_box.cpp @@ -50,8 +50,8 @@ bool BoundingBoxShapeModel::estimate( { float min_angle, max_angle; if (reference_yaw_) { - min_angle = reference_yaw_.get() - autoware_utils::deg2rad(3); - max_angle = reference_yaw_.get() + autoware_utils::deg2rad(3); + min_angle = reference_yaw_.get() - tier4_autoware_utils::deg2rad(3); + max_angle = reference_yaw_.get() + tier4_autoware_utils::deg2rad(3); } else { min_angle = 0.0; max_angle = M_PI / 2.0; diff --git a/perception/shape_estimation/package.xml b/perception/shape_estimation/package.xml index 9bbf01521475e..0621114bc9bb8 100644 --- a/perception/shape_estimation/package.xml +++ b/perception/shape_estimation/package.xml @@ -11,7 +11,6 @@ ament_cmake autoware_auto_perception_msgs - autoware_utils builtin_interfaces eigen libopencv-dev @@ -22,6 +21,7 @@ tf2 tf2_eigen tf2_geometry_msgs + tier4_autoware_utils tier4_perception_msgs ament_lint_auto diff --git a/perception/traffic_light_map_based_detector/package.xml b/perception/traffic_light_map_based_detector/package.xml index 268f9f19a9e5e..087f29b85399f 100644 --- a/perception/traffic_light_map_based_detector/package.xml +++ b/perception/traffic_light_map_based_detector/package.xml @@ -12,7 +12,6 @@ autoware_auto_mapping_msgs autoware_auto_perception_msgs autoware_auto_planning_msgs - autoware_utils geometry_msgs image_geometry lanelet2_extension @@ -20,6 +19,7 @@ sensor_msgs tf2 tf2_ros + tier4_autoware_utils ament_lint_auto diff --git a/perception/traffic_light_map_based_detector/src/node.cpp b/perception/traffic_light_map_based_detector/src/node.cpp index 39e8d87471073..39af074f7846d 100644 --- a/perception/traffic_light_map_based_detector/src/node.cpp +++ b/perception/traffic_light_map_based_detector/src/node.cpp @@ -32,11 +32,11 @@ #include "traffic_light_map_based_detector/node.hpp" -#include #include #include #include #include +#include #include @@ -139,7 +139,7 @@ void MapBasedDetector::cameraInfoCallback( "map", input_msg->header.frame_id, input_msg->header.stamp, rclcpp::Duration::from_seconds(0.2)); camera_pose_stamped.header = input_msg->header; - camera_pose_stamped.pose = autoware_utils::transform2pose(transform.transform); + camera_pose_stamped.pose = tier4_autoware_utils::transform2pose(transform.transform); } catch (tf2::TransformException & ex) { RCLCPP_WARN_THROTTLE( get_logger(), *get_clock(), 5000, "cannot get transform from map frame to camera frame"); @@ -345,12 +345,12 @@ void MapBasedDetector::getVisibleTrafficLights( } // check angle range - const double tl_yaw = autoware_utils::normalizeRadian( + const double tl_yaw = tier4_autoware_utils::normalizeRadian( std::atan2( tl_right_down_point.y() - tl_left_down_point.y(), tl_right_down_point.x() - tl_left_down_point.x()) + M_PI_2); - constexpr double max_angle_range = autoware_utils::deg2rad(40.0); + constexpr double max_angle_range = tier4_autoware_utils::deg2rad(40.0); // get direction of z axis tf2::Vector3 camera_z_dir(0, 0, 1); @@ -359,7 +359,7 @@ void MapBasedDetector::getVisibleTrafficLights( camera_pose.orientation.w)); 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_utils::normalizeRadian(camera_yaw); + camera_yaw = tier4_autoware_utils::normalizeRadian(camera_yaw); if (!isInAngleRange(tl_yaw, camera_yaw, max_angle_range)) { continue; } diff --git a/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp b/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp index 21220d00b5282..2f9f10f9941f8 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp @@ -25,8 +25,8 @@ #include "behavior_path_planner/scene_module/side_shift/side_shift_module.hpp" #include "behavior_path_planner/turn_signal_decider.hpp" -#include #include +#include #include #include @@ -92,7 +92,7 @@ class BehaviorPathPlannerNode : public rclcpp::Node std::shared_ptr planner_data_; std::shared_ptr bt_manager_; - autoware_utils::SelfPoseListener self_pose_listener_{this}; + tier4_autoware_utils::SelfPoseListener self_pose_listener_{this}; std::string prev_ready_module_name_ = "NONE"; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/path_shifter/path_shifter.hpp b/planning/behavior_path_planner/include/behavior_path_planner/path_shifter/path_shifter.hpp index 737e383812d21..c420e203c0b64 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/path_shifter/path_shifter.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/path_shifter/path_shifter.hpp @@ -17,8 +17,8 @@ #include "behavior_path_planner/parameters.hpp" -#include #include +#include #include #include diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/debug.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/debug.hpp index 1f0d2d5dea9b3..c732e810e4104 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/debug.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/debug.hpp @@ -18,7 +18,7 @@ #include "behavior_path_planner/path_shifter/path_shifter.hpp" #include "behavior_path_planner/scene_module/avoidance/avoidance_module_data.hpp" -#include +#include #include #include diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_out/pull_out_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_out/pull_out_module.hpp index 590d44f29c8cc..d23fc3e759e3d 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_out/pull_out_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_out/pull_out_module.hpp @@ -125,7 +125,7 @@ class PullOutModule : public SceneModuleInterface void updatePullOutStatus(); bool isInLane( const lanelet::ConstLanelet & candidate_lanelet, - const autoware_utils::LinearRing2d & vehicle_footprint) const; + const tier4_autoware_utils::LinearRing2d & vehicle_footprint) const; bool isLongEnough(const lanelet::ConstLanelets & lanelets) const; bool isSafe() const; bool isNearEndOfLane() const; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_out/util.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_out/util.hpp index 132526a4d7245..187073465f44b 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_out/util.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_out/util.hpp @@ -69,13 +69,13 @@ bool selectSafePath( const PredictedObjects::ConstSharedPtr & dynamic_objects, const Pose & current_pose, const Twist & current_twist, const double vehicle_width, const behavior_path_planner::PullOutParameters & ros_parameters, - const autoware_utils::LinearRing2d & vehicle_footprint, PullOutPath * selected_path); + const tier4_autoware_utils::LinearRing2d & vehicle_footprint, PullOutPath * selected_path); bool isPullOutPathSafe( const behavior_path_planner::PullOutPath & path, const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes, const PredictedObjects::ConstSharedPtr & dynamic_objects, const behavior_path_planner::PullOutParameters & ros_parameters, - const autoware_utils::LinearRing2d & vehicle_footprint, const bool use_buffer = true, + const tier4_autoware_utils::LinearRing2d & vehicle_footprint, const bool use_buffer = true, const bool use_dynamic_object = false); bool hasEnoughDistance( const PullOutPath & path, const lanelet::ConstLanelets & current_lanes, diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/pull_over_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/pull_over_module.hpp index 7e1b241757f28..d55ab0e695e5e 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/pull_over_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/pull_over_module.hpp @@ -121,7 +121,7 @@ class PullOverModule : public SceneModuleInterface void updatePullOverStatus(); bool isInLane( const lanelet::ConstLanelet & candidate_lanelet, - const autoware_utils::LinearRing2d & vehicle_footprint) const; + const tier4_autoware_utils::LinearRing2d & vehicle_footprint) const; bool isLongEnough(const lanelet::ConstLanelets & lanelets) const; bool isSafe() const; bool isLaneBlocked(const lanelet::ConstLanelets & lanes) const; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/util/create_vehicle_footprint.hpp b/planning/behavior_path_planner/include/behavior_path_planner/util/create_vehicle_footprint.hpp index cff714659e77a..06f6ac44a1071 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/util/create_vehicle_footprint.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/util/create_vehicle_footprint.hpp @@ -15,14 +15,14 @@ #ifndef BEHAVIOR_PATH_PLANNER__UTIL__CREATE_VEHICLE_FOOTPRINT_HPP_ #define BEHAVIOR_PATH_PLANNER__UTIL__CREATE_VEHICLE_FOOTPRINT_HPP_ -#include +#include #include -inline autoware_utils::LinearRing2d createVehicleFootprint( +inline tier4_autoware_utils::LinearRing2d createVehicleFootprint( const vehicle_info_util::VehicleInfo & vehicle_info, const double margin = 0.0) { - using autoware_utils::LinearRing2d; - using autoware_utils::Point2d; + using tier4_autoware_utils::LinearRing2d; + using tier4_autoware_utils::Point2d; const auto & i = vehicle_info; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utilities.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utilities.hpp index 6e9a1a19e15c9..fa2b671893ce2 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utilities.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utilities.hpp @@ -19,10 +19,10 @@ #include "behavior_path_planner/scene_module/lane_change/lane_change_path.hpp" #include "behavior_path_planner/scene_module/pull_out/pull_out_path.hpp" -#include #include #include #include +#include #include #include @@ -52,7 +52,7 @@ #include #include -namespace autoware_utils +namespace tier4_autoware_utils { template <> inline geometry_msgs::msg::Pose getPose( @@ -60,7 +60,7 @@ inline geometry_msgs::msg::Pose getPose( { return p.point.pose; } -} // namespace autoware_utils +} // namespace tier4_autoware_utils namespace tf2 { @@ -115,9 +115,6 @@ using autoware_auto_perception_msgs::msg::PredictedPath; using autoware_auto_planning_msgs::msg::Path; using autoware_auto_planning_msgs::msg::PathPointWithLaneId; using autoware_auto_planning_msgs::msg::PathWithLaneId; -using autoware_utils::LineString2d; -using autoware_utils::Point2d; -using autoware_utils::Polygon2d; using geometry_msgs::msg::Point; using geometry_msgs::msg::Pose; using geometry_msgs::msg::PoseArray; @@ -126,6 +123,9 @@ using geometry_msgs::msg::Twist; using geometry_msgs::msg::Vector3; using nav_msgs::msg::OccupancyGrid; using route_handler::RouteHandler; +using tier4_autoware_utils::LineString2d; +using tier4_autoware_utils::Point2d; +using tier4_autoware_utils::Polygon2d; struct FrenetCoordinate3d { @@ -204,7 +204,7 @@ double getDistanceBetweenPredictedPathAndObject( double getDistanceBetweenPredictedPathAndObjectPolygon( const PredictedObject & object, const PullOutPath & ego_path, - const autoware_utils::LinearRing2d & vehicle_footprint, double distance_resolution, + const tier4_autoware_utils::LinearRing2d & vehicle_footprint, double distance_resolution, const lanelet::ConstLanelets & road_lanes); /** diff --git a/planning/behavior_path_planner/package.xml b/planning/behavior_path_planner/package.xml index 4561073e96448..03f5b6e865fe5 100644 --- a/planning/behavior_path_planner/package.xml +++ b/planning/behavior_path_planner/package.xml @@ -17,7 +17,6 @@ autoware_auto_perception_msgs autoware_auto_planning_msgs autoware_auto_vehicle_msgs - autoware_utils behaviortree_cpp_v3 geometry_msgs interpolation @@ -31,6 +30,7 @@ tf2 tf2_geometry_msgs tf2_ros + tier4_autoware_utils tier4_planning_msgs vehicle_info_util visualization_msgs diff --git a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp index c2dc95625536d..3ed89165c8ba6 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -22,7 +22,7 @@ #include "behavior_path_planner/scene_module/side_shift/side_shift_module.hpp" #include "behavior_path_planner/utilities.hpp" -#include +#include #include #include @@ -280,7 +280,7 @@ LaneChangeParameters BehaviorPathPlannerNode::getLaneChangeParam() p.use_all_predicted_path = dp("use_all_predicted_path", false); p.abort_lane_change_velocity_thresh = dp("abort_lane_change_velocity_thresh", 0.5); p.abort_lane_change_angle_thresh = - dp("abort_lane_change_angle_thresh", autoware_utils::deg2rad(10.0)); + dp("abort_lane_change_angle_thresh", tier4_autoware_utils::deg2rad(10.0)); p.abort_lane_change_distance_thresh = dp("abort_lane_change_distance_thresh", 0.3); p.enable_blocked_by_obstacle = dp("enable_blocked_by_obstacle", false); p.lane_change_search_distance = dp("lane_change_search_distance", 30.0); @@ -601,7 +601,7 @@ void BehaviorPathPlannerNode::publishDebugMarker(const std::vector { MarkerArray msg{}; for (const auto & markers : debug_markers) { - autoware_utils::appendMarkerArray(markers, &msg); + tier4_autoware_utils::appendMarkerArray(markers, &msg); } debug_marker_publisher_->publish(msg); } diff --git a/planning/behavior_path_planner/src/path_shifter/path_shifter.cpp b/planning/behavior_path_planner/src/path_shifter/path_shifter.cpp index 9272d86ab51b7..780c0842d6617 100644 --- a/planning/behavior_path_planner/src/path_shifter/path_shifter.cpp +++ b/planning/behavior_path_planner/src/path_shifter/path_shifter.cpp @@ -17,9 +17,9 @@ #include "behavior_path_planner/path_utilities.hpp" #include "behavior_path_planner/utilities.hpp" -#include #include #include +#include #include #include @@ -260,7 +260,7 @@ bool PathShifter::calcShiftPointFromArcLength( return false; } - const auto origin_idx = autoware_utils::findNearestIndex(path.points, origin); + const auto origin_idx = tier4_autoware_utils::findNearestIndex(path.points, origin); const auto arclength_from_origin = util::calcPathArcLengthArray(path, origin_idx); if (dist_to_end > arclength_from_origin.back()) { @@ -311,8 +311,8 @@ bool PathShifter::calcShiftPointFromArcLength( void PathShifter::updateShiftPointIndices() { for (auto & p : shift_points_) { - p.start_idx = autoware_utils::findNearestIndex(reference_path_.points, p.start.position); - p.end_idx = autoware_utils::findNearestIndex(reference_path_.points, p.end.position); + p.start_idx = tier4_autoware_utils::findNearestIndex(reference_path_.points, p.start.position); + p.end_idx = tier4_autoware_utils::findNearestIndex(reference_path_.points, p.end.position); } is_index_aligned_ = true; } @@ -381,7 +381,7 @@ bool PathShifter::sortShiftPointsAlongPath([[maybe_unused]] const PathWithLaneId void PathShifter::removeBehindShiftPointAndSetBaseOffset(const Point & base_point) { - const auto base_idx = autoware_utils::findNearestIndex(reference_path_.points, base_point); + const auto base_idx = tier4_autoware_utils::findNearestIndex(reference_path_.points, base_point); // If shift_point.end is behind the base_point, remove the shift_point and // set its shift_length to the base_offset. diff --git a/planning/behavior_path_planner/src/path_utilities.cpp b/planning/behavior_path_planner/src/path_utilities.cpp index f974afc8b6cbb..0d6e0245e69d3 100644 --- a/planning/behavior_path_planner/src/path_utilities.cpp +++ b/planning/behavior_path_planner/src/path_utilities.cpp @@ -16,12 +16,12 @@ #include "behavior_path_planner/utilities.hpp" -#include #include #include #include #include #include +#include #include @@ -49,7 +49,8 @@ std::vector calcPathArcLengthArray( end = std::min(end, path.points.size()); for (size_t i = start; i < end; ++i) { - sum += autoware_utils::calcDistance2d(path.points.at(i).point, path.points.at(i - 1).point); + sum += + tier4_autoware_utils::calcDistance2d(path.points.at(i).point, path.points.at(i - 1).point); out.push_back(sum); } return out; @@ -76,7 +77,8 @@ double calcPathArcLength(const PathWithLaneId & path, size_t start, size_t end) double sum = 0.0; for (size_t i = start; i < end; ++i) { - sum += autoware_utils::calcDistance2d(path.points.at(i).point, path.points.at(i - 1).point); + sum += + tier4_autoware_utils::calcDistance2d(path.points.at(i).point, path.points.at(i - 1).point); } return is_negative_direction ? -sum : sum; @@ -88,7 +90,7 @@ double calcPathArcLength(const PathWithLaneId & path, size_t start, size_t end) PathWithLaneId resamplePathWithSpline(const PathWithLaneId & path, double interval) { const auto base_points = calcPathArcLengthArray(path); - const auto sampling_points = autoware_utils::arange(0.0, base_points.back(), interval); + const auto sampling_points = tier4_autoware_utils::arange(0.0, base_points.back(), interval); if (base_points.empty() || sampling_points.empty()) { return path; @@ -114,7 +116,7 @@ PathWithLaneId resamplePathWithSpline(const PathWithLaneId & path, double interv for (size_t i = 0; i < sampling_points.size(); ++i) { PathPointWithLaneId p{}; p.point.pose.position = - autoware_utils::createPoint(resampled_x.at(i), resampled_y.at(i), resampled_z.at(i)); + tier4_autoware_utils::createPoint(resampled_x.at(i), resampled_y.at(i), resampled_z.at(i)); resampled_path.points.push_back(p); } @@ -153,7 +155,7 @@ PathWithLaneId resamplePathWithSpline(const PathWithLaneId & path, double interv const auto & p1 = resampled_path.points.at(i + 1).point.pose.position; const double yaw = std::atan2(p1.y - p0.y, p1.x - p0.x); resampled_path.points.at(i).point.pose.orientation = - autoware_utils::createQuaternionFromYaw(yaw); + tier4_autoware_utils::createQuaternionFromYaw(yaw); } if (resampled_path.points.size() > 2) { resampled_path.points.back().point.pose.orientation = @@ -181,9 +183,9 @@ size_t getIdxByArclength(const PathWithLaneId & path, const Point & origin, cons throw std::runtime_error("[getIdxByArclength] path points must be > 0"); } - const auto closest_idx = autoware_utils::findNearestIndex(path.points, origin); + const auto closest_idx = tier4_autoware_utils::findNearestIndex(path.points, origin); - using autoware_utils::calcDistance2d; + using tier4_autoware_utils::calcDistance2d; double sum_length = 0.0; if (signed_arc >= 0.0) { for (size_t i = closest_idx; i < path.points.size() - 1; ++i) { diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp index 391b290479b46..3b99a29d69b7a 100644 --- a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp @@ -19,10 +19,10 @@ #include "behavior_path_planner/scene_module/avoidance/debug.hpp" #include "behavior_path_planner/utilities.hpp" -#include #include #include #include +#include #include #include @@ -40,11 +40,11 @@ namespace behavior_path_planner { -using autoware_utils::calcDistance2d; -using autoware_utils::calcLateralDeviation; -using autoware_utils::calcSignedArcLength; -using autoware_utils::createPoint; -using autoware_utils::findNearestIndex; +using tier4_autoware_utils::calcDistance2d; +using tier4_autoware_utils::calcLateralDeviation; +using tier4_autoware_utils::calcSignedArcLength; +using tier4_autoware_utils::createPoint; +using tier4_autoware_utils::findNearestIndex; AvoidanceModule::AvoidanceModule( const std::string & name, rclcpp::Node & node, const AvoidanceParameters & parameters) @@ -573,7 +573,7 @@ AvoidPointArray AvoidanceModule::combineRawShiftPointsWithUniqueCheck( { // TODO(Horibe) parametrize const auto isSimilar = [](const AvoidPoint & a, const AvoidPoint & b) { - using autoware_utils::calcDistance2d; + using tier4_autoware_utils::calcDistance2d; if (calcDistance2d(a.start, b.start) > 1.0) { return false; } @@ -1700,7 +1700,7 @@ PathWithLaneId AvoidanceModule::calcCenterLinePath( // for debug: check if the path backward distance is same as the desired length. // { - // const auto back_to_ego = autoware_utils::calcSignedArcLength( + // const auto back_to_ego = tier4_autoware_utils::calcSignedArcLength( // centerline_path.points, centerline_path.points.front().point.pose.position, // getEgoPosition()); // RCLCPP_INFO(getLogger(), "actual back_to_ego distance = %f", back_to_ego); @@ -2349,7 +2349,7 @@ void AvoidanceModule::setDebugData(const PathShifter & shifter, const DebugData debug_marker_.markers.clear(); const auto add = [this](const MarkerArray & added) { - autoware_utils::appendMarkerArray(added, &debug_marker_); + tier4_autoware_utils::appendMarkerArray(added, &debug_marker_); }; const auto addAvoidPoint = diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_utils.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_utils.cpp index ec6babd992be4..623f0c97200b1 100644 --- a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_utils.cpp +++ b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_utils.cpp @@ -16,9 +16,9 @@ #include "behavior_path_planner/scene_module/avoidance/avoidance_module.hpp" #include "behavior_path_planner/utilities.hpp" -#include #include #include +#include #include #include @@ -128,14 +128,15 @@ void clipByMinStartIdx(const AvoidPointArray & shift_points, PathWithLaneId & pa double calcDistanceToClosestFootprintPoint( const PathWithLaneId & path, const PredictedObject & object, const Point & ego_pos) { - autoware_utils::Polygon2d object_poly{}; + tier4_autoware_utils::Polygon2d object_poly{}; util::calcObjectPolygon(object, &object_poly); - double distance = autoware_utils::calcSignedArcLength( + double distance = tier4_autoware_utils::calcSignedArcLength( path.points, ego_pos, object.kinematics.initial_pose_with_covariance.pose.position); for (const auto & p : object_poly.outer()) { - const auto point = autoware_utils::createPoint(p.x(), p.y(), 0.0); - distance = std::min(distance, autoware_utils::calcSignedArcLength(path.points, ego_pos, point)); + const auto point = tier4_autoware_utils::createPoint(p.x(), p.y(), 0.0); + distance = + std::min(distance, tier4_autoware_utils::calcSignedArcLength(path.points, ego_pos, point)); } return distance; } @@ -144,12 +145,12 @@ double calcOverhangDistance(const ObjectData & object_data, const Pose & base_po { double largest_overhang = isOnRight(object_data) ? -100.0 : 100.0; // large number - autoware_utils::Polygon2d object_poly{}; + tier4_autoware_utils::Polygon2d object_poly{}; util::calcObjectPolygon(object_data.object, &object_poly); for (const auto & p : object_poly.outer()) { - const auto point = autoware_utils::createPoint(p.x(), p.y(), 0.0); - const auto lateral = autoware_utils::calcLateralDeviation(base_pose, point); + const auto point = tier4_autoware_utils::createPoint(p.x(), p.y(), 0.0); + const auto lateral = tier4_autoware_utils::calcLateralDeviation(base_pose, point); largest_overhang = isOnRight(object_data) ? std::max(largest_overhang, lateral) : std::min(largest_overhang, lateral); } diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/debug.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/debug.cpp index c4718eec23458..e3b9eed8859ad 100644 --- a/planning/behavior_path_planner/src/scene_module/avoidance/debug.cpp +++ b/planning/behavior_path_planner/src/scene_module/avoidance/debug.cpp @@ -49,9 +49,9 @@ MarkerArray createShiftLengthMarkerArray( marker.ns = ns; marker.lifetime = rclcpp::Duration::from_seconds(0.2); marker.action = Marker::ADD; - marker.pose.orientation = autoware_utils::createMarkerOrientation(0, 0, 0, 1.0); - marker.scale = autoware_utils::createMarkerScale(0.1, 0.0, 0.0); - marker.color = autoware_utils::createMarkerColor(r, g, b, 0.9); + marker.pose.orientation = tier4_autoware_utils::createMarkerOrientation(0, 0, 0, 1.0); + marker.scale = tier4_autoware_utils::createMarkerScale(0.1, 0.0, 0.0); + marker.color = tier4_autoware_utils::createMarkerColor(r, g, b, 0.9); marker.type = Marker::LINE_STRIP; for (size_t i = 0; i < shift_distance.size(); ++i) { @@ -85,9 +85,9 @@ MarkerArray createAvoidPointMarkerArray( marker.ns = ns; marker.lifetime = rclcpp::Duration::from_seconds(0.2); marker.action = Marker::ADD; - marker.pose.orientation = autoware_utils::createMarkerOrientation(0, 0, 0, 1.0); - marker.scale = autoware_utils::createMarkerScale(0.5, 0.5, 0.5); - marker.color = autoware_utils::createMarkerColor(r, g, b, 0.9); + marker.pose.orientation = tier4_autoware_utils::createMarkerOrientation(0, 0, 0, 1.0); + marker.scale = tier4_autoware_utils::createMarkerScale(0.5, 0.5, 0.5); + marker.color = tier4_autoware_utils::createMarkerColor(r, g, b, 0.9); { marker.type = Marker::CUBE; @@ -110,7 +110,7 @@ MarkerArray createAvoidPointMarkerArray( auto marker_l = marker; marker_l.id = id++; marker_l.type = Marker::LINE_STRIP; - marker_l.scale = autoware_utils::createMarkerScale(w, 0.0, 0.0); + marker_l.scale = tier4_autoware_utils::createMarkerScale(w, 0.0, 0.0); marker_l.points.push_back(marker_s.pose.position); marker_l.points.push_back(marker_e.pose.position); msg.markers.push_back(marker_l); @@ -144,9 +144,9 @@ MarkerArray createShiftPointMarkerArray( marker.ns = ns; marker.lifetime = rclcpp::Duration::from_seconds(0.2); marker.action = Marker::ADD; - marker.pose.orientation = autoware_utils::createMarkerOrientation(0, 0, 0, 1.0); - marker.scale = autoware_utils::createMarkerScale(0.5, 0.5, 0.5); - marker.color = autoware_utils::createMarkerColor(r, g, b, 0.5); + marker.pose.orientation = tier4_autoware_utils::createMarkerOrientation(0, 0, 0, 1.0); + marker.scale = tier4_autoware_utils::createMarkerScale(0.5, 0.5, 0.5); + marker.color = tier4_autoware_utils::createMarkerColor(r, g, b, 0.5); { marker.type = Marker::CUBE; @@ -168,7 +168,7 @@ MarkerArray createShiftPointMarkerArray( auto marker_l = marker; marker_l.id = id++; marker_l.type = Marker::LINE_STRIP; - marker_l.scale = autoware_utils::createMarkerScale(w, 0.0, 0.0); + marker_l.scale = tier4_autoware_utils::createMarkerScale(w, 0.0, 0.0); marker_l.points.push_back(marker_s.pose.position); marker_l.points.push_back(marker_e.pose.position); msg.markers.push_back(marker_l); @@ -196,9 +196,9 @@ MarkerArray createLaneletsAreaMarkerArray( marker.lifetime = rclcpp::Duration::from_seconds(0.2); marker.type = Marker::LINE_STRIP; marker.action = Marker::ADD; - marker.pose.orientation = autoware_utils::createMarkerOrientation(0, 0, 0, 1.0); - marker.scale = autoware_utils::createMarkerScale(0.1, 0.0, 0.0); - marker.color = autoware_utils::createMarkerColor(r, g, b, 0.999); + marker.pose.orientation = tier4_autoware_utils::createMarkerOrientation(0, 0, 0, 1.0); + marker.scale = tier4_autoware_utils::createMarkerScale(0.1, 0.0, 0.0); + marker.color = tier4_autoware_utils::createMarkerColor(r, g, b, 0.999); for (const auto & p : lanelet.polygon3d()) { Point point; point.x = p.x(); @@ -234,9 +234,9 @@ MarkerArray createLaneletPolygonsMarkerArray( marker.lifetime = rclcpp::Duration::from_seconds(0.2); marker.type = Marker::LINE_STRIP; marker.action = Marker::ADD; - marker.pose.orientation = autoware_utils::createMarkerOrientation(0, 0, 0, 1.0); - marker.scale = autoware_utils::createMarkerScale(0.1, 0.0, 0.0); - marker.color = autoware_utils::createMarkerColor(0.0, 1.0, 0.0, 0.999); + marker.pose.orientation = tier4_autoware_utils::createMarkerOrientation(0, 0, 0, 1.0); + marker.scale = tier4_autoware_utils::createMarkerScale(0.1, 0.0, 0.0); + marker.color = tier4_autoware_utils::createMarkerColor(0.0, 1.0, 0.0, 0.999); for (const auto & p : polygon) { Point point; point.x = p.x(); @@ -269,9 +269,9 @@ MarkerArray createPolygonMarkerArray( marker.lifetime = rclcpp::Duration::from_seconds(0.2); marker.type = Marker::LINE_STRIP; marker.action = Marker::ADD; - marker.pose.orientation = autoware_utils::createMarkerOrientation(0, 0, 0, 1.0); - marker.scale = autoware_utils::createMarkerScale(0.3, 0.0, 0.0); - marker.color = autoware_utils::createMarkerColor(r, g, b, 0.8); + marker.pose.orientation = tier4_autoware_utils::createMarkerOrientation(0, 0, 0, 1.0); + marker.scale = tier4_autoware_utils::createMarkerScale(0.3, 0.0, 0.0); + marker.color = tier4_autoware_utils::createMarkerColor(r, g, b, 0.8); for (const auto & p : polygon.points) { Point point; point.x = p.x; @@ -307,8 +307,8 @@ MarkerArray createObjectsMarkerArray( marker.type = Marker::CUBE; marker.action = Marker::ADD; marker.pose = object.kinematics.initial_pose_with_covariance.pose; - marker.scale = autoware_utils::createMarkerScale(3.0, 1.0, 1.0); - marker.color = autoware_utils::createMarkerColor(r, g, b, 0.8); + marker.scale = tier4_autoware_utils::createMarkerScale(3.0, 1.0, 1.0); + marker.color = tier4_autoware_utils::createMarkerColor(r, g, b, 0.8); msg.markers.push_back(marker); } @@ -326,8 +326,8 @@ MarkerArray createAvoidanceObjectsMarkerArray( marker.header.stamp = current_time; marker.ns = ns; - const auto normal_color = autoware_utils::createMarkerColor(0.9, 0.0, 0.0, 0.8); - const auto disappearing_color = autoware_utils::createMarkerColor(0.9, 0.5, 0.9, 0.6); + const auto normal_color = tier4_autoware_utils::createMarkerColor(0.9, 0.0, 0.0, 0.8); + const auto disappearing_color = tier4_autoware_utils::createMarkerColor(0.9, 0.5, 0.9, 0.6); int32_t i = 0; for (const auto & object : objects) { @@ -336,7 +336,7 @@ MarkerArray createAvoidanceObjectsMarkerArray( marker.type = Marker::CUBE; marker.action = Marker::ADD; marker.pose = object.object.kinematics.initial_pose_with_covariance.pose; - marker.scale = autoware_utils::createMarkerScale(3.0, 1.5, 1.5); + marker.scale = tier4_autoware_utils::createMarkerScale(3.0, 1.5, 1.5); marker.color = object.lost_count == 0 ? normal_color : disappearing_color; msg.markers.push_back(marker); } @@ -364,8 +364,8 @@ MarkerArray createPathMarkerArray( marker.type = Marker::ARROW; marker.action = Marker::ADD; marker.pose = p.point.pose; - marker.scale = autoware_utils::createMarkerScale(0.2, 0.1, 0.3); - marker.color = autoware_utils::createMarkerColor(r, g, b, 0.999); + marker.scale = tier4_autoware_utils::createMarkerScale(0.2, 0.1, 0.3); + marker.color = tier4_autoware_utils::createMarkerColor(r, g, b, 0.999); msg.markers.push_back(marker); if (idx % 10 == 0) { auto marker_text = marker; @@ -374,7 +374,7 @@ MarkerArray createPathMarkerArray( std::stringstream ss; ss << std::fixed << std::setprecision(1) << "i=" << idx << "\ns=" << arclength.at(idx); marker_text.text = ss.str(); - marker_text.color = autoware_utils::createMarkerColor(1, 1, 1, 0.999); + marker_text.color = tier4_autoware_utils::createMarkerColor(1, 1, 1, 0.999); msg.markers.push_back(marker_text); } ++idx; @@ -398,8 +398,8 @@ MarkerArray createVirtualWallMarkerArray( marker_virtual_wall.action = Marker::ADD; marker_virtual_wall.pose = pose; marker_virtual_wall.pose.position.z += 1.0; - marker_virtual_wall.scale = autoware_utils::createMarkerScale(0.1, 5.0, 2.0); - marker_virtual_wall.color = autoware_utils::createMarkerColor(1.0, 0.0, 0.0, 0.5); + marker_virtual_wall.scale = tier4_autoware_utils::createMarkerScale(0.1, 5.0, 2.0); + marker_virtual_wall.color = tier4_autoware_utils::createMarkerColor(1.0, 0.0, 0.0, 0.5); msg.markers.push_back(marker_virtual_wall); Marker marker_factor_text{}; @@ -412,8 +412,8 @@ MarkerArray createVirtualWallMarkerArray( marker_factor_text.action = Marker::ADD; marker_factor_text.pose = pose; marker_factor_text.pose.position.z += 2.0; - marker_factor_text.scale = autoware_utils::createMarkerScale(0.0, 0.0, 1.0); - marker_factor_text.color = autoware_utils::createMarkerColor(1.0, 1.0, 1.0, 0.999); + marker_factor_text.scale = tier4_autoware_utils::createMarkerScale(0.0, 0.0, 1.0); + marker_factor_text.color = tier4_autoware_utils::createMarkerColor(1.0, 1.0, 1.0, 0.999); marker_factor_text.text = stop_factor; msg.markers.push_back(marker_factor_text); @@ -435,9 +435,9 @@ MarkerArray createPoseLineMarkerArray( marker_line.lifetime = rclcpp::Duration::from_seconds(0.2); marker_line.type = Marker::LINE_STRIP; marker_line.action = Marker::ADD; - marker_line.pose.orientation = autoware_utils::createMarkerOrientation(0, 0, 0, 1.0); - marker_line.scale = autoware_utils::createMarkerScale(0.1, 0.0, 0.0); - marker_line.color = autoware_utils::createMarkerColor(r, g, b, 0.999); + marker_line.pose.orientation = tier4_autoware_utils::createMarkerOrientation(0, 0, 0, 1.0); + marker_line.scale = tier4_autoware_utils::createMarkerScale(0.1, 0.0, 0.0); + marker_line.color = tier4_autoware_utils::createMarkerColor(r, g, b, 0.999); const double yaw = tf2::getYaw(pose.orientation); @@ -475,8 +475,8 @@ MarkerArray createPoseMarkerArray( marker.type = Marker::ARROW; marker.action = Marker::ADD; marker.pose = pose; - marker.scale = autoware_utils::createMarkerScale(0.7, 0.3, 0.3); - marker.color = autoware_utils::createMarkerColor(r, g, b, 0.999); + marker.scale = tier4_autoware_utils::createMarkerScale(0.7, 0.3, 0.3); + marker.color = tier4_autoware_utils::createMarkerColor(r, g, b, 0.999); msg.markers.push_back(marker); return msg; diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/lane_change_module.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/lane_change_module.cpp index a10eb86ccf7c1..cd563228ec4eb 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/lane_change_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/lane_change_module.cpp @@ -18,9 +18,9 @@ #include "behavior_path_planner/scene_module/lane_change/util.hpp" #include "behavior_path_planner/utilities.hpp" -#include #include #include +#include #include @@ -499,7 +499,7 @@ bool LaneChangeModule::isAbortConditionSatisfied() const const double lane_angle = lanelet::utils::getLaneletAngle(closest_lanelet, current_pose.position); const double vehicle_yaw = tf2::getYaw(current_pose.orientation); - const double yaw_diff = autoware_utils::normalizeRadian(lane_angle - vehicle_yaw); + const double yaw_diff = tier4_autoware_utils::normalizeRadian(lane_angle - vehicle_yaw); is_angle_diff_small = std::abs(yaw_diff) < parameters_.abort_lane_change_angle_thresh; } diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/util.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/util.cpp index 2322aebeb6c14..650b23c5f40da 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/util.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/util.cpp @@ -184,9 +184,9 @@ std::vector getLaneChangePaths( shift_point.length = lane_change_start_on_self_lane_arc.distance; shift_point.start = lane_change_start_on_self_lane; shift_point.end = lane_change_end_on_target_lane; - shift_point.start_idx = autoware_utils::findNearestIndex( + shift_point.start_idx = tier4_autoware_utils::findNearestIndex( target_lane_reference_path.points, lane_change_start_on_self_lane.position); - shift_point.end_idx = autoware_utils::findNearestIndex( + shift_point.end_idx = tier4_autoware_utils::findNearestIndex( target_lane_reference_path.points, lane_change_end_on_target_lane.position); } @@ -202,7 +202,7 @@ std::vector getLaneChangePaths( rclcpp::get_logger("behavior_path_planner").get_child("lane_change").get_child("util"), "failed to generate shifted path."); } - const auto lanechange_end_idx = autoware_utils::findNearestIndex( + const auto lanechange_end_idx = tier4_autoware_utils::findNearestIndex( shifted_path.path.points, reference_path2.points.front().point.pose); if (lanechange_end_idx) { for (size_t i = 0; i < shifted_path.path.points.size(); ++i) { @@ -224,7 +224,7 @@ std::vector getLaneChangePaths( static_cast( std::max(lane_change_distance / lane_changing_duration, minimum_lane_change_velocity))); const auto nearest_idx = - autoware_utils::findNearestIndex(reference_path2.points, point.point.pose); + tier4_autoware_utils::findNearestIndex(reference_path2.points, point.point.pose); point.lane_ids = reference_path2.points.at(*nearest_idx).lane_ids; } diff --git a/planning/behavior_path_planner/src/scene_module/pull_out/pull_out_module.cpp b/planning/behavior_path_planner/src/scene_module/pull_out/pull_out_module.cpp index 3c5e18bcfb5e2..d7afa1e308b27 100644 --- a/planning/behavior_path_planner/src/scene_module/pull_out/pull_out_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/pull_out/pull_out_module.cpp @@ -22,10 +22,10 @@ #include "behavior_path_planner/util/create_vehicle_footprint.hpp" #include "behavior_path_planner/utilities.hpp" -#include #include #include #include +#include #include #include @@ -92,7 +92,8 @@ bool PullOutModule::isExecutionRequested() const const auto vehicle_info = getVehicleInfo(planner_data_->parameters); const auto local_vehicle_footprint = createVehicleFootprint(vehicle_info); const auto vehicle_footprint = transformVector( - local_vehicle_footprint, autoware_utils::pose2transform(planner_data_->self_pose->pose)); + local_vehicle_footprint, + tier4_autoware_utils::pose2transform(planner_data_->self_pose->pose)); const auto road_lanes = getCurrentLanes(); // check if goal pose is in shoulder lane and distance is long enough for pull out @@ -475,7 +476,7 @@ std::pair PullOutModule::getSafeRetreatPath( const auto shoulder_line_path = route_handler->getCenterLinePath( pull_out_lanes, arc_position_pose.length - pull_out_lane_length_, arc_position_pose.length + pull_out_lane_length_); - const auto idx = autoware_utils::findNearestIndex(shoulder_line_path.points, current_pose); + const auto idx = tier4_autoware_utils::findNearestIndex(shoulder_line_path.points, current_pose); const auto yaw_shoulder_lane = tf2::getYaw(shoulder_line_path.points.at(*idx).point.pose.orientation); @@ -552,7 +553,8 @@ bool PullOutModule::getBackDistance( const auto shoulder_line_path = route_handler->getCenterLinePath( pull_out_lanes, arc_position_pose.length - pull_out_lane_length_, arc_position_pose.length + pull_out_lane_length_); - const auto idx = autoware_utils::findNearestIndex(shoulder_line_path.points, current_pose); + const auto idx = + tier4_autoware_utils::findNearestIndex(shoulder_line_path.points, current_pose); yaw_shoulder_lane = tf2::getYaw(shoulder_line_path.points.at(*idx).point.pose.orientation); } @@ -603,7 +605,7 @@ bool PullOutModule::getBackDistance( bool PullOutModule::isInLane( const lanelet::ConstLanelet & candidate_lanelet, - const autoware_utils::LinearRing2d & vehicle_footprint) const + const tier4_autoware_utils::LinearRing2d & vehicle_footprint) const { for (const auto & point : vehicle_footprint) { if (boost::geometry::within(point, candidate_lanelet.polygon2d().basicPolygon())) { @@ -678,7 +680,7 @@ bool PullOutModule::hasFinishedBack() const // check ego car is close enough to goal pose const auto current_pose = planner_data_->self_pose->pose; const auto backed_pose = status_.backed_pose; - const auto distance = autoware_utils::calcDistance2d(current_pose, backed_pose); + const auto distance = tier4_autoware_utils::calcDistance2d(current_pose, backed_pose); return distance < 1; } @@ -690,7 +692,7 @@ TurnSignalInfo PullOutModule::calcTurnSignalInfo(const ShiftPoint & shift_point) if (status_.is_retreat_path_valid && !status_.back_finished) { turn_signal.hazard_signal.command = HazardLightsCommand::ENABLE; turn_signal.signal_distance = - autoware_utils::calcDistance2d(status_.backed_pose, planner_data_->self_pose->pose); + tier4_autoware_utils::calcDistance2d(status_.backed_pose, planner_data_->self_pose->pose); return turn_signal; } diff --git a/planning/behavior_path_planner/src/scene_module/pull_out/util.cpp b/planning/behavior_path_planner/src/scene_module/pull_out/util.cpp index ff956f527d8a3..3e00c75f052bf 100644 --- a/planning/behavior_path_planner/src/scene_module/pull_out/util.cpp +++ b/planning/behavior_path_planner/src/scene_module/pull_out/util.cpp @@ -18,10 +18,10 @@ #include "behavior_path_planner/path_utilities.hpp" #include "behavior_path_planner/util/create_vehicle_footprint.hpp" -#include #include #include #include +#include #include @@ -194,14 +194,14 @@ std::vector getPullOutPaths( continue; } - const auto pull_out_end_idx = autoware_utils::findNearestIndex( + const auto pull_out_end_idx = tier4_autoware_utils::findNearestIndex( shifted_path.path.points, reference_path2.points.front().point.pose); const auto goal_idx = - autoware_utils::findNearestIndex(shifted_path.path.points, route_handler.getGoalPose()); + tier4_autoware_utils::findNearestIndex(shifted_path.path.points, route_handler.getGoalPose()); if (pull_out_end_idx && goal_idx) { - const auto distance_pull_out_end_to_goal = autoware_utils::calcDistance2d( + const auto distance_pull_out_end_to_goal = tier4_autoware_utils::calcDistance2d( shifted_path.path.points.at(*pull_out_end_idx).point.pose, shifted_path.path.points.at(*goal_idx).point.pose); for (size_t i = 0; i < shifted_path.path.points.size(); ++i) { @@ -216,7 +216,7 @@ std::vector getPullOutPaths( continue; } - auto distance_to_goal = autoware_utils::calcDistance2d( + auto distance_to_goal = tier4_autoware_utils::calcDistance2d( point.point.pose, shifted_path.path.points.at(*goal_idx).point.pose); point.point.longitudinal_velocity_mps = std::min( minimum_pull_out_velocity, @@ -340,7 +340,7 @@ bool selectSafePath( const PredictedObjects::ConstSharedPtr & dynamic_objects, [[maybe_unused]] const Pose & current_pose, [[maybe_unused]] const Twist & current_twist, [[maybe_unused]] const double vehicle_width, const PullOutParameters & ros_parameters, - const autoware_utils::LinearRing2d & local_vehicle_footprint, PullOutPath * selected_path) + const tier4_autoware_utils::LinearRing2d & local_vehicle_footprint, PullOutPath * selected_path) { const bool use_dynamic_object = ros_parameters.use_dynamic_object; for (const auto & path : paths) { @@ -399,7 +399,7 @@ bool isPullOutPathSafe( const lanelet::ConstLanelets & shoulder_lanes, const PredictedObjects::ConstSharedPtr & dynamic_objects, const PullOutParameters & ros_parameters, - const autoware_utils::LinearRing2d & local_vehicle_footprint, const bool use_buffer, + const tier4_autoware_utils::LinearRing2d & local_vehicle_footprint, const bool use_buffer, const bool use_dynamic_object) { // TODO(sugahara) check road lanes safety and output road lanes safety diff --git a/planning/behavior_path_planner/src/scene_module/pull_over/pull_over_module.cpp b/planning/behavior_path_planner/src/scene_module/pull_over/pull_over_module.cpp index 43366a8a3ab91..2808e6845a157 100644 --- a/planning/behavior_path_planner/src/scene_module/pull_over/pull_over_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/pull_over/pull_over_module.cpp @@ -21,10 +21,10 @@ #include "behavior_path_planner/scene_module/pull_over/util.hpp" #include "behavior_path_planner/utilities.hpp" -#include #include #include #include +#include #include #include @@ -89,7 +89,7 @@ bool PullOverModule::isExecutionRequested() const const auto lane_yaw = lanelet::utils::getLaneletAngle(closest_shoulder_lanelet, goal_pose.position); const auto goal_yaw = tf2::getYaw(goal_pose.orientation); - const auto angle_diff = autoware_utils::normalizeRadian(lane_yaw - goal_yaw); + const auto angle_diff = tier4_autoware_utils::normalizeRadian(lane_yaw - goal_yaw); constexpr double th_angle = M_PI / 4; if (std::abs(angle_diff) < th_angle) { goal_is_in_shoulder_lane = true; diff --git a/planning/behavior_path_planner/src/scene_module/pull_over/util.cpp b/planning/behavior_path_planner/src/scene_module/pull_over/util.cpp index 702a5ddaa99d1..fa4f55a5c0267 100644 --- a/planning/behavior_path_planner/src/scene_module/pull_over/util.cpp +++ b/planning/behavior_path_planner/src/scene_module/pull_over/util.cpp @@ -17,10 +17,10 @@ #include "behavior_path_planner/path_shifter/path_shifter.hpp" #include "behavior_path_planner/path_utilities.hpp" -#include #include #include #include +#include #include @@ -227,14 +227,14 @@ std::vector getPullOverPaths( continue; } - const auto shift_end_idx = autoware_utils::findNearestIndex( + const auto shift_end_idx = tier4_autoware_utils::findNearestIndex( shifted_path.path.points, reference_path2.points.front().point.pose); const auto goal_idx = - autoware_utils::findNearestIndex(shifted_path.path.points, route_handler.getGoalPose()); + tier4_autoware_utils::findNearestIndex(shifted_path.path.points, route_handler.getGoalPose()); if (shift_end_idx && goal_idx) { - const auto distance_pull_over_end_to_goal = autoware_utils::calcDistance2d( + const auto distance_pull_over_end_to_goal = tier4_autoware_utils::calcDistance2d( shifted_path.path.points.at(*shift_end_idx).point.pose, shifted_path.path.points.at(*goal_idx).point.pose); for (size_t i = 0; i < shifted_path.path.points.size(); ++i) { @@ -251,7 +251,7 @@ std::vector getPullOverPaths( continue; } // set velocity between shift end and goal. decelerate linearly from shift end to 0 - auto distance_to_goal = autoware_utils::calcDistance2d( + auto distance_to_goal = tier4_autoware_utils::calcDistance2d( point.point.pose, shifted_path.path.points.at(*goal_idx).point.pose); point.point.longitudinal_velocity_mps = std::min( minimum_pull_over_velocity, @@ -261,10 +261,10 @@ std::vector getPullOverPaths( } candidate_path.path = combineReferencePath(reference_path1, shifted_path.path); candidate_path.shifted_path = shifted_path; - shift_point.start_idx = - autoware_utils::findNearestIndex(shifted_path.path.points, shift_point.start.position); + shift_point.start_idx = tier4_autoware_utils::findNearestIndex( + shifted_path.path.points, shift_point.start.position); shift_point.end_idx = - autoware_utils::findNearestIndex(shifted_path.path.points, shift_point.end.position); + tier4_autoware_utils::findNearestIndex(shifted_path.path.points, shift_point.end.position); candidate_path.shift_point = shift_point; // candidate_path.acceleration = acceleration; candidate_path.preparation_length = straight_distance; diff --git a/planning/behavior_path_planner/src/scene_module/side_shift/side_shift_module.cpp b/planning/behavior_path_planner/src/scene_module/side_shift/side_shift_module.cpp index 54922bae5c5a2..f32dbc8ee7848 100644 --- a/planning/behavior_path_planner/src/scene_module/side_shift/side_shift_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/side_shift/side_shift_module.cpp @@ -177,7 +177,7 @@ bool SideShiftModule::addShiftPoint() auto shift_points = path_shifter_.getShiftPoints(); const auto calcLongitudinal = [this](const auto & sp) { - return autoware_utils::calcSignedArcLength( + return tier4_autoware_utils::calcSignedArcLength( reference_path_->points, getEgoPose().pose.position, sp.start.position); }; @@ -333,7 +333,7 @@ double SideShiftModule::getClosestShiftLength() const } const auto ego_point = planner_data_->self_pose->pose.position; - const auto closest = autoware_utils::findNearestIndex(prev_output_.path.points, ego_point); + const auto closest = tier4_autoware_utils::findNearestIndex(prev_output_.path.points, ego_point); return prev_output_.shift_length.at(closest); } @@ -364,7 +364,7 @@ PoseStamped SideShiftModule::getUnshiftedEgoPose(const ShiftedPath & prev_path) // un-shifted fot current ideal pose const auto closest = - autoware_utils::findNearestIndex(prev_path.path.points, ego_pose.pose.position); + tier4_autoware_utils::findNearestIndex(prev_path.path.points, ego_pose.pose.position); PoseStamped unshifted_pose{}; unshifted_pose.header = ego_pose.header; @@ -388,7 +388,7 @@ PathWithLaneId SideShiftModule::calcCenterLinePath( const auto longest_dist_to_shift_point = [&]() { double max_dist = 0.0; for (const auto & pnt : path_shifter_.getShiftPoints()) { - max_dist = std::max(max_dist, autoware_utils::calcDistance2d(getEgoPose(), pnt.start)); + max_dist = std::max(max_dist, tier4_autoware_utils::calcDistance2d(getEgoPose(), pnt.start)); } return max_dist; }(); diff --git a/planning/behavior_path_planner/src/turn_signal_decider.cpp b/planning/behavior_path_planner/src/turn_signal_decider.cpp index 2ad96283d06ed..9fca606455c2d 100644 --- a/planning/behavior_path_planner/src/turn_signal_decider.cpp +++ b/planning/behavior_path_planner/src/turn_signal_decider.cpp @@ -16,7 +16,7 @@ #include "behavior_path_planner/utilities.hpp" -#include +#include #include #include @@ -69,7 +69,8 @@ std::pair TurnSignalDecider::getIntersectionTurnS auto prev_point = path.points.front(); auto prev_lane_id = lanelet::InvalId; for (const auto & path_point : path.points) { - accumulated_distance += autoware_utils::calcDistance3d(prev_point.point, path_point.point); + accumulated_distance += + tier4_autoware_utils::calcDistance3d(prev_point.point, path_point.point); prev_point = path_point; const double distance_from_vehicle_front = accumulated_distance - vehicle_pose_frenet.length - base_link2front_; diff --git a/planning/behavior_path_planner/src/utilities.cpp b/planning/behavior_path_planner/src/utilities.cpp index ebdb277e2e6f1..07025674bb3f5 100644 --- a/planning/behavior_path_planner/src/utilities.cpp +++ b/planning/behavior_path_planner/src/utilities.cpp @@ -14,11 +14,11 @@ #include "behavior_path_planner/utilities.hpp" -#include #include #include #include #include +#include #include #include @@ -65,7 +65,7 @@ bool convertToFrenetCoordinate3d( return false; } - const auto search_pt = autoware_utils::fromMsg(search_point_geom); + const auto search_pt = tier4_autoware_utils::fromMsg(search_point_geom); bool found = false; double min_distance = std::numeric_limits::max(); @@ -76,11 +76,11 @@ bool convertToFrenetCoordinate3d( for (std::size_t i = 0; i < linestring.size(); i++) { const auto geom_pt = linestring.at(i); - const auto current_pt = autoware_utils::fromMsg(geom_pt); + const auto current_pt = tier4_autoware_utils::fromMsg(geom_pt); const auto current2search_pt = (search_pt - current_pt); // update accumulated length if (i != 0) { - const auto p1 = autoware_utils::fromMsg(linestring.at(i - 1)); + const auto p1 = tier4_autoware_utils::fromMsg(linestring.at(i - 1)); const auto p2 = current_pt; accumulated_length += (p2 - p1).norm(); } @@ -101,8 +101,8 @@ bool convertToFrenetCoordinate3d( auto prev_geom_pt = linestring.front(); double accumulated_length = 0; for (const auto & geom_pt : linestring) { - const auto start_pt = autoware_utils::fromMsg(prev_geom_pt); - const auto end_pt = autoware_utils::fromMsg(geom_pt); + const auto start_pt = tier4_autoware_utils::fromMsg(prev_geom_pt); + const auto end_pt = tier4_autoware_utils::fromMsg(geom_pt); const auto line_segment = end_pt - start_pt; const double line_segment_length = line_segment.norm(); @@ -269,7 +269,7 @@ Point lerpByLength(const std::vector & point_array, const double length) Point prev_pt = point_array.front(); double accumulated_length = 0; for (const auto & pt : point_array) { - const double distance = autoware_utils::calcDistance3d(prev_pt, pt); + const double distance = tier4_autoware_utils::calcDistance3d(prev_pt, pt); if (accumulated_length + distance > length) { return lerpByPoint(prev_pt, pt, (length - accumulated_length) / distance); } @@ -371,7 +371,7 @@ double getDistanceBetweenPredictedPaths( if (!lerpByTimeStamp(ego_path, t, &ego_pose)) { continue; } - double distance = autoware_utils::calcDistance3d(object_pose, ego_pose); + double distance = tier4_autoware_utils::calcDistance3d(object_pose, ego_pose); if (distance < min_distance) { min_distance = distance; } @@ -410,7 +410,7 @@ double getDistanceBetweenPredictedPathAndObject( double getDistanceBetweenPredictedPathAndObjectPolygon( const PredictedObject & object, const PullOutPath & ego_path, - const autoware_utils::LinearRing2d & vehicle_footprint, double distance_resolution, + const tier4_autoware_utils::LinearRing2d & vehicle_footprint, double distance_resolution, const lanelet::ConstLanelets & road_lanes) { double min_distance = std::numeric_limits::max(); @@ -432,7 +432,7 @@ double getDistanceBetweenPredictedPathAndObjectPolygon( continue; } const auto vehicle_footprint_on_path = - transformVector(vehicle_footprint, autoware_utils::pose2transform(ego_pose)); + transformVector(vehicle_footprint, tier4_autoware_utils::pose2transform(ego_pose)); Point2d ego_point{ego_pose.position.x, ego_pose.position.y}; for (const auto & vehicle_footprint : vehicle_footprint_on_path) { double distance = boost::geometry::distance(obj_polygon, vehicle_footprint); @@ -654,7 +654,9 @@ PathWithLaneId removeOverlappingPoints(const PathWithLaneId & input_path) } constexpr double min_dist = 0.001; - if (autoware_utils::calcDistance3d(filtered_path.points.back().point, pt.point) < min_dist) { + if ( + tier4_autoware_utils::calcDistance3d(filtered_path.points.back().point, pt.point) < + min_dist) { filtered_path.points.back().lane_ids.push_back(pt.lane_ids.front()); filtered_path.points.back().point.longitudinal_velocity_mps = std::min( pt.point.longitudinal_velocity_mps, @@ -688,12 +690,12 @@ bool setGoal( PathPointWithLaneId refined_goal{}; { // NOTE: goal does not have valid z, that will be calculated by interpolation here const size_t closest_seg_idx = - autoware_utils::findNearestSegmentIndex(input.points, goal.position); - const double closest_to_goal_dist = autoware_utils::calcSignedArcLength( + tier4_autoware_utils::findNearestSegmentIndex(input.points, goal.position); + const double closest_to_goal_dist = tier4_autoware_utils::calcSignedArcLength( input.points, input.points.at(closest_seg_idx).point.pose.position, goal.position); // TODO(murooka) implement calcSignedArcLength(points, idx, point) - const double seg_dist = - autoware_utils::calcSignedArcLength(input.points, closest_seg_idx, closest_seg_idx + 1); + const double seg_dist = tier4_autoware_utils::calcSignedArcLength( + input.points, closest_seg_idx, closest_seg_idx + 1); const double closest_z = input.points.at(closest_seg_idx).point.pose.position.z; const double next_z = input.points.at(closest_seg_idx + 1).point.pose.position.z; const double goal_z = std::abs(seg_dist) < 1e-6 @@ -722,12 +724,12 @@ bool setGoal( pre_refined_goal.point.pose.orientation = goal.orientation; { // NOTE: interpolate z and velocity of pre_refined_goal - const size_t closest_seg_idx = - autoware_utils::findNearestSegmentIndex(input.points, pre_refined_goal.point.pose.position); - const double closest_to_pre_goal_dist = autoware_utils::calcSignedArcLength( + const size_t closest_seg_idx = tier4_autoware_utils::findNearestSegmentIndex( + input.points, pre_refined_goal.point.pose.position); + const double closest_to_pre_goal_dist = tier4_autoware_utils::calcSignedArcLength( input.points, input.points.at(closest_seg_idx).point.pose.position, pre_refined_goal.point.pose.position); - const double seg_dist = autoware_utils::calcSignedArcLength( + const double seg_dist = tier4_autoware_utils::calcSignedArcLength( input.points, closest_seg_idx, closest_seg_idx + 1); // TODO(murooka) implement calcSignedArcLength(points, idx, point) @@ -750,7 +752,7 @@ bool setGoal( // find min_dist_index whose distance to goal is shorter than search_radius_range const auto min_dist_index_opt = - autoware_utils::findNearestIndex(input.points, goal, search_radius_range); + tier4_autoware_utils::findNearestIndex(input.points, goal, search_radius_range); if (!min_dist_index_opt) { return false; } @@ -762,7 +764,7 @@ bool setGoal( { // NOTE: type of i must be int since i will be -1 even if the condition is 0<=i for (int i = min_dist_index; 0 <= i; --i) { - const double dist = autoware_utils::calcDistance2d(input.points.at(i).point, goal); + const double dist = tier4_autoware_utils::calcDistance2d(input.points.at(i).point, goal); min_dist_out_of_circle_index = i; if (search_radius_range < dist) { break; @@ -933,7 +935,7 @@ OccupancyGrid generateDrivableArea( std::vector cv_polygon; for (const auto & p : lane_poly) { const double z = lane.polygon3d().basicPolygon().at(0).z(); - Point geom_pt = autoware_utils::createPoint(p.x(), p.y(), z); + Point geom_pt = tier4_autoware_utils::createPoint(p.x(), p.y(), z); Point transformed_geom_pt; tf2::doTransform(geom_pt, transformed_geom_pt, geom_tf_map2grid); cv_polygon.push_back(toCVPoint(transformed_geom_pt, width, height, resolution)); @@ -1156,7 +1158,7 @@ PathPointWithLaneId insertStopPoint(double length, PathWithLaneId * path) for (size_t i = 1; i < path->points.size(); i++) { const auto prev_pose = path->points.at(i - 1).point.pose; const auto curr_pose = path->points.at(i).point.pose; - const double segment_length = autoware_utils::calcDistance3d(prev_pose, curr_pose); + const double segment_length = tier4_autoware_utils::calcDistance3d(prev_pose, curr_pose); accumulated_length += segment_length; if (accumulated_length > length) { insert_idx = i; diff --git a/planning/behavior_velocity_planner/CMakeLists.txt b/planning/behavior_velocity_planner/CMakeLists.txt index a952cd63e3fb8..054fff44d407c 100644 --- a/planning/behavior_velocity_planner/CMakeLists.txt +++ b/planning/behavior_velocity_planner/CMakeLists.txt @@ -27,7 +27,7 @@ set(BEHAVIOR_VELOCITY_PLANNER_DEPENDENCIES autoware_auto_perception_msgs autoware_auto_planning_msgs tier4_planning_msgs - autoware_utils + tier4_autoware_utils Boost Eigen3 diagnostic_msgs diff --git a/planning/behavior_velocity_planner/include/scene_module/intersection/scene_intersection.hpp b/planning/behavior_velocity_planner/include/scene_module/intersection/scene_intersection.hpp index 0abe7f2730167..2bf3276aa9ef6 100644 --- a/planning/behavior_velocity_planner/include/scene_module/intersection/scene_intersection.hpp +++ b/planning/behavior_velocity_planner/include/scene_module/intersection/scene_intersection.hpp @@ -15,9 +15,9 @@ #ifndef SCENE_MODULE__INTERSECTION__SCENE_INTERSECTION_HPP_ #define SCENE_MODULE__INTERSECTION__SCENE_INTERSECTION_HPP_ -#include #include #include +#include #include #include diff --git a/planning/behavior_velocity_planner/include/scene_module/intersection/scene_merge_from_private_road.hpp b/planning/behavior_velocity_planner/include/scene_module/intersection/scene_merge_from_private_road.hpp index 9f8ad21be2286..407d9715c65b7 100644 --- a/planning/behavior_velocity_planner/include/scene_module/intersection/scene_merge_from_private_road.hpp +++ b/planning/behavior_velocity_planner/include/scene_module/intersection/scene_merge_from_private_road.hpp @@ -15,10 +15,10 @@ #ifndef SCENE_MODULE__INTERSECTION__SCENE_MERGE_FROM_PRIVATE_ROAD_HPP_ #define SCENE_MODULE__INTERSECTION__SCENE_MERGE_FROM_PRIVATE_ROAD_HPP_ -#include #include #include #include +#include #include #include diff --git a/planning/behavior_velocity_planner/include/scene_module/virtual_traffic_light/scene.hpp b/planning/behavior_velocity_planner/include/scene_module/virtual_traffic_light/scene.hpp index c9ac5d6f83d9b..2a7f19bcb164a 100644 --- a/planning/behavior_velocity_planner/include/scene_module/virtual_traffic_light/scene.hpp +++ b/planning/behavior_velocity_planner/include/scene_module/virtual_traffic_light/scene.hpp @@ -15,13 +15,13 @@ #ifndef SCENE_MODULE__VIRTUAL_TRAFFIC_LIGHT__SCENE_HPP_ #define SCENE_MODULE__VIRTUAL_TRAFFIC_LIGHT__SCENE_HPP_ -#include #include #include #include #include #include #include +#include #include #include @@ -49,10 +49,10 @@ class VirtualTrafficLightModule : public SceneModuleInterface std::string instrument_type{}; std::string instrument_id{}; std::vector custom_tags{}; - autoware_utils::Point3d instrument_center{}; - boost::optional stop_line{}; - autoware_utils::LineString3d start_line{}; - std::vector end_lines{}; + tier4_autoware_utils::Point3d instrument_center{}; + boost::optional stop_line{}; + tier4_autoware_utils::LineString3d start_line{}; + std::vector end_lines{}; }; struct ModuleData diff --git a/planning/behavior_velocity_planner/include/utilization/arc_lane_util.hpp b/planning/behavior_velocity_planner/include/utilization/arc_lane_util.hpp index 0720234ce7d77..b03362dd60575 100644 --- a/planning/behavior_velocity_planner/include/utilization/arc_lane_util.hpp +++ b/planning/behavior_velocity_planner/include/utilization/arc_lane_util.hpp @@ -15,8 +15,8 @@ #ifndef UTILIZATION__ARC_LANE_UTIL_HPP_ #define UTILIZATION__ARC_LANE_UTIL_HPP_ -#include #include +#include #include #include @@ -103,7 +103,7 @@ boost::optional findForwardOffsetSegment( { double sum_length = 0.0; for (size_t i = base_idx; i < path.points.size() - 1; ++i) { - sum_length += autoware_utils::calcDistance2d(path.points.at(i), path.points.at(i + 1)); + sum_length += tier4_autoware_utils::calcDistance2d(path.points.at(i), path.points.at(i + 1)); // If it's over offset point, return front index and remain offset length if (sum_length >= offset_length) { @@ -122,7 +122,7 @@ boost::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_utils::calcDistance2d(path.points.at(i), path.points.at(i + 1)); + sum_length += tier4_autoware_utils::calcDistance2d(path.points.at(i), path.points.at(i + 1)); // If it's over offset point, return front index and remain offset length if (sum_length >= offset_length) { @@ -177,8 +177,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_utils::calcAzimuthAngle(p_front, p_back); - target_pose.orientation = autoware_utils::createQuaternionFromYaw(yaw); + const double yaw = tier4_autoware_utils::calcAzimuthAngle(p_front, p_back); + target_pose.orientation = tier4_autoware_utils::createQuaternionFromYaw(yaw); return target_pose; } diff --git a/planning/behavior_velocity_planner/include/utilization/path_utilization.hpp b/planning/behavior_velocity_planner/include/utilization/path_utilization.hpp index 0a41a8846860c..cb233c236d2b1 100644 --- a/planning/behavior_velocity_planner/include/utilization/path_utilization.hpp +++ b/planning/behavior_velocity_planner/include/utilization/path_utilization.hpp @@ -15,8 +15,8 @@ #ifndef UTILIZATION__PATH_UTILIZATION_HPP_ #define UTILIZATION__PATH_UTILIZATION_HPP_ -#include #include +#include #include diff --git a/planning/behavior_velocity_planner/package.xml b/planning/behavior_velocity_planner/package.xml index 47ad4ec2d1d9e..2c835429311e8 100644 --- a/planning/behavior_velocity_planner/package.xml +++ b/planning/behavior_velocity_planner/package.xml @@ -14,7 +14,6 @@ autoware_auto_mapping_msgs autoware_auto_perception_msgs autoware_auto_planning_msgs - autoware_utils diagnostic_msgs eigen geometry_msgs @@ -34,6 +33,7 @@ tf2_geometry_msgs tf2_ros tier4_api_msgs + tier4_autoware_utils tier4_planning_msgs tier4_v2x_msgs vehicle_info_util diff --git a/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp b/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp index 431120734d3c7..d36c32d6ba9d4 100644 --- a/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp @@ -285,7 +285,7 @@ bool IntersectionModule::checkCollision( lanelet::ConstLanelets ego_lane_with_next_lane = getEgoLaneWithNextLane(lanelet_map_ptr, path); const auto closest_arc_coords = getArcCoordinates( - ego_lane_with_next_lane, autoware_utils::getPose(path.points.at(closest_idx).point)); + ego_lane_with_next_lane, tier4_autoware_utils::getPose(path.points.at(closest_idx).point)); const double distance_until_intersection = calcDistanceUntilIntersectionLanelet(lanelet_map_ptr, path, closest_idx); const double base_link2front = planner_data_->vehicle_info_.max_longitudinal_offset_m; @@ -390,10 +390,10 @@ Polygon2d IntersectionModule::generateEgoIntersectionLanePolygon( lanelet::ConstLanelets ego_lane_with_next_lane = getEgoLaneWithNextLane(lanelet_map_ptr, path); const auto start_arc_coords = getArcCoordinates( - ego_lane_with_next_lane, autoware_utils::getPose(path.points.at(start_idx).point)); + ego_lane_with_next_lane, tier4_autoware_utils::getPose(path.points.at(start_idx).point)); const auto closest_arc_coords = getArcCoordinates( - ego_lane_with_next_lane, autoware_utils::getPose(path.points.at(closest_idx).point)); + ego_lane_with_next_lane, tier4_autoware_utils::getPose(path.points.at(closest_idx).point)); const double start_arc_length = start_arc_coords.length + ignore_dist < closest_arc_coords.length ? closest_arc_coords.length @@ -609,7 +609,7 @@ bool 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_utils::normalizeRadian(ll_angle - pose_angle); + const double angle_diff = tier4_autoware_utils::normalizeRadian(ll_angle - pose_angle); if (std::fabs(angle_diff) < planner_param_.detection_area_angle_thr) { return true; } diff --git a/planning/behavior_velocity_planner/src/scene_module/intersection/scene_merge_from_private_road.cpp b/planning/behavior_velocity_planner/src/scene_module/intersection/scene_merge_from_private_road.cpp index ff24e3dcf3132..3ae3f0c2faa9f 100644 --- a/planning/behavior_velocity_planner/src/scene_module/intersection/scene_merge_from_private_road.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/intersection/scene_merge_from_private_road.cpp @@ -165,7 +165,7 @@ MergeFromPrivateRoadModule::extractPathNearExitOfPrivateRoad( if (prev_has_target_lane_id) { // extend path to the front private_path.points.emplace_back(path.points.at(i)); - sum_dist += autoware_utils::calcDistance2d( + sum_dist += tier4_autoware_utils::calcDistance2d( path.points.at(i).point.pose, path.points.at(i + 1).point.pose); if (sum_dist > extend_length) { break; diff --git a/planning/behavior_velocity_planner/src/scene_module/no_stopping_area/debug.cpp b/planning/behavior_velocity_planner/src/scene_module/no_stopping_area/debug.cpp index 3855c67397337..a467bedec8aa0 100644 --- a/planning/behavior_velocity_planner/src/scene_module/no_stopping_area/debug.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/no_stopping_area/debug.cpp @@ -16,7 +16,7 @@ #include "utilization/marker_helper.hpp" #include "utilization/util.hpp" -#include +#include #include @@ -72,7 +72,7 @@ visualization_msgs::msg::MarkerArray createMarkerArray( tf2::Transform tf_map2front = tf_map2base_link * tf_base_link2front; geometry_msgs::msg::Pose pose; tf2::toMsg(tf_map2front, pose); - msg = autoware_utils::createStopVirtualWallMarker(pose, "no_stopping_area", now, uid + j); + msg = tier4_autoware_utils::createStopVirtualWallMarker(pose, "no_stopping_area", now, uid + j); } return msg; } diff --git a/planning/behavior_velocity_planner/src/scene_module/no_stopping_area/scene_no_stopping_area.cpp b/planning/behavior_velocity_planner/src/scene_module/no_stopping_area/scene_no_stopping_area.cpp index 7a8f46e9e456a..f133c3d84f14d 100644 --- a/planning/behavior_velocity_planner/src/scene_module/no_stopping_area/scene_no_stopping_area.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/no_stopping_area/scene_no_stopping_area.cpp @@ -18,8 +18,8 @@ #include "utilization/interpolate.hpp" #include "utilization/util.hpp" -#include #include +#include #include #include @@ -156,7 +156,7 @@ boost::optional NoStoppingAreaModule::getStopLineGeometry2d( if (collision_points.empty()) { continue; } - const double yaw = autoware_utils::calcAzimuthAngle(p0, p1); + const double yaw = tier4_autoware_utils::calcAzimuthAngle(p0, p1); if (!collision_points.empty()) { geometry_msgs::msg::Point left_point; const double w = planner_data_->vehicle_info_.vehicle_width_m; @@ -422,7 +422,8 @@ bool NoStoppingAreaModule::isOverDeadLine( const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const geometry_msgs::msg::Pose & self_pose, const geometry_msgs::msg::Pose & line_pose) const { - return autoware_utils::calcSignedArcLength(path.points, self_pose.position, line_pose.position) + + return tier4_autoware_utils::calcSignedArcLength( + path.points, self_pose.position, line_pose.position) + planner_param_.dead_line_margin < 0.0; } diff --git a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/debug.cpp b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/debug.cpp index 34ae06d5daa2c..5884aee1e9ae8 100644 --- a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/debug.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/debug.cpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include #include #include #include +#include #include #include @@ -34,9 +34,9 @@ visualization_msgs::msg::Marker makeArrowMarker( debug_marker.ns = "occlusion spot arrow"; debug_marker.id = id; debug_marker.type = visualization_msgs::msg::Marker::ARROW; - debug_marker.pose.orientation = autoware_utils::createMarkerOrientation(0, 0, 0, 1.0); - debug_marker.scale = autoware_utils::createMarkerScale(0.05, 0.2, 0.5); - debug_marker.color = autoware_utils::createMarkerColor(0.1, 0.1, 0.1, 0.5); + debug_marker.pose.orientation = tier4_autoware_utils::createMarkerOrientation(0, 0, 0, 1.0); + debug_marker.scale = tier4_autoware_utils::createMarkerScale(0.05, 0.2, 0.5); + debug_marker.color = tier4_autoware_utils::createMarkerColor(0.1, 0.1, 0.1, 0.5); debug_marker.lifetime = rclcpp::Duration::from_seconds(0.5); geometry_msgs::msg::Point obs_point, intersection_point{}; obs_point = possible_collision.obstacle_info.position; @@ -62,8 +62,8 @@ std::vector makeSlowDownMarkers( // cylinder at collision point wall_marker.pose = possible_collision.intersection_pose; wall_marker.pose.position.z += 1.0; - wall_marker.scale = autoware_utils::createMarkerScale(0.1, 5.0, 2.0); - wall_marker.color = autoware_utils::createMarkerColor(1.0, 1.0, 0.0, 0.5); + wall_marker.scale = tier4_autoware_utils::createMarkerScale(0.1, 5.0, 2.0); + wall_marker.color = tier4_autoware_utils::createMarkerColor(1.0, 1.0, 0.0, 0.5); wall_marker.lifetime = rclcpp::Duration::from_seconds(0.5); debug_markers.emplace_back(wall_marker); @@ -77,11 +77,11 @@ std::vector makeSlowDownMarkers( slowdown_reason_marker.type = visualization_msgs::msg::Marker::TEXT_VIEW_FACING; slowdown_reason_marker.action = visualization_msgs::msg::Marker::ADD; slowdown_reason_marker.pose = possible_collision.intersection_pose; - slowdown_reason_marker.scale = autoware_utils::createMarkerScale(0.0, 0.0, 1.0); - slowdown_reason_marker.color = autoware_utils::createMarkerColor(1.0, 1.0, 1.0, 0.999); + slowdown_reason_marker.scale = tier4_autoware_utils::createMarkerScale(0.0, 0.0, 1.0); + slowdown_reason_marker.color = tier4_autoware_utils::createMarkerColor(1.0, 1.0, 1.0, 0.999); slowdown_reason_marker.text = "occlusion spot"; debug_markers.emplace_back(slowdown_reason_marker); - slowdown_reason_marker.scale = autoware_utils::createMarkerScale(0.0, 0.0, 0.5); + slowdown_reason_marker.scale = tier4_autoware_utils::createMarkerScale(0.0, 0.0, 0.5); slowdown_reason_marker.id = id + 100; slowdown_reason_marker.text = "\n \n" + road_type; debug_markers.emplace_back(slowdown_reason_marker); @@ -100,8 +100,8 @@ std::vector makeCollisionMarkers( // cylinder at collision_point point debug_marker.type = visualization_msgs::msg::Marker::CYLINDER; debug_marker.pose = possible_collision.collision_path_point.pose; - debug_marker.scale = autoware_utils::createMarkerScale(1.0, 1.0, 0.5); - debug_marker.color = autoware_utils::createMarkerColor(1.0, 0.0, 0.0, 0.5); + debug_marker.scale = tier4_autoware_utils::createMarkerScale(1.0, 1.0, 0.5); + debug_marker.color = tier4_autoware_utils::createMarkerColor(1.0, 0.0, 0.0, 0.5); debug_marker.lifetime = rclcpp::Duration::from_seconds(0.5); debug_markers.push_back(debug_marker); @@ -109,8 +109,8 @@ std::vector makeCollisionMarkers( debug_marker.ns = "obstacle"; debug_marker.type = visualization_msgs::msg::Marker::CYLINDER; debug_marker.pose.position = possible_collision.obstacle_info.position; - debug_marker.color = autoware_utils::createMarkerColor(0.5, 0.5, 0.5, 0.5); - debug_marker.scale = autoware_utils::createMarkerScale(1.0, 1.0, 1.0); + debug_marker.color = tier4_autoware_utils::createMarkerColor(0.5, 0.5, 0.5, 0.5); + debug_marker.scale = tier4_autoware_utils::createMarkerScale(1.0, 1.0, 1.0); debug_markers.push_back(debug_marker); if (show_text) { // info text at obstacle point @@ -118,7 +118,7 @@ std::vector makeCollisionMarkers( debug_marker.type = visualization_msgs::msg::Marker::TEXT_VIEW_FACING; debug_marker.pose = possible_collision.collision_path_point.pose; debug_marker.scale.z = 1.0; - debug_marker.color = autoware_utils::createMarkerColor(1.0, 1.0, 0.0, 1.0); + debug_marker.color = tier4_autoware_utils::createMarkerColor(1.0, 1.0, 0.0, 1.0); std::ostringstream string_stream; string_stream << "(s,d,v)=(" << possible_collision.arc_lane_dist_at_collision.length << " , " << possible_collision.arc_lane_dist_at_collision.distance << " , " @@ -138,14 +138,14 @@ std::vector makePolygonMarker( debug_marker.id = id; debug_marker.type = visualization_msgs::msg::Marker::LINE_STRIP; debug_marker.action = visualization_msgs::msg::Marker::ADD; - debug_marker.pose.position = autoware_utils::createMarkerPosition(0.0, 0.0, z); - debug_marker.pose.orientation = autoware_utils::createMarkerOrientation(0, 0, 0, 1.0); - debug_marker.scale = autoware_utils::createMarkerScale(0.05, 0.05, 0.05); - debug_marker.color = autoware_utils::createMarkerColor(1.0, 0.0, 1.0, 0.3); + debug_marker.pose.position = tier4_autoware_utils::createMarkerPosition(0.0, 0.0, z); + debug_marker.pose.orientation = tier4_autoware_utils::createMarkerOrientation(0, 0, 0, 1.0); + debug_marker.scale = tier4_autoware_utils::createMarkerScale(0.05, 0.05, 0.05); + debug_marker.color = tier4_autoware_utils::createMarkerColor(1.0, 0.0, 1.0, 0.3); debug_marker.lifetime = rclcpp::Duration::from_seconds(0.1); debug_marker.ns = "sidewalk"; for (const auto & p : polygon) { - geometry_msgs::msg::Point point = autoware_utils::createMarkerPosition(p.x(), p.y(), 0.0); + geometry_msgs::msg::Point point = tier4_autoware_utils::createMarkerPosition(p.x(), p.y(), 0.0); debug_marker.points.push_back(point); } debug_markers.push_back(debug_marker); diff --git a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/occlusion_spot_utils.cpp b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/occlusion_spot_utils.cpp index 1ac33fccb90a6..416a273983277 100644 --- a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/occlusion_spot_utils.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/occlusion_spot_utils.cpp @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include -#include #include #include #include +#include +#include #include #include #include @@ -180,7 +180,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_utils::calcDistance2d(p_prev.pose.position, p_next.pose.position); + tier4_autoware_utils::calcDistance2d(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++) { @@ -368,7 +368,7 @@ void createPossibleCollisionBehindParkedVehicle( double path_angle = lanelet::utils::getLaneletAngle(path_lanelet, search_point); // ignore if angle is more different than 10[degree] double obj_angle = tf2::getYaw(q); - const double diff_angle = autoware_utils::normalizeRadian(path_angle - obj_angle); + const double diff_angle = tier4_autoware_utils::normalizeRadian(path_angle - obj_angle); if (std::abs(diff_angle) > param.angle_thr) { continue; } @@ -406,7 +406,7 @@ bool extractTargetRoad( if (!found_target && i < src_path.points.size() - 1) { const auto & curr_p = src_path.points[i].point.pose.position; const auto & next_p = src_path.points[i + 1].point.pose.position; - offset_from_closest_to_target += autoware_utils::calcDistance2d(curr_p, next_p); + offset_from_closest_to_target += tier4_autoware_utils::calcDistance2d(curr_p, next_p); } } return found_target; diff --git a/planning/behavior_velocity_planner/src/scene_module/virtual_traffic_light/debug.cpp b/planning/behavior_velocity_planner/src/scene_module/virtual_traffic_light/debug.cpp index 997a6fdea5569..146b4ee9ec0a0 100644 --- a/planning/behavior_velocity_planner/src/scene_module/virtual_traffic_light/debug.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/virtual_traffic_light/debug.cpp @@ -12,30 +12,30 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include #include - -using autoware_utils::appendMarkerArray; -using autoware_utils::createDefaultMarker; -using autoware_utils::createMarkerColor; -using autoware_utils::createMarkerOrientation; -using autoware_utils::createMarkerPosition; -using autoware_utils::createMarkerScale; -using autoware_utils::createStopVirtualWallMarker; -using autoware_utils::toMsg; +#include + +using tier4_autoware_utils::appendMarkerArray; +using tier4_autoware_utils::createDefaultMarker; +using tier4_autoware_utils::createMarkerColor; +using tier4_autoware_utils::createMarkerOrientation; +using tier4_autoware_utils::createMarkerPosition; +using tier4_autoware_utils::createMarkerScale; +using tier4_autoware_utils::createStopVirtualWallMarker; +using tier4_autoware_utils::toMsg; using namespace std::literals::string_literals; namespace behavior_velocity_planner { namespace { -[[maybe_unused]] autoware_utils::LinearRing3d createCircle( - const autoware_utils::Point3d & p, const double radius, const size_t num_points = 50) +[[maybe_unused]] tier4_autoware_utils::LinearRing3d createCircle( + const tier4_autoware_utils::Point3d & p, const double radius, const size_t num_points = 50) { - autoware_utils::LinearRing3d ring; // clockwise and closed + tier4_autoware_utils::LinearRing3d ring; // clockwise and closed for (size_t i = 0; i < num_points; ++i) { - const double theta = i * (2 * autoware_utils::pi / num_points); + const double theta = i * (2 * tier4_autoware_utils::pi / num_points); const double x = p.x() + radius * std::sin(theta); const double y = p.y() + radius * std::cos(theta); ring.emplace_back(x, y, p.z()); diff --git a/planning/behavior_velocity_planner/src/scene_module/virtual_traffic_light/scene.cpp b/planning/behavior_velocity_planner/src/scene_module/virtual_traffic_light/scene.cpp index 775dfa33ef178..7541c7495e0d7 100644 --- a/planning/behavior_velocity_planner/src/scene_module/virtual_traffic_light/scene.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/virtual_traffic_light/scene.cpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include #include +#include #include #include @@ -27,7 +27,7 @@ namespace behavior_velocity_planner namespace { namespace bg = boost::geometry; -using autoware_utils::calcDistance2d; +using tier4_autoware_utils::calcDistance2d; struct SegmentIndexWithPoint { @@ -46,16 +46,16 @@ tier4_v2x_msgs::msg::KeyValue createKeyValue(const std::string & key, const std: return tier4_v2x_msgs::build().key(key).value(value); } -autoware_utils::LineString3d toAutowarePoints(const lanelet::ConstLineString3d & line_string) +tier4_autoware_utils::LineString3d toAutowarePoints(const lanelet::ConstLineString3d & line_string) { - autoware_utils::LineString3d output; + tier4_autoware_utils::LineString3d output; for (const auto & p : line_string) { output.emplace_back(p.x(), p.y(), p.z()); } return output; } -boost::optional toAutowarePoints( +boost::optional toAutowarePoints( const lanelet::Optional & line_string) { if (!line_string) { @@ -64,26 +64,26 @@ boost::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_utils::LineString2d to_2d(const autoware_utils::LineString3d & line_string) +tier4_autoware_utils::LineString2d to_2d(const tier4_autoware_utils::LineString3d & line_string) { - autoware_utils::LineString2d output; + tier4_autoware_utils::LineString2d output; for (const auto & p : line_string) { output.emplace_back(p.x(), p.y()); } return output; } -autoware_utils::Point3d calcCenter(const autoware_utils::LineString3d & line_string) +tier4_autoware_utils::Point3d calcCenter(const tier4_autoware_utils::LineString3d & line_string) { const auto p1 = line_string.front(); const auto p2 = line_string.back(); @@ -94,21 +94,22 @@ autoware_utils::Point3d calcCenter(const autoware_utils::LineString3d & line_str geometry_msgs::msg::Pose calcHeadPose( const geometry_msgs::msg::Pose & base_link_pose, const double base_link_to_front) { - return autoware_utils::calcOffsetPose(base_link_pose, base_link_to_front, 0.0, 0.0); + return tier4_autoware_utils::calcOffsetPose(base_link_pose, base_link_to_front, 0.0, 0.0); } template boost::optional findCollision( - const T & points, const autoware_utils::LineString3d & line) + const T & points, const tier4_autoware_utils::LineString3d & line) { for (size_t i = 0; i < points.size() - 1; ++i) { // Create path segment - const auto & p_front = autoware_utils::getPoint(points.at(i)); - const auto & p_back = autoware_utils::getPoint(points.at(i + 1)); - const autoware_utils::LineString2d path_segment{{p_front.x, p_front.y}, {p_back.x, p_back.y}}; + const auto & p_front = tier4_autoware_utils::getPoint(points.at(i)); + const auto & p_back = tier4_autoware_utils::getPoint(points.at(i + 1)); + const tier4_autoware_utils::LineString2d path_segment{ + {p_front.x, p_front.y}, {p_back.x, p_back.y}}; // Find intersection - std::vector collision_points; + std::vector collision_points; bg::intersection(to_2d(line), path_segment, collision_points); // Ignore if no collision found @@ -127,8 +128,8 @@ boost::optional findCollision( const double interpolated_z = p_front.z + interpolate_ratio * (p_back.z - p_front.z); // To point - const auto collision_point = - autoware_utils::createPoint(collision_point_2d.x(), collision_point_2d.y(), interpolated_z); + const auto collision_point = tier4_autoware_utils::createPoint( + collision_point_2d.x(), collision_point_2d.y(), interpolated_z); return SegmentIndexWithPoint{i, collision_point}; } @@ -138,7 +139,7 @@ boost::optional findCollision( template boost::optional findCollision( - const T & points, const std::vector & lines) + const T & points, const std::vector & lines) { for (const auto & line : lines) { const auto collision = findCollision(points, line); @@ -232,7 +233,7 @@ geometry_msgs::msg::Pose calcInterpolatedPose( // Calculate orientation so that X-axis would be along the trajectory tf2::Quaternion quat; - quat.setRPY(0, 0, autoware_utils::calcAzimuthAngle(p_front, p_back)); + quat.setRPY(0, 0, tier4_autoware_utils::calcAzimuthAngle(p_front, p_back)); // To Pose geometry_msgs::msg::Pose interpolated_pose; @@ -255,8 +256,8 @@ size_t insertStopVelocityAtCollision( const SegmentIndexWithPoint & collision, const double offset, autoware_auto_planning_msgs::msg::PathWithLaneId * path) { - const auto collision_offset = - autoware_utils::calcLongitudinalOffsetToSegment(path->points, collision.index, collision.point); + const auto collision_offset = tier4_autoware_utils::calcLongitudinalOffsetToSegment( + path->points, collision.index, collision.point); const auto offset_segment = findOffsetSegment(*path, collision.index, offset + collision_offset); const auto interpolated_pose = @@ -452,7 +453,7 @@ bool VirtualTrafficLightModule::isBeforeStartLine() return false; } - const auto signed_arc_length = autoware_utils::calcSignedArcLength( + const auto signed_arc_length = tier4_autoware_utils::calcSignedArcLength( module_data_.path.points, module_data_.head_pose.position, collision->point); return signed_arc_length > 0; @@ -468,7 +469,7 @@ bool VirtualTrafficLightModule::isBeforeStopLine() return false; } - const auto signed_arc_length = autoware_utils::calcSignedArcLength( + const auto signed_arc_length = tier4_autoware_utils::calcSignedArcLength( module_data_.path.points, module_data_.head_pose.position, collision->point); return signed_arc_length > 0; @@ -489,7 +490,7 @@ bool VirtualTrafficLightModule::isAfterAnyEndLine() return false; } - const auto signed_arc_length = autoware_utils::calcSignedArcLength( + const auto signed_arc_length = tier4_autoware_utils::calcSignedArcLength( module_data_.path.points, module_data_.head_pose.position, collision->point); constexpr double max_excess_distance = 3.0; @@ -504,7 +505,7 @@ bool VirtualTrafficLightModule::isNearAnyEndLine() return false; } - const auto signed_arc_length = autoware_utils::calcSignedArcLength( + const auto signed_arc_length = tier4_autoware_utils::calcSignedArcLength( module_data_.path.points, module_data_.head_pose.position, collision->point); constexpr double near_distance = 1.0; diff --git a/planning/behavior_velocity_planner/src/utilization/path_utilization.cpp b/planning/behavior_velocity_planner/src/utilization/path_utilization.cpp index f824be8af89fe..456e30995286f 100644 --- a/planning/behavior_velocity_planner/src/utilization/path_utilization.cpp +++ b/planning/behavior_velocity_planner/src/utilization/path_utilization.cpp @@ -57,7 +57,7 @@ autoware_auto_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_utils::calcDistance3d(path_point_prev.pose, path_point.pose); + s += tier4_autoware_utils::calcDistance3d(path_point_prev.pose, path_point.pose); } s_in.push_back(s); } diff --git a/planning/behavior_velocity_planner/test/src/test_occlusion_spot_utils.cpp b/planning/behavior_velocity_planner/test/src/test_occlusion_spot_utils.cpp index 500784db5482d..19815f1a37369 100644 --- a/planning/behavior_velocity_planner/test/src/test_occlusion_spot_utils.cpp +++ b/planning/behavior_velocity_planner/test/src/test_occlusion_spot_utils.cpp @@ -163,7 +163,7 @@ TEST(createPossibleCollisionBehindParkedVehicle, TooManyPathPointsAndObstacles) obj.shape.dimensions.x = 0.0; obj.shape.dimensions.y = 0.0; obj.kinematics.initial_pose_with_covariance.pose.orientation = - autoware_utils::createQuaternionFromYaw(0.0); + tier4_autoware_utils::createQuaternionFromYaw(0.0); obj.kinematics.initial_twist_with_covariance.twist.linear.x = 0; obj.classification.push_back(autoware_auto_perception_msgs::msg::ObjectClassification{}); @@ -186,7 +186,7 @@ TEST(createPossibleCollisionBehindParkedVehicle, TooManyPathPointsAndObstacles) obj.kinematics.initial_pose_with_covariance.pose.position.x = 4.5; obj.kinematics.initial_pose_with_covariance.pose.position.y = 2.0; obj.kinematics.initial_pose_with_covariance.pose.orientation = - autoware_utils::createQuaternionFromYaw(M_PI); + tier4_autoware_utils::createQuaternionFromYaw(M_PI); obj.classification.at(0).label = autoware_auto_perception_msgs::msg::ObjectClassification::BUS; obj_arr.objects.emplace_back(obj); diff --git a/planning/freespace_planner/package.xml b/planning/freespace_planner/package.xml index dfa6592d0ece9..5f94d5c606456 100644 --- a/planning/freespace_planner/package.xml +++ b/planning/freespace_planner/package.xml @@ -14,7 +14,7 @@ autoware_auto_planning_msgs tier4_planning_msgs - autoware_utils + tier4_autoware_utils freespace_planning_algorithms geometry_msgs nav_msgs diff --git a/planning/freespace_planner/src/freespace_planner/freespace_planner_node.cpp b/planning/freespace_planner/src/freespace_planner/freespace_planner_node.cpp index f1a32e4c7614a..aba12c4718593 100644 --- a/planning/freespace_planner/src/freespace_planner/freespace_planner_node.cpp +++ b/planning/freespace_planner/src/freespace_planner/freespace_planner_node.cpp @@ -30,7 +30,7 @@ #include "freespace_planner/freespace_planner_node.hpp" -#include +#include #include #include @@ -138,8 +138,8 @@ Trajectory getPartialTrajectory( double calcDistance2d(const Trajectory & trajectory, const Pose & pose) { - const auto idx = autoware_utils::findNearestIndex(trajectory.points, pose.position); - return autoware_utils::calcDistance2d(trajectory.points.at(idx), pose); + const auto idx = tier4_autoware_utils::findNearestIndex(trajectory.points, pose.position); + return tier4_autoware_utils::calcDistance2d(trajectory.points.at(idx), pose); } Pose transformPose(const Pose & pose, const TransformStamped & transform) @@ -352,7 +352,7 @@ bool FreespacePlannerNode::isPlanRequired() algo_->setMap(*occupancy_grid_); const size_t nearest_index_partial = - autoware_utils::findNearestIndex(partial_trajectory_.points, current_pose_.pose.position); + tier4_autoware_utils::findNearestIndex(partial_trajectory_.points, current_pose_.pose.position); const size_t end_index_partial = partial_trajectory_.points.size() - 1; const auto forward_trajectory = @@ -381,7 +381,7 @@ bool FreespacePlannerNode::isPlanRequired() void FreespacePlannerNode::updateTargetIndex() { const auto is_near_target = - autoware_utils::calcDistance2d(trajectory_.points.at(target_index_), current_pose_) < + tier4_autoware_utils::calcDistance2d(trajectory_.points.at(target_index_), current_pose_) < node_param_.th_arrived_distance_m; const auto is_stopped = isStopped(odom_buffer_, node_param_.th_stopped_velocity_mps); @@ -423,7 +423,7 @@ void FreespacePlannerNode::onTimer() // Get current pose constexpr const char * vehicle_frame = "base_link"; current_pose_ = - autoware_utils::transform2pose(getTransform(occupancy_grid_->header.frame_id, vehicle_frame)); + tier4_autoware_utils::transform2pose(getTransform(occupancy_grid_->header.frame_id, vehicle_frame)); if (current_pose_.header.frame_id == "") { return; } diff --git a/planning/freespace_planning_algorithms/package.xml b/planning/freespace_planning_algorithms/package.xml index 2475e2f54b631..ff54138e74012 100644 --- a/planning/freespace_planning_algorithms/package.xml +++ b/planning/freespace_planning_algorithms/package.xml @@ -11,12 +11,12 @@ ament_cmake_auto - autoware_utils geometry_msgs nav_msgs std_msgs tf2 tf2_geometry_msgs + tier4_autoware_utils ament_cmake_gtest ament_lint_auto diff --git a/planning/freespace_planning_algorithms/src/abstract_algorithm.cpp b/planning/freespace_planning_algorithms/src/abstract_algorithm.cpp index 9a75506b5eefe..53f085632f5b4 100644 --- a/planning/freespace_planning_algorithms/src/abstract_algorithm.cpp +++ b/planning/freespace_planning_algorithms/src/abstract_algorithm.cpp @@ -14,14 +14,14 @@ #include "freespace_planning_algorithms/abstract_algorithm.hpp" -#include +#include #include namespace freespace_planning_algorithms { -using autoware_utils::createQuaternionFromYaw; -using autoware_utils::normalizeRadian; +using tier4_autoware_utils::createQuaternionFromYaw; +using tier4_autoware_utils::normalizeRadian; geometry_msgs::msg::Pose transformPose( const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::TransformStamped & transform) diff --git a/planning/freespace_planning_algorithms/src/astar_search.cpp b/planning/freespace_planning_algorithms/src/astar_search.cpp index 94397adc89275..858c383cc4cc8 100644 --- a/planning/freespace_planning_algorithms/src/astar_search.cpp +++ b/planning/freespace_planning_algorithms/src/astar_search.cpp @@ -14,7 +14,7 @@ #include "freespace_planning_algorithms/astar_search.hpp" -#include +#include #include #include @@ -34,7 +34,7 @@ double calcReedsSheppDistance( void setYaw(geometry_msgs::msg::Quaternion * orientation, const double yaw) { - *orientation = autoware_utils::createQuaternionFromYaw(yaw); + *orientation = tier4_autoware_utils::createQuaternionFromYaw(yaw); } geometry_msgs::msg::Pose calcRelativePose( @@ -61,7 +61,7 @@ geometry_msgs::msg::Pose node2pose(const AstarNode & node) pose_local.position.x = node.x; pose_local.position.y = node.y; pose_local.position.z = 0; - pose_local.orientation = autoware_utils::createQuaternionFromYaw(node.theta); + pose_local.orientation = tier4_autoware_utils::createQuaternionFromYaw(node.theta); return pose_local; } @@ -208,8 +208,8 @@ double AstarSearch::estimateCost(const geometry_msgs::msg::Pose & pose) total_cost += calcReedsSheppDistance(pose, goal_pose_, radius) * astar_param_.distance_heuristic_weight; } else { - total_cost += - autoware_utils::calcDistance2d(pose, goal_pose_) * astar_param_.distance_heuristic_weight; + total_cost += tier4_autoware_utils::calcDistance2d(pose, goal_pose_) * + astar_param_.distance_heuristic_weight; } return total_cost; } @@ -336,7 +336,8 @@ bool AstarSearch::isGoal(const AstarNode & node) { 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_utils::deg2rad(planner_common_param_.angle_goal_range / 2.0); + const double goal_angle = + tier4_autoware_utils::deg2rad(planner_common_param_.angle_goal_range / 2.0); const auto relative_pose = calcRelativePose(goal_pose_, node2pose(node)); @@ -351,7 +352,8 @@ bool AstarSearch::isGoal(const AstarNode & node) return false; } - const auto angle_diff = autoware_utils::normalizeRadian(tf2::getYaw(relative_pose.orientation)); + const auto angle_diff = + tier4_autoware_utils::normalizeRadian(tf2::getYaw(relative_pose.orientation)); if (std::abs(angle_diff) > goal_angle) { return false; } diff --git a/planning/motion_velocity_smoother/include/motion_velocity_smoother/motion_velocity_smoother_node.hpp b/planning/motion_velocity_smoother/include/motion_velocity_smoother/motion_velocity_smoother_node.hpp index 21c6224eb678d..8191ce9d4ac59 100644 --- a/planning/motion_velocity_smoother/include/motion_velocity_smoother/motion_velocity_smoother_node.hpp +++ b/planning/motion_velocity_smoother/include/motion_velocity_smoother/motion_velocity_smoother_node.hpp @@ -15,12 +15,6 @@ #ifndef MOTION_VELOCITY_SMOOTHER__MOTION_VELOCITY_SMOOTHER_NODE_HPP_ #define MOTION_VELOCITY_SMOOTHER__MOTION_VELOCITY_SMOOTHER_NODE_HPP_ -#include "autoware_utils/geometry/geometry.hpp" -#include "autoware_utils/math/unit_conversion.hpp" -#include "autoware_utils/ros/self_pose_listener.hpp" -#include "autoware_utils/system/stop_watch.hpp" -#include "autoware_utils/trajectory/tmp_conversion.hpp" -#include "autoware_utils/trajectory/trajectory.hpp" #include "motion_velocity_smoother/resample.hpp" #include "motion_velocity_smoother/smoother/jerk_filtered_smoother.hpp" #include "motion_velocity_smoother/smoother/l2_pseudo_jerk_smoother.hpp" @@ -29,6 +23,12 @@ #include "rclcpp/rclcpp.hpp" #include "tf2/utils.h" #include "tf2_ros/transform_listener.h" +#include "tier4_autoware_utils/geometry/geometry.hpp" +#include "tier4_autoware_utils/math/unit_conversion.hpp" +#include "tier4_autoware_utils/ros/self_pose_listener.hpp" +#include "tier4_autoware_utils/system/stop_watch.hpp" +#include "tier4_autoware_utils/trajectory/tmp_conversion.hpp" +#include "tier4_autoware_utils/trajectory/trajectory.hpp" #include "autoware_auto_planning_msgs/msg/trajectory.hpp" #include "autoware_auto_planning_msgs/msg/trajectory_point.hpp" @@ -90,7 +90,7 @@ class MotionVelocitySmootherNode : public rclcpp::Node boost::optional prev_closest_point_{}; // previous trajectory point // closest to ego vehicle - autoware_utils::SelfPoseListener self_pose_listener_{this}; + tier4_autoware_utils::SelfPoseListener self_pose_listener_{this}; enum class AlgorithmType { INVALID = 0, @@ -208,7 +208,7 @@ class MotionVelocitySmootherNode : public rclcpp::Node void initAnalyticalJerkConstrainedSmootherParam(); // debug - autoware_utils::StopWatch stop_watch_; + tier4_autoware_utils::StopWatch stop_watch_; std::shared_ptr prev_time_; double prev_acc_; rclcpp::Publisher::SharedPtr pub_dist_to_stopline_; diff --git a/planning/motion_velocity_smoother/include/motion_velocity_smoother/resample.hpp b/planning/motion_velocity_smoother/include/motion_velocity_smoother/resample.hpp index cd13a081e94a2..ac510efc4df85 100644 --- a/planning/motion_velocity_smoother/include/motion_velocity_smoother/resample.hpp +++ b/planning/motion_velocity_smoother/include/motion_velocity_smoother/resample.hpp @@ -15,8 +15,8 @@ #ifndef MOTION_VELOCITY_SMOOTHER__RESAMPLE_HPP_ #define MOTION_VELOCITY_SMOOTHER__RESAMPLE_HPP_ -#include "autoware_utils/trajectory/trajectory.hpp" #include "motion_velocity_smoother/trajectory_utils.hpp" +#include "tier4_autoware_utils/trajectory/trajectory.hpp" #include "autoware_auto_planning_msgs/msg/trajectory_point.hpp" diff --git a/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.hpp b/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.hpp index 412427f0277f9..a8e6c20ca6276 100644 --- a/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.hpp +++ b/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.hpp @@ -17,9 +17,9 @@ #define MOTION_VELOCITY_SMOOTHER__SMOOTHER__ANALYTICAL_JERK_CONSTRAINED_SMOOTHER__ANALYTICAL_JERK_CONSTRAINED_SMOOTHER_HPP_ // NOLINT // *INDENT-ON* -#include "autoware_utils/trajectory/trajectory.hpp" #include "rclcpp/rclcpp.hpp" #include "tf2/utils.h" +#include "tier4_autoware_utils/trajectory/trajectory.hpp" #include "autoware_auto_planning_msgs/msg/trajectory_point.hpp" #include "geometry_msgs/msg/pose.hpp" diff --git a/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/analytical_jerk_constrained_smoother/velocity_planning_utils.hpp b/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/analytical_jerk_constrained_smoother/velocity_planning_utils.hpp index 6c0156257cd96..4875fac23e18d 100644 --- a/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/analytical_jerk_constrained_smoother/velocity_planning_utils.hpp +++ b/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/analytical_jerk_constrained_smoother/velocity_planning_utils.hpp @@ -17,9 +17,9 @@ #define MOTION_VELOCITY_SMOOTHER__SMOOTHER__ANALYTICAL_JERK_CONSTRAINED_SMOOTHER__VELOCITY_PLANNING_UTILS_HPP_ // NOLINT // *INDENT-ON* -#include "autoware_utils/trajectory/trajectory.hpp" #include "motion_velocity_smoother/linear_interpolation.hpp" #include "rclcpp/rclcpp.hpp" +#include "tier4_autoware_utils/trajectory/trajectory.hpp" #include "autoware_auto_planning_msgs/msg/trajectory_point.hpp" diff --git a/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/jerk_filtered_smoother.hpp b/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/jerk_filtered_smoother.hpp index c3e4f16cc344b..0cec057d172b3 100644 --- a/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/jerk_filtered_smoother.hpp +++ b/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/jerk_filtered_smoother.hpp @@ -15,10 +15,10 @@ #ifndef MOTION_VELOCITY_SMOOTHER__SMOOTHER__JERK_FILTERED_SMOOTHER_HPP_ #define MOTION_VELOCITY_SMOOTHER__SMOOTHER__JERK_FILTERED_SMOOTHER_HPP_ -#include "autoware_utils/geometry/geometry.hpp" -#include "autoware_utils/trajectory/trajectory.hpp" #include "motion_velocity_smoother/smoother/smoother_base.hpp" #include "osqp_interface/osqp_interface.hpp" +#include "tier4_autoware_utils/geometry/geometry.hpp" +#include "tier4_autoware_utils/trajectory/trajectory.hpp" #include "autoware_auto_planning_msgs/msg/trajectory_point.hpp" diff --git a/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/l2_pseudo_jerk_smoother.hpp b/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/l2_pseudo_jerk_smoother.hpp index fd203dee78a67..8088d0cbfb1f6 100644 --- a/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/l2_pseudo_jerk_smoother.hpp +++ b/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/l2_pseudo_jerk_smoother.hpp @@ -15,10 +15,10 @@ #ifndef MOTION_VELOCITY_SMOOTHER__SMOOTHER__L2_PSEUDO_JERK_SMOOTHER_HPP_ #define MOTION_VELOCITY_SMOOTHER__SMOOTHER__L2_PSEUDO_JERK_SMOOTHER_HPP_ -#include "autoware_utils/geometry/geometry.hpp" -#include "autoware_utils/trajectory/trajectory.hpp" #include "motion_velocity_smoother/smoother/smoother_base.hpp" #include "osqp_interface/osqp_interface.hpp" +#include "tier4_autoware_utils/geometry/geometry.hpp" +#include "tier4_autoware_utils/trajectory/trajectory.hpp" #include "autoware_auto_planning_msgs/msg/trajectory_point.hpp" diff --git a/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/linf_pseudo_jerk_smoother.hpp b/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/linf_pseudo_jerk_smoother.hpp index 89e641d183751..2036050b59657 100644 --- a/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/linf_pseudo_jerk_smoother.hpp +++ b/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/linf_pseudo_jerk_smoother.hpp @@ -15,10 +15,10 @@ #ifndef MOTION_VELOCITY_SMOOTHER__SMOOTHER__LINF_PSEUDO_JERK_SMOOTHER_HPP_ #define MOTION_VELOCITY_SMOOTHER__SMOOTHER__LINF_PSEUDO_JERK_SMOOTHER_HPP_ -#include "autoware_utils/geometry/geometry.hpp" -#include "autoware_utils/trajectory/trajectory.hpp" #include "motion_velocity_smoother/smoother/smoother_base.hpp" #include "osqp_interface/osqp_interface.hpp" +#include "tier4_autoware_utils/geometry/geometry.hpp" +#include "tier4_autoware_utils/trajectory/trajectory.hpp" #include "autoware_auto_planning_msgs/msg/trajectory_point.hpp" diff --git a/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/smoother_base.hpp b/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/smoother_base.hpp index c14bdc0c21ad5..75b86d5a51397 100644 --- a/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/smoother_base.hpp +++ b/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/smoother_base.hpp @@ -15,11 +15,11 @@ #ifndef MOTION_VELOCITY_SMOOTHER__SMOOTHER__SMOOTHER_BASE_HPP_ #define MOTION_VELOCITY_SMOOTHER__SMOOTHER__SMOOTHER_BASE_HPP_ -#include "autoware_utils/geometry/geometry.hpp" -#include "autoware_utils/trajectory/trajectory.hpp" #include "motion_velocity_smoother/resample.hpp" #include "motion_velocity_smoother/trajectory_utils.hpp" #include "rclcpp/rclcpp.hpp" +#include "tier4_autoware_utils/geometry/geometry.hpp" +#include "tier4_autoware_utils/trajectory/trajectory.hpp" #include "autoware_auto_planning_msgs/msg/trajectory_point.hpp" diff --git a/planning/motion_velocity_smoother/include/motion_velocity_smoother/trajectory_utils.hpp b/planning/motion_velocity_smoother/include/motion_velocity_smoother/trajectory_utils.hpp index aea9fed6641a9..8a930bd9244ca 100644 --- a/planning/motion_velocity_smoother/include/motion_velocity_smoother/trajectory_utils.hpp +++ b/planning/motion_velocity_smoother/include/motion_velocity_smoother/trajectory_utils.hpp @@ -15,8 +15,8 @@ #ifndef MOTION_VELOCITY_SMOOTHER__TRAJECTORY_UTILS_HPP_ #define MOTION_VELOCITY_SMOOTHER__TRAJECTORY_UTILS_HPP_ -#include "autoware_utils/geometry/geometry.hpp" -#include "autoware_utils/trajectory/trajectory.hpp" +#include "tier4_autoware_utils/geometry/geometry.hpp" +#include "tier4_autoware_utils/trajectory/trajectory.hpp" #include "autoware_auto_planning_msgs/msg/trajectory_point.hpp" #include "geometry_msgs/msg/pose.hpp" diff --git a/planning/motion_velocity_smoother/package.xml b/planning/motion_velocity_smoother/package.xml index 4e4b1adc06b0d..93b6bf59625ea 100644 --- a/planning/motion_velocity_smoother/package.xml +++ b/planning/motion_velocity_smoother/package.xml @@ -14,7 +14,6 @@ eigen3_cmake_module autoware_auto_planning_msgs - autoware_utils geometry_msgs interpolation libboost-dev @@ -22,6 +21,7 @@ osqp_interface tf2 tf2_ros + tier4_autoware_utils tier4_debug_msgs tier4_planning_msgs diff --git a/planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp b/planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp index c51a6ec640f9a..9c6edbf1d29a4 100644 --- a/planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp +++ b/planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp @@ -14,10 +14,10 @@ #include "motion_velocity_smoother/motion_velocity_smoother_node.hpp" -#include "autoware_utils/ros/update_param.hpp" #include "motion_velocity_smoother/smoother/jerk_filtered_smoother.hpp" #include "motion_velocity_smoother/smoother/l2_pseudo_jerk_smoother.hpp" #include "motion_velocity_smoother/smoother/linf_pseudo_jerk_smoother.hpp" +#include "tier4_autoware_utils/ros/update_param.hpp" #include #include @@ -39,7 +39,7 @@ MotionVelocitySmootherNode::MotionVelocitySmootherNode(const rclcpp::NodeOptions // set common params initCommonParam(); over_stop_velocity_warn_thr_ = - declare_parameter("over_stop_velocity_warn_thr", autoware_utils::kmph2mps(5.0)); + declare_parameter("over_stop_velocity_warn_thr", tier4_autoware_utils::kmph2mps(5.0)); // create smoother initSmootherBaseParam(); @@ -249,7 +249,8 @@ void MotionVelocitySmootherNode::initCommonParam() p.engage_acceleration = declare_parameter("engage_acceleration", 0.1); p.engage_exit_ratio = declare_parameter("engage_exit_ratio", 0.5); p.engage_exit_ratio = std::min(std::max(p.engage_exit_ratio, 0.0), 1.0); - p.stopping_velocity = declare_parameter("stopping_velocity", autoware_utils::kmph2mps(10.0)); + p.stopping_velocity = + declare_parameter("stopping_velocity", tier4_autoware_utils::kmph2mps(10.0)); p.stopping_distance = declare_parameter("stopping_distance", 0.0); p.extract_ahead_dist = declare_parameter("extract_ahead_dist", 200.0); p.extract_behind_dist = declare_parameter("extract_behind_dist", 3.0); @@ -346,7 +347,7 @@ void MotionVelocitySmootherNode::initAnalyticalJerkConstrainedSmootherParam() void MotionVelocitySmootherNode::publishTrajectory(const TrajectoryPoints & trajectory) const { - Trajectory publishing_trajectory = autoware_utils::convertToTrajectory(trajectory); + Trajectory publishing_trajectory = tier4_autoware_utils::convertToTrajectory(trajectory); publishing_trajectory.header = base_traj_raw_ptr_->header; pub_trajectory_->publish(publishing_trajectory); } @@ -454,15 +455,15 @@ void MotionVelocitySmootherNode::onCurrentTrajectory(const Trajectory::ConstShar } // calculate trajectory velocity - TrajectoryPoints output = - calcTrajectoryVelocity(autoware_utils::convertToTrajectoryPointArray(*base_traj_raw_ptr_)); + TrajectoryPoints output = calcTrajectoryVelocity( + tier4_autoware_utils::convertToTrajectoryPointArray(*base_traj_raw_ptr_)); if (output.empty()) { RCLCPP_WARN(get_logger(), "Output Point is empty"); return; } // Get the nearest point - const auto output_closest_idx = autoware_utils::findNearestIndex( + const auto output_closest_idx = tier4_autoware_utils::findNearestIndex( output, current_pose_ptr_->pose, std::numeric_limits::max(), node_param_.delta_yaw_threshold); const auto output_closest_point = @@ -512,7 +513,7 @@ TrajectoryPoints MotionVelocitySmootherNode::calcTrajectoryVelocity( TrajectoryPoints output{}; // velocity is optimized by qp solver // Extract trajectory around self-position with desired forward-backward length - const auto input_closest = autoware_utils::findNearestIndex( + const auto input_closest = tier4_autoware_utils::findNearestIndex( traj_input, current_pose_ptr_->pose, std::numeric_limits::max(), node_param_.delta_yaw_threshold); if (!input_closest) { @@ -550,7 +551,7 @@ TrajectoryPoints MotionVelocitySmootherNode::calcTrajectoryVelocity( applyExternalVelocityLimit(*traj_extracted); // Change trajectory velocity to zero when current_velocity == 0 & stop_dist is close - const auto traj_extracted_closest = autoware_utils::findNearestIndex( + const auto traj_extracted_closest = tier4_autoware_utils::findNearestIndex( *traj_extracted, current_pose_ptr_->pose, std::numeric_limits::max(), node_param_.delta_yaw_threshold); if (!traj_extracted_closest) { @@ -591,7 +592,7 @@ bool MotionVelocitySmootherNode::smoothVelocity( } // Resample trajectory with ego-velocity based interval distance - const auto traj_pre_resampled_closest = autoware_utils::findNearestIndex( + const auto traj_pre_resampled_closest = tier4_autoware_utils::findNearestIndex( *traj_lateral_acc_filtered, current_pose_ptr_->pose, std::numeric_limits::max(), node_param_.delta_yaw_threshold); auto traj_resampled = smoother_->resampleTrajectory( @@ -614,7 +615,7 @@ bool MotionVelocitySmootherNode::smoothVelocity( double initial_vel{}; double initial_acc{}; InitializeType type{}; - const auto traj_resampled_closest = autoware_utils::findNearestIndex( + const auto traj_resampled_closest = tier4_autoware_utils::findNearestIndex( *traj_resampled, current_pose_ptr_->pose, std::numeric_limits::max(), node_param_.delta_yaw_threshold); if (!traj_resampled_closest) { @@ -706,7 +707,7 @@ void MotionVelocitySmootherNode::publishStopDistance( // stop distance calculation const double stop_dist_lim{50.0}; double stop_dist{stop_dist_lim}; - const auto stop_idx{autoware_utils::searchZeroVelocityIndex(trajectory)}; + const auto stop_idx{tier4_autoware_utils::searchZeroVelocityIndex(trajectory)}; if (stop_idx) { stop_dist = trajectory_utils::calcArcLength(trajectory, closest, *stop_idx); stop_dist = closest > *stop_idx ? stop_dist : -stop_dist; @@ -762,10 +763,10 @@ MotionVelocitySmootherNode::calcInitialMotion( const double engage_vel_thr = node_param_.engage_velocity * node_param_.engage_exit_ratio; if (vehicle_speed < engage_vel_thr) { if (target_vel >= node_param_.engage_velocity) { - const auto idx = autoware_utils::searchZeroVelocityIndex(input_traj); - const double stop_dist = - idx ? autoware_utils::calcDistance2d(input_traj.at(*idx), input_traj.at(input_closest)) - : 0.0; + const auto idx = tier4_autoware_utils::searchZeroVelocityIndex(input_traj); + const double stop_dist = idx ? tier4_autoware_utils::calcDistance2d( + input_traj.at(*idx), input_traj.at(input_closest)) + : 0.0; if (!idx || stop_dist > node_param_.stop_dist_to_prohibit_engage) { type = InitializeType::ENGAGING; initial_vel = node_param_.engage_velocity; @@ -803,13 +804,13 @@ MotionVelocitySmootherNode::calcInitialMotion( void MotionVelocitySmootherNode::overwriteStopPoint( const TrajectoryPoints & input, TrajectoryPoints & output) const { - const auto stop_idx = autoware_utils::searchZeroVelocityIndex(input); + const auto stop_idx = tier4_autoware_utils::searchZeroVelocityIndex(input); if (!stop_idx) { return; } // Get Closest Point from Output - const auto nearest_output_point_idx = autoware_utils::findNearestIndex( + const auto nearest_output_point_idx = tier4_autoware_utils::findNearestIndex( output, input.at(*stop_idx).pose, std::numeric_limits::max(), node_param_.delta_yaw_threshold); @@ -855,7 +856,7 @@ void MotionVelocitySmootherNode::applyExternalVelocityLimit(TrajectoryPoints & t trajectory_utils::applyMaximumVelocityLimit( 0, traj.size(), max_velocity_with_deceleration_, traj); - const auto closest_idx = autoware_utils::findNearestIndex( + const auto closest_idx = tier4_autoware_utils::findNearestIndex( traj, current_pose_ptr_->pose, std::numeric_limits::max(), node_param_.delta_yaw_threshold); if (!closest_idx) { @@ -864,7 +865,7 @@ void MotionVelocitySmootherNode::applyExternalVelocityLimit(TrajectoryPoints & t double dist = 0.0; for (size_t idx = *closest_idx; idx < traj.size() - 1; ++idx) { - dist += autoware_utils::calcDistance2d(traj.at(idx), traj.at(idx + 1)); + dist += tier4_autoware_utils::calcDistance2d(traj.at(idx), traj.at(idx + 1)); if (dist > external_velocity_limit_dist_) { trajectory_utils::applyMaximumVelocityLimit( idx + 1, traj.size(), external_velocity_limit_, traj); @@ -878,13 +879,13 @@ void MotionVelocitySmootherNode::applyExternalVelocityLimit(TrajectoryPoints & t void MotionVelocitySmootherNode::applyStopApproachingVelocity(TrajectoryPoints & traj) const { - const auto stop_idx = autoware_utils::searchZeroVelocityIndex(traj); + const auto stop_idx = tier4_autoware_utils::searchZeroVelocityIndex(traj); if (!stop_idx) { return; // no stop point. } double distance_sum = 0.0; for (size_t i = *stop_idx - 1; i < traj.size(); --i) { // search backward - distance_sum += autoware_utils::calcDistance2d(traj.at(i), traj.at(i + 1)); + distance_sum += tier4_autoware_utils::calcDistance2d(traj.at(i), traj.at(i + 1)); if (distance_sum > node_param_.stopping_distance) { break; } @@ -984,7 +985,8 @@ double MotionVelocitySmootherNode::calcTravelDistance() const trajectory_utils::calcInterpolatedTrajectoryPoint(prev_output_, current_pose_ptr_->pose); if (prev_closest_point_) { - const double travel_dist = autoware_utils::calcDistance2d(*prev_closest_point_, closest_point); + const double travel_dist = + tier4_autoware_utils::calcDistance2d(*prev_closest_point_, closest_point); return travel_dist; } @@ -1001,7 +1003,7 @@ bool MotionVelocitySmootherNode::isEngageStatus(const double target_vel) const Trajectory MotionVelocitySmootherNode::toTrajectoryMsg( const TrajectoryPoints & points, const std_msgs::msg::Header & header) const { - auto trajectory = autoware_utils::convertToTrajectory(points); + auto trajectory = tier4_autoware_utils::convertToTrajectory(points); trajectory.header = header; return trajectory; } diff --git a/planning/motion_velocity_smoother/src/resample.cpp b/planning/motion_velocity_smoother/src/resample.cpp index aa70a3ec99eb5..d0768086ffa4b 100644 --- a/planning/motion_velocity_smoother/src/resample.cpp +++ b/planning/motion_velocity_smoother/src/resample.cpp @@ -29,13 +29,13 @@ boost::optional resampleTrajectory( const double front_arclength_value = trajectory_utils::calcArcLength(input, 0, closest_id); // Get the nearest point where velocity is zero - auto zero_vel_id = autoware_utils::searchZeroVelocityIndex(input, closest_id, input.size()); + auto zero_vel_id = tier4_autoware_utils::searchZeroVelocityIndex(input, closest_id, input.size()); // Arc length from the closest point to the point where velocity is zero double zero_vel_arclength_value = param.max_trajectory_length; if (zero_vel_id) { zero_vel_arclength_value = std::min( zero_vel_arclength_value, - autoware_utils::calcSignedArcLength(input, closest_id, *zero_vel_id)); + tier4_autoware_utils::calcSignedArcLength(input, closest_id, *zero_vel_id)); } // Get the resample size from the closest point @@ -132,7 +132,7 @@ boost::optional resampleTrajectory( // add end point directly to consider the endpoint velocity. if (is_endpoint_included) { constexpr double ep_dist = 1.0E-3; - if (autoware_utils::calcDistance2d(output->back(), input.back()) < ep_dist) { + if (tier4_autoware_utils::calcDistance2d(output->back(), input.back()) < ep_dist) { output->back() = input.back(); } else { output->push_back(input.back()); @@ -151,12 +151,12 @@ boost::optional resampleTrajectory( // Get the nearest point where velocity is zero // to avoid getting closest_id as a stop point, search zero velocity index from closest_id + 1. - auto stop_id = autoware_utils::searchZeroVelocityIndex(input, closest_id + 1, input.size()); + auto stop_id = tier4_autoware_utils::searchZeroVelocityIndex(input, closest_id + 1, input.size()); // Arc length from the closest point to the point where velocity is zero double stop_arclength_value = param.max_trajectory_length; if (stop_id) { stop_arclength_value = std::min( - stop_arclength_value, autoware_utils::calcSignedArcLength(input, closest_id, *stop_id)); + stop_arclength_value, tier4_autoware_utils::calcSignedArcLength(input, closest_id, *stop_id)); } // Do dense resampling before the stop line(3[m] ahead of the stop line) @@ -252,7 +252,7 @@ boost::optional resampleTrajectory( // add end point directly to consider the endpoint velocity. if (is_endpoint_included) { constexpr double ep_dist = 1.0E-3; - if (autoware_utils::calcDistance2d(output->back(), input.back()) < ep_dist) { + if (tier4_autoware_utils::calcDistance2d(output->back(), input.back()) < ep_dist) { output->back() = input.back(); } else { output->push_back(input.back()); diff --git a/planning/motion_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.cpp b/planning/motion_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.cpp index da0c630a05c55..31792e1322f45 100644 --- a/planning/motion_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.cpp +++ b/planning/motion_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.cpp @@ -223,7 +223,7 @@ boost::optional AnalyticalJerkConstrainedSmoother::resampleTra const auto tp1 = input.at(i + 1); const double dist_thr = 0.001; // 1mm - const double dist_tp0_tp1 = autoware_utils::calcDistance2d(tp0, tp1); + const double dist_tp0_tp1 = tier4_autoware_utils::calcDistance2d(tp0, tp1); if (std::fabs(dist_tp0_tp1) < dist_thr) { output.push_back(input.at(i)); continue; @@ -325,7 +325,9 @@ boost::optional AnalyticalJerkConstrainedSmoother::applyLatera continue; } - if (autoware_utils::calcDistance2d(output->at(end_index), output->at(index)) < dist_threshold) { + if ( + tier4_autoware_utils::calcDistance2d(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); @@ -410,7 +412,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_utils::calcDistance2d(base_trajectory.at(i - 1), base_trajectory.at(i)); + tier4_autoware_utils::calcDistance2d(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; @@ -456,7 +458,8 @@ bool AnalyticalJerkConstrainedSmoother::applyBackwardDecelFilter( } } for (size_t i = decel_target_index; i > start_index; --i) { - dist += autoware_utils::calcDistance2d(output_trajectory.at(i - 1), output_trajectory.at(i)); + dist += + tier4_autoware_utils::calcDistance2d(output_trajectory.at(i - 1), output_trajectory.at(i)); dist_to_target.at(i - 1) = dist; } diff --git a/planning/motion_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/velocity_planning_utils.cpp b/planning/motion_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/velocity_planning_utils.cpp index 4d9da44ab8fbe..8fabcb13be0f3 100644 --- a/planning/motion_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/velocity_planning_utils.cpp +++ b/planning/motion_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/velocity_planning_utils.cpp @@ -226,7 +226,8 @@ bool calcStopVelocityWithConstantJerkAccLimit( std::vector dists; dists.push_back(dist); for (size_t i = start_index; i < output_trajectory.size() - 1; ++i) { - dist += autoware_utils::calcDistance2d(output_trajectory.at(i), output_trajectory.at(i + 1)); + dist += + tier4_autoware_utils::calcDistance2d(output_trajectory.at(i), output_trajectory.at(i + 1)); if (dist > xs.back()) { break; } diff --git a/planning/motion_velocity_smoother/src/smoother/jerk_filtered_smoother.cpp b/planning/motion_velocity_smoother/src/smoother/jerk_filtered_smoother.cpp index 596df3d26f6f1..7809c7b4828a6 100644 --- a/planning/motion_velocity_smoother/src/smoother/jerk_filtered_smoother.cpp +++ b/planning/motion_velocity_smoother/src/smoother/jerk_filtered_smoother.cpp @@ -115,7 +115,7 @@ bool JerkFilteredSmoother::apply( // to avoid getting 0 as a stop point, search zero velocity index from 1. // the size of the resampled trajectory must not be less than 2. - const auto zero_vel_id = autoware_utils::searchZeroVelocityIndex( + const auto zero_vel_id = tier4_autoware_utils::searchZeroVelocityIndex( *opt_resampled_trajectory, 1, opt_resampled_trajectory->size()); if (!zero_vel_id) { @@ -374,7 +374,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_utils::calcDistance2d(input.at(i), input.at(i - 1)); + const double ds = tier4_autoware_utils::calcDistance2d(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); @@ -430,7 +430,7 @@ TrajectoryPoints JerkFilteredSmoother::mergeFilteredTrajectory( merged.at(i).acceleration_mps2 = current_acc; const double ds = - autoware_utils::calcDistance2d(forward_filtered.at(i + 1), forward_filtered.at(i)); + tier4_autoware_utils::calcDistance2d(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); diff --git a/planning/motion_velocity_smoother/src/trajectory_utils.cpp b/planning/motion_velocity_smoother/src/trajectory_utils.cpp index 8f0618c39b574..de3250da63d3e 100644 --- a/planning/motion_velocity_smoother/src/trajectory_utils.cpp +++ b/planning/motion_velocity_smoother/src/trajectory_utils.cpp @@ -33,7 +33,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_utils::normalizeRadian(da); + a[i] = a[i - 1] + tier4_autoware_utils::normalizeRadian(da); } } @@ -76,7 +76,7 @@ TrajectoryPoint calcInterpolatedTrajectoryPoint( } const size_t segment_idx = - autoware_utils::findNearestSegmentIndex(trajectory, target_pose.position); + tier4_autoware_utils::findNearestSegmentIndex(trajectory, target_pose.position); auto v1 = getTransVector3(trajectory.at(segment_idx).pose, trajectory.at(segment_idx + 1).pose); auto v2 = getTransVector3(trajectory.at(segment_idx).pose, target_pose); @@ -112,7 +112,7 @@ boost::optional extractPathAroundIndex( { double dist_sum = 0.0; for (size_t i = index; i < trajectory.size() - 1; ++i) { - dist_sum += autoware_utils::calcDistance2d(trajectory.at(i), trajectory.at(i + 1)); + dist_sum += tier4_autoware_utils::calcDistance2d(trajectory.at(i), trajectory.at(i + 1)); if (dist_sum > ahead_length) { ahead_index = i + 1; break; @@ -125,7 +125,7 @@ boost::optional extractPathAroundIndex( { double dist_sum{0.0}; for (size_t i = index; i != 0; --i) { - dist_sum += autoware_utils::calcDistance2d(trajectory.at(i), trajectory[i - 1]); + dist_sum += tier4_autoware_utils::calcDistance2d(trajectory.at(i), trajectory[i - 1]); if (dist_sum > behind_length) { behind_index = i - 1; break; @@ -161,7 +161,7 @@ double calcArcLength(const TrajectoryPoints & path, const int idx1, const int id const int idx_to = std::max(idx1, idx2); double dist_sum = 0.0; for (int i = idx_from; i < idx_to; ++i) { - dist_sum += autoware_utils::calcDistance2d(path.at(i), path.at(i + 1)); + dist_sum += tier4_autoware_utils::calcDistance2d(path.at(i), path.at(i + 1)); } return dist_sum; } @@ -175,7 +175,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_utils::calcDistance2d(tp.pose, tp_prev.pose); + dist += tier4_autoware_utils::calcDistance2d(tp.pose, tp_prev.pose); arclength.push_back(dist); } return arclength; @@ -187,7 +187,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_utils::calcDistance2d(tp.pose, tp_prev.pose); + const double dist = tier4_autoware_utils::calcDistance2d(tp.pose, tp_prev.pose); intervals.push_back(dist); } return intervals; @@ -216,8 +216,8 @@ boost::optional> calcTrajectoryCurvatureFrom3Points( p2.y = trajectory.at(i).pose.position.y; p3.y = trajectory.at(i + idx_dist).pose.position.y; double den = std::max( - autoware_utils::calcDistance2d(p1, p2) * autoware_utils::calcDistance2d(p2, p3) * - autoware_utils::calcDistance2d(p3, p1), + tier4_autoware_utils::calcDistance2d(p1, p2) * tier4_autoware_utils::calcDistance2d(p2, p3) * + tier4_autoware_utils::calcDistance2d(p3, p1), 0.0001); double curvature = 2.0 * ((p2.x - p1.x) * (p3.y - p1.y) - (p2.y - p1.y) * (p3.x - p1.x)) / den; k_arr.push_back(curvature); @@ -325,7 +325,7 @@ boost::optional applyLinearInterpolation( point.pose.position.x = px_p->at(i); point.pose.position.y = py_p->at(i); point.pose.position.z = pz_p->at(i); - point.pose.orientation = autoware_utils::createQuaternionFromYaw(pyaw_p->at(i)); + point.pose.orientation = tier4_autoware_utils::createQuaternionFromYaw(pyaw_p->at(i)); point.longitudinal_velocity_mps = tlx_p->at(i); point.heading_rate_rps = taz_p->at(i); diff --git a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/util.hpp b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/util.hpp index bb559f2a489f4..1ad7a77cf416a 100644 --- a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/util.hpp +++ b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/util.hpp @@ -15,8 +15,8 @@ #ifndef OBSTACLE_AVOIDANCE_PLANNER__UTIL_HPP_ #define OBSTACLE_AVOIDANCE_PLANNER__UTIL_HPP_ -#include #include +#include #include #include diff --git a/planning/obstacle_avoidance_planner/package.xml b/planning/obstacle_avoidance_planner/package.xml index b72eeb7a50356..b76c6db96f215 100644 --- a/planning/obstacle_avoidance_planner/package.xml +++ b/planning/obstacle_avoidance_planner/package.xml @@ -12,7 +12,6 @@ autoware_auto_perception_msgs autoware_auto_planning_msgs - autoware_utils geometry_msgs interpolation nav_msgs @@ -21,6 +20,7 @@ rclcpp_components std_msgs tf2_ros + tier4_autoware_utils tier4_planning_msgs vehicle_info_util visualization_msgs diff --git a/planning/obstacle_avoidance_planner/src/node.cpp b/planning/obstacle_avoidance_planner/src/node.cpp index bb1b4fb8b2d1c..68064d2be7896 100644 --- a/planning/obstacle_avoidance_planner/src/node.cpp +++ b/planning/obstacle_avoidance_planner/src/node.cpp @@ -20,10 +20,10 @@ #include "obstacle_avoidance_planner/process_cv.hpp" #include "obstacle_avoidance_planner/util.hpp" -#include #include #include #include +#include #include #include @@ -348,7 +348,7 @@ autoware_auto_planning_msgs::msg::Trajectory ObstacleAvoidancePlanner::generateT const auto post_processed_traj = generatePostProcessedTrajectory(*current_ego_pose_ptr_, path.points, traj_points); - auto output = autoware_utils::convertToTrajectory(post_processed_traj); + auto output = tier4_autoware_utils::convertToTrajectory(post_processed_traj); output.header = path.header; prev_path_points_ptr_ = @@ -597,11 +597,12 @@ void ObstacleAvoidancePlanner::publishingDebugData( const std::vector & traj_points, const VehicleParam & vehicle_param) { - auto traj = autoware_utils::convertToTrajectory(debug_data.foa_data.avoiding_traj_points); + auto traj = tier4_autoware_utils::convertToTrajectory(debug_data.foa_data.avoiding_traj_points); traj.header = path.header; avoiding_traj_pub_->publish(traj); - auto debug_smoothed_points = autoware_utils::convertToTrajectory(debug_data.smoothed_points); + auto debug_smoothed_points = + tier4_autoware_utils::convertToTrajectory(debug_data.smoothed_points); debug_smoothed_points.header = path.header; debug_smoothed_points_pub_->publish(debug_smoothed_points); diff --git a/planning/obstacle_avoidance_planner/src/util.cpp b/planning/obstacle_avoidance_planner/src/util.cpp index dc8d171169baf..527726e5cd801 100644 --- a/planning/obstacle_avoidance_planner/src/util.cpp +++ b/planning/obstacle_avoidance_planner/src/util.cpp @@ -630,13 +630,13 @@ std::vector alignVelocityWith const T truncated_points(first, last); const size_t closest_seg_idx = - autoware_utils::findNearestSegmentIndex(truncated_points, traj_points[i].pose.position); + tier4_autoware_utils::findNearestSegmentIndex(truncated_points, traj_points[i].pose.position); // TODO(murooka) implement calcSignedArcLength(points, idx, point) - const double closest_to_target_dist = autoware_utils::calcSignedArcLength( + const double closest_to_target_dist = tier4_autoware_utils::calcSignedArcLength( truncated_points, truncated_points.at(closest_seg_idx).pose.position, traj_points[i].pose.position); - const double seg_dist = - autoware_utils::calcSignedArcLength(truncated_points, closest_seg_idx, closest_seg_idx + 1); + const double seg_dist = tier4_autoware_utils::calcSignedArcLength( + truncated_points, closest_seg_idx, closest_seg_idx + 1); // interpolate 1st-nearest (v1) value and 2nd-nearest value (v2) const auto lerp = [&](const double v1, const double v2, const double ratio) { diff --git a/planning/obstacle_stop_planner/include/obstacle_stop_planner/node.hpp b/planning/obstacle_stop_planner/include/obstacle_stop_planner/node.hpp index 06e0cf9d81885..c68e3fad50ddb 100644 --- a/planning/obstacle_stop_planner/include/obstacle_stop_planner/node.hpp +++ b/planning/obstacle_stop_planner/include/obstacle_stop_planner/node.hpp @@ -18,14 +18,14 @@ #include "obstacle_stop_planner/adaptive_cruise_control.hpp" #include "obstacle_stop_planner/debug_marker.hpp" -#include -#include #include #include #include #include #include #include +#include +#include #include #include @@ -63,8 +63,8 @@ using autoware_auto_perception_msgs::msg::PredictedObjects; using autoware_auto_planning_msgs::msg::Trajectory; using autoware_auto_planning_msgs::msg::TrajectoryPoint; using TrajectoryPoints = std::vector; -using autoware_utils::Point2d; -using autoware_utils::Polygon2d; +using tier4_autoware_utils::Point2d; +using tier4_autoware_utils::Polygon2d; using tier4_debug_msgs::msg::BoolStamped; using tier4_debug_msgs::msg::Float32MultiArrayStamped; using tier4_debug_msgs::msg::Float32Stamped; diff --git a/planning/obstacle_stop_planner/package.xml b/planning/obstacle_stop_planner/package.xml index 6388ad674fe05..45100bec069f6 100644 --- a/planning/obstacle_stop_planner/package.xml +++ b/planning/obstacle_stop_planner/package.xml @@ -12,7 +12,6 @@ autoware_auto_perception_msgs autoware_auto_planning_msgs - autoware_utils diagnostic_msgs nav_msgs pcl_conversions @@ -26,6 +25,7 @@ tf2_eigen tf2_geometry_msgs tf2_ros + tier4_autoware_utils tier4_debug_msgs tier4_planning_msgs vehicle_info_util diff --git a/planning/obstacle_stop_planner/src/debug_marker.cpp b/planning/obstacle_stop_planner/src/debug_marker.cpp index e9cc70f96138d..42358e1aed101 100644 --- a/planning/obstacle_stop_planner/src/debug_marker.cpp +++ b/planning/obstacle_stop_planner/src/debug_marker.cpp @@ -14,22 +14,22 @@ #include "obstacle_stop_planner/debug_marker.hpp" -#include +#include #include #include #include -using autoware_utils::appendMarkerArray; -using autoware_utils::calcOffsetPose; -using autoware_utils::createDefaultMarker; -using autoware_utils::createMarkerColor; -using autoware_utils::createMarkerOrientation; -using autoware_utils::createMarkerScale; -using autoware_utils::createPoint; -using autoware_utils::createSlowDownVirtualWallMarker; -using autoware_utils::createStopVirtualWallMarker; +using tier4_autoware_utils::appendMarkerArray; +using tier4_autoware_utils::calcOffsetPose; +using tier4_autoware_utils::createDefaultMarker; +using tier4_autoware_utils::createMarkerColor; +using tier4_autoware_utils::createMarkerOrientation; +using tier4_autoware_utils::createMarkerScale; +using tier4_autoware_utils::createPoint; +using tier4_autoware_utils::createSlowDownVirtualWallMarker; +using tier4_autoware_utils::createStopVirtualWallMarker; namespace motion_planning { diff --git a/planning/obstacle_stop_planner/src/node.cpp b/planning/obstacle_stop_planner/src/node.cpp index 1fcdbbff79365..e94d6dceab84e 100644 --- a/planning/obstacle_stop_planner/src/node.cpp +++ b/planning/obstacle_stop_planner/src/node.cpp @@ -34,12 +34,12 @@ namespace motion_planning { -using autoware_utils::calcAzimuthAngle; -using autoware_utils::calcDistance2d; -using autoware_utils::calcSignedArcLength; -using autoware_utils::createPoint; -using autoware_utils::findNearestIndex; -using autoware_utils::getRPY; +using tier4_autoware_utils::calcAzimuthAngle; +using tier4_autoware_utils::calcDistance2d; +using tier4_autoware_utils::calcSignedArcLength; +using tier4_autoware_utils::createPoint; +using tier4_autoware_utils::findNearestIndex; +using tier4_autoware_utils::getRPY; namespace { @@ -566,11 +566,11 @@ void ObstacleStopPlannerNode::pathCallback(const Trajectory::ConstSharedPtr inpu Trajectory output_trajectory = *input_msg; TrajectoryPoints output_trajectory_points = - autoware_utils::convertToTrajectoryPointArray(*input_msg); + tier4_autoware_utils::convertToTrajectoryPointArray(*input_msg); // trim trajectory from self pose const auto base_trajectory = trimTrajectoryWithIndexFromSelfPose( - autoware_utils::convertToTrajectoryPointArray(*input_msg), planner_data.current_pose, + tier4_autoware_utils::convertToTrajectoryPointArray(*input_msg), planner_data.current_pose, planner_data.trajectory_trim_index); // extend trajectory to consider obstacles after the goal const auto extend_trajectory = extendTrajectory(base_trajectory, stop_param_.extend_distance); @@ -590,7 +590,7 @@ void ObstacleStopPlannerNode::pathCallback(const Trajectory::ConstSharedPtr inpu resetExternalVelocityLimit(); } - auto trajectory = autoware_utils::convertToTrajectory(output_trajectory_points); + auto trajectory = tier4_autoware_utils::convertToTrajectory(output_trajectory_points); trajectory.header = input_msg->header; path_pub_->publish(trajectory); publishDebugData(planner_data); diff --git a/planning/planning_error_monitor/package.xml b/planning/planning_error_monitor/package.xml index a412ef8f60bbc..8bef053fc095d 100644 --- a/planning/planning_error_monitor/package.xml +++ b/planning/planning_error_monitor/package.xml @@ -11,13 +11,13 @@ ament_cmake_auto autoware_auto_planning_msgs - autoware_utils diagnostic_updater geometry_msgs rclcpp rclcpp_components tf2 tf2_ros + tier4_autoware_utils visualization_msgs ament_cmake_gtest diff --git a/planning/planning_error_monitor/src/debug_marker.cpp b/planning/planning_error_monitor/src/debug_marker.cpp index a52a7da6bf454..60404d2059793 100644 --- a/planning/planning_error_monitor/src/debug_marker.cpp +++ b/planning/planning_error_monitor/src/debug_marker.cpp @@ -14,7 +14,7 @@ #include "planning_error_monitor/debug_marker.hpp" -#include +#include #include #include @@ -48,18 +48,18 @@ void PlanningErrorMonitorDebugNode::pushPoseMarker( marker.type = Marker::ARROW; marker.action = Marker::ADD; marker.pose = pose; - marker.scale = autoware_utils::createMarkerScale(0.2, 0.1, 0.3); + marker.scale = tier4_autoware_utils::createMarkerScale(0.2, 0.1, 0.3); if (id == 0) // Red { - marker.color = autoware_utils::createMarkerColor(1.0, 0.0, 0.0, 0.999); + marker.color = tier4_autoware_utils::createMarkerColor(1.0, 0.0, 0.0, 0.999); } if (id == 1) // Green { - marker.color = autoware_utils::createMarkerColor(0.0, 1.0, 0.0, 0.999); + marker.color = tier4_autoware_utils::createMarkerColor(0.0, 1.0, 0.0, 0.999); } if (id == 2) // Blue { - marker.color = autoware_utils::createMarkerColor(0.0, 0.0, 1.0, 0.999); + marker.color = tier4_autoware_utils::createMarkerColor(0.0, 0.0, 1.0, 0.999); } marker_array_.markers.push_back(marker); } diff --git a/planning/planning_error_monitor/src/planning_error_monitor_node.cpp b/planning/planning_error_monitor/src/planning_error_monitor_node.cpp index 027023d53b8c8..fa5a0073d9cfd 100644 --- a/planning/planning_error_monitor/src/planning_error_monitor_node.cpp +++ b/planning/planning_error_monitor/src/planning_error_monitor_node.cpp @@ -14,7 +14,7 @@ #include "planning_error_monitor/planning_error_monitor_node.hpp" -#include +#include #include #include @@ -22,9 +22,9 @@ namespace planning_diagnostics { -using autoware_utils::calcCurvature; -using autoware_utils::calcDistance2d; using diagnostic_msgs::msg::DiagnosticStatus; +using tier4_autoware_utils::calcCurvature; +using tier4_autoware_utils::calcDistance2d; PlanningErrorMonitorNode::PlanningErrorMonitorNode(const rclcpp::NodeOptions & node_options) : Node("planning_error_monitor", node_options) diff --git a/planning/route_handler/include/route_handler/route_handler.hpp b/planning/route_handler/include/route_handler/route_handler.hpp index 558d49d28867e..ae4eb6c9b5f02 100644 --- a/planning/route_handler/include/route_handler/route_handler.hpp +++ b/planning/route_handler/include/route_handler/route_handler.hpp @@ -15,9 +15,9 @@ #ifndef ROUTE_HANDLER__ROUTE_HANDLER_HPP_ #define ROUTE_HANDLER__ROUTE_HANDLER_HPP_ -#include #include #include +#include #include #include diff --git a/planning/route_handler/package.xml b/planning/route_handler/package.xml index a86b7f60824ee..f56b62a3a23ea 100644 --- a/planning/route_handler/package.xml +++ b/planning/route_handler/package.xml @@ -13,12 +13,12 @@ autoware_auto_mapping_msgs autoware_auto_planning_msgs - autoware_utils geometry_msgs lanelet2_extension rclcpp rclcpp_components tf2_ros + tier4_autoware_utils ament_cmake diff --git a/planning/route_handler/src/route_handler.cpp b/planning/route_handler/src/route_handler.cpp index 53f6c3d83e968..9b9e464f324fe 100644 --- a/planning/route_handler/src/route_handler.cpp +++ b/planning/route_handler/src/route_handler.cpp @@ -122,7 +122,9 @@ PathWithLaneId removeOverlappingPoints(const PathWithLaneId & input_path) } constexpr double min_dist = 0.001; - if (autoware_utils::calcDistance3d(filtered_path.points.back().point, pt.point) < min_dist) { + if ( + tier4_autoware_utils::calcDistance3d(filtered_path.points.back().point, pt.point) < + min_dist) { filtered_path.points.back().lane_ids.push_back(pt.lane_ids.front()); filtered_path.points.back().point.longitudinal_velocity_mps = std::min( pt.point.longitudinal_velocity_mps, @@ -380,12 +382,12 @@ void RouteHandler::setPullOverGoalPose( lanelet::utils::getArcCoordinates({target_lane}, route_msg_.goal_pose); Path centerline_path = convertToPathFromPathWithLaneId( getCenterLinePath({target_lane}, 0.0, arc_position_goal.length + 10)); - const auto seg_idx = - autoware_utils::findNearestSegmentIndex(centerline_path.points, route_msg_.goal_pose.position); - const double d_lat = autoware_utils::calcLongitudinalOffsetToSegment( + const auto seg_idx = tier4_autoware_utils::findNearestSegmentIndex( + centerline_path.points, route_msg_.goal_pose.position); + const double d_lat = tier4_autoware_utils::calcLongitudinalOffsetToSegment( centerline_path.points, seg_idx, route_msg_.goal_pose.position); const auto shoulder_point = - autoware_utils::calcOffsetPose(centerline_path.points.at(seg_idx).pose, d_lat, 0.0, 0.0); + tier4_autoware_utils::calcOffsetPose(centerline_path.points.at(seg_idx).pose, d_lat, 0.0, 0.0); pull_over_goal_pose_.orientation = shoulder_point.orientation; pull_over_goal_pose_.position = shoulder_point.position; @@ -1018,14 +1020,14 @@ PathWithLaneId RouteHandler::getCenterLinePath( double angle{0.0}; const auto & pts = reference_path.points; if (i + 1 < reference_path.points.size()) { - angle = autoware_utils::calcAzimuthAngle( + angle = tier4_autoware_utils::calcAzimuthAngle( pts.at(i).point.pose.position, pts.at(i + 1).point.pose.position); } else if (i != 0) { - angle = autoware_utils::calcAzimuthAngle( + angle = tier4_autoware_utils::calcAzimuthAngle( pts.at(i - 1).point.pose.position, pts.at(i).point.pose.position); } reference_path.points.at(i).point.pose.orientation = - autoware_utils::createQuaternionFromYaw(angle); + tier4_autoware_utils::createQuaternionFromYaw(angle); } return reference_path; diff --git a/planning/surround_obstacle_checker/include/surround_obstacle_checker/node.hpp b/planning/surround_obstacle_checker/include/surround_obstacle_checker/node.hpp index 586d5d77a3057..7e4bbb9ffd15a 100644 --- a/planning/surround_obstacle_checker/include/surround_obstacle_checker/node.hpp +++ b/planning/surround_obstacle_checker/include/surround_obstacle_checker/node.hpp @@ -15,8 +15,8 @@ #ifndef SURROUND_OBSTACLE_CHECKER__NODE_HPP_ #define SURROUND_OBSTACLE_CHECKER__NODE_HPP_ -#include "autoware_utils/trajectory/tmp_conversion.hpp" #include "surround_obstacle_checker/debug_marker.hpp" +#include "tier4_autoware_utils/trajectory/tmp_conversion.hpp" #include #include diff --git a/planning/surround_obstacle_checker/package.xml b/planning/surround_obstacle_checker/package.xml index 3b488160d0e40..ec743d3aa9d33 100644 --- a/planning/surround_obstacle_checker/package.xml +++ b/planning/surround_obstacle_checker/package.xml @@ -13,7 +13,6 @@ eigen3_cmake_module autoware_auto_perception_msgs autoware_auto_planning_msgs - autoware_utils diagnostic_msgs eigen libpcl-all-dev @@ -26,6 +25,7 @@ tf2_eigen tf2_geometry_msgs tf2_ros + tier4_autoware_utils tier4_planning_msgs vehicle_info_util visualization_msgs diff --git a/planning/surround_obstacle_checker/src/node.cpp b/planning/surround_obstacle_checker/src/node.cpp index 75adc2962cabd..30cde09775e18 100644 --- a/planning/surround_obstacle_checker/src/node.cpp +++ b/planning/surround_obstacle_checker/src/node.cpp @@ -93,7 +93,7 @@ void SurroundObstacleCheckerNode::pathCallback( // parameter description TrajectoryPoints output_trajectory_points = - autoware_utils::convertToTrajectoryPointArray(*input_msg); + tier4_autoware_utils::convertToTrajectoryPointArray(*input_msg); diagnostic_msgs::msg::DiagnosticStatus no_start_reason_diag; @@ -140,7 +140,7 @@ void SurroundObstacleCheckerNode::pathCallback( } // publish trajectory and debug info - auto output_msg = autoware_utils::convertToTrajectory(output_trajectory_points); + auto output_msg = tier4_autoware_utils::convertToTrajectory(output_trajectory_points); output_msg.header = input_msg->header; path_pub_->publish(output_msg); stop_reason_diag_pub_->publish(no_start_reason_diag); diff --git a/sensing/laserscan_to_occupancy_grid_map/package.xml b/sensing/laserscan_to_occupancy_grid_map/package.xml index 503b3a4ecc304..df2868ba9dfdd 100644 --- a/sensing/laserscan_to_occupancy_grid_map/package.xml +++ b/sensing/laserscan_to_occupancy_grid_map/package.xml @@ -9,7 +9,6 @@ Apache License 2.0 ament_cmake_auto - autoware_utils eigen3_cmake_module laser_geometry message_filters @@ -23,6 +22,7 @@ tf2_geometry_msgs tf2_ros tf2_sensor_msgs + tier4_autoware_utils visualization_msgs pointcloud_to_laserscan diff --git a/sensing/laserscan_to_occupancy_grid_map/src/laserscan_to_occupancy_grid_map_node.cpp b/sensing/laserscan_to_occupancy_grid_map/src/laserscan_to_occupancy_grid_map_node.cpp index 50e5784abbce0..971303b5ce72c 100644 --- a/sensing/laserscan_to_occupancy_grid_map/src/laserscan_to_occupancy_grid_map_node.cpp +++ b/sensing/laserscan_to_occupancy_grid_map/src/laserscan_to_occupancy_grid_map_node.cpp @@ -16,8 +16,8 @@ #include "laserscan_to_occupancy_grid_map/cost_value.hpp" -#include #include +#include #include @@ -97,7 +97,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_utils::transform2pose(tf_stamped.transform); + pose = tier4_autoware_utils::transform2pose(tf_stamped.transform); return pose; } } // namespace diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/vector_map_filter/lanelet2_map_filter_nodelet.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/vector_map_filter/lanelet2_map_filter_nodelet.hpp index 9294329eb6900..59ce02d8439a0 100644 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/vector_map_filter/lanelet2_map_filter_nodelet.hpp +++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/vector_map_filter/lanelet2_map_filter_nodelet.hpp @@ -15,10 +15,10 @@ #ifndef POINTCLOUD_PREPROCESSOR__VECTOR_MAP_FILTER__LANELET2_MAP_FILTER_NODELET_HPP_ #define POINTCLOUD_PREPROCESSOR__VECTOR_MAP_FILTER__LANELET2_MAP_FILTER_NODELET_HPP_ -#include #include #include #include +#include #include #include @@ -33,9 +33,9 @@ #include #include -using autoware_utils::LinearRing2d; -using autoware_utils::MultiPoint2d; -using autoware_utils::Point2d; +using tier4_autoware_utils::LinearRing2d; +using tier4_autoware_utils::MultiPoint2d; +using tier4_autoware_utils::Point2d; namespace pointcloud_preprocessor { diff --git a/sensing/pointcloud_preprocessor/package.xml b/sensing/pointcloud_preprocessor/package.xml index 0fd2a175c1830..073f9b9581d43 100644 --- a/sensing/pointcloud_preprocessor/package.xml +++ b/sensing/pointcloud_preprocessor/package.xml @@ -18,7 +18,6 @@ ament_cmake_auto autoware_auto_vehicle_msgs - autoware_utils diagnostic_updater image_transport lanelet2_extension @@ -37,6 +36,7 @@ tf2_eigen tf2_geometry_msgs tf2_ros + tier4_autoware_utils tier4_debug_msgs tier4_pcl_extensions diff --git a/simulator/fault_injection/package.xml b/simulator/fault_injection/package.xml index 48bb0f3a8c068..72394431d1b0a 100644 --- a/simulator/fault_injection/package.xml +++ b/simulator/fault_injection/package.xml @@ -9,12 +9,12 @@ ament_cmake_auto - autoware_utils diagnostic_aggregator diagnostic_msgs diagnostic_updater rclcpp rclcpp_components + tier4_autoware_utils tier4_simulation_msgs ament_cmake_gtest diff --git a/simulator/fault_injection/src/fault_injection_node/fault_injection_node.cpp b/simulator/fault_injection/src/fault_injection_node/fault_injection_node.cpp index 5ef45885eba0d..fcbf69d5032e5 100644 --- a/simulator/fault_injection/src/fault_injection_node/fault_injection_node.cpp +++ b/simulator/fault_injection/src/fault_injection_node/fault_injection_node.cpp @@ -14,7 +14,7 @@ #include "fault_injection/fault_injection_node.hpp" -#include +#include #include #include @@ -92,7 +92,7 @@ rcl_interfaces::msg::SetParametersResult FaultInjectionNode::onSetParam( try { double value; - if (autoware_utils::updateParam(params, "diagnostic_updater.period", value)) { + if (tier4_autoware_utils::updateParam(params, "diagnostic_updater.period", value)) { updater_.setPeriod(value); } } catch (const rclcpp::exceptions::InvalidParameterTypeException & e) { diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp index 96bc91df3b674..e25160bd86f9c 100644 --- a/simulator/simple_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp +++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp @@ -49,7 +49,7 @@ #include "autoware_auto_geometry_msgs/msg/complex32.hpp" #include "common/types.hpp" -#include "autoware_api_utils/autoware_api_utils.hpp" +#include "tier4_api_utils/tier4_api_utils.hpp" #include "tier4_external_api_msgs/srv/initialize_pose.hpp" #include "simple_planning_simulator/vehicle_model/sim_model_interface.hpp" @@ -142,7 +142,7 @@ class PLANNING_SIMULATOR_PUBLIC SimplePlanningSimulator : public rclcpp::Node rclcpp::Subscription::SharedPtr sub_trajectory_; rclcpp::CallbackGroup::SharedPtr group_api_service_; - autoware_api_utils::Service::SharedPtr srv_set_pose_; + tier4_api_utils::Service::SharedPtr srv_set_pose_; uint32_t timer_sampling_time_ms_; //!< @brief timer sampling time rclcpp::TimerBase::SharedPtr on_timer_; //!< @brief timer for simulation diff --git a/simulator/simple_planning_simulator/package.xml b/simulator/simple_planning_simulator/package.xml index 345edc944265f..619694b3de0dc 100644 --- a/simulator/simple_planning_simulator/package.xml +++ b/simulator/simple_planning_simulator/package.xml @@ -10,7 +10,7 @@ ament_cmake_auto autoware_auto_cmake - autoware_api_utils + tier4_api_utils autoware_auto_planning_msgs autoware_auto_control_msgs autoware_auto_vehicle_msgs @@ -20,7 +20,7 @@ geometry_msgs nav_msgs vehicle_info_util - autoware_utils + tier4_autoware_utils motion_common diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp index ef8010b87b936..32bbcbe8fcad9 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp @@ -15,7 +15,7 @@ #include "simple_planning_simulator/simple_planning_simulator_core.hpp" #include "autoware_auto_tf2/tf2_autoware_auto_msgs.hpp" -#include "autoware_utils/ros/update_param.hpp" +#include "tier4_autoware_utils/ros/update_param.hpp" #include "common/types.hpp" #include "motion_common/motion_common.hpp" #include "rclcpp_components/register_node_macro.hpp" @@ -124,7 +124,7 @@ SimplePlanningSimulator::SimplePlanningSimulator(const rclcpp::NodeOptions & opt std::chrono::milliseconds(timer_sampling_time_ms_), std::bind(&SimplePlanningSimulator::on_timer, this)); - autoware_api_utils::ServiceProxyNodeInterface proxy(this); + tier4_api_utils::ServiceProxyNodeInterface proxy(this); group_api_service_ = create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); srv_set_pose_ = proxy.create_service( "/api/simulator/set/pose", std::bind(&SimplePlanningSimulator::on_set_pose, this, _1, _2), @@ -212,8 +212,8 @@ rcl_interfaces::msg::SetParametersResult SimplePlanningSimulator::on_parameter( result.reason = "success"; try { - autoware_utils::updateParam(parameters, "x_stddev", x_stddev_); - autoware_utils::updateParam(parameters, "y_stddev", y_stddev_); + tier4_autoware_utils::updateParam(parameters, "x_stddev", x_stddev_); + tier4_autoware_utils::updateParam(parameters, "y_stddev", y_stddev_); } catch (const rclcpp::exceptions::InvalidParameterTypeException & e) { result.successful = false; result.reason = e.what(); @@ -285,7 +285,7 @@ void SimplePlanningSimulator::on_set_pose( initial_pose.header = request->pose.header; initial_pose.pose = request->pose.pose.pose; set_initial_state_with_transform(initial_pose, initial_twist); - response->status = autoware_api_utils::response_success(); + response->status = tier4_api_utils::response_success(); } void SimplePlanningSimulator::on_ackermann_cmd( diff --git a/system/autoware_state_monitor/CMakeLists.txt b/system/ad_service_state_monitor/CMakeLists.txt similarity index 63% rename from system/autoware_state_monitor/CMakeLists.txt rename to system/ad_service_state_monitor/CMakeLists.txt index 081e6b757d92a..efc7238c2ccac 100644 --- a/system/autoware_state_monitor/CMakeLists.txt +++ b/system/ad_service_state_monitor/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.5) -project(autoware_state_monitor) +project(ad_service_state_monitor) ### Compile options if(NOT CMAKE_CXX_STANDARD) @@ -17,16 +17,16 @@ ament_auto_find_build_dependencies() # Target ## Set source files -set(AUTOWARE_STATE_MONITOR_SRC - src/autoware_state_monitor_node/autoware_state_monitor_node.cpp - src/autoware_state_monitor_node/state_machine.cpp - src/autoware_state_monitor_node/diagnostics.cpp +set(AD_SERVICE_STATE_MONITOR_SRC + src/ad_service_state_monitor_node/ad_service_state_monitor_node.cpp + src/ad_service_state_monitor_node/state_machine.cpp + src/ad_service_state_monitor_node/diagnostics.cpp ) ## Add executables -ament_auto_add_executable(autoware_state_monitor - src/autoware_state_monitor_node/main.cpp - ${AUTOWARE_STATE_MONITOR_SRC} +ament_auto_add_executable(ad_service_state_monitor + src/ad_service_state_monitor_node/main.cpp + ${AD_SERVICE_STATE_MONITOR_SRC} ) if(BUILD_TESTING) diff --git a/system/autoware_state_monitor/README.md b/system/ad_service_state_monitor/README.md similarity index 98% rename from system/autoware_state_monitor/README.md rename to system/ad_service_state_monitor/README.md index 24a659a1eb459..53ff7efc4502f 100644 --- a/system/autoware_state_monitor/README.md +++ b/system/ad_service_state_monitor/README.md @@ -1,4 +1,4 @@ -# autoware_state_monitor +# ad_service_state_monitor ## Purpose diff --git a/system/autoware_state_monitor/config/autoware_state_monitor.param.yaml b/system/ad_service_state_monitor/config/ad_service_state_monitor.param.yaml similarity index 100% rename from system/autoware_state_monitor/config/autoware_state_monitor.param.yaml rename to system/ad_service_state_monitor/config/ad_service_state_monitor.param.yaml diff --git a/system/autoware_state_monitor/config/autoware_state_monitor.planning_simulation.param.yaml b/system/ad_service_state_monitor/config/ad_service_state_monitor.planning_simulation.param.yaml similarity index 100% rename from system/autoware_state_monitor/config/autoware_state_monitor.planning_simulation.param.yaml rename to system/ad_service_state_monitor/config/ad_service_state_monitor.planning_simulation.param.yaml diff --git a/system/autoware_state_monitor/include/autoware_state_monitor/autoware_state.hpp b/system/ad_service_state_monitor/include/ad_service_state_monitor/ad_service_state.hpp similarity index 93% rename from system/autoware_state_monitor/include/autoware_state_monitor/autoware_state.hpp rename to system/ad_service_state_monitor/include/ad_service_state_monitor/ad_service_state.hpp index fc1c77f316287..000ab94812bcd 100644 --- a/system/autoware_state_monitor/include/autoware_state_monitor/autoware_state.hpp +++ b/system/ad_service_state_monitor/include/ad_service_state_monitor/ad_service_state.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE_STATE_MONITOR__AUTOWARE_STATE_HPP_ -#define AUTOWARE_STATE_MONITOR__AUTOWARE_STATE_HPP_ +#ifndef AD_SERVICE_STATE_MONITOR__AD_SERVICE_STATE_HPP_ +#define AD_SERVICE_STATE_MONITOR__AD_SERVICE_STATE_HPP_ #include @@ -87,4 +87,4 @@ inline int toMsg(const AutowareState & state) throw std::runtime_error("invalid state"); } -#endif // AUTOWARE_STATE_MONITOR__AUTOWARE_STATE_HPP_ +#endif // AD_SERVICE_STATE_MONITOR__AD_SERVICE_STATE_HPP_ diff --git a/system/autoware_state_monitor/include/autoware_state_monitor/autoware_state_monitor_node.hpp b/system/ad_service_state_monitor/include/ad_service_state_monitor/ad_service_state_monitor_node.hpp similarity index 92% rename from system/autoware_state_monitor/include/autoware_state_monitor/autoware_state_monitor_node.hpp rename to system/ad_service_state_monitor/include/ad_service_state_monitor/ad_service_state_monitor_node.hpp index e00dbcec288a4..254cdb45d5f10 100644 --- a/system/autoware_state_monitor/include/autoware_state_monitor/autoware_state_monitor_node.hpp +++ b/system/ad_service_state_monitor/include/ad_service_state_monitor/ad_service_state_monitor_node.hpp @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE_STATE_MONITOR__AUTOWARE_STATE_MONITOR_NODE_HPP_ -#define AUTOWARE_STATE_MONITOR__AUTOWARE_STATE_MONITOR_NODE_HPP_ +#ifndef AD_SERVICE_STATE_MONITOR__AD_SERVICE_STATE_MONITOR_NODE_HPP_ +#define AD_SERVICE_STATE_MONITOR__AD_SERVICE_STATE_MONITOR_NODE_HPP_ -#include "autoware_state_monitor/autoware_state.hpp" -#include "autoware_state_monitor/config.hpp" -#include "autoware_state_monitor/state_machine.hpp" +#include "ad_service_state_monitor/ad_service_state.hpp" +#include "ad_service_state_monitor/config.hpp" +#include "ad_service_state_monitor/state_machine.hpp" #include #include @@ -131,4 +131,4 @@ class AutowareStateMonitorNode : public rclcpp::Node diagnostic_updater::DiagnosticStatusWrapper & stat, const std::string & module_name); }; -#endif // AUTOWARE_STATE_MONITOR__AUTOWARE_STATE_MONITOR_NODE_HPP_ +#endif // AD_SERVICE_STATE_MONITOR__AD_SERVICE_STATE_MONITOR_NODE_HPP_ diff --git a/system/autoware_state_monitor/include/autoware_state_monitor/config.hpp b/system/ad_service_state_monitor/include/ad_service_state_monitor/config.hpp similarity index 96% rename from system/autoware_state_monitor/include/autoware_state_monitor/config.hpp rename to system/ad_service_state_monitor/include/ad_service_state_monitor/config.hpp index e758fd2f6267a..656d8233a89b2 100644 --- a/system/autoware_state_monitor/include/autoware_state_monitor/config.hpp +++ b/system/ad_service_state_monitor/include/ad_service_state_monitor/config.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE_STATE_MONITOR__CONFIG_HPP_ -#define AUTOWARE_STATE_MONITOR__CONFIG_HPP_ +#ifndef AD_SERVICE_STATE_MONITOR__CONFIG_HPP_ +#define AD_SERVICE_STATE_MONITOR__CONFIG_HPP_ #include #include @@ -119,4 +119,4 @@ struct TfStats std::vector> timeout_list; // pair }; -#endif // AUTOWARE_STATE_MONITOR__CONFIG_HPP_ +#endif // AD_SERVICE_STATE_MONITOR__CONFIG_HPP_ diff --git a/system/autoware_state_monitor/include/autoware_state_monitor/module_name.hpp b/system/ad_service_state_monitor/include/ad_service_state_monitor/module_name.hpp similarity index 87% rename from system/autoware_state_monitor/include/autoware_state_monitor/module_name.hpp rename to system/ad_service_state_monitor/include/ad_service_state_monitor/module_name.hpp index 760d08e215721..4e2a7500f3ff6 100644 --- a/system/autoware_state_monitor/include/autoware_state_monitor/module_name.hpp +++ b/system/ad_service_state_monitor/include/ad_service_state_monitor/module_name.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE_STATE_MONITOR__MODULE_NAME_HPP_ -#define AUTOWARE_STATE_MONITOR__MODULE_NAME_HPP_ +#ifndef AD_SERVICE_STATE_MONITOR__MODULE_NAME_HPP_ +#define AD_SERVICE_STATE_MONITOR__MODULE_NAME_HPP_ struct ModuleName { @@ -27,4 +27,4 @@ struct ModuleName static constexpr const char * system = "system"; }; -#endif // AUTOWARE_STATE_MONITOR__MODULE_NAME_HPP_ +#endif // AD_SERVICE_STATE_MONITOR__MODULE_NAME_HPP_ diff --git a/system/autoware_state_monitor/include/autoware_state_monitor/state_machine.hpp b/system/ad_service_state_monitor/include/ad_service_state_monitor/state_machine.hpp similarity index 88% rename from system/autoware_state_monitor/include/autoware_state_monitor/state_machine.hpp rename to system/ad_service_state_monitor/include/ad_service_state_monitor/state_machine.hpp index 99a4e1b2e2e26..764441cf84db4 100644 --- a/system/autoware_state_monitor/include/autoware_state_monitor/state_machine.hpp +++ b/system/ad_service_state_monitor/include/ad_service_state_monitor/state_machine.hpp @@ -12,14 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE_STATE_MONITOR__STATE_MACHINE_HPP_ -#define AUTOWARE_STATE_MONITOR__STATE_MACHINE_HPP_ +#ifndef AD_SERVICE_STATE_MONITOR__STATE_MACHINE_HPP_ +#define AD_SERVICE_STATE_MONITOR__STATE_MACHINE_HPP_ -#include "autoware_state_monitor/autoware_state.hpp" -#include "autoware_state_monitor/config.hpp" -#include "autoware_state_monitor/module_name.hpp" -#include "autoware_utils/math/normalization.hpp" -#include "autoware_utils/math/unit_conversion.hpp" +#include "ad_service_state_monitor/ad_service_state.hpp" +#include "ad_service_state_monitor/config.hpp" +#include "ad_service_state_monitor/module_name.hpp" +#include "tier4_autoware_utils/math/normalization.hpp" +#include "tier4_autoware_utils/math/unit_conversion.hpp" #include @@ -111,4 +111,4 @@ class StateMachine bool isRouteResetRequired() const; }; -#endif // AUTOWARE_STATE_MONITOR__STATE_MACHINE_HPP_ +#endif // AD_SERVICE_STATE_MONITOR__STATE_MACHINE_HPP_ diff --git a/system/autoware_state_monitor/launch/autoware_state_monitor.launch.xml b/system/ad_service_state_monitor/launch/ad_service_state_monitor.launch.xml similarity index 89% rename from system/autoware_state_monitor/launch/autoware_state_monitor.launch.xml rename to system/ad_service_state_monitor/launch/ad_service_state_monitor.launch.xml index 7a19bd200753b..0b2102fc1b7b6 100644 --- a/system/autoware_state_monitor/launch/autoware_state_monitor.launch.xml +++ b/system/ad_service_state_monitor/launch/ad_service_state_monitor.launch.xml @@ -14,7 +14,7 @@ - + @@ -25,7 +25,7 @@ - + diff --git a/system/autoware_state_monitor/package.xml b/system/ad_service_state_monitor/package.xml similarity index 87% rename from system/autoware_state_monitor/package.xml rename to system/ad_service_state_monitor/package.xml index 314a1226bf8ce..21f04d46eab5b 100644 --- a/system/autoware_state_monitor/package.xml +++ b/system/ad_service_state_monitor/package.xml @@ -1,9 +1,9 @@ - autoware_state_monitor + ad_service_state_monitor 0.1.1 - The autoware_state_monitor package ROS2 + The ad_service_state_monitor package ROS2 Kenji Miyake Apache License 2.0 @@ -13,7 +13,6 @@ autoware_auto_planning_msgs autoware_auto_system_msgs autoware_auto_vehicle_msgs - autoware_utils diagnostic_updater fmt geometry_msgs @@ -24,6 +23,7 @@ tf2 tf2_geometry_msgs tf2_ros + tier4_autoware_utils sensor_msgs tier4_perception_msgs diff --git a/system/autoware_state_monitor/src/autoware_state_monitor_node/autoware_state_monitor_node.cpp b/system/ad_service_state_monitor/src/ad_service_state_monitor_node/ad_service_state_monitor_node.cpp similarity index 98% rename from system/autoware_state_monitor/src/autoware_state_monitor_node/autoware_state_monitor_node.cpp rename to system/ad_service_state_monitor/src/ad_service_state_monitor_node/ad_service_state_monitor_node.cpp index c3f6714246cbb..55adf8db46133 100644 --- a/system/autoware_state_monitor/src/autoware_state_monitor_node/autoware_state_monitor_node.cpp +++ b/system/ad_service_state_monitor/src/ad_service_state_monitor_node/ad_service_state_monitor_node.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware_state_monitor/autoware_state_monitor_node.hpp" +#include "ad_service_state_monitor/ad_service_state_monitor_node.hpp" #include @@ -383,7 +383,7 @@ void AutowareStateMonitorNode::setDisengage() } AutowareStateMonitorNode::AutowareStateMonitorNode() -: Node("autoware_state_monitor"), +: Node("ad_service_state_monitor"), tf_buffer_(this->get_clock()), tf_listener_(tf_buffer_), updater_(this) @@ -399,7 +399,7 @@ AutowareStateMonitorNode::AutowareStateMonitorNode() // Parameter for StateMachine state_param_.th_arrived_distance_m = this->declare_parameter("th_arrived_distance_m", 1.0); state_param_.th_arrived_angle = - this->declare_parameter("th_arrived_angle_deg", autoware_utils::deg2rad(45.0)); + this->declare_parameter("th_arrived_angle_deg", tier4_autoware_utils::deg2rad(45.0)); state_param_.th_stopped_time_sec = this->declare_parameter("th_stopped_time_sec", 1.0); state_param_.th_stopped_velocity_mps = this->declare_parameter("th_stopped_velocity_mps", 0.01); diff --git a/system/autoware_state_monitor/src/autoware_state_monitor_node/diagnostics.cpp b/system/ad_service_state_monitor/src/ad_service_state_monitor_node/diagnostics.cpp similarity index 97% rename from system/autoware_state_monitor/src/autoware_state_monitor_node/diagnostics.cpp rename to system/ad_service_state_monitor/src/ad_service_state_monitor_node/diagnostics.cpp index db11d1fd41783..4d5fe4a77742b 100644 --- a/system/autoware_state_monitor/src/autoware_state_monitor_node/diagnostics.cpp +++ b/system/ad_service_state_monitor/src/ad_service_state_monitor_node/diagnostics.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware_state_monitor/autoware_state_monitor_node.hpp" +#include "ad_service_state_monitor/ad_service_state_monitor_node.hpp" #include @@ -24,7 +24,7 @@ void AutowareStateMonitorNode::setupDiagnosticUpdater() { - updater_.setHardwareID("autoware_state_monitor"); + updater_.setHardwareID("ad_service_state_monitor"); const auto module_names = this->declare_parameter>("module_names"); diff --git a/system/autoware_state_monitor/src/autoware_state_monitor_node/main.cpp b/system/ad_service_state_monitor/src/ad_service_state_monitor_node/main.cpp similarity index 93% rename from system/autoware_state_monitor/src/autoware_state_monitor_node/main.cpp rename to system/ad_service_state_monitor/src/ad_service_state_monitor_node/main.cpp index ad88c1a89b8ee..84e28f401e7e6 100644 --- a/system/autoware_state_monitor/src/autoware_state_monitor_node/main.cpp +++ b/system/ad_service_state_monitor/src/ad_service_state_monitor_node/main.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware_state_monitor/autoware_state_monitor_node.hpp" +#include "ad_service_state_monitor/ad_service_state_monitor_node.hpp" #include diff --git a/system/autoware_state_monitor/src/autoware_state_monitor_node/state_machine.cpp b/system/ad_service_state_monitor/src/ad_service_state_monitor_node/state_machine.cpp similarity index 98% rename from system/autoware_state_monitor/src/autoware_state_monitor_node/state_machine.cpp rename to system/ad_service_state_monitor/src/ad_service_state_monitor_node/state_machine.cpp index 62dec5f8c1449..062f8b92e1cf8 100644 --- a/system/autoware_state_monitor/src/autoware_state_monitor_node/state_machine.cpp +++ b/system/ad_service_state_monitor/src/ad_service_state_monitor_node/state_machine.cpp @@ -16,7 +16,7 @@ #include #define FMT_HEADER_ONLY -#include "autoware_state_monitor/state_machine.hpp" +#include "ad_service_state_monitor/state_machine.hpp" #include @@ -38,7 +38,7 @@ bool isValidAngle( { const double yaw_curr = tf2::getYaw(current_pose.orientation); const double yaw_ref = tf2::getYaw(ref_pose.orientation); - const double yaw_diff = autoware_utils::normalizeRadian(yaw_curr - yaw_ref); + const double yaw_diff = tier4_autoware_utils::normalizeRadian(yaw_curr - yaw_ref); return std::fabs(yaw_diff) < th_angle_rad; } diff --git a/system/autoware_version/CMakeLists.txt b/system/autoware_version/CMakeLists.txt deleted file mode 100644 index 9182a3b8bda20..0000000000000 --- a/system/autoware_version/CMakeLists.txt +++ /dev/null @@ -1,17 +0,0 @@ -cmake_minimum_required(VERSION 3.5) -project(autoware_version) - -find_package(ament_cmake_auto REQUIRED) -ament_auto_find_build_dependencies() - -install(PROGRAMS - script/print - DESTINATION lib/${PROJECT_NAME} -) - -if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - ament_lint_auto_find_test_dependencies() -endif() - -ament_auto_package() diff --git a/system/autoware_version/README.md b/system/autoware_version/README.md deleted file mode 100644 index 82e6760f64c14..0000000000000 --- a/system/autoware_version/README.md +++ /dev/null @@ -1,9 +0,0 @@ -# autoware_version - -This package provides a command line tool to know the architecture version. This feature is temporary and will be removed when the interface is unified in the future. - -## How to use - -```sh -ros2 run autoware_version print arch {full,type,version} -``` diff --git a/system/autoware_version/package.xml b/system/autoware_version/package.xml deleted file mode 100644 index f0ed975d24f56..0000000000000 --- a/system/autoware_version/package.xml +++ /dev/null @@ -1,20 +0,0 @@ - - - - - autoware_version - 0.0.0 - The autoware_version package - Takagi, Isamu - Apache License 2.0 - - ament_cmake_auto - - ament_lint_auto - autoware_lint_common - - - ament_cmake - - - diff --git a/system/autoware_version/script/print b/system/autoware_version/script/print deleted file mode 100755 index 7b796ab791b9b..0000000000000 --- a/system/autoware_version/script/print +++ /dev/null @@ -1,14 +0,0 @@ -#!/usr/bin/env python3 -# TODO(Takagi, Isamu): this is a temporary script and may change in the future -import argparse - -parser = argparse.ArgumentParser() -parser.add_argument("target", choices=["arch"]) -parser.add_argument("format", choices=["full", "type", "version"], default="full", nargs="?") -args = parser.parse_args() -data = {"arch": {"type": "universe", "version": "1.0.0"}} - -if args.format == "full": - print("{type} {version}".format(**data[args.target])) -else: - print(data[args.target][args.format]) diff --git a/system/dummy_diag_publisher/package.xml b/system/dummy_diag_publisher/package.xml index 1e29f7fdc6689..15574771ec2f6 100644 --- a/system/dummy_diag_publisher/package.xml +++ b/system/dummy_diag_publisher/package.xml @@ -9,9 +9,9 @@ ament_cmake_auto - autoware_utils diagnostic_updater rclcpp + tier4_autoware_utils rqt_reconfigure diff --git a/system/dummy_diag_publisher/src/dummy_diag_publisher_node/dummy_diag_publisher_node.cpp b/system/dummy_diag_publisher/src/dummy_diag_publisher_node/dummy_diag_publisher_node.cpp index 43d8e9e10b44d..e128411b6283b 100644 --- a/system/dummy_diag_publisher/src/dummy_diag_publisher_node/dummy_diag_publisher_node.cpp +++ b/system/dummy_diag_publisher/src/dummy_diag_publisher_node/dummy_diag_publisher_node.cpp @@ -14,8 +14,8 @@ #include "dummy_diag_publisher/dummy_diag_publisher_node.hpp" -#include #include +#include #include #include @@ -32,9 +32,9 @@ rcl_interfaces::msg::SetParametersResult DummyDiagPublisherNode::paramCallback( DummyDiagPublisherConfig config = config_; try { int status = static_cast(config.status); - autoware_utils::updateParam(parameters, "status", status); + tier4_autoware_utils::updateParam(parameters, "status", status); config.status = Status(status); - autoware_utils::updateParam(parameters, "is_active", config.is_active); + tier4_autoware_utils::updateParam(parameters, "is_active", config.is_active); config_ = config; } catch (const rclcpp::exceptions::InvalidParameterTypeException & e) { result.successful = false; diff --git a/system/emergency_handler/include/emergency_handler/emergency_handler_core.hpp b/system/emergency_handler/include/emergency_handler/emergency_handler_core.hpp index 292af3a8b7533..db3acf500fb97 100644 --- a/system/emergency_handler/include/emergency_handler/emergency_handler_core.hpp +++ b/system/emergency_handler/include/emergency_handler/emergency_handler_core.hpp @@ -28,9 +28,9 @@ #include // ROS2 core -#include #include #include +#include #include #include diff --git a/system/emergency_handler/package.xml b/system/emergency_handler/package.xml index 1860d7a7820b3..2fd696d481432 100644 --- a/system/emergency_handler/package.xml +++ b/system/emergency_handler/package.xml @@ -11,11 +11,11 @@ autoware_auto_control_msgs autoware_auto_system_msgs autoware_auto_vehicle_msgs - autoware_utils nav_msgs rclcpp std_msgs std_srvs + tier4_autoware_utils ament_lint_auto autoware_lint_common diff --git a/system/autoware_error_monitor/CMakeLists.txt b/system/system_error_monitor/CMakeLists.txt similarity index 76% rename from system/autoware_error_monitor/CMakeLists.txt rename to system/system_error_monitor/CMakeLists.txt index c3ab1f354f780..fe824b040382f 100644 --- a/system/autoware_error_monitor/CMakeLists.txt +++ b/system/system_error_monitor/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.5) -project(autoware_error_monitor) +project(system_error_monitor) ### Compile options if(NOT CMAKE_CXX_STANDARD) @@ -16,14 +16,14 @@ find_package(ament_cmake_auto REQUIRED) ament_auto_find_build_dependencies() # Target -## autoware_error_monitor_node -set(AUTOWARE_ERROR_MONITOR_SRC - src/autoware_error_monitor_core.cpp +## system_error_monitor_node +set(SYSTEM_ERROR_MONITOR_SRC + src/system_error_monitor_core.cpp ) ament_auto_add_executable(${PROJECT_NAME} - src/autoware_error_monitor_node.cpp - ${AUTOWARE_ERROR_MONITOR_SRC} + src/system_error_monitor_node.cpp + ${SYSTEM_ERROR_MONITOR_SRC} ) if(BUILD_TESTING) diff --git a/system/autoware_error_monitor/README.md b/system/system_error_monitor/README.md similarity index 95% rename from system/autoware_error_monitor/README.md rename to system/system_error_monitor/README.md index 0249d613dbdd7..1fcc74475b19b 100644 --- a/system/autoware_error_monitor/README.md +++ b/system/system_error_monitor/README.md @@ -1,4 +1,4 @@ -# autoware_error_monitor +# system_error_monitor ## Purpose @@ -90,12 +90,12 @@ endif ### Input -| Name | Type | Description | -| ---------------------------- | ---------------------------------------------------- | --------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | -| `/diagnostics_agg` | `diagnostic_msgs::msg::DiagnosticArray` | Diagnostic information aggregated based [diagnostic_aggregator setting](https://github.com/tier4/autoware.iv/tree/main/system/autoware_error_monitor/config/diagnostic_aggregator) is used to | -| `/autoware/state` | `autoware_auto_system_msgs::msg::AutowareState` | Required to ignore error during Route, Planning and Finalizing. | -| `/control/current_gate_mode` | `tier4_control_msgs::msg::GateMode` | Required to select the appropriate module from `autonomous_driving` or `external_control` | -| `/vehicle/control_mode` | `autoware_auto_vehicle_msgs::msg::ControlModeReport` | Required to not hold emergency during manual driving | +| Name | Type | Description | +| ---------------------------- | ---------------------------------------------------- | ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | +| `/diagnostics_agg` | `diagnostic_msgs::msg::DiagnosticArray` | Diagnostic information aggregated based [diagnostic_aggregator setting](https://github.com/tier4/autoware.iv/tree/main/system/system_error_monitor/config/diagnostic_aggregator) is used to | +| `/autoware/state` | `autoware_auto_system_msgs::msg::AutowareState` | Required to ignore error during Route, Planning and Finalizing. | +| `/control/current_gate_mode` | `tier4_control_msgs::msg::GateMode` | Required to select the appropriate module from `autonomous_driving` or `external_control` | +| `/vehicle/control_mode` | `autoware_auto_vehicle_msgs::msg::ControlModeReport` | Required to not hold emergency during manual driving | ### Output @@ -108,12 +108,12 @@ endif ### Node Parameters -| Name | Type | Default Value | Explanation | -| ---------------------------- | ------ | ------------- | --------------------------------------------------------------------------------------------------------------------------------------------------------- | -| `ignore_missing_diagnostics` | bool | `false` | If this parameter is turned off, it will be ignored if required modules have not been received. | -| `add_leaf_diagnostics` | bool | `true` | Required to use children diagnostics. | -| `diag_timeout_sec` | double | `1.0` (sec) | If required diagnostic is not received for a `diag_timeout_sec`, the diagnostic state become STALE state. | -| `data_ready_timeout` | double | `30.0` | If input topics required for autoware_error_monitor are not available for `data_ready_timeout` seconds, autoware_state will translate to emergency state. | +| Name | Type | Default Value | Explanation | +| ---------------------------- | ------ | ------------- | ------------------------------------------------------------------------------------------------------------------------------------------------------- | +| `ignore_missing_diagnostics` | bool | `false` | If this parameter is turned off, it will be ignored if required modules have not been received. | +| `add_leaf_diagnostics` | bool | `true` | Required to use children diagnostics. | +| `diag_timeout_sec` | double | `1.0` (sec) | If required diagnostic is not received for a `diag_timeout_sec`, the diagnostic state become STALE state. | +| `data_ready_timeout` | double | `30.0` | If input topics required for system_error_monitor are not available for `data_ready_timeout` seconds, autoware_state will translate to emergency state. | ### Core Parameters @@ -124,7 +124,7 @@ endif | `use_emergency_hold_in_manual_driving` | bool | `false` | If this parameter is turned off, emergencies will be ignored during manual driving. | | `emergency_hazard_level` | int | `2` | If hazard_level is more than emergency_hazard_level, autoware state will translate to emergency state | -### YAML format for autoware_error_monitor +### YAML format for system_error_monitor The parameter key should be filled with the hierarchical diagnostics output by diagnostic_aggregator. Parameters prefixed with `required_modules.autonomous_driving` are for autonomous driving. Parameters with the `required_modules.remote_control` prefix are for remote control. If the value is `default`, the default value will be set. diff --git a/system/autoware_error_monitor/config/diagnostic_aggregator/_empty.param.yaml b/system/system_error_monitor/config/diagnostic_aggregator/_empty.param.yaml similarity index 100% rename from system/autoware_error_monitor/config/diagnostic_aggregator/_empty.param.yaml rename to system/system_error_monitor/config/diagnostic_aggregator/_empty.param.yaml diff --git a/system/autoware_error_monitor/config/diagnostic_aggregator/control.param.yaml b/system/system_error_monitor/config/diagnostic_aggregator/control.param.yaml similarity index 100% rename from system/autoware_error_monitor/config/diagnostic_aggregator/control.param.yaml rename to system/system_error_monitor/config/diagnostic_aggregator/control.param.yaml diff --git a/system/autoware_error_monitor/config/diagnostic_aggregator/localization.param.yaml b/system/system_error_monitor/config/diagnostic_aggregator/localization.param.yaml similarity index 100% rename from system/autoware_error_monitor/config/diagnostic_aggregator/localization.param.yaml rename to system/system_error_monitor/config/diagnostic_aggregator/localization.param.yaml diff --git a/system/autoware_error_monitor/config/diagnostic_aggregator/map.param.yaml b/system/system_error_monitor/config/diagnostic_aggregator/map.param.yaml similarity index 100% rename from system/autoware_error_monitor/config/diagnostic_aggregator/map.param.yaml rename to system/system_error_monitor/config/diagnostic_aggregator/map.param.yaml diff --git a/system/autoware_error_monitor/config/diagnostic_aggregator/perception.param.yaml b/system/system_error_monitor/config/diagnostic_aggregator/perception.param.yaml similarity index 100% rename from system/autoware_error_monitor/config/diagnostic_aggregator/perception.param.yaml rename to system/system_error_monitor/config/diagnostic_aggregator/perception.param.yaml diff --git a/system/autoware_error_monitor/config/diagnostic_aggregator/planning.param.yaml b/system/system_error_monitor/config/diagnostic_aggregator/planning.param.yaml similarity index 100% rename from system/autoware_error_monitor/config/diagnostic_aggregator/planning.param.yaml rename to system/system_error_monitor/config/diagnostic_aggregator/planning.param.yaml diff --git a/system/autoware_error_monitor/config/diagnostic_aggregator/sensing.param.yaml b/system/system_error_monitor/config/diagnostic_aggregator/sensing.param.yaml similarity index 100% rename from system/autoware_error_monitor/config/diagnostic_aggregator/sensing.param.yaml rename to system/system_error_monitor/config/diagnostic_aggregator/sensing.param.yaml diff --git a/system/autoware_error_monitor/config/diagnostic_aggregator/system.param.yaml b/system/system_error_monitor/config/diagnostic_aggregator/system.param.yaml similarity index 100% rename from system/autoware_error_monitor/config/diagnostic_aggregator/system.param.yaml rename to system/system_error_monitor/config/diagnostic_aggregator/system.param.yaml diff --git a/system/autoware_error_monitor/config/diagnostic_aggregator/vehicle.param.yaml b/system/system_error_monitor/config/diagnostic_aggregator/vehicle.param.yaml similarity index 100% rename from system/autoware_error_monitor/config/diagnostic_aggregator/vehicle.param.yaml rename to system/system_error_monitor/config/diagnostic_aggregator/vehicle.param.yaml diff --git a/system/autoware_error_monitor/config/autoware_error_monitor.param.yaml b/system/system_error_monitor/config/system_error_monitor.param.yaml similarity index 100% rename from system/autoware_error_monitor/config/autoware_error_monitor.param.yaml rename to system/system_error_monitor/config/system_error_monitor.param.yaml diff --git a/system/autoware_error_monitor/config/autoware_error_monitor.planning_simulation.param.yaml b/system/system_error_monitor/config/system_error_monitor.planning_simulation.param.yaml similarity index 100% rename from system/autoware_error_monitor/config/autoware_error_monitor.planning_simulation.param.yaml rename to system/system_error_monitor/config/system_error_monitor.planning_simulation.param.yaml diff --git a/system/autoware_error_monitor/include/autoware_error_monitor/diagnostics_filter.hpp b/system/system_error_monitor/include/system_error_monitor/diagnostics_filter.hpp similarity index 94% rename from system/autoware_error_monitor/include/autoware_error_monitor/diagnostics_filter.hpp rename to system/system_error_monitor/include/system_error_monitor/diagnostics_filter.hpp index 2ae05500b3de5..cbca3ed1c477c 100644 --- a/system/autoware_error_monitor/include/autoware_error_monitor/diagnostics_filter.hpp +++ b/system/system_error_monitor/include/system_error_monitor/diagnostics_filter.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE_ERROR_MONITOR__DIAGNOSTICS_FILTER_HPP_ -#define AUTOWARE_ERROR_MONITOR__DIAGNOSTICS_FILTER_HPP_ +#ifndef SYSTEM_ERROR_MONITOR__DIAGNOSTICS_FILTER_HPP_ +#define SYSTEM_ERROR_MONITOR__DIAGNOSTICS_FILTER_HPP_ #include @@ -100,4 +100,4 @@ inline std::vector extractLeafChildrenDi } // namespace diagnostics_filter -#endif // AUTOWARE_ERROR_MONITOR__DIAGNOSTICS_FILTER_HPP_ +#endif // SYSTEM_ERROR_MONITOR__DIAGNOSTICS_FILTER_HPP_ diff --git a/system/autoware_error_monitor/include/autoware_error_monitor/autoware_error_monitor_core.hpp b/system/system_error_monitor/include/system_error_monitor/system_error_monitor_core.hpp similarity index 96% rename from system/autoware_error_monitor/include/autoware_error_monitor/autoware_error_monitor_core.hpp rename to system/system_error_monitor/include/system_error_monitor/system_error_monitor_core.hpp index 9c5db68b2eba5..19c94cd21d45c 100644 --- a/system/autoware_error_monitor/include/autoware_error_monitor/autoware_error_monitor_core.hpp +++ b/system/system_error_monitor/include/system_error_monitor/system_error_monitor_core.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE_ERROR_MONITOR__AUTOWARE_ERROR_MONITOR_CORE_HPP_ -#define AUTOWARE_ERROR_MONITOR__AUTOWARE_ERROR_MONITOR_CORE_HPP_ +#ifndef SYSTEM_ERROR_MONITOR__SYSTEM_ERROR_MONITOR_CORE_HPP_ +#define SYSTEM_ERROR_MONITOR__SYSTEM_ERROR_MONITOR_CORE_HPP_ #include #include @@ -137,4 +137,4 @@ class AutowareErrorMonitor : public rclcpp::Node bool isEmergencyHoldingRequired() const; }; -#endif // AUTOWARE_ERROR_MONITOR__AUTOWARE_ERROR_MONITOR_CORE_HPP_ +#endif // SYSTEM_ERROR_MONITOR__SYSTEM_ERROR_MONITOR_CORE_HPP_ diff --git a/system/autoware_error_monitor/launch/autoware_error_monitor.launch.xml b/system/system_error_monitor/launch/system_error_monitor.launch.xml similarity index 68% rename from system/autoware_error_monitor/launch/autoware_error_monitor.launch.xml rename to system/system_error_monitor/launch/system_error_monitor.launch.xml index b7ee912debb22..981fc8395a545 100644 --- a/system/autoware_error_monitor/launch/autoware_error_monitor.launch.xml +++ b/system/system_error_monitor/launch/system_error_monitor.launch.xml @@ -1,5 +1,5 @@ - + @@ -8,16 +8,16 @@ - - - - - - - - - - + + + + + + + + + + @@ -38,7 +38,7 @@ - + diff --git a/system/autoware_error_monitor/launch/autoware_error_monitor_node.launch.xml b/system/system_error_monitor/launch/system_error_monitor_node.launch.xml similarity index 87% rename from system/autoware_error_monitor/launch/autoware_error_monitor_node.launch.xml rename to system/system_error_monitor/launch/system_error_monitor_node.launch.xml index c642833e170f2..fe1632f67c0e2 100644 --- a/system/autoware_error_monitor/launch/autoware_error_monitor_node.launch.xml +++ b/system/system_error_monitor/launch/system_error_monitor_node.launch.xml @@ -1,5 +1,5 @@ - + @@ -10,7 +10,7 @@ - + diff --git a/system/autoware_error_monitor/package.xml b/system/system_error_monitor/package.xml similarity index 85% rename from system/autoware_error_monitor/package.xml rename to system/system_error_monitor/package.xml index d013a7f1b1838..efc96edf09a23 100644 --- a/system/autoware_error_monitor/package.xml +++ b/system/system_error_monitor/package.xml @@ -1,9 +1,9 @@ - autoware_error_monitor + system_error_monitor 0.1.1 - The autoware_error_monitor package in ROS2 + The system_error_monitor package in ROS2 Kenji Miyake Apache License 2.0 @@ -11,11 +11,11 @@ autoware_auto_system_msgs autoware_auto_vehicle_msgs - autoware_utils diagnostic_msgs fmt rclcpp std_srvs + tier4_autoware_utils tier4_control_msgs diagnostic_aggregator diff --git a/system/autoware_error_monitor/src/autoware_error_monitor_core.cpp b/system/system_error_monitor/src/system_error_monitor_core.cpp similarity index 98% rename from system/autoware_error_monitor/src/autoware_error_monitor_core.cpp rename to system/system_error_monitor/src/system_error_monitor_core.cpp index b27b3decefb6b..2fc2133af1b33 100644 --- a/system/autoware_error_monitor/src/autoware_error_monitor_core.cpp +++ b/system/system_error_monitor/src/system_error_monitor_core.cpp @@ -21,8 +21,8 @@ #include #define FMT_HEADER_ONLY -#include "autoware_error_monitor/autoware_error_monitor_core.hpp" -#include "autoware_error_monitor/diagnostics_filter.hpp" +#include "system_error_monitor/diagnostics_filter.hpp" +#include "system_error_monitor/system_error_monitor_core.hpp" #include @@ -152,8 +152,8 @@ autoware_auto_system_msgs::msg::HazardStatus createTimeoutHazardStatus() hazard_status.emergency = true; hazard_status.emergency_holding = false; diagnostic_msgs::msg::DiagnosticStatus diag; - diag.name = "autoware_error_monitor/input_data_timeout"; - diag.hardware_id = "autoware_error_monitor"; + diag.name = "system_error_monitor/input_data_timeout"; + diag.hardware_id = "system_error_monitor"; diag.level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; hazard_status.diag_single_point_fault.push_back(diag); return hazard_status; @@ -189,7 +189,7 @@ int isInNoFaultCondition( AutowareErrorMonitor::AutowareErrorMonitor() : Node( - "autoware_error_monitor", + "system_error_monitor", rclcpp::NodeOptions().automatically_declare_parameters_from_overrides(true)) { // Parameter @@ -465,7 +465,7 @@ autoware_auto_system_msgs::msg::HazardStatus AutowareErrorMonitor::judgeHazardSt DiagnosticStatus missing_diag; missing_diag.name = diag_name; - missing_diag.hardware_id = "autoware_error_monitor"; + missing_diag.hardware_id = "system_error_monitor"; missing_diag.level = DiagnosticStatus::STALE; missing_diag.message = "no diag found"; diff --git a/system/autoware_error_monitor/src/autoware_error_monitor_node.cpp b/system/system_error_monitor/src/system_error_monitor_node.cpp similarity index 92% rename from system/autoware_error_monitor/src/autoware_error_monitor_node.cpp rename to system/system_error_monitor/src/system_error_monitor_node.cpp index a8b48ba826b6b..f389497b93a43 100644 --- a/system/autoware_error_monitor/src/autoware_error_monitor_node.cpp +++ b/system/system_error_monitor/src/system_error_monitor_node.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware_error_monitor/autoware_error_monitor_core.hpp" +#include "system_error_monitor/system_error_monitor_core.hpp" #include diff --git a/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/include/accel_brake_map_calibrator/accel_brake_map_calibrator_node.hpp b/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/include/accel_brake_map_calibrator/accel_brake_map_calibrator_node.hpp index c767fb678f758..1874e77c868bc 100644 --- a/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/include/accel_brake_map_calibrator/accel_brake_map_calibrator_node.hpp +++ b/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/include/accel_brake_map_calibrator/accel_brake_map_calibrator_node.hpp @@ -17,13 +17,13 @@ #ifndef ACCEL_BRAKE_MAP_CALIBRATOR__ACCEL_BRAKE_MAP_CALIBRATOR_NODE_HPP_ #define ACCEL_BRAKE_MAP_CALIBRATOR__ACCEL_BRAKE_MAP_CALIBRATOR_NODE_HPP_ -#include "autoware_utils/planning/planning_marker_helper.hpp" -#include "autoware_utils/ros/transform_listener.hpp" #include "diagnostic_updater/diagnostic_updater.hpp" #include "raw_vehicle_cmd_converter/accel_map.hpp" #include "raw_vehicle_cmd_converter/brake_map.hpp" #include "rclcpp/rclcpp.hpp" #include "tf2/utils.h" +#include "tier4_autoware_utils/planning/planning_marker_helper.hpp" +#include "tier4_autoware_utils/ros/transform_listener.hpp" #include "autoware_auto_vehicle_msgs/msg/steering_report.hpp" #include "autoware_auto_vehicle_msgs/msg/velocity_report.hpp" @@ -61,7 +61,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_; diff --git a/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/package.xml b/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/package.xml index 9ed716f6bc237..d8971e55a3976 100644 --- a/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/package.xml +++ b/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/package.xml @@ -10,7 +10,6 @@ ament_cmake_auto autoware_auto_vehicle_msgs - autoware_utils diagnostic_updater geometry_msgs raw_vehicle_cmd_converter @@ -18,6 +17,7 @@ std_msgs std_srvs tf2_ros + tier4_autoware_utils tier4_debug_msgs tier4_vehicle_msgs visualization_msgs diff --git a/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/src/accel_brake_map_calibrator_node.cpp b/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/src/accel_brake_map_calibrator_node.cpp index eef729ee9a01b..1b9c8ee9c275d 100644 --- a/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/src/accel_brake_map_calibrator_node.cpp +++ b/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/src/accel_brake_map_calibrator_node.cpp @@ -29,7 +29,7 @@ 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_ = this->declare_parameter("update_hz", 10.0); covariance_ = this->declare_parameter("initial_covariance", 0.05);