Merge branch 'main' into feature/smooth_ndt_map_update

Author: anhnv3991

control/pid_longitudinal_controller/package.xml

@@ -7,6 +7,7 @@
 
   <maintainer email="takamasa.horibe@tier4.jp">Takamasa Horibe</maintainer>
   <maintainer email="takayuki.murooka@tier4.jp">Takayuki Murooka</maintainer>
+  <maintainer email="mamoru.sobue@tier4.jp">Mamoru Sobue</maintainer>
 
   <license>Apache 2.0</license>
 

control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp

@@ -456,6 +456,8 @@ PidLongitudinalController::ControlData PidLongitudinalController::getControlData
     current_interpolated_pose.first);
   control_data.nearest_idx = current_interpolated_pose.second + 1;
   control_data.target_idx = control_data.nearest_idx;
+  const auto nearest_point = current_interpolated_pose.first;
+  auto target_point = current_interpolated_pose.first;
 
   // check if the deviation is worth emergency
   m_diagnostic_data.trans_deviation =
@@ -491,11 +493,25 @@ PidLongitudinalController::ControlData PidLongitudinalController::getControlData
     control_data.interpolated_traj.points.insert(
       control_data.interpolated_traj.points.begin() + control_data.target_idx,
       target_interpolated_point.first);
+    target_point = target_interpolated_point.first;
   }
 
+  // ==========================================================================================
+  // NOTE: due to removeOverlapPoints(), the obtained control_data.target_idx and
+  // control_data.nearest_idx may become invalid if the number of points decreased.
+  // current API does not provide the way to check duplication beforehand and this function
+  // does not tell how many/which index points were removed, so there is no way
+  // to tell if our `control_data.target_idx` point still exists or removed.
+  // ==========================================================================================
   // Remove overlapped points after inserting the interpolated points
   control_data.interpolated_traj.points =
     motion_utils::removeOverlapPoints(control_data.interpolated_traj.points);
+  control_data.nearest_idx = motion_utils::findFirstNearestIndexWithSoftConstraints(
+    control_data.interpolated_traj.points, nearest_point.pose, m_ego_nearest_dist_threshold,
+    m_ego_nearest_yaw_threshold);
+  control_data.target_idx = motion_utils::findFirstNearestIndexWithSoftConstraints(
+    control_data.interpolated_traj.points, target_point.pose, m_ego_nearest_dist_threshold,
+    m_ego_nearest_yaw_threshold);
 
   // send debug values
   m_debug_values.setValues(DebugValues::TYPE::PREDICTED_VEL, control_data.state_after_delay.vel);
@@ -610,8 +626,12 @@ void PidLongitudinalController::updateControlState(const ControlData & control_d
       ? (clock_->now() - *m_last_running_time).seconds() > p.stopped_state_entry_duration_time
       : false;
 
-  const double current_vel_cmd =
-    std::fabs(m_trajectory.points.at(control_data.nearest_idx).longitudinal_velocity_mps);
+  // ==========================================================================================
+  // NOTE: due to removeOverlapPoints() in getControlData() m_trajectory and
+  // control_data.interpolated_traj have different size.
+  // ==========================================================================================
+  const double current_vel_cmd = std::fabs(
+    control_data.interpolated_traj.points.at(control_data.nearest_idx).longitudinal_velocity_mps);
   const bool emergency_condition = m_enable_overshoot_emergency &&
                                    stop_dist < -p.emergency_state_overshoot_stop_dist &&
                                    current_vel_cmd < vel_epsilon;

perception/image_projection_based_fusion/src/fusion_node.cpp

@@ -167,7 +167,6 @@ void FusionNode<Msg, Obj, Msg2D>::subCallback(const typename Msg::ConstSharedPtr
     timer_->cancel();
     postprocess(*(cached_msg_.second));
     publish(*(cached_msg_.second));
-    cached_msg_.second = nullptr;
     std::fill(is_fused_.begin(), is_fused_.end(), false);
 
     // add processing time for debug
@@ -187,6 +186,8 @@ void FusionNode<Msg, Obj, Msg2D>::subCallback(const typename Msg::ConstSharedPtr
         "debug/pipeline_latency_ms", pipeline_latency_ms);
       processing_time_ms = 0;
     }
+
+    cached_msg_.second = nullptr;
   }
 
   std::lock_guard<std::mutex> lock(mutex_cached_msgs_);

perception/radar_tracks_msgs_converter/config/radar_tracks_msgs_converter.param.yaml

@@ -1,6 +1,7 @@
 /**:
   ros__parameters:
     update_rate_hz: 20.0
+    new_frame_id: "base_link"
     use_twist_compensation: true
     use_twist_yaw_compensation: false
     static_object_speed_threshold: 1.0

planning/behavior_path_planner_common/src/utils/drivable_area_expansion/static_drivable_area.cpp

@@ -650,14 +650,14 @@ std::optional<size_t> getOverlappedLaneletId(const std::vector<DrivableLanes> &
 
     for (const auto & lanelet : lanelets) {
       for (const auto & target_lanelet : target_lanelets) {
-        std::vector<Point2d> intersections{};
+        std::vector<Polygon2d> intersections{};
         boost::geometry::intersection(
-          lanelet.polygon2d().basicPolygon(), target_lanelet.polygon2d().basicPolygon(),
-          intersections);
+          toPolygon2d(lanelet), toPolygon2d(target_lanelet), intersections);
 
-        // if only one point intersects, it is assumed not to be overlapped
-        if (intersections.size() > 1) {
-          return true;
+        for (const auto & polygon : intersections) {
+          if (boost::geometry::area(polygon) > 1e-3) {
+            return true;
+          }
         }
       }
     }
@@ -1463,11 +1463,10 @@ std::pair<std::vector<lanelet::ConstPoint3d>, bool> getBoundWithFreeSpaceAreas(
       throw std::domain_error("invalid case.");
   }
 
-  const auto goal_is_in_freespace = boost::geometry::within(
-    to2D(toLaneletPoint(route_handler->getGoalPose().position).basicPoint()),
-    to2D(polygon).basicPolygon());
-
-  return std::make_pair(expanded_bound, is_driving_freespace || goal_is_in_freespace);
+  const auto skip_post_process = route_case == RouteCase::INIT_POS_IS_IN_FREESPACE ||
+                                 route_case == RouteCase::GOAL_POS_IS_IN_FREESPACE ||
+                                 is_driving_freespace;
+  return std::make_pair(expanded_bound, skip_post_process);
 }
 
 std::vector<geometry_msgs::msg::Point> postProcess(
@@ -1569,7 +1568,9 @@ std::vector<geometry_msgs::msg::Point> postProcess(
     // Insert a start point
     processed_bound.push_back(start_point);
 
-    return findNearestSegmentIndexFromLateralDistance(tmp_bound, start_point);
+    const auto p_tmp =
+      geometry_msgs::build<Pose>().position(start_point).orientation(front_pose.orientation);
+    return findNearestSegmentIndexFromLateralDistance(tmp_bound, p_tmp, M_PI_2);
   }();
 
   // Get Closest segment for the goal point
@@ -1579,8 +1580,10 @@ std::vector<geometry_msgs::msg::Point> postProcess(
       findNearestSegmentIndexFromLateralDistance(tmp_bound, goal_pose, M_PI_2);
     const auto goal_point =
       calcLongitudinalOffsetGoalPoint(tmp_bound, goal_pose, goal_start_idx, vehicle_length);
-    const size_t goal_idx =
-      std::max(goal_start_idx, findNearestSegmentIndexFromLateralDistance(tmp_bound, goal_point));
+    const auto p_tmp =
+      geometry_msgs::build<Pose>().position(goal_point).orientation(goal_pose.orientation);
+    const size_t goal_idx = std::max(
+      goal_start_idx, findNearestSegmentIndexFromLateralDistance(tmp_bound, p_tmp, M_PI_2));
 
     return std::make_pair(goal_idx, goal_point);
   }();

planning/external_velocity_limit_selector/config/default_common.param.yaml

@@ -1,6 +1,8 @@
 /**:
   ros__parameters:
     # constraints param for normal driving
+    max_vel: 11.1           # max velocity limit [m/s]
+
     normal:
       min_acc: -0.5         # min deceleration [m/ss]
       max_acc: 1.0          # max acceleration [m/ss]

planning/motion_velocity_smoother/config/default_common.param.yaml

@@ -1,6 +1,8 @@
 /**:
   ros__parameters:
     # constraints param for normal driving
+    max_vel: 11.1           # max velocity limit [m/s]
+
     normal:
       min_acc: -0.5         # min deceleration [m/ss]
       max_acc: 1.0          # max acceleration [m/ss]

planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp

@@ -173,7 +173,7 @@ rcl_interfaces::msg::SetParametersResult MotionVelocitySmootherNode::onParameter
     update_param_bool("enable_lateral_acc_limit", p.enable_lateral_acc_limit);
     update_param_bool("enable_steering_rate_limit", p.enable_steering_rate_limit);
 
-    update_param("max_velocity", p.max_velocity);
+    update_param("max_vel", p.max_velocity);
     update_param(
       "margin_to_insert_external_velocity_limit", p.margin_to_insert_external_velocity_limit);
     update_param("replan_vel_deviation", p.replan_vel_deviation);
@@ -277,7 +277,7 @@ void MotionVelocitySmootherNode::initCommonParam()
   p.enable_lateral_acc_limit = declare_parameter<bool>("enable_lateral_acc_limit");
   p.enable_steering_rate_limit = declare_parameter<bool>("enable_steering_rate_limit");
 
-  p.max_velocity = declare_parameter<double>("max_velocity");  // 72.0 kmph
+  p.max_velocity = declare_parameter<double>("max_vel");
   p.margin_to_insert_external_velocity_limit =
     declare_parameter<double>("margin_to_insert_external_velocity_limit");
   p.replan_vel_deviation = declare_parameter<double>("replan_vel_deviation");

planning/obstacle_cruise_planner/config/default_common.param.yaml

@@ -1,6 +1,8 @@
 /**:
   ros__parameters:
     # constraints param for normal driving
+    max_vel: 11.1           # max velocity limit [m/s]
+
     normal:
       min_acc: -1.0         # min deceleration [m/ss]
       max_acc: 1.0          # max acceleration [m/ss]

planning/obstacle_stop_planner/config/common.param.yaml

@@ -1,6 +1,8 @@
 /**:
   ros__parameters:
     # constraints param for normal driving
+    max_vel: 11.1           # max velocity limit [m/s]
+
     normal:
       min_acc: -0.5         # min deceleration [m/ss]
       max_acc: 1.0          # max acceleration [m/ss]

planning/planning_test_utils/config/test_common.param.yaml

@@ -1,6 +1,8 @@
 /**:
   ros__parameters:
     # constraints param for normal driving
+    max_vel: 11.1           # max velocity limit [m/s]
+
     normal:
       min_acc: -1.0         # min deceleration [m/ss]
       max_acc: 1.0          # max acceleration [m/ss]

planning/planning_validator/README.md

@@ -18,6 +18,8 @@ The following features are supported for trajectory validation and can have thre
 - **Steering angle rate** : invalid if the expected steering rate value is too large
 - **Velocity deviation** : invalid if the planning velocity is too far from the ego velocity
 - **Distance deviation** : invalid if the ego is too far from the trajectory
+- **Longitudinal distance deviation** : invalid if the trajectory is too far from ego in longitudinal direction
+- **Forward trajectory length** : invalid if the trajectory length is not enough to stop within a given deceleration
 
 The following features are to be implemented.
 
@@ -63,15 +65,23 @@ The following parameters can be set for the `planning_validator`:
 
 The input trajectory is detected as invalid if the index exceeds the following thresholds.
 
-| Name                              | Type   | Description                                                                                                        | Default value |
-| :-------------------------------- | :----- | :----------------------------------------------------------------------------------------------------------------- | :------------ |
-| `thresholds.interval`             | double | invalid threshold of the distance of two neighboring trajectory points [m]                                         | 100.0         |
-| `thresholds.relative_angle`       | double | invalid threshold of the relative angle of two neighboring trajectory points [rad]                                 | 2.0           |
-| `thresholds.curvature`            | double | invalid threshold of the curvature in each trajectory point [1/m]                                                  | 1.0           |
-| `thresholds.lateral_acc`          | double | invalid threshold of the lateral acceleration in each trajectory point [m/ss]                                      | 9.8           |
-| `thresholds.longitudinal_max_acc` | double | invalid threshold of the maximum longitudinal acceleration in each trajectory point [m/ss]                         | 9.8           |
-| `thresholds.longitudinal_min_acc` | double | invalid threshold of the minimum longitudinal deceleration in each trajectory point [m/ss]                         | -9.8          |
-| `thresholds.steering`             | double | invalid threshold of the steering angle in each trajectory point [rad]                                             | 1.414         |
-| `thresholds.steering_rate`        | double | invalid threshold of the steering angle rate in each trajectory point [rad/s]                                      | 10.0          |
-| `thresholds.velocity_deviation`   | double | invalid threshold of the velocity deviation between the ego velocity and the trajectory point closest to ego [m/s] | 100.0         |
-| `thresholds.distance_deviation`   | double | invalid threshold of the distance deviation between the ego position and the trajectory point closest to ego [m]   | 100.0         |
+| Name                                         | Type   | Description                                                                                                        | Default value |
+| :------------------------------------------- | :----- | :----------------------------------------------------------------------------------------------------------------- | :------------ |
+| `thresholds.interval`                        | double | invalid threshold of the distance of two neighboring trajectory points [m]                                         | 100.0         |
+| `thresholds.relative_angle`                  | double | invalid threshold of the relative angle of two neighboring trajectory points [rad]                                 | 2.0           |
+| `thresholds.curvature`                       | double | invalid threshold of the curvature in each trajectory point [1/m]                                                  | 1.0           |
+| `thresholds.lateral_acc`                     | double | invalid threshold of the lateral acceleration in each trajectory point [m/ss]                                      | 9.8           |
+| `thresholds.longitudinal_max_acc`            | double | invalid threshold of the maximum longitudinal acceleration in each trajectory point [m/ss]                         | 9.8           |
+| `thresholds.longitudinal_min_acc`            | double | invalid threshold of the minimum longitudinal deceleration in each trajectory point [m/ss]                         | -9.8          |
+| `thresholds.steering`                        | double | invalid threshold of the steering angle in each trajectory point [rad]                                             | 1.414         |
+| `thresholds.steering_rate`                   | double | invalid threshold of the steering angle rate in each trajectory point [rad/s]                                      | 10.0          |
+| `thresholds.velocity_deviation`              | double | invalid threshold of the velocity deviation between the ego velocity and the trajectory point closest to ego [m/s] | 100.0         |
+| `thresholds.distance_deviation`              | double | invalid threshold of the distance deviation between the ego position and the trajectory point closest to ego [m]   | 100.0         |
+| `parameters.longitudinal_distance_deviation` | double | invalid threshold of the longitudinal distance deviation between the ego position and the trajectory [m]           | 2.0           |
+
+#### Parameters
+
+For parameters used e.g. to calculate threshold.
+
+| `parameters.forward_trajectory_length_acceleration` | double | This value is used to calculate required trajectory length. | -5.0 |
+| `parameters.forward_trajectory_length_margin` | double | A margin of the required trajectory length not to raise an error when the ego slightly exceeds the trajectory end point. | 2.0 |

planning/planning_validator/config/planning_validator.param.yaml

@@ -26,3 +26,14 @@
       steering_rate: 10.0
       velocity_deviation: 100.0
       distance_deviation: 100.0
+      longitudinal_distance_deviation: 1.0
+
+    parameters:
+      # The required trajectory length is calculated as the distance needed
+      # to stop from the current speed at this deceleration.
+      forward_trajectory_length_acceleration: -3.0
+
+      # An error is raised if the required trajectory length is less than this distance.
+      # Setting it to 0 means an error will occur if even slightly exceeding the end of the path,
+      # therefore, a certain margin is necessary.
+      forward_trajectory_length_margin: 2.0

planning/planning_validator/include/planning_validator/planning_validator.hpp

@@ -45,6 +45,7 @@ using tier4_debug_msgs::msg::Float64Stamped;
 
 struct ValidationParams
 {
+  // thresholds
   double interval_threshold;
   double relative_angle_threshold;
   double curvature_threshold;
@@ -55,6 +56,11 @@ struct ValidationParams
   double steering_rate_threshold;
   double velocity_deviation_threshold;
   double distance_deviation_threshold;
+  double longitudinal_distance_deviation_threshold;
+
+  // parameters
+  double forward_trajectory_length_acceleration;
+  double forward_trajectory_length_margin;
 };
 
 class PlanningValidator : public rclcpp::Node
@@ -64,7 +70,7 @@ class PlanningValidator : public rclcpp::Node
 
   void onTrajectory(const Trajectory::ConstSharedPtr msg);
 
-  bool checkValidSize(const Trajectory & trajectory) const;
+  bool checkValidSize(const Trajectory & trajectory);
   bool checkValidFiniteValue(const Trajectory & trajectory);
   bool checkValidInterval(const Trajectory & trajectory);
   bool checkValidRelativeAngle(const Trajectory & trajectory);
@@ -76,6 +82,8 @@ class PlanningValidator : public rclcpp::Node
   bool checkValidSteeringRate(const Trajectory & trajectory);
   bool checkValidVelocityDeviation(const Trajectory & trajectory);
   bool checkValidDistanceDeviation(const Trajectory & trajectory);
+  bool checkValidLongitudinalDistanceDeviation(const Trajectory & trajectory);
+  bool checkValidForwardTrajectoryLength(const Trajectory & trajectory);
 
 private:
   void setupDiag();

planning/planning_validator/msg/PlanningValidatorStatus.msg

@@ -13,8 +13,11 @@ bool is_valid_steering
 bool is_valid_steering_rate
 bool is_valid_velocity_deviation
 bool is_valid_distance_deviation
+bool is_valid_longitudinal_distance_deviation
+bool is_valid_forward_trajectory_length
 
 # values
+int64 trajectory_size
 float64 max_interval_distance
 float64 max_relative_angle
 float64 max_curvature
@@ -25,5 +28,8 @@ float64 max_steering
 float64 max_steering_rate
 float64 velocity_deviation
 float64 distance_deviation
+float64 longitudinal_distance_deviation
+float64 forward_trajectory_length_required
+float64 forward_trajectory_length_measured
 
 int64 invalid_count