refactor(behavior_velocity): refactor codes #8

taikitanaka3 · 2022-03-28T05:17:26Z

Signed-off-by: tanaka3 ttatcoder@outlook.jp

Note: Confirm the contribution guidelines before submitting a pull request.

Click the Preview tab and select a PR template:

Do NOT send a PR with this description.

Signed-off-by: tanaka3 <ttatcoder@outlook.jp>

taikitanaka3 · 2022-03-28T07:47:06Z

planning/behavior_velocity_planner/config/occlusion_spot.param.yaml

@@ -1,7 +1,7 @@
 /**:
  ros__parameters:
    occlusion_spot:
-      detection_method: "predicted_object"  # [-] candidate is "occupancy_grid" or "predicted_object"
+      detection_method: "occupancy_grid"    # [-] candidate is "occupancy_grid" or "predicted_object"


default to occ grid

taikitanaka3 · 2022-03-28T07:47:59Z

planning/behavior_velocity_planner/src/scene_module/occlusion_spot/occlusion_spot_utils.cpp

@@ -275,7 +276,7 @@ PossibleCollisionInfo calculateCollisionPathPointFromOcclusionSpot(
  const ArcCoordinates & arc_coord_occlusion_with_offset,
  const lanelet::ConstLanelet & path_lanelet, const PlannerParam & param)
 {
-  auto setPose = [](const lanelet::ConstLanelet & pl, const ArcCoordinates & arc) {
+  auto calcPose = [](const lanelet::ConstLanelet & pl, const ArcCoordinates & arc) {


taikitanaka3 · 2022-03-28T07:48:10Z

planning/behavior_velocity_planner/src/scene_module/occlusion_spot/occlusion_spot_utils.cpp

@@ -395,28 +390,22 @@ bool createPossibleCollisionsInDetectionArea(
    const auto pc = generateOneNotableCollisionFromOcclusionSpot(
      grid, occlusion_spot_positions, offset_from_start_to_ego, base_point, path_lanelet, param,
      debug_data);
-    if (!pc) continue;
+    if (pc == boost::none) continue;


boost::none

taikitanaka3 · 2022-03-28T07:48:28Z

planning/behavior_velocity_planner/src/scene_module/occlusion_spot/occlusion_spot_utils.cpp

 {
  for (const auto & p : partitions) {
-    if (bg::intersects(direction, p)) return false;
+    if (bg::intersects(direction, p)) return true;


blocked case

taikitanaka3 · 2022-03-28T07:48:36Z

planning/behavior_velocity_planner/src/scene_module/occlusion_spot/occlusion_spot_utils.cpp

  }
-  return true;
+  return false;


no blocking

taikitanaka3 · 2022-03-28T07:49:01Z

planning/behavior_velocity_planner/src/scene_module/occlusion_spot/occlusion_spot_utils.cpp

@@ -453,22 +442,22 @@ boost::optional<PossibleCollisionInfo> generateOneNotableCollisionFromOcclusionS
    const auto & ip = pc.intersection_pose.position;
    bool collision_free_at_intersection = grid_utils::isCollisionFree(
      grid, occlusion_spot_position, grid_map::Position(ip.x, ip.y), param.pedestrian_radius);
-    bool obstacle_not_blocked_by_partition = true;
+    bool is_obstacle_blocked_by_partition = false;


default is not blocked

taikitanaka3 · 2022-03-28T07:49:31Z

planning/behavior_velocity_planner/src/scene_module/occlusion_spot/occlusion_spot_utils.cpp

    }
-    if (!obstacle_not_blocked_by_partition) continue;
+    if (is_obstacle_blocked_by_partition) continue;


if blocked continue

taikitanaka3 · 2022-03-28T07:50:15Z

planning/behavior_velocity_planner/src/scene_module/occlusion_spot/scene_occlusion_spot.cpp

+#define DEBUG_PRINT(enable, n, x)                                   \
+  if (enable) {                                                     \
+    const double time = x;                                          \
+    RCLCPP_INFO_STREAM_THROTTLE(logger_, *clock_, 3000, n << time); \


tick at every time
print at given interval

taikitanaka3 · 2022-03-28T07:50:40Z

planning/behavior_velocity_planner/src/scene_module/occlusion_spot/scene_occlusion_spot.cpp

  } else if (param_.detection_method == utils::DETECTION_METHOD::PREDICTED_OBJECT) {
    debug_data_.detection_type = "object";
  }
  if (param_.use_partition_lanelet) {
    const lanelet::LaneletMapConstPtr & ll = planner_data->route_handler_->getLaneletMapPtr();
-    planning_utils::getAllPartitionLanelets(ll, debug_data_.partition_lanelets);
+    planning_utils::getAllPartitionLanelets(ll, partition_lanelets_);


store at class variables

taikitanaka3 · 2022-03-28T07:51:17Z

planning/behavior_velocity_planner/src/scene_module/occlusion_spot/scene_occlusion_spot.cpp

  }
 }

 bool OcclusionSpotModule::modifyPathVelocity(
  autoware_auto_planning_msgs::msg::PathWithLaneId * path,
  [[maybe_unused]] tier4_planning_msgs::msg::StopReason * stop_reason)
 {
+  if (param_.is_show_processing_time) stop_watch_.tic("total_processing_time");


[rviz2-30] [INFO] [1648453219.536895053] [CarInitialPoseTool]: Setting twist: 0.000 0.000 0.000 [frame=map] [component_container_mt-13] [INFO] [1648453219.689546629] [planning.scenario_planning.lane_driving.behavior_planning.behavior_velocity_planner.occlusion_spot_module]: occlusion [ms]: 0.797 [component_container_mt-13] [INFO] [1648453219.689573733] [planning.scenario_planning.lane_driving.behavior_planning.behavior_velocity_planner.occlusion_spot_module]: num collision:1 [component_container_mt-13] [INFO] [1648453219.689611884] [planning.scenario_planning.lane_driving.behavior_planning.behavior_velocity_planner.occlusion_spot_module]: total [ms]: 1.111 [component_container_mt-13] [INFO] [1648453219.888806628] [planning.scenario_planning.lane_driving.behavior_planning.behavior_velocity_planner.occlusion_spot_module]: apply velocity [ms]: 0.002 [component_container_mt-13] [INFO] [1648453219.888841272] [planning.scenario_planning.lane_driving.behavior_planning.behavior_velocity_planner.occlusion_spot_module]: generate poly[ms]: 0.038 [component_container_mt-13] [INFO] [1648453219.888849376] [planning.scenario_planning.lane_driving.behavior_planning.behavior_velocity_planner.occlusion_spot_module]: extract[ms]: 0.009 [component_container_mt-13] [INFO] [1648453219.889075952] [planning.scenario_planning.lane_driving.behavior_planning.behavior_velocity_planner.occlusion_spot_module]: grid [ms]: 0.225 [ad_service_state_monitor-3] [INFO] [1648453219.954095513] [system.ad_service_state_monitor]: state changed: 3 -> 4 [component_container_mt-13] [INFO] [1648453222.690365723] [planning.scenario_planning.lane_driving.behavior_planning.behavior_velocity_planner.occlusion_spot_module]: occlusion [ms]: 1.564 [component_container_mt-13] [INFO] [1648453222.690387574] [planning.scenario_planning.lane_driving.behavior_planning.behavior_velocity_planner.occlusion_spot_module]: num collision:1 [component_container_mt-13] [INFO] [1648453222.690397659] [planning.scenario_planning.lane_driving.behavior_planning.behavior_velocity_planner.occlusion_spot_module]: total [ms]: 1.85 [component_container_mt-13] [INFO] [1648453222.988800341] [planning.scenario_planning.lane_driving.behavior_planning.behavior_velocity_planner.occlusion_spot_module]: apply velocity [ms]: 0.003 [component_container_mt-13] [INFO] [1648453222.988835603] [planning.scenario_planning.lane_driving.behavior_planning.behavior_velocity_planner.occlusion_spot_module]: generate poly[ms]: 0.038 [component_container_mt-13] [INFO] [1648453222.988844615] [planning.scenario_planning.lane_driving.behavior_planning.behavior_velocity_planner.occlusion_spot_module]: extract[ms]: 0.01 [component_container_mt-13] [INFO] [1648453222.989077456] [planning.scenario_planning.lane_driving.behavior_planning.behavior_velocity_planner.occlusion_spot_module]: grid [ms]: 0.231 [component_container_mt-13] [INFO] [1648453225.690586225] [planning.scenario_planning.lane_driving.behavior_planning.behavior_velocity_planner.occlusion_spot_module]: occlusion [ms]: 1.672 [component_container_mt-13] [INFO] [1648453225.690608788] [planning.scenario_planning.lane_driving.behavior_planning.behavior_velocity_planner.occlusion_spot_module]: num collision:1 [component_container_mt-13] [INFO] [1648453225.690618656] [planning.scenario_planning.lane_driving.behavior_planning.behavior_velocity_planner.occlusion_spot_module]: total [ms]: 1.977 [component_container_mt-13] [INFO] [1648453225.990610094] [planning.scenario_planning.lane_driving.behavior_planning.behavior_velocity_planner.occlusion_spot_module]: apply velocity [ms]: 0.002 [component_container_mt-13] [INFO] [1648453225.990650222] [planning.scenario_planning.lane_driving.behavior_planning.behavior_velocity_planner.occlusion_spot_module]: generate poly[ms]: 0.044 [component_container_mt-13] [INFO] [1648453225.990659870] [planning.scenario_planning.lane_driving.behavior_planning.behavior_velocity_planner.occlusion_spot_module]: extract[ms]: 0.01 [component_container_mt-13] [INFO] [1648453225.990939249] [planning.scenario_planning.lane_driving.behavior_planning.behavior_velocity_planner.occlusion_spot_module]: grid [ms]: 0.278

rich printing to compare performance

taikitanaka3 · 2022-03-28T07:52:06Z

planning/behavior_velocity_planner/src/scene_module/occlusion_spot/scene_occlusion_spot.cpp

  if (param_.detection_method == utils::DETECTION_METHOD::OCCUPANCY_GRID) {
    const auto & occ_grid_ptr = planner_data_->occupancy_grid;
-    if (!occ_grid_ptr) return true;  // mo data
+    if (!occ_grid_ptr) return true;  // no data


taikitanaka3 · 2022-03-28T07:57:00Z

planning/behavior_velocity_planner/src/scene_module/occlusion_spot/scene_occlusion_spot.cpp

@@ -45,20 +48,22 @@ OcclusionSpotModule::OcclusionSpotModule(
  if (param_.detection_method == utils::DETECTION_METHOD::OCCUPANCY_GRID) {
    debug_data_.detection_type = "occupancy";
    //! occupancy grid limitation( 100 * 100 )
-    param_.detection_area_length = std::min(50.0, param_.detection_area_length);
+    const double max_length = 35.0;  // current available length


set max usable length

github-actions

⚠️ clang-tidy found issue(s) with the introduced code (1/1)

github-actions · 2022-03-28T10:13:07Z

planning/behavior_velocity_planner/src/scene_module/occlusion_spot/occlusion_spot_utils.cpp

-    possible_collisions.begin(), possible_collisions.end(),
-    [](PossibleCollisionInfo pc1, PossibleCollisionInfo pc2) {
-      return pc1.arc_lane_dist_at_collision.length < pc2.arc_lane_dist_at_collision.length;
-    });
  if (possible_collisions.empty()) return false;


⚠️ readability-simplify-boolean-expr ⚠️
redundant boolean literal in conditional return statement

Suggested change

if (possible_collisions.empty()) return false;

return !possible_collisions.empty()

github-actions · 2022-03-28T10:13:07Z

planning/behavior_velocity_planner/src/scene_module/occlusion_spot/occlusion_spot_utils.cpp

-    possible_collisions.begin(), possible_collisions.end(),
-    [](PossibleCollisionInfo pc1, PossibleCollisionInfo pc2) {
-      return pc1.arc_lane_dist_at_collision.length < pc2.arc_lane_dist_at_collision.length;
-    });
  if (possible_collisions.empty()) return false;


⚠️ readability-simplify-boolean-expr ⚠️
redundant boolean literal in conditional return statement

Suggested change

if (possible_collisions.empty()) return false;

return !possible_collisions.empty()

github-actions · 2022-03-28T10:13:07Z

planning/behavior_velocity_planner/src/scene_module/occlusion_spot/occlusion_spot_utils.cpp

    }
-    if (!obstacle_not_blocked_by_partition) continue;
+    if (is_obstacle_blocked_by_partition) continue;
    distance_lower_bound = dist;
    candidate = pc;
    has_collision = true;
  }
  if (has_collision) {
    return candidate;
  } else {


⚠️ readability-else-after-return ⚠️
do not use else after return

Suggested change

} else {

} {

github-actions · 2022-03-28T10:13:07Z

planning/behavior_velocity_planner/src/scene_module/occlusion_spot/occlusion_spot_utils.cpp

    }
-    if (!obstacle_not_blocked_by_partition) continue;
+    if (is_obstacle_blocked_by_partition) continue;
    distance_lower_bound = dist;
    candidate = pc;
    has_collision = true;
  }
  if (has_collision) {
    return candidate;
  } else {


⚠️ readability-else-after-return ⚠️
do not use else after return

Suggested change

} else {

} else return boost::none;

github-actions · 2022-03-28T10:13:07Z

planning/behavior_velocity_planner/src/scene_module/occlusion_spot/scene_occlusion_spot.cpp

+#define DEBUG_PRINT(enable, n, x)                                   \
+  if (enable) {                                                     \
+    const double time = x;                                          \
+    RCLCPP_INFO_STREAM_THROTTLE(logger_, *clock_, 3000, n << time); \


⚠️ bugprone-macro-parentheses ⚠️
macro argument should be enclosed in parentheses

Suggested change

RCLCPP_INFO_STREAM_THROTTLE(logger_, *clock_, 3000, n << time); \

RCLCPP_INFO_STREAM_THROTTLE(logger_, *clock_, 3000, (n << time); \

github-actions · 2022-03-28T10:13:08Z

planning/behavior_velocity_planner/src/scene_module/occlusion_spot/scene_occlusion_spot.cpp

+#define DEBUG_PRINT(enable, n, x)                                   \
+  if (enable) {                                                     \
+    const double time = x;                                          \
+    RCLCPP_INFO_STREAM_THROTTLE(logger_, *clock_, 3000, n << time); \


⚠️ bugprone-macro-parentheses ⚠️
macro argument should be enclosed in parentheses

Suggested change

RCLCPP_INFO_STREAM_THROTTLE(logger_, *clock_, 3000, n << time); \

RCLCPP_INFO_STREAM_THROTTLE(logger_, *clock_, 3000, n) << time); \

Signed-off-by: tanaka3 <ttatcoder@outlook.jp>

github-actions

⚠️ clang-tidy found issue(s) with the introduced code (1/1)

github-actions · 2022-03-29T06:55:18Z

planning/behavior_velocity_planner/src/scene_module/occlusion_spot/scene_occlusion_spot.cpp

-  if (enable) RCLCPP_INFO_STREAM_THROTTLE(logger_, *clock_, 3000, x);
+#define DEBUG_PRINT(enable, n, x)                                  \
+  if (enable) {                                                    \
+    const std::string time_msg = n + std::to_string(x);            \


⚠️ bugprone-macro-parentheses ⚠️
macro argument should be enclosed in parentheses

Suggested change

const std::string time_msg = n + std::to_string(x); \

const std::string time_msg = (n + std::to_string(x); \

github-actions · 2022-03-29T06:55:19Z

planning/behavior_velocity_planner/src/scene_module/occlusion_spot/scene_occlusion_spot.cpp

-  if (enable) RCLCPP_INFO_STREAM_THROTTLE(logger_, *clock_, 3000, x);
+#define DEBUG_PRINT(enable, n, x)                                  \
+  if (enable) {                                                    \
+    const std::string time_msg = n + std::to_string(x);            \


⚠️ bugprone-macro-parentheses ⚠️
macro argument should be enclosed in parentheses

Suggested change

const std::string time_msg = n + std::to_string(x); \

const std::string time_msg = n) + std::to_string(x); \

…us (autowarefoundation#3511) * Merge feat/intersection-occlusion-latest (#8) Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp> * fixed virtual wall marker Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp> * use is_occluded flag Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp> * 3 rtc intersection appears Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp> * clearCooperateStatus before sendRTC not to duplicate default/occlusion_first_stop_uuid Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp> --------- Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp>

fix autowarefoundation#3511) (autowarefoundation#3512) * Merge feat/intersection-occlusion-latest (#8) Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp> * fixed virtual wall marker Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp> * use is_occluded flag Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp> * 3 rtc intersection appears Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp> * clearCooperateStatus before sendRTC not to duplicate default/occlusion_first_stop_uuid Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp> * clearCooperateStatus at the beginning of send RTC Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp> --------- Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp>

taikitanaka3 added 3 commits March 28, 2022 14:10

refactor(behavior_velocity): refactor codes

c4ee2eb

Signed-off-by: tanaka3 <ttatcoder@outlook.jp>

refactor(behavior_velocity): fix typo

fa71d7e

Signed-off-by: tanaka3 <ttatcoder@outlook.jp>

refactor(behavior_velocity): refactor debuggings

b985d0d

Signed-off-by: tanaka3 <ttatcoder@outlook.jp>

taikitanaka3 commented Mar 28, 2022

View reviewed changes

github-actions bot reviewed Mar 28, 2022

View reviewed changes

taikitanaka3 added 3 commits March 29, 2022 12:13

refactor(behavior_velocity): unite to generate

59e3180

Signed-off-by: tanaka3 <ttatcoder@outlook.jp>

refactor(behavior_velocity): apply clang tidy

0ecb73c

Signed-off-by: tanaka3 <ttatcoder@outlook.jp>

chore(behavior_velocity): retun to default settting

60a17c9

Signed-off-by: tanaka3 <ttatcoder@outlook.jp>

github-actions bot reviewed Mar 29, 2022

View reviewed changes

taikitanaka3 closed this Mar 30, 2022

taikitanaka3 deleted the refactor-behavior-velocity-occlusion-spot branch March 30, 2022 05:14

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

refactor(behavior_velocity): refactor codes #8

refactor(behavior_velocity): refactor codes #8

taikitanaka3 commented Mar 28, 2022

taikitanaka3 Mar 28, 2022

taikitanaka3 Mar 28, 2022

taikitanaka3 Mar 28, 2022

taikitanaka3 Mar 28, 2022

taikitanaka3 Mar 28, 2022

taikitanaka3 Mar 28, 2022 •

edited

Loading

taikitanaka3 Mar 28, 2022

taikitanaka3 Mar 28, 2022

taikitanaka3 Mar 28, 2022

taikitanaka3 Mar 28, 2022

taikitanaka3 Mar 28, 2022

taikitanaka3 Mar 28, 2022

taikitanaka3 Mar 28, 2022

github-actions bot left a comment

github-actions bot Mar 28, 2022

github-actions bot Mar 28, 2022

github-actions bot Mar 28, 2022

github-actions bot Mar 28, 2022

github-actions bot Mar 28, 2022

github-actions bot Mar 28, 2022

github-actions bot left a comment

github-actions bot Mar 29, 2022

github-actions bot Mar 29, 2022

	if (possible_collisions.empty()) return false;
	return !possible_collisions.empty()

	RCLCPP_INFO_STREAM_THROTTLE(logger_, *clock_, 3000, n << time); \
	RCLCPP_INFO_STREAM_THROTTLE(logger_, *clock_, 3000, (n << time); \

	const std::string time_msg = n + std::to_string(x); \
	const std::string time_msg = (n + std::to_string(x); \

	const std::string time_msg = n + std::to_string(x); \
	const std::string time_msg = n) + std::to_string(x); \

refactor(behavior_velocity): refactor codes #8

refactor(behavior_velocity): refactor codes #8

Conversation

taikitanaka3 commented Mar 28, 2022

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

taikitanaka3 Mar 28, 2022 • edited Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

github-actions bot left a comment

Choose a reason for hiding this comment

github-actions bot Mar 28, 2022

Choose a reason for hiding this comment

github-actions bot Mar 28, 2022

Choose a reason for hiding this comment

github-actions bot Mar 28, 2022

Choose a reason for hiding this comment

github-actions bot Mar 28, 2022

Choose a reason for hiding this comment

github-actions bot Mar 28, 2022

Choose a reason for hiding this comment

github-actions bot Mar 28, 2022

Choose a reason for hiding this comment

github-actions bot left a comment

Choose a reason for hiding this comment

github-actions bot Mar 29, 2022

Choose a reason for hiding this comment

github-actions bot Mar 29, 2022

Choose a reason for hiding this comment

taikitanaka3 Mar 28, 2022 •

edited

Loading