diff --git a/control/autoware_mpc_lateral_controller/package.xml b/control/autoware_mpc_lateral_controller/package.xml index 3493767a9663b..a639357a29e08 100644 --- a/control/autoware_mpc_lateral_controller/package.xml +++ b/control/autoware_mpc_lateral_controller/package.xml @@ -8,6 +8,7 @@ Takamasa Horibe Takayuki Murooka Kyoichi Sugahara + Alqudah Mohammad Apache 2.0 diff --git a/launch/tier4_system_launch/package.xml b/launch/tier4_system_launch/package.xml index f7117c2b23892..c7a91a99505ad 100644 --- a/launch/tier4_system_launch/package.xml +++ b/launch/tier4_system_launch/package.xml @@ -11,7 +11,16 @@ ament_cmake_auto autoware_cmake + autoware_component_interface_tools autoware_component_state_monitor + autoware_dummy_diag_publisher + autoware_duplicated_node_checker + autoware_hazard_status_converter + autoware_mrm_comfortable_stop_operator + autoware_mrm_emergency_stop_operator + autoware_mrm_handler + autoware_processing_time_checker + autoware_system_diagnostic_monitor autoware_system_monitor ament_lint_auto diff --git a/launch/tier4_vehicle_launch/package.xml b/launch/tier4_vehicle_launch/package.xml index 1333da1d138b6..b6c5d41bc45a8 100644 --- a/launch/tier4_vehicle_launch/package.xml +++ b/launch/tier4_vehicle_launch/package.xml @@ -10,6 +10,7 @@ ament_cmake_auto autoware_cmake + autoware_raw_vehicle_cmd_converter robot_state_publisher xacro diff --git a/planning/autoware_obstacle_cruise_planner/package.xml b/planning/autoware_obstacle_cruise_planner/package.xml index c7400d1b05695..fe914bdc5626e 100644 --- a/planning/autoware_obstacle_cruise_planner/package.xml +++ b/planning/autoware_obstacle_cruise_planner/package.xml @@ -19,6 +19,7 @@ autoware_cmake autoware_internal_debug_msgs + autoware_internal_planning_msgs autoware_interpolation autoware_lanelet2_extension autoware_motion_utils diff --git a/planning/autoware_obstacle_cruise_planner/src/pid_based_planner/pid_based_planner.cpp b/planning/autoware_obstacle_cruise_planner/src/pid_based_planner/pid_based_planner.cpp index 195f47b5e4ee4..ca368307b8b36 100644 --- a/planning/autoware_obstacle_cruise_planner/src/pid_based_planner/pid_based_planner.cpp +++ b/planning/autoware_obstacle_cruise_planner/src/pid_based_planner/pid_based_planner.cpp @@ -336,8 +336,8 @@ std::vector PIDBasedPlanner::planCruise( planning_factor_interface_->add( stop_traj_points, planner_data.ego_pose, stop_traj_points.at(wall_idx).pose, - tier4_planning_msgs::msg::PlanningFactor::NONE, - tier4_planning_msgs::msg::SafetyFactorArray{}); + autoware_internal_planning_msgs::msg::PlanningFactor::NONE, + autoware_internal_planning_msgs::msg::SafetyFactorArray{}); } // do cruise planning diff --git a/planning/autoware_obstacle_cruise_planner/src/planner_interface.cpp b/planning/autoware_obstacle_cruise_planner/src/planner_interface.cpp index a969ec1a1c890..0590629c95a89 100644 --- a/planning/autoware_obstacle_cruise_planner/src/planner_interface.cpp +++ b/planning/autoware_obstacle_cruise_planner/src/planner_interface.cpp @@ -336,8 +336,8 @@ std::vector PlannerInterface::generateStopTrajectory( const auto stop_pose = output_traj_points.at(*zero_vel_idx).pose; planning_factor_interface_->add( output_traj_points, planner_data.ego_pose, stop_pose, - tier4_planning_msgs::msg::PlanningFactor::STOP, - tier4_planning_msgs::msg::SafetyFactorArray{}); + autoware_internal_planning_msgs::msg::PlanningFactor::STOP, + autoware_internal_planning_msgs::msg::SafetyFactorArray{}); // Store stop reason debug data debug_data_ptr_->stop_metrics = makeMetrics("PlannerInterface", "stop", planner_data, stop_pose, *determined_stop_obstacle); @@ -594,8 +594,8 @@ std::vector PlannerInterface::generateSlowDownTrajectory( slow_down_traj_points, planner_data.ego_pose, slow_down_traj_points.at(*slow_down_start_idx).pose, slow_down_traj_points.at(*slow_down_end_idx).pose, - tier4_planning_msgs::msg::PlanningFactor::SLOW_DOWN, - tier4_planning_msgs::msg::SafetyFactorArray{}, planner_data.is_driving_forward, + autoware_internal_planning_msgs::msg::PlanningFactor::SLOW_DOWN, + autoware_internal_planning_msgs::msg::SafetyFactorArray{}, planner_data.is_driving_forward, stable_slow_down_vel); } diff --git a/planning/autoware_obstacle_stop_planner/package.xml b/planning/autoware_obstacle_stop_planner/package.xml index 60e2db3de32a9..e0de7bca6de83 100644 --- a/planning/autoware_obstacle_stop_planner/package.xml +++ b/planning/autoware_obstacle_stop_planner/package.xml @@ -22,6 +22,7 @@ autoware_adapi_v1_msgs autoware_internal_debug_msgs + autoware_internal_planning_msgs autoware_motion_utils autoware_perception_msgs autoware_planning_factor_interface diff --git a/planning/autoware_obstacle_stop_planner/src/debug_marker.cpp b/planning/autoware_obstacle_stop_planner/src/debug_marker.cpp index 4fb3cb8828abe..1b43ac6bbae59 100644 --- a/planning/autoware_obstacle_stop_planner/src/debug_marker.cpp +++ b/planning/autoware_obstacle_stop_planner/src/debug_marker.cpp @@ -194,8 +194,9 @@ void ObstacleStopPlannerDebugNode::publish() if (stop_pose_ptr_) { planning_factor_interface_->add( std::numeric_limits::quiet_NaN(), *stop_pose_ptr_, - tier4_planning_msgs::msg::PlanningFactor::STOP, tier4_planning_msgs::msg::SafetyFactorArray{}, - true /*is_driving_forward*/, 0.0, 0.0 /*shift distance*/, ""); + autoware_internal_planning_msgs::msg::PlanningFactor::STOP, + autoware_internal_planning_msgs::msg::SafetyFactorArray{}, true /*is_driving_forward*/, 0.0, + 0.0 /*shift distance*/, ""); } planning_factor_interface_->publish(); diff --git a/planning/autoware_planning_factor_interface/include/autoware/planning_factor_interface/planning_factor_interface.hpp b/planning/autoware_planning_factor_interface/include/autoware/planning_factor_interface/planning_factor_interface.hpp index 05c57d8739d47..a88fdbb218cd4 100644 --- a/planning/autoware_planning_factor_interface/include/autoware/planning_factor_interface/planning_factor_interface.hpp +++ b/planning/autoware_planning_factor_interface/include/autoware/planning_factor_interface/planning_factor_interface.hpp @@ -18,14 +18,14 @@ #include #include +#include #include +#include +#include +#include #include #include #include -#include -#include -#include -#include #include #include @@ -33,11 +33,11 @@ namespace autoware::planning_factor_interface { +using autoware_internal_planning_msgs::msg::ControlPoint; +using autoware_internal_planning_msgs::msg::PlanningFactor; +using autoware_internal_planning_msgs::msg::PlanningFactorArray; +using autoware_internal_planning_msgs::msg::SafetyFactorArray; using geometry_msgs::msg::Pose; -using tier4_planning_msgs::msg::ControlPoint; -using tier4_planning_msgs::msg::PlanningFactor; -using tier4_planning_msgs::msg::PlanningFactorArray; -using tier4_planning_msgs::msg::SafetyFactorArray; class PlanningFactorInterface { @@ -124,13 +124,13 @@ class PlanningFactorInterface const SafetyFactorArray & safety_factors, const bool is_driving_forward = true, const double velocity = 0.0, const double shift_length = 0.0, const std::string & detail = "") { - const auto control_point = tier4_planning_msgs::build() + const auto control_point = autoware_internal_planning_msgs::build() .pose(control_point_pose) .velocity(velocity) .shift_length(shift_length) .distance(distance); - const auto factor = tier4_planning_msgs::build() + const auto factor = autoware_internal_planning_msgs::build() .module(name_) .is_driving_forward(is_driving_forward) .control_points({control_point}) @@ -161,19 +161,19 @@ class PlanningFactorInterface const bool is_driving_forward = true, const double velocity = 0.0, const double shift_length = 0.0, const std::string & detail = "") { - const auto control_start_point = tier4_planning_msgs::build() + const auto control_start_point = autoware_internal_planning_msgs::build() .pose(start_pose) .velocity(velocity) .shift_length(shift_length) .distance(start_distance); - const auto control_end_point = tier4_planning_msgs::build() + const auto control_end_point = autoware_internal_planning_msgs::build() .pose(end_pose) .velocity(velocity) .shift_length(shift_length) .distance(end_distance); - const auto factor = tier4_planning_msgs::build() + const auto factor = autoware_internal_planning_msgs::build() .module(name_) .is_driving_forward(is_driving_forward) .control_points({control_start_point, control_end_point}) diff --git a/planning/autoware_planning_factor_interface/package.xml b/planning/autoware_planning_factor_interface/package.xml index d4a0074b6f62f..286e465d12150 100644 --- a/planning/autoware_planning_factor_interface/package.xml +++ b/planning/autoware_planning_factor_interface/package.xml @@ -13,11 +13,11 @@ ament_cmake_auto autoware_cmake + autoware_internal_planning_msgs autoware_motion_utils autoware_planning_msgs autoware_utils rclcpp - tier4_planning_msgs ament_cmake_ros ament_lint_auto diff --git a/planning/autoware_surround_obstacle_checker/package.xml b/planning/autoware_surround_obstacle_checker/package.xml index 78282cf143d7f..e31e34cda81a3 100644 --- a/planning/autoware_surround_obstacle_checker/package.xml +++ b/planning/autoware_surround_obstacle_checker/package.xml @@ -14,6 +14,7 @@ autoware_cmake eigen3_cmake_module + autoware_internal_planning_msgs autoware_motion_utils autoware_perception_msgs autoware_planning_factor_interface diff --git a/planning/autoware_surround_obstacle_checker/src/debug_marker.cpp b/planning/autoware_surround_obstacle_checker/src/debug_marker.cpp index 67e2f34177090..2cc16f8a433e5 100644 --- a/planning/autoware_surround_obstacle_checker/src/debug_marker.cpp +++ b/planning/autoware_surround_obstacle_checker/src/debug_marker.cpp @@ -146,8 +146,8 @@ void SurroundObstacleCheckerDebugNode::publish() /* publish stop reason for autoware api */ if (stop_pose_ptr_ != nullptr) { planning_factor_interface_->add( - 0.0, *stop_pose_ptr_, tier4_planning_msgs::msg::PlanningFactor::STOP, - tier4_planning_msgs::msg::SafetyFactorArray{}); + 0.0, *stop_pose_ptr_, autoware_internal_planning_msgs::msg::PlanningFactor::STOP, + autoware_internal_planning_msgs::msg::SafetyFactorArray{}); } planning_factor_interface_->publish(); diff --git a/planning/autoware_surround_obstacle_checker/src/debug_marker.hpp b/planning/autoware_surround_obstacle_checker/src/debug_marker.hpp index 0569c6815a252..216bad212a648 100644 --- a/planning/autoware_surround_obstacle_checker/src/debug_marker.hpp +++ b/planning/autoware_surround_obstacle_checker/src/debug_marker.hpp @@ -19,9 +19,9 @@ #include #include +#include #include #include -#include #include #include @@ -34,10 +34,10 @@ namespace autoware::surround_obstacle_checker { using autoware::vehicle_info_utils::VehicleInfo; +using autoware_internal_planning_msgs::msg::ControlPoint; +using autoware_internal_planning_msgs::msg::PlanningFactor; +using autoware_internal_planning_msgs::msg::PlanningFactorArray; using geometry_msgs::msg::PolygonStamped; -using tier4_planning_msgs::msg::ControlPoint; -using tier4_planning_msgs::msg::PlanningFactor; -using tier4_planning_msgs::msg::PlanningFactorArray; using visualization_msgs::msg::Marker; using visualization_msgs::msg::MarkerArray; diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/interface/scene_module_interface.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/interface/scene_module_interface.hpp index b2b0ff6184005..629acd344c569 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/interface/scene_module_interface.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/interface/scene_module_interface.hpp @@ -57,11 +57,11 @@ using autoware::objects_of_interest_marker_interface::ObjectsOfInterestMarkerInt using autoware::planning_factor_interface::PlanningFactorInterface; using autoware::rtc_interface::RTCInterface; using autoware_internal_planning_msgs::msg::PathWithLaneId; +using autoware_internal_planning_msgs::msg::PlanningFactor; +using autoware_internal_planning_msgs::msg::SafetyFactorArray; using autoware_utils::calc_offset_pose; using autoware_utils::generate_uuid; using tier4_planning_msgs::msg::AvoidanceDebugMsgArray; -using tier4_planning_msgs::msg::PlanningFactor; -using tier4_planning_msgs::msg::SafetyFactorArray; using tier4_rtc_msgs::msg::State; using unique_identifier_msgs::msg::UUID; using visualization_msgs::msg::MarkerArray; diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp index 04f70ffbe63e5..d642a8528efc5 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp @@ -22,11 +22,11 @@ #include #include +#include #include #include #include #include -#include #include #include @@ -286,7 +286,7 @@ CollisionCheckDebugPair createObjectDebug(const ExtendedPredictedObject & obj); void updateCollisionCheckDebugMap( CollisionCheckDebugMap & debug_map, CollisionCheckDebugPair & object_debug, bool is_safe); -tier4_planning_msgs::msg::SafetyFactorArray to_safety_factor_array( +autoware_internal_planning_msgs::msg::SafetyFactorArray to_safety_factor_array( const CollisionCheckDebugMap & debug_map); } // namespace autoware::behavior_path_planner::utils::path_safety_checker diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/package.xml b/planning/behavior_path_planner/autoware_behavior_path_planner_common/package.xml index f5e65f385522e..1444b8e5e0ef8 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/package.xml +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/package.xml @@ -45,6 +45,7 @@ autoware_adapi_v1_msgs autoware_freespace_planning_algorithms + autoware_internal_planning_msgs autoware_interpolation autoware_lane_departure_checker autoware_lanelet2_extension diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_safety_checker/safety_check.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_safety_checker/safety_check.cpp index 217cd53f21dbe..4526a81637687 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_safety_checker/safety_check.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_safety_checker/safety_check.cpp @@ -877,13 +877,13 @@ double calculateRoughDistanceToObjects( return min_distance; } -tier4_planning_msgs::msg::SafetyFactorArray to_safety_factor_array( +autoware_internal_planning_msgs::msg::SafetyFactorArray to_safety_factor_array( const CollisionCheckDebugMap & debug_map) { - tier4_planning_msgs::msg::SafetyFactorArray safety_factors; + autoware_internal_planning_msgs::msg::SafetyFactorArray safety_factors; for (const auto & [uuid, data] : debug_map) { - tier4_planning_msgs::msg::SafetyFactor safety_factor; - safety_factor.type = tier4_planning_msgs::msg::SafetyFactor::OBJECT; + autoware_internal_planning_msgs::msg::SafetyFactor safety_factor; + safety_factor.type = autoware_internal_planning_msgs::msg::SafetyFactor::OBJECT; safety_factor.is_safe = data.is_safe; safety_factor.object_id = autoware_utils::to_uuid_msg(uuid); safety_factor.points.push_back(data.current_obj_pose.position); diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/package.xml b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/package.xml index d68d626eba2ff..bb06fcb0b8f82 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/package.xml +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/package.xml @@ -20,6 +20,7 @@ autoware_behavior_velocity_planner autoware_behavior_velocity_planner_common autoware_behavior_velocity_rtc_interface + autoware_internal_planning_msgs autoware_lanelet2_extension autoware_motion_utils autoware_perception_msgs diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/decisions.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/decisions.cpp index 40be0866abc66..0dac946353634 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/decisions.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/decisions.cpp @@ -103,9 +103,9 @@ void BlindSpotModule::reactRTCApprovalByDecision( const auto stop_pose = path->points.at(decision.stop_line_idx).point.pose; planning_factor_interface_->add( path->points, planner_data_->current_odometry->pose, stop_pose, stop_pose, - tier4_planning_msgs::msg::PlanningFactor::STOP, tier4_planning_msgs::msg::SafetyFactorArray{}, - true /*is_driving_forward*/, 0.0, 0.0 /*shift distance*/, - "blind_spot(module is judging as UNSAFE)"); + autoware_internal_planning_msgs::msg::PlanningFactor::STOP, + autoware_internal_planning_msgs::msg::SafetyFactorArray{}, true /*is_driving_forward*/, 0.0, + 0.0 /*shift distance*/, "blind_spot(module is judging as UNSAFE)"); } return; } @@ -137,9 +137,9 @@ void BlindSpotModule::reactRTCApprovalByDecision( const auto stop_pose = path->points.at(decision.stop_line_idx).point.pose; planning_factor_interface_->add( path->points, planner_data_->current_odometry->pose, stop_pose, stop_pose, - tier4_planning_msgs::msg::PlanningFactor::STOP, tier4_planning_msgs::msg::SafetyFactorArray{}, - true /*is_driving_forward*/, 0.0, 0.0 /*shift distance*/, - "blind_spot(module is judging as SAFE and RTC is not approved)"); + autoware_internal_planning_msgs::msg::PlanningFactor::STOP, + autoware_internal_planning_msgs::msg::SafetyFactorArray{}, true /*is_driving_forward*/, 0.0, + 0.0 /*shift distance*/, "blind_spot(module is judging as SAFE and RTC is not approved)"); } return; } diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/package.xml b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/package.xml index 5c90ac1ea2280..9cf31aae3cd88 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/package.xml +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/package.xml @@ -26,6 +26,7 @@ autoware_behavior_velocity_rtc_interface autoware_grid_map_utils autoware_internal_debug_msgs + autoware_internal_planning_msgs autoware_interpolation autoware_lanelet2_extension autoware_motion_utils diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp index c2d56e86b16b1..5face5aa6035a 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp @@ -882,8 +882,8 @@ void CrosswalkModule::applySlowDown( if (slowdown_pose) planning_factor_interface_->add( output.points, planner_data_->current_odometry->pose, *slowdown_pose, *slowdown_pose, - tier4_planning_msgs::msg::PlanningFactor::SLOW_DOWN, - tier4_planning_msgs::msg::SafetyFactorArray{}, true /*is_driving_forward*/, + autoware_internal_planning_msgs::msg::PlanningFactor::SLOW_DOWN, + autoware_internal_planning_msgs::msg::SafetyFactorArray{}, true /*is_driving_forward*/, safety_slow_down_speed, 0.0 /*shift distance*/, "crosswalk_safety_slowdown_for_approaching"); } @@ -1365,9 +1365,9 @@ void CrosswalkModule::planStop( insertDecelPointWithDebugInfo(stop_factor->stop_pose.position, 0.0, ego_path); planning_factor_interface_->add( ego_path.points, planner_data_->current_odometry->pose, stop_factor->stop_pose, - stop_factor->stop_pose, tier4_planning_msgs::msg::PlanningFactor::STOP, - tier4_planning_msgs::msg::SafetyFactorArray{}, true /*is_driving_forward*/, 0.0 /*velocity*/, - 0.0 /*shift distance*/, "crosswalk_stop"); + stop_factor->stop_pose, autoware_internal_planning_msgs::msg::PlanningFactor::STOP, + autoware_internal_planning_msgs::msg::SafetyFactorArray{}, true /*is_driving_forward*/, + 0.0 /*velocity*/, 0.0 /*shift distance*/, "crosswalk_stop"); } bool CrosswalkModule::checkRestartSuppression( diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/scene.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/scene.cpp index b46b3ca4e36bc..14220d1b630af 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/scene.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/scene.cpp @@ -182,8 +182,9 @@ bool DetectionAreaModule::modifyPathVelocity(PathWithLaneId * path) { planning_factor_interface_->add( path->points, planner_data_->current_odometry->pose, stop_pose, stop_pose, - tier4_planning_msgs::msg::PlanningFactor::STOP, tier4_planning_msgs::msg::SafetyFactorArray{}, - true /*is_driving_forward*/, 0.0, 0.0 /*shift distance*/, ""); + autoware_internal_planning_msgs::msg::PlanningFactor::STOP, + autoware_internal_planning_msgs::msg::SafetyFactorArray{}, true /*is_driving_forward*/, 0.0, + 0.0 /*shift distance*/, ""); } return true; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/package.xml b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/package.xml index 23daacd26a8c1..64c327a570265 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/package.xml +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/package.xml @@ -23,6 +23,7 @@ autoware_behavior_velocity_planner_common autoware_behavior_velocity_rtc_interface autoware_internal_debug_msgs + autoware_internal_planning_msgs autoware_interpolation autoware_lanelet2_extension autoware_motion_utils diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection.cpp index 7e3faa8f30ff7..81013615a5f79 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection.cpp @@ -765,8 +765,8 @@ void reactRTCApprovalByDecisionResult( planning_factor_interface->add( path->points, path->points.at(closest_idx).point.pose, path->points.at(stopline_idx).point.pose, path->points.at(stopline_idx).point.pose, - tier4_planning_msgs::msg::PlanningFactor::STOP, - tier4_planning_msgs::msg::SafetyFactorArray{}, true /*is_driving_forward*/, 0.0, + autoware_internal_planning_msgs::msg::PlanningFactor::STOP, + autoware_internal_planning_msgs::msg::SafetyFactorArray{}, true /*is_driving_forward*/, 0.0, 0.0 /*shift distance*/, "intersection(pure StuckStop)"); } } @@ -780,8 +780,8 @@ void reactRTCApprovalByDecisionResult( path->points, path->points.at(closest_idx).point.pose, path->points.at(occlusion_stopline_idx).point.pose, path->points.at(occlusion_stopline_idx).point.pose, - tier4_planning_msgs::msg::PlanningFactor::STOP, - tier4_planning_msgs::msg::SafetyFactorArray{}, true /*is_driving_forward*/, 0.0, + autoware_internal_planning_msgs::msg::PlanningFactor::STOP, + autoware_internal_planning_msgs::msg::SafetyFactorArray{}, true /*is_driving_forward*/, 0.0, 0.0 /*shift distance*/, "intersection(StuckStop with occlusion)"); } } @@ -812,8 +812,8 @@ void reactRTCApprovalByDecisionResult( planning_factor_interface->add( path->points, path->points.at(closest_idx).point.pose, path->points.at(stopline_idx).point.pose, path->points.at(stopline_idx).point.pose, - tier4_planning_msgs::msg::PlanningFactor::STOP, - tier4_planning_msgs::msg::SafetyFactorArray{}, true /*is_driving_forward*/, 0.0, + autoware_internal_planning_msgs::msg::PlanningFactor::STOP, + autoware_internal_planning_msgs::msg::SafetyFactorArray{}, true /*is_driving_forward*/, 0.0, 0.0 /*shift distance*/, "intersection(Yield Stuck)"); } } @@ -842,8 +842,8 @@ void reactRTCApprovalByDecisionResult( planning_factor_interface->add( path->points, path->points.at(decision_result.closest_idx).point.pose, path->points.at(stopline_idx).point.pose, path->points.at(stopline_idx).point.pose, - tier4_planning_msgs::msg::PlanningFactor::STOP, - tier4_planning_msgs::msg::SafetyFactorArray{}, true /*is_driving_forward*/, 0.0, + autoware_internal_planning_msgs::msg::PlanningFactor::STOP, + autoware_internal_planning_msgs::msg::SafetyFactorArray{}, true /*is_driving_forward*/, 0.0, 0.0 /*shift distance*/, "intersection(CollisionStop)"); } } @@ -856,8 +856,8 @@ void reactRTCApprovalByDecisionResult( planning_factor_interface->add( path->points, path->points.at(decision_result.closest_idx).point.pose, path->points.at(stopline_idx).point.pose, path->points.at(stopline_idx).point.pose, - tier4_planning_msgs::msg::PlanningFactor::STOP, - tier4_planning_msgs::msg::SafetyFactorArray{}, true /*is_driving_forward*/, 0.0, + autoware_internal_planning_msgs::msg::PlanningFactor::STOP, + autoware_internal_planning_msgs::msg::SafetyFactorArray{}, true /*is_driving_forward*/, 0.0, 0.0 /*shift distance*/, "intersection(CollisionStop with occlusion)"); } } @@ -886,8 +886,8 @@ void reactRTCApprovalByDecisionResult( planning_factor_interface->add( path->points, path->points.at(decision_result.closest_idx).point.pose, path->points.at(stopline_idx).point.pose, path->points.at(stopline_idx).point.pose, - tier4_planning_msgs::msg::PlanningFactor::STOP, - tier4_planning_msgs::msg::SafetyFactorArray{}, true /*is_driving_forward*/, 0.0, + autoware_internal_planning_msgs::msg::PlanningFactor::STOP, + autoware_internal_planning_msgs::msg::SafetyFactorArray{}, true /*is_driving_forward*/, 0.0, 0.0 /*shift distance*/, "intersection(FirstWaitBeforeOcclusion with collision)"); } } @@ -908,8 +908,8 @@ void reactRTCApprovalByDecisionResult( planning_factor_interface->add( path->points, path->points.at(decision_result.closest_idx).point.pose, path->points.at(stopline_idx).point.pose, path->points.at(stopline_idx).point.pose, - tier4_planning_msgs::msg::PlanningFactor::STOP, - tier4_planning_msgs::msg::SafetyFactorArray{}, true /*is_driving_forward*/, 0.0, + autoware_internal_planning_msgs::msg::PlanningFactor::STOP, + autoware_internal_planning_msgs::msg::SafetyFactorArray{}, true /*is_driving_forward*/, 0.0, 0.0 /*shift distance*/, "intersection(FirstWaitBeforeOcclusion with occlusion)"); } } @@ -952,8 +952,8 @@ void reactRTCApprovalByDecisionResult( path->points, path->points.at(decision_result.closest_idx).point.pose, path->points.at(occlusion_peeking_stopline).point.pose, path->points.at(occlusion_peeking_stopline).point.pose, - tier4_planning_msgs::msg::PlanningFactor::STOP, - tier4_planning_msgs::msg::SafetyFactorArray{}, true /*is_driving_forward*/, 0.0, + autoware_internal_planning_msgs::msg::PlanningFactor::STOP, + autoware_internal_planning_msgs::msg::SafetyFactorArray{}, true /*is_driving_forward*/, 0.0, 0.0 /*shift distance*/, "intersection(PeekingToOcclusion)"); } } @@ -966,8 +966,8 @@ void reactRTCApprovalByDecisionResult( planning_factor_interface->add( path->points, path->points.at(decision_result.closest_idx).point.pose, path->points.at(stopline_idx).point.pose, path->points.at(stopline_idx).point.pose, - tier4_planning_msgs::msg::PlanningFactor::STOP, - tier4_planning_msgs::msg::SafetyFactorArray{}, true /*is_driving_forward*/, 0.0, + autoware_internal_planning_msgs::msg::PlanningFactor::STOP, + autoware_internal_planning_msgs::msg::SafetyFactorArray{}, true /*is_driving_forward*/, 0.0, 0.0 /*shift distance*/, "intersection(PeekingToOcclusion while stopping for collision)"); } } @@ -996,8 +996,8 @@ void reactRTCApprovalByDecisionResult( planning_factor_interface->add( path->points, path->points.at(decision_result.closest_idx).point.pose, path->points.at(stopline_idx).point.pose, path->points.at(stopline_idx).point.pose, - tier4_planning_msgs::msg::PlanningFactor::STOP, - tier4_planning_msgs::msg::SafetyFactorArray{}, true /*is_driving_forward*/, 0.0, + autoware_internal_planning_msgs::msg::PlanningFactor::STOP, + autoware_internal_planning_msgs::msg::SafetyFactorArray{}, true /*is_driving_forward*/, 0.0, 0.0 /*shift distance*/, "intersection(CollisionStop with occlusion)"); } } @@ -1014,8 +1014,8 @@ void reactRTCApprovalByDecisionResult( planning_factor_interface->add( path->points, path->points.at(decision_result.closest_idx).point.pose, path->points.at(stopline_idx).point.pose, path->points.at(stopline_idx).point.pose, - tier4_planning_msgs::msg::PlanningFactor::STOP, - tier4_planning_msgs::msg::SafetyFactorArray{}, true /*is_driving_forward*/, 0.0, + autoware_internal_planning_msgs::msg::PlanningFactor::STOP, + autoware_internal_planning_msgs::msg::SafetyFactorArray{}, true /*is_driving_forward*/, 0.0, 0.0 /*shift distance*/, "intersection(CollisionStop with occlusion)"); } } @@ -1044,8 +1044,8 @@ void reactRTCApprovalByDecisionResult( planning_factor_interface->add( path->points, path->points.at(decision_result.closest_idx).point.pose, path->points.at(stopline_idx).point.pose, path->points.at(stopline_idx).point.pose, - tier4_planning_msgs::msg::PlanningFactor::STOP, - tier4_planning_msgs::msg::SafetyFactorArray{}, true /*is_driving_forward*/, 0.0, + autoware_internal_planning_msgs::msg::PlanningFactor::STOP, + autoware_internal_planning_msgs::msg::SafetyFactorArray{}, true /*is_driving_forward*/, 0.0, 0.0 /*shift distance*/, "intersection(Occlusion without traffic light, collision detected)"); } @@ -1059,8 +1059,8 @@ void reactRTCApprovalByDecisionResult( planning_factor_interface->add( path->points, path->points.at(decision_result.closest_idx).point.pose, path->points.at(stopline_idx).point.pose, path->points.at(stopline_idx).point.pose, - tier4_planning_msgs::msg::PlanningFactor::STOP, - tier4_planning_msgs::msg::SafetyFactorArray{}, true /*is_driving_forward*/, 0.0, + autoware_internal_planning_msgs::msg::PlanningFactor::STOP, + autoware_internal_planning_msgs::msg::SafetyFactorArray{}, true /*is_driving_forward*/, 0.0, 0.0 /*shift distance*/, "intersection(Occlusion without traffic light)"); } } @@ -1097,8 +1097,8 @@ void reactRTCApprovalByDecisionResult( planning_factor_interface->add( path->points, path->points.at(decision_result.closest_idx).point.pose, path->points.at(stopline_idx).point.pose, path->points.at(stopline_idx).point.pose, - tier4_planning_msgs::msg::PlanningFactor::STOP, - tier4_planning_msgs::msg::SafetyFactorArray{}, true /*is_driving_forward*/, 0.0, + autoware_internal_planning_msgs::msg::PlanningFactor::STOP, + autoware_internal_planning_msgs::msg::SafetyFactorArray{}, true /*is_driving_forward*/, 0.0, 0.0 /*shift distance*/, "intersection(Safe, but RTC interrupted for collision)"); } } @@ -1111,8 +1111,8 @@ void reactRTCApprovalByDecisionResult( planning_factor_interface->add( path->points, path->points.at(decision_result.closest_idx).point.pose, path->points.at(stopline_idx).point.pose, path->points.at(stopline_idx).point.pose, - tier4_planning_msgs::msg::PlanningFactor::STOP, - tier4_planning_msgs::msg::SafetyFactorArray{}, true /*is_driving_forward*/, 0.0, + autoware_internal_planning_msgs::msg::PlanningFactor::STOP, + autoware_internal_planning_msgs::msg::SafetyFactorArray{}, true /*is_driving_forward*/, 0.0, 0.0 /*shift distance*/, "intersection(Safe, but RTC interrupted for occlusion)"); } } @@ -1141,8 +1141,8 @@ void reactRTCApprovalByDecisionResult( planning_factor_interface->add( path->points, path->points.at(decision_result.closest_idx).point.pose, path->points.at(stopline_idx).point.pose, path->points.at(stopline_idx).point.pose, - tier4_planning_msgs::msg::PlanningFactor::STOP, - tier4_planning_msgs::msg::SafetyFactorArray{}, true /*is_driving_forward*/, 0.0, + autoware_internal_planning_msgs::msg::PlanningFactor::STOP, + autoware_internal_planning_msgs::msg::SafetyFactorArray{}, true /*is_driving_forward*/, 0.0, 0.0 /*shift distance*/, "intersection(FullyPrioritized, collision detected)"); } } @@ -1155,8 +1155,8 @@ void reactRTCApprovalByDecisionResult( planning_factor_interface->add( path->points, path->points.at(decision_result.closest_idx).point.pose, path->points.at(stopline_idx).point.pose, path->points.at(stopline_idx).point.pose, - tier4_planning_msgs::msg::PlanningFactor::STOP, - tier4_planning_msgs::msg::SafetyFactorArray{}, true /*is_driving_forward*/, 0.0, + autoware_internal_planning_msgs::msg::PlanningFactor::STOP, + autoware_internal_planning_msgs::msg::SafetyFactorArray{}, true /*is_driving_forward*/, 0.0, 0.0 /*shift distance*/, "intersection(FullyPrioritized, RTC for occlusion is interrupting)"); } diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_merge_from_private_road.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_merge_from_private_road.cpp index f968e75e248bb..22801295511d0 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_merge_from_private_road.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_merge_from_private_road.cpp @@ -158,8 +158,9 @@ bool MergeFromPrivateRoadModule::modifyPathVelocity(PathWithLaneId * path) const auto & stop_pose = path->points.at(stopline_idx).point.pose; planning_factor_interface_->add( path->points, planner_data_->current_odometry->pose, stop_pose, stop_pose, - tier4_planning_msgs::msg::PlanningFactor::STOP, tier4_planning_msgs::msg::SafetyFactorArray{}, - true /*is_driving_forward*/, 0.0, 0.0 /*shift distance*/, "merge_from_private"); + autoware_internal_planning_msgs::msg::PlanningFactor::STOP, + autoware_internal_planning_msgs::msg::SafetyFactorArray{}, true /*is_driving_forward*/, 0.0, + 0.0 /*shift distance*/, "merge_from_private"); const double signed_arc_dist_to_stop_point = autoware::motion_utils::calcSignedArcLength( path->points, current_pose.position, path->points.at(stopline_idx).point.pose.position); diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/src/scene.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/src/scene.cpp index 0fdda1f54924c..ac66f1d1a6b07 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/src/scene.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/src/scene.cpp @@ -168,8 +168,9 @@ void NoDrivableLaneModule::handle_approaching_state(PathWithLaneId * path) const auto & stop_pose = op_stop_pose.value(); planning_factor_interface_->add( path->points, planner_data_->current_odometry->pose, stop_pose, stop_pose, - tier4_planning_msgs::msg::PlanningFactor::STOP, tier4_planning_msgs::msg::SafetyFactorArray{}, - true /*is_driving_forward*/, 0.0, 0.0 /*shift distance*/, ""); + autoware_internal_planning_msgs::msg::PlanningFactor::STOP, + autoware_internal_planning_msgs::msg::SafetyFactorArray{}, true /*is_driving_forward*/, 0.0, + 0.0 /*shift distance*/, ""); const auto virtual_wall_pose = autoware::motion_utils::calcLongitudinalOffsetPose( path->points, stop_pose.position, debug_data_.base_link2front); @@ -218,8 +219,9 @@ void NoDrivableLaneModule::handle_inside_no_drivable_lane_state(PathWithLaneId * const auto & stop_pose = autoware_utils::get_pose(path->points.at(0)); planning_factor_interface_->add( path->points, planner_data_->current_odometry->pose, stop_pose, stop_pose, - tier4_planning_msgs::msg::PlanningFactor::STOP, tier4_planning_msgs::msg::SafetyFactorArray{}, - true /*is_driving_forward*/, 0.0, 0.0 /*shift distance*/, ""); + autoware_internal_planning_msgs::msg::PlanningFactor::STOP, + autoware_internal_planning_msgs::msg::SafetyFactorArray{}, true /*is_driving_forward*/, 0.0, + 0.0 /*shift distance*/, ""); const auto & virtual_wall_pose = autoware::motion_utils::calcLongitudinalOffsetPose( path->points, stop_pose.position, debug_data_.base_link2front); @@ -258,8 +260,9 @@ void NoDrivableLaneModule::handle_stopped_state(PathWithLaneId * path) const auto & stop_pose = ego_pos_on_path.pose; planning_factor_interface_->add( path->points, planner_data_->current_odometry->pose, stop_pose, stop_pose, - tier4_planning_msgs::msg::PlanningFactor::STOP, tier4_planning_msgs::msg::SafetyFactorArray{}, - true /*is_driving_forward*/, 0.0, 0.0 /*shift distance*/, ""); + autoware_internal_planning_msgs::msg::PlanningFactor::STOP, + autoware_internal_planning_msgs::msg::SafetyFactorArray{}, true /*is_driving_forward*/, 0.0, + 0.0 /*shift distance*/, ""); const auto virtual_wall_pose = autoware::motion_utils::calcLongitudinalOffsetPose( path->points, stop_pose.position, debug_data_.base_link2front); diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/package.xml b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/package.xml index 000880d6ed3bd..875f57a2ad40d 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/package.xml +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/package.xml @@ -20,6 +20,7 @@ autoware_behavior_velocity_planner autoware_behavior_velocity_planner_common autoware_behavior_velocity_rtc_interface + autoware_internal_planning_msgs autoware_interpolation autoware_lanelet2_extension autoware_motion_utils diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.cpp index ee262339148a7..ad39fe532ee4d 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.cpp @@ -146,8 +146,8 @@ bool NoStoppingAreaModule::modifyPathVelocity(PathWithLaneId * path) { planning_factor_interface_->add( path->points, planner_data_->current_odometry->pose, stop_point->second, stop_point->second, - tier4_planning_msgs::msg::PlanningFactor::STOP, - tier4_planning_msgs::msg::SafetyFactorArray{}, true /*is_driving_forward*/, 0.0, + autoware_internal_planning_msgs::msg::PlanningFactor::STOP, + autoware_internal_planning_msgs::msg::SafetyFactorArray{}, true /*is_driving_forward*/, 0.0, 0.0 /*shift distance*/, ""); } diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/scene.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/scene.cpp index e9165926f829d..8aa0bd0cdea08 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/scene.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/scene.cpp @@ -769,8 +769,9 @@ bool RunOutModule::insertStopPoint( planning_factor_interface_->add( path.points, planner_data_->current_odometry->pose, stop_point.value(), stop_point.value(), - tier4_planning_msgs::msg::PlanningFactor::STOP, tier4_planning_msgs::msg::SafetyFactorArray{}, - true /*is_driving_forward*/, 0.0 /*velocity*/, 0.0 /*shift_distance*/, "run_out_stop"); + autoware_internal_planning_msgs::msg::PlanningFactor::STOP, + autoware_internal_planning_msgs::msg::SafetyFactorArray{}, true /*is_driving_forward*/, + 0.0 /*velocity*/, 0.0 /*shift_distance*/, "run_out_stop"); return true; } @@ -883,9 +884,9 @@ void RunOutModule::insertApproachingVelocity( planning_factor_interface_->add( output_path.points, planner_data_->current_odometry->pose, stop_point.value(), - stop_point.value(), tier4_planning_msgs::msg::PlanningFactor::STOP, - tier4_planning_msgs::msg::SafetyFactorArray{}, true /*is_driving_forward*/, 0.0 /*velocity*/, - 0.0 /*shift_distance*/, "run_out_approaching_velocity"); + stop_point.value(), autoware_internal_planning_msgs::msg::PlanningFactor::STOP, + autoware_internal_planning_msgs::msg::SafetyFactorArray{}, true /*is_driving_forward*/, + 0.0 /*velocity*/, 0.0 /*shift_distance*/, "run_out_approaching_velocity"); // debug debug_ptr_->pushStopPose(autoware_utils::calc_offset_pose( diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/scene.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/scene.cpp index 87b75b7126fa7..a9b1175287266 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/scene.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/scene.cpp @@ -68,8 +68,9 @@ bool StopLineModule::modifyPathVelocity(PathWithLaneId * path) planning_factor_interface_->add( path->points, trajectory->compute(*stop_point).point.pose, planner_data_->current_odometry->pose, planner_data_->current_odometry->pose, - tier4_planning_msgs::msg::PlanningFactor::STOP, tier4_planning_msgs::msg::SafetyFactorArray{}, - true /*is_driving_forward*/, 0.0, 0.0 /*shift distance*/, "stopline"); + autoware_internal_planning_msgs::msg::PlanningFactor::STOP, + autoware_internal_planning_msgs::msg::SafetyFactorArray{}, true /*is_driving_forward*/, 0.0, + 0.0 /*shift distance*/, "stopline"); updateStateAndStoppedTime( &state_, &stopped_time_, clock_->now(), *stop_point - ego_s, planner_data_->isVehicleStopped()); diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/package.xml b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/package.xml index 0923e9645b53e..2ece1b9506b9c 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/package.xml +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/package.xml @@ -22,6 +22,7 @@ autoware_behavior_velocity_planner autoware_behavior_velocity_planner_common autoware_behavior_velocity_rtc_interface + autoware_internal_planning_msgs autoware_lanelet2_extension autoware_motion_utils autoware_perception_msgs diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/scene.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/scene.cpp index 2d3479b855787..8e02c91afab69 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/scene.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/scene.cpp @@ -297,8 +297,9 @@ autoware_internal_planning_msgs::msg::PathWithLaneId TrafficLightModule::insertS planning_factor_interface_->add( modified_path.points, planner_data_->current_odometry->pose, target_point_with_lane_id.point.pose, target_point_with_lane_id.point.pose, - tier4_planning_msgs::msg::PlanningFactor::STOP, tier4_planning_msgs::msg::SafetyFactorArray{}, - true /*is_driving_forward*/, 0.0, 0.0 /*shift distance*/, ""); + autoware_internal_planning_msgs::msg::PlanningFactor::STOP, + autoware_internal_planning_msgs::msg::SafetyFactorArray{}, true /*is_driving_forward*/, 0.0, + 0.0 /*shift distance*/, ""); return modified_path; } diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/package.xml b/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/package.xml index 4ccc96599cf42..d7d59e2b056a3 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/package.xml +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/package.xml @@ -20,6 +20,7 @@ autoware_adapi_v1_msgs autoware_behavior_velocity_planner autoware_behavior_velocity_planner_common + autoware_internal_planning_msgs autoware_lanelet2_extension autoware_motion_utils autoware_planning_msgs diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.cpp index 5db3983be12e2..d2aad40b7a178 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.cpp @@ -421,8 +421,9 @@ void VirtualTrafficLightModule::insertStopVelocityAtStopLine( // Set StopReason planning_factor_interface_->add( path->points, planner_data_->current_odometry->pose, stop_pose, stop_pose, - tier4_planning_msgs::msg::PlanningFactor::STOP, tier4_planning_msgs::msg::SafetyFactorArray{}, - true /*is_driving_forward*/, 0.0, 0.0 /*shift distance*/, ""); + autoware_internal_planning_msgs::msg::PlanningFactor::STOP, + autoware_internal_planning_msgs::msg::SafetyFactorArray{}, true /*is_driving_forward*/, 0.0, + 0.0 /*shift distance*/, ""); // Set data for visualization module_data_.stop_head_pose_at_stop_line = @@ -455,8 +456,9 @@ void VirtualTrafficLightModule::insertStopVelocityAtEndLine( // Set StopReason planning_factor_interface_->add( path->points, planner_data_->current_odometry->pose, stop_pose, stop_pose, - tier4_planning_msgs::msg::PlanningFactor::STOP, tier4_planning_msgs::msg::SafetyFactorArray{}, - true /*is_driving_forward*/, 0.0, 0.0 /*shift distance*/, ""); + autoware_internal_planning_msgs::msg::PlanningFactor::STOP, + autoware_internal_planning_msgs::msg::SafetyFactorArray{}, true /*is_driving_forward*/, 0.0, + 0.0 /*shift distance*/, ""); // Set data for visualization module_data_.stop_head_pose_at_end_line = diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/src/scene_walkway.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/src/scene_walkway.cpp index f2e8ecff6894c..4d8651d05c330 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/src/scene_walkway.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/src/scene_walkway.cpp @@ -124,8 +124,9 @@ bool WalkwayModule::modifyPathVelocity(PathWithLaneId * path) /* get stop point and stop factor */ planning_factor_interface_->add( path->points, planner_data_->current_odometry->pose, stop_pose.value(), stop_pose.value(), - tier4_planning_msgs::msg::PlanningFactor::STOP, tier4_planning_msgs::msg::SafetyFactorArray{}, - true /*is_driving_forward*/, 0.0 /*velocity*/, 0.0 /*shift distance*/, "walkway_stop"); + autoware_internal_planning_msgs::msg::PlanningFactor::STOP, + autoware_internal_planning_msgs::msg::SafetyFactorArray{}, true /*is_driving_forward*/, + 0.0 /*velocity*/, 0.0 /*shift distance*/, "walkway_stop"); // use arc length to identify if ego vehicle is in front of walkway stop or not. const double signed_arc_dist_to_stop_point = diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/package.xml b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/package.xml index dca6e3b5a473a..f66e1afc48dac 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/package.xml +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/package.xml @@ -13,6 +13,7 @@ ament_cmake_auto autoware_cmake + autoware_internal_planning_msgs autoware_motion_utils autoware_motion_velocity_planner_common_universe autoware_osqp_interface diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/src/type_alias.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/src/type_alias.hpp index 24468376c5ae7..d69a33fd918de 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/src/type_alias.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/src/type_alias.hpp @@ -29,10 +29,10 @@ #include "tier4_planning_msgs/msg/velocity_limit.hpp" #include "tier4_planning_msgs/msg/velocity_limit_clear_command.hpp" #include "visualization_msgs/msg/marker_array.hpp" +#include +#include #include #include -#include -#include #include #include @@ -63,8 +63,8 @@ using autoware_utils::Polygon2d; using Metric = tier4_metric_msgs::msg::Metric; using MetricArray = tier4_metric_msgs::msg::MetricArray; using PointCloud = pcl::PointCloud; -using tier4_planning_msgs::msg::PlanningFactor; -using tier4_planning_msgs::msg::SafetyFactorArray; +using autoware_internal_planning_msgs::msg::PlanningFactor; +using autoware_internal_planning_msgs::msg::SafetyFactorArray; } // namespace autoware::motion_velocity_planner #endif // TYPE_ALIAS_HPP_ diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_slow_down_module/package.xml b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_slow_down_module/package.xml index 0d7eeba38a935..2475101311ba6 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_slow_down_module/package.xml +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_slow_down_module/package.xml @@ -13,6 +13,7 @@ ament_cmake_auto autoware_cmake + autoware_internal_planning_msgs autoware_motion_utils autoware_motion_velocity_planner_common_universe autoware_perception_msgs diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_slow_down_module/src/type_alias.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_slow_down_module/src/type_alias.hpp index 24468376c5ae7..d69a33fd918de 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_slow_down_module/src/type_alias.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_slow_down_module/src/type_alias.hpp @@ -29,10 +29,10 @@ #include "tier4_planning_msgs/msg/velocity_limit.hpp" #include "tier4_planning_msgs/msg/velocity_limit_clear_command.hpp" #include "visualization_msgs/msg/marker_array.hpp" +#include +#include #include #include -#include -#include #include #include @@ -63,8 +63,8 @@ using autoware_utils::Polygon2d; using Metric = tier4_metric_msgs::msg::Metric; using MetricArray = tier4_metric_msgs::msg::MetricArray; using PointCloud = pcl::PointCloud; -using tier4_planning_msgs::msg::PlanningFactor; -using tier4_planning_msgs::msg::SafetyFactorArray; +using autoware_internal_planning_msgs::msg::PlanningFactor; +using autoware_internal_planning_msgs::msg::SafetyFactorArray; } // namespace autoware::motion_velocity_planner #endif // TYPE_ALIAS_HPP_ diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_stop_module/package.xml b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_stop_module/package.xml index 2a838239fbe95..8092ce71290f5 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_stop_module/package.xml +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_stop_module/package.xml @@ -14,6 +14,7 @@ ament_cmake_auto autoware_cmake + autoware_internal_planning_msgs autoware_motion_utils autoware_motion_velocity_planner_common_universe autoware_perception_msgs diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_stop_module/src/type_alias.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_stop_module/src/type_alias.hpp index 11ebdf1139c63..a8d9a4c8ea9ee 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_stop_module/src/type_alias.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_stop_module/src/type_alias.hpp @@ -26,10 +26,10 @@ #include "nav_msgs/msg/odometry.hpp" #include "sensor_msgs/msg/point_cloud2.hpp" #include "visualization_msgs/msg/marker_array.hpp" +#include +#include #include #include -#include -#include #include #include @@ -57,7 +57,7 @@ using autoware_utils::Polygon2d; using Metric = tier4_metric_msgs::msg::Metric; using MetricArray = tier4_metric_msgs::msg::MetricArray; using PointCloud = pcl::PointCloud; -using tier4_planning_msgs::msg::PlanningFactor; -using tier4_planning_msgs::msg::SafetyFactorArray; +using autoware_internal_planning_msgs::msg::PlanningFactor; +using autoware_internal_planning_msgs::msg::SafetyFactorArray; } // namespace autoware::motion_velocity_planner #endif // TYPE_ALIAS_HPP_ diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_common_universe/include/autoware/motion_velocity_planner_common_universe/plugin_module_interface.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common_universe/include/autoware/motion_velocity_planner_common_universe/plugin_module_interface.hpp index 8ede2e3686bb6..48f7330070ff9 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_common_universe/include/autoware/motion_velocity_planner_common_universe/plugin_module_interface.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common_universe/include/autoware/motion_velocity_planner_common_universe/plugin_module_interface.hpp @@ -32,8 +32,8 @@ namespace autoware::motion_velocity_planner { -using tier4_planning_msgs::msg::PlanningFactor; -using tier4_planning_msgs::msg::SafetyFactorArray; +using autoware_internal_planning_msgs::msg::PlanningFactor; +using autoware_internal_planning_msgs::msg::SafetyFactorArray; class PluginModuleInterface { diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_common_universe/package.xml b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common_universe/package.xml index fe19c60342ced..731d443c28bc2 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_common_universe/package.xml +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common_universe/package.xml @@ -20,6 +20,7 @@ autoware_behavior_velocity_planner_common autoware_internal_debug_msgs + autoware_internal_planning_msgs autoware_motion_utils autoware_object_recognition_utils autoware_perception_msgs diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node_universe/README.md b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node_universe/README.md index ef09e856eb72b..cfa9c593fba20 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node_universe/README.md +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node_universe/README.md @@ -30,10 +30,10 @@ This means that to stop before a wall, a stop point is inserted in the trajector ## Output topics -| Name | Type | Description | -| ----------------------------------------- | ---------------------------------------------- | -------------------------------------------------- | -| `~/output/trajectory` | autoware_planning_msgs::msg::Trajectory | Ego trajectory with updated velocity profile | -| `~/output/planning_factors/` | tier4_planning_msgs::msg::PlanningFactorsArray | factors causing change in the ego velocity profile | +| Name | Type | Description | +| ----------------------------------------- | ---------------------------------------------------------- | -------------------------------------------------- | +| `~/output/trajectory` | autoware_planning_msgs::msg::Trajectory | Ego trajectory with updated velocity profile | +| `~/output/planning_factors/` | autoware_internal_planning_msgs::msg::PlanningFactorsArray | factors causing change in the ego velocity profile | ## Services diff --git a/system/autoware_default_adapi/package.xml b/system/autoware_default_adapi/package.xml index 3ad96db1399ec..af210f2b6c4c8 100644 --- a/system/autoware_default_adapi/package.xml +++ b/system/autoware_default_adapi/package.xml @@ -19,6 +19,7 @@ autoware_component_interface_utils autoware_diagnostic_graph_utils autoware_geography_utils + autoware_internal_planning_msgs autoware_motion_utils autoware_planning_msgs autoware_system_msgs diff --git a/system/autoware_default_adapi/src/planning.hpp b/system/autoware_default_adapi/src/planning.hpp index 2204b92dc3568..35f2f935c08f5 100644 --- a/system/autoware_default_adapi/src/planning.hpp +++ b/system/autoware_default_adapi/src/planning.hpp @@ -22,7 +22,7 @@ #include #include -#include +#include #include #include @@ -39,8 +39,8 @@ using autoware_adapi_v1_msgs::msg::SteeringFactor; using autoware_adapi_v1_msgs::msg::SteeringFactorArray; using autoware_adapi_v1_msgs::msg::VelocityFactor; using autoware_adapi_v1_msgs::msg::VelocityFactorArray; -using tier4_planning_msgs::msg::PlanningFactor; -using tier4_planning_msgs::msg::PlanningFactorArray; +using autoware_internal_planning_msgs::msg::PlanningFactor; +using autoware_internal_planning_msgs::msg::PlanningFactorArray; class PlanningNode : public rclcpp::Node { diff --git a/system/autoware_dummy_infrastructure/include/autoware/dummy_infrastructure/dummy_infrastructure_node.hpp b/system/autoware_dummy_infrastructure/include/autoware/dummy_infrastructure/dummy_infrastructure_node.hpp index 5b60d94fb7859..7d44247e58cce 100644 --- a/system/autoware_dummy_infrastructure/include/autoware/dummy_infrastructure/dummy_infrastructure_node.hpp +++ b/system/autoware_dummy_infrastructure/include/autoware/dummy_infrastructure/dummy_infrastructure_node.hpp @@ -19,8 +19,8 @@ #include +#include #include -#include #include #include @@ -34,8 +34,8 @@ namespace autoware::dummy_infrastructure { +using autoware_internal_planning_msgs::msg::PlanningFactorArray; using nav_msgs::msg::Odometry; -using tier4_planning_msgs::msg::PlanningFactorArray; using tier4_v2x_msgs::msg::InfrastructureCommand; using tier4_v2x_msgs::msg::InfrastructureCommandArray; using tier4_v2x_msgs::msg::VirtualTrafficLightState; diff --git a/system/autoware_dummy_infrastructure/package.xml b/system/autoware_dummy_infrastructure/package.xml index f40221a73fa5e..705e2bb77f8b3 100644 --- a/system/autoware_dummy_infrastructure/package.xml +++ b/system/autoware_dummy_infrastructure/package.xml @@ -17,10 +17,10 @@ libboost-dev + autoware_internal_planning_msgs autoware_utils rclcpp rclcpp_components - tier4_planning_msgs tier4_v2x_msgs ament_lint_auto diff --git a/visualization/tier4_planning_factor_rviz_plugin/package.xml b/visualization/tier4_planning_factor_rviz_plugin/package.xml index 93c611ba9b966..264d7c632410c 100644 --- a/visualization/tier4_planning_factor_rviz_plugin/package.xml +++ b/visualization/tier4_planning_factor_rviz_plugin/package.xml @@ -14,12 +14,12 @@ qtbase5-dev + autoware_internal_planning_msgs autoware_motion_utils autoware_utils autoware_vehicle_info_utils rviz_common rviz_default_plugins - tier4_planning_msgs libqt5-widgets rviz2 diff --git a/visualization/tier4_planning_factor_rviz_plugin/plugins.xml b/visualization/tier4_planning_factor_rviz_plugin/plugins.xml index 7e5b53d2b1d61..d3adcff83409f 100644 --- a/visualization/tier4_planning_factor_rviz_plugin/plugins.xml +++ b/visualization/tier4_planning_factor_rviz_plugin/plugins.xml @@ -2,7 +2,7 @@ tier4_planning_factor_rviz_plugin - tier4_planning_msgs/msg/PlanningFactorArray + autoware_internal_planning_msgs/msg/PlanningFactorArray diff --git a/visualization/tier4_planning_factor_rviz_plugin/src/planning_factor_rviz_plugin.cpp b/visualization/tier4_planning_factor_rviz_plugin/src/planning_factor_rviz_plugin.cpp index 52bba6f9d2736..75e937a6955d5 100644 --- a/visualization/tier4_planning_factor_rviz_plugin/src/planning_factor_rviz_plugin.cpp +++ b/visualization/tier4_planning_factor_rviz_plugin/src/planning_factor_rviz_plugin.cpp @@ -149,14 +149,14 @@ visualization_msgs::msg::MarkerArray createTargetMarker( } // namespace void PlanningFactorRvizPlugin::processMessage( - const tier4_planning_msgs::msg::PlanningFactorArray::ConstSharedPtr msg) + const autoware_internal_planning_msgs::msg::PlanningFactorArray::ConstSharedPtr msg) { size_t i = 0L; for (const auto & factor : msg->factors) { const auto text = factor.module + (factor.detail.empty() ? "" : " (" + factor.detail + ")"); switch (factor.behavior) { - case tier4_planning_msgs::msg::PlanningFactor::STOP: + case autoware_internal_planning_msgs::msg::PlanningFactor::STOP: for (const auto & control_point : factor.control_points) { const auto virtual_wall = createStopVirtualWallMarker( control_point.pose, text, msg->header.stamp, i++, baselink2front_.getFloat()); @@ -164,7 +164,7 @@ void PlanningFactorRvizPlugin::processMessage( } break; - case tier4_planning_msgs::msg::PlanningFactor::SLOW_DOWN: + case autoware_internal_planning_msgs::msg::PlanningFactor::SLOW_DOWN: for (const auto & control_point : factor.control_points) { const auto virtual_wall = createSlowDownVirtualWallMarker( control_point.pose, text, msg->header.stamp, i++, baselink2front_.getFloat()); diff --git a/visualization/tier4_planning_factor_rviz_plugin/src/planning_factor_rviz_plugin.hpp b/visualization/tier4_planning_factor_rviz_plugin/src/planning_factor_rviz_plugin.hpp index f2af16e3e06b1..2d0c78042c07e 100644 --- a/visualization/tier4_planning_factor_rviz_plugin/src/planning_factor_rviz_plugin.hpp +++ b/visualization/tier4_planning_factor_rviz_plugin/src/planning_factor_rviz_plugin.hpp @@ -21,17 +21,18 @@ #include #include -#include +#include #include namespace autoware::rviz_plugins { -using RosTopicDisplay = rviz_common::RosTopicDisplay; +using RosTopicDisplay = + rviz_common::RosTopicDisplay; class PlanningFactorRvizPlugin -: public rviz_common::RosTopicDisplay +: public rviz_common::RosTopicDisplay { public: PlanningFactorRvizPlugin() @@ -46,7 +47,7 @@ class PlanningFactorRvizPlugin RosTopicDisplay::RTDClass::onInitialize(); marker_common_.initialize(this->context_, this->scene_node_); QString message_type = QString::fromStdString( - rosidl_generator_traits::name()); + rosidl_generator_traits::name()); this->topic_property_->setMessageType(message_type); this->topic_property_->setValue(topic_name_.c_str()); this->topic_property_->setDescription("Topic to subscribe to."); @@ -90,7 +91,7 @@ class PlanningFactorRvizPlugin private: void processMessage( - const tier4_planning_msgs::msg::PlanningFactorArray::ConstSharedPtr msg) override; + const autoware_internal_planning_msgs::msg::PlanningFactorArray::ConstSharedPtr msg) override; rviz_default_plugins::displays::MarkerCommon marker_common_;