Merge branch 'main' into fix/mpc/replace-vectorxd-with-vector3d

control/autoware_mpc_lateral_controller/package.xml

@@ -8,6 +8,7 @@
   <maintainer email="takamasa.horibe@tier4.jp">Takamasa Horibe</maintainer>
   <maintainer email="takayuki.murooka@tier4.jp">Takayuki Murooka</maintainer>
   <maintainer email="kyoichi.sugahara@tier4.jp">Kyoichi Sugahara</maintainer>
+  <maintainer email="alqudah.mohammad@tier4.jp">Alqudah Mohammad</maintainer>
 
   <license>Apache 2.0</license>
 

launch/tier4_system_launch/package.xml

@@ -11,7 +11,16 @@
   <buildtool_depend>ament_cmake_auto</buildtool_depend>
   <buildtool_depend>autoware_cmake</buildtool_depend>
 
+  <exec_depend>autoware_component_interface_tools</exec_depend>
   <exec_depend>autoware_component_state_monitor</exec_depend>
+  <exec_depend>autoware_dummy_diag_publisher</exec_depend>
+  <exec_depend>autoware_duplicated_node_checker</exec_depend>
+  <exec_depend>autoware_hazard_status_converter</exec_depend>
+  <exec_depend>autoware_mrm_comfortable_stop_operator</exec_depend>
+  <exec_depend>autoware_mrm_emergency_stop_operator</exec_depend>
+  <exec_depend>autoware_mrm_handler</exec_depend>
+  <exec_depend>autoware_processing_time_checker</exec_depend>
+  <exec_depend>autoware_system_diagnostic_monitor</exec_depend>
   <exec_depend>autoware_system_monitor</exec_depend>
 
   <test_depend>ament_lint_auto</test_depend>

launch/tier4_vehicle_launch/package.xml

@@ -10,6 +10,7 @@
   <buildtool_depend>ament_cmake_auto</buildtool_depend>
   <buildtool_depend>autoware_cmake</buildtool_depend>
 
+  <exec_depend>autoware_raw_vehicle_cmd_converter</exec_depend>
   <exec_depend>robot_state_publisher</exec_depend>
   <exec_depend>xacro</exec_depend>
 

planning/autoware_obstacle_cruise_planner/package.xml

@@ -19,6 +19,7 @@
   <buildtool_depend>autoware_cmake</buildtool_depend>
 
   <depend>autoware_internal_debug_msgs</depend>
+  <depend>autoware_internal_planning_msgs</depend>
   <depend>autoware_interpolation</depend>
   <depend>autoware_lanelet2_extension</depend>
   <depend>autoware_motion_utils</depend>

planning/autoware_obstacle_cruise_planner/src/pid_based_planner/pid_based_planner.cpp

@@ -336,8 +336,8 @@ std::vector<TrajectoryPoint> 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

planning/autoware_obstacle_cruise_planner/src/planner_interface.cpp

@@ -336,8 +336,8 @@ std::vector<TrajectoryPoint> 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<TrajectoryPoint> 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);
     }
 

planning/autoware_obstacle_stop_planner/package.xml

@@ -22,6 +22,7 @@
 
   <depend>autoware_adapi_v1_msgs</depend>
   <depend>autoware_internal_debug_msgs</depend>
+  <depend>autoware_internal_planning_msgs</depend>
   <depend>autoware_motion_utils</depend>
   <depend>autoware_perception_msgs</depend>
   <depend>autoware_planning_factor_interface</depend>

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<float>::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();
 

planning/autoware_planning_factor_interface/include/autoware/planning_factor_interface/planning_factor_interface.hpp

@@ -18,26 +18,26 @@
 #include <autoware/motion_utils/trajectory/trajectory.hpp>
 #include <rclcpp/rclcpp.hpp>
 
+#include <autoware_internal_planning_msgs/msg/control_point.hpp>
 #include <autoware_internal_planning_msgs/msg/path_point_with_lane_id.hpp>
+#include <autoware_internal_planning_msgs/msg/planning_factor.hpp>
+#include <autoware_internal_planning_msgs/msg/planning_factor_array.hpp>
+#include <autoware_internal_planning_msgs/msg/safety_factor_array.hpp>
 #include <autoware_planning_msgs/msg/path_point.hpp>
 #include <autoware_planning_msgs/msg/trajectory_point.hpp>
 #include <geometry_msgs/msg/pose.hpp>
-#include <tier4_planning_msgs/msg/control_point.hpp>
-#include <tier4_planning_msgs/msg/planning_factor.hpp>
-#include <tier4_planning_msgs/msg/planning_factor_array.hpp>
-#include <tier4_planning_msgs/msg/safety_factor_array.hpp>
 
 #include <string>
 #include <vector>
 
 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<ControlPoint>()
+    const auto control_point = autoware_internal_planning_msgs::build<ControlPoint>()
                                  .pose(control_point_pose)
                                  .velocity(velocity)
                                  .shift_length(shift_length)
                                  .distance(distance);
 
-    const auto factor = tier4_planning_msgs::build<PlanningFactor>()
+    const auto factor = autoware_internal_planning_msgs::build<PlanningFactor>()
                           .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<ControlPoint>()
+    const auto control_start_point = autoware_internal_planning_msgs::build<ControlPoint>()
                                        .pose(start_pose)
                                        .velocity(velocity)
                                        .shift_length(shift_length)
                                        .distance(start_distance);
 
-    const auto control_end_point = tier4_planning_msgs::build<ControlPoint>()
+    const auto control_end_point = autoware_internal_planning_msgs::build<ControlPoint>()
                                      .pose(end_pose)
                                      .velocity(velocity)
                                      .shift_length(shift_length)
                                      .distance(end_distance);
 
-    const auto factor = tier4_planning_msgs::build<PlanningFactor>()
+    const auto factor = autoware_internal_planning_msgs::build<PlanningFactor>()
                           .module(name_)
                           .is_driving_forward(is_driving_forward)
                           .control_points({control_start_point, control_end_point})

planning/autoware_planning_factor_interface/package.xml

@@ -13,11 +13,11 @@
   <buildtool_depend>ament_cmake_auto</buildtool_depend>
   <buildtool_depend>autoware_cmake</buildtool_depend>
 
+  <depend>autoware_internal_planning_msgs</depend>
   <depend>autoware_motion_utils</depend>
   <depend>autoware_planning_msgs</depend>
   <depend>autoware_utils</depend>
   <depend>rclcpp</depend>
-  <depend>tier4_planning_msgs</depend>
 
   <test_depend>ament_cmake_ros</test_depend>
   <test_depend>ament_lint_auto</test_depend>

planning/autoware_surround_obstacle_checker/package.xml

@@ -14,6 +14,7 @@
   <buildtool_depend>autoware_cmake</buildtool_depend>
   <buildtool_depend>eigen3_cmake_module</buildtool_depend>
 
+  <depend>autoware_internal_planning_msgs</depend>
   <depend>autoware_motion_utils</depend>
   <depend>autoware_perception_msgs</depend>
   <depend>autoware_planning_factor_interface</depend>

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();
 

planning/autoware_surround_obstacle_checker/src/debug_marker.hpp

@@ -19,9 +19,9 @@
 #include <autoware_vehicle_info_utils/vehicle_info_utils.hpp>
 #include <rclcpp/rclcpp.hpp>
 
+#include <autoware_internal_planning_msgs/msg/planning_factor_array.hpp>
 #include <geometry_msgs/msg/polygon_stamped.hpp>
 #include <geometry_msgs/msg/pose.hpp>
-#include <tier4_planning_msgs/msg/planning_factor_array.hpp>
 #include <visualization_msgs/msg/marker.hpp>
 #include <visualization_msgs/msg/marker_array.hpp>
 
@@ -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;
 

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;

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 <autoware_utils/geometry/geometry.hpp>
 #include <autoware_utils/geometry/pose_deviation.hpp>
 
+#include <autoware_internal_planning_msgs/msg/planning_factor_array.hpp>
 #include <autoware_perception_msgs/msg/predicted_object.hpp>
 #include <autoware_perception_msgs/msg/predicted_path.hpp>
 #include <geometry_msgs/msg/pose.hpp>
 #include <geometry_msgs/msg/twist.hpp>
-#include <tier4_planning_msgs/msg/planning_factor_array.hpp>
 
 #include <cmath>
 #include <string>
@@ -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
 

planning/behavior_path_planner/autoware_behavior_path_planner_common/package.xml

@@ -45,6 +45,7 @@
 
   <depend>autoware_adapi_v1_msgs</depend>
   <depend>autoware_freespace_planning_algorithms</depend>
+  <depend>autoware_internal_planning_msgs</depend>
   <depend>autoware_interpolation</depend>
   <depend>autoware_lane_departure_checker</depend>
   <depend>autoware_lanelet2_extension</depend>

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);

planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/package.xml

@@ -20,6 +20,7 @@
   <depend>autoware_behavior_velocity_planner</depend>
   <depend>autoware_behavior_velocity_planner_common</depend>
   <depend>autoware_behavior_velocity_rtc_interface</depend>
+  <depend>autoware_internal_planning_msgs</depend>
   <depend>autoware_lanelet2_extension</depend>
   <depend>autoware_motion_utils</depend>
   <depend>autoware_perception_msgs</depend>

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;
 }

planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/package.xml

@@ -26,6 +26,7 @@
   <depend>autoware_behavior_velocity_rtc_interface</depend>
   <depend>autoware_grid_map_utils</depend>
   <depend>autoware_internal_debug_msgs</depend>
+  <depend>autoware_internal_planning_msgs</depend>
   <depend>autoware_interpolation</depend>
   <depend>autoware_lanelet2_extension</depend>
   <depend>autoware_motion_utils</depend>

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(

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;

planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/package.xml

@@ -23,6 +23,7 @@
   <depend>autoware_behavior_velocity_planner_common</depend>
   <depend>autoware_behavior_velocity_rtc_interface</depend>
   <depend>autoware_internal_debug_msgs</depend>
+  <depend>autoware_internal_planning_msgs</depend>
   <depend>autoware_interpolation</depend>
   <depend>autoware_lanelet2_extension</depend>
   <depend>autoware_motion_utils</depend>