Merge branch 'main' into main

control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp

@@ -497,8 +497,13 @@ void PidLongitudinalController::updateControlState(const ControlData & control_d
     stop_dist > p.drive_state_stop_dist + p.drive_state_offset_stop_dist;
   const bool departure_condition_from_stopped = stop_dist > p.drive_state_stop_dist;
 
-  const bool keep_stopped_condition =
-    m_enable_keep_stopped_until_steer_convergence && !lateral_sync_data_.is_steer_converged;
+  // NOTE: the same velocity threshold as motion_utils::searchZeroVelocity
+  static constexpr double vel_epsilon = 1e-3;
+
+  // Let vehicle start after the steering is converged for control accuracy
+  const bool keep_stopped_condition = std::fabs(current_vel) < vel_epsilon &&
+                                      m_enable_keep_stopped_until_steer_convergence &&
+                                      !lateral_sync_data_.is_steer_converged;
 
   const bool stopping_condition = stop_dist < p.stopping_state_stop_dist;
 
@@ -528,8 +533,6 @@ void PidLongitudinalController::updateControlState(const ControlData & control_d
       ? (clock_->now() - *m_last_running_time).seconds() > p.stopped_state_entry_duration_time
       : false;
 
-  static constexpr double vel_epsilon =
-    1e-3;  // NOTE: the same velocity threshold as motion_utils::searchZeroVelocity
   const double current_vel_cmd =
     std::fabs(m_trajectory.points.at(control_data.nearest_idx).longitudinal_velocity_mps);
   const bool emergency_condition = m_enable_overshoot_emergency &&

planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp

@@ -222,6 +222,23 @@ void NormalLaneChange::insertStopPoint(
   const auto target_objects = getTargetObjects(status_.current_lanes, status_.target_lanes);
   double stopping_distance = distance_to_terminal - lane_change_buffer - stop_point_buffer;
 
+  const auto is_valid_start_point = std::invoke([&]() -> bool {
+    auto lc_start_point = lanelet::utils::conversion::toLaneletPoint(
+      status_.lane_change_path.info.lane_changing_start.position);
+    const auto target_neighbor_preferred_lane_poly_2d =
+      utils::lane_change::getTargetNeighborLanesPolygon(
+        *route_handler, status_.current_lanes, type_);
+    return boost::geometry::covered_by(
+      lanelet::traits::to2D(lc_start_point), target_neighbor_preferred_lane_poly_2d);
+  });
+
+  if (!is_valid_start_point) {
+    const auto stop_point = utils::insertStopPoint(stopping_distance, path);
+    setStopPose(stop_point.point.pose);
+
+    return;
+  }
+
   // calculate minimum distance from path front to the stationary object on the ego lane.
   const auto distance_to_ego_lane_obj = [&]() -> double {
     double distance_to_obj = distance_to_terminal;
@@ -646,7 +663,8 @@ double NormalLaneChange::calcMaximumLaneChangeLength(
   const auto shift_intervals =
     getRouteHandler()->getLateralIntervalsToPreferredLane(current_terminal_lanelet);
   return utils::lane_change::calcMaximumLaneChangeLength(
-    getEgoVelocity(), getCommonParam(), shift_intervals, max_acc);
+    std::max(getCommonParam().minimum_lane_changing_velocity, getEgoVelocity()), getCommonParam(),
+    shift_intervals, max_acc);
 }
 
 std::vector<double> NormalLaneChange::sampleLongitudinalAccValues(
@@ -1250,6 +1268,20 @@ bool NormalLaneChange::getLaneChangePaths(
           boost::geometry::covered_by(lc_start_point, target_neighbor_preferred_lane_poly_2d) ||
           boost::geometry::covered_by(lc_start_point, target_lane_poly_2d);
 
+        LaneChangeInfo lane_change_info;
+        lane_change_info.longitudinal_acceleration =
+          LaneChangePhaseInfo{longitudinal_acc_on_prepare, longitudinal_acc_on_lane_changing};
+        lane_change_info.duration = LaneChangePhaseInfo{prepare_duration, lane_changing_time};
+        lane_change_info.velocity =
+          LaneChangePhaseInfo{prepare_velocity, initial_lane_changing_velocity};
+        lane_change_info.length = LaneChangePhaseInfo{prepare_length, lane_changing_length};
+        lane_change_info.current_lanes = current_lanes;
+        lane_change_info.target_lanes = target_lanes;
+        lane_change_info.lane_changing_start = prepare_segment.points.back().point.pose;
+        lane_change_info.lane_changing_end = target_segment.points.front().point.pose;
+        lane_change_info.lateral_acceleration = lateral_acc;
+        lane_change_info.terminal_lane_changing_velocity = terminal_lane_changing_velocity;
+
         if (!is_valid_start_point) {
           debug_print(
             "Reject: lane changing points are not inside of the target preferred lanes or its "
@@ -1269,21 +1301,8 @@ bool NormalLaneChange::getLaneChangePaths(
           continue;
         }
 
-        LaneChangeInfo lane_change_info;
-        lane_change_info.longitudinal_acceleration =
-          LaneChangePhaseInfo{longitudinal_acc_on_prepare, longitudinal_acc_on_lane_changing};
-        lane_change_info.duration = LaneChangePhaseInfo{prepare_duration, lane_changing_time};
-        lane_change_info.velocity =
-          LaneChangePhaseInfo{prepare_velocity, initial_lane_changing_velocity};
-        lane_change_info.length = LaneChangePhaseInfo{prepare_length, lane_changing_length};
-        lane_change_info.current_lanes = current_lanes;
-        lane_change_info.target_lanes = target_lanes;
-        lane_change_info.lane_changing_start = prepare_segment.points.back().point.pose;
-        lane_change_info.lane_changing_end = target_segment.points.front().point.pose;
         lane_change_info.shift_line = utils::lane_change::getLaneChangingShiftLine(
           prepare_segment, target_segment, target_lane_reference_path, shift_length);
-        lane_change_info.lateral_acceleration = lateral_acc;
-        lane_change_info.terminal_lane_changing_velocity = terminal_lane_changing_velocity;
 
         const auto candidate_path = utils::lane_change::constructCandidatePath(
           lane_change_info, prepare_segment, target_segment, target_lane_reference_path,

planning/planning_validator/include/planning_validator/planning_validator.hpp

@@ -109,7 +109,7 @@ class PlanningValidator : public rclcpp::Node
   int diag_error_count_threshold_ = 0;
   bool display_on_terminal_ = true;
 
-  Updater diag_updater_{this};
+  std::shared_ptr<Updater> diag_updater_ = nullptr;
 
   PlanningValidatorStatus validation_status_;
   ValidationParams validation_params_;  // for thresholds

planning/planning_validator/src/planning_validator.cpp

@@ -47,10 +47,6 @@ PlanningValidator::PlanningValidator(const rclcpp::NodeOptions & options)
 
   setupParameters();
 
-  if (publish_diag_) {
-    setupDiag();
-  }
-
   logger_configure_ = std::make_unique<tier4_autoware_utils::LoggerLevelConfigure>(this);
 }
 
@@ -112,39 +108,40 @@ void PlanningValidator::setStatus(
 
 void PlanningValidator::setupDiag()
 {
+  diag_updater_ = std::make_shared<Updater>(this);
   auto & d = diag_updater_;
-  d.setHardwareID("planning_validator");
+  d->setHardwareID("planning_validator");
 
   std::string ns = "trajectory_validation_";
-  d.add(ns + "finite", [&](auto & stat) {
+  d->add(ns + "finite", [&](auto & stat) {
     setStatus(stat, validation_status_.is_valid_finite_value, "infinite value is found");
   });
-  d.add(ns + "interval", [&](auto & stat) {
+  d->add(ns + "interval", [&](auto & stat) {
     setStatus(stat, validation_status_.is_valid_interval, "points interval is too long");
   });
-  d.add(ns + "relative_angle", [&](auto & stat) {
+  d->add(ns + "relative_angle", [&](auto & stat) {
     setStatus(stat, validation_status_.is_valid_relative_angle, "relative angle is too large");
   });
-  d.add(ns + "curvature", [&](auto & stat) {
+  d->add(ns + "curvature", [&](auto & stat) {
     setStatus(stat, validation_status_.is_valid_curvature, "curvature is too large");
   });
-  d.add(ns + "lateral_acceleration", [&](auto & stat) {
+  d->add(ns + "lateral_acceleration", [&](auto & stat) {
     setStatus(stat, validation_status_.is_valid_lateral_acc, "lateral acceleration is too large");
   });
-  d.add(ns + "acceleration", [&](auto & stat) {
+  d->add(ns + "acceleration", [&](auto & stat) {
     setStatus(stat, validation_status_.is_valid_longitudinal_max_acc, "acceleration is too large");
   });
-  d.add(ns + "deceleration", [&](auto & stat) {
+  d->add(ns + "deceleration", [&](auto & stat) {
     setStatus(stat, validation_status_.is_valid_longitudinal_min_acc, "deceleration is too large");
   });
-  d.add(ns + "steering", [&](auto & stat) {
+  d->add(ns + "steering", [&](auto & stat) {
     setStatus(stat, validation_status_.is_valid_steering, "expected steering is too large");
   });
-  d.add(ns + "steering_rate", [&](auto & stat) {
+  d->add(ns + "steering_rate", [&](auto & stat) {
     setStatus(
       stat, validation_status_.is_valid_steering_rate, "expected steering rate is too large");
   });
-  d.add(ns + "velocity_deviation", [&](auto & stat) {
+  d->add(ns + "velocity_deviation", [&](auto & stat) {
     setStatus(
       stat, validation_status_.is_valid_velocity_deviation, "velocity deviation is too large");
   });
@@ -174,11 +171,15 @@ void PlanningValidator::onTrajectory(const Trajectory::ConstSharedPtr msg)
 
   if (!isDataReady()) return;
 
+  if (publish_diag_ && !diag_updater_) {
+    setupDiag();  // run setup after all data is ready.
+  }
+
   debug_pose_publisher_->clearMarkers();
 
   validate(*current_trajectory_);
 
-  diag_updater_.force_update();
+  diag_updater_->force_update();
 
   publishTrajectory();
 

simulator/simple_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp

@@ -171,36 +171,36 @@ class PLANNING_SIMULATOR_PUBLIC SimplePlanningSimulator : public rclcpp::Node
   tf2_ros::TransformListener tf_listener_;
 
   /* received & published topics */
-  PoseWithCovarianceStamped::ConstSharedPtr initial_pose_;
-  TwistStamped initial_twist_;
-  VelocityReport current_velocity_;
-  Odometry current_odometry_;
-  SteeringReport current_steer_;
-  AckermannControlCommand current_ackermann_cmd_;
-  AckermannControlCommand current_manual_ackermann_cmd_;
-  GearCommand current_gear_cmd_;
-  GearCommand current_manual_gear_cmd_;
-  TurnIndicatorsCommand::ConstSharedPtr current_turn_indicators_cmd_ptr_;
-  HazardLightsCommand::ConstSharedPtr current_hazard_lights_cmd_ptr_;
-  Trajectory::ConstSharedPtr current_trajectory_ptr_;
-  bool simulate_motion_;  //!< stop vehicle motion simulation if false
-  ControlModeReport current_control_mode_;
-  bool enable_road_slope_simulation_;
+  PoseWithCovarianceStamped::ConstSharedPtr initial_pose_{};
+  TwistStamped initial_twist_{};
+  VelocityReport current_velocity_{};
+  Odometry current_odometry_{};
+  SteeringReport current_steer_{};
+  AckermannControlCommand current_ackermann_cmd_{};
+  AckermannControlCommand current_manual_ackermann_cmd_{};
+  GearCommand current_gear_cmd_{};
+  GearCommand current_manual_gear_cmd_{};
+  TurnIndicatorsCommand::ConstSharedPtr current_turn_indicators_cmd_ptr_{};
+  HazardLightsCommand::ConstSharedPtr current_hazard_lights_cmd_ptr_{};
+  Trajectory::ConstSharedPtr current_trajectory_ptr_{};
+  bool simulate_motion_ = true;  //!< stop vehicle motion simulation if false
+  ControlModeReport current_control_mode_{};
+  bool enable_road_slope_simulation_ = true;
 
   /* frame_id */
-  std::string simulated_frame_id_;  //!< @brief simulated vehicle frame id
-  std::string origin_frame_id_;     //!< @brief map frame_id
+  std::string simulated_frame_id_ = "";  //!< @brief simulated vehicle frame id
+  std::string origin_frame_id_ = "";     //!< @brief map frame_id
 
   /* flags */
-  bool is_initialized_;         //!< @brief flag to check the initial position is set
-  bool add_measurement_noise_;  //!< @brief flag to add measurement noise
+  bool is_initialized_ = false;         //!< @brief flag to check the initial position is set
+  bool add_measurement_noise_ = false;  //!< @brief flag to add measurement noise
 
-  DeltaTime delta_time_;  //!< @brief to calculate delta time
+  DeltaTime delta_time_{};  //!< @brief to calculate delta time
 
-  MeasurementNoiseGenerator measurement_noise_;  //!< @brief for measurement noise
+  MeasurementNoiseGenerator measurement_noise_{};  //!< @brief for measurement noise
 
-  double x_stddev_;  //!< @brief x standard deviation for dummy covariance in map coordinate
-  double y_stddev_;  //!< @brief y standard deviation for dummy covariance in map coordinate
+  double x_stddev_ = 0.0;  //!< @brief x standard deviation for dummy covariance in map coordinate
+  double y_stddev_ = 0.0;  //!< @brief y standard deviation for dummy covariance in map coordinate
 
   /* vehicle model */
   enum class VehicleModelType {

system/system_diagnostic_graph/CMakeLists.txt

@@ -4,23 +4,27 @@ project(system_diagnostic_graph)
 find_package(autoware_cmake REQUIRED)
 autoware_package()
 
-ament_auto_add_executable(aggregator
+ament_auto_add_library(${PROJECT_NAME} SHARED
   src/core/config.cpp
   src/core/debug.cpp
   src/core/graph.cpp
-  src/core/nodes.cpp
-  src/core/exprs.cpp
+  src/core/units.cpp
   src/core/modes.cpp
+)
+
+ament_auto_add_executable(aggregator
   src/main.cpp
 )
 
 ament_auto_add_executable(converter
   src/tool.cpp
 )
 
-install(
-  PROGRAMS util/debug.py
-  DESTINATION lib/${PROJECT_NAME}
-)
+if(BUILD_TESTING)
+  get_filename_component(RESOURCE_PATH ${CMAKE_CURRENT_SOURCE_DIR}/test/files ABSOLUTE)
+  ament_auto_add_gtest(gtest_${PROJECT_NAME} test/src/test.cpp)
+  target_compile_definitions(gtest_${PROJECT_NAME} PRIVATE TEST_RESOURCE_PATH="${RESOURCE_PATH}")
+  target_include_directories(gtest_${PROJECT_NAME} PRIVATE src)
+endif()
 
 ament_auto_package(INSTALL_TO_SHARE config example launch)

system/system_diagnostic_graph/example/example_0.yaml

@@ -1,32 +1,37 @@
 files:
-  - { package: system_diagnostic_graph, path: example/example_1.yaml }
-  - { package: system_diagnostic_graph, path: example/example_2.yaml }
+  - { path: $(find-pkg-share system_diagnostic_graph)/example/example_1.yaml }
+  - { path: $(find-pkg-share system_diagnostic_graph)/example/example_2.yaml }
 
 nodes:
   - path: /autoware/modes/stop
-    type: debug-ok
+    type: ok
 
   - path: /autoware/modes/autonomous
     type: and
     list:
-      - { type: link, path: /functions/pose_estimation }
-      - { type: link, path: /functions/obstacle_detection }
+      - { type: link, link: /functions/pose_estimation }
+      - { type: link, link: /functions/obstacle_detection }
 
   - path: /autoware/modes/local
     type: and
     list:
-      - { type: link, path: /external/joystick_command }
+      - { type: link, link: /external/joystick_command }
 
   - path: /autoware/modes/remote
     type: and
     list:
-      - { type: link, path: /external/remote_command }
+      - { type: link, link: /external/remote_command }
 
   - path: /autoware/modes/emergency-stop
-    type: debug-ok
+    type: ok
 
   - path: /autoware/modes/comfortable-stop
-    type: debug-ok
+    type: and
+    list:
+      - { type: link, link: /functions/obstacle_detection }
 
   - path: /autoware/modes/pull-over
-    type: debug-ok
+    type: and
+    list:
+      - { type: link, link: /functions/pose_estimation }
+      - { type: link, link: /functions/obstacle_detection }

system/system_diagnostic_graph/example/example_1.yaml

@@ -2,22 +2,22 @@ nodes:
   - path: /functions/pose_estimation
     type: and
     list:
-      - { type: link, path: /sensing/lidars/top }
+      - { type: link, link: /sensing/lidars/top }
 
   - path: /functions/obstacle_detection
     type: or
     list:
-      - { type: link, path: /sensing/lidars/front }
-      - { type: link, path: /sensing/radars/front }
+      - { type: link, link: /sensing/lidars/front }
+      - { type: link, link: /sensing/radars/front }
 
   - path: /sensing/lidars/top
     type: diag
-    name: "lidar_driver/top: status"
+    diag: "lidar_driver/top: status"
 
   - path: /sensing/lidars/front
     type: diag
-    name: "lidar_driver/front: status"
+    diag: "lidar_driver/front: status"
 
   - path: /sensing/radars/front
     type: diag
-    name: "radar_driver/front: status"
+    diag: "radar_driver/front: status"

system/system_diagnostic_graph/example/example_2.yaml

@@ -1,8 +1,8 @@
 nodes:
   - path: /external/joystick_command
     type: diag
-    name: "external_command_checker: joystick_command"
+    diag: "external_command_checker: joystick_command"
 
   - path: /external/remote_command
     type: diag
-    name: "external_command_checker: remote_command"
+    diag: "external_command_checker: remote_command"

system/system_diagnostic_graph/package.xml

@@ -15,6 +15,7 @@
   <depend>tier4_system_msgs</depend>
   <depend>yaml_cpp_vendor</depend>
 
+  <test_depend>ament_cmake_gtest</test_depend>
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>autoware_lint_common</test_depend>