Merge pull request #401 from tier4/sync-upstream

chore: sync upstream

common/motion_utils/include/motion_utils/trajectory/trajectory.hpp

@@ -1107,17 +1107,27 @@ inline boost::optional<size_t> insertTargetPoint(
 {
   validateNonEmpty(points);
 
-  if (insert_point_length < 0.0 || src_segment_idx >= points.size() - 1) {
+  if (src_segment_idx >= points.size() - 1) {
     return boost::none;
   }
 
   // Get Nearest segment index
   boost::optional<size_t> segment_idx = boost::none;
-  for (size_t i = src_segment_idx + 1; i < points.size(); ++i) {
-    const double length = calcSignedArcLength(points, src_segment_idx, i);
-    if (insert_point_length <= length) {
-      segment_idx = i - 1;
-      break;
+  if (0.0 <= insert_point_length) {
+    for (size_t i = src_segment_idx + 1; i < points.size(); ++i) {
+      const double length = calcSignedArcLength(points, src_segment_idx, i);
+      if (insert_point_length <= length) {
+        segment_idx = i - 1;
+        break;
+      }
+    }
+  } else {
+    for (int i = src_segment_idx - 1; 0 <= i; --i) {
+      const double length = calcSignedArcLength(points, src_segment_idx, i);
+      if (length <= insert_point_length) {
+        segment_idx = i;
+        break;
+      }
     }
   }
 
@@ -1128,7 +1138,7 @@ inline boost::optional<size_t> insertTargetPoint(
   // Get Target Point
   const double segment_length = calcSignedArcLength(points, *segment_idx, *segment_idx + 1);
   const double target_length =
-    std::max(0.0, insert_point_length - calcSignedArcLength(points, src_segment_idx, *segment_idx));
+    insert_point_length - calcSignedArcLength(points, src_segment_idx, *segment_idx);
   const double ratio = std::clamp(target_length / segment_length, 0.0, 1.0);
   const auto p_target = tier4_autoware_utils::calcInterpolatedPoint(
     tier4_autoware_utils::getPoint(points.at(*segment_idx)),

common/motion_utils/test/src/trajectory/test_trajectory.cpp

@@ -2750,6 +2750,193 @@ TEST(trajectory, insertTargetPoint_Length_Without_Target_Point_Non_Zero_Start_Id
   }
 }
 
+TEST(trajectory, insertTargetPoint_Negative_Length_Without_Target_Point_Non_Zero_Start_Idx)
+{
+  using motion_utils::calcArcLength;
+  using motion_utils::findNearestSegmentIndex;
+  using motion_utils::insertTargetPoint;
+  using tier4_autoware_utils::calcDistance2d;
+  using tier4_autoware_utils::createPoint;
+  using tier4_autoware_utils::deg2rad;
+  using tier4_autoware_utils::getPose;
+
+  const auto traj = generateTestTrajectory<Trajectory>(10, 1.0);
+
+  // Insert
+  for (double x_start = -0.5; x_start < -5.0; x_start -= 1.0) {
+    auto traj_out = traj;
+
+    const size_t start_idx = 7;
+    const auto p_target = createPoint(7.0 + x_start, 0.0, 0.0);
+    const size_t base_idx = findNearestSegmentIndex(traj.points, p_target);
+    const auto insert_idx = insertTargetPoint(start_idx, x_start, traj_out.points);
+
+    EXPECT_NE(insert_idx, boost::none);
+    EXPECT_EQ(insert_idx.get(), base_idx + 1);
+    EXPECT_EQ(traj_out.points.size(), traj.points.size() + 1);
+
+    for (size_t i = 0; i < traj_out.points.size() - 1; ++i) {
+      EXPECT_TRUE(calcDistance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3);
+    }
+
+    {
+      const auto p_insert = getPose(traj_out.points.at(insert_idx.get()));
+      const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0));
+      EXPECT_EQ(p_insert.position.x, p_target.x);
+      EXPECT_EQ(p_insert.position.y, p_target.y);
+      EXPECT_EQ(p_insert.position.z, p_target.z);
+      EXPECT_NEAR(p_insert.orientation.x, ans_quat.x, epsilon);
+      EXPECT_NEAR(p_insert.orientation.y, ans_quat.y, epsilon);
+      EXPECT_NEAR(p_insert.orientation.z, ans_quat.z, epsilon);
+      EXPECT_NEAR(p_insert.orientation.w, ans_quat.w, epsilon);
+    }
+
+    {
+      const auto p_base = getPose(traj_out.points.at(base_idx));
+      const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0));
+      EXPECT_NEAR(p_base.orientation.x, ans_quat.x, epsilon);
+      EXPECT_NEAR(p_base.orientation.y, ans_quat.y, epsilon);
+      EXPECT_NEAR(p_base.orientation.z, ans_quat.z, epsilon);
+      EXPECT_NEAR(p_base.orientation.w, ans_quat.w, epsilon);
+    }
+  }
+
+  // Boundary Condition(Before End Point)
+  {
+    auto traj_out = traj;
+    const double x_start = -1.0 - 1e-2;
+
+    const size_t start_idx = 8;
+    const auto p_target = createPoint(7.0 - 1e-2, 0.0, 0.0);
+    const size_t base_idx = findNearestSegmentIndex(traj.points, p_target);
+    const auto insert_idx = insertTargetPoint(start_idx, x_start, traj_out.points);
+
+    EXPECT_NE(insert_idx, boost::none);
+    EXPECT_EQ(insert_idx.get(), base_idx + 1);
+    EXPECT_EQ(traj_out.points.size(), traj.points.size() + 1);
+
+    for (size_t i = 0; i < traj_out.points.size() - 1; ++i) {
+      EXPECT_TRUE(calcDistance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3);
+    }
+
+    {
+      const auto p_insert = getPose(traj_out.points.at(insert_idx.get()));
+      const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0));
+      EXPECT_EQ(p_insert.position.x, p_target.x);
+      EXPECT_EQ(p_insert.position.y, p_target.y);
+      EXPECT_EQ(p_insert.position.z, p_target.z);
+      EXPECT_NEAR(p_insert.orientation.x, ans_quat.x, epsilon);
+      EXPECT_NEAR(p_insert.orientation.y, ans_quat.y, epsilon);
+      EXPECT_NEAR(p_insert.orientation.z, ans_quat.z, epsilon);
+      EXPECT_NEAR(p_insert.orientation.w, ans_quat.w, epsilon);
+    }
+
+    {
+      const auto p_base = getPose(traj_out.points.at(base_idx));
+      const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0));
+      EXPECT_NEAR(p_base.orientation.x, ans_quat.x, epsilon);
+      EXPECT_NEAR(p_base.orientation.y, ans_quat.y, epsilon);
+      EXPECT_NEAR(p_base.orientation.z, ans_quat.z, epsilon);
+      EXPECT_NEAR(p_base.orientation.w, ans_quat.w, epsilon);
+    }
+  }
+
+  // Boundary Condition(Right on End Point)
+  {
+    auto traj_out = traj;
+    const double x_start = -1.0;
+
+    const size_t start_idx = 8;
+    const auto p_target = createPoint(7.0, 0.0, 0.0);
+    const size_t base_idx = findNearestSegmentIndex(traj.points, p_target);
+    const auto insert_idx = insertTargetPoint(start_idx, x_start, traj_out.points);
+
+    EXPECT_NE(insert_idx, boost::none);
+    EXPECT_EQ(insert_idx.get(), base_idx + 1);
+    EXPECT_EQ(traj_out.points.size(), traj.points.size());
+
+    for (size_t i = 0; i < traj_out.points.size() - 1; ++i) {
+      EXPECT_TRUE(calcDistance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3);
+    }
+
+    {
+      const auto p_insert = getPose(traj_out.points.at(insert_idx.get()));
+      const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0));
+      EXPECT_EQ(p_insert.position.x, p_target.x);
+      EXPECT_EQ(p_insert.position.y, p_target.y);
+      EXPECT_EQ(p_insert.position.z, p_target.z);
+      EXPECT_NEAR(p_insert.orientation.x, ans_quat.x, epsilon);
+      EXPECT_NEAR(p_insert.orientation.y, ans_quat.y, epsilon);
+      EXPECT_NEAR(p_insert.orientation.z, ans_quat.z, epsilon);
+      EXPECT_NEAR(p_insert.orientation.w, ans_quat.w, epsilon);
+    }
+
+    {
+      const auto p_base = getPose(traj_out.points.at(base_idx));
+      const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0));
+      EXPECT_NEAR(p_base.orientation.x, ans_quat.x, epsilon);
+      EXPECT_NEAR(p_base.orientation.y, ans_quat.y, epsilon);
+      EXPECT_NEAR(p_base.orientation.z, ans_quat.z, epsilon);
+      EXPECT_NEAR(p_base.orientation.w, ans_quat.w, epsilon);
+    }
+  }
+
+  // Boundary Condition(start point)
+  {
+    auto traj_out = traj;
+    const double x_start = -5.0;
+
+    const size_t start_idx = 5;
+    const auto p_target = createPoint(0.0, 0.0, 0.0);
+    const size_t base_idx = findNearestSegmentIndex(traj.points, p_target);
+    const auto insert_idx = insertTargetPoint(start_idx, x_start, traj_out.points);
+
+    EXPECT_NE(insert_idx, boost::none);
+    EXPECT_EQ(insert_idx.get(), base_idx);
+    EXPECT_EQ(traj_out.points.size(), traj.points.size());
+
+    for (size_t i = 0; i < traj_out.points.size() - 1; ++i) {
+      EXPECT_TRUE(calcDistance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3);
+    }
+
+    {
+      const auto p_insert = getPose(traj_out.points.at(insert_idx.get()));
+      const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0));
+      EXPECT_EQ(p_insert.position.x, p_target.x);
+      EXPECT_EQ(p_insert.position.y, p_target.y);
+      EXPECT_EQ(p_insert.position.z, p_target.z);
+      EXPECT_NEAR(p_insert.orientation.x, ans_quat.x, epsilon);
+      EXPECT_NEAR(p_insert.orientation.y, ans_quat.y, epsilon);
+      EXPECT_NEAR(p_insert.orientation.z, ans_quat.z, epsilon);
+      EXPECT_NEAR(p_insert.orientation.w, ans_quat.w, epsilon);
+    }
+
+    {
+      const auto p_base = getPose(traj_out.points.at(base_idx));
+      const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0));
+      EXPECT_NEAR(p_base.orientation.x, ans_quat.x, epsilon);
+      EXPECT_NEAR(p_base.orientation.y, ans_quat.y, epsilon);
+      EXPECT_NEAR(p_base.orientation.z, ans_quat.z, epsilon);
+      EXPECT_NEAR(p_base.orientation.w, ans_quat.w, epsilon);
+    }
+  }
+
+  // No Insert (Index Out of range)
+  {
+    auto traj_out = traj;
+    EXPECT_EQ(insertTargetPoint(0, -1.0, traj_out.points), boost::none);
+    EXPECT_EQ(insertTargetPoint(0, -1.0, traj_out.points), boost::none);
+  }
+
+  // No Insert (Length out of range)
+  {
+    auto traj_out = traj;
+    EXPECT_EQ(insertTargetPoint(9, -10.0, traj_out.points), boost::none);
+    EXPECT_EQ(insertTargetPoint(9, -9.0001, traj_out.points), boost::none);
+    EXPECT_EQ(insertTargetPoint(1, -1.0001, traj_out.points), boost::none);
+  }
+}
+
 TEST(trajectory, insertTargetPoint_Length_from_a_pose)
 {
   using motion_utils::calcArcLength;

planning/behavior_path_planner/CMakeLists.txt

@@ -109,6 +109,14 @@ if(BUILD_TESTING)
     behavior_path_planner_node
   )
 
+  ament_add_ros_isolated_gmock(test_${CMAKE_PROJECT_NAME}_lane_change_module
+    test/test_lane_change_utils.cpp
+  )
+
+  target_link_libraries(test_${CMAKE_PROJECT_NAME}_lane_change_module
+    behavior_path_planner_node
+  )
+
   ament_add_ros_isolated_gmock(test_${CMAKE_PROJECT_NAME}_turn_signal
     test/test_turn_signal.cpp
   )

planning/behavior_path_planner/config/dynamic_avoidance/dynamic_avoidance.param.yaml

@@ -15,6 +15,7 @@
         min_obstacle_vel: 1.0 # [m/s]
 
       drivable_area_generation:
-        lat_offset_from_obstacle: 0.8 # [m]
-        time_to_avoid: 5.0 # [s]
-        max_lat_offset_to_avoid: 1.0 # [m]
+        lat_offset_from_obstacle: 2.0 # [m]
+        time_to_avoid_same_directional_object: 5.0 # [s]
+        time_to_avoid_opposite_directional_object: 6.0 # [s]
+        max_lat_offset_to_avoid: 2.0 # [m]

planning/behavior_path_planner/docs/behavior_path_planner_dynamic_avoidance_design.md

@@ -11,12 +11,13 @@ Then the motion planner, in detail obstacle_avoidance_planner, will generate an
 
 ### Parameters
 
-| Name                                              | Unit  | Type   | Description                             | Default value |
-| :------------------------------------------------ | :---- | :----- | :-------------------------------------- | :------------ |
-| target_object.car                                 | [-]   | bool   | The flag whether to avoid cars or not   | true          |
-| target_object.truck                               | [-]   | bool   | The flag whether to avoid trucks or not | true          |
-| ...                                               | [-]   | bool   | ...                                     | ...           |
-| target_object.min_obstacle_vel                    | [m/s] | double | Minimum obstacle velocity to avoid      | 1.0           |
-| drivable_area_generation.lat_offset_from_obstacle | [m]   | double | Lateral offset to avoid from obstacles  | 0.8           |
-| drivable_area_generation.time_to_avoid            | [s]   | double | Elapsed time for avoiding an obstacle   | 5.0           |
-| drivable_area_generation.max_lat_offset_to_avoid  | [m]   | double | Maximum lateral offset to avoid         | 0.5           |
+| Name                                                               | Unit  | Type   | Description                                                 | Default value |
+| :----------------------------------------------------------------- | :---- | :----- | :---------------------------------------------------------- | :------------ |
+| target_object.car                                                  | [-]   | bool   | The flag whether to avoid cars or not                       | true          |
+| target_object.truck                                                | [-]   | bool   | The flag whether to avoid trucks or not                     | true          |
+| ...                                                                | [-]   | bool   | ...                                                         | ...           |
+| target_object.min_obstacle_vel                                     | [m/s] | double | Minimum obstacle velocity to avoid                          | 1.0           |
+| drivable_area_generation.lat_offset_from_obstacle                  | [m]   | double | Lateral offset to avoid from obstacles                      | 0.8           |
+| drivable_area_generation.time_to_avoid_same_directional_object     | [s]   | double | Elapsed time for avoiding the same directional obstacle     | 5.0           |
+| drivable_area_generation.time_to_avoid_opposite_directional_object | [s]   | double | Elapsed time for avoiding the opposite directional obstacle | 6.0           |
+| drivable_area_generation.max_lat_offset_to_avoid                   | [m]   | double | Maximum lateral offset to avoid                             | 0.5           |

planning/behavior_path_planner/include/behavior_path_planner/data_manager.hpp

@@ -81,8 +81,13 @@ struct DrivableLanes
 // quite messy. Needs to be refactored.
 struct DrivableAreaInfo
 {
+  struct Obstacle
+  {
+    geometry_msgs::msg::Pose pose;
+    tier4_autoware_utils::Polygon2d poly;
+  };
   std::vector<DrivableLanes> drivable_lanes;
-  std::vector<tier4_autoware_utils::Polygon2d> obstacle_polys;
+  std::vector<Obstacle> obstacles;  // obstacles to extract from the drivable area
 
   // temporary only for pull over's freespace planning
   double drivable_margin{0.0};

planning/behavior_path_planner/include/behavior_path_planner/scene_module/dynamic_avoidance/dynamic_avoidance_module.hpp

@@ -50,7 +50,8 @@ struct DynamicAvoidanceParameters
 
   // drivable area generation
   double lat_offset_from_obstacle{0.0};
-  double time_to_avoid{0.0};
+  double time_to_avoid_same_directional_object{0.0};
+  double time_to_avoid_opposite_directional_object{0.0};
   double max_lat_offset_to_avoid{0.0};
 };
 

planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/utils.hpp

@@ -65,7 +65,7 @@ void setStartData(
 Polygon2d createEnvelopePolygon(
   const ObjectData & object_data, const Pose & closest_pose, const double envelope_buffer);
 
-std::vector<tier4_autoware_utils::Polygon2d> generateObstaclePolygonsForDrivableArea(
+std::vector<DrivableAreaInfo::Obstacle> generateObstaclePolygonsForDrivableArea(
   const ObjectDataArray & objects, const std::shared_ptr<AvoidanceParameters> & parameters,
   const double vehicle_width);
 

planning/behavior_path_planner/include/behavior_path_planner/utils/utils.hpp

@@ -377,7 +377,7 @@ std::vector<DrivableLanes> combineDrivableLanes(
   const std::vector<DrivableLanes> & new_drivable_lanes_vec);
 
 void extractObstaclesFromDrivableArea(
-  PathWithLaneId & path, const std::vector<tier4_autoware_utils::Polygon2d> & obj_polys);
+  PathWithLaneId & path, const std::vector<DrivableAreaInfo::Obstacle> & obstacles);
 }  // namespace behavior_path_planner::utils
 
 #endif  // BEHAVIOR_PATH_PLANNER__UTILS__UTILS_HPP_

planning/behavior_path_planner/src/behavior_path_planner_node.cpp

@@ -676,7 +676,10 @@ DynamicAvoidanceParameters BehaviorPathPlannerNode::getDynamicAvoidanceParam()
   {  // drivable_area_generation
     std::string ns = "dynamic_avoidance.drivable_area_generation.";
     p.lat_offset_from_obstacle = declare_parameter<double>(ns + "lat_offset_from_obstacle");
-    p.time_to_avoid = declare_parameter<double>(ns + "time_to_avoid");
+    p.time_to_avoid_same_directional_object =
+      declare_parameter<double>(ns + "time_to_avoid_same_directional_object");
+    p.time_to_avoid_opposite_directional_object =
+      declare_parameter<double>(ns + "time_to_avoid_opposite_directional_object");
     p.max_lat_offset_to_avoid = declare_parameter<double>(ns + "max_lat_offset_to_avoid");
   }
 
@@ -1490,12 +1493,17 @@ SetParametersResult BehaviorPathPlannerNode::onSetParam(
     return result;
   }
 
+#ifndef USE_OLD_ARCHITECTURE
+  {
+    const std::lock_guard<std::mutex> lock(mutex_manager_);  // for planner_manager_
+    planner_manager_->updateModuleParams(parameters);
+  }
+#endif
+
   result.successful = true;
   result.reason = "success";
 
   try {
-    updateParam(
-      parameters, "avoidance.publish_debug_marker", avoidance_param_ptr_->publish_debug_marker);
     updateParam(
       parameters, "lane_change.publish_debug_marker", lane_change_param_ptr_->publish_debug_marker);
     // Drivable area expansion parameters

planning/behavior_path_planner/src/planner_manager.cpp

@@ -136,7 +136,7 @@ void PlannerManager::generateCombinedDrivableArea(
   }
 
   // extract obstacles from drivable area
-  utils::extractObstaclesFromDrivableArea(*output.path, output.drivable_area_info.obstacle_polys);
+  utils::extractObstaclesFromDrivableArea(*output.path, output.drivable_area_info.obstacles);
 }
 
 std::vector<SceneModulePtr> PlannerManager::getRequestModules(

planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp

@@ -2532,9 +2532,8 @@ void AvoidanceModule::generateExtendedDrivableArea(BehaviorModuleOutput & output
     output.drivable_area_info.drivable_lanes = utils::combineDrivableLanes(
       getPreviousModuleOutput().drivable_area_info.drivable_lanes, drivable_lanes);
     // generate obstacle polygons
-    output.drivable_area_info.obstacle_polys =
-      utils::avoidance::generateObstaclePolygonsForDrivableArea(
-        avoidance_data_.target_objects, parameters_, planner_data_->parameters.vehicle_width / 2.0);
+    output.drivable_area_info.obstacles = utils::avoidance::generateObstaclePolygonsForDrivableArea(
+      avoidance_data_.target_objects, parameters_, planner_data_->parameters.vehicle_width / 2.0);
   }
 
   {  // for old architecture