Merge pull request #783 from tier4/sync-upstream

chore: sync upstream

.github/CODEOWNERS

@@ -14,6 +14,7 @@ common/component_interface_tools/** [email protected] [email protected] m
 common/component_interface_utils/** [email protected] [email protected] [email protected] [email protected]
 common/cuda_utils/** [email protected] [email protected]
 common/fake_test_node/** [email protected] [email protected] [email protected] [email protected]
+common/geography_utils/** [email protected]
 common/global_parameter_loader/** [email protected]
 common/glog_component/** [email protected]
 common/goal_distance_calculator/** [email protected]
@@ -117,7 +118,7 @@ perception/lidar_apollo_segmentation_tvm/** [email protected] xinyu.wang@
 perception/lidar_apollo_segmentation_tvm_nodes/** [email protected] [email protected]
 perception/lidar_centerpoint/** [email protected] [email protected]
 perception/lidar_centerpoint_tvm/** [email protected] [email protected]
-perception/map_based_prediction/** [email protected] [email protected] [email protected] [email protected]
+perception/map_based_prediction/** [email protected] shunsuke.miura@tier4.jp takayuki.murooka@tier4.jp [email protected] [email protected] [email protected]
 perception/multi_object_tracker/** [email protected] [email protected]
 perception/object_merger/** [email protected] [email protected]
 perception/object_range_splitter/** [email protected]
@@ -166,7 +167,7 @@ planning/freespace_planning_algorithms/** [email protected] takamasa.hori
 planning/mission_planner/** [email protected] [email protected] [email protected] [email protected] [email protected] [email protected]
 planning/motion_velocity_smoother/** [email protected] [email protected] [email protected] [email protected]
 planning/obstacle_avoidance_planner/** [email protected] [email protected] [email protected]
-planning/obstacle_cruise_planner/** [email protected] [email protected]
+planning/obstacle_cruise_planner/** [email protected] [email protected] [email protected] [email protected]
 planning/obstacle_stop_planner/** [email protected] [email protected] [email protected] [email protected] [email protected]
 planning/obstacle_velocity_limiter/** [email protected]
 planning/path_smoother/** [email protected] [email protected]

common/path_distance_calculator/src/path_distance_calculator.cpp

@@ -14,7 +14,7 @@
 
 #include "path_distance_calculator.hpp"
 
-#include <motion_utils/motion_utils.hpp>
+#include <motion_utils/trajectory/trajectory.hpp>
 #include <tier4_autoware_utils/tier4_autoware_utils.hpp>
 
 #include <algorithm>

control/autonomous_emergency_braking/include/autonomous_emergency_braking/node.hpp

@@ -28,6 +28,7 @@
 #include <sensor_msgs/msg/imu.hpp>
 #include <sensor_msgs/msg/point_cloud2.hpp>
 #include <visualization_msgs/msg/marker.hpp>
+#include <visualization_msgs/msg/marker_array.hpp>
 
 #include <boost/optional.hpp>
 

control/control_validator/src/control_validator.cpp

@@ -204,7 +204,9 @@ void ControlValidator::displayStatus()
 
   const auto & s = validation_status_;
 
-  warn(s.is_valid_max_distance_deviation, "planning trajectory is too far from ego!!");
+  warn(
+    s.is_valid_max_distance_deviation,
+    "predicted trajectory is too far from planning trajectory!!");
 }
 
 }  // namespace control_validator

control/trajectory_follower_node/include/trajectory_follower_node/controller_node.hpp

@@ -36,6 +36,7 @@
 #include "geometry_msgs/msg/pose_stamped.hpp"
 #include "nav_msgs/msg/odometry.hpp"
 #include "tf2_msgs/msg/tf_message.hpp"
+#include "visualization_msgs/msg/marker_array.hpp"
 
 #include <memory>
 #include <string>

control/trajectory_follower_node/package.xml

@@ -32,6 +32,7 @@
   <depend>rclcpp_components</depend>
   <depend>trajectory_follower_base</depend>
   <depend>vehicle_info_util</depend>
+  <depend>visualization_msgs</depend>
 
   <exec_depend>ros2launch</exec_depend>
 

control/trajectory_follower_node/src/controller_node.cpp

@@ -17,6 +17,7 @@
 #include "mpc_lateral_controller/mpc_lateral_controller.hpp"
 #include "pid_longitudinal_controller/pid_longitudinal_controller.hpp"
 #include "pure_pursuit/pure_pursuit_lateral_controller.hpp"
+#include "tier4_autoware_utils/ros/marker_helper.hpp"
 
 #include <algorithm>
 #include <limits>

mkdocs.yaml

@@ -55,7 +55,8 @@ plugins:
       regex:
         - ^(?!(.*/)?assets/).*\.(?!(.*\.)?md|(.*\.)?svg|(.*\.)?png|(.*\.)?gif|(.*\.)?jpg).*$
         - ^(.*/)?[^.]*$
-  - macros
+  - macros:
+      module_name: mkdocs_macros
   - mkdocs-video
   - same-dir
   - search

mkdocs_macros.py

@@ -0,0 +1,70 @@
+import json
+
+from tabulate import tabulate
+
+# This file is for defining macros for mkdocs-macros plugin
+# Check https://mkdocs-macros-plugin.readthedocs.io/en/latest/macros/ for the details
+
+
+def format_param_type(param_type):
+    if param_type == "number":
+        return "float"
+    else:
+        return param_type
+
+
+def format_param_range(param):
+    list_of_range = []
+    if "enum" in param.keys():
+        list_of_range.append(param["enum"])
+    if "minimum" in param.keys():
+        list_of_range.append("≥" + str(param["minimum"]))
+    if "exclusiveMinimum" in param.keys():
+        list_of_range.append(">" + str(param["exclusiveMinimum"]))
+    if "maximum" in param.keys():
+        list_of_range.append("≤" + str(param["maximum"]))
+    if "exclusiveMaximum" in param.keys():
+        list_of_range.append("<" + str(param["exclusiveMaximum"]))
+    if "exclusive" in param.keys():
+        list_of_range.append("≠" + str(param["exclusive"]))
+
+    if len(list_of_range) == 0:
+        return "N/A"
+    else:
+        range_in_text = ""
+        for item in list_of_range:
+            if range_in_text != "":
+                range_in_text += "<br/>"
+            range_in_text += str(item)
+        return range_in_text
+
+
+def extract_parameter_info(parameters, namespace=""):
+    params = []
+    for k, v in parameters.items():
+        if v["type"] != "object":
+            param = {}
+            param["Name"] = namespace + k
+            param["Type"] = format_param_type(v["type"])
+            param["Description"] = v["description"]
+            param["Default"] = v["default"]
+            param["Range"] = format_param_range(v)
+            params.append(param)
+        else:  # if the object is namespace, then dive deeper in to json value
+            params.extend(extract_parameter_info(v["properties"], k + "."))
+    return params
+
+
+def format_json(json_data):
+    parameters = list(json_data["definitions"].values())[0]["properties"]
+    # cspell: ignore tablefmt
+    markdown_table = tabulate(extract_parameter_info(parameters), headers="keys", tablefmt="github")
+    return markdown_table
+
+
+def define_env(env):
+    @env.macro
+    def json_to_markdown(json_schema_file_path):
+        with open(json_schema_file_path) as f:
+            data = json.load(f)
+            return format_json(data)

perception/lidar_apollo_segmentation_tvm_nodes/README.md

@@ -49,6 +49,10 @@ The input are non-ground points as a PointCloud2 message from the sensor_msgs pa
 
 The output is a [DetectedObjectsWithFeature](https://github.com/tier4/tier4_autoware_msgs/blob/tier4/universe/tier4_perception_msgs/msg/object_recognition/DetectedObjectsWithFeature.msg).
 
+#### Parameters
+
+{{ json_to_markdown("perception/lidar_apollo_segmentation_tvm_nodes/schema/lidar_apollo_segmentation_tvm_nodes.schema.json") }}
+
 ### Error detection and handling
 
 Abort and warn when the input frame can't be converted to `base_link`.

planning/behavior_path_planner/config/scene_module_manager.param.yaml

@@ -8,6 +8,7 @@
       enable_rtc: true
       enable_simultaneous_execution_as_approved_module: false
       enable_simultaneous_execution_as_candidate_module: true
+      keep_last: false
       priority: 6
       max_module_size: 1
 
@@ -16,6 +17,7 @@
       enable_rtc: true
       enable_simultaneous_execution_as_approved_module: false
       enable_simultaneous_execution_as_candidate_module: true
+      keep_last: false
       priority: 6
       max_module_size: 1
 
@@ -24,6 +26,7 @@
       enable_rtc: true
       enable_simultaneous_execution_as_approved_module: true
       enable_simultaneous_execution_as_candidate_module: true
+      keep_last: false
       priority: 5
       max_module_size: 1
 
@@ -32,6 +35,7 @@
       enable_rtc: true
       enable_simultaneous_execution_as_approved_module: true
       enable_simultaneous_execution_as_candidate_module: true
+      keep_last: false
       priority: 5
       max_module_size: 1
 
@@ -40,6 +44,7 @@
       enable_rtc: true
       enable_simultaneous_execution_as_approved_module: false
       enable_simultaneous_execution_as_candidate_module: false
+      keep_last: false
       priority: 0
       max_module_size: 1
 
@@ -48,6 +53,7 @@
       enable_rtc: true
       enable_simultaneous_execution_as_approved_module: false
       enable_simultaneous_execution_as_candidate_module: false
+      keep_last: false
       priority: 2
       max_module_size: 1
 
@@ -56,6 +62,7 @@
       enable_rtc: true
       enable_simultaneous_execution_as_approved_module: false
       enable_simultaneous_execution_as_candidate_module: false
+      keep_last: true
       priority: 1
       max_module_size: 1
 
@@ -64,6 +71,7 @@
       enable_rtc: true
       enable_simultaneous_execution_as_approved_module: true
       enable_simultaneous_execution_as_candidate_module: false
+      keep_last: false
       priority: 4
       max_module_size: 1
 
@@ -72,6 +80,7 @@
       enable_rtc: true
       enable_simultaneous_execution_as_approved_module: false
       enable_simultaneous_execution_as_candidate_module: false
+      keep_last: false
       priority: 3
       max_module_size: 1
 
@@ -80,5 +89,6 @@
       enable_rtc: true
       enable_simultaneous_execution_as_approved_module: true
       enable_simultaneous_execution_as_candidate_module: true
+      keep_last: false
       priority: 7
       max_module_size: 1

planning/behavior_path_planner/docs/behavior_path_planner_avoidance_design.md

@@ -192,7 +192,7 @@ The avoidance target should be limited to stationary objects (you should not avo
 Not only the length from the centerline, but also the length from the road shoulder is calculated and used for the filtering process. It calculates the ratio of _the actual length between the the object's center and the center line_ `shift_length` and _the maximum length the object can shift_ `shiftable_length`.
 
 $$
-l_D = l_a - \frac{width}{2} \\
+l_D = l_a - \frac{width}{2}, \\
 ratio =  \frac{l_d}{l_D}
 $$
 
@@ -201,7 +201,7 @@ $$
 - $l_a$ : distance between centerline and most left boundary.
 - $width$ : object width
 
-The closer the object is to the shoulder, the larger the value of $ratio$ (theoretical max value is 1.0), and it compares the value and `object_check_shiftable_ratio` to determine whether the object is a parked-car.
+The closer the object is to the shoulder, the larger the value of $ratio$ (theoretical max value is 1.0), and it compares the value and `object_check_shiftable_ratio` to determine whether the object is a parked-car. If the road has no road shoulders, it uses `object_check_min_road_shoulder_width` as a road shoulder width virtually.
 
 ![fig2](../image/avoidance/parked-car-detection.svg)
 
@@ -692,7 +692,7 @@ namespace: `avoidance.target_filtering.`
 | object_check_backward_distance                        | [m]  | double | Backward distance to search the avoidance target.                                                                                                                                                                                      | 2.0           |
 | object_check_goal_distance                            | [m]  | double | Backward distance to search the avoidance target.                                                                                                                                                                                      | 20.0          |
 | object_check_shiftable_ratio                          | [m]  | double | Vehicles around the center line within this distance will be excluded from avoidance target.                                                                                                                                           | 0.6           |
-| object_check_min_road_shoulder_width                  | [m]  | double | Vehicles around the center line within this distance will be excluded from avoidance target.                                                                                                                                           | 0.5           |
+| object_check_min_road_shoulder_width                  | [m]  | double | Width considered as a road shoulder if the lane does not have a road shoulder target.                                                                                                                                                  | 0.5           |
 | object_last_seen_threshold                            | [s]  | double | For the compensation of the detection lost. The object is registered once it is observed as an avoidance target. When the detection loses, the timer will start and the object will be un-registered when the time exceeds this limit. | 2.0           |
 
 ### Safety check parameters

planning/behavior_path_planner/docs/behavior_path_planner_lane_change_design.md

@@ -163,7 +163,7 @@ First, we divide the target objects into obstacles in the target lane, obstacles
 
 ![object lanes](../image/lane_change/lane_objects.drawio.svg)
 
-Furthermore, to change lanes behind a vehicle waiting at a traffic light, we skip the safety check for the stopping vehicles near the traffic light.
+Furthermore, to change lanes behind a vehicle waiting at a traffic light, we skip the safety check for the stopping vehicles near the traffic light.　The explanation for parked car detection is written in [documentation for avoidance module](../docs/behavior_path_planner_avoidance_design.md).
 
 ##### Collision check in prepare phase
 
@@ -276,7 +276,7 @@ The following parameters are configurable in `lane_change.param.yaml`.
 | `prediction_time_resolution`                | [s]    | double  | Time resolution for object's path interpolation and collision check.                                            | 0.5                |
 | `longitudinal_acceleration_sampling_num`    | [-]    | int     | Number of possible lane-changing trajectories that are being influenced by longitudinal acceleration            | 5                  |
 | `lateral_acceleration_sampling_num`         | [-]    | int     | Number of possible lane-changing trajectories that are being influenced by lateral acceleration                 | 3                  |
-| `object_check_min_road_shoulder_width`      | [m]    | double  | Vehicles around the center line within this distance will be excluded from parking objects                      | 0.5                |
+| `object_check_min_road_shoulder_width`      | [m]    | double  | Width considered as a road shoulder if the lane does not have a road shoulder                                   | 0.5                |
 | `object_shiftable_ratio_threshold`          | [-]    | double  | Vehicles around the center line within this distance ratio will be excluded from parking objects                | 0.6                |
 | `min_length_for_turn_signal_activation`     | [m]    | double  | Turn signal will be activated if the ego vehicle approaches to this length from minimum lane change length      | 10.0               |
 | `length_ratio_for_turn_signal_deactivation` | [-]    | double  | Turn signal will be deactivated if the ego vehicle approaches to this length ratio for lane change finish point | 0.8                |

planning/behavior_path_planner/include/behavior_path_planner/marker_utils/lane_change/debug.hpp

@@ -26,12 +26,8 @@ namespace marker_utils::lane_change_markers
 {
 using behavior_path_planner::LaneChangePath;
 using visualization_msgs::msg::MarkerArray;
-MarkerArray showObjectInfo(const CollisionCheckDebugMap & obj_debug_vec, std::string && ns);
 MarkerArray showAllValidLaneChangePath(
   const std::vector<LaneChangePath> & lanes, std::string && ns);
-MarkerArray showLerpedPose(const CollisionCheckDebugMap & obj_debug_vec, std::string && ns);
-MarkerArray showPolygon(const CollisionCheckDebugMap & obj_debug_vec, std::string && ns);
-MarkerArray showPolygonPose(const CollisionCheckDebugMap & obj_debug_vec, std::string && ns);
 MarkerArray createLaneChangingVirtualWallMarker(
   const geometry_msgs::msg::Pose & lane_changing_pose, const std::string & module_name,
   const rclcpp::Time & now, const std::string & ns);

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

@@ -104,6 +104,11 @@ MarkerArray createPredictedPathMarkerArray(
   const PredictedPath & ego_predicted_path, const vehicle_info_util::VehicleInfo & vehicle_info,
   std::string && ns, const int32_t & id, const float & r, const float & g, const float & b);
 
+MarkerArray showPolygon(const CollisionCheckDebugMap & obj_debug_vec, std::string && ns);
+
+MarkerArray showPredictedPath(const CollisionCheckDebugMap & obj_debug_vec, std::string && ns);
+
+MarkerArray showSafetyCheckInfo(const CollisionCheckDebugMap & obj_debug_vec, std::string && ns);
 }  // namespace marker_utils
 
 #endif  // BEHAVIOR_PATH_PLANNER__MARKER_UTILS__UTILS_HPP_

planning/behavior_path_planner/include/behavior_path_planner/parameters.hpp

@@ -28,6 +28,7 @@ struct ModuleConfigParameters
   bool enable_rtc{false};
   bool enable_simultaneous_execution_as_approved_module{false};
   bool enable_simultaneous_execution_as_candidate_module{false};
+  bool keep_last{false};
   uint8_t priority{0};
   uint8_t max_module_size{0};
 };

planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/base_class.hpp

@@ -209,6 +209,10 @@ class LaneChangeBase
     return direction_;
   }
 
+  boost::optional<Pose> getStopPose() const { return lane_change_stop_pose_; }
+
+  void resetStopPose() { lane_change_stop_pose_ = boost::none; }
+
 protected:
   virtual lanelet::ConstLanelets getCurrentLanes() const = 0;
 
@@ -244,6 +248,7 @@ class LaneChangeBase
   PathWithLaneId prev_module_path_{};
   DrivableAreaInfo prev_drivable_area_info_{};
   TurnSignalInfo prev_turn_signal_info_{};
+  boost::optional<Pose> lane_change_stop_pose_{boost::none};
 
   PathWithLaneId prev_approved_path_{};
 

planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp

@@ -147,6 +147,8 @@ class NormalLaneChange : public LaneChangeBase
     const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes,
     const lanelet::ConstLanelets & target_backward_lanes) const;
 
+  void setStopPose(const Pose & stop_pose);
+
   rclcpp::Logger logger_ = rclcpp::get_logger("lane_change").get_child(getModuleTypeStr());
 };
 }  // namespace behavior_path_planner