refactor(behavior_velocity_planner): use common utils functions (#3316)

Signed-off-by: yutaka <purewater0901@gmail.com>
autowarefoundation · Apr 7, 2023 · 1ca02e1 · 1ca02e1
1 parent 33e1a44
commit 1ca02e1
Show file tree

Hide file tree

Showing 3 changed files with 3 additions and 32 deletions.
diff --git a/planning/behavior_velocity_planner/include/utilization/util.hpp b/planning/behavior_velocity_planner/include/utilization/util.hpp
@@ -195,9 +195,6 @@ void appendStopReason(const StopFactor stop_factor, StopReason * stop_reason);
 
 std::vector<geometry_msgs::msg::Point> toRosPoints(const PredictedObjects & object);
 
-geometry_msgs::msg::Point toRosPoint(const pcl::PointXYZ & pcl_point);
-geometry_msgs::msg::Point toRosPoint(const Point2d & boost_point, const double z);
-
 LineString2d extendLine(
   const lanelet::ConstPoint3d & lanelet_point1, const lanelet::ConstPoint3d & lanelet_point2,
   const double & length);

diff --git a/planning/behavior_velocity_planner/src/scene_module/detection_area/scene.cpp b/planning/behavior_velocity_planner/src/scene_module/detection_area/scene.cpp
@@ -287,7 +287,7 @@ std::vector<geometry_msgs::msg::Point> DetectionAreaModule::getObstaclePoints()
                                   (circle.first.y() - p.y) * (circle.first.y() - p.y);
       if (squared_dist <= circle.second) {
         if (bg::within(Point2d{p.x, p.y}, poly.basicPolygon())) {
-          obstacle_points.push_back(planning_utils::toRosPoint(p));
+          obstacle_points.push_back(tier4_autoware_utils::createPoint(p.x, p.y, p.z));
           // get all obstacle point becomes high computation cost so skip if any point is found
           break;
         }

diff --git a/planning/behavior_velocity_planner/src/utilization/util.cpp b/planning/behavior_velocity_planner/src/utilization/util.cpp
@@ -514,24 +514,6 @@ std::vector<geometry_msgs::msg::Point> toRosPoints(const PredictedObjects & obje
   return points;
 }
 
-geometry_msgs::msg::Point toRosPoint(const pcl::PointXYZ & pcl_point)
-{
-  geometry_msgs::msg::Point point;
-  point.x = pcl_point.x;
-  point.y = pcl_point.y;
-  point.z = pcl_point.z;
-  return point;
-}
-
-geometry_msgs::msg::Point toRosPoint(const Point2d & boost_point, const double z)
-{
-  geometry_msgs::msg::Point point;
-  point.x = boost_point.x();
-  point.y = boost_point.y();
-  point.z = z;
-  return point;
-}
-
 LineString2d extendLine(
   const lanelet::ConstPoint3d & lanelet_point1, const lanelet::ConstPoint3d & lanelet_point2,
   const double & length)
@@ -666,32 +648,24 @@ boost::optional<geometry_msgs::msg::Pose> insertStopPoint(
   const geometry_msgs::msg::Point & stop_point, PathWithLaneId & output)
 {
   const size_t base_idx = motion_utils::findNearestSegmentIndex(output.points, stop_point);
-  const auto insert_idx = motion_utils::insertTargetPoint(base_idx, stop_point, output.points);
+  const auto insert_idx = motion_utils::insertStopPoint(base_idx, stop_point, output.points);
 
   if (!insert_idx) {
     return {};
   }
 
-  for (size_t i = insert_idx.get(); i < output.points.size(); ++i) {
-    output.points.at(i).point.longitudinal_velocity_mps = 0.0;
-  }
-
   return tier4_autoware_utils::getPose(output.points.at(insert_idx.get()));
 }
 
 boost::optional<geometry_msgs::msg::Pose> insertStopPoint(
   const geometry_msgs::msg::Point & stop_point, const size_t stop_seg_idx, PathWithLaneId & output)
 {
-  const auto insert_idx = motion_utils::insertTargetPoint(stop_seg_idx, stop_point, output.points);
+  const auto insert_idx = motion_utils::insertStopPoint(stop_seg_idx, stop_point, output.points);
 
   if (!insert_idx) {
     return {};
   }
 
-  for (size_t i = insert_idx.get(); i < output.points.size(); ++i) {
-    output.points.at(i).point.longitudinal_velocity_mps = 0.0;
-  }
-
   return tier4_autoware_utils::getPose(output.points.at(insert_idx.get()));
 }