diff --git a/.cppcheck_suppressions b/.cppcheck_suppressions index bcbb222fc659f..d5417133956f7 100644 --- a/.cppcheck_suppressions +++ b/.cppcheck_suppressions @@ -1,7 +1,6 @@ *:*/test/* checkersReport -functionStatic missingInclude missingIncludeSystem noConstructor diff --git a/common/perception_utils/CMakeLists.txt b/common/perception_utils/CMakeLists.txt index c05dbdaaef644..18ef18303fb1b 100644 --- a/common/perception_utils/CMakeLists.txt +++ b/common/perception_utils/CMakeLists.txt @@ -4,8 +4,6 @@ project(perception_utils) find_package(autoware_cmake REQUIRED) autoware_package() -ament_auto_package() - ament_auto_add_library(${PROJECT_NAME} SHARED src/run_length_encoder.cpp ) @@ -14,3 +12,5 @@ find_package(OpenCV REQUIRED) target_link_libraries(${PROJECT_NAME} ${OpenCV_LIBS} ) + +ament_auto_package() diff --git a/control/autoware_external_cmd_selector/README.md b/control/autoware_external_cmd_selector/README.md index 0495004b68b84..0c76afff35e53 100644 --- a/control/autoware_external_cmd_selector/README.md +++ b/control/autoware_external_cmd_selector/README.md @@ -32,3 +32,7 @@ The current mode is set via service, `remote` is remotely operated, `local` is t | `/external/selected/hazard_lights_cmd` | autoware_vehicle_msgs::msg::HazardLightsCommand | Pass through hazard light with current mode. | | `/external/selected/heartbeat` | TBD | Pass through heartbeat with current mode. | | `/external/selected/turn_indicators_cmd` | autoware_vehicle_msgs::msg::TurnIndicatorsCommand | Pass through turn indicator with current mode. | + +## Parameters + +{{json_to_markdown("control/autoware_external_cmd_selector/schema/external_cmd_selector.schema.json")}} diff --git a/control/autoware_external_cmd_selector/schema/external_cmd_selector.schema.json b/control/autoware_external_cmd_selector/schema/external_cmd_selector.schema.json new file mode 100644 index 0000000000000..b577277695767 --- /dev/null +++ b/control/autoware_external_cmd_selector/schema/external_cmd_selector.schema.json @@ -0,0 +1,36 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for external_cmd_selector", + "type": "object", + "definitions": { + "external_cmd_selector_node": { + "type": "object", + "properties": { + "update_rate": { + "type": "number", + "default": 10.0, + "description": "The rate in Hz at which the external command selector node updates." + }, + "initial_selector_mode": { + "type": "string", + "enum": ["local", "remote"], + "default": "local", + "description": "The initial mode for command selection, either 'local' or 'remote'." + } + }, + "required": ["update_rate", "initial_selector_mode"] + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/external_cmd_selector_node" + } + }, + "required": ["ros__parameters"] + } + }, + "required": ["/**"] +} diff --git a/control/autoware_vehicle_cmd_gate/src/vehicle_cmd_gate.cpp b/control/autoware_vehicle_cmd_gate/src/vehicle_cmd_gate.cpp index a02ab010e8cea..b26a2630ed994 100644 --- a/control/autoware_vehicle_cmd_gate/src/vehicle_cmd_gate.cpp +++ b/control/autoware_vehicle_cmd_gate/src/vehicle_cmd_gate.cpp @@ -361,12 +361,14 @@ template T VehicleCmdGate::getContinuousTopic( const std::shared_ptr & prev_topic, const T & current_topic, const std::string & topic_name) { - if ((rclcpp::Time(current_topic.stamp) - rclcpp::Time(prev_topic->stamp)).seconds() > 0.0) { + if ((rclcpp::Time(current_topic.stamp) - rclcpp::Time(prev_topic->stamp)).seconds() >= 0.0) { return current_topic; } else { - RCLCPP_INFO( - get_logger(), - "The operation mode is changed, but the %s is not received yet:", topic_name.c_str()); + if (topic_name != "") { + RCLCPP_INFO( + get_logger(), + "The operation mode is changed, but the %s is not received yet:", topic_name.c_str()); + } return *prev_topic; } } diff --git a/control/autoware_vehicle_cmd_gate/src/vehicle_cmd_gate.hpp b/control/autoware_vehicle_cmd_gate/src/vehicle_cmd_gate.hpp index 569136847f1a6..6863015443363 100644 --- a/control/autoware_vehicle_cmd_gate/src/vehicle_cmd_gate.hpp +++ b/control/autoware_vehicle_cmd_gate/src/vehicle_cmd_gate.hpp @@ -240,7 +240,8 @@ class VehicleCmdGate : public rclcpp::Node template T getContinuousTopic( - const std::shared_ptr & prev_topic, const T & current_topic, const std::string & topic_name); + const std::shared_ptr & prev_topic, const T & current_topic, + const std::string & topic_name = ""); // Algorithm Control prev_control_cmd_; diff --git a/evaluator/kinematic_evaluator/CMakeLists.txt b/evaluator/kinematic_evaluator/CMakeLists.txt index 6c951d610bd8b..6c1b6abc3dace 100644 --- a/evaluator/kinematic_evaluator/CMakeLists.txt +++ b/evaluator/kinematic_evaluator/CMakeLists.txt @@ -38,8 +38,7 @@ if(BUILD_TESTING) ) endif() -ament_auto_package( - INSTALL_TO_SHARE +ament_auto_package(INSTALL_TO_SHARE param launch ) diff --git a/evaluator/kinematic_evaluator/README.md b/evaluator/kinematic_evaluator/README.md index 6e7caddd1f5bf..7cc826195c1b4 100644 --- a/evaluator/kinematic_evaluator/README.md +++ b/evaluator/kinematic_evaluator/README.md @@ -1,3 +1,7 @@ # Kinematic Evaluator TBD + +## Parameters + +{{json_to_markdown("evaluator/kinematic_evaluator/schema/kinematic_evaluator.schema.json")}} diff --git a/evaluator/kinematic_evaluator/schema/kinematic_evaluator.schema.json b/evaluator/kinematic_evaluator/schema/kinematic_evaluator.schema.json new file mode 100644 index 0000000000000..86bb8c74f27f6 --- /dev/null +++ b/evaluator/kinematic_evaluator/schema/kinematic_evaluator.schema.json @@ -0,0 +1,35 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for kinematic_evaluator_node", + "type": "object", + "definitions": { + "kinematic_evaluator_node": { + "type": "object", + "properties": { + "output_file": { + "type": "string", + "default": "kinematic_metrics.results", + "description": "Name of the file to which kinematic metrics are written. If empty, metrics are not written to a file." + }, + "selected_metrics": { + "type": "string", + "default": "velocity_stats", + "description": "The specific metrics selected for evaluation." + } + }, + "required": ["output_file", "selected_metrics"] + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/kinematic_evaluator_node" + } + }, + "required": ["ros__parameters"] + } + }, + "required": ["/**"] +} diff --git a/perception/autoware_detected_object_validation/src/obstacle_pointcloud/debugger.hpp b/perception/autoware_detected_object_validation/src/obstacle_pointcloud/debugger.hpp index e097755c6c506..22027ec6dbd0d 100644 --- a/perception/autoware_detected_object_validation/src/obstacle_pointcloud/debugger.hpp +++ b/perception/autoware_detected_object_validation/src/obstacle_pointcloud/debugger.hpp @@ -66,6 +66,7 @@ class Debugger pointcloud_within_polygon_->clear(); } + // cppcheck-suppress functionStatic void addNeighborPointcloud(const pcl::PointCloud::Ptr & input) { pcl::PointCloud::Ptr input_xyz = toXYZ(input); diff --git a/perception/autoware_map_based_prediction/src/map_based_prediction_node.cpp b/perception/autoware_map_based_prediction/src/map_based_prediction_node.cpp index e6980b323f492..bd8b6557bc3b6 100644 --- a/perception/autoware_map_based_prediction/src/map_based_prediction_node.cpp +++ b/perception/autoware_map_based_prediction/src/map_based_prediction_node.cpp @@ -32,6 +32,7 @@ #include #include +#include #include #include #include @@ -360,20 +361,11 @@ CrosswalkEdgePoints getCrosswalkEdgePoints(const lanelet::ConstLanelet & crosswa } bool withinRoadLanelet( - const TrackedObject & object, const lanelet::LaneletMapPtr & lanelet_map_ptr, + const TrackedObject & object, + const std::vector> & surrounding_lanelets_with_dist, const bool use_yaw_information = false) { - const auto & obj_pos = object.kinematics.pose_with_covariance.pose.position; - lanelet::BasicPoint2d search_point(obj_pos.x, obj_pos.y); - // nearest lanelet - constexpr double search_radius = 10.0; // [m] - const auto surrounding_lanelets = - lanelet::geometry::findNearest(lanelet_map_ptr->laneletLayer, search_point, search_radius); - - for (const auto & lanelet_with_dist : surrounding_lanelets) { - const auto & dist = lanelet_with_dist.first; - const auto & lanelet = lanelet_with_dist.second; - + for (const auto & [dist, lanelet] : surrounding_lanelets_with_dist) { if (lanelet.hasAttribute(lanelet::AttributeName::Subtype)) { lanelet::Attribute attr = lanelet.attribute(lanelet::AttributeName::Subtype); if ( @@ -397,6 +389,20 @@ bool withinRoadLanelet( return false; } +bool withinRoadLanelet( + const TrackedObject & object, const lanelet::LaneletMapPtr & lanelet_map_ptr, + const bool use_yaw_information = false) +{ + const auto & obj_pos = object.kinematics.pose_with_covariance.pose.position; + lanelet::BasicPoint2d search_point(obj_pos.x, obj_pos.y); + // nearest lanelet + constexpr double search_radius = 10.0; // [m] + const auto surrounding_lanelets_with_dist = + lanelet::geometry::findWithin2d(lanelet_map_ptr->laneletLayer, search_point, search_radius); + + return withinRoadLanelet(object, surrounding_lanelets_with_dist, use_yaw_information); +} + boost::optional isReachableCrosswalkEdgePoints( const TrackedObject & object, const lanelet::ConstLanelets & surrounding_lanelets, const lanelet::ConstLanelets & surrounding_crosswalks, const CrosswalkEdgePoints & edge_points, @@ -1411,9 +1417,7 @@ PredictedObject MapBasedPredictionNode::getPredictedObjectAsCrosswalkUser( const auto & obj_vel = object.kinematics.twist_with_covariance.twist.linear; const auto estimated_velocity = std::hypot(obj_vel.x, obj_vel.y); const auto velocity = std::max(min_crosswalk_user_velocity_, estimated_velocity); - // TODO(Mamoru Sobue): 3rd argument of findNearest is the number of lanelets, not radius, so past - // implementation has been wrong. - const auto surrounding_lanelets_with_dist = lanelet::geometry::findNearest( + const auto surrounding_lanelets_with_dist = lanelet::geometry::findWithin2d( lanelet_map_ptr_->laneletLayer, lanelet::BasicPoint2d{obj_pos.x, obj_pos.y}, prediction_time_horizon_.pedestrian * velocity); lanelet::ConstLanelets surrounding_lanelets; @@ -1458,7 +1462,7 @@ PredictedObject MapBasedPredictionNode::getPredictedObjectAsCrosswalkUser( // If the object is not crossing the crosswalk, in the road lanelets, try to find the closest // crosswalk and generate path to the crosswalk edge - } else if (withinRoadLanelet(object, lanelet_map_ptr_)) { + } else if (withinRoadLanelet(object, surrounding_lanelets_with_dist)) { lanelet::ConstLanelet closest_crosswalk{}; const auto & obj_pose = object.kinematics.pose_with_covariance.pose; const auto found_closest_crosswalk = diff --git a/perception/autoware_multi_object_tracker/schema/input_channels.schema.json b/perception/autoware_multi_object_tracker/schema/input_channels.schema.json index 96f612828b597..3d7bba9daae87 100644 --- a/perception/autoware_multi_object_tracker/schema/input_channels.schema.json +++ b/perception/autoware_multi_object_tracker/schema/input_channels.schema.json @@ -8,22 +8,26 @@ "properties": { "topic": { "type": "string", - "description": "The ROS topic name for the input channel." + "description": "The ROS topic name for the input channel.", + "default": "/perception/object_recognition/detection/objects" }, "can_spawn_new_tracker": { "type": "boolean", - "description": "Indicates if the input channel can spawn new trackers." + "description": "Indicates if the input channel can spawn new trackers.", + "default": true }, "optional": { "type": "object", "properties": { "name": { "type": "string", - "description": "The name of the input channel." + "description": "The name of the input channel.", + "default": "detected_objects" }, "short_name": { "type": "string", - "description": "The short name of the input channel." + "description": "The short name of the input channel.", + "default": "all" } } } diff --git a/perception/autoware_traffic_light_visualization/src/traffic_light_roi_visualizer/node.cpp b/perception/autoware_traffic_light_visualization/src/traffic_light_roi_visualizer/node.cpp index 7482749630a48..7ef13cf457c07 100644 --- a/perception/autoware_traffic_light_visualization/src/traffic_light_roi_visualizer/node.cpp +++ b/perception/autoware_traffic_light_visualization/src/traffic_light_roi_visualizer/node.cpp @@ -20,7 +20,6 @@ #include #include -#include namespace autoware::traffic_light { @@ -103,26 +102,18 @@ bool TrafficLightRoiVisualizerNode::createRect( cv::Mat & image, const tier4_perception_msgs::msg::TrafficLightRoi & tl_roi, const ClassificationResult & result) { - cv::Scalar color; - if (result.label.find("red") != std::string::npos) { - color = cv::Scalar{254, 149, 149}; - } else if (result.label.find("yellow") != std::string::npos) { - color = cv::Scalar{254, 250, 149}; - } else if (result.label.find("green") != std::string::npos) { - color = cv::Scalar{149, 254, 161}; - } else { - color = cv::Scalar{250, 250, 250}; - } + const auto info = extractShapeInfo(result.label); cv::rectangle( image, cv::Point(tl_roi.roi.x_offset, tl_roi.roi.y_offset), cv::Point(tl_roi.roi.x_offset + tl_roi.roi.width, tl_roi.roi.y_offset + tl_roi.roi.height), - color, 2); + info.color, 2); - std::string shape_name = extractShapeName(result.label); + constexpr int shape_img_size = 16; + const auto position = cv::Point(tl_roi.roi.x_offset, tl_roi.roi.y_offset); visualization::drawTrafficLightShape( - image, shape_name, cv::Point(tl_roi.roi.x_offset, tl_roi.roi.y_offset), color, 16, result.prob); + image, info.shapes, shape_img_size, position, info.color, result.prob); return true; } diff --git a/perception/autoware_traffic_light_visualization/src/traffic_light_roi_visualizer/node.hpp b/perception/autoware_traffic_light_visualization/src/traffic_light_roi_visualizer/node.hpp index 47a13389950a5..245c6caa6946c 100644 --- a/perception/autoware_traffic_light_visualization/src/traffic_light_roi_visualizer/node.hpp +++ b/perception/autoware_traffic_light_visualization/src/traffic_light_roi_visualizer/node.hpp @@ -36,7 +36,9 @@ #include #include #include +#include #include +#include namespace autoware::traffic_light { @@ -46,6 +48,15 @@ struct ClassificationResult std::string label; }; +/** + * @brief A struct to represent parsed traffic light shape information. + */ +struct TrafficLightShapeInfo +{ + cv::Scalar color; //!< Color associated with "circle". + std::vector shapes; //!< Shape names. +}; + class TrafficLightRoiVisualizerNode : public rclcpp::Node { public: @@ -78,6 +89,8 @@ class TrafficLightRoiVisualizerNode : public rclcpp::Node {tier4_perception_msgs::msg::TrafficLightElement::RIGHT_ARROW, "right"}, {tier4_perception_msgs::msg::TrafficLightElement::UP_ARROW, "straight"}, {tier4_perception_msgs::msg::TrafficLightElement::DOWN_ARROW, "down"}, + {tier4_perception_msgs::msg::TrafficLightElement::UP_LEFT_ARROW, "straight_left"}, + {tier4_perception_msgs::msg::TrafficLightElement::UP_RIGHT_ARROW, "straight_right"}, {tier4_perception_msgs::msg::TrafficLightElement::DOWN_LEFT_ARROW, "down_left"}, {tier4_perception_msgs::msg::TrafficLightElement::DOWN_RIGHT_ARROW, "down_right"}, {tier4_perception_msgs::msg::TrafficLightElement::CROSS, "cross"}, @@ -85,18 +98,48 @@ class TrafficLightRoiVisualizerNode : public rclcpp::Node {tier4_perception_msgs::msg::TrafficLightElement::UNKNOWN, "unknown"}, }; - std::string extractShapeName(const std::string & label) + /** + * @brief Return RGB color from color string associated with "circle". + * @param color Color string. + * @return RGB color. + */ + static cv::Scalar strToColor(const std::string & color) + { + if (color == "red") { + return {254, 149, 149}; + } else if (color == "yellow") { + return {254, 250, 149}; + } else if (color == "green") { + return {149, 254, 161}; + } else { + return {250, 250, 250}; + } + } + + /** + * @brief Extract color and shape names from label. + * @param label String formatted as `-,-,...,-`. + * @return Extracted information includes a color associated with "circle" and shape names. + */ + static TrafficLightShapeInfo extractShapeInfo(const std::string & label) { - size_t start_pos = label.find('-'); - if (start_pos != std::string::npos) { - start_pos++; // Start after the hyphen - size_t end_pos = label.find(',', start_pos); // Find the next comma after the hyphen - if (end_pos == std::string::npos) { // If no comma is found, take the rest of the string - end_pos = label.length(); + cv::Scalar color{255, 255, 255}; + std::vector shapes; + + std::stringstream ss(label); + std::string segment; + while (std::getline(ss, segment, ',')) { + size_t hyphen_pos = segment.find('-'); + if (hyphen_pos != std::string::npos) { + auto shape = segment.substr(hyphen_pos + 1); + if (shape == "circle") { + const auto color_str = segment.substr(0, hyphen_pos); + color = strToColor(color_str); + } + shapes.emplace_back(shape); } - return label.substr(start_pos, end_pos - start_pos); } - return "unknown"; // Return "unknown" if no hyphen is found + return {color, shapes}; } bool createRect( diff --git a/perception/autoware_traffic_light_visualization/src/traffic_light_roi_visualizer/shape_draw.cpp b/perception/autoware_traffic_light_visualization/src/traffic_light_roi_visualizer/shape_draw.cpp index c685bc0639601..8c7a59cbcb003 100644 --- a/perception/autoware_traffic_light_visualization/src/traffic_light_roi_visualizer/shape_draw.cpp +++ b/perception/autoware_traffic_light_visualization/src/traffic_light_roi_visualizer/shape_draw.cpp @@ -17,164 +17,128 @@ #include "opencv2/highgui.hpp" #include "opencv2/imgproc.hpp" +#include +#include +#include +#include +#include + namespace autoware::traffic_light::visualization { void drawShape( - const DrawFunctionParams & params, const std::string & filename, bool flipHorizontally, - bool flipVertically, int x_offset, int y_offset, double scale_factor) + cv::Mat & image, const std::vector & params, int size, const cv::Point & position, + const cv::Scalar & color, float probability) { - std::string filepath = - ament_index_cpp::get_package_share_directory("autoware_traffic_light_visualization") + - "/images/" + filename; - cv::Mat shapeImg = cv::imread(filepath, cv::IMREAD_UNCHANGED); - if (shapeImg.empty()) { - std::cerr << "Failed to load image: " << filepath << std::endl; - return; - } + // load concatenated shape image + const auto shape_img = loadShapeImage(params, size); - if (flipHorizontally) { - cv::flip(shapeImg, shapeImg, 1); // Flip horizontally - } + // Calculate the width of the text + std::string prob_str = std::to_string(static_cast(round(probability * 100))) + "%"; - if (flipVertically) { - cv::flip(shapeImg, shapeImg, 0); // Flip vertically - } + int baseline = 0; + cv::Size text_size = cv::getTextSize(prob_str, cv::FONT_HERSHEY_SIMPLEX, 0.75, 2, &baseline); - cv::resize( - shapeImg, shapeImg, cv::Size(params.size, params.size), scale_factor, scale_factor, - cv::INTER_AREA); + const int fill_rect_w = shape_img.cols + text_size.width + 20; + const int fill_rect_h = std::max(shape_img.rows, text_size.height) + 10; - // Calculate the center position including offsets - cv::Point position( - params.position.x + x_offset, params.position.y - shapeImg.rows / 2 + y_offset); + const cv::Point rect_position(position.x, position.y - fill_rect_h); - // Check for image boundaries if ( - position.x < 0 || position.y < 0 || position.x + shapeImg.cols > params.image.cols || - position.y + shapeImg.rows > params.image.rows) { + rect_position.x < 0 || rect_position.y < 0 || rect_position.x + fill_rect_w > image.cols || + position.y > image.rows) { // TODO(KhalilSelyan): This error message may flood the terminal logs, so commented out // temporarily. Need to consider a better way. // std::cerr << "Adjusted position is out of image bounds." << std::endl; return; } - - // Calculate the width of the text - std::string probabilityText = - std::to_string(static_cast(round(params.probability * 100))) + "%"; - int baseline = 0; - cv::Size textSize = - cv::getTextSize(probabilityText, cv::FONT_HERSHEY_SIMPLEX, 0.75, 2, &baseline); - - // Adjust the filled rectangle to be at the top edge and the correct width - int filledRectWidth = - shapeImg.cols + (filename != "unknown.png" ? textSize.width + 10 : 5); // Add some padding - int filledRectHeight = shapeImg.rows + 10; // Add some padding - - cv::rectangle( - params.image, cv::Rect(position.x - 2, position.y - 5, filledRectWidth, filledRectHeight), - params.color, - -1); // Filled rectangle - - // Create ROI on the destination image - cv::Mat destinationROI = params.image(cv::Rect(position, cv::Size(shapeImg.cols, shapeImg.rows))); - - // Overlay the image onto the main image - for (int y = 0; y < shapeImg.rows; ++y) { - for (int x = 0; x < shapeImg.cols; ++x) { - cv::Vec4b & pixel = shapeImg.at(y, x); - if (pixel[3] != 0) { // Only non-transparent pixels - destinationROI.at(y, x) = cv::Vec3b(pixel[0], pixel[1], pixel[2]); + cv::Rect rectangle(rect_position.x, rect_position.y, fill_rect_w, fill_rect_h); + cv::rectangle(image, rectangle, color, -1); + + // Position the probability text right next to the shape. (Text origin: bottom-left) + const int prob_x_offset = shape_img.cols + 15; + const int prob_y_offset = fill_rect_h / 4; + cv::putText( + image, prob_str, cv::Point(position.x + prob_x_offset, position.y - prob_y_offset), + cv::FONT_HERSHEY_SIMPLEX, 0.75, cv::Scalar(0, 0, 0), 2, cv::LINE_AA); + + if (!shape_img.empty()) { + // Create ROI on the destination image + const int shape_y_offset = fill_rect_h / 4; + auto shapeRoi = image(cv::Rect( + rect_position.x + 5, rect_position.y + shape_y_offset, shape_img.cols, shape_img.rows)); + + // Overlay the image onto the main image + for (int y = 0; y < shape_img.rows; ++y) { + for (int x = 0; x < shape_img.cols; ++x) { + const auto & pixel = shape_img.at(y, x); + if (pixel[3] != 0) { // Only non-transparent pixels + shapeRoi.at(y, x) = cv::Vec3b(pixel[0], pixel[1], pixel[2]); + } } } } - - // Position the probability text right next to the shape - if (filename != "unknown.png") { - cv::putText( - params.image, probabilityText, - cv::Point( - position.x + shapeImg.cols + 5, position.y + shapeImg.rows / 2 + textSize.height / 2), - cv::FONT_HERSHEY_SIMPLEX, 0.75, cv::Scalar(0, 0, 0), 2, cv::LINE_AA); - } -} - -void drawCircle(const DrawFunctionParams & params) -{ - int y_offset = params.size / 2 + 5; - drawShape(params, "circle.png", false, false, 0, -y_offset); -} - -void drawLeftArrow(const DrawFunctionParams & params) -{ - int y_offset = params.size / 2 + 5; - drawShape(params, "left_arrow.png", false, false, 0, -y_offset); -} - -void drawRightArrow(const DrawFunctionParams & params) -{ - int y_offset = params.size / 2 + 5; - drawShape(params, "left_arrow.png", true, false, 0, -y_offset); } -void drawStraightArrow(const DrawFunctionParams & params) +cv::Mat loadShapeImage(const std::vector & params, int size, double scale_factor) { - int y_offset = params.size / 2 + 5; // This adjusts the base position upwards + if (params.empty()) { + return {}; + } - drawShape(params, "straight_arrow.png", false, false, 0, -y_offset); -} -void drawDownArrow(const DrawFunctionParams & params) -{ - int y_offset = params.size / 2 + 5; // This adjusts the base position upwards - drawShape(params, "straight_arrow.png", false, true, 0, -y_offset); -} + static const auto img_dir = + ament_index_cpp::get_package_share_directory("autoware_traffic_light_visualization") + + "/images/"; -void drawDownLeftArrow(const DrawFunctionParams & params) -{ - int y_offset = params.size / 2 + 5; - drawShape(params, "down_left_arrow.png", false, false, 0, -y_offset); -} + std::vector src_img; + for (const auto & param : params) { + auto filepath = img_dir + param.filename; + auto img = cv::imread(filepath, cv::IMREAD_UNCHANGED); -void drawDownRightArrow(const DrawFunctionParams & params) -{ - int y_offset = params.size / 2 + 5; - drawShape(params, "down_left_arrow.png", true, false, 0, -y_offset); -} + cv::resize(img, img, cv::Size(size, size), scale_factor, scale_factor, cv::INTER_AREA); -void drawCross(const DrawFunctionParams & params) -{ - int y_offset = params.size / 2 + 5; + if (param.h_flip) { + cv::flip(img, img, 1); + } + if (param.v_flip) { + cv::flip(img, img, 0); + } + src_img.emplace_back(img); + } - drawShape(params, "cross.png", false, false, 0, -y_offset); -} + cv::Mat dst; + cv::hconcat(src_img, dst); // cspell:ignore hconcat -void drawUnknown(const DrawFunctionParams & params) -{ - int y_offset = params.size / 2 + 5; - drawShape(params, "unknown.png", false, false, 0, -y_offset); + return dst; } void drawTrafficLightShape( - cv::Mat & image, const std::string & shape, const cv::Point & position, const cv::Scalar & color, - int size, float probability) + cv::Mat & image, const std::vector & shapes, int size, const cv::Point & position, + const cv::Scalar & color, float probability) { - static std::map shapeToFunction = { - {"circle", drawCircle}, - {"left", drawLeftArrow}, - {"right", drawRightArrow}, - {"straight", drawStraightArrow}, - {"down", drawDownArrow}, - {"down_left", drawDownLeftArrow}, - {"down_right", drawDownRightArrow}, - {"cross", drawCross}, - {"unknown", drawUnknown}}; - auto it = shapeToFunction.find(shape); - if (it != shapeToFunction.end()) { - DrawFunctionParams params{image, position, color, size, probability}; - it->second(params); - } else { - std::cerr << "Unknown shape: " << shape << std::endl; + using ShapeImgParamFunction = std::function; + + static const std::unordered_map shapeToParamFunction = { + {"circle", circleImgParam}, + {"left", leftArrowImgParam}, + {"right", rightArrowImgParam}, + {"straight", straightArrowImgParam}, + {"down", downArrowImgParam}, + {"straight_left", straightLeftArrowImgParam}, + {"straight_right", straightRightArrowImgParam}, + {"down_left", downLeftArrowImgParam}, + {"down_right", downRightArrowImgParam}, + {"cross", crossImgParam}, + {"unknown", unknownImgParam}}; + + std::vector params; + for (const auto & shape : shapes) { + if (shapeToParamFunction.find(shape) != shapeToParamFunction.end()) { + auto func = shapeToParamFunction.at(shape); + params.emplace_back(func()); + } } -} + drawShape(image, params, size, position, color, probability); +} } // namespace autoware::traffic_light::visualization diff --git a/perception/autoware_traffic_light_visualization/src/traffic_light_roi_visualizer/shape_draw.hpp b/perception/autoware_traffic_light_visualization/src/traffic_light_roi_visualizer/shape_draw.hpp index 150d78f767ee3..93aae041ede11 100644 --- a/perception/autoware_traffic_light_visualization/src/traffic_light_roi_visualizer/shape_draw.hpp +++ b/perception/autoware_traffic_light_visualization/src/traffic_light_roi_visualizer/shape_draw.hpp @@ -20,36 +20,167 @@ #include #include -#include #include +#include namespace autoware::traffic_light::visualization { -struct DrawFunctionParams +/** + * @brief A struct of parameters to load shape image. + */ +struct ShapeImgParam { - cv::Mat & image; - cv::Point position; - cv::Scalar color; - int size; - float probability; + std::string filename; //!< Filename of shape image. + bool h_flip; //!< Whether to flip horizontally + bool v_flip; //!< Whether to flip vertically }; -using DrawFunction = std::function; - +/** + * @brief Draw traffic light shapes on the camera view image. + * @param image Camera view image. + * @param params Shape parameters to load shape image. + * @param size Shape image size to resize. + * @param position Top-left position of a ROI. + * @param color Rectangle color. + * @param probability Classification probability. + */ void drawShape( - const DrawFunctionParams & params, const std::string & filename, bool flipHorizontally, - bool flipVertically, int x_offset, int y_offset, double scale_factor = 0.3); -void drawCircle(const DrawFunctionParams & params); -void drawLeftArrow(const DrawFunctionParams & params); -void drawRightArrow(const DrawFunctionParams & params); -void drawStraightArrow(const DrawFunctionParams & params); -void drawDownArrow(const DrawFunctionParams & params); -void drawDownLeftArrow(const DrawFunctionParams & params); -void drawDownRightArrow(const DrawFunctionParams & params); -void drawCross(const DrawFunctionParams & params); -void drawUnknown(const DrawFunctionParams & params); + cv::Mat & image, const std::vector & params, int size, const cv::Point & position, + const cv::Scalar & color, float probability); + +/** + * @brief Load shape images and concatenate them. + * @param params Parameters for each shape image. + * @param size Image size to resize. + * @param scale_factor Scale factor to resize. + * @return If no parameter is specified returns empty Mat, otherwise returns horizontally + * concatenated image. + */ +cv::Mat loadShapeImage( + const std::vector & params, int size, double scale_factor = 0.3); + +/** + * @brief Load parameter of circle. + * + * @return Parameter of circle. + */ +inline ShapeImgParam circleImgParam() +{ + return {"circle.png", false, false}; +} + +/** + * @brief Load parameter of left-arrow. + * + * @return Parameter of left-arrow. + */ +inline ShapeImgParam leftArrowImgParam() +{ + return {"left_arrow.png", false, false}; +} + +/** + * @brief Load parameter of right-arrow. + * + * @return Parameter of right-arrow, the image is flipped left-arrow horizontally. + */ +inline ShapeImgParam rightArrowImgParam() +{ + return {"left_arrow.png", true, false}; +} + +/** + * @brief Load parameter of straight-arrow. + * + * @return Parameter of straight-arrow. + */ +inline ShapeImgParam straightArrowImgParam() +{ + return {"straight_arrow.png", false, false}; +} + +/** + * @brief Load parameter of down-arrow. + * + * @return Parameter of down-arrow, the image is flipped straight-arrow vertically. + */ +inline ShapeImgParam downArrowImgParam() +{ + return {"straight_arrow.png", false, true}; +} + +/** + * @brief Load parameter of straight-left-arrow. + * + * @return Parameter of straight-left-arrow, the image is flipped down-left-arrow vertically. + */ +inline ShapeImgParam straightLeftArrowImgParam() +{ + return {"down_left_arrow.png", false, true}; +} + +/** + * @brief Load parameter of straight-right-arrow. + * + * @return Parameter of straight-right-arrow, the image is flipped down-left-arrow both horizontally + * and vertically. + */ +inline ShapeImgParam straightRightArrowImgParam() +{ + return {"down_left_arrow.png", true, true}; +} + +/** + * @brief Load parameter of down-left-arrow. + * + * @return Parameter of down-left-arrow. + */ +inline ShapeImgParam downLeftArrowImgParam() +{ + return {"down_left_arrow.png", false, false}; +} + +/** + * @brief Load parameter of down-right-arrow. + * + * @return Parameter of down-right-arrow, the image is flipped straight-arrow horizontally. + */ +inline ShapeImgParam downRightArrowImgParam() +{ + return {"down_left_arrow.png", true, false}; +} + +/** + * @brief Load parameter of cross-arrow. + * + * @return Parameter of cross-arrow. + */ +inline ShapeImgParam crossImgParam() +{ + return {"cross.png", false, false}; +} + +/** + * @brief Load parameter of unknown shape. + * + * @return Parameter of unkown shape. + */ +inline ShapeImgParam unknownImgParam() +{ + return {"unknown.png", false, false}; +} + +/** + * @brief Draw traffic light shapes on the camera view image. + * @param image Camera view image. + * @param shapes Shape names. + * @param size Shape image size to resize. + * @param position Top-left position of a ROI. + * @param color Color of traffic light. + * @param probability Classification probability. + */ void drawTrafficLightShape( - cv::Mat & image, const std::string & shape, const cv::Point & position, const cv::Scalar & color, - int size, float probability); + cv::Mat & image, const std::vector & shapes, int size, const cv::Point & position, + const cv::Scalar & color, float probability); } // namespace autoware::traffic_light::visualization diff --git a/planning/autoware_freespace_planner/README.md b/planning/autoware_freespace_planner/README.md index 91c880924857d..4783cf410f0e4 100644 --- a/planning/autoware_freespace_planner/README.md +++ b/planning/autoware_freespace_planner/README.md @@ -38,6 +38,8 @@ None ### Parameters +{{json_to_markdown("planning/autoware_freespace_planner/schema/freespace_planner.schema.json")}} + #### Node parameters | Parameter | Type | Description | diff --git a/planning/autoware_freespace_planner/schema/freespace_planner.schema.json b/planning/autoware_freespace_planner/schema/freespace_planner.schema.json new file mode 100644 index 0000000000000..9d17a07ee9204 --- /dev/null +++ b/planning/autoware_freespace_planner/schema/freespace_planner.schema.json @@ -0,0 +1,232 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for freespace_planner", + "type": "object", + "definitions": { + "freespace_planner_params": { + "type": "object", + "properties": { + "planning_algorithm": { + "type": "string", + "enum": ["astar", "rrtstar"], + "default": "astar", + "description": "Planning algorithm to use, options: astar, rrtstar." + }, + "waypoints_velocity": { + "type": "number", + "default": 5.0, + "description": "velocity in output trajectory (currently, only constant velocity is supported." + }, + "update_rate": { + "type": "number", + "default": 10.0, + "description": "timer's update rate" + }, + "th_arrived_distance_m": { + "type": "number", + "default": 1.0, + "description": "Threshold for considering the vehicle has arrived at its goal." + }, + "th_stopped_time_sec": { + "type": "number", + "default": 1.0, + "description": "Time threshold for considering the vehicle as stopped." + }, + "th_stopped_velocity_mps": { + "type": "number", + "default": 0.01, + "description": "Velocity threshold for considering the vehicle as stopped." + }, + "th_course_out_distance_m": { + "type": "number", + "default": 1.0, + "description": "Threshold distance for considering the vehicle has deviated from its course." + }, + "vehicle_shape_margin_m": { + "type": "number", + "default": 1.0, + "description": "Margin around the vehicle shape for planning." + }, + "replan_when_obstacle_found": { + "type": "boolean", + "default": true, + "description": "Replan path when an obstacle is found." + }, + "replan_when_course_out": { + "type": "boolean", + "default": true, + "description": "Replan path when the vehicle deviates from its course." + }, + "time_limit": { + "type": "number", + "default": 30000.0, + "description": "Time limit for the planner." + }, + "max_turning_ratio": { + "type": "number", + "default": 0.5, + "description": "Maximum turning ratio, relative to the actual turning limit of the vehicle." + }, + "turning_steps": { + "type": "number", + "default": 1, + "description": "Number of steps for turning." + }, + "theta_size": { + "type": "number", + "default": 144, + "description": "Number of discretized angles for search." + }, + "angle_goal_range": { + "type": "number", + "default": 6.0, + "description": "Range of acceptable angles at the goal." + }, + "lateral_goal_range": { + "type": "number", + "default": 0.5, + "description": "Lateral range of acceptable goal positions." + }, + "longitudinal_goal_range": { + "type": "number", + "default": 2.0, + "description": "Longitudinal range of acceptable goal positions." + }, + "curve_weight": { + "type": "number", + "default": 0.5, + "description": "Weight for curves in the cost function." + }, + "reverse_weight": { + "type": "number", + "default": 1.0, + "description": "Weight for reverse movement in the cost function." + }, + "direction_change_weight": { + "type": "number", + "default": 1.5, + "description": "Weight for direction changes in the cost function." + }, + "obstacle_threshold": { + "type": "number", + "default": 100, + "description": "Threshold for considering an obstacle in the costmap." + }, + "astar": { + "type": "object", + "properties": { + "only_behind_solutions": { + "type": "boolean", + "default": false, + "description": "Consider only solutions behind the vehicle." + }, + "use_back": { + "type": "boolean", + "default": true, + "description": "Allow reverse motion in A* search." + }, + "expansion_distance": { + "type": "number", + "default": 0.5, + "description": "Distance for expanding nodes in A* search." + }, + "distance_heuristic_weight": { + "type": "number", + "default": 1.0, + "description": "Weight for the distance heuristic in A* search." + }, + "smoothness_weight": { + "type": "number", + "default": 0.5, + "description": "Weight for the smoothness heuristic in A* search." + } + }, + "required": [ + "only_behind_solutions", + "use_back", + "expansion_distance", + "distance_heuristic_weight", + "smoothness_weight" + ] + }, + "rrtstar": { + "type": "object", + "properties": { + "enable_update": { + "type": "boolean", + "default": true, + "description": "Enable updates in RRT*." + }, + "use_informed_sampling": { + "type": "boolean", + "default": true, + "description": "Use informed sampling in RRT*." + }, + "max_planning_time": { + "type": "number", + "default": 150.0, + "description": "Maximum planning time for RRT*." + }, + "neighbor_radius": { + "type": "number", + "default": 8.0, + "description": "Radius for neighboring nodes in RRT*." + }, + "margin": { + "type": "number", + "default": 0.1, + "description": "Margin for RRT* sampling." + } + }, + "required": [ + "enable_update", + "use_informed_sampling", + "max_planning_time", + "neighbor_radius", + "margin" + ] + } + }, + "required": [ + "planning_algorithm", + "waypoints_velocity", + "update_rate", + "th_arrived_distance_m", + "th_stopped_time_sec", + "th_stopped_velocity_mps", + "th_course_out_distance_m", + "vehicle_shape_margin_m", + "replan_when_obstacle_found", + "replan_when_course_out", + "time_limit", + "max_turning_ratio", + "turning_steps", + "theta_size", + "angle_goal_range", + "lateral_goal_range", + "longitudinal_goal_range", + "curve_weight", + "reverse_weight", + "direction_change_weight", + "obstacle_threshold", + "astar", + "rrtstar" + ], + "additionalProperties": false + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/freespace_planner_params" + } + }, + "required": ["ros__parameters"], + "additionalProperties": false + } + }, + "required": ["/**"], + "additionalProperties": false +} diff --git a/planning/autoware_freespace_planning_algorithms/package.xml b/planning/autoware_freespace_planning_algorithms/package.xml index 05e906c998096..ecd8cfb7adb02 100644 --- a/planning/autoware_freespace_planning_algorithms/package.xml +++ b/planning/autoware_freespace_planning_algorithms/package.xml @@ -23,10 +23,13 @@ nlohmann-json-dev pybind11_vendor rosbag2_cpp + rosbag2_storage std_msgs tf2 tf2_geometry_msgs + rosbag2_storage_mcap + ament_cmake_ros ament_lint_auto autoware_lint_common diff --git a/planning/autoware_freespace_planning_algorithms/src/reeds_shepp.cpp b/planning/autoware_freespace_planning_algorithms/src/reeds_shepp.cpp index 97149dbbb071f..ee6d1cb8d1d9a 100644 --- a/planning/autoware_freespace_planning_algorithms/src/reeds_shepp.cpp +++ b/planning/autoware_freespace_planning_algorithms/src/reeds_shepp.cpp @@ -676,7 +676,7 @@ ReedsSheppStateSpace::StateXYT ReedsSheppStateSpace::interpolate( } StateXYT s_out; - double phi, v; + double v; s_out.x = s_out.y = 0.0; s_out.yaw = s0.yaw; @@ -689,7 +689,7 @@ ReedsSheppStateSpace::StateXYT ReedsSheppStateSpace::interpolate( v = std::min(seg, path.length_[i]); seg -= v; } - phi = s_out.yaw; + double phi = s_out.yaw; switch (path.type_[i]) { case RS_LEFT: s_out.x += (sin(phi + v) - std::sin(phi)); diff --git a/planning/autoware_mission_planner/src/lanelet2_plugins/default_planner.hpp b/planning/autoware_mission_planner/src/lanelet2_plugins/default_planner.hpp index 19e28ee2abfc7..5394caf698664 100644 --- a/planning/autoware_mission_planner/src/lanelet2_plugins/default_planner.hpp +++ b/planning/autoware_mission_planner/src/lanelet2_plugins/default_planner.hpp @@ -44,6 +44,8 @@ struct DefaultPlannerParameters class DefaultPlanner : public mission_planner::PlannerPlugin { public: + DefaultPlanner() : is_graph_ready_(false), route_handler_(), param_(), node_(nullptr) {} + void initialize(rclcpp::Node * node) override; void initialize(rclcpp::Node * node, const LaneletMapBin::ConstSharedPtr msg) override; bool ready() const override; diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_module.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_module.hpp index 749ed57f95cc9..aaad9d5f1a8f7 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_module.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_module.hpp @@ -143,38 +143,21 @@ class ThreadSafeData void set_pull_over_path(const PullOverPath & path) { const std::lock_guard lock(mutex_); - pull_over_path_ = std::make_shared(path); - if (path.type != PullOverPlannerType::NONE && path.type != PullOverPlannerType::FREESPACE) { - lane_parking_pull_over_path_ = std::make_shared(path); - } - - last_path_update_time_ = clock_->now(); + set_pull_over_path_no_lock(path); } void set_pull_over_path(const std::shared_ptr & path) { const std::lock_guard lock(mutex_); - pull_over_path_ = path; - if (path->type != PullOverPlannerType::NONE && path->type != PullOverPlannerType::FREESPACE) { - lane_parking_pull_over_path_ = path; - } - last_path_update_time_ = clock_->now(); + set_pull_over_path_no_lock(path); } template void set(Args... args) { std::lock_guard lock(mutex_); - (..., set(args)); + (..., set_no_lock(args)); } - void set(const GoalCandidates & arg) { set_goal_candidates(arg); } - void set(const std::vector & arg) { set_pull_over_path_candidates(arg); } - void set(const std::shared_ptr & arg) { set_pull_over_path(arg); } - void set(const PullOverPath & arg) { set_pull_over_path(arg); } - void set(const GoalCandidate & arg) { set_modified_goal_pose(arg); } - void set(const BehaviorModuleOutput & arg) { set_last_previous_module_output(arg); } - void set(const PreviousPullOverData & arg) { set_prev_data(arg); } - void set(const CollisionCheckDebugMap & arg) { set_collision_check(arg); } void clearPullOverPath() { @@ -232,6 +215,34 @@ class ThreadSafeData DEFINE_SETTER_GETTER_WITH_MUTEX(PredictedObjects, dynamic_target_objects) private: + void set_pull_over_path_no_lock(const PullOverPath & path) + { + pull_over_path_ = std::make_shared(path); + if (path.type != PullOverPlannerType::NONE && path.type != PullOverPlannerType::FREESPACE) { + lane_parking_pull_over_path_ = std::make_shared(path); + } + + last_path_update_time_ = clock_->now(); + } + + void set_pull_over_path_no_lock(const std::shared_ptr & path) + { + pull_over_path_ = path; + if (path->type != PullOverPlannerType::NONE && path->type != PullOverPlannerType::FREESPACE) { + lane_parking_pull_over_path_ = path; + } + last_path_update_time_ = clock_->now(); + } + + void set_no_lock(const GoalCandidates & arg) { goal_candidates_ = arg; } + void set_no_lock(const std::vector & arg) { pull_over_path_candidates_ = arg; } + void set_no_lock(const std::shared_ptr & arg) { set_pull_over_path_no_lock(arg); } + void set_no_lock(const PullOverPath & arg) { set_pull_over_path_no_lock(arg); } + void set_no_lock(const GoalCandidate & arg) { modified_goal_pose_ = arg; } + void set_no_lock(const BehaviorModuleOutput & arg) { last_previous_module_output_ = arg; } + void set_no_lock(const PreviousPullOverData & arg) { prev_data_ = arg; } + void set_no_lock(const CollisionCheckDebugMap & arg) { collision_check_ = arg; } + std::shared_ptr pull_over_path_{nullptr}; std::shared_ptr lane_parking_pull_over_path_{nullptr}; std::vector pull_over_path_candidates_; diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/scene.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/scene.hpp index 64882b3c6add0..ff3af92442fbe 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/scene.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/scene.hpp @@ -53,6 +53,8 @@ class NormalLaneChange : public LaneChangeBase void update_lanes(const bool is_approved) final; + void update_filtered_objects() final; + void updateLaneChangeStatus() override; std::pair getSafePath(LaneChangePath & safe_path) const override; diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/base_class.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/base_class.hpp index 1ebe1de4641ab..6c7dc31e857e5 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/base_class.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/base_class.hpp @@ -67,6 +67,8 @@ class LaneChangeBase virtual void update_lanes(const bool is_approved) = 0; + virtual void update_filtered_objects() = 0; + virtual void updateLaneChangeStatus() = 0; virtual std::pair getSafePath(LaneChangePath & safe_path) const = 0; @@ -252,6 +254,7 @@ class LaneChangeBase std::shared_ptr abort_path_{}; std::shared_ptr planner_data_{}; lane_change::CommonDataPtr common_data_ptr_{}; + FilteredByLanesExtendedObjects filtered_objects_{}; BehaviorModuleOutput prev_module_output_{}; std::optional lane_change_stop_pose_{std::nullopt}; diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/calculation.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/calculation.hpp index 3dc5e7ee62a57..6c48445b47567 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/calculation.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/calculation.hpp @@ -58,6 +58,23 @@ double calc_dist_from_pose_to_terminal_end( * @return The required backward buffer distance in meters. */ double calc_stopping_distance(const LCParamPtr & lc_param_ptr); + +/** + * @brief Calculates the distance to last fit width position along the lane + * + * This function computes the distance from the current ego position to the last + * position along the lane where the ego foot prints stays within the lane + * boundaries. + * + * @param lanelets current ego lanelets + * @param src_pose source pose to calculate distance from + * @param bpp_param common parameters used in behavior path planner. + * @param margin additional margin for checking lane width + * @return distance to last fit width position along the lane + */ +double calc_dist_to_last_fit_width( + const lanelet::ConstLanelets lanelets, const Pose & src_pose, + const BehaviorPathPlannerParameters & bpp_param, const double margin = 0.1); } // namespace autoware::behavior_path_planner::utils::lane_change::calculation #endif // AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__CALCULATION_HPP_ diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/interface.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/interface.cpp index 6451d6abadfa2..a7cf89193b37f 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/interface.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/interface.cpp @@ -76,6 +76,7 @@ void LaneChangeInterface::updateData() universe_utils::ScopedTimeTrack st(__func__, *getTimeKeeper()); module_type_->setPreviousModuleOutput(getPreviousModuleOutput()); module_type_->update_lanes(getCurrentStatus() == ModuleStatus::RUNNING); + module_type_->update_filtered_objects(); module_type_->updateSpecialData(); if (isWaitingApproval() || module_type_->isAbortState()) { diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp index 8502831572470..23925d56badb3 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp @@ -99,6 +99,11 @@ void NormalLaneChange::update_lanes(const bool is_approved) *common_data_ptr_->lanes_polygon_ptr = create_lanes_polygon(common_data_ptr_); } +void NormalLaneChange::update_filtered_objects() +{ + filtered_objects_ = filterObjects(); +} + void NormalLaneChange::updateLaneChangeStatus() { universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); @@ -392,6 +397,14 @@ void NormalLaneChange::insertStopPoint( const auto target_objects = filterObjects(); double stopping_distance = distance_to_terminal - lane_change_buffer - stop_point_buffer; + const auto & curr_lanes_poly = common_data_ptr_->lanes_polygon_ptr->current.value(); + if (utils::isEgoWithinOriginalLane(curr_lanes_poly, getEgoPose(), planner_data_->parameters)) { + const double distance_to_last_fit_width = + utils::lane_change::calculation::calc_dist_to_last_fit_width( + lanelets, path.points.front().point.pose, planner_data_->parameters); + stopping_distance = std::min(stopping_distance, distance_to_last_fit_width); + } + const auto & lc_start_point = status_.lane_change_path.info.lane_changing_start; if (!is_valid_start_point(common_data_ptr_, lc_start_point)) { @@ -732,8 +745,9 @@ bool NormalLaneChange::isAbleToReturnCurrentLane() const return false; } + const auto & curr_lanes_poly = common_data_ptr_->lanes_polygon_ptr->current.value(); if (!utils::isEgoWithinOriginalLane( - get_current_lanes(), getEgoPose(), planner_data_->parameters, + curr_lanes_poly, getEgoPose(), planner_data_->parameters, lane_change_parameters_->cancel.overhang_tolerance)) { lane_change_debug_.is_able_to_return_to_current_lane = false; return false; @@ -754,7 +768,7 @@ bool NormalLaneChange::isAbleToReturnCurrentLane() const if (dist > estimated_travel_dist) { const auto & estimated_pose = status_.lane_change_path.path.points.at(idx + 1).point.pose; auto is_ego_within_original_lane = utils::isEgoWithinOriginalLane( - get_current_lanes(), estimated_pose, planner_data_->parameters, + curr_lanes_poly, estimated_pose, planner_data_->parameters, lane_change_parameters_->cancel.overhang_tolerance); lane_change_debug_.is_able_to_return_to_current_lane = is_ego_within_original_lane; return is_ego_within_original_lane; @@ -812,13 +826,14 @@ bool NormalLaneChange::isAbleToStopSafely() const const auto stop_dist = -(current_velocity * current_velocity / (2.0 * planner_data_->parameters.min_acc)); + const auto & curr_lanes_poly = common_data_ptr_->lanes_polygon_ptr->current.value(); double dist = 0.0; for (size_t idx = nearest_idx; idx < status_.lane_change_path.path.points.size() - 1; ++idx) { dist += calcSignedArcLength(status_.lane_change_path.path.points, idx, idx + 1); if (dist > stop_dist) { const auto & estimated_pose = status_.lane_change_path.path.points.at(idx + 1).point.pose; return utils::isEgoWithinOriginalLane( - get_current_lanes(), estimated_pose, planner_data_->parameters); + curr_lanes_poly, estimated_pose, planner_data_->parameters); } } return true; @@ -1412,8 +1427,7 @@ bool NormalLaneChange::getLaneChangePaths( return false; } - const auto filtered_objects = filterObjects(); - const auto target_objects = getTargetObjects(filtered_objects, current_lanes); + const auto target_objects = getTargetObjects(filtered_objects_, current_lanes); const auto prepare_durations = calcPrepareDuration(current_lanes, target_lanes); @@ -1661,7 +1675,7 @@ bool NormalLaneChange::getLaneChangePaths( if ( !is_stuck && !utils::lane_change::passed_parked_objects( - common_data_ptr_, *candidate_path, filtered_objects.target_lane_leading, + common_data_ptr_, *candidate_path, filtered_objects_.target_lane_leading, lane_change_buffer, lane_change_debug_.collision_check_objects)) { debug_print_lat( "Reject: parking vehicle exists in the target lane, and the ego is not in stuck. Skip " @@ -1849,8 +1863,7 @@ PathSafetyStatus NormalLaneChange::isApprovedPathSafe() const return {true, true}; } - const auto filtered_objects = filterObjects(); - const auto target_objects = getTargetObjects(filtered_objects, current_lanes); + const auto target_objects = getTargetObjects(filtered_objects_, current_lanes); CollisionCheckDebugMap debug_data; @@ -1859,7 +1872,7 @@ PathSafetyStatus NormalLaneChange::isApprovedPathSafe() const common_data_ptr_->route_handler_ptr->getLateralIntervalsToPreferredLane(current_lanes.back())); const auto has_passed_parked_objects = utils::lane_change::passed_parked_objects( - common_data_ptr_, path, filtered_objects.target_lane_leading, min_lc_length, debug_data); + common_data_ptr_, path, filtered_objects_.target_lane_leading, min_lc_length, debug_data); if (!has_passed_parked_objects) { RCLCPP_DEBUG(logger_, "Lane change has been delayed."); diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/calculation.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/calculation.cpp index ac205ceedb34b..8f04395be572a 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/calculation.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/calculation.cpp @@ -16,6 +16,8 @@ #include #include +#include + namespace autoware::behavior_path_planner::utils::lane_change::calculation { double calc_ego_dist_to_terminal_end(const CommonDataPtr & common_data_ptr) @@ -54,4 +56,49 @@ double calc_stopping_distance(const LCParamPtr & lc_param_ptr) const auto param_back_dist = lc_param_ptr->backward_length_buffer_for_end_of_lane; return std::max(min_back_dist, param_back_dist); } + +double calc_dist_to_last_fit_width( + const lanelet::ConstLanelets lanelets, const Pose & src_pose, + const BehaviorPathPlannerParameters & bpp_param, const double margin) +{ + if (lanelets.empty()) return 0.0; + + const auto lane_polygon = lanelets.back().polygon2d().basicPolygon(); + const auto center_line = lanelet::utils::generateFineCenterline(lanelets.back(), 1.0); + + if (center_line.size() <= 1) return 0.0; + + universe_utils::LineString2d line_string; + line_string.reserve(center_line.size() - 1); + std::for_each(center_line.begin() + 1, center_line.end(), [&line_string](const auto & point) { + boost::geometry::append(line_string, universe_utils::Point2d(point.x(), point.y())); + }); + + const double buffer_distance = 0.5 * bpp_param.vehicle_width + margin; + universe_utils::MultiPolygon2d center_line_polygon; + namespace strategy = boost::geometry::strategy::buffer; + boost::geometry::buffer( + line_string, center_line_polygon, strategy::distance_symmetric(buffer_distance), + strategy::side_straight(), strategy::join_miter(), strategy::end_flat(), + strategy::point_square()); + + if (center_line_polygon.empty()) return 0.0; + + std::vector intersection_points; + boost::geometry::intersection(lane_polygon, center_line_polygon, intersection_points); + + if (intersection_points.empty()) { + return utils::getDistanceToEndOfLane(src_pose, lanelets); + } + + Pose pose; + double distance = std::numeric_limits::max(); + for (const auto & point : intersection_points) { + pose.position.x = boost::geometry::get<0>(point); + pose.position.y = boost::geometry::get<1>(point); + distance = std::min(distance, utils::getSignedDistance(src_pose, pose, lanelets)); + } + + return std::max(distance - (bpp_param.base_link2front + margin), 0.0); +} } // namespace autoware::behavior_path_planner::utils::lane_change::calculation diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/docs/behavior_path_planner_develop_maps.md b/planning/behavior_path_planner/autoware_behavior_path_planner_common/docs/behavior_path_planner_develop_maps.md new file mode 100644 index 0000000000000..1c3c01857e73f --- /dev/null +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/docs/behavior_path_planner_develop_maps.md @@ -0,0 +1,11 @@ +# Develop purpose maps + +## road_shoulders + +The road_shoulders lanelet map consist of a variety of pick-up/drop-off site maps with road_shoulder tags including: + +- pick-up/drop-off sites on the side of street lanes +- pick-up/drop-off sites on the side of curved lanes +- pick-up/drop-off sites inside a private area + +![road_shoulder_test](./road_shoulder_test_map.png) diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/docs/road_shoulder_test_map.png b/planning/behavior_path_planner/autoware_behavior_path_planner_common/docs/road_shoulder_test_map.png new file mode 100644 index 0000000000000..0815bd93538a5 Binary files /dev/null and b/planning/behavior_path_planner/autoware_behavior_path_planner_common/docs/road_shoulder_test_map.png differ diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp index 05f7c70fa42c2..f70e64730d119 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp @@ -59,9 +59,10 @@ Polygon2d createExtendedPolygon( const Pose & base_link_pose, const autoware::vehicle_info_utils::VehicleInfo & vehicle_info, const double lon_length, const double lat_margin, const bool is_stopped_obj, CollisionCheckDebug & debug); + Polygon2d createExtendedPolygon( - const Pose & obj_pose, const Shape & shape, const double lon_length, const double lat_margin, - const bool is_stopped_obj, CollisionCheckDebug & debug); + const PoseWithVelocityAndPolygonStamped & obj_pose_with_poly, const double lon_length, + const double lat_margin, const bool is_stopped_obj, CollisionCheckDebug & debug); PredictedPath convertToPredictedPath( const std::vector & path, const double time_resolution); diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/utils.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/utils.hpp index 5c1eee92c2a5d..03e6c2d7f8167 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/utils.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/utils.hpp @@ -55,6 +55,8 @@ using geometry_msgs::msg::Vector3; using tier4_planning_msgs::msg::PathPointWithLaneId; using tier4_planning_msgs::msg::PathWithLaneId; +static constexpr double eps = 0.01; + struct PolygonPoint { geometry_msgs::msg::Point point; @@ -241,6 +243,10 @@ bool isEgoWithinOriginalLane( const lanelet::ConstLanelets & current_lanes, const Pose & current_pose, const BehaviorPathPlannerParameters & common_param, const double outer_margin = 0.0); +bool isEgoWithinOriginalLane( + const lanelet::BasicPolygon2d & lane_polygon, const Pose & current_pose, + const BehaviorPathPlannerParameters & common_param, const double outer_margin = 0.0); + // path management // TODO(Horibe) There is a similar function in route_handler. Check. @@ -249,8 +255,10 @@ std::shared_ptr generateCenterLinePath( PathPointWithLaneId insertStopPoint(const double length, PathWithLaneId & path); +double getSignedDistanceFromLaneBoundary( + const lanelet::ConstLanelet & lanelet, const Point & position, const bool left_side); double getSignedDistanceFromBoundary( - const lanelet::ConstLanelets & shoulder_lanelets, const Pose & pose, const bool left_side); + const lanelet::ConstLanelets & lanelets, const Pose & pose, const bool left_side); std::optional getSignedDistanceFromBoundary( const lanelet::ConstLanelets & lanelets, const double vehicle_width, const double base_link2front, const double base_link2rear, const Pose & vehicle_pose, const bool left_side); diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_safety_checker/safety_check.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_safety_checker/safety_check.cpp index 8d2dd2fb86e29..1726638c1bf9b 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_safety_checker/safety_check.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_safety_checker/safety_check.cpp @@ -129,6 +129,7 @@ Polygon2d createExtendedPolygon( autoware::universe_utils::calcOffsetPose(base_link_pose, backward_lon_offset, lat_offset, 0.0); Polygon2d polygon; + polygon.outer().reserve(5); appendPointToPolygon(polygon, p1.position); appendPointToPolygon(polygon, p2.position); appendPointToPolygon(polygon, p3.position); @@ -140,13 +141,14 @@ Polygon2d createExtendedPolygon( } Polygon2d createExtendedPolygon( - const Pose & obj_pose, const Shape & shape, const double lon_length, const double lat_margin, - const bool is_stopped_obj, CollisionCheckDebug & debug) + const PoseWithVelocityAndPolygonStamped & obj_pose_with_poly, const double lon_length, + const double lat_margin, const bool is_stopped_obj, CollisionCheckDebug & debug) { - const auto obj_polygon = autoware::universe_utils::toPolygon2d(obj_pose, shape); + const auto & obj_polygon = obj_pose_with_poly.poly; if (obj_polygon.outer().empty()) { return obj_polygon; } + const auto & obj_pose = obj_pose_with_poly.pose; double max_x = std::numeric_limits::lowest(); double min_x = std::numeric_limits::max(); @@ -186,6 +188,7 @@ Polygon2d createExtendedPolygon( autoware::universe_utils::calcOffsetPose(obj_pose, backward_lon_offset, left_lat_offset, 0.0); Polygon2d polygon; + polygon.outer().reserve(5); appendPointToPolygon(polygon, p1.position); appendPointToPolygon(polygon, p2.position); appendPointToPolygon(polygon, p3.position); @@ -670,10 +673,9 @@ std::vector getCollidedPolygons( throw std::domain_error("invalid rss parameter. please select 'rectangle' or 'along_path'."); }(); const auto & extended_obj_polygon = - is_object_front - ? obj_polygon - : createExtendedPolygon( - obj_pose, target_object.shape, lon_offset, lat_margin, is_stopped_object, debug); + is_object_front ? obj_polygon + : createExtendedPolygon( + obj_pose_with_poly, lon_offset, lat_margin, is_stopped_object, debug); // check overlap with extended polygon if (boost::geometry::overlaps(extended_ego_polygon, extended_obj_polygon)) { diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/utils.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/utils.cpp index aa16f1bb23b79..0f3751cc32dd3 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/utils.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/utils.cpp @@ -559,19 +559,24 @@ bool isEgoWithinOriginalLane( const lanelet::ConstLanelets & current_lanes, const Pose & current_pose, const BehaviorPathPlannerParameters & common_param, const double outer_margin) { - const auto lane_length = lanelet::utils::getLaneletLength2d(current_lanes); - const auto lane_poly = lanelet::utils::getPolygonFromArcLength(current_lanes, 0, lane_length); - const auto base_link2front = common_param.base_link2front; - const auto base_link2rear = common_param.base_link2rear; - const auto vehicle_width = common_param.vehicle_width; + const auto combined_lane = lanelet::utils::combineLaneletsShape(current_lanes); + const auto lane_polygon = combined_lane.polygon2d().basicPolygon(); + return isEgoWithinOriginalLane(lane_polygon, current_pose, common_param, outer_margin); +} + +bool isEgoWithinOriginalLane( + const lanelet::BasicPolygon2d & lane_polygon, const Pose & current_pose, + const BehaviorPathPlannerParameters & common_param, const double outer_margin) +{ const auto vehicle_poly = autoware::universe_utils::toFootprint( - current_pose, base_link2front, base_link2rear, vehicle_width); + current_pose, common_param.base_link2front, common_param.base_link2rear, + common_param.vehicle_width); // Check if the ego vehicle is entirely within the lane with a given outer margin. for (const auto & p : vehicle_poly.outer()) { // When the point is in the polygon, the distance is 0. When it is out of the polygon, return a // positive value. - const auto dist = boost::geometry::distance(p, lanelet::utils::to2D(lane_poly).basicPolygon()); + const auto dist = boost::geometry::distance(p, lane_polygon); if (dist > std::max(outer_margin, 0.0)) { return false; // out of polygon } @@ -816,25 +821,29 @@ PathPointWithLaneId insertStopPoint(const double length, PathWithLaneId & path) return path.points.at(*insert_idx); } +double getSignedDistanceFromLaneBoundary( + const lanelet::ConstLanelet & lanelet, const Point & position, bool left_side) +{ + const auto lanelet_point = lanelet::utils::conversion::toLaneletPoint(position); + const auto & boundary_line_2d = left_side ? lanelet.leftBound2d() : lanelet.rightBound2d(); + const auto arc_coordinates = lanelet::geometry::toArcCoordinates( + boundary_line_2d, lanelet::utils::to2D(lanelet_point).basicPoint()); + return arc_coordinates.distance; +} + double getSignedDistanceFromBoundary( const lanelet::ConstLanelets & lanelets, const Pose & pose, bool left_side) { lanelet::ConstLanelet closest_lanelet; - lanelet::ArcCoordinates arc_coordinates; + if (lanelet::utils::query::getClosestLanelet(lanelets, pose, &closest_lanelet)) { - const auto lanelet_point = lanelet::utils::conversion::toLaneletPoint(pose.position); - const auto & boundary_line_2d = left_side - ? lanelet::utils::to2D(closest_lanelet.leftBound3d()) - : lanelet::utils::to2D(closest_lanelet.rightBound3d()); - arc_coordinates = lanelet::geometry::toArcCoordinates( - boundary_line_2d, lanelet::utils::to2D(lanelet_point).basicPoint()); - } else { - RCLCPP_ERROR_STREAM( - rclcpp::get_logger("behavior_path_planner").get_child("utils"), - "closest shoulder lanelet not found."); + return getSignedDistanceFromLaneBoundary(closest_lanelet, pose.position, left_side); } - return arc_coordinates.distance; + RCLCPP_ERROR_STREAM( + rclcpp::get_logger("behavior_path_planner").get_child("utils"), "closest lanelet not found."); + + return 0.0; } std::optional getSignedDistanceFromBoundary( @@ -1211,7 +1220,6 @@ PathWithLaneId setDecelerationVelocity( const auto stop_point_length = autoware::motion_utils::calcSignedArcLength(reference_path.points, 0, target_pose.position) + buffer; - constexpr double eps{0.01}; if (std::abs(target_velocity) < eps && stop_point_length > 0.0) { const auto stop_point = utils::insertStopPoint(stop_point_length, reference_path); } diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/test/test_safety_check.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/test/test_safety_check.cpp index 85a80eac8f215..2708b1e608779 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/test/test_safety_check.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/test/test_safety_check.cpp @@ -161,8 +161,14 @@ TEST(BehaviorPathPlanningSafetyUtilsTest, createExtendedObjPolygon) const bool is_stopped_object = false; CollisionCheckDebug debug; + + using autoware::behavior_path_planner::utils::path_safety_checker:: + PoseWithVelocityAndPolygonStamped; + + PoseWithVelocityAndPolygonStamped obj_pose_with_poly( + 0.0, obj_pose, 0.0, autoware::universe_utils::toPolygon2d(obj_pose, shape)); const auto polygon = - createExtendedPolygon(obj_pose, shape, lon_length, lat_margin, is_stopped_object, debug); + createExtendedPolygon(obj_pose_with_poly, lon_length, lat_margin, is_stopped_object, debug); EXPECT_EQ(polygon.outer().size(), static_cast(5)); diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/test_map/road_shoulder.osm b/planning/behavior_path_planner/autoware_behavior_path_planner_common/test_map/road_shoulder.osm new file mode 100644 index 0000000000000..88f1da37f46d1 --- /dev/null +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/test_map/road_shoulder.osm @@ -0,0 +1,9984 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/README.md b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/README.md index 318758220c239..825bb4043d993 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/README.md +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/README.md @@ -647,3 +647,15 @@ This module is activated when the following conditions are met: ### Known Issue If ego go over the stop line for a certain distance, then it will not transit from STOP. + +## Test Maps + +The intersections lanelet map consist of a variety of intersections including: + +- 4-way crossing with traffic light +- 4-way crossing without traffic light +- T-shape crossing without traffic light +- intersection with a loop +- complicated intersection + +![intersection_test](./docs/intersection_test_map.png) diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/package.xml b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/package.xml index 7772235342266..39ed187cd867a 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/package.xml +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/package.xml @@ -10,6 +10,7 @@ Tomoya Kimura Shumpei Wakabayashi Kyoichi Sugahara + Yukinari Hisaki Apache License 2.0 diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/CMakeLists.txt b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/CMakeLists.txt index 9cb992312f52a..49ab46e26bd17 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/CMakeLists.txt +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/CMakeLists.txt @@ -26,4 +26,4 @@ if(BUILD_TESTING) ) endif() -ament_auto_package() +ament_auto_package(INSTALL_TO_SHARE test_map) diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/README.md b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/README.md index 2abbb83575af5..311728b6b58d5 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/README.md +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/README.md @@ -1,3 +1,17 @@ # Behavior Velocity Planner Common This package provides common functions as a library, which are used in the `behavior_velocity_planner` node and modules. + +## Test map + +### intersection + +The intersections lanelet map consist of a variety of intersections including: + +- 4-way crossing with traffic light +- 4-way crossing without traffic light +- T-shape crossing without traffic light +- intersection with a loop +- complicated intersection + +![intersection_test](./docs/intersection_test_map.png) diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/docs/intersection_test_map.png b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/docs/intersection_test_map.png new file mode 100644 index 0000000000000..a2f3db6ad2a31 Binary files /dev/null and b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/docs/intersection_test_map.png differ diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/test_map/intersection/pointcloud_map.pcd b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/test_map/intersection/pointcloud_map.pcd new file mode 100644 index 0000000000000..d97cbe19e0085 Binary files /dev/null and b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/test_map/intersection/pointcloud_map.pcd differ