update acc document

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>

planning/obstacle_stop_planner/README.md

@@ -207,18 +207,35 @@ The above method means that the smaller the lateral deviation of the pointcloud,
 
 ```plantuml
 @startuml
-title insertTargetVelocity
+title insertTargetVelocity()
 start
 
-:get obstacle pointcloud in detection area;
+:get target vehicle point (*1) in detection area ;
 
-if (estimate velocity of target pointcloud?) then (yes(success to estimate))
-else (no(fail to estimate))
+partition Estimate-Target-Velocity {
+
+if (Is there a DynamicObject on the target vehicle point?) then (yes)
+:use the DynamicObject velocity\nas a target vehicle point velocity (*2);
+else (no)
+
+  if (The target vehicle point is found in the previous step?) then (yes)
+  else (no (estimation failed))
   stop
-endif
+  endif
+
+  :estimate the target vehicle point velocity \nby the travel distance from the previous step;
+
+  if (The estimated velocity is valid?) then (yes)
+  else (no (estimation failed))
+  stop
+  endif
+
+  :use the estimated velocity\nas a target vehicle point velocity (*2);
 
+endif
+}
 
-if (the velocity of the pointcloud is fast enough?) then (yes)
+if (Is the target vehicle point velocity fast enough?) then (yes)
 else (no)
   stop
 endif
@@ -228,59 +245,33 @@ else (no)
   stop
 endif
 
-:calculate target velocity to insert;
+:calculate target velocity to be inserted in the trajectory;
 
 if (the target velocity is fast enough?) then (yes)
 else (no)
   stop
 endif
 
-:embed target velocity;
+:insert the target velocity;
 
 stop
 @enduml
 ```
 
-This module works only when the obstacle pointcloud is found in the detection area of the `Obstacle stop planner` module.
-At first, the velocity of the pointcloud is estimated.
-
-```plantuml
-@startuml
-title estimate pointcloud velocity
-start
-
-if (Is there a dynamic object with a bounding box that overlaps the pointcloud?) then (yes)
-:use the velocity of dynamic object;
-stop
-else (no)
-endif
-
-if (The target pointcloud is found in the previous step?) then (yes)
-else (no(failed to estimate the pointcloud velocity))
-stop
-endif
-
-:calculate the pointcloud velocity from the pointclouds of the previous and current step;
-
-if (The calculated velocity is valid?) then (yes)
-else (no(failed to estimate the pointcloud velocity))
-stop
-endif
-
-:use the velocity of pointclouds;
+(*1) The target vehicle point is calculated as a closest obstacle PointCloud from ego along the trajectory.
 
-stop
-@enduml
-```
+(*2) The sources of velocity estimation can be changed by the following ROS parameters.
 
-The velocity estimation of the pointcloud uses the velocity information of dynamic objects, or the distance to the point cloud found in the previous step. If the pointcloud is contained in the bounding box of a dynamic object geometrically, the velocity of the dynamic object is used as the pointcloud velocity. If not, and when a target pointcloud is found in the previous step, the pointcloud velocity is calculated using the target pointcloud found in the previous step and current step; the distance and time difference between two point clouds is used for calculating pointcloud velocity. For the stability of the velocity estimation, the median of the calculation result for several steps is used as pointcloud velocity.
+- `adaptive_cruise_control.use_object_to_estimate_vel`
+- `adaptive_cruise_control.use_pcl_to_estimate_vel`
 
-If the calculated velocity is within the threshold range, it is used as the pointcloud velocity.
+This module works only when the target point is found in the detection area of the `Obstacle stop planner` module.
 
-\*The sources of velocity estimation can be changed by the following ROS parameters.
+The first process of this module is to estimate the velocity of the target vehicle point. The velocity estimation uses the velocity information of dynamic objects or the travel distance of the target vehicle point from the previous step. The dynamic object information is primal, and the travel distance estimation is used as a backup in case of the perception failure.
+If the target vehicle point is contained in the bounding box of a dynamic object geometrically, the velocity of the dynamic object is used as the target point velocity.
+Otherwise, the target point velocity is calculated by the travel distance of the target point from the previous step; that is `(current_position - previous_position) / dt`. Note that this travel distance based estimation fails when the target point is detected in the first time (it mainly happens in the cut-in situation). To improve the stability of the estimation, the median of the calculation result for several steps is used.
 
-- `adaptive_cruise_control.use_object_to_estimate_vel`
-- `adaptive_cruise_control.use_pcl_to_estimate_vel`
+If the calculated velocity is within the threshold range, it is used as the target point velocity.
 
 Only when the estimation is succeeded and the estimated velocity exceeds the value of `obstacle_stop_velocity_thresh_*`, the distance to the pointcloud from self-position is calculated. For prevent chattering in the mode transition, `obstacle_velocity_thresh_to_start_acc` is used for the threshold to start adaptive cruise, and `obstacle_velocity_thresh_to_stop_acc` is used for the threshold to stop adaptive cruise. When the calculated distance value exceeds the emergency distance $d\_{emergency}$ calculated by emergency_stop parameters, target velocity to insert is calculated.