feat(behavior_velocity_planner): add velocity factors (autowarefounda…

…tion#1985) * (editting) add intersection_coordination to stop reason Signed-off-by: TakumiKozaka-T4 <[email protected]> * (editting) add intersection coordination to stop reasons Signed-off-by: TakumiKozaka-T4 <[email protected]> * (Editting) add v2x to stop reason Signed-off-by: TakumiKozaka-T4 <[email protected]> * (editting) add stop reason2 publisher Signed-off-by: TakumiKozaka-T4 <[email protected]> * (editting) add stop reason2 to scene modules Signed-off-by: TakumiKozaka-T4 <[email protected]> * add stop reason2 to obstacle stop planner and surround obstacle checker Signed-off-by: TakumiKozaka-T4 <[email protected]> * Modify files including unintended change by rebase Signed-off-by: TakumiKozaka-T4 <[email protected]> * ci(pre-commit): autofix * Modification 1: not to publsh vacant stop reason, 2: change default status in obstacle stop and surround obstacle checker Signed-off-by: TakumiKozaka-T4 <[email protected]> * fix error Signed-off-by: TakumiKozaka-T4 <[email protected]> * ci(pre-commit): autofix * modification for renaming stop_reason2 to motion_factor Signed-off-by: TakumiKozaka-T4 <[email protected]> * (Editting) rename variables Signed-off-by: TakumiKozaka-T4 <[email protected]> * bug fix Signed-off-by: TakumiKozaka-T4 <[email protected]> * (WIP) Add motion factor message. Modify scene modules due to new motion factor. Moving motion factor aggregator. Signed-off-by: TakumiKozaka-T4 <[email protected]> * (WIP) Save current work. Modify aggregator, CMakeList. Add launcher Signed-off-by: TakumiKozaka-T4 <[email protected]> * (WIP) Solved build error, but not launched Signed-off-by: TakumiKozaka-T4 <[email protected]> * (WIP) fixing error in launch Signed-off-by: TakumiKozaka-T4 <[email protected]> * (WIP) fixing error in launch Signed-off-by: TakumiKozaka-T4 <[email protected]> * (WIP) fixing launch error Signed-off-by: TakumiKozaka-T4 <[email protected]> * Fix error in launching motion factor aggregator Signed-off-by: TakumiKozaka-T4 <[email protected]> * Delete unnecessary comment-out in CMakelists. Change remapping in launcher. Signed-off-by: TakumiKozaka-T4 <[email protected]> * ci(pre-commit): autofix * pull the latest foundation/main Signed-off-by: TakumiKozaka-T4 <[email protected]> * (fix for pre-commit.ci) Add <memory> to motion_factor_aggregator.hpp Signed-off-by: TakumiKozaka-T4 <[email protected]> * ci(pre-commit): autofix * feat: add velocity factor interface Signed-off-by: Takagi, Isamu <[email protected]> * fix: fix build error Signed-off-by: Takagi, Isamu <[email protected]> * feat: stop sign Signed-off-by: Takagi, Isamu <[email protected]> * WIP Signed-off-by: Takagi, Isamu <[email protected]> * feat: update visualizer Signed-off-by: Takagi, Isamu <[email protected]> * feat: modify traffic light manager Signed-off-by: Takagi, Isamu <[email protected]> * feat: update velocity factors Signed-off-by: Takagi, Isamu <[email protected]> * feat: update api Signed-off-by: Takagi, Isamu <[email protected]> * feat: move adapi msgs Signed-off-by: Takagi, Isamu <[email protected]> * feat: remove old aggregator Signed-off-by: Takagi, Isamu <[email protected]> * feat: move api Signed-off-by: Takagi, Isamu <[email protected]> * feat: rename message Signed-off-by: Takagi, Isamu <[email protected]> * feat: add using Signed-off-by: Takagi, Isamu <[email protected]> * feat: add distance Signed-off-by: Takagi, Isamu <[email protected]> * feat: fix build error Signed-off-by: Takagi, Isamu <[email protected]> * feat: use nan as default distance Signed-off-by: Takagi, Isamu <[email protected]> * fix: set virtual traffic light detail Signed-off-by: Takagi, Isamu <[email protected]> * fix: remove debug code Signed-off-by: Takagi, Isamu <[email protected]> * fix: copyright Signed-off-by: Takagi, Isamu <[email protected]> Signed-off-by: TakumiKozaka-T4 <[email protected]> Signed-off-by: Takagi, Isamu <[email protected]> Co-authored-by: TakumiKozaka-T4 <[email protected]> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
asana17 · Dec 8, 2022 · 2dde073 · 2dde073
1 parent 9a5057e
commit 2dde073
Show file tree

Hide file tree

Showing 26 changed files with 253 additions and 8 deletions.
diff --git a/planning/behavior_velocity_planner/include/scene_module/scene_module_interface.hpp b/planning/behavior_velocity_planner/include/scene_module/scene_module_interface.hpp
@@ -16,12 +16,15 @@
 #define SCENE_MODULE__SCENE_MODULE_INTERFACE_HPP_
 
 #include "behavior_velocity_planner/planner_data.hpp"
+#include "velocity_factor_interface.hpp"
 
 #include <builtin_interfaces/msg/time.hpp>
 #include <rtc_interface/rtc_interface.hpp>
 #include <tier4_autoware_utils/tier4_autoware_utils.hpp>
 #include <utilization/util.hpp>
 
+#include <autoware_adapi_v1_msgs/msg/velocity_factor.hpp>
+#include <autoware_adapi_v1_msgs/msg/velocity_factor_array.hpp>
 #include <autoware_auto_planning_msgs/msg/path.hpp>
 #include <autoware_auto_planning_msgs/msg/path_with_lane_id.hpp>
 #include <tier4_debug_msgs/msg/float64_stamped.hpp>
@@ -100,6 +103,9 @@ class SceneModuleInterface
   bool isSafe() const { return safe_; }
   double getDistance() const { return distance_; }
 
+  void resetVelocityFactor() { velocity_factor_.reset(); }
+  VelocityFactor getVelocityFactor() const { return velocity_factor_.get(); }
+
 protected:
   const int64_t module_id_;
   bool activated_;
@@ -110,6 +116,7 @@ class SceneModuleInterface
   std::shared_ptr<const PlannerData> planner_data_;
   boost::optional<tier4_v2x_msgs::msg::InfrastructureCommand> infrastructure_command_;
   boost::optional<int> first_stop_path_point_index_;
+  VelocityFactorInterface velocity_factor_;
 
   void setSafe(const bool safe) { safe_ = safe; }
   void setDistance(const double distance) { distance_ = distance; }
@@ -150,6 +157,8 @@ class SceneModuleManagerInterface
     }
     pub_virtual_wall_ = node.create_publisher<visualization_msgs::msg::MarkerArray>(
       std::string("~/virtual_wall/") + module_name, 5);
+    pub_velocity_factor_ = node.create_publisher<autoware_adapi_v1_msgs::msg::VelocityFactorArray>(
+      std::string("/planning/velocity_factors/") + module_name, 1);
     pub_stop_reason_ =
       node.create_publisher<tier4_planning_msgs::msg::StopReasonArray>("~/output/stop_reasons", 1);
     pub_infrastructure_commands_ =
@@ -188,18 +197,27 @@ class SceneModuleManagerInterface
     visualization_msgs::msg::MarkerArray debug_marker_array;
     visualization_msgs::msg::MarkerArray virtual_wall_marker_array;
     tier4_planning_msgs::msg::StopReasonArray stop_reason_array;
+    autoware_adapi_v1_msgs::msg::VelocityFactorArray velocity_factor_array;
     stop_reason_array.header.frame_id = "map";
     stop_reason_array.header.stamp = clock_->now();
+    velocity_factor_array.header.frame_id = "map";
+    velocity_factor_array.header.stamp = clock_->now();
 
     tier4_v2x_msgs::msg::InfrastructureCommandArray infrastructure_command_array;
     infrastructure_command_array.stamp = clock_->now();
 
     first_stop_path_point_index_ = static_cast<int>(path->points.size()) - 1;
     for (const auto & scene_module : scene_modules_) {
       tier4_planning_msgs::msg::StopReason stop_reason;
+      scene_module->resetVelocityFactor();
       scene_module->setPlannerData(planner_data_);
       scene_module->modifyPathVelocity(path, &stop_reason);
 
+      // The velocity factor must be called after modifyPathVelocity.
+      const auto velocity_factor = scene_module->getVelocityFactor();
+      if (velocity_factor.type != VelocityFactor::UNKNOWN) {
+        velocity_factor_array.factors.emplace_back(velocity_factor);
+      }
       if (stop_reason.reason != "") {
         stop_reason_array.stop_reasons.emplace_back(stop_reason);
       }
@@ -224,6 +242,7 @@ class SceneModuleManagerInterface
     if (!stop_reason_array.stop_reasons.empty()) {
       pub_stop_reason_->publish(stop_reason_array);
     }
+    pub_velocity_factor_->publish(velocity_factor_array);
     pub_infrastructure_commands_->publish(infrastructure_command_array);
     pub_debug_->publish(debug_marker_array);
     if (is_publish_debug_path_) {
@@ -311,6 +330,8 @@ class SceneModuleManagerInterface
   rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr pub_debug_;
   rclcpp::Publisher<autoware_auto_planning_msgs::msg::PathWithLaneId>::SharedPtr pub_debug_path_;
   rclcpp::Publisher<tier4_planning_msgs::msg::StopReasonArray>::SharedPtr pub_stop_reason_;
+  rclcpp::Publisher<autoware_adapi_v1_msgs::msg::VelocityFactorArray>::SharedPtr
+    pub_velocity_factor_;
   rclcpp::Publisher<tier4_v2x_msgs::msg::InfrastructureCommandArray>::SharedPtr
     pub_infrastructure_commands_;
 

diff --git a/planning/behavior_velocity_planner/include/scene_module/velocity_factor_interface.hpp b/planning/behavior_velocity_planner/include/scene_module/velocity_factor_interface.hpp
@@ -0,0 +1,68 @@
+
+// Copyright 2022 TIER IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SCENE_MODULE__VELOCITY_FACTOR_INTERFACE_HPP_
+#define SCENE_MODULE__VELOCITY_FACTOR_INTERFACE_HPP_
+
+#include <rclcpp/rclcpp.hpp>
+
+#include <autoware_adapi_v1_msgs/msg/velocity_factor.hpp>
+#include <autoware_adapi_v1_msgs/msg/velocity_factor_array.hpp>
+#include <geometry_msgs/msg/pose.hpp>
+
+#include <mutex>
+#include <string>
+#include <vector>
+
+namespace behavior_velocity_planner
+{
+
+using autoware_adapi_v1_msgs::msg::VelocityFactor;
+using autoware_adapi_v1_msgs::msg::VelocityFactorArray;
+using geometry_msgs::msg::Pose;
+using VelocityFactorType = VelocityFactor::_type_type;
+using VelocityFactorStatus = VelocityFactor::_status_type;
+
+class VelocityFactorInterface
+{
+public:
+  VelocityFactorInterface() { type_ = VelocityFactor::UNKNOWN; }
+
+  VelocityFactor get() const { return velocity_factor_; }
+  void init(const VelocityFactorType type) { type_ = type; }
+  void reset() { velocity_factor_.type = VelocityFactor::UNKNOWN; }
+
+  template <class T>
+  void set(
+    const T & points, const Pose & curr_pose, const Pose & stop_pose,
+    const VelocityFactorStatus status, const std::string detail = "")
+  {
+    const auto & curr_point = curr_pose.position;
+    const auto & stop_point = stop_pose.position;
+    velocity_factor_.type = type_;
+    velocity_factor_.pose = stop_pose;
+    velocity_factor_.distance = motion_utils::calcSignedArcLength(points, curr_point, stop_point);
+    velocity_factor_.status = status;
+    velocity_factor_.detail = detail;
+  }
+
+private:
+  VelocityFactorType type_;
+  VelocityFactor velocity_factor_;
+};
+
+}  // namespace behavior_velocity_planner
+
+#endif  // SCENE_MODULE__VELOCITY_FACTOR_INTERFACE_HPP_
diff --git a/planning/behavior_velocity_planner/include/scene_module/virtual_traffic_light/scene.hpp b/planning/behavior_velocity_planner/include/scene_module/virtual_traffic_light/scene.hpp
@@ -99,6 +99,10 @@ class VirtualTrafficLightModule : public SceneModuleInterface
 
   void setStopReason(const Pose & stop_pose, StopReason * stop_reason);
 
+  void setVelocityFactor(
+    const geometry_msgs::msg::Pose & stop_pose,
+    autoware_adapi_v1_msgs::msg::VelocityFactor * velocity_factor);
+
   boost::optional<size_t> getPathIndexOfFirstEndLine();
 
   bool isBeforeStartLine(const size_t end_line_idx);

diff --git a/planning/behavior_velocity_planner/package.xml b/planning/behavior_velocity_planner/package.xml
@@ -47,6 +47,7 @@
 
   <build_depend>autoware_cmake</build_depend>
 
+  <depend>autoware_adapi_v1_msgs</depend>
   <depend>autoware_auto_mapping_msgs</depend>
   <depend>autoware_auto_perception_msgs</depend>
   <depend>autoware_auto_planning_msgs</depend>

diff --git a/planning/behavior_velocity_planner/src/scene_module/blind_spot/scene.cpp b/planning/behavior_velocity_planner/src/scene_module/blind_spot/scene.cpp
@@ -63,6 +63,7 @@ BlindSpotModule::BlindSpotModule(
   turn_direction_(TurnDirection::INVALID),
   is_over_pass_judge_line_(false)
 {
+  velocity_factor_.init(VelocityFactor::REAR_CHECK);
   planner_param_ = planner_param;
 
   const auto & assigned_lanelet =
@@ -183,6 +184,8 @@ bool BlindSpotModule::modifyPathVelocity(PathWithLaneId * path, StopReason * sto
     stop_factor.stop_pose = debug_data_.stop_point_pose;
     stop_factor.stop_factor_points = planning_utils::toRosPoints(debug_data_.conflicting_targets);
     planning_utils::appendStopReason(stop_factor, stop_reason);
+    velocity_factor_.set(
+      path->points, planner_data_->current_pose.pose, stop_pose, VelocityFactor::UNKNOWN);
   } else {
     *path = input_path;  // reset path
   }

diff --git a/planning/behavior_velocity_planner/src/scene_module/crosswalk/scene_crosswalk.cpp b/planning/behavior_velocity_planner/src/scene_module/crosswalk/scene_crosswalk.cpp
@@ -137,6 +137,7 @@ CrosswalkModule::CrosswalkModule(
   crosswalk_(std::move(crosswalk)),
   planner_param_(planner_param)
 {
+  velocity_factor_.init(VelocityFactor::CROSSWALK);
   passed_safety_slow_point_ = false;
 }
 
@@ -218,9 +219,15 @@ bool CrosswalkModule::modifyPathVelocity(PathWithLaneId * path, StopReason * sto
   if (nearest_stop_point) {
     insertDecelPointWithDebugInfo(nearest_stop_point.get(), 0.0, *path);
     planning_utils::appendStopReason(stop_factor, stop_reason);
+    velocity_factor_.set(
+      path->points, planner_data_->current_pose.pose, stop_factor.stop_pose,
+      VelocityFactor::UNKNOWN);
   } else if (rtc_stop_point) {
     insertDecelPointWithDebugInfo(rtc_stop_point.get(), 0.0, *path);
     planning_utils::appendStopReason(stop_factor_rtc, stop_reason);
+    velocity_factor_.set(
+      path->points, planner_data_->current_pose.pose, stop_factor_rtc.stop_pose,
+      VelocityFactor::UNKNOWN);
   }
 
   RCLCPP_INFO_EXPRESSION(

diff --git a/planning/behavior_velocity_planner/src/scene_module/crosswalk/scene_walkway.cpp b/planning/behavior_velocity_planner/src/scene_module/crosswalk/scene_walkway.cpp
@@ -39,6 +39,7 @@ WalkwayModule::WalkwayModule(
   state_(State::APPROACH),
   planner_param_(planner_param)
 {
+  velocity_factor_.init(VelocityFactor::SIDEWALK);
 }
 
 boost::optional<std::pair<double, geometry_msgs::msg::Point>> WalkwayModule::getStopLine(
@@ -121,6 +122,8 @@ bool WalkwayModule::modifyPathVelocity(PathWithLaneId * path, StopReason * stop_
     stop_factor.stop_pose = stop_pose.get();
     stop_factor.stop_factor_points.push_back(path_intersects_.front());
     planning_utils::appendStopReason(stop_factor, stop_reason);
+    velocity_factor_.set(
+      path->points, planner_data_->current_pose.pose, stop_pose.get(), VelocityFactor::UNKNOWN);
 
     // use arc length to identify if ego vehicle is in front of walkway stop or not.
     const double signed_arc_dist_to_stop_point =

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
@@ -46,6 +46,7 @@ DetectionAreaModule::DetectionAreaModule(
   state_(State::GO),
   planner_param_(planner_param)
 {
+  velocity_factor_.init(VelocityFactor::USER_DEFINED_DETECTION_AREA);
 }
 
 LineString2d DetectionAreaModule::getStopLineGeometry2d() const
@@ -182,6 +183,8 @@ bool DetectionAreaModule::modifyPathVelocity(PathWithLaneId * path, StopReason *
     stop_factor.stop_pose = stop_point->second;
     stop_factor.stop_factor_points = obstacle_points;
     planning_utils::appendStopReason(stop_factor, stop_reason);
+    velocity_factor_.set(
+      path->points, planner_data_->current_pose.pose, stop_point->second, VelocityFactor::UNKNOWN);
   }
 
   // Create legacy StopReason

diff --git a/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp b/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp
@@ -57,6 +57,7 @@ IntersectionModule::IntersectionModule(
   const rclcpp::Clock::SharedPtr clock)
 : SceneModuleInterface(module_id, logger, clock), lane_id_(lane_id), is_go_out_(false)
 {
+  velocity_factor_.init(VelocityFactor::INTERSECTION);
   planner_param_ = planner_param;
 
   const auto & assigned_lanelet =
@@ -256,14 +257,21 @@ bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason *
     debug_data_.stop_point_pose = path->points.at(stop_line_idx_final).point.pose;
     debug_data_.judge_point_pose = path->points.at(pass_judge_line_idx_final).point.pose;
 
-    /* get stop point and stop factor */
-    tier4_planning_msgs::msg::StopFactor stop_factor;
-    stop_factor.stop_pose = debug_data_.stop_point_pose;
-    const auto stop_factor_conflict = planning_utils::toRosPoints(debug_data_.conflicting_targets);
-    const auto stop_factor_stuck = planning_utils::toRosPoints(debug_data_.stuck_targets);
-    stop_factor.stop_factor_points =
-      planning_utils::concatVector(stop_factor_conflict, stop_factor_stuck);
-    planning_utils::appendStopReason(stop_factor, stop_reason);
+    // Get stop point and stop factor
+    {
+      tier4_planning_msgs::msg::StopFactor stop_factor;
+      stop_factor.stop_pose = debug_data_.stop_point_pose;
+      const auto stop_factor_conflict =
+        planning_utils::toRosPoints(debug_data_.conflicting_targets);
+      const auto stop_factor_stuck = planning_utils::toRosPoints(debug_data_.stuck_targets);
+      stop_factor.stop_factor_points =
+        planning_utils::concatVector(stop_factor_conflict, stop_factor_stuck);
+      planning_utils::appendStopReason(stop_factor, stop_reason);
+
+      const auto & stop_pose = path->points.at(stop_line_idx_final).point.pose;
+      velocity_factor_.set(
+        path->points, planner_data_->current_pose.pose, stop_pose, VelocityFactor::UNKNOWN);
+    }
 
     RCLCPP_DEBUG(logger_, "not activated. stop at the line.");
     RCLCPP_DEBUG(logger_, "===== plan end =====");

diff --git a/...behavior_velocity_planner/src/scene_module/intersection/scene_merge_from_private_road.cpp b/...behavior_velocity_planner/src/scene_module/intersection/scene_merge_from_private_road.cpp
@@ -39,6 +39,7 @@ MergeFromPrivateRoadModule::MergeFromPrivateRoadModule(
   const rclcpp::Clock::SharedPtr clock)
 : SceneModuleInterface(module_id, logger, clock), lane_id_(lane_id)
 {
+  velocity_factor_.init(VelocityFactor::MERGE);
   planner_param_ = planner_param;
   state_machine_.setState(StateMachine::State::STOP);
 }
@@ -111,6 +112,9 @@ bool MergeFromPrivateRoadModule::modifyPathVelocity(PathWithLaneId * path, StopR
     stop_factor.stop_pose = debug_data_.stop_point_pose;
     stop_factor.stop_factor_points.emplace_back(debug_data_.first_collision_point);
     planning_utils::appendStopReason(stop_factor, stop_reason);
+    const auto & stop_pose = path->points.at(stop_line_idx).point.pose;
+    velocity_factor_.set(
+      path->points, planner_data_->current_pose.pose, stop_pose, VelocityFactor::UNKNOWN);
 
     const double signed_arc_dist_to_stop_point = motion_utils::calcSignedArcLength(
       path->points, current_pose.pose.position, path->points.at(stop_line_idx).point.pose.position);

diff --git a/...ng/behavior_velocity_planner/src/scene_module/no_stopping_area/scene_no_stopping_area.cpp b/...ng/behavior_velocity_planner/src/scene_module/no_stopping_area/scene_no_stopping_area.cpp
@@ -49,6 +49,7 @@ NoStoppingAreaModule::NoStoppingAreaModule(
   no_stopping_area_reg_elem_(no_stopping_area_reg_elem),
   planner_param_(planner_param)
 {
+  velocity_factor_.init(VelocityFactor::NO_STOPPING_AREA);
   state_machine_.setState(StateMachine::State::GO);
   state_machine_.setMarginTime(planner_param_.state_clear_time);
 }
@@ -193,6 +194,9 @@ bool NoStoppingAreaModule::modifyPathVelocity(PathWithLaneId * path, StopReason
       stop_factor.stop_pose = stop_point->second;
       stop_factor.stop_factor_points = debug_data_.stuck_points;
       planning_utils::appendStopReason(stop_factor, stop_reason);
+      velocity_factor_.set(
+        path->points, planner_data_->current_pose.pose, stop_point->second,
+        VelocityFactor::UNKNOWN);
     }
 
     // Create legacy StopReason

diff --git a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/scene_occlusion_spot.cpp b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/scene_occlusion_spot.cpp
@@ -63,6 +63,8 @@ OcclusionSpotModule::OcclusionSpotModule(
   const rclcpp::Clock::SharedPtr clock)
 : SceneModuleInterface(module_id, logger, clock), param_(planner_param)
 {
+  velocity_factor_.init(VelocityFactor::UNKNOWN);
+
   if (param_.detection_method == utils::DETECTION_METHOD::OCCUPANCY_GRID) {
     debug_data_.detection_type = "occupancy";
     //! occupancy grid limitation( 100 * 100 )

diff --git a/planning/behavior_velocity_planner/src/scene_module/run_out/scene.cpp b/planning/behavior_velocity_planner/src/scene_module/run_out/scene.cpp
@@ -33,6 +33,8 @@ RunOutModule::RunOutModule(
   debug_ptr_(debug_ptr),
   state_machine_(std::make_unique<run_out_utils::StateMachine>(planner_param.state_param))
 {
+  velocity_factor_.init(VelocityFactor::UNKNOWN);
+
   if (planner_param.run_out.use_partition_lanelet) {
     const lanelet::LaneletMapConstPtr & ll = planner_data->route_handler_->getLaneletMapPtr();
     planning_utils::getAllPartitionLanelets(ll, partition_lanelets_);

diff --git a/planning/behavior_velocity_planner/src/scene_module/stop_line/scene.cpp b/planning/behavior_velocity_planner/src/scene_module/stop_line/scene.cpp
@@ -33,6 +33,7 @@ StopLineModule::StopLineModule(
   stop_line_(stop_line),
   state_(State::APPROACH)
 {
+  velocity_factor_.init(VelocityFactor::STOP_SIGN);
   planner_param_ = planner_param;
 }
 
@@ -89,6 +90,8 @@ bool StopLineModule::modifyPathVelocity(PathWithLaneId * path, StopReason * stop
         stop_factor.stop_pose = stop_pose;
         stop_factor.stop_factor_points.push_back(getCenterOfStopLine(stop_line_));
         planning_utils::appendStopReason(stop_factor, stop_reason);
+        velocity_factor_.set(
+          path->points, planner_data_->current_pose.pose, stop_pose, VelocityFactor::APPROACHING);
       }
 
       // Move to stopped state if stopped
@@ -133,6 +136,8 @@ bool StopLineModule::modifyPathVelocity(PathWithLaneId * path, StopReason * stop
         stop_factor.stop_pose = ego_pos_on_path.pose;
         stop_factor.stop_factor_points.push_back(getCenterOfStopLine(stop_line_));
         planning_utils::appendStopReason(stop_factor, stop_reason);
+        velocity_factor_.set(
+          path->points, planner_data_->current_pose.pose, stop_pose, VelocityFactor::STOPPED);
       }
 
       const auto elapsed_time = (clock_->now() - *stopped_time_).seconds();